4 ➡️ 5 (main) (#292)

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:/ignitionrobotics/ign-physics/pull/265)
 
 1. Fix DART deprecation warning
  * [Pull request #263](https:/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:/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;
 };
 
 }