Added DART feature for setting joint limits dynamically. (#260)

* Added DART feature for setting joint limits dynamically. Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz> * Make code compatible with older DART release (this commit can be reverted once newer DART is out). Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz> * Updated the number of models for test. Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz> * Apply suggestions from code review Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz> Co-authored-by: Addisu Z. Taddese <addisu@gmail.com> * Fixed issues from review. Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz> * Accomodate DART 6.9 Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Addisu Z. Taddese <addisu@gmail.com> Co-authored-by: Steve Peters <scpeters@openrobotics.org>
gazebosim · Jul 22, 2021 · 55a4901 · 55a4901
1 parent c788a0d
commit 55a4901
Show file tree

Hide file tree

Showing 7 changed files with 909 additions and 4 deletions.
diff --git a/dartsim/src/JointFeatures.cc b/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
 {

diff --git a/dartsim/src/JointFeatures.hh b/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;
 };
 
 }