Slight changes in the timing.

python/mocap/planche.py

@@ -33,6 +33,7 @@ from viewer_helper import addRobotViewer,VisualPinger
 # --- ROBOT SIMU -------------------------------------------------------------
 #-----------------------------------------------------------------------------
 
+#robotName = 'hrp14small'
 robotName = 'hrp14small'
 robotDim  = robotDimension[robotName]
 RobotClass = RobotDynSimu
@@ -41,8 +42,10 @@ robot.resize(robotDim)
 addRobotViewer(robot,small=True,verbose=False)
 dt = 5e-3
 
+initialConfig['hrp14small'] = (-0.033328803958899381, -0.0019839923040723341, 0.62176527349722499, 2.379901541270165e-05, 0.037719492175904465, 0.00043085147714449579, -0.00028574496353126724, 0.0038294370786961648, -0.64798319906979551, 1.0552418879542016, -0.44497846451873851, -0.0038397195926379991, -0.00028578259876671871, 0.0038284398205732629, -0.64712828871069394, 1.0534202525984278, -0.4440117393779604, -0.0038387216246160054, 0.00014352031102944824, 0.013151503268540811, -0.00057411504064861592, -0.050871000025766742, 0.21782780288481224, -0.37965640592672439, -0.14072647716213352, -1.1942332339530364, 0.0055454863752273523, -0.66956710808008013, 0.1747981826611808, 0.21400703176352612, 0.38370527720078107, 0.14620204468509851, -1.1873407322935838, -0.0038746980026940735, -0.66430172366423146, 0.17500428384087438)
+
 # Similar initial position with hand forward
-robot.set((-0.033328803958899381, -0.0019839923040723341, 0.62176527349722499, 2.379901541270165e-05, 0.037719492175904465, 0.00043085147714449579, -0.00028574496353126724, 0.0038294370786961648, -0.64798319906979551, 1.0552418879542016, -0.44497846451873851, -0.0038397195926379991, -0.00028578259876671871, 0.0038284398205732629, -0.64712828871069394, 1.0534202525984278, -0.4440117393779604, -0.0038387216246160054, 0.00014352031102944824, 0.013151503268540811, -0.00057411504064861592, -0.050871000025766742, 0.21782780288481224, -0.37965640592672439, -0.14072647716213352, -1.1942332339530364, 0.0055454863752273523, -0.66956710808008013, 0.1747981826611808, 0.21400703176352612, 0.38370527720078107, 0.14620204468509851, -1.1873407322935838, -0.0038746980026940735, -0.66430172366423146, 0.17500428384087438))
+robot.set(initialConfig[robotName])
 
 #-------------------------------------------------------------------------------
 #----- MAIN LOOP ---------------------------------------------------------------
@@ -260,13 +263,17 @@ history = History(dyn,1,zmp.zmp)
 DEG = 180.0/pi
 
 # Angles for both knee, plus angle for the chest wrt the world.
-#MAX_EXT = 35/DEG
-#MAX_SUPPORT_EXT = 45/DEG
-#CHEST = 60/DEG # 80 ... 90?
+MAX_EXT = 5/DEG
+MAX_SUPPORT_EXT = 25/DEG
+CHEST = 80/DEG # 80 ... 90?
+WITH_FULL_EXTENTION=True
 
+'''
 MAX_EXT = 45/DEG
 MAX_SUPPORT_EXT = 45/DEG
 CHEST = 50/DEG # 80 ... 90?
+WITH_FULL_EXTENTION=False
+'''
 
 sot.clear()
 contact(contactLF)
@@ -320,13 +327,12 @@ attime(1000
        ,(lambda: taskPosture.gotoq(chest=(0,0),rleg=(0,0,0,MAX_EXT,0,0),head=(0,0)), "extend body")
        )
 
-with_full_extention=False
-if with_full_extention:
-    attime(1300
+if WITH_FULL_EXTENTION:
+    attime(1250
            ,lambda: taskPosture.gotoq(chest=(0,0),rleg=(0,0,0,MAX_EXT,0,0),head=(0,0),rarm=(0,-pi/2,0,0,0,0),larm=(0,pi/2,0,0,0,0)), "extend arm")
 
-    attime(2000
-           ,lambda: taskPosture.gotoq(30,chest=(0,0),rleg=(0,0,0,MAX_EXT,0.73,0),head=(0,0),rarm=(0,-pi/2,0,0,0,0),larm=(0,pi/2,0,0,0,0)), "extend foot")
+    attime(2080
+           ,lambda: taskPosture.gotoq(30,chest=(0,0),rleg=(0,0,0,MAX_EXT,0.73,0),head=(0,-pi/4),rarm=(0,-pi/2,0,0,0,0),larm=(0,pi/2,0,0,0,0)), "extend foot")
     tex=1000
 else: tex=0