Allow customization of contact surface properties

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()

dartsim/src/SimulationFeatures.cc

@@ -15,8 +15,17 @@
  *
 */
 
+#include <memory>
+#include <string>
+#include <unordered_map>
+
 #include <dart/collision/CollisionObject.hpp>
 #include <dart/collision/CollisionResult.hpp>
+#include <dart/constraint/ConstraintSolver.hpp>
+#include <dart/constraint/ContactConstraint.hpp>
+#ifdef DART_HAS_CONTACT_SURFACE
+#include <dart/constraint/ContactSurface.hpp>
+#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::ContactInternal>
+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<SimulationFeatures::ExtraContactData>();
+    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<DartWorld>(_worldID);
+
+  auto handler = std::make_shared<IgnContactSurfaceHandler>();
+  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<DartWorld>(_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<P> 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>();
-      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>& 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
+
 }
 }
 }

dartsim/src/SimulationFeatures.hh

@@ -18,25 +18,80 @@
 #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_
 #define IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_
 
+#include <memory>
+#include <string>
+#include <unordered_map>
 #include <vector>
+
+#include <dart/config.hpp>
+#ifdef DART_HAS_CONTACT_SURFACE
+#include <dart/constraint/ContactSurface.hpp>
+#endif
+
 #include <ignition/physics/ForwardStep.hh>
 #include <ignition/physics/GetContacts.hh>
+#include <ignition/physics/ContactProperties.hh>
 
 #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<FeaturePolicy3d> Impl;
+
+  public: Impl::SurfaceParamsCallback surfaceParamsCallback;
+
+  public: std::function<
+  std::optional<Impl::ContactImpl>(const dart::collision::Contact&)
+  > convertContact;
+
+  public: mutable typename Feature::ContactSurfaceParams<FeaturePolicy3d>
+  lastIgnParams;
+};
+
+using IgnContactSurfaceHandlerPtr = std::shared_ptr<IgnContactSurfaceHandler>;
+#endif
+
 class SimulationFeatures :
     public virtual Base,
     public virtual Implements3d<SimulationFeatureList>
 {
+  public: using GetContactsFromLastStepFeature::Implementation<FeaturePolicy3d>
+    ::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<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;
+
+  private: std::optional<ContactInternal> 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
 };
 
 }