search

rdiankov / openrave

Open Robotics Automation Virtual Environment: An environment for testing, developing, and deploying robotics motion planning algorithms.
http://www.openrave.org
Other
684 stars 339 forks source link

Fix body-link transform update #1369

Closed superfashi closed 2 months ago

superfashi commented 3 months ago

Currently the body transform is incorrectly updated inside KinBody::UpdateFromKinBodyInfo due to an early change to _baseLinkInBodyTransform, which resulted in a wrong link-body transformation state. This resulted in body transform changed in an unexpected way. This MR fixes this issue.

woswos commented 3 months ago

Pipeline ID: 797811

superfashi commented 2 months ago

Before the update, $L = B \times BL$. However, $B = L \times BL^{-1}$ is actually recorded.

If we just update $B$ to be $B'$, we actually want to update $L' = B' \times BL = L \times BL^{-1}$.

However, if this line exists, the code preemptively updated the internal state of $BL$, so you lose the original $BL$ which you needed for the calculation of $L'$.

I can be fairly sure that originally this bug is discovered, and instead of fixing the right place, an extra Transform bodyTransform = info._transform * _invBaseLinkInBodyTransform; is introduced in L6399 to "fix" the calculation. However this fix fails to cover the other case when the transform of base link isn't identity.

rdiankov commented 2 months ago

THanks!