Add AddLinkForce Function to Apply Forces in Link Frame #2816

Vedant87 · 2025-03-07T16:12:04Z

🎉 New feature

Summary

Function added: to apply force relative to the center of mass of the link frame, the function converts the force to a force with respect to the world coordinates. It uses the AddWorldForce method to apply force either at the center of mass of the link or at an offset from the center of mass, based on user input.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if the library has them)
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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

iche033 · 2025-03-07T19:13:17Z

The CI picked up some lint issues - can you remove trailing whitespace and limit lines to 80 char? I also made some minor comments on removing empty lines.

.gitignore

src/Link.cc

iche033 · 2025-03-07T19:12:01Z

include/gz/sim/Link.hh

+      /// and applied at the center of mass of the link.
+      /// \param[in] _ecm Mutable Entity-component manager.
+      /// \param[in] _force Force to be applied expressed in link's center of mass coordinates
+      public: void AddLinkForce(EntityComponentManager &_ecm,


Maybe just AddForce, which is more consistent with gazebo-classic's API. Do you have any preference on the API name, @scpeters?.

friendly ping @scpeters

given that the other functions in this class do not have the Link string in the name, e.g. SetLinearVelocity (instead of SetLinkLinearVelocity), I think we should rename this to AddForce to be consistent.

I was thinking to use the following naming convention

The function which applies force directly at the COM, can be named either AddForce or AddRelativeForce

The function which applies force at an offset from the COM, can be named AddLinkForce.

This naming convention is consistent with gazebo-classic's API.

apologies for the delayed response; this is complicated and I wrote an overly long response, which I will post as a separate comment to this PR

/// \param[in] _force Force to be applied expressed in link's
/// center of mass coordinates

by center of mass coordinates, do you mean the coordinates of the inertial frame defined by the SDFormat //link/inertial/pose offset from the link frame?

If so, you could name this method AddInertialForce or AddForceInInertialFrame or AddForceRelativeToInertialFrame

Signed-off-by: Vedant87 <[email protected]>

… to 80 characters Signed-off-by: Vedant87 <[email protected]>

.gitignore

src/Link.cc

arjo129 · 2025-03-11T07:04:57Z

Once you decide on the naming, it'll also be good to update the python API.

Signed-off-by: Vedant87 <[email protected]>

arjo129 · 2025-03-13T00:22:19Z

@vedant do address @iche033's comment and add python bndngs.

scpeters · 2025-03-13T06:27:28Z

Maybe just AddForce, which is more consistent with gazebo-classic's API. Do you have any preference on the API name, @scpeters?.

Sorry for the delay; this is complicated and it took me a while to collect my thoughts.
The Vector3 force parameter requires two pieces of context in order to interpret it:

the location on the Link where it acts
the coordinates in which the force Vector components are expressed

Some references for naming these quantities:

In the gz-physics FrameSemantics API, an overload of the Resolve function accepts parameters _relativeTo and _inCoordinatesOf that identify the frames corresponding to these quantities
In SDFormat, the //axis/xyz element has an @expressed_in attribute that can specify the "Name of frame in whose coordinates the xyz unit vector is expressed"

The gz::sim::Link class already has two overloads of the AddWorldForce() method:

Accepting a single Vector3 force parameter (see Link.hh:319-324):
- the force vector is "expressed-in-coordinates-of" the world frame and applied "relative-to" the link center-of-mass
Accepting two Vector3 parameters: force and position (see Link.hh:326-334)
- the position vector specifies "The point of application of the force expressed in the link-fixed frame." I assume this means that the position vector is "relative-to" the link frame origin and "expressed-in-coordinates-of" the link frame.
- the force vector is "expressed-in-coordinates-of" the world frame and applied "relative-to" the point specified by the position parameter

I suppose the "World" in the "AddWorldForce" method name refers to the "expressed-in-coordinates-of" frame for the force vector, but it seems inconsistent to me that the position parameter is "expressed-in-coordinates-of" the link frame. It also seems inconsistent that for one overload, the force is applied "relative-to" the link center-of-mass, but in the other overload it is applied "relative-to" an offset from the link-fixed frame.

In gazebo-classic, the gazebo::physics::Link class has the following APIs with varying levels of documented behavior in the header file:

AddForce: relative-to and expressed-in-coordinates-of not documented in the header
AddRelativeForce: relative-to not documented, expressed-in-coordinates-of "body's own frame of reference"
AddForceAtWorldPosition: the force's expressed-in-coordinates-of frame is not documented, but the force is applied relative-to a position specified in the "global coordinate frame" (aka world).
AddForceAtRelativePosition: the force is expressed-in-coordinates-of the world reference frame, and relative-to a position vector expressed-in-coordinates-of the link frame and relative-to the link's center of mass
AddLinkForce: this is the most consistent, as the force is expressed-in-coordinates-of the link frame and applied relative-to a position vector expressed-in-coordinates of the link frame and relative-to the link frame's origin

Having said all that, here are some guidelines:

Mandatory: be clear about how the parameters to these APIs are defined. For force and position parameters, this includes the location relative-to which they are defined and which frame they are "expressed-in-coordinates-of". Be as verbose as needed in the header file documentation.
If possible, prefer consistent frame conventions for the parameters of an API. For example, gazebo::physics::AddLinkForce uses the Link frame consistently
You can try to fully describe the frame convention in the API name (AddForceAtWorldPosition, AddForceAtRelativePosition) but the name will likely become very long

I'll follow up with concrete suggestions inline

scpeters · 2025-03-13T06:37:33Z

include/gz/sim/Link.hh

+      /// and applied at the center of mass of the link.
+      /// \param[in] _ecm Mutable Entity-component manager.
+      /// \param[in] _force Force to be applied expressed in link's center of mass coordinates
+      public: void AddLinkForce(EntityComponentManager &_ecm,


/// \param[in] _force Force to be applied expressed in link's
/// center of mass coordinates

by center of mass coordinates, do you mean the coordinates of the inertial frame defined by the SDFormat //link/inertial/pose offset from the link frame?

If so, you could name this method AddInertialForce or AddForceInInertialFrame or AddForceRelativeToInertialFrame

scpeters · 2025-03-13T06:38:33Z

include/gz/sim/Link.hh

+      /// \param[in] _force Force to be applied expressed in link's center
+      /// of mass coordinates
+      /// \param[in] _position The point of application of the force expressed
+      /// in the link-fixed frame.


consider defining _position in the same coordinates as _force for consistency

consider defining _position in the same coordinates as _force for consistency

The documentation is conflicting:

According to Link.hh:330-331 , _position is defined in link-fixed coordinates.

According to Link.cc:457-459 , _position is defined in Link's center of mass coordinates.

In the current implementation I am assuming _position is defined in Link's center of mass coordinates, which are the same coordinates as _force.

well if the inconsistent use of frames is a documentation error, then let's fix the documentation. We should confirm the actual behavior with the test though

well if the inconsistent use of frames is a documentation error, then let's fix the documentation. We should confirm the actual behavior with the test though

_position , is expressed in terms of link fixed frame. In Link.cc:460-461 , _position is directly added to inertial , if _position is expressed in terms of Link's center of mass there should be a calculation to convert _position from in terms of Link's center of mass to _position in terms of link-fixed coordinates.

There is a major issue in my code that I overlooked, the calculations in my code consider the force is expressed in coordinates of the link frame, so coincidently _force and _position are both expressed in link-fixed coordinates.

I'll work on fixing all these issues in the next commit.

test/integration/link.cc

scpeters · 2025-03-13T22:19:10Z

include/gz/sim/Link.hh

+      /// \param[in] _force Force to be applied expressed in link's center
+      /// of mass coordinates
+      /// \param[in] _position The point of application of the force expressed
+      /// in the link-fixed frame.


well if the inconsistent use of frames is a documentation error, then let's fix the documentation. We should confirm the actual behavior with the test though

Signed-off-by: Vedant87 <[email protected]>

Vedant87 · 2025-03-14T17:03:10Z

I've made the required changes to the code, I am not sure why the commit is failing the following 2 tests:

scpeters · 2025-03-14T23:52:02Z

gz_sim-ci-pr_any-noble-amd64

this had an unrelated flaky test failure (see #2609)

gz_sim-pr-clowin

this was an infra problem, fixed by gazebo-tooling/release-tools#1281, I retriggered it

src/Link.cc

test/integration/link.cc

Signed-off-by: Vedant87 <[email protected]>

Vedant87 requested a review from mjcarroll as a code owner March 7, 2025 16:12

github-actions bot added the 🪵 jetty Gazebo Jetty label Mar 7, 2025

iche033 reviewed Mar 7, 2025

View reviewed changes

Vedant87 requested a review from iche033 March 8, 2025 03:35

Vedant87 force-pushed the add_link_force_api branch 4 times, most recently from 0cbdd0f to 4e44110 Compare March 8, 2025 09:37

Vedant87 and others added 5 commits March 8, 2025 09:41

Add AddLinkForce function

024e0d6

Signed-off-by: Vedant87 <[email protected]>

Add AddLinkForce function using AddWorld force method

94ad125

Signed-off-by: Vedant87 <[email protected]>

Update .gitignore

f2cd174

Signed-off-by: Vedant87 <[email protected]>

Fixed lint issues by removing trailing whitespaces and limiting lines…

9869266

… to 80 characters Signed-off-by: Vedant87 <[email protected]>

Fixed lint issues by removing trailing whitespaces and limiting lines…

9660d0c

… to 80 characters Signed-off-by: Vedant87 <[email protected]>

Vedant87 force-pushed the add_link_force_api branch from 4e44110 to 9660d0c Compare March 8, 2025 09:45

arjo129 requested changes Mar 11, 2025

View reviewed changes

.gitignore Outdated Show resolved Hide resolved

src/Link.cc Outdated Show resolved Hide resolved

Vedant87 added 4 commits March 12, 2025 01:17

Add Unit test

a4d3ee3

Signed-off-by: Vedant87 <[email protected]>

Add Unit test

2aa9794

Signed-off-by: Vedant87 <[email protected]>

Fix formatting for unit test function

d0d498b

Signed-off-by: Vedant87 <[email protected]>

Fix errors and typos

f7c09b7

Signed-off-by: Vedant87 <[email protected]>

scpeters reviewed Mar 13, 2025

View reviewed changes

Vedant87 added 2 commits March 14, 2025 13:21

Fix naming convention, added Python bindings.

1282177

Signed-off-by: Vedant87 <[email protected]>

Fix errors and limit lines to 80 characters

03cbfba

Signed-off-by: Vedant87 <[email protected]>

Vedant87 requested review from scpeters and arjo129 March 14, 2025 17:03

scpeters reviewed Mar 15, 2025

View reviewed changes

src/Link.cc Outdated Show resolved Hide resolved

src/Link.cc Outdated Show resolved Hide resolved

test/integration/link.cc Outdated Show resolved Hide resolved

Vedant87 and others added 5 commits March 18, 2025 09:44

Changes after review, add handcalculations to Unit test function.

334aea2

Signed-off-by: Vedant87 <[email protected]>

Fix errors.

ee1a690

Signed-off-by: Vedant87 <[email protected]>

Fix errors.

b107a65

Signed-off-by: Vedant87 <[email protected]>

Correct Expected_torque_with_offset calculations.

85bdea9

Signed-off-by: Vedant87 <[email protected]>

Merge branch 'main' into add_link_force_api

c5ceda6

Vedant87 requested a review from scpeters March 19, 2025 03:36

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add AddLinkForce Function to Apply Forces in Link Frame #2816

Add AddLinkForce Function to Apply Forces in Link Frame #2816

Vedant87 commented Mar 7, 2025

iche033 commented Mar 7, 2025

iche033 Mar 7, 2025

iche033 Mar 12, 2025

Vedant87 Mar 13, 2025 •

edited

Loading

scpeters Mar 13, 2025

scpeters Mar 13, 2025

arjo129 commented Mar 11, 2025

arjo129 commented Mar 13, 2025

scpeters commented Mar 13, 2025

scpeters Mar 13, 2025

scpeters Mar 13, 2025

Vedant87 Mar 13, 2025

scpeters Mar 13, 2025

Vedant87 Mar 13, 2025

scpeters Mar 13, 2025

Vedant87 commented Mar 14, 2025

scpeters commented Mar 14, 2025

Add AddLinkForce Function to Apply Forces in Link Frame #2816

Are you sure you want to change the base?

Add AddLinkForce Function to Apply Forces in Link Frame #2816

Conversation

Vedant87 commented Mar 7, 2025

🎉 New feature

Summary

Checklist

iche033 commented Mar 7, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Vedant87 Mar 13, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

arjo129 commented Mar 11, 2025

arjo129 commented Mar 13, 2025

scpeters commented Mar 13, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Vedant87 commented Mar 14, 2025

scpeters commented Mar 14, 2025

Vedant87 Mar 13, 2025 •

edited

Loading