From 3f72282dcb23a11157110560712b077f83c4e18f Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Fri, 19 Apr 2024 11:53:03 +0200 Subject: [PATCH 1/4] [featherstone] Publish joint feedback forces. Signed-off-by: Davide Graziato --- bullet-featherstone/src/JointFeatures.cc | 38 ++++++++++++++++++++---- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 980313a7b..9ea877a00 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -80,16 +80,42 @@ double JointFeatures::GetJointAcceleration( double JointFeatures::GetJointForce( 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) - { + + 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()); + // 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) { + 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); + return results / 2.0; + } } - - return gz::math::NAN_D; + return results; } ///////////////////////////////////////////////// From 1019f6e6f0ace5aa1edbd133c31310421e6ae419 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Tue, 23 Apr 2024 11:09:14 +0200 Subject: [PATCH 2/4] review fixes Signed-off-by: Davide Graziato --- 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(_id); const auto *identifier = std::get_if(&joint->identifier); - if (identifier) { + if (identifier) + { const auto *model = this->ReferenceInterface(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; From ff02286648b22486c73e23298303299ea9374547 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Thu, 2 May 2024 09:33:16 +0200 Subject: [PATCH 3/4] fixed linting Signed-off-by: Davide Graziato --- bullet-featherstone/src/JointFeatures.cc | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 594c08f0a..0a2f44adb 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -84,16 +84,17 @@ double JointFeatures::GetJointForce( const auto *joint = this->ReferenceInterface(_id); const auto *identifier = std::get_if(&joint->identifier); - if (identifier) + if (identifier) { const auto *model = this->ReferenceInterface(joint->model); - auto feedback = model->body->getLink(identifier->indexInBtModel).m_jointFeedback; + 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. + // 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(); @@ -120,7 +121,9 @@ double JointFeatures::GetJointForce( linear.getZ()); results += axis_converted.Dot(linearForce); #if BT_BULLET_VERSION < 326 - // not always true + // 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; From 20ed31f50ecc5f1bd8dba0be4a324472995c0619 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 7 May 2024 02:35:34 +0000 Subject: [PATCH 4/4] fix compile warning Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index f7d20e49d..c73021a01 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -78,7 +78,7 @@ 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);