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()
