Link::AddForceInInertialFrame APIs added #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 <Vedantsr3@gmail.com>

… to 80 characters Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

.gitignore

arjo129 · 2025-03-11T07:01:07Z

src/Link.cc

+}
+
+//////////////////////////////////////////////////
+void Link::AddLinkForce(EntityComponentManager &_ecm,


Can you add a unit test for this?

You can find other tests for the link class here: https://github.com/gazebosim/gz-sim/blob/gz-sim9/src/Link_TEST.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 <Vedantsr3@gmail.com>

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 <Vedantsr3@gmail.com>

scpeters · 2025-05-06T16:48:08Z

sorry I missed the notification that you updated this PR; I'll take a look

scpeters

thanks for documenting your test! I just have some minor suggestions about formatting, and one comment that you don't need to act on since you've already put in a lot of work here

src/Link.cc

test/integration/link.cc

scpeters · 2025-05-08T22:51:20Z

test/integration/link.cc

+      wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z()));
+
+  // Add force in Inertial Frame at an offset
+  math::Vector3d offset{0.0, 1.0, 0.0};


since the inertial frame pose only has a rotation about the Y axis, this offset won't be rotated. I think you've put in enough work here, so I won't ask you to do more, but I think it would be a stronger test if the offset was 1, 0, 0, since this offset would pass through a rotation

since the inertial frame pose only has a rotation about the Y axis, this offset won't be rotated. I think you've put in enough work here, so I won't ask you to do more, but I think it would be a stronger test if the offset was 1, 0, 0, since this offset would pass through a rotation

I can work on this, just need to do a few matrix multiplications and edit a few lines of code.

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

scpeters · 2025-05-09T19:20:43Z

test/integration/link.cc

+  //                      [-1 0  0 | 3]   [0]
+  //                      [0  0  0 | 1]   [1]
+  //                    = [1, 2, 2]
+  // offsetInLInkFrame = [1, 2, 2]


this looks right to me

test/integration/link.cc

scpeters · 2025-05-09T19:25:20Z

test/integration/link.cc

+  // First, calculate the effective position vector in the world frame.
+  // offsetInLinkFrame = [1, 2, 2]
+  // inertiaPose.Pos() = [1, 2, 3]
+  // effectivePosition = offsetInLinkFrame + inertiaPose.Pos() = [2, 4, 5]


I don't know why this step is needed. I would think the torque could be computed by rotating offsetInLinkFrame to the world frame and then computing the cross product with worldForceWithOffset.

I don't know why this step is needed. I would think the torque could be computed by rotating offsetInLinkFrame to the world frame and then computing the cross product with worldForceWithOffset.

This is consistent with the torque calculations done for AddWorldForce's offset test (see: Link.cc:657-658 ).

the offsetInLinkFrame variable has already been computed by multiplication with the homogeneous transformation matrix H_inertial, which has the inertiaPose.Pos() embedded within it. So I think it should not be added again

the offsetInLinkFrame variable has already been computed by multiplication with the homogeneous transformation matrix H_inertial, which has the inertiaPose.Pos() embedded within it. So I think it should not be added again

Understood, will change the H_inertial to a pure rotational, this will be more consistent with the following changes.

scpeters · 2025-05-09T22:05:21Z

src/Link.cc

+    math::Quaterniond::Identity);
+  math::Vector3d positionInLinkFrame =
+    (inertial->Data().Pose() *
+     forceApplicationRelativeToInertialFrame).Pos();


the AddWorldForce API is a bit confusing

the force is expressed in world coordinates

the force is applied at an offset from the link center of mass / inertial frame

the offset is expressed in link-frame coordinates

so I think we just need to rotate _position here, to express it in link-frame coordinates, since it is already relative to the center-of-mass / inertial frame

the AddWorldForce API is a bit confusing

the force is expressed in world coordinates

the force is applied at an offset from the link center of mass / inertial frame

the offset is expressed in link-frame coordinates

so I think we just need to rotate _position here, to express it in link-frame coordinates, since it is already relative to the center-of-mass / inertial frame

Understood, changing this to
math::Vector3d positionInLinkFrame = inertial->Data().Pose().Rot().RotateVector(_position);

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

scpeters

thanks, that fix looks correct. we are almost there! just one more comment to be fixed

scpeters · 2025-05-10T23:57:07Z

src/Link.cc

+  // ApplyWorldForce applies the force at a position expressed in terms
+  // Rotate offset from inertial frame to link frame.


Suggested change

// ApplyWorldForce applies the force at a position expressed in terms

// Rotate offset from inertial frame to link frame.

