From 17aa8b7b5c18d7cc562777965842bae1732a8eca Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 17 Jun 2011 15:10:45 +0200
Subject: [PATCH] Minor modifs.
---
python/dyn2openhrp.py | 53 +++++++++++++++++++++++++++++++++++--------
1 file changed, 44 insertions(+), 9 deletions(-)
diff --git a/python/dyn2openhrp.py b/python/dyn2openhrp.py
index 26f8348..4762d96 100644
--- a/python/dyn2openhrp.py
+++ b/python/dyn2openhrp.py
@@ -5,6 +5,10 @@
#import random
#q=[random.random() for i in range(36)]
#q=dyn.position.value
+q=[0, 0, 0.6487, 0, 0, 0, 0, 0, -0.453786, 0.872665, -0.418879, 0, 0, 0, -0.453786, 0.872665, -0.418879, 0, 0, 0, 0, 0, 0.261799, -0.174533, 0, -1.3, 0, -0.5, 0.174533, 0.261799, 0.174533, 0, -1.3, 0, -0.5, 0.174533 ]
+
+
+
from dynamic_graph import plug
from dynamic_graph.sot.core import *
@@ -12,15 +16,9 @@ from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
+import math
# --- Conversion
-toUT=PoseRollPitchYawToPoseUTheta('toUT')
-#plug(dyn.position,toUT.sin)
-toUT.sin.value = tuple(q[0:6])
-
-import math
-toUT.sout.recompute(1)
-u=toUT.sout.value[3:6]
jointNames = ("RLEG_JOINT0",
"RLEG_JOINT1",
@@ -54,12 +52,17 @@ jointNames = ("RLEG_JOINT0",
"LARM_JOINT6")
-pattern = "<property name=\"%s\" value=\"%f\"/>"
+pattern = "<property name=\"%s.angle\" value=\"%f\"/>"
for i in range(len(q)-6):
print pattern % (jointNames[i],q[i+6])
-u=[random.random() for i in range(3)]
+
+toUT=PoseRollPitchYawToPoseUTheta('toUT')
+#plug(dyn.position,toUT.sin)
+toUT.sin.value = tuple(q[0:6])
+toUT.sout.recompute(1)
+u=toUT.sout.value[3:6]
theta=math.sqrt(sum(v*v for v in u))
if theta>1e-4:
(ux,uy,uz)=[v/theta for v in u]
@@ -78,3 +81,35 @@ for i in range(len(bushJointNames)):
+
+print " <property name=\"RHAND_JOINT4.angle\" value=\"0.0 \"/>"
+print " <property name=\"RHAND_JOINT1.angle\" value=\"0.0 \"/>"
+print " <property name=\"RHAND_JOINT3.angle\" value=\"0.0 \"/>"
+print " <property name=\"RHAND_JOINT0.angle\" value=\"0.0 \"/>"
+print " <property name=\"RHAND_JOINT2.angle\" value=\"0.0 \"/>"
+print " <property name=\"LHAND_JOINT2.angle\" value=\"0.0 \"/>"
+print " <property name=\"LHAND_JOINT1.angle\" value=\"0.0 \"/>"
+print " <property name=\"LHAND_JOINT4.angle\" value=\"0.0 \"/>"
+print " <property name=\"LHAND_JOINT0.angle\" value=\"0.0 \"/>"
+print " <property name=\"LHAND_JOINT3.angle\" value=\"0.0 \"/>"
+print ""
+print " <property name=\"setupDirectory\" value=\"/opt/grx3.0/HRP2LAAS/bin\"/>"
+print " <property name=\"imageProcessTime\" value=\"0.033\"/>"
+print " <property name=\"imageProcessor\" value=\"\"/>"
+print " <property name=\"controller\" value=\"HRP2LAASControllerFactory\"/>"
+print " <property name=\"isRobot\" value=\"true\"/>"
+print " <property name=\"controlTime\" value=\"0.0010\"/>"
+print " <property name=\"setupCommand\" value=\"HRP2Controller$(BIN_SFX)\"/>"
+
+
+
+#import robotviewer
+#viewer = robotviewer.client('XML-RPC')
+#viewer.updateElementConfig('hrp',[float(val) for val in q]+10*[0.0])
+
+
+
+posinit=""
+for v in q[6:]:
+ posinit+=str(v*180/math.pi)+" "
+print "seq.sendMsg(\":joint-angles \"+\"" + posinit + "\"+\" 0 0 0 0 0 0 0 0 0 0 2.5\")"
--
GitLab