Might be an error in kinematic_model.py
Harsha19111999 opened this issue · 1 comments
"""defines 4 vertices which rotates with the body"""
_bodytoFR0 = geo.transform(self.bodytoFR0 , orn, pos)
_bodytoFL0 = geo.transform(self.bodytoFL0 , orn, pos)
_bodytoBR0 = geo.transform(self.bodytoBR0 , orn, pos)
_bodytoBL0 = geo.transform(self.bodytoBL0 , orn, pos)
In this portion of code, a vector is being transformed from one frame to the other. However, bodytoFR0, bodytoFL0, bodytoBR0, bodytoBL0 are vectors in the initial frame and they are being multiplied as ornposvector by using the transform() function. But this multiplication is valid if the "vector" is in the rotated (new) frame and not in the initial frame. Since bodyto* vectors are all in the initial frame before rotation, we could not get the new vectors after rotation with respect to the intial frame.
Correct me if I'm wrong. Thank you.
"""undo transformation of leg vector to keep feet still"""
undoOrn = -orn
undoPos = -pos
Also, can we undo the rotation and translation by taking a negative sign? Aren't we supposed to take the inverse of the transformation matrix to undo it?