diff --git a/python/mocap/planche.py b/python/mocap/planche.py index c2876255dc9e7ff5ef8e27a44f9b1ef0346b0fcf..34262780acb5d619d92976665ce16821470bd042 100644 --- a/python/mocap/planche.py +++ b/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