From e35bb06b6bff582ef7adac535e9a168bcc1d3ca0 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Sat, 10 Sep 2011 19:19:32 +0200
Subject: [PATCH] Complete version, but cannot replicate on openhrp.
---
python/mocap/planche.py | 47 +++++++++++++++++++++++------------------
1 file changed, 26 insertions(+), 21 deletions(-)
diff --git a/python/mocap/planche.py b/python/mocap/planche.py
index a939081..ec2755c 100644
--- a/python/mocap/planche.py
+++ b/python/mocap/planche.py
@@ -332,6 +332,7 @@ class MetaTaskDynPosture(object):
else: qdes[r,0] = v
self.ref = vectorToTuple(qdes)
self.feature.selec.value = toFlags(act)
+ setGain(self.gain,gain)
taskPosture = MetaTaskDynPosture(dyn,dt)
@@ -358,7 +359,7 @@ taskLim.referenceVelSup.value = tuple([val*pi/180 for val in dqup])
sot = SolverDynReduced('sot')
sot.setSize(robotDim-6)
#sot.damping.value = 1e-2
-sot.breakFactor.value = 20
+sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
@@ -467,12 +468,8 @@ robot.after.addSignal('dyn.waist')
robot.after.addSignal('taskLim.normalizedPosition')
tr.add('taskLim.normalizedPosition','qn')
-
-
history = History(dyn,1,zmp.zmp)
-
-
#-----------------------------------------------------------------------------
# --- FUNCTIONS TO PUSH/PULL/UP/DOWN TASKS -----------------------------------
@@ -497,6 +494,7 @@ taskrf.gain.setByPoint(40,2,0.002,0.8)
taskChest.feature.selec.value='111000'
taskChest.ref = rotate('y',80/DEG)
+#taskChest.ref = rotate('y',80/DEG)
taskChest.gain.setByPoint(30,3,1/DEG,0.8)
#taskPosture.gain.setByPoint(60,5,5/DEG,0.9)
@@ -538,42 +536,49 @@ attime(1000
,(lambda: taskPosture.gotoq(chest=(0,0),rleg=(0,0,0,0,0,0),head=(0,0)), "extend body")
)
-'''
-attime(1300
- ,lambda: taskPosture.gotoq(chest=(0,0),rleg=(0,0,0,0,0,0),head=(0,0),rarm=(0,-pi/2,0,0,0,0),larm=(0,pi/2,0,0,0,0)), "extend arm")
+with_full_extention=False
+if with_full_extention:
+ attime(1300
+ ,lambda: taskPosture.gotoq(chest=(0,0),rleg=(0,0,0,0,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(chest=(0,0),rleg=(0,0,0,0,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(2000
+ ,lambda: taskPosture.gotoq(30,chest=(0,0),rleg=(0,0,0,0,0.73,0),head=(0,0),rarm=(0,-pi/2,0,0,0,0),larm=(0,pi/2,0,0,0,0)), "extend foot")
+ tex=1000
+else: tex=0
-attime(1500
+
+attime(1500 + tex
,(lambda: sot.rm(taskChest.task.name),"rm chest")
- ,(lambda: taskPosture.gotoq(chest=(0,0),rleg=q0[6:12],head=(0,0),rarm=q0[22:28],larm=q0[29:35]),"fold arms&leg")
+ ,(lambda: taskPosture.gotoq((30,3,1/DEG,0.8),chest=(0,0),rleg=q0[6:12],head=(0,0),rarm=q0[22:28],larm=q0[29:35]),"fold arms&leg")
)
-attime(1800
+attime(1800+tex
,(lambda: sot.rm(taskPosture.task.name),"Remove posture" )
,(lambda: sot.push(taskrf.task.name), "Add RF")
,(lambda: gotoNd(taskrf, (-0.1,-0.1,0.25),"111" ), "Back RF")
)
-attime(2100 ,lambda: goto6d(taskrf,vectorToTuple(rf0+(0,0,0.15)),(100,15,0.005,0.8)) , "RF to upper ground" )
-attime(2300 ,lambda: goto6d(taskrf,vectorToTuple(rf0),(100,15,0.005,0.8)) , "RF to ground" )
-attime(2500
+attime(2200+tex ,lambda: goto6d(taskrf,vectorToTuple(rf0.T+(0,0,0.07)),(80,15,0.005,0.8)) , "RF to upper ground" )
+attime(2400+tex ,lambda: goto6d(taskrf,vectorToTuple(rf0),(100,15,0.005,0.8)) , "RF to ground" )
+attime(2550+tex
,(lambda: refset(taskCom,(0.01,0,0.797)) , "COM to zero" )
,(lambda: taskCom.gain.setConstant(3) , "lower com gain" )
)
-attime(2600 ,(lambda: contact(contactRF) , "RF to contact" )
+attime(2650+tex, lambda: sigset(sot.posture,q0), "Robot to initial pose")
+attime(2650+tex ,(lambda: contact(contactRF) , "RF to contact" )
,(lambda: sigset(taskCom.feature.selec,"111") , "COM XYZ" )
,(lambda: taskCom.gain.setByPoint(100,10,0.005,0.8) , "upper com gain" )
)
-attime(2500, lambda: sigset(sot.posture,q0), "Robot to initial pose")
-attime(4000,stop)
+attime(3200+tex,stop)
-inc()
+'''robot.set( (-0.22338017682364666, 0.094679696008257069, 0.6406354063194315, -0.046149502155439906, 1.3834043086232479, -0.045751564595492393, -0.00043528996402237706, 0.0028744648411845957, -0.47567845262453851, 0.7731765985092065, -0.32594537966076575, -0.0029603563556822449, -0.16456900265664706, -0.031430863650961184, -2.0148121310236262, 0.43929960908288562, 0.18928315181673525, 0.17611103058143049, -0.00037370245565424205, -0.00014143571164947397, 8.7559046906180755e-07, 7.1211147920686882e-05, 0.20950155287705138, -0.38261191572903752, -0.1409579892969334, -1.2006478349239718, 0.0055192144589002994, -0.67056461724265271, 0.17476330298560117, 0.20672021910771882, 0.39182703814168718, 0.14945548959841934, -1.1920338277316804, -0.003453520219234802, -0.66498751012318535, 0.17474628650307528) )
+robot.setVelocity( (-0.0034980508073499746, -7.8546901386487115e-05, -0.0016849885158409757, -0.00042323578018366197, -0.0080054937692844511, 0.00086344279117106829, -0.00015755559034456312, 0.0013060411790611658, -0.21786102712256575, 0.35440517206749117, -0.14941610627508575, -0.0013290768265727577, 0.00011113538937583873, -0.00054127148646175311, 8.5548931282521993e-05, 0.0024653080400074217, 0.0054855827172477344, 0.00041334449428695003, -0.00010747410962737952, -3.8505135328234213e-05, 2.3358739214336891e-07, 1.9098782957311844e-05, 0.010521707268533676, 0.0037563490996982444, 0.00030433568575777238, 0.0081039468043348356, 3.4077369449514742e-05, 0.0012601869472091919, 8.3190734016776364e-07, 0.0092086079693020056, -0.010246626556435254, -0.0041025198155700817, 0.0059363185419371552, -0.00053151985927071734, 0.00086774348235294993, 6.7138299807645551e-07) )
+robot.state.time = 1800-1
+attime.fastForward(1800)
+'''
go()
--
GitLab