Add joint limit features and update test

Signed-off-by: Ian Chen <[email protected]>

bullet-featherstone/README.md

@@ -2,7 +2,7 @@
 
 This component enables the use of this [Bullet Physics](https://github.com/bulletphysics/bullet3) `btMultiBody` Featherstone implementation.
 The Featherstone uses minimal coordinates to represent articulated bodies and efficiently simulate them.
- 
+
 # Features
 
 Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gazebosim/gz-physics/issues/423)
@@ -11,7 +11,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz
 
 * Constructing SDF Models
 * Constructing SDF Worlds
-* Joint Types: 
+* Joint Types:
   * Fixed
   * Prismatic
   * Revolute
@@ -25,7 +25,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz
 
 These are the specific physics API features implemented.
 
-* Entity Management Features 
+* Entity Management Features
   * ConstructEmptyWorldFeature
   * GetEngineInfo
   * GetJointFromModel
@@ -47,6 +47,9 @@ These are the specific physics API features implemented.
   * SetBasicJointState
   * GetBasicJointProperties
   * SetJointVelocityCommandFeature
+  * SetJointPositionLimitsFeature,
+  * SetJointVelocityLimitsFeature,
+  * SetJointEffortLimitsFeature,
   * SetJointTransformFromParentFeature
   * AttachFixedJointFeature
   * DetachJointFeature

bullet-featherstone/src/Base.hh

@@ -191,8 +191,14 @@ struct JointInfo
   Identity model;
   // This field gets set by AddJoint
   std::size_t indexInGzModel = 0;
-  double effortLimit = 0.0;
-  double velocityLimit = 0.0;
+
+  // joint limits
+  double minEffort = 0.0;
+  double maxEffort = 0.0;
+  double minVelocity = 0.0;
+  double maxVelocity = 0.0;
+  double axisLower = 0.0;
+  double axisUpper = 0.0;
 
   std::shared_ptr<btMultiBodyJointMotor> motor = nullptr;
   std::shared_ptr<btMultiBodyJointLimitConstraint> jointLimits = nullptr;

bullet-featherstone/src/JointFeatures.cc

@@ -277,8 +277,13 @@ void JointFeatures::SetJointForce(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
+
+  // clamp the values
+  double force = std::clamp(_value,
+      joint->minEffort, joint->maxEffort);
+
   model->body->getJointTorqueMultiDof(
-    identifier->indexInBtModel)[_dof] = static_cast<btScalar>(_value);
+    identifier->indexInBtModel)[_dof] = static_cast<btScalar>(force);
   model->body->wakeUp();
 }
 
@@ -415,16 +420,177 @@ void JointFeatures::SetJointVelocityCommand(
       std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
       0,
       static_cast<btScalar>(0),
-      static_cast<btScalar>(jointInfo->effortLimit * world->stepSize));
+      static_cast<btScalar>(jointInfo->maxEffort * world->stepSize));
     world->world->addMultiBodyConstraint(jointInfo->motor.get());
   }
 
   // clamp the values
   double velocity = std::clamp(_value,
-      -jointInfo->velocityLimit, jointInfo->velocityLimit);
+      jointInfo->minVelocity, jointInfo->maxVelocity);
 
   jointInfo->motor->setVelocityTarget(static_cast<btScalar>(velocity));
   modelInfo->body->wakeUp();
