Use the Macro solver in ros scripts.

python/ros/ros-kineromeo.py

@@ -14,6 +14,10 @@
 # received a copy of the GNU Lesser General Public License along with
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from numpy import *
+
 from dynamic_graph.sot.romeo.romeo import *
 robot = Robot('robot')
 plug(robot.device.state, robot.dynamic.position)
@@ -23,34 +27,15 @@ ros = Ros(robot)
 
 
 # Create a solver.
-# from dynamic_graph.sot.dynamics.solver import Solver
-# solver = Solver(robot)
-
-from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
-from dynamic_graph.sot.core.meta_tasks_kine import *
-from numpy import *
-
-
-# --- ROBOT SIMU ---------------------------------------------------------------
-# robotName = 'romeo'
-# robotDim=robotDimension[robotName]
-# robot = RobotSimu("romeo")
-# robot.resize(robotDim)
-dt=5e-3
-
-# robot.set( initialConfig[robotName] )
-# addRobotViewer(robot,small=True,small_extra=24,verbose=False)
-
-# --- ROMEO HANDS ---------------------------------------------------------------
-# robot.gripper=0.0
-# RobotClass = RobotSimu
-# RobotClass.stateFullSizeOld = RobotClass.stateFullSize
-# RobotClass.stateFullSize = lambda x: [float(v) for v in x.state.value+24*(x.gripper,)]
+from dynamic_graph.sot.dynamics.solver import Solver
+solver = Solver(robot)
 
 #-------------------------------------------------------------------------------
 #----- MAIN LOOP ---------------------------------------------------------------
 #-------------------------------------------------------------------------------
+# define the macro allowing to run the simulation.
 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+dt=5e-3
 @loopInThread
 def inc():
     robot.device.increment(dt)
@@ -58,14 +43,6 @@ def inc():
 runner=inc()
 [go,stop,next,n]=loopShortcuts(runner)
 
-# ---- SOT ---------------------------------------------------------------------
-# ---- SOT ---------------------------------------------------------------------
-# ---- SOT ---------------------------------------------------------------------
-
-sot = SOT('sot')
-sot.setNumberDofs(robot.dimension)
-plug(sot.control,robot.device.control)
-
 # ---- TASKS -------------------------------------------------------------------
 # ---- TASKS -------------------------------------------------------------------
 # ---- TASKS -------------------------------------------------------------------
@@ -79,11 +56,6 @@ taskRH.feature.frame('desired')
 # robot.tasks['right-wrist'].add(taskRH.feature.name)
 
 # --- STATIC COM (if not walking)
-# taskCom = MetaTaskKineCom(robot.dynamic)
-# robot.dynamic.com.recompute(0)
-# taskCom.featureDes.errorIN.value = robot.dynamic.com.value
-# taskCom.task.controlGain.value = 10
-
 robot.createCenterOfMassFeatureAndTask(
     'featureCom', 'featureComDef', 'comTask',
     selec = '011',
@@ -104,13 +76,11 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
 target=(0.5,-0.2,0.8)
 gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
 
-# sot.push(taskCom.task.name)
-# sot.push(robot.tasks['right-wrist'].name)
-
-sot.push(contactRF.task.name)
-sot.push(contactLF.task.name)
-sot.push((robot.comTask.name))
-sot.push(taskRH.task.name)
+solver.push(contactRF.task)
+solver.push(contactLF.task)
+solver.push(robot.comTask)
+solver.push(taskRH.task)
 
+# type inc to run one iteration, or go to run indefinitely.
 # go()
 

python/ros/ros-walkromeo.py

@@ -12,32 +12,30 @@ from dynamic_graph.sot.core.meta_tasks_kine import *
 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
 from dynamic_graph.sot.dyninv import SolverKine
 
+# Create the robot.
 from dynamic_graph.sot.romeo.romeo import *
 robot = Robot('robot')
 plug(robot.device.state, robot.dynamic.position)
 
-
-
+# Binds with ros, export joint_state.
 from dynamic_graph.ros import *
 ros = Ros(robot)
 
-
 # Create a solver.
-# from dynamic_graph.sot.dynamics.solver import Solver
-# solver = Solver(robot)
+from dynamic_graph.sot.dynamics.solver import Solver
+solver = Solver(robot)
 
 from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
 from dynamic_graph.sot.core.meta_tasks_kine import *
 from numpy import *
 
 
-# --- ROBOT SIMU ---------------------------------------------------------------
-dt=5e-3
-
-
 #-------------------------------------------------------------------------------
 #----- MAIN LOOP ---------------------------------------------------------------
 #-------------------------------------------------------------------------------
+
+dt=5e-3
+
 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
 @loopInThread
 def inc():
@@ -51,12 +49,6 @@ from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
 pg = MetaPG(robot.dynamic)
 pg.plugZmp(robot.device)
 
-# ---- SOT ---------------------------------------------------------------------
-# The basic SOT solver would work too.
-sot = SolverKine('sot')
-sot.setSize(robot.dimension)
-plug(sot.control,robot.device.control)
-
 # ---- TASKS -------------------------------------------------------------------
 # ---- WAIST TASK ---
 taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist')
@@ -78,13 +70,22 @@ taskLF=MetaTask6d('lf',robot.dynamic,'lf','left-ankle')
 plug(pg.pg.leftfootref,taskLF.featureDes.position)
 taskLF.task.controlGain.value = 40
 
+
+# ---- WAIST TASK ORIENTATION ---
+#  set the orientation of the waist to be the same as the one of the foot.
+taskWaistOr=MetaTask6d('waistOr',robot.dynamic,'waist','waist')
+plug(pg.pg.rightfootref,taskWaistOr.featureDes.position)
+taskWaistOr.task.controlGain.value = 40
+taskWaistOr.feature.selec.value = '100000'
+
 # --- RUN ----------------------------------------------------------------------
 # --- RUN ----------------------------------------------------------------------
 # --- RUN ----------------------------------------------------------------------
-sot.push(taskWaist.task.name)
-sot.push(taskRF.task.name)
-sot.push(taskLF.task.name)
-sot.push(taskCom.task.name)
+solver.push(taskWaist.task)
+solver.push(taskRF.task)
+solver.push(taskLF.task)
+solver.push(taskCom.task)
+solver.push(taskWaistOr.task)
 
 # --- HERDT PG AND START -------------------------------------------------------
 # Set the algorithm generating the ZMP reference trajectory to Herdt's one.
@@ -92,6 +93,6 @@ pg.startHerdt(False)
 # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
 pg.pg.velocitydes.value =(0.1,0.0,0.0)
 
-go()
+# go()