Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[featherstone] Publish JointFeedback forces. #628

Merged
merged 7 commits into from
Jun 18, 2024
Merged

[featherstone] Publish JointFeedback forces. #628

Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3f72282
[featherstone] Publish joint feedback forces.
Fixit-Davide Apr 19, 2024
1019f6e
review fixes
Fixit-Davide Apr 23, 2024
ff02286
fixed linting
Fixit-Davide May 2, 2024
9aee181
Merge branch 'gz-physics7' into jointFeedback
iche033 May 2, 2024
20ed31f
fix compile warning
iche033 May 7, 2024
dabbbb8
Merge branch 'gz-physics7' into jointFeedback
iche033 May 7, 2024
beaacc7
Merge branch 'gz-physics7' into jointFeedback
iche033 Jun 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 32 additions & 6 deletions bullet-featherstone/src/JointFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<JointInfo>(_id);
const auto *identifier = std::get_if<InternalJoint>(&joint->identifier);
if (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());
// 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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is the results / 2.0 done to workaround the bug? Note that it was recently fixed upstream: bulletphysics/bullet3#4583. I think we also found that total force is not always 2 times than it should be (based on example in #565 and comment: bulletphysics/bullet3#3713 (comment)).

Anyways, best to guard this with a bullet version check e.g. for users using latest bullet version built from source.

#if BT_BULLET_VERSION < 326
  // not sure if this is always true
  return results / 2.0; 
#else
  return results;
#endif

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, the results / 2.0 part is used as an ugly workaround in order to retrieve a data that is reasonable. I didn't know that the PR was merged when I have write the code.
Probably another workaround (for bullet version < 3.26) could be measured_torque = measured_torque - applied_torque ? But I don't know how to retrieve the information about the applied torque from the jointInfo class.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just did some quick tests and saw that bullet clears the applied joint forces and torques after each step so I think we're not able to retrieve the applied torque at this point (e.g. model->body->getJointTorqueMultiDof just returns 0). We could cache the applied torque in JointInfo struct but we would need to clear them before every step. For now, I think we can just add a comment about this bug affecting bullet versions < 326 - basically what you had before, a link to bulletphysics/bullet3#3713 or #565.

} 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);
return results / 2.0;
}
}

return gz::math::NAN_D;
return results;
}

/////////////////////////////////////////////////
Expand Down