diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py index 2a991fe7d5434ca46d9eb846bbb068c67407eeb1..3c2fe4b2d1d35eb516efd58414ed9f66f1d35a18 100755 --- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py +++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py @@ -31,7 +31,7 @@ from dynamic_graph.sot.dynamics import AngleEstimator from dynamic_graph import plug I3 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (2-i)*(0.,),), range(3), ()) -I4 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (2-i)*(0.,),), range(4), ()) +I4 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (3-i)*(0.,),), range(4), ()) class AbstractHumanoidRobot (object): """ @@ -349,6 +349,23 @@ class AbstractHumanoidRobot (object): # --- additional frames --- self.frames = dict() + frameName = 'rightHand' + hp = self.dynamic.getHandParameter (True) + transformation = list (map (list, I4)) + for i in range (3): transformation [i][3] = hp [i][3] + transformation = tuple (map (tuple, transformation)) + self.frames [frameName] = self.createFrame ( + "{0}_{1}".format (self.name, frameName), + transformation, "right-wrist") + frameName = 'leftHand' + hp = self.dynamic.getHandParameter (False) + transformation = list (map (list, I4)) + for i in range (3): transformation [i][3] = hp [i][3] + transformation = tuple (map (tuple, transformation)) + self.frames [frameName] = self.createFrame ( + "{0}_{1}".format (self.name, frameName), + self.dynamic.getHandParameter (False), "left-wrist") + for (frameName, transformation, signalName) in self.AdditionalFrames: self.frames[frameName] = self.createFrame( "{0}_{1}".format(self.name, frameName),