review fixes

Author: Fixit-Davide

bullet-featherstone/src/JointFeatures.cc

@@ -84,12 +84,14 @@ double JointFeatures::GetJointForce(
   const auto *joint = this->ReferenceInterface<JointInfo>(_id);
   const auto *identifier = std::get_if<InternalJoint>(&joint->identifier);
 
-  if (identifier) {
+  if (identifier) 
+  {
     const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
     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) {
+    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));
@@ -99,11 +101,16 @@ double JointFeatures::GetJointForce(
         angular.getX(),
         angular.getY(),
         angular.getZ());
-      // BUG: The total force is 2 times the cmd one see:
-      // https://github.com/bulletphysics/bullet3/discussions/3713
       results += axis_converted.Dot(angularTorque);
-      return results / 2.0;
-    } else if (link.m_jointType == btMultibodyLink::ePrismatic) {
+      #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();
@@ -112,7 +119,12 @@ double JointFeatures::GetJointForce(
         linear.getY(),
         linear.getZ());
       results += axis_converted.Dot(linearForce);
-      return results / 2.0;
+      #if BT_BULLET_VERSION < 326
+        // not always true
+        return results / 2.0;
+      #else
+        return results;
+      #endif
     }
   }
   return results;