From 75dba1ec40acef6323699d9c484e6076c6e8a5bc Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Wed, 14 Sep 2011 14:28:44 +0200
Subject: [PATCH] Slight refactoring.
---
python/mocap/slide.py | 30 ++++++++++++++++++++++++------
1 file changed, 24 insertions(+), 6 deletions(-)
diff --git a/python/mocap/slide.py b/python/mocap/slide.py
index 795c821..2895be7 100644
--- a/python/mocap/slide.py
+++ b/python/mocap/slide.py
@@ -115,7 +115,7 @@ def status(): print runner.isPlay
@optionalparentheses
def dump():
- history.dumpToOpenHRP('openhrp/slide')
+ history.dumpToOpenHRP('openhrp/slide-2')
attime.addPing( VisualPinger(robot.viewer) )
@@ -269,7 +269,7 @@ zmp.declare(sot,dyn)
from dynamic_graph.tracer import *
tr = Tracer('tr')
-tr.open('/tmp/','slide_','.dat')
+tr.open('/tmp/','slide-2_','.dat')
tr.add('dyn.com','com')
tr.add(taskCom.feature.name+'.error','ecom')
@@ -303,6 +303,10 @@ history = History(dyn,1,zmp.zmp)
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
+RADIUS = (0.35,-0.2)
+#RADIUS = (0.4,-0.42) # WARNING: this version induce a collision of the hips.
+
+
q0 = robot.state.value
sot.clear()
@@ -317,7 +321,7 @@ rfz0=0.020
rf0=matrix((0.0095,-0.095,rfz0))
#traj.set( vectorToTuple(rf0[0,0:2]),(0.35,-0.2),1200 )
-traj.set( vectorToTuple(rf0[0,0:2]),(0.4,-0.42),1200 )
+traj.set( vectorToTuple(rf0[0,0:2]),RADIUS,1200 )
mrf=eye(4)
mrf[0:3,3] = (0,0,-0.105)
@@ -381,12 +385,26 @@ attime(1550
)
attime(1600 ,(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" )
+ ,(lambda: taskCom.gain.setByPoint(20,5,0.005,0.8) , "upper com gain" )
+ , lambda: sot.rm( taskrf.task.name)
+ , lambda: sot.rm( taskrfz.task.name)
)
-attime(1800, lambda: sigset(sot.posture,q0), "Robot to initial pose")
+attime(2500
+ ,(lambda: sigset(sot.posture,q0), "Robot to initial pose")
+ ,(lambda: sigset(sot.breakFactor,3), "Lower posture gain")
+ , lambda: sot.rm( taskHead.task.name)
+ )
-attime(2000,stop)
+attime(3000,stop)
+'''
+robot.set( (0.0020606154246394122, 0.10134042021667664, 0.61936165668568544, -0.0069259319186176036, -0.088137660944143853, 0.030236257457065652, -0.023750073776956582, -0.1957733198575172, -0.46599463380389894, 1.0973898715109023, -0.54132289909300224, 0.19981647227445462, -0.012633700355146953, -0.19087680943253901, -0.45975963797457531, 1.0637916492234973, -0.51408979115153641, 0.19590665168251745, -0.075500171543301034, 0.079772437229333554, -0.028852479345143907, 0.0086379205967497454, 0.24033491666650419, -0.36260993615301357, -0.13416418244023331, -1.1916854959672925, 0.0064805605150930791, -0.67051517004062, 0.1749447362673188, 0.2361617052377866, 0.39587454978519498, 0.1548077334633976, -1.1789536523601791, -0.002872468654339478, -0.66375196866756503, 0.17491113084058069) )
+robot.setVelocity( (0.0012245359646462779, -0.00032846450320344624, -0.00093107652772514925, -0.0007955865184161037, -0.011820643723145284, 0.040775490423764035, 0.035389733758687876, -0.0014469585725611232, -0.0047820151310369683, 0.0007092489354239243, 0.016061287694630787, 0.0051663205983227952, -0.038343004278138207, -0.0021817700955914393, 0.0065270913884630192, 0.0067049582970967834, -0.00096588953172016101, -0.00036702270414771848, -0.00030275572644021824, 0.0053179853289967161, -4.2997566323087006e-05, 0.0065349436167033929, 0.00054950871518563607, 0.00043593391876344098, 0.00016810635101048734, 6.2907669506110598e-05, 2.3928791393290976e-05, -2.3184107987587771e-05, 3.7224090914736729e-06, 0.00054402320867447302, 0.00031356537946185351, 0.00021647307753845703, 0.0002087522875211192, 2.5324863895013726e-05, 1.4465869940528473e-05, -2.4050288069934116e-06) )
+T0 = 1500
+robot.state.time = T0
+[ t.feature.position.recompute(T0) for t in taskrh,tasklh]
+attime.fastForward(T0)
+'''
inc()
go()
--
GitLab