Ejemplo n.º 1
0
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])
Ejemplo n.º 2
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)

Ejemplo n.º 4
0
# 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

Ejemplo n.º 5
0
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()