diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 6abe4bda..40527fca 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -2,7 +2,7 @@ INCLUDE(../cmake/python.cmake) FINDPYTHON() INSTALL( - FILES walk_romeo.py + FILES walk_romeo.py walk_romeo_small.py DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial ) diff --git a/python/walk_romeo_small.py b/python/walk_romeo_small.py new file mode 100644 index 00000000..91e4e7b2 --- /dev/null +++ b/python/walk_romeo_small.py @@ -0,0 +1,53 @@ +# ______________________________________________________________________________ +# ****************************************************************************** +# A simple Herdt walking pattern generator for Romeo. +# ______________________________________________________________________________ +# ****************************************************************************** + +# from dynamic_graph.sot.romeo.sot_romeo_controller import * +# Create the robot. +from dynamic_graph.sot.core import * +from dynamic_graph.sot.dynamics import * +from dynamic_graph.sot.romeo.robot import * +robot = Robot('romeo_small', device=RobotSimu('romeo_small')) + +from dynamic_graph import plug +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.application.velocity.precomputed_tasks import initialize +solver = initialize ( robot ) + +from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPG , walkFewSteps, walkAndrei +CreateEverythingForPG ( robot , solver ) +# walkFewSteps ( robot ) +walkAndrei( robot ) + +#------------------------------------------------------------------------------- +#----- MAIN LOOP --------------------------------------------------------------- +#------------------------------------------------------------------------------- + +from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts +dt=5e-3 +@loopInThread +def inc(): + robot.device.increment(dt) + +runner=inc() +[go,stop,next,n]=loopShortcuts(runner) + +# --- HERDT PG AND START ------------------------------------------------------- +# Set the algorithm generating the ZMP reference trajectory to Herdt's one. +# pg.startHerdt(False) + +print('You can now modifiy the speed of the robot by setting pg.pg.velocitydes') +print('example : robot.pg.velocitydes.value =(0.1,0.0,0.0)\n') + +robot.pg.velocitydes.value =(0.1,0.0,0.0) + +go() +