diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index e2c460a32..4ead27ed3 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/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(_id); const auto *identifier = std::get_if(&joint->identifier); + if (identifier) { const auto *model = this->ReferenceInterface(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 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 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; } /////////////////////////////////////////////////