search

borglab / GTDynamics

Full kinodynamics constraints for arbitrary robot configurations with factor graphs.
BSD 2-Clause "Simplified" License
39 stars 10 forks source link

pScrewAxis #302

Open dellaert opened 2 years ago

dellaert commented 2 years ago

I'm not so sure about the minus sign for pScrewAxis in the joint constructor. Can someone explain, e.g. @gchenfc or @yetongumich ? The Adjoint formula does not have a minus sign. I know test break, but only a few are "real", others seem to be self-fulfilling prohecies.

gchenfc commented 2 years ago

Yes I remember working through this. I'm a little bit foggy on the details but I think the rough intuition was that the adjoint is used to change reference frames while the negative sign is used to change which link we're getting the pose/twist of.

Referring, for example, to the functions childTwist and parentTwist:

The adjoint formula is describing the twist of the child link "assuming the parent link is fixed" (aka "Joint-induced twist" is the wording in the docstrings), but when we ask for the parent twist, then that should be "the twist of the parent link assuming the child link is fixed", so we get a negative sign. If there is no negative sign, I think this would be the twist of the child link in the parent link's frame (as opposed to the twist of the parent link in the parent link's frame)

I think you can apply a similar logic to the other instances where screwAxis(link) is used.

As a note, this is my interpretation of the reason we have a minus sign, but it's possible that it's not the original reason. Specifically, someone else originally wrote the unit test, then when I was refactoring the way pScrewJoint/cScrewJoint were stored, then I had to figure out why the unit test was that way and came to this interpretation.

dellaert commented 2 years ago

I will make a PR to just have one screw, in the joint frame, that should resolve this.

gchenfc commented 2 years ago

I think we/I had considered that, but then you have to perform an SE3::inverse and Adjoint operation every single call to poseOf, twist, twistAccel, basically every single function will require one or multiple redundant operations. I think my logic was that memory is cheaper than compute and the screw axis is protected and will never change (functional programming style) so there's no risk of getting out-of-sync.

dellaert commented 2 years ago

Right... I'll re-evaluate whether it's wise.