+
+  this->GetJointVelocity(_id, _dof);
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinPosition(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid minimum joint position value [" << _value
+           << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+  const auto *identifier = std::get_if<InternalJoint>(&jointInfo->identifier);
+  if (!identifier)
+    return;
+
+  jointInfo->axisLower = _value;
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+  auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+
+  if (jointInfo->jointLimits)
+  {
+    world->world->removeMultiBodyConstraint(jointInfo->jointLimits.get());
+    jointInfo->jointLimits.reset();
+  }
+
+  jointInfo->jointLimits =
+    std::make_shared<btMultiBodyJointLimitConstraint>(
+      modelInfo->body.get(), identifier->indexInBtModel,
+      static_cast<btScalar>(jointInfo->axisLower),
+      static_cast<btScalar>(jointInfo->axisUpper));
+
+  world->world->addMultiBodyConstraint(jointInfo->jointLimits.get());
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxPosition(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid maximum joint position value [" << _value
+           << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+           << "]. The command will be ignored\n";
+    return;
+  }
+
+  const auto *identifier = std::get_if<InternalJoint>(&jointInfo->identifier);
+  if (!identifier)
+    return;
+
+  jointInfo->axisUpper = _value;
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+  auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+
+  if (jointInfo->jointLimits)
+  {
+    world->world->removeMultiBodyConstraint(jointInfo->jointLimits.get());
+    jointInfo->jointLimits.reset();
+  }
+
+  jointInfo->jointLimits =
+    std::make_shared<btMultiBodyJointLimitConstraint>(
+      modelInfo->body.get(), identifier->indexInBtModel,
+      static_cast<btScalar>(jointInfo->axisLower),
+      static_cast<btScalar>(jointInfo->axisUpper));
+  world->world->addMultiBodyConstraint(jointInfo->jointLimits.get());
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinVelocity(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid minimum joint velocity value [" << _value
+          << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+          << "]. The command will be ignored\n";
+    return;
+  }
+
+  jointInfo->minVelocity = _value;
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxVelocity(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid maximum joint velocity value [" << _value
+          << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+          << "]. The command will be ignored\n";
+    return;
+  }
+
+  jointInfo->maxVelocity = _value;
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMinEffort(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid minimum joint effort value [" << _value
+          << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+          << "]. The command will be ignored\n";
+
+    return;
+  }
+
+  // min effort is currently unused by bullet-featherstone
+  jointInfo->minEffort = _value;
+}
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMaxEffort(
+    const Identity &_id, std::size_t _dof, double _value)
+{
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (std::isnan(_value))
+  {
+    gzerr << "Invalid maximum joint effort value [" << _value
+          << "] commanded on joint [" << jointInfo->name << " DOF " << _dof
+          << "]. The command will be ignored\n";
+    return;
+  }
+
+  const auto *identifier = std::get_if<InternalJoint>(&jointInfo->identifier);
+  if (!identifier)
+    return;
+
+  jointInfo->maxEffort = _value;
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+  auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+
+  if (jointInfo->motor)
+  {
+    world->world->removeMultiBodyConstraint(jointInfo->motor.get());
+    jointInfo->motor.reset();
+  }
+
+  jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
+    modelInfo->body.get(),
+    std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
+    0,
+    static_cast<btScalar>(0),
+    static_cast<btScalar>(jointInfo->maxEffort * world->stepSize));
+  world->world->addMultiBodyConstraint(jointInfo->motor.get());
 }
 
 /////////////////////////////////////////////////

bullet-featherstone/src/JointFeatures.hh

@@ -37,6 +37,9 @@ struct JointFeatureList : FeatureList<
   GetBasicJointProperties,
 
   SetJointVelocityCommandFeature,
+  SetJointPositionLimitsFeature,
+  SetJointVelocityLimitsFeature,
+  SetJointEffortLimitsFeature,
 
   SetJointTransformFromParentFeature,
   AttachFixedJointFeature,
@@ -127,6 +130,30 @@ class JointFeatures :
     const Identity &_id, const std::size_t _dof,
     const double _value) override;
 
+  public: void SetJointMinPosition(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
+  public: void SetJointMaxPosition(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
+  public: void SetJointMinVelocity(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
+  public: void SetJointMaxVelocity(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
+  public: void SetJointMinEffort(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
+  public: void SetJointMaxEffort(
+      const Identity &_id, std::size_t _dof,
+      double _value) override;
+
   // ----- AttachFixedJointFeature -----
   public: Identity AttachFixedJoint(
       const Identity &_childID,

bullet-featherstone/src/SDFFeatures.cc

@@ -32,6 +32,7 @@
 #include <sdf/JointAxis.hh>
 #include <sdf/Link.hh>
 #include <sdf/Mesh.hh>
+#include <sdf/Physics.hh>
 #include <sdf/Plane.hh>
 #include <sdf/Sphere.hh>
 #include <sdf/Surface.hh>
@@ -88,7 +89,7 @@ Identity SDFFeatures::ConstructSdfWorld(
 
   worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity()));
 
-  ::sdf::Physics *physics = _sdfWorld.PhysicsByIndex(0);
+  const ::sdf::Physics *physics = _sdfWorld.PhysicsByIndex(0);
   if (physics)
     worldInfo->stepSize = physics->MaxStepSize();
   else
@@ -792,8 +793,12 @@ Identity SDFFeatures::ConstructSdfModelImpl(
         model->body->getLink(i).m_jointMaxForce =
             static_cast<btScalar>(joint->Axis()->Effort());
 
-        jointInfo->effortLimit = joint->Axis()->Effort();
-        jointInfo->velocityLimit = joint->Axis()->MaxVelocity();
+        jointInfo->minEffort = -joint->Axis()->Effort();
+        jointInfo->maxEffort = joint->Axis()->Effort();
+        jointInfo->minVelocity = -joint->Axis()->MaxVelocity();
+        jointInfo->maxVelocity = joint->Axis()->MaxVelocity();
+        jointInfo->axisLower = joint->Axis()->Lower();
+        jointInfo->axisUpper = joint->Axis()->Upper();
 
         jointInfo->jointLimits =
           std::make_shared<btMultiBodyJointLimitConstraint>(

test/common_test/joint_features.cc

@@ -254,6 +254,7 @@ TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceContr
       joint->SetForce(0, 100);
       world->Step(output, state, input);
     }
+
     EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3);
 
     for (std::size_t i = 0; i < 100; ++i)
@@ -300,9 +301,6 @@ struct JointFeaturePositionLimitsForceControlList : gz::physics::FeatureList<
     gz::physics::GetEngineInfo,
     gz::physics::GetJointFromModel,
     gz::physics::GetModelFromWorld,
-    // This feature is not requited but it will force to use dart6.10 which is required
-    // to run these tests
-    gz::physics::GetShapeFrictionPyramidSlipCompliance,
     gz::physics::SetBasicJointState,
     gz::physics::SetJointEffortLimitsFeature,
     gz::physics::SetJointPositionLimitsFeature,
@@ -592,11 +590,11 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi
 // TODO(anyone): position limits do not work very well with velocity control
 // bug https://github.com/dartsim/dart/issues/1583
 // resolved in DART 6.11.0
-TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositionLimitsWithVelocityControl)
+TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetPositionLimitsWithVelocityControl)
 {
   for (const std::string &name : this->pluginNames)
   {
-    if(this->PhysicsEngineName(name) != "dartsim")
+    if (this->PhysicsEngineName(name) == "dartsim")
     {
       GTEST_SKIP();
     }
@@ -638,7 +636,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositio
       if (i % 500 == 499)
       {
         EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2);
-        EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6);
+        EXPECT_NEAR(0, joint->GetVelocity(0), 1e-5);
       }
     }
   }
@@ -648,11 +646,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi
 {
   for (const std::string &name : this->pluginNames)
   {
-    if(this->PhysicsEngineName(name) != "dartsim")
-    {
-      GTEST_SKIP();
-    }
-
     std::cout << "Testing plugin: " << name << std::endl;
     gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
 
@@ -719,11 +712,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith
 {
   for (const std::string &name : this->pluginNames)
   {
-    if(this->PhysicsEngineName(name) != "dartsim")
-    {
-      GTEST_SKIP();
-    }
-
     std::cout << "Testing plugin: " << name << std::endl;
     gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
 
@@ -779,11 +767,6 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi
 {
   for (const std::string &name : this->pluginNames)
   {
-    if(this->PhysicsEngineName(name) != "dartsim")
-    {
-      GTEST_SKIP();
-    }
-
     std::cout << "Testing plugin: " << name << std::endl;
     gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);