Commit 5a1b8ecc authored by Francois Keith's avatar Francois Keith
Browse files

Clean and comment the script ros-walkromeo.

Add the possibility to use both viewer.
parent d0bd2a2b
......@@ -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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment