From e7135d12970df54dc7e17a2579a331d4e1b7f2c9 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Wed, 17 Aug 2011 15:48:05 +0200
Subject: [PATCH] Current working version, comparing both regular solver and
reduced solver.
---
python/dynreduced.py | 156 ++++++++++++++++++++++++++++++++++---------
1 file changed, 126 insertions(+), 30 deletions(-)
diff --git a/python/dynreduced.py b/python/dynreduced.py
index d44546e..279a05e 100644
--- a/python/dynreduced.py
+++ b/python/dynreduced.py
@@ -25,6 +25,13 @@ def totuple( a ):
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
+initialConfig['hrp14small'] = ( 0.0274106623863, 0.143843868989, 0.646921914726, 0.00221064938462, 0.101393756965, 1.36729741242e-05,
+ -0.00911630330837, -0.0914099637938, -0.471978743283, 0.840380192617, -0.470232799053, 0.0896624078591,
+0.00950781802257, 0.0911102868041, -0.469450351848, 0.835307995386, -0.467686190904, -0.0938029466367,
+ 0.3, 0.3, 0, 0,
+ 0.6, -0.3, 0, -0.52, 0, -0.1, 0,
+-0.2, 0.17, 0, -0.52, 0, 0.1, 0 )
+
robotDim=robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
@@ -172,9 +179,15 @@ contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-
contactRF.feature.frame('desired')
# --- SOT Dyn OpSpaceH --------------------------------------
+rs=True # Regular solver
+#rs=False # Reduced solver
+
# SOT controller.
-sot = SolverDynRed2('sot')
-#sot = SolverOpSpace('sot')
+if rs:
+ sot = SolverOpSpace('sot')
+else:
+ sot = SolverDynRed2('sot')
+
sot.setSize(robotDim-6)
#sot.damping.value = 2e-2
sot.breakFactor.value = 10
@@ -183,9 +196,16 @@ plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
-plug(sot.torque,robot.control)
+if rs: plug(sot.control,robot.control)
+else: plug(sot.solution,robot.control)
plug(sot.acceleration,robot.acceleration)
+sot.addContactFromTask(contactLF.task.name,'LF')
+sot._LF_p.value = contactLF.support
+sot.addContactFromTask(contactRF.task.name,'RF')
+sot._RF_p.value = contactRF.support
+
+
# --- TRACE ----------------------------------------------
from dynamic_graph.tracer import *
@@ -205,15 +225,31 @@ tr.add('taskCom.error','com')
tr.add('taskCom.task','')
tr.add(contactLF.task.name+'.error','lf')
+if not rs: tr.add('sot.solution','')
+
+if rs:
+ tr.add('sot._LF_f','fl')
+ tr.add('sot._RF_f','fr')
+ tr.add('sot._LF_fn','fnl')
+ tr.add('sot._RF_fn','fnr')
+else:
+ tr.add('sot.forces','')
+ robot.after.addSignal('sot.forces')
+
+if not rs:
+ tr.add('sot.forcesNormal','')
+ robot.after.addSignal('sot.forcesNormal')
+ tr.add('sot.activeForces','')
+ robot.after.addSignal('sot.activeForces')
+
# --- RUN ------------------------------------------------
-sot.addContactFromTask(contactLF.task.name,'LF')
-sot._LF_p.value = contactLF.support
-sot.addContactFromTask(contactRF.task.name,'RF')
-sot._RF_p.value = contactRF.support
taskCom.controlGain.value = 100
#featureComDes.errorIN.value = (0.06, 0.15, 0.8)
-featureComDes.errorIN.value = (0.06, 0.145, 0.8 )
+#featureComDes.errorIN.value = (0.06, 0.145, 0.8 )
+
+featureComDes.errorIN.value = (0.1, 0.145, 0.8 )
+#Nominal: no zmp overshoot featureComDes.errorIN.value = (0.084, 0.144, 0.804 )
sot.push('taskCom')
#@attime(5)
@@ -222,34 +258,94 @@ def rm():
attime(500,stop,'stop')
-plug(robot.state,sot.position)
-sot.posture.value = ( 0,0,0,0,0,0, -0.009116303, -0.091409964, -0.471978743, 0.840380193, -0.470232799, 0.089662408, 0.009507818, 0.091110287, -0.469450352, 0.835307995, -0.467686191, -0.093802947, -0.000087565, 0.003264319, -0.000007834, 0.000194743, 0.258370257, -0.175099102, -0.000061173, -0.524953549, 0.000003183, -0.000257600, -0.000003412, 0.258367272, 0.174322098, -0.000088902, -0.524983691, -0.000000346, -0.000265401, 0.3 )
+#plug(robot.state,sot.position)
+#sot.posture.value = ( 0,0,0,0,0,0, -0.009116303, -0.091409964, -0.471978743, 0.840380193, -0.470232799, 0.089662408, 0.009507818, 0.091110287, -0.469450352, 0.835307995, -0.467686191, -0.093802947, -0.000087565, 0.003264319, -0.000007834, 0.000194743, 0.258370257, -0.175099102, -0.000061173, -0.524953549, 0.000003183, -0.000257600, -0.000003412, 0.258367272, 0.174322098, -0.000088902, -0.524983691, -0.000000346, -0.000265401, 0.3 )
+
+
+class Chrono:
+ t0=0
+ def __init__(self):
+ self.t0 = time.time()
+ def tic(self):
+ print 'elapsed time = ',time.time()-self.t0
+chrono=Chrono()
+attime(499,chrono.tic)
+
+go()
+
matlab.prec=9
matlab.fullPrec=0
+'''
+for i in range(25): inc()
-for i in range(100): inc()
print sot.velocity.m
print sot.dyndrift.m
-print sot.forceGenerator.m
-print sot.Jc.m
-print sot.freeMotionBase.m
-print sot.freeForceBase.m
-print sot.inertiaSqroot.m
-print sot.inertiaSqrootInv.m
-print sot.driftContact.m
-print taskCom.jacobian.m
-print taskCom.task.m
-print taskCom.Jdot.m
-print "dq=velocity; X=forceGenerator; V=freeMotionBase; K = freeForceBase; B=inertiaSqrootInv; Bi=inertiaSqroot; Jcom=jacobian; b=dyndrift; A=Bi'*Bi; dc = driftContact; J=X*Jc; G=J*B; Gc=Jc*B;"
-
-print sot.solution.m
+print sot.matrixInertia.m
print sot.acceleration.m
-sot.forces.recompute(sot.acceleration.time)
-print sot.forces.m
-print "x=solution; ddq=acceleration; f=forces; phi = X'*f;"
-print "u=x(1:24); psi=x(25:end);"
+print "dq=velocity; b=dyndrift; A=matrixInertia; ddq=acceleration;"
+
+if rs:
+ print sot.control.m
+ print sot._LF_f.m
+ print sot._LF_fn.m
+ print sot._RF_f.m
+ print sot._RF_fn.m
+ print "ddq1=ddq; phil1= _LF_f; phir1 = _RF_f; phi1=[ phil1; phir1 ]; fnl1=_LF_fn; fnr1 = _RF_fn; fn1 = [fnl1; fnr1 ]; tau1=control;"
+
+else:
+ print sot.sizeForceSpatial.m
+ print sot.velocity.m
+ print sot.dyndrift.m
+ print sot.forceGenerator.m
+ print sot.Jc.m
+ print sot.freeMotionBase.m
+ print sot.freeForceBase.m
+ print sot.inertiaSqroot.m
+ print sot.inertiaSqrootInv.m
+ print sot.driftContact.m
+ print taskCom.jacobian.m
+ print taskCom.task.m
+ print taskCom.Jdot.m
+ print "X=forceGenerator; V=freeMotionBase; K = freeForceBase; B=inertiaSqrootInv; Bi=inertiaSqroot; Jcom=jacobian; Ab=Bi'*Bi; dc = driftContact; J=X*Jc; G=J*B; Gc=Jc*B; Sb=[eye(6) zeros(6,30)];"
+
+ print sot.solution.m
+ print sot.acceleration.m
+ sot.forces.recompute(sot.acceleration.time)
+ print sot.forces.m
+ print "x=solution; ddq=acceleration; f=forces; phi = X'*f;"
+ print "u=x(1:24); psi=x(25:end); "
+
+ print "ddq2= ddq; phi2=phi; f2=f; fn2=f(3:3:end);"
+'''
+
+
+if 0: # double check
+ sotreg = SolverOpSpace('sotreg')
+ sotreg.setSize(robotDim-6)
+ sotreg.breakFactor.value = 10
+
+ plug(dyn.inertiaReal,sotreg.matrixInertia)
+ plug(dyn.dynamicDrift,sotreg.dyndrift)
+ plug(dyn.velocity,sotreg.velocity)
+
+ sotreg.addContactFromTask(contactLF.task.name,'LF')
+ sotreg._LF_p.value = contactLF.support
+ sotreg.addContactFromTask(contactRF.task.name,'RF')
+ sotreg._RF_p.value = contactRF.support
+
+ sotreg.push('taskCom')
+ sotreg.control.recompute(sot.acceleration.time)
+
+ print sotreg.control.m
+ print sotreg._LF_f.m
+ print sotreg._LF_fn.m
+ print sotreg._RF_f.m
+ print sotreg._RF_fn.m
+ print sotreg.acceleration.m
+ print "ddq1=acceleration; phil1= _LF_f; phir1 = _RF_f; phi1=[ phil1; phir1 ]; fnl1=_LF_fn; fnr1 = _RF_fn; fn1 = [fnl1; fnr1 ]; tau1=control;"
+
+ print taskCom.task.m
+
-#sot.solution.recompute(1)
-#sot.acceleration.recompute(1)
--
GitLab