Minor modifs.

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\")"