Commit d0bd2a2b authored by Francois Keith's avatar Francois Keith
Browse files

Clean and comment the script ros-planche with romeo.

Add the possibility to use both viewer.
parent 2b0cc83d
......@@ -24,21 +24,27 @@ from dynamic_graph.sot.core.utils.attime import attime
from numpy import *
from dynamic_graph.sot.dyninv.robot_specific import postureRange
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger
from dynamic_graph.tracer import *
#-----------------------------------------------------------------------------
# --- ROBOT SIMU -------------------------------------------------------------
#-----------------------------------------------------------------------------
# create the robot. Use a RobotDynSimu for the integration.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot', device=RobotDynSimu('robot'))
robot = Robot('romeo', device=RobotDynSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
# Binds with ROS. assert that roscore is running.
# if you prefer a ROS free installation, please comment those lines.
# Export joint_state into ros.
from dynamic_graph.ros import *
ros = Ros(robot)
# Alternate visualization tool
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
......@@ -70,6 +76,7 @@ plug(robot.device.state, robot.dynamic.position)
plug(robot.device.velocity,robot.dynamic.velocity)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
# set the free flyer free.
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
......@@ -77,8 +84,6 @@ robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
#-----------------------------------------------------------------------------
# --- OPERATIONAL TASKS (For HRP2-14)---------------------------------------------
#-----------------------------------------------------------------------------
......@@ -132,6 +137,8 @@ plug(robot.dynamic.inertiaReal,sot.matrixInertia)
plug(robot.dynamic.dynamicDrift,sot.dyndrift)
plug(robot.dynamic.velocity,sot.velocity)
# replug the control signal
robot.device.control.unplug()
plug(sot.solution, robot.device.control)
#For the integration of q = int(int(qddot)).
......@@ -208,11 +215,10 @@ plug(robot.device.state,sot.position)
q0 = robot.device.state.value
rf0 = matrix(taskrf.feature.position.value)[0:3,3]
MetaTaskDynPosture.postureRange = postureRange['romeo']
# --- Events ---------------------------------------------
# The sequence of tasks.
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
......@@ -227,7 +233,6 @@ attime(140
,(lambda: gotoNd(taskrf, (0,0,0.65),"000110" ), "Up foot RF")
)
# Was -.8,0,.8 with full extension
attime(500,lambda: gotoNd(taskrf, (-0.55,0,0.7),"000101" ), "Extend RF")
attime(800,lambda: sot.push(taskChest.task.name), "add chest")
......@@ -279,7 +284,8 @@ attime(2650+tex ,(lambda: contact(contactRF) , "RF to contact" )
attime(3200+tex,stop)
go()
# start the motion
# 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