bullet-featherstone: Enforce joint velocity and effort limits for velocity control commands

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

@@ -76,6 +76,8 @@ struct WorldInfo
   std::unordered_map<std::string, std::size_t> modelNameToEntityId;
   int nextModelIndex = 0;
 
+  double stepSize = 0.001;
+
   explicit WorldInfo(std::string name);
 };
 
@@ -189,7 +191,14 @@ struct JointInfo
   Identity model;
   // This field gets set by AddJoint
   std::size_t indexInGzModel = 0;
-  btScalar effort = 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();
 }
 
@@ -409,20 +414,183 @@ void JointFeatures::SetJointVelocityCommand(
   auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
   if (!jointInfo->motor)
   {
+    auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
     jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
       modelInfo->body.get(),
       std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
       0,
       static_cast<btScalar>(0),
-      static_cast<btScalar>(jointInfo->effort));
-    auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+      static_cast<btScalar>(jointInfo->maxEffort * world->stepSize));
     world->world->addMultiBodyConstraint(jointInfo->motor.get());
   }
 
-  jointInfo->motor->setVelocityTarget(static_cast<btScalar>(_value));
+  // clamp the values
+  double velocity = std::clamp(_value,
+      jointInfo->minVelocity, jointInfo->maxVelocity);
+
+  jointInfo->motor->setVelocityTarget(static_cast<btScalar>(velocity));
   modelInfo->body->wakeUp();
 }
 
+/////////////////////////////////////////////////
+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());
+}
+
 /////////////////////////////////////////////////
 Identity JointFeatures::AttachFixedJoint(
     const Identity &_childID,

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,6 +89,12 @@ Identity SDFFeatures::ConstructSdfWorld(
 
   worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity()));
 
+  const ::sdf::Physics *physics = _sdfWorld.PhysicsByIndex(0);
+  if (physics)
+    worldInfo->stepSize = physics->MaxStepSize();
+  else
+    worldInfo->stepSize = 0.001;
+
   for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i)
   {
     const ::sdf::Model *model = _sdfWorld.ModelByIndex(i);
@@ -769,6 +776,10 @@ Identity SDFFeatures::ConstructSdfModelImpl(
       if (::sdf::JointType::PRISMATIC == joint->Type() ||
           ::sdf::JointType::REVOLUTE == joint->Type())
       {
+        // Note: These m_joint* properties below are currently not supported by
+        // bullet-featherstone and so setting them does not have any effect.
+        // The lower and uppper limit is supported via the
+        // btMultiBodyJointLimitConstraint.
         model->body->getLink(i).m_jointLowerLimit =
             static_cast<btScalar>(joint->Axis()->Lower());
         model->body->getLink(i).m_jointUpperLimit =
@@ -781,7 +792,13 @@ Identity SDFFeatures::ConstructSdfModelImpl(
             static_cast<btScalar>(joint->Axis()->MaxVelocity());
         model->body->getLink(i).m_jointMaxForce =
             static_cast<btScalar>(joint->Axis()->Effort());
-        jointInfo->effort = static_cast<btScalar>(joint->Axis()->Effort());
+
+        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>(

bullet-featherstone/src/SimulationFeatures.cc

@@ -37,14 +37,15 @@ void SimulationFeatures::WorldForwardStep(
   const auto worldInfo = this->ReferenceInterface<WorldInfo>(_worldID);
   auto *dtDur =
     _u.Query<std::chrono::steady_clock::duration>();
+  double stepSize = 0.001;
   if (dtDur)
   {
     std::chrono::duration<double> dt = *dtDur;
     stepSize = dt.count();
   }
 
-  worldInfo->world->stepSimulation(static_cast<btScalar>(this->stepSize), 1,
-                                   static_cast<btScalar>(this->stepSize));
+  worldInfo->world->stepSimulation(static_cast<btScalar>(stepSize), 1,
+                                   static_cast<btScalar>(stepSize));
 
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());

bullet-featherstone/src/SimulationFeatures.hh

@@ -58,8 +58,6 @@ class SimulationFeatures :
   public: std::vector<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;
 
-  private: double stepSize = 0.001;
-
   /// \brief link poses from the most recent pose change/update.
   /// The key is the link's ID, and the value is the link's pose
   private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;