// ApplyWorldForce applies the force at a position relative to the

// center of mass and expressed in coordinates of the link frame.

// Since _position is relative to the center of mass in coordinates

// of the inertial frame, it just needs to be rotated to be expressed

// in coordiantes of the link frame.

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

requested changes have been made

scpeters · 2025-05-12T16:15:09Z

thanks for your contribution!

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

azeey added this to Core development Mar 7, 2025

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

github-project-automation bot moved this to Inbox in Core development 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 <Vedantsr3@gmail.com>

Add AddLinkForce function using AddWorld force method

94ad125

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Update .gitignore

f2cd174

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Fixed lint issues by removing trailing whitespaces and limiting lines…

9869266

… to 80 characters Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Fixed lint issues by removing trailing whitespaces and limiting lines…

9660d0c

… to 80 characters Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

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

arjo129 previously requested changes Mar 11, 2025

View reviewed changes

github-project-automation bot moved this from Inbox to In review in Core development Mar 11, 2025

Vedant87 added 4 commits March 12, 2025 01:17

Add Unit test

a4d3ee3

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Add Unit test

2aa9794

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Fix formatting for unit test function

d0d498b

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Fix errors and typos

f7c09b7

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

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 <Vedantsr3@gmail.com>

Fix errors and limit lines to 80 characters

03cbfba

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

Vedant87 and others added 5 commits April 16, 2025 09:59

Merge branch 'main' into add_link_force_api

f77af75

Merge branch 'main' into add_link_force_api

fcbe12d

Merge branch 'main' into add_link_force_api

6d5067f

Merge branch 'main' into add_link_force_api

54e4c5d

Merge branch 'main' into add_link_force_api

06f6ee8

scpeters approved these changes May 8, 2025

View reviewed changes

Vedant87 and others added 10 commits May 9, 2025 07:37

Update src/Link.cc

3371d9f

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update src/Link.cc

431d151

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update src/Link.cc

eb0233d

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update src/Link.cc

10e1b9d

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update src/Link.cc

352cffc

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update test/integration/link.cc

3b4180c

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update test/integration/link.cc

6c228b0

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update test/integration/link.cc

cb4eea4

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update test/integration/link.cc

0a9e696

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Vedant Randive <vedantsr3@gmail.com>

Update Offset test

9aa3d10

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

scpeters reviewed May 9, 2025

View reviewed changes

Vedant87 added 2 commits May 10, 2025 00:25

Update calculations

cb4cf99

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

FIx errors

d5e24a1

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

scpeters reviewed May 10, 2025

View reviewed changes

Update comments

a38852c

Signed-off-by: Vedant87 <Vedantsr3@gmail.com>

scpeters approved these changes May 12, 2025

View reviewed changes

scpeters changed the title ~~Add AddLinkForce Function to Apply Forces in Link Frame~~ Link::AddForceInInertialFrame API added May 12, 2025

scpeters changed the title ~~Link::AddForceInInertialFrame API added~~ Link::AddForceInInertialFrame APIs added May 12, 2025

scpeters merged commit a90e292 into gazebosim:main May 12, 2025
6 of 8 checks passed

github-project-automation bot moved this from In review to Done in Core development May 12, 2025

		// ApplyWorldForce applies the force at a position expressed in terms
		// Rotate offset from inertial frame to link frame.

-  // ApplyWorldForce applies the force at a position expressed in terms
-  // Rotate offset from inertial frame to link frame.
+  // ApplyWorldForce applies the force at a position relative to the
+  // center of mass and expressed in coordinates of the link frame.
+  // Since _position is relative to the center of mass in coordinates
+  // of the inertial frame, it just needs to be rotated to be expressed
+  // in coordiantes of the link frame.

Link::AddForceInInertialFrame APIs added #2816

Link::AddForceInInertialFrame APIs added #2816

Uh oh!

Conversation

Vedant87 commented Mar 7, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🎉 New feature

Summary

Checklist

Uh oh!

iche033 commented Mar 7, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Vedant87 Mar 13, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

arjo129 Mar 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

arjo129 commented Mar 11, 2025

Uh oh!

arjo129 commented Mar 13, 2025

Uh oh!

scpeters commented Mar 13, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

scpeters commented May 6, 2025

Uh oh!

scpeters left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Vedant87 May 9, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Vedant87 commented Mar 7, 2025 •

edited

Loading

Vedant87 Mar 13, 2025 •

edited

Loading

arjo129 Mar 11, 2025 •

edited

Loading

Vedant87 May 9, 2025 •

edited

Loading