From ffbfa7764c354e7c789769d81f53056137535dd4 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 16 Mar 2012 17:26:58 +0100
Subject: [PATCH] Create frames for right and left hands.
---
.../sot/dynamics/humanoid_robot.py | 19 ++++++++++++++++++-
1 file changed, 18 insertions(+), 1 deletion(-)
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 2a991fe..3c2fe4b 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),
--
GitLab