5 ➡️ 6 (main)

Changelog.md

@@ -7,6 +7,26 @@
 
 ### Ignition Physics 5.x.x (20XX-XX-XX)
 
+### Ignition Physics 5.1.0 (2021-11-12)
+
+1. Remove unused ign_auto_headers.hh.in
+    * [Pull request #305](https://github.com/ignitionrobotics/ign-physics/pull/305)
+
+1. Added DART feature for setting joint limits dynamically.
+    * [Pull request #260](https://github.com/ignitionrobotics/ign-physics/pull/260)
+
+1. Allow customization of contact surface properties
+    * [Pull request #267](https://github.com/ignitionrobotics/ign-physics/pull/267)
+
+1. Avoid `auto` usage in Eigen expressions.
+    * [Pull request #301](https://github.com/ignitionrobotics/ign-physics/pull/301)
+
+1. Fix cmake script for bullet
+    * [Pull request #297](https://github.com/ignitionrobotics/ign-physics/pull/297)
+
+1. Improved README.md folder structure
+    * [Pull request #295](https://github.com/ignitionrobotics/ign-physics/pull/295)
+
 ### Ignition Physics 5.0.0 (2021-09-30)
 
 1. Add GetJointTransmittedWrench feature
@@ -274,6 +294,34 @@
 
 ### Ignition Physics 2.x.x (20XX-XX-XX)
 
+### Ignition Physics 2.5.0 (2021-11-09)
+
+1. Remove unused ign_auto_headers.hh.in
+    * [Pull request #305](https://github.com/ignitionrobotics/ign-physics/pull/305)
+
+1. Added DART feature for setting joint limits dynamically.
+    * [Pull request #260](https://github.com/ignitionrobotics/ign-physics/pull/260)
+
+1. Allow customization of contact surface properties
+    * [Pull request #267](https://github.com/ignitionrobotics/ign-physics/pull/267)
+
+1. [dartsim] Add support for joint frame semantics
+    * [Pull request #288](https://github.com/ignitionrobotics/ign-physics/pull/288)
+
+1. Use slip compliance API's available in upstream dartsim release
+    * [Pull request #265](https://github.com/ignitionrobotics/ign-physics/pull/265)
+
+1. Fix dart deprecation warning
+    * [Pull request #263](https://github.com/ignitionrobotics/ign-physics/pull/263)
+
+1. [Citadel] Update tutorials
+    * [Pull request #204](https://github.com/ignitionrobotics/ign-physics/pull/204)
+
+1. Infrastructure
+    * [Pull request #257](https://github.com/ignitionrobotics/ign-physics/pull/257)
+    * [Pull request #281](https://github.com/ignitionrobotics/ign-physics/pull/281)
+    * [Pull request #287](https://github.com/ignitionrobotics/ign-physics/pull/287)
+
 ### Ignition Physics 2.4.0 (2021-04-14)
 
 1. [TPE] Update link pose and velocity

bullet/CMakeLists.txt

@@ -34,7 +34,7 @@ set(versioned ${CMAKE_SHARED_LIBRARY_PREFIX}${bullet_plugin}${CMAKE_SHARED_LIBRA
 set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${engine_name}${CMAKE_SHARED_LIBRARY_SUFFIX})
 if (WIN32)
   # disable MSVC inherit via dominance warning
-  target_compile_options(${dartsim_plugin} PUBLIC "/wd4250")
+  target_compile_options(${bullet_plugin} PUBLIC "/wd4250")
   INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy
       ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${versioned}
       ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")

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
@@ -38,6 +37,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})
 
@@ -77,6 +96,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,11 +15,19 @@
  *
 */
 
+#include <memory>
+#include <string>
 #include <unordered_map>
 #include <utility>
 
+
 #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 <ignition/common/Profiler.hh>
 
@@ -111,41 +119,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
+
 }
 }
 }