From 1019f6e6f0ace5aa1edbd133c31310421e6ae419 Mon Sep 17 00:00:00 2001
From: Davide Graziato <dgraziato10@gmail.com>
Date: Tue, 23 Apr 2024 11:09:14 +0200
Subject: [PATCH] review fixes

Signed-off-by: Davide Graziato <dgraziato10@gmail.com>
---
 bullet-featherstone/src/JointFeatures.cc | 26 +++++++++++++++++-------
 1 file changed, 19 insertions(+), 7 deletions(-)

diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
index 9ea877a00..594c08f0a 100644
--- a/bullet-featherstone/src/JointFeatures.cc
+++ b/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;