4 ➡️ 5

Signed-off-by: Louise Poubel <[email protected]>

.github/CODEOWNERS

@@ -1,5 +1,4 @@
 # More info:
 # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
 
-*                     @mxgrey
-include/*             @scpeters
+*                     @azeey @mxgrey @scpeters

CMakeLists.txt

@@ -15,7 +15,6 @@ find_package(ignition-cmake2 2.8.0 REQUIRED)
 #============================================================================
 ign_configure_project(VERSION_SUFFIX)
 
-
 #============================================================================
 # Set project-specific options
 #============================================================================

Changelog.md

@@ -127,7 +127,10 @@
 
 ### Ignition Physics 3.x.x (20XX-XX-XX)
 
-### Ignition Physics 3.3.0 (2021-06-18)
+### Ignition Physics 3.3.0 (2021-07-12)
+
+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)

dartsim/src/Base.hh

@@ -72,6 +72,7 @@ struct ModelInfo
 struct JointInfo
 {
   dart::dynamics::JointPtr joint;
+  dart::dynamics::SimpleFramePtr frame;
 };
 
 struct ShapeInfo
@@ -352,6 +353,13 @@ class Base : public Implements3d<FeatureList<Feature>>
     this->joints.idToObject[id] = std::make_shared<JointInfo>();
     this->joints.idToObject[id]->joint = _joint;
     this->joints.objectToID[_joint] = id;
+    dart::dynamics::SimpleFramePtr jointFrame =
+        dart::dynamics::SimpleFrame::createShared(
+            _joint->getChildBodyNode(), _joint->getName() + "_frame",
+            _joint->getTransformFromChildBodyNode());
+
+    this->joints.idToObject[id]->frame = jointFrame;
+    this->frames[id] = this->joints.idToObject[id]->frame.get();
 
     return id;
   }

dartsim/src/JointFeatures.cc

@@ -163,9 +163,167 @@ void JointFeatures::SetJointVelocityCommand(
   {
     joint->setActuatorType(dart::dynamics::Joint::SERVO);
   }
+  // warn about bug https://github.com/dartsim/dart/issues/1583
+  if ((joint->getPositionLowerLimit(_dof) > -1e16 ||
+       joint->getPositionUpperLimit(_dof) < 1e16 ) &&
+      (!std::isfinite(joint->getForceUpperLimit(_dof)) ||
+       !std::isfinite(joint->getForceLowerLimit(_dof))))
+  {
+    static bool informed = false;
+    if (!informed)
+    {
+      ignerr  << "Velocity control does not respect positional limits of "
+              << "joints if these joints do not have an effort limit. Please, "
+              << "set min and max effort for joint [" << joint->getName()
+              << "] to values about -1e6 and 1e6 (or higher if working with "
+              << "heavy links)." << std::endl;
+      informed = true;
+    }
+  }
+
   joint->setCommand(_dof, _value);
 }
 
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinPosition(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid minimum joint position value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+#if DART_VERSION_AT_LEAST(6, 10, 0)
+  joint->setLimitEnforcement(true);
+#else
+  joint->setPositionLimitEnforced(true);
+#endif
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setPositionLowerLimit(_dof, _value);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxPosition(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid maximum joint position value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+#if DART_VERSION_AT_LEAST(6, 10, 0)
+  joint->setLimitEnforcement(true);
+#else
+  joint->setPositionLimitEnforced(true);
+#endif
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setPositionUpperLimit(_dof, _value);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinVelocity(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid minimum joint velocity value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+#if DART_VERSION_AT_LEAST(6, 10, 0)
+  joint->setLimitEnforcement(true);
+#else
+  joint->setPositionLimitEnforced(true);
+#endif
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setVelocityLowerLimit(_dof, _value);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxVelocity(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid maximum joint velocity value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+#if DART_VERSION_AT_LEAST(6, 10, 0)
+  joint->setLimitEnforcement(true);
+#else
+  joint->setPositionLimitEnforced(true);
+#endif
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setVelocityUpperLimit(_dof, _value);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinEffort(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid minimum joint effort value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setForceLowerLimit(_dof, _value);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxEffort(
+    const Identity &_id, const std::size_t _dof, const double _value)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_value))
+  {
+    ignerr << "Invalid maximum joint effort value [" << _value
+           << "] commanded on joint [" << joint->getName() << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+  // We do not check min/max mismatch, we leave that to DART.
+  joint->setForceUpperLimit(_dof, _value);
+}
+
 /////////////////////////////////////////////////
 std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const
 {

dartsim/src/JointFeatures.hh

@@ -52,7 +52,10 @@ struct JointFeatureList : FeatureList<
   GetPrismaticJointProperties,
   AttachPrismaticJointFeature,
 
-  SetJointVelocityCommandFeature
+  SetJointVelocityCommandFeature,
+  SetJointPositionLimitsFeature,
+  SetJointVelocityLimitsFeature,
+  SetJointEffortLimitsFeature
 > { };
 
 class JointFeatures :
@@ -171,6 +174,30 @@ class JointFeatures :
   public: void SetJointVelocityCommand(
       const Identity &_id, const std::size_t _dof,
       const double _value) override;
+
+  public: void SetJointMinPosition(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
+
+  public: void SetJointMaxPosition(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
+
+  public: void SetJointMinVelocity(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
+
+  public: void SetJointMaxVelocity(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
+
+  public: void SetJointMinEffort(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
+
+  public: void SetJointMaxEffort(
+      const Identity &_id, const std::size_t _dof,
+      const double _value) override;
 };
 
 }