From 563be530ca30626a6f47788c4fc9bf0ffef747c5 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 7 Nov 2011 17:01:42 +0100
Subject: [PATCH] Working version used for the novela.
---
python/walktrans.py | 75 +++++++++++++++++++++++----------------------
1 file changed, 38 insertions(+), 37 deletions(-)
diff --git a/python/walktrans.py b/python/walktrans.py
index 9f1700c..31137b8 100644
--- a/python/walktrans.py
+++ b/python/walktrans.py
@@ -1,3 +1,8 @@
+'''
+Walking transition: give an initial posture and final posture, the robot will
+make one step to go from one to the other.
+'''
+
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
@@ -214,16 +219,7 @@ plug(dyn.position,pg.position)
plug(dyn.com,pg.com)
pg.motorcontrol.value = robotDim*(0,)
pg.zmppreviouscontroller.value = (0,0,0)
-
-# DEBUG
-plug(dyn.lf,pg.leftfootcurrentpos)
-plug(dyn.rf,pg.rightfootcurrentpos)
-
-print "Pose before init state: " ,dyn.position.value
-pg.initState()
-pg.initrightfootref.recompute(0)
-pg.initleftfootref.recompute(0)
-pg.initcomref.recompute(0)
+#pg.initState()
# --- PG INIT FRAMES ---
geom = Dynamic("geom")
@@ -262,7 +258,6 @@ plug(pg.zmpref,wa_zmp.sin2)
pg.parseCmd(':SetZMPFrame world')
plug(wa_zmp.sout,robot.zmp)
-
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
@@ -278,9 +273,6 @@ waistReference=PoseRollPitchYawToMatrixHomo('waistReference')
plug(waistReferenceVector.sout,waistReference.sin)
plug(waistReference.sout,taskWaist.featureDes.position)
-taskWaist.feature.selec.value = '011100'
-taskWaist.task.controlGain.value = 5
-
# --- TASK COM ---
featureCom = FeatureGeneric('featureCom')
plug(dyn.com,featureCom.errorIN)
@@ -311,10 +303,9 @@ plug(gainPosture.gain,taskPosture.controlGain)
# --- TASK RIGHT FOOT
# Task right hand
-
-plug(pg.rightfootref,taskRF.featureDes.position)
+#plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
-plug(pg.leftfootref,taskLF.featureDes.position)
+#plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5
# ---- SOT ---------------------------------------------------------------------
@@ -322,11 +313,6 @@ taskLF.task.controlGain.value = 5
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
-#sot.push(taskWaist.task.name)
-sot.push(taskRF.task.name)
-sot.push(taskLF.task.name)
-sot.push(taskCom.name)
-
plug(sot.control,robot.control)
# --- TRACER -----------------------------------------------------------------
@@ -345,34 +331,49 @@ tr.add('dyn.lf','l')
tr.add('featureComDes.errorIN','comref')
tr.add('dyn.com','com')
+tr.add(taskWaist.gain.name+'.gain','gainwaist')
history = History(dyn,1)
# --- RUN -----------------------------------------------------------------
-pg.parseCmd(":stepseq " + seqpart)
featurePosture.selec.value = toFlags( range(6,36) )
-attime(300,lambda: sot.push(taskPosture.name),'Add Posture')
-attime(500,lambda: sot.rm(taskCom.name),'rm com')
-attime(1000,lambda: dump(),'dump!')
-attime(1000,lambda: sys.exit(),'Bye bye')
-
-#go()
-next()
-'''
-
sot.clear()
-
for task in [taskWaist, taskRF, taskLF]:
- M = eye(4)
task.feature.position.recompute(0)
- M[0:2,3] = array(task.feature.position.value)[0:2,3]
- M[2,3] = 0.105
- task.ref = matrixToTuple(M)
+ task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
+taskWaist.ref = matrixToTuple(eye(4))
+taskWaist.feature.selec.value = '111011'
+taskWaist.gain.setByPoint(18,0.1,0.005,0.8)
+
+attime(200
+ ,(lambda : sot.rm(taskWaist.task.name),'Rm waist')
+ ,(lambda : pg.initState(),'Init PG')
+ ,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list')
+ ,(lambda : sot.push(taskCom.name),'Push COM')
+ ,(lambda : plug(pg.rightfootref,taskRF.featureDes.position),'Plug RF')
+ ,(lambda : plug(pg.leftfootref,taskLF.featureDes.position),'plug LF' )
+)
+
+
+#attime(700,lambda: sot.push(taskPosture.name),'Add Posture')
+#attime(900,lambda: sot.rm(taskCom.name),'rm com')
+#attime(1200,lambda: dump(),'dump!')
+#attime(1200,lambda: sys.exit(),'Bye bye')
+attime(1200,dump,stop)
+
+go()
+
+
+'''
+
+sot.clear()
+
+
if 0:
Mwref = eye(4)
Mwref[0:2,3] = dyn.com.value[0:2]
--
GitLab