Fix velocity and position integration for FreeJoint #1437

azeey · 2020-02-28T22:13:01Z

This resolves issue #1433 and possibly the other issues mentioned in there. The two changes made in this PR are:

When integrating a FreeJoint's velocity, an additional term is added to account for changing linear velocity in the inertial frame.
The joint velocities of FreeJoint are in se3 and since they are expressed in the body frame, they have to be updated after the joint's positions (SE3) are updated. The same is true for accelerations after velocities are integrated.

This PR breaks a test in test_Dynamics.cpp because integration of the joint positions of FreeJoint updates joint velocities unlike other joint types. I left this as WIP to get some feedback on what to do about this test failure.

Before creating a pull request

Document new methods and classes
Format new code files using clang-format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

The main fix is in the expression used to integrate velocities. The secondary fix is to update spatial accelerations after integrating velocities and to update both spatial velocities and accelerations after integrating positions.

scpeters · 2020-03-16T22:00:56Z

@jslee02 let us know if you need any explanation of these changes

cc @mxgrey

jslee02

I think the correct fix for the numerical inaccuracy of FreeJoint is to change the dynamics equation to use the global frame. Currently, DART uses local frame (or body attached frame) that leads to that the linear acceleration depends on the angular velocity, which doesn't exist in the global frame version.

This PR is resolving the issue by reverting the velocity and acceleration that are computed by the forward dynamics (represented in body frame). It may fix the issue but not the root cause.

Let me rework on the dynamics equations switching the reference frame to global frame, but this could take some time. In the meantime, I might be okay to accept this PR as long as it doesn't break the current behavior but resolving the issue.

Sorry for taking too long in response.

dart/dynamics/FreeJoint.cpp

unittests/regression/test_Issue1193.cpp

scpeters · 2020-03-17T22:15:19Z

This PR is resolving the issue by reverting the velocity and acceleration that are computed by the forward dynamics (represented in body frame). It may fix the issue but not the root cause.

Let me rework on the dynamics equations switching the reference frame to global frame, but this could take some time. In the meantime, I might be okay to accept this PR as long as it doesn't break the current behavior but resolving the issue.

Sounds good to me.

azeey · 2020-03-19T21:45:54Z

Yes, I think doing the dynamics in the global frame would probably be a better fix.

This PR is resolving the issue by reverting the velocity and acceleration that are computed by the forward dynamics (represented in body frame).

Is it reverting? My understanding was that it was updating the velocity and acceleration to be in the updated (after integration) body frame.

jslee02 · 2020-03-19T23:28:13Z

Is it reverting? My understanding was that it was updating the velocity and acceleration to be in the updated (after integration) body frame.

Sorry, "overwriting" would be a better word. I intended to say that the function additionally updates the velocity and acceleration that are already computed by other functions.

scpeters · 2020-03-24T16:51:17Z

@jslee02 wrote: In the meantime, I might be okay to accept this PR as long as it doesn't break the current behavior but resolving the issue.

If you have a better fix ready, we can use that one, but if we are going to merge a short-term fix, would it be acceptable to modify test_Dynamics.cpp to match the new behavior?

jslee02 · 2020-03-25T04:40:21Z

If you have a better fix ready, we can use that one, but if we are going to merge a short-term fix, would it be acceptable to modify test_Dynamics.cpp to match the new behavior?

Yes, it would work.

* Fix issue dartsim#1433 The main fix is in the expression used to integrate velocities. The secondary fix is to update spatial accelerations after integrating velocities and to update both spatial velocities and accelerations after integrating positions. * Add regression test for issue dartsim#1193 * Remove main in test, fix style

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

azeey · 2020-10-06T16:50:54Z

While working on #1505 (a copy of gazebo-forks#9) , I found that getLinearAcceleration did not return the correct value. Investigating that further, I found that the integration fix I have so far did not apply when constraint impulses are involved. Furthermore, I found problems with the integration of position in FreeJoint::integratePosition when the COM of a body is not at the origin. f34aa32 fixes both issues and adds a test for the COM offset issue.

The linear acceleration issue is fixed by a Coriolis term from the acceleration before integrating it to obtain a new velocity. My intuition for this is that the FreeJoint::integratePosition updates the spatial velocity based on the newly integrated position. This update to spatial velocity is in itself an acceleration and it does what the Coriolis term would have done had we FreeJoint::integratePosition not updated the spatial velocity.

The COM offset issue is fixed by transforming all quantities into the COM frame before integration. After integration, the values are transformed back into the body's origin frame.

costashatz · 2021-09-20T06:51:47Z

@jslee02 any updates on this one and when it will be fixed in DART main repo?

jslee02 · 2021-09-20T16:05:57Z

Let me take a look this PR this weekend

…on_fix

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

azeey · 2021-09-24T21:31:54Z

I've updated the PR with the changes we've made on our fork. I've relaxed the tolerances in test_Dynamics to make tests pass. IMO, a proper way to fix those tests would be to compute the finite differences for 6DOF joints differently than simple vector subtraction because the velocities in the two time steps are in two different reference frames (the body has undergone a rotation).

costashatz · 2021-09-27T21:25:58Z

@azeey thanks! @jslee02 any updates?

jslee02 · 2021-12-15T02:05:46Z

My apologies for the supre late response. Left a few comments, one build related one and mostly style fixes.

As pointed out in gazebo-forks#28 (comment), changing the reference frame would require lots of work and need throught tests to make sure it doesn't introduce unforeseen side effects. Let me merge this change as it's currently the best solution. Thank you for the fix with the decent tests!

jslee02 · 2021-12-14T21:24:43Z

dart/dynamics/FreeJoint.cpp


+  setVelocitiesStatic(math::AdR(Qdiff.inverse(), getVelocitiesStatic()));


Could we cache math::AdR(Qdiff.inverse()) to compute it only once?

I cached Qdiff.inverse() in the variable QdiffInv in 5091a56. I hope that's what you meant.

dart/dynamics/FreeJoint.cpp

unittests/regression/CMakeLists.txt

unittests/regression/test_Issue1193.cpp

jslee02 · 2021-12-15T01:49:44Z

unittests/regression/test_Issue1193.cpp

+  auto comLinearVel = rootBn->getCOMLinearVelocity();
+  // TODO (addisu) Improve FreeJoint integration so that a higher tolerance is
+  // not necessary here.
+  EXPECT_NEAR(0.01, comLinearVel.x(), tol * 1e2);


Nit: Would it be better to change 0.01 to 10 * dt to give hint on where the expected value come from?

Yes, I added a variable extForce and then used the x component of that in the expectation. 5091a56

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

jslee02

Thank you for being patient with me! LGTM

azeey added 2 commits February 28, 2020 15:46

Fix issue dartsim#1433

2913e04

The main fix is in the expression used to integrate velocities. The secondary fix is to update spatial accelerations after integrating velocities and to update both spatial velocities and accelerations after integrating positions.

Add regression test for issue dartsim#1193

cf3bf80

azeey mentioned this pull request Mar 7, 2020

Symmetry unittest #1193

Closed

jslee02 self-requested a review March 12, 2020 19:48

jslee02 reviewed Mar 17, 2020

View reviewed changes

dart/dynamics/FreeJoint.cpp Outdated Show resolved Hide resolved

jslee02 reviewed Mar 17, 2020

View reviewed changes

unittests/regression/test_Issue1193.cpp Outdated Show resolved Hide resolved

Remove main in test, fix style

521e518

Merge branch 'master' into freejoint_integration_fix

7de55b6

Merge branch 'master' into freejoint_integration_fix

dbe970c

azeey mentioned this pull request Apr 2, 2020

Copy of dartsim PR #1437 gazebo-forks/dart#5

Merged

azeey mentioned this pull request Jul 28, 2020

Upstream changes from our DART fork gazebosim/gz-physics#49

Open

5 tasks

More integration fixes

f34aa32

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

mxgrey mentioned this pull request Jun 9, 2021

Fix FreeJoint velocity and position integration for articulated bodies gazebo-forks/dart#28

Merged

jslee02 closed this Jul 15, 2021

jslee02 deleted the branch dartsim:main July 15, 2021 07:59

jslee02 reopened this Jul 15, 2021

azeey added 3 commits September 24, 2021 15:54

Merge remote-tracking branch 'upstream/main' into freejoint_integrati…

e16efe2

…on_fix

Bring latest changes from fork

62322cc

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Relax tolerances for velocity expectations

6eaf865

azeey changed the title ~~[WIP] Fix velocity and position integration for FreeJoint~~ Fix velocity and position integration for FreeJoint Sep 24, 2021

azeey mentioned this pull request Oct 5, 2021

Use a function-local static variable to replace globals gazebo-forks/dart#32

Merged

jslee02 added this to the DART 6.13.0 milestone Nov 1, 2021

jslee02 reviewed Dec 15, 2021

View reviewed changes

azeey added 2 commits December 15, 2021 23:00

Fix formatting

497d843

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Address reviewer feedback

5091a56

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

jslee02 approved these changes Dec 16, 2021

View reviewed changes

jslee02 merged commit 8bf2bab into dartsim:main Dec 16, 2021

jslee02 mentioned this pull request Dec 18, 2021

Acceleration grows to infinity for a rolling ball #1433

Closed

2 tasks

j-rivero mentioned this pull request Mar 5, 2024

Document DART library versions in packages gazebo-tooling/release-tools#1121

Open

This pull request was closed.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix velocity and position integration for FreeJoint #1437

Fix velocity and position integration for FreeJoint #1437

azeey commented Feb 28, 2020

scpeters commented Mar 16, 2020

jslee02 left a comment •

edited

Loading

scpeters commented Mar 17, 2020

azeey commented Mar 19, 2020

jslee02 commented Mar 19, 2020

scpeters commented Mar 24, 2020

jslee02 commented Mar 25, 2020

azeey commented Oct 6, 2020

costashatz commented Sep 20, 2021

jslee02 commented Sep 20, 2021

azeey commented Sep 24, 2021

costashatz commented Sep 27, 2021

jslee02 commented Dec 15, 2021

jslee02 Dec 14, 2021

azeey Dec 16, 2021

jslee02 Dec 15, 2021

azeey Dec 16, 2021

jslee02 left a comment


		setVelocitiesStatic(math::AdR(Qdiff.inverse(), getVelocitiesStatic()));

Fix velocity and position integration for FreeJoint #1437

Fix velocity and position integration for FreeJoint #1437

Conversation

azeey commented Feb 28, 2020

scpeters commented Mar 16, 2020

jslee02 left a comment • edited Loading

Choose a reason for hiding this comment

scpeters commented Mar 17, 2020

azeey commented Mar 19, 2020

jslee02 commented Mar 19, 2020

scpeters commented Mar 24, 2020

jslee02 commented Mar 25, 2020

azeey commented Oct 6, 2020

costashatz commented Sep 20, 2021

jslee02 commented Sep 20, 2021

azeey commented Sep 24, 2021

costashatz commented Sep 27, 2021

jslee02 commented Dec 15, 2021

jslee02 Dec 14, 2021

Choose a reason for hiding this comment

azeey Dec 16, 2021

Choose a reason for hiding this comment

jslee02 Dec 15, 2021

Choose a reason for hiding this comment

azeey Dec 16, 2021

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

jslee02 left a comment •

edited

Loading