initialConfig['hrp10small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, 0, -0.5236, 0, 0, 0, 0, 0.2618, 0.1745, 0, -0.5236, 0, 0, 0, 0 ) initialConfig['hrp14small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, -0.5236, 0, 0, 0, 0, 0.2618, 0.1745, -0.5236, 0, 0, 0, 0 ) # ------------------------------------------------------------------------------ # --- ROBOT DYNAMIC SIMULATION ------------------------------------------------- # ------------------------------------------------------------------------------ robotName = 'hrp14small' robotDim = robotDimension[robotName] RobotClass = RobotDynSimu robot = RobotClass("robot") robot.resize(robotDim) robot.set( initialConfig[robotName] ) addRobotViewer(robot,small=True,verbose=False) dt=5e-3 # ------------------------------------------------------------------------------ # --- MAIN LOOP ---------------------------------------------------------------- # ------------------------------------------------------------------------------ def inc(): robot.increment(dt) attime.run(robot.control.time) if 'refresh' in ZmpEstimator.__dict__: zmp.refresh() # robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0]) # if dyn.com.time >0: # robot.viewer.updateElementConfig('com',[dyn.com.value[0],dyn.com.value[1],0,0,0,0])
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple, rotate, matrixToRPY # from dynamic_graph.sot.core.utils.history import History # from dynamic_graph.sot.dyninv.zmp_estimator import ZmpEstimator from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer, VisualPinger #----------------------------------------------------------------------------- # --- ROBOT SIMU ------------------------------------------------------------- #----------------------------------------------------------------------------- #robotName = 'hrp14small' robotName = 'romeo' robotDim = robotDimension[robotName] RobotClass = RobotDynSimu robot = RobotClass("romeo") robot.resize(robotDim) addRobotViewer(robot, small=True, small_extra=24, verbose=False) # Similar initial position with hand forward robot.set(initialConfig[robotName]) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- dt = 5e-3 def inc(): robot.increment(dt) # Execute a function at time t, if specified with t.add(...) # if 'refresh' in ZmpEstimator.__dict__: zmp.refresh()
## Create the robot romeo. from dynamic_graph.sot.romeo.robot import * robot = Robot('romeo', device=RobotSimu('romeo')) ## Binds with ROS. assert that roscore is running. from dynamic_graph.ros import * ros = Ros(robot) # Create a simple kinematic solver. from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize ( 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 --------------------------------------------------------------- #------------------------------------------------------------------------------- # 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) runner=inc() [go,stop,next,n]=loopShortcuts(runner)
# 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) # Alternate visualization tool from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer addRobotViewer(robot.device, small=True, small_extra=0, verbose=False) # state publisher from vec2odo.msg import * import rospy pub = rospy.Publisher('state_vector_stream', vector6) rospy.init_node('vec6_publisher') #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts dt = 5e-3
robot = RobotSimu("robot") robot.resize(robotDim) dt = 5e-3 from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig x0 = -0.00949035111398315034 y0 = 0 z0 = 0.64870185118253043 halfSittingConfig[robotName] = (x0, y0, z0, 0, 0, 0) + halfSittingConfig[robotName][6:] q0 = list(halfSittingConfig[robotName]) initialConfig[robotName] = tuple(q0) robot.set(initialConfig[robotName]) addRobotViewer(robot, small=True, verbose=True) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts @loopInThread def inc(): robot.increment(dt) attime.run(robot.control.time) updateComDisplay(robot, dyn.com) history.record()
robotDim = robotDimension[robotName] dt = 5e-3 from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig x0 = -0.00949035111398315034 y0 = 0 z0 = 0.64870185118253043 # halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+initialConfig[robotName][6:] halfSittingConfig[robotName] = initialConfig[robotName] q0 = list(halfSittingConfig[robotName]) initialConfig[robotName] = tuple(q0) #robot.set( initialConfig[robotName] ) robot.halfSitting = halfSittingConfig addRobotViewer(robot.device, small=True, small_extra=24, verbose=True) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts @loopInThread def inc(): robot.device.increment(dt) attime.run(robot.device.control.time) updateComDisplay(robot, robot.dynamic.com) #history.record()