Add GetJointTransmittedWrench feature

[azeey]

🎉 New feature

Relates to #16, #124, and #143

Summary

This adds a new feature that exposes the transmitted forces on a joint, which are needed for creating a Force/Torque sensor. The computation of the wrench is similar to what's done in Gazebo-classic, but in Gazebo-classic, the wrench is expressed in the child body frame, but applied at the joint origin. Here, the wrench is expressed in the joint frame and applied at the joint origin.

This also adds a new quantity Wrench, which is a stacking of Angular and Linear vectors.

TODO

• Create a RelativeQuantity that corresponds to Wrench
• [ ] Ensure that applied force is not included in the computed wrench.

Test it

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[codecov]

Codecov Report

Merging #283 (42a5613) into main (45b52a4) will increase coverage by 0.02%.
The diff coverage is 65.38%.

@@            Coverage Diff             @@
##             main     #283      +/-   ##
==========================================
+ Coverage   75.52%   75.54%   +0.02%     
==========================================
  Files         127      127              
  Lines        5597     5655      +58     
==========================================
+ Hits         4227     4272      +45     
- Misses       1370     1383      +13     

Impacted Files	Coverage Δ
include/ignition/physics/Geometry.hh	100.00% <ø> (ø)
...nclude/ignition/physics/detail/RelativeQuantity.hh	91.86% <54.83%> (-8.14%)	⬇️
dartsim/src/JointFeatures.cc	64.83% <73.33%> (+1.26%)	⬆️
include/ignition/physics/detail/Joint.hh	100.00% <100.00%> (ø)
include/ignition/physics/detail/GetEntities.hh	100.00% <0.00%> (ø)
dartsim/src/EntityManagementFeatures.cc	78.47% <0.00%> (+0.99%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 45b52a4...42a5613. Read the comment docs.

[scpeters]

is there an advantage to using a single vector over std::pair<LinearVector, AngularVector> or a struct? I would have trouble remembering if the linear or angular part of the wrench came first.

[azeey]

Switched to a struct in 1bd5c00

[traversaro]

This adds a new feature that exposes the constraint forces on a joint, which are needed for creating a Force/Torque sensor.

Just to understand, why is this needed to create a Force/Torque sensor? Most simulators already directly expose the joint force/torque that the child joint is applying to the parent joint, already including the constraint forces, the applied forces and anything else that could be necessary. I am probably missing something, but I don't see the advantage of computing/reading from the physics engine the "motor" joint force, the constraint force/torque and then adding them back at high level when most physics engines already provide directly this info.

[azeey]

The current API Joint::GetForce returns a force in the DOF of the joint (even though it's currently not working correctly for dartsim as mentioned in #143). It would not give you the force in the constraint directions. Aside from that, I think it's beneficial to have a separate Feature for this because it allows physics engines that don't expose their constraint forces to opt out of the feature.

When thinking of joint force/torque sensing, two applications come to mind: force/impedance control and 6 axis Force/Torque sensor. For the first one, for example we'd use revolute joints and would want to know the "measured" joint torque in the DOF of the joint. For this, I think the Joint::GetForce API makes sense. For the second application, we'd typically use a fixed joint and measure the constraint forces and torques in directions. This is where we'd use Joint::GetConstraintWrench.

I am probably missing something, but I don't see the advantage of computing/reading from the physics engine the "motor" joint force, the constraint force/torque and then adding them back at high level when most physics engines already provide directly this info.

I'm not sure I understand when you say "adding them back at high level". Are you saying users would want the two results added together?

[traversaro]

I am probably missing something, but I don't see the advantage of computing/reading from the physics engine the "motor" joint force, the constraint force/torque and then adding them back at high level when most physics engines already provide directly this info.

I'm not sure I understand when you say "adding them back at high level". Are you saying users would want the two results added together?

Sorry, I definitely was not clear. If we want to implement the same semantics described in the last section of http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors, the FT sensor that is mounted so close to the revolute joint so that it is possible to ignore the flange and avoid to model it as a fixed joint, and can be modeled directly as a FT sensor on the revolute joint, is able to read the Force/Torque that is caused by two causes:

• The Force/Torque created by the constraint in the joint, that is the one that you can read from getBodyConstraintWrench
• If the joint is not fixed, also by the Force/Torque created by the actuation/motor, i.e. the \tau_control from [dart] Compute Inverse Dynamics for all skeletons to populate joint forces #143 (comment)

If you need to simulate a FT sensor attached to a revolute joint, you need to sum this two contributions. However, I think that alternativly we can just expose getBodyForce that is a quantity that we are looking for (and that for fixed joints should match getBodyForce), so that no higher level processing is necessary. Furthermore, that quantity is a byproduct of most forward dynamics algorithms, so most physics engine should expose it.

[azeey]

I've used the term TransmittedWrench in this PR pere @traversaro's suggestion, but I'm open to suggestions if that doesn't seem like a suitable name.

[scpeters]

I'm still reviewing the test but have some suggestions for the API documentation in the meantime

[azeey]

Retargetted to main. @mjcarroll PTAL

[scpeters]

still reviewing the test, will get back to it after my next meeting

[scpeters]

I would expect 3 spheres to be more stable than 4 with planar contact, but this is fine

[scpeters]

I updated the sign convention and some test comments in 7bd1556

[scpeters]

I updated the sign convention and some test comments in 7bd1556

one final set of fixes in 42a5613

[azeey]

Thanks for those fixes @scpeters!