[featherstone] Publish JointFeedback forces. (#628)

Signed-off-by: Davide Graziato <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Ian Chen <[email protected]>

Author: Fixit-Davide

bullet-featherstone/src/JointFeatures.cc

@@ -158,18 +158,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::Vector3 axis_converted(axis[0], axis[1], axis[2]);
+      btVector3 angular = feedback->m_reactionForces.getAngular();
+      math::Vector3<double> 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::Vector3 axis_converted(axis[0], axis[1], axis[2]);
+      btVector3 linear = feedback->m_reactionForces.getLinear();
+      math::Vector3<double> 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;
 }
 
 /////////////////////////////////////////////////