Merge branch 'gz-physics7' into scpeters/merge_7_main_part2

Changelog.md

@@ -4,6 +4,53 @@
 
 ## Gazebo Physics 7.x
 
+### Gazebo Physics 7.3.0 (2024-06-25)
+
+1. Backport: Add Cone as a collision shape
+    * [Pull request #639](https://github.com/gazebosim/gz-physics/pull/639)
+
+1. [featherstone] Publish JointFeedback forces.
+    * [Pull request #628](https://github.com/gazebosim/gz-physics/pull/628)
+
+1. Parse voxel resolution when decomposing meshes
+    * [Pull request #655](https://github.com/gazebosim/gz-physics/pull/655)
+
+1. bullet-featherstone: Fix attaching fixed joint between models with inertial pose offset
+    * [Pull request #653](https://github.com/gazebosim/gz-physics/pull/653)
+
+1. Ray intersection simulation feature
+    * [Pull request #641](https://github.com/gazebosim/gz-physics/pull/641)
+
+1. bullet-featherstone: Fix bounding box for collisions with pose offset
+    * [Pull request #647](https://github.com/gazebosim/gz-physics/pull/647)
+
+1. bullet-featherstone: Update fixed constraint behavior
+    * [Pull request #632](https://github.com/gazebosim/gz-physics/pull/632)
+
+1. Update InspectFeatures.hh
+    * [Pull request #645](https://github.com/gazebosim/gz-physics/pull/645)
+
+1. bullet-featherstone: Fix convex hull shape's AABB
+    * [Pull request #637](https://github.com/gazebosim/gz-physics/pull/637)
+
+1. Add package.xml
+    * [Pull request #608](https://github.com/gazebosim/gz-physics/pull/608)
+
+1. bullet-featurestore: Enable auto deactivation
+    * [Pull request #630](https://github.com/gazebosim/gz-physics/pull/630)
+
+1. bullet-featherstone: Support convex decomposition for meshes
+    * [Pull request #606](https://github.com/gazebosim/gz-physics/pull/606)
+
+1. backport bullet-featherstone solver iters
+    * [Pull request #619](https://github.com/gazebosim/gz-physics/pull/619)
+
+1. bullet-featherstone: fix SetWorldPose with off-diagonal moment of inertia
+    * [Pull request #623](https://github.com/gazebosim/gz-physics/pull/623)
+
+1. Revert "Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS (#613)"
+    * [Pull request #615](https://github.com/gazebosim/gz-physics/pull/615)
+
 ### Gazebo Physics 7.2.0 (2024-04-10)
 
 1. Use relative install paths for plugin shared libraries

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,15 @@ struct JointInfo
   Identity model;
   // This field gets set by AddJoint
   std::size_t indexInGzModel = 0;
-  btScalar effort = 0;
+
+  // joint limits
+  // \todo(iche033) Extend to support joints with more than 1 dof
+  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

@@ -18,6 +18,8 @@
 #include "JointFeatures.hh"
 
 #include <algorithm>
+#include <cmath>
+#include <cstddef>
 #include <memory>
 #include <unordered_map>
 
@@ -28,6 +30,29 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+void recreateJointLimitConstraint(JointInfo *_jointInfo, ModelInfo *_modelInfo,
+    WorldInfo *_worldInfo)
+{
+  const auto *identifier = std::get_if<InternalJoint>(&_jointInfo->identifier);
+  if (!identifier)
+    return;
+
+  if (_jointInfo->jointLimits)
+  {
+    _worldInfo->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));
+
+  _worldInfo->world->addMultiBodyConstraint(_jointInfo->jointLimits.get());
+}
+
 /////////////////////////////////////////////////
 void makeColliderStatic(LinkInfo *_linkInfo)
 {
@@ -158,18 +183,59 @@ double JointFeatures::GetJointAcceleration(
 
 /////////////////////////////////////////////////
 double JointFeatures::GetJointForce(
-    const Identity &_id, const std::size_t _dof) const
+    const Identity &_id, const std::size_t /*_dof*/) const
 {
+  double results = gz::math::NAN_D;
   const auto *joint = this->ReferenceInterface<JointInfo>(_id);
   const auto *identifier = std::get_if<InternalJoint>(&joint->identifier);
+
   if (identifier)
   {
     const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-    return model->body->getJointTorqueMultiDof(
-          identifier->indexInBtModel)[_dof];
+    auto feedback = model->body->getLink(
+      identifier->indexInBtModel).m_jointFeedback;
+    const auto &link = model->body->getLink(identifier->indexInBtModel);
+    results = 0.0;
+    if (link.m_jointType == btMultibodyLink::eRevolute)
+    {
+      // According to the documentation in btMultibodyLink.h,
+      // m_axesTop[0] is the joint axis for revolute joints.
+      Eigen::Vector3d axis = convert(link.getAxisTop(0));
+      math::Vector3d axis_converted(axis[0], axis[1], axis[2]);
+      btVector3 angular = feedback->m_reactionForces.getAngular();
+      math::Vector3d angularTorque(
+        angular.getX(),
+        angular.getY(),
+        angular.getZ());
+      results += axis_converted.Dot(angularTorque);
+      #if BT_BULLET_VERSION < 326
+        // not always true
+        return results / 2.0;
+      #else
+        return results;
+      #endif
+    }
+    else if (link.m_jointType == btMultibodyLink::ePrismatic)
+    {
+      auto axis = convert(link.getAxisBottom(0));
+      math::Vector3d axis_converted(axis[0], axis[1], axis[2]);
+      btVector3 linear = feedback->m_reactionForces.getLinear();
+      math::Vector3d linearForce(
+        linear.getX(),
+        linear.getY(),
+        linear.getZ());
+      results += axis_converted.Dot(linearForce);
+      #if BT_BULLET_VERSION < 326
+        // Not always true see for reference:
+        // https://github.com/bulletphysics/bullet3/discussions/3713
+        // https://github.com/gazebosim/gz-physics/issues/565
+        return results / 2.0;
+      #else
+        return results;
+      #endif
+    }
   }
-
-  return gz::math::NAN_D;
+  return results;
 }
 
 /////////////////////////////////////////////////
@@ -236,8 +302,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();
 }
 
@@ -368,20 +439,153 @@ void JointFeatures::SetJointVelocityCommand(
   auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
   if (!jointInfo->motor)
   {
+    auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+    // \todo(iche033) The motor constraint is created with a max impulse
+    // computed by maxEffort * stepsize. However, our API supports
+    // stepping sim with varying dt. We should recompute max impulse
+    // if stepSize changes.
     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;
+  }
+  jointInfo->axisLower = _value;
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+  auto *worldInfo = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+  recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo);
+}
+
+/////////////////////////////////////////////////
+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;
+  }
+
+  jointInfo->axisUpper = _value;
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+  auto *worldInfo = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+  recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo);
+}
+
+/////////////////////////////////////////////////
+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;
+  }
+
+  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,