diff --git a/src/dynamic_graph/sot/dynamics/hrp2.py.in b/src/dynamic_graph/sot/dynamics/hrp2.py.in index 6398a5d71f3c375dde2fcf3ddfbfec79fca770e3..a40b5ed31e3a2c28fbbe0ea1460b5e8736db38e2 100755 --- a/src/dynamic_graph/sot/dynamics/hrp2.py.in +++ b/src/dynamic_graph/sot/dynamics/hrp2.py.in @@ -164,22 +164,18 @@ class Hrp2Laas (Hrp2): [ 0., -1., 0., 0.], [ 0., 0., 0., 1.]]) - for camera in [cameraBottomLeftPosition, cameraBottomRightPosition, - cameraTopLeftPosition, cameraTopRightPosition]: - camera = camera * c1_M_c - self.AdditionalFrames.append( ("cameraBottomLeft", - matrixToTuple(cameraBottomLeftPosition), "gaze")) + matrixToTuple(c1_M_c * cameraBottomLeftPosition), "gaze")) self.AdditionalFrames.append( ("cameraBottomRight", - matrixToTuple(cameraBottomRightPosition), "gaze")) + matrixToTuple(c1_M_c * cameraBottomRightPosition), "gaze")) self.AdditionalFrames.append( ("cameraTopLeft", - matrixToTuple(cameraTopLeftPosition), "gaze")) + matrixToTuple(c1_M_c * cameraTopLeftPosition), "gaze")) self.AdditionalFrames.append( ("cameraTopRight", - matrixToTuple(cameraTopRightPosition), "gaze")) + matrixToTuple(c1_M_c * cameraTopRightPosition), "gaze")) Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2, tracer)