diff --git a/python/ros/ros-walkromeo.py b/python/ros/ros-walkromeo.py index 5b57d0fd6dbe00b99fcc2399562cd0bc28bc7282..73bd02e9ac299d89cfd67bf68d92118baeca72cd 100644 --- a/python/ros/ros-walkromeo.py +++ b/python/ros/ros-walkromeo.py @@ -10,11 +10,14 @@ from dynamic_graph.sot.dynamics import * from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY 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.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay from dynamic_graph.sot.dyninv import SolverKine +from numpy import * + # Create the robot. from dynamic_graph.sot.romeo.romeo import * -robot = Robot('robot') +robot = Robot('romeo', device=RobotSimu('romeo')) plug(robot.device.state, robot.dynamic.position) # Binds with ros, export joint_state. @@ -25,9 +28,8 @@ ros = Ros(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 * +# Alternate visualization tool +addRobotViewer(robot.device,small=True,small_extra=24,verbose=False) #------------------------------------------------------------------------------- @@ -36,10 +38,10 @@ from numpy import * dt=5e-3 -from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts @loopInThread def inc(): robot.device.increment(dt) + updateComDisplay(robot.device,robot.dynamic.com) runner=inc() [go,stop,next,n]=loopShortcuts(runner) @@ -51,17 +53,20 @@ pg.plugZmp(robot.device) # ---- TASKS ------------------------------------------------------------------- # ---- WAIST TASK --- +# This task fix the motion of the waist around the z axis, the roll and the pitch taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist') pg.plugWaistTask(taskWaist) taskWaist.task.controlGain.value = 5 taskWaist.feature.selec.value = '011100' # --- TASK COM --- +# the x,y coords of the centor of mass are given by the pattern generator taskCom = MetaTaskKineCom(robot.dynamic,"compd") pg.plugComTask(taskCom) taskCom.feature.selec.value = '011' # --- TASK FEET +# The feet are constrained by two 6dofs tasks. taskRF=MetaTask6d('rf',robot.dynamic,'rf','right-ankle') plug(pg.pg.rightfootref,taskRF.featureDes.position) taskRF.task.controlGain.value = 40 @@ -74,10 +79,17 @@ 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 +plug(pg.pg.rightfootref,taskWaistOr.featureDes.position) taskWaistOr.feature.selec.value = '100000' +# ---- HEAD ORIENTATION --- +# set the orientation of the gaze (head) to be the same as the one of the foot. +taskHead=MetaTask6d('head',robot.dynamic,'gaze','gaze') +plug(taskRF.featureDes.position, taskHead.featureDes.position) +taskHead.feature.selec.value = '111000' +taskHead.task.controlGain.value = 5 + # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- @@ -90,9 +102,10 @@ solver.push(taskWaistOr.task) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. 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() +print('You can now modifiy the speed of the robot by setting pg.pg.velocitydes') +print('example : pg.pg.velocitydes.value =(0.1,0.0,0.0)\n') + +go()