From 1fd83e07960207bf7a8d557a8892e5970e5c8ab6 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 28 Oct 2021 18:53:42 +0200 Subject: [PATCH] Allow customization of contact surface properties (#267) Adds support for registering callbacks that modify properties of all contacts generated by the dynamics engine. Example implementation for DART is provided. Signed-off-by: Martin Pecka Co-authored-by: Addisu Z. Taddese --- dartsim/CMakeLists.txt | 25 +- dartsim/src/SimulationFeatures.cc | 218 +++++++++++++++--- dartsim/src/SimulationFeatures.hh | 71 ++++++ dartsim/src/SimulationFeatures_TEST.cc | 171 ++++++++++++-- include/ignition/physics/ContactProperties.hh | 169 ++++++++++++++ .../physics/detail/ContactProperties.hh | 103 +++++++++ tpe/plugin/src/SimulationFeatures_TEST.cc | 6 +- 7 files changed, 713 insertions(+), 50 deletions(-) create mode 100644 include/ignition/physics/ContactProperties.hh create mode 100644 include/ignition/physics/detail/ContactProperties.hh diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 22bad22a5..59396ab58 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -1,4 +1,3 @@ - # This component expresses custom features of the dartsim plugin, which can # expose native dartsim data types. ign_add_component(dartsim INTERFACE @@ -33,6 +32,26 @@ target_link_libraries(${dartsim_plugin} ignition-common${IGN_COMMON_VER}::profiler ) +# The ignition fork of DART contains additional code that allows customizing +# contact constraints. We check for the presence of "ContactSurface.hpp", which +# was added to enable these customizations, to detect if the feature is +# available. +include(CheckIncludeFileCXX) +if (MSVC) + set(CMAKE_REQUIRED_FLAGS "/std:c++14") +else() + set(CMAKE_REQUIRED_FLAGS "-std=c++14") +endif() +set(CMAKE_REQUIRED_INCLUDES "${DART_INCLUDE_DIRS};${EIGEN3_INCLUDE_DIRS}") +set(CMAKE_REQUIRED_QUIET false) + +CHECK_INCLUDE_FILE_CXX(dart/constraint/ContactSurface.hpp DART_HAS_CONTACT_SURFACE_HEADER) +if (DART_HAS_CONTACT_SURFACE_HEADER) + target_compile_definitions(${dartsim_plugin} PRIVATE DART_HAS_CONTACT_SURFACE) +else() + message(STATUS "The version of DART does not support Contact constraint customizations.") +endif() + # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir install(TARGETS ${dartsim_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}) @@ -67,6 +86,10 @@ foreach(test ${tests}) "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" "IGNITION_PHYSICS_RESOURCE_DIR=\"${IGNITION_PHYSICS_RESOURCE_DIR}\"") + if (DART_HAS_CONTACT_SURFACE_HEADER) + target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE) + endif() + # Helps when we want to build a single test after making changes to dartsim_plugin add_dependencies(${test} ${dartsim_plugin}) endforeach() diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 27f135212..4ca731892 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -15,8 +15,17 @@ * */ +#include +#include +#include + #include #include +#include +#include +#ifdef DART_HAS_CONTACT_SURFACE +#include +#endif #include "SimulationFeatures.hh" @@ -64,41 +73,194 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const for (const auto &dtContact : colResult.getContacts()) { - dart::collision::CollisionObject *dtCollObj1 = dtContact.collisionObject1; - dart::collision::CollisionObject *dtCollObj2 = dtContact.collisionObject2; + auto contact = this->convertContact(dtContact); + if (contact) + outContacts.push_back(contact.value()); + } + + return outContacts; +} + +std::optional +SimulationFeatures::convertContact( + const dart::collision::Contact& _contact) const +{ + auto *dtCollObj1 = _contact.collisionObject1; + auto *dtCollObj2 = _contact.collisionObject2; - const dart::dynamics::ShapeFrame *dtShapeFrame1 = - dtCollObj1->getShapeFrame(); - const dart::dynamics::ShapeFrame *dtShapeFrame2 = - dtCollObj2->getShapeFrame(); + auto *dtShapeFrame1 = dtCollObj1->getShapeFrame(); + auto *dtShapeFrame2 = dtCollObj2->getShapeFrame(); - dart::dynamics::ConstBodyNodePtr dtBodyNode1; - dart::dynamics::ConstBodyNodePtr dtBodyNode2; + if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && + this->shapes.HasEntity(dtShapeFrame2->asShapeNode())) + { + std::size_t shape1ID = + this->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); + std::size_t shape2ID = + this->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); + + CompositeData extraData; + + // Add normal, depth and wrench to extraData. + auto& extraContactData = + extraData.Get(); + extraContactData.force = _contact.force; + extraContactData.normal = _contact.normal; + extraContactData.depth = _contact.penetrationDepth; + + + return SimulationFeatures::ContactInternal { + this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)), + this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)), + _contact.point, extraData + }; + } + + return std::nullopt; +} - if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && - this->shapes.HasEntity(dtShapeFrame2->asShapeNode())) +#ifdef DART_HAS_CONTACT_SURFACE +void SimulationFeatures::AddContactPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID, + SurfaceParamsCallback _callback) +{ + auto *world = this->ReferenceInterface(_worldID); + + auto handler = std::make_shared(); + handler->surfaceParamsCallback = _callback; + handler->convertContact = [this](const dart::collision::Contact& _contact) { + return this->convertContact(_contact); + }; + + this->contactSurfaceHandlers[_callbackID] = handler; + world->getConstraintSolver()->addContactSurfaceHandler(handler); +} + +bool SimulationFeatures::RemoveContactPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID) +{ + auto *world = this->ReferenceInterface(_worldID); + + if (this->contactSurfaceHandlers.find(_callbackID) != + this->contactSurfaceHandlers.end()) + { + const auto handler = this->contactSurfaceHandlers[_callbackID]; + this->contactSurfaceHandlers.erase(_callbackID); + return world->getConstraintSolver()->removeContactSurfaceHandler(handler); + } + else + { + ignerr << "Could not find the contact surface handler to be removed" + << std::endl; + return false; + } +} + +dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( + const dart::collision::Contact& _contact, + const size_t _numContactsOnCollisionObject) const +{ + auto pDart = ContactSurfaceHandler::createParams( + _contact, _numContactsOnCollisionObject); + + if (!this->surfaceParamsCallback) + return pDart; + + typedef SetContactPropertiesCallbackFeature F; + typedef FeaturePolicy3d P; + typename F::ContactSurfaceParams

pIgn; + + pIgn.frictionCoeff = pDart.mFrictionCoeff; + pIgn.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff; + pIgn.slipCompliance = pDart.mSlipCompliance; + pIgn.secondarySlipCompliance = pDart.mSecondarySlipCompliance; + pIgn.restitutionCoeff = pDart.mRestitutionCoeff; + pIgn.firstFrictionalDirection = pDart.mFirstFrictionalDirection; + pIgn.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity; + + auto contactInternal = this->convertContact(_contact); + if (contactInternal) + { + this->surfaceParamsCallback(contactInternal.value(), + _numContactsOnCollisionObject, pIgn); + + if (pIgn.frictionCoeff) + pDart.mFrictionCoeff = pIgn.frictionCoeff.value(); + if (pIgn.secondaryFrictionCoeff) + pDart.mSecondaryFrictionCoeff = pIgn.secondaryFrictionCoeff.value(); + if (pIgn.slipCompliance) + pDart.mSlipCompliance = pIgn.slipCompliance.value(); + if (pIgn.secondarySlipCompliance) + pDart.mSecondarySlipCompliance = pIgn.secondarySlipCompliance.value(); + if (pIgn.restitutionCoeff) + pDart.mRestitutionCoeff = pIgn.restitutionCoeff.value(); + if (pIgn.firstFrictionalDirection) + pDart.mFirstFrictionalDirection = pIgn.firstFrictionalDirection.value(); + if (pIgn.contactSurfaceMotionVelocity) + pDart.mContactSurfaceMotionVelocity = + pIgn.contactSurfaceMotionVelocity.value(); + + static bool warnedRollingFrictionCoeff = false; + if (!warnedRollingFrictionCoeff && pIgn.rollingFrictionCoeff) + { + ignwarn << "DART doesn't support rolling friction setting" << std::endl; + warnedRollingFrictionCoeff = true; + } + + static bool warnedSecondaryRollingFrictionCoeff = false; + if (!warnedSecondaryRollingFrictionCoeff && + pIgn.secondaryRollingFrictionCoeff) + { + ignwarn << "DART doesn't support secondary rolling friction setting" + << std::endl; + warnedSecondaryRollingFrictionCoeff = true; + } + + static bool warnedTorsionalFrictionCoeff = false; + if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff) { - std::size_t shape1ID = - this->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); - std::size_t shape2ID = - this->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); - - CompositeData extraData; - - // Add normal, depth and wrench to extraData. - auto &extraContactData = extraData.Get(); - extraContactData.force = dtContact.force; - extraContactData.normal = dtContact.normal; - extraContactData.depth = dtContact.penetrationDepth; - - outContacts.push_back( - {this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)), - this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)), - dtContact.point, extraData}); + ignwarn << "DART doesn't support torsional friction setting" + << std::endl; + warnedTorsionalFrictionCoeff = true; } } - return outContacts; + + this->lastIgnParams = pIgn; + + return pDart; +} + +dart::constraint::ContactConstraintPtr +IgnContactSurfaceHandler::createConstraint( + dart::collision::Contact& _contact, + const size_t _numContactsOnCollisionObject, + const double _timeStep) const +{ + // this call sets this->lastIgnParams + auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint( + _contact, _numContactsOnCollisionObject, _timeStep); + + typedef SetContactPropertiesCallbackFeature F; + typedef FeaturePolicy3d P; + typename F::ContactSurfaceParams

& p = this->lastIgnParams; + + if (this->lastIgnParams.errorReductionParameter) + constraint->setErrorReductionParameter(p.errorReductionParameter.value()); + + if (this->lastIgnParams.maxErrorAllowance) + constraint->setErrorAllowance(p.maxErrorAllowance.value()); + + if (this->lastIgnParams.maxErrorReductionVelocity) + constraint->setMaxErrorReductionVelocity( + p.maxErrorReductionVelocity.value()); + + if (this->lastIgnParams.constraintForceMixing) + constraint->setConstraintForceMixing(p.constraintForceMixing.value()); + + return constraint; } +#endif + } } } diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 0ceb2cbe3..ac87dc691 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -18,25 +18,80 @@ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ +#include +#include +#include #include + +#include +#ifdef DART_HAS_CONTACT_SURFACE +#include +#endif + #include #include +#include #include "Base.hh" +namespace dart +{ +namespace collision +{ +class Contact; +} +} + namespace ignition { namespace physics { namespace dartsim { struct SimulationFeatureList : FeatureList< ForwardStep, +#ifdef DART_HAS_CONTACT_SURFACE + SetContactPropertiesCallbackFeature, +#endif GetContactsFromLastStepFeature > { }; +#ifdef DART_HAS_CONTACT_SURFACE +class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler +{ + public: dart::constraint::ContactSurfaceParams createParams( + const dart::collision::Contact& _contact, + size_t _numContactsOnCollisionObject) const override; + + public: dart::constraint::ContactConstraintPtr createConstraint( + dart::collision::Contact& _contact, + size_t _numContactsOnCollisionObject, + double _timeStep) const override; + + public: typedef SetContactPropertiesCallbackFeature Feature; + public: typedef Feature::Implementation Impl; + + public: Impl::SurfaceParamsCallback surfaceParamsCallback; + + public: std::function< + std::optional(const dart::collision::Contact&) + > convertContact; + + public: mutable typename Feature::ContactSurfaceParams + lastIgnParams; +}; + +using IgnContactSurfaceHandlerPtr = std::shared_ptr; +#endif + class SimulationFeatures : public virtual Base, public virtual Implements3d { + public: using GetContactsFromLastStepFeature::Implementation + ::ContactInternal; + + public: SimulationFeatures() = default; + public: ~SimulationFeatures() override = default; + public: void WorldForwardStep( const Identity &_worldID, ForwardStep::Output &_h, @@ -45,6 +100,22 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; + + private: std::optional convertContact( + const dart::collision::Contact& _contact) const; + +#ifdef DART_HAS_CONTACT_SURFACE + public: void AddContactPropertiesCallback( + const Identity &_worldID, + const std::string &_callbackID, + SurfaceParamsCallback _callback) override; + + public: bool RemoveContactPropertiesCallback( + const Identity &_worldID, const std::string &_callbackID) override; + + private: std::unordered_map< + std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; +#endif }; } diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 265a7b732..e987f56cb 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -48,14 +49,23 @@ struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetEntities, ignition::physics::GetShapeBoundingBox, ignition::physics::CollisionFilterMaskFeature, +#ifdef DART_HAS_CONTACT_SURFACE + ignition::physics::SetContactPropertiesCallbackFeature, +#endif ignition::physics::sdf::ConstructSdfWorld > { }; using TestWorldPtr = ignition::physics::World3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; -using ContactPoint = ignition::physics::World3d::ContactPoint; -using ExtraContactData = - ignition::physics::World3d::ExtraContactData; +using TestWorld = ignition::physics::World3d; +using TestContactPoint = TestWorld::ContactPoint; +using TestExtraContactData = TestWorld::ExtraContactData; +using TestContact = TestWorld::Contact; +#ifdef DART_HAS_CONTACT_SURFACE +using ContactSurfaceParams = + ignition::physics::SetContactPropertiesCallbackFeature:: + ContactSurfaceParams; +#endif std::unordered_set LoadWorlds( const std::string &_library, @@ -237,17 +247,6 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto groundPlane = world->GetModel("ground_plane"); auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0); - // The first step already has contacts, but the contact force due to the - // impact does not match the steady-state force generated by the - // body's weight. - StepWorld(world); - - // After a second step, the contact force reaches steady-state - StepWorld(world); - - auto contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); - // Use a set because the order of collisions is not determined. std::set possibleCollisions = { groundPlaneCollision, @@ -274,9 +273,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) {sphere->GetLink(3)->GetShape(0), 3.0 * gravity}, }; - for (auto &contact : contacts) + // This procedure checks the validity of a generated contact point. It is + // used both when checking the contacts after the step is finished and for + // checking them inside the contact joint properties callback. The callback + // is called after the contacts are generated but before they affect the + // physics. That is why contact force is zero during the callback. + auto checkContact = [&](const TestContact& _contact, const bool zeroForce) { - const auto &contactPoint = contact.Get(); + const auto &contactPoint = _contact.Get(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); @@ -302,7 +306,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contactPoint.point, 1e-6)); // Check if the engine populated the extra contact data struct - const auto* extraContactData = contact.Query(); + const auto* extraContactData = _contact.Query(); ASSERT_NE(nullptr, extraContactData); // The normal of the contact force is a vector pointing up (z positive) @@ -315,8 +319,139 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[2], - forceExpectations.at(testCollision), 1e-3); + zeroForce ? 0 : forceExpectations.at(testCollision), 1e-3); + }; + +#ifdef DART_HAS_CONTACT_SURFACE + size_t numContactCallbackCalls = 0u; + auto contactCallback = [&]( + const TestContact& _contact, + size_t _numContactsOnCollision, + ContactSurfaceParams& _surfaceParams) + { + numContactCallbackCalls++; + checkContact(_contact, true); + EXPECT_EQ(1u, _numContactsOnCollision); + // the values in _surfaceParams are implemented as std::optional to allow + // physics engines fill only those parameters that are actually + // implemented + ASSERT_TRUE(_surfaceParams.frictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.secondaryFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.rollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.torsionalFrictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.firstFrictionalDirection.has_value()); + ASSERT_TRUE(_surfaceParams.contactSurfaceMotionVelocity.has_value()); + // these constraint parameters are implemented in DART but are not filled + // when the callback is called; they are only read after the callback ends + EXPECT_FALSE(_surfaceParams.errorReductionParameter.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorReductionVelocity.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorAllowance.has_value()); + EXPECT_FALSE(_surfaceParams.constraintForceMixing.has_value()); + + EXPECT_NEAR(_surfaceParams.frictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondaryFrictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.slipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); + + EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 1), + _surfaceParams.firstFrictionalDirection.value(), 1e-6)); + + EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 0), + _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); + }; + world->AddContactPropertiesCallback("test", contactCallback); +#endif + + // The first step already has contacts, but the contact force due to the + // impact does not match the steady-state force generated by the + // body's weight. + StepWorld(world); + +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(4u, numContactCallbackCalls); +#endif + + // After a second step, the contact force reaches steady-state + StepWorld(world); + +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(8u, numContactCallbackCalls); +#endif + + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + + for (auto &contact : contacts) + { + checkContact(contact, false); + } + +#ifdef DART_HAS_CONTACT_SURFACE + // removing a non-existing callback yields no error but returns false + EXPECT_FALSE(world->RemoveContactPropertiesCallback("foo")); + + // removing an existing callback works and the callback is no longer called + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); + + // Third step + StepWorld(world); + + // Number of callback calls is the same as after the 2nd call + EXPECT_EQ(8u, numContactCallbackCalls); + + // Now we check that changing _surfaceParams inside the contact properties + // callback affects the result of the simulation; we set + // contactSurfaceMotionVelocity to [1,0,0] which accelerates the contact + // points from 0 m/s to 1 m/s in a single simulation step. + + auto contactCallback2 = [&]( + const TestContact& /*_contact*/, + size_t /*_numContactsOnCollision*/, + ContactSurfaceParams& _surfaceParams) + { + numContactCallbackCalls++; + // friction direction is [0,0,1] and contact surface motion velocity uses + // the X value to denote the desired velocity along the friction direction + _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; + }; + world->AddContactPropertiesCallback("test2", contactCallback2); + + numContactCallbackCalls = 0u; + // Fourth step + StepWorld(world); + EXPECT_EQ(4u, numContactCallbackCalls); + + // Adjust the expected forces to account for the added acceleration along Z + forceExpectations = + { + // Contact force expectations are: + // link mass * (gravity + acceleration to 1 m.s^-1 in 1 ms) + {sphere->GetLink(0)->GetShape(0), 0.1 * gravity + 100}, + {sphere->GetLink(1)->GetShape(0), 1.0 * gravity + 999.99}, + {sphere->GetLink(2)->GetShape(0), 2.0 * gravity + 1999.98}, + {sphere->GetLink(3)->GetShape(0), 3.0 * gravity + 2999.97}, + }; + + // Verify that the detected contacts correspond to the adjusted expectations + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + for (auto &contact : contacts) + { + checkContact(contact, false); } + + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test2")); +#endif } } diff --git a/include/ignition/physics/ContactProperties.hh b/include/ignition/physics/ContactProperties.hh new file mode 100644 index 000000000..302e8dfee --- /dev/null +++ b/include/ignition/physics/ContactProperties.hh @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ +#define IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace physics +{ +/// \brief SetContactPropertiesCallbackFeature is a feature for setting the +/// properties of a contact after it is created but before it affects the +/// forward step. +class IGNITION_PHYSICS_VISIBLE SetContactPropertiesCallbackFeature + : public virtual FeatureWithRequirements +{ + /// \brief This struct gets filled by the simulator and contains various + /// properties of a contact joint (surface, constraint). All of the values + /// are optional, which means that they are only filled if the physics engine + /// supports them. Some originally unfilled values may still be processed by + /// the physics engine if they are set - this just means there is no default + /// value for them. + public: template struct ContactSurfaceParams + { + /// \brief Coefficient of friction along the 1st friction direction. + std::optional frictionCoeff; + + /// \brief Coefficient of friction along the 2nd friction direction. + std::optional secondaryFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 1st friction direction. + std::optional rollingFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 2nd friction direction. + std::optional secondaryRollingFrictionCoeff; + + /// \brief Coefficient of torsional friction. + std::optional torsionalFrictionCoeff; + + /// \brief Force-dependent slip coefficient along the 1st friction + /// direction. + std::optional slipCompliance; + + /// \brief Force-dependent slip coefficient along the 2nd friction + /// direction. + std::optional secondarySlipCompliance; + + /// \brief Defines the bounciness of the contact. 0 is not bouncy. Values + /// between 0 and 1 are allowed. + std::optional restitutionCoeff; + + /// \brief The first frictional direction. It should be perpendicular to the + /// contact normal. The second frictional direction can be computed as a + /// vector perpendicular both to the normal and to the first direction. + std::optional::template Use> + firstFrictionalDirection; + + /// \brief Desired velocity of the colliding bodies in the contact point. + /// Setting this to non-zero asks the physics engine to add such forces + /// that can achieve that the colliding bodies have the specified velocity. + /// The X component specifies velocity along 1st friction direction. + /// The Y component specifies velocity along 2nd friction direction. + /// The Z component specifies velocity along the contact normal. + std::optional::template Use> + contactSurfaceMotionVelocity; + + /// \brief Joint error reduction parameter. This is the fraction of the + /// joint error that will be attempted to be corrected in each simulation + /// step. Allowed values are 0 to 1. Default is usually somewhere between. + std::optional errorReductionParameter; + + /// \brief Maximum velocity that can be used to reduce joint error. + std::optional maxErrorReductionVelocity; + + /// \brief Maximum joint error for which no error reduction is performed. + std::optional maxErrorAllowance; + + /// \brief Constraint force mixing. This should be a non-negative number. + /// If greater than 0, this number is added to the diagonal of the system + /// matrix making the contact softer and the solution more stable. + std::optional constraintForceMixing; + }; + + public: template + class World : public virtual Feature::World + { + public: using ShapePtrType = typename GetContactsFromLastStepFeature + ::World::ShapePtrType; + + /// \brief This callback is called for every detected contact point and + /// allows customizing properties of the contact surface. + /// \param _contact[in] The contact object containing contact point, + /// normal, force etc. Please note that the force will + /// be always zero because the forward step has not yet + /// been run to compute the force. + /// \param _numContactsOnCollision[in] Number of contact points on the same + /// collision object. This can be used + /// e.g. for force normalization. + /// \param _surfaceParams[in,out] Parameters of the contact surface. They + /// are pre-filled by the physics engine and + /// the callback can alter them. + public: typedef std::function< + void( + const typename GetContactsFromLastStepFeature:: + World::Contact& /*_contact*/, + size_t /*_numContactsOnCollision*/, + ContactSurfaceParams& /*_surfaceParams*/) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: void AddContactPropertiesCallback( + const std::string &_callbackID, SurfaceParamsCallback _callback); + + /// \brief Remove the callback. + public: bool RemoveContactPropertiesCallback( + const std::string &_callbackID); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using ContactImpl = typename GetContactsFromLastStepFeature + ::Implementation::ContactInternal; + + public: typedef std::function< + void(const ContactImpl&, size_t, ContactSurfaceParams&) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: virtual void AddContactPropertiesCallback( + const Identity &_worldID, + const std::string &_callbackID, + SurfaceParamsCallback _callback) = 0; + + /// \brief Remove the callback. + public: virtual bool RemoveContactPropertiesCallback( + const Identity &_worldID, const std::string &_callbackID) = 0; + }; +}; + +} +} + +#include "ignition/physics/detail/ContactProperties.hh" + +#endif /* end of include guard: IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ */ diff --git a/include/ignition/physics/detail/ContactProperties.hh b/include/ignition/physics/detail/ContactProperties.hh new file mode 100644 index 000000000..055693ffd --- /dev/null +++ b/include/ignition/physics/detail/ContactProperties.hh @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_PHYSICS_DETAIL_CONTACTPROPERTIES_HH_ +#define IGNITION_PHYSICS_DETAIL_CONTACTPROPERTIES_HH_ + +#include +#include +#include + +#include + +namespace ignition +{ +namespace physics +{ + +///////////////////////////////////////////////// +template +void SetContactPropertiesCallbackFeature::World:: + AddContactPropertiesCallback( + const std::string &_callbackID, + SurfaceParamsCallback _callback) +{ + using Impl = Implementation; + using ContactInternal = typename Impl::ContactImpl; + + auto pimplPtr = this->pimpl; + auto convertContact = [pimplPtr](const ContactInternal& _internal) + { + using ContactPoint = + typename GetContactsFromLastStepFeature::World + ::ContactPoint; + + ContactPoint contactPoint { + ShapePtrType(pimplPtr, _internal.collision1), + ShapePtrType(pimplPtr, _internal.collision2), + _internal.point + }; + + typename GetContactsFromLastStepFeature::World::Contact + contactOutput; + contactOutput.template Get() = std::move(contactPoint); + + using ExtraContactData = + GetContactsFromLastStepFeature::ExtraContactDataT; + + auto *extraContactData = + _internal.extraData.template Query(); + + if (extraContactData) + { + contactOutput.template Get() = + std::move(*extraContactData); + } + + return contactOutput; + }; + + typename Impl::SurfaceParamsCallback callbackInternal = nullptr; + if (_callback) + { + callbackInternal = [_callback, convertContact]( + const typename Impl::ContactImpl &_contact, + const size_t _numContactsOnCollision, + ContactSurfaceParams &_params) + { + _callback(convertContact(_contact), _numContactsOnCollision, _params); + }; + } + + this->template Interface() + ->AddContactPropertiesCallback( + this->identity, _callbackID, callbackInternal); +} + +///////////////////////////////////////////////// +template +bool SetContactPropertiesCallbackFeature::World:: + RemoveContactPropertiesCallback(const std::string& _callbackID) +{ + return this->template Interface()-> + RemoveContactPropertiesCallback(this->identity, _callbackID); +} + +} // namespace physics +} // namespace ignition + +#endif diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 17eb8aaab..aa4de7af4 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -54,7 +54,7 @@ struct TestFeatureList : ignition::physics::FeatureList< using TestWorldPtr = ignition::physics::World3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; -using ContactPoint = ignition::physics::World3d::ContactPoint; +using TestContactPoint = ignition::physics::World3d::ContactPoint; std::unordered_set LoadWorlds( const std::string &_library, @@ -327,7 +327,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) for (auto &contact : contacts) { - const auto &contactPoint = contact.Get(); + const auto &contactPoint = contact.Get(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -375,7 +375,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contactBoxCylinder = 0u; for (auto contact : contacts) { - const auto &contactPoint = contact.Get<::ContactPoint>(); + const auto &contactPoint = contact.Get<::TestContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2);