Exemplo n.º 1
0
def initialize (robot, solverType=SOT):
    """
    Initialize the solver, and define shortcuts for the operational points
    """

    # TODO: this should disappear in the end.     
    # --- center of mass ------------
    (robot.featureCom, robot.featureComDes, robot.comTask) = \
        createCenterOfMassFeatureAndTask(robot,
        '{0}_feature_com'.format(robot.name),
        '{0}_feature_ref_com'.format(robot.name),
        '{0}_task_com'.format(robot.name))

    # --- operational points tasks -----
    robot.features = dict()
    robot.tasks = dict()
    for op in robot.OperationalPoints:
        (robot.features[op], robot.tasks[op]) = \
            createOperationalPointFeatureAndTask(robot,
            op, '{0}_feature_{1}'.format(robot.name, op),
            '{0}_task_{1}'.format(robot.name, op))
        # define a member for each operational point
        w = op.split('-')
        memberName = w[0]
        for i in w[1:]:
            memberName += i.capitalize()
        setattr(robot, memberName, robot.features[op])

    initializeSignals (robot)

    # --- create solver --- #
    solver = Solver (robot, solverType)

    # --- push balance task --- #
    metaContact = Pr2ContactTask(robot)
    robot.tasks ['contact'] = metaContact.task
    robot.features ['contact'] = metaContact.feature
    metaContact.feature.selec.value = '011100'
    metaContact.featureDes.position.value = \
      array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]])
    solver.push(robot.tasks['contact'])
#    solver.sot.addContact(robot.tasks['contact'])

    return solver
Exemplo n.º 2
0
def initialize(robot, solverType=SOT):
    """
    Initialize the solver, and define shortcuts for the operational points
    """

    # TODO: this should disappear in the end.
    # --- center of mass ------------
    (robot.featureCom, robot.featureComDes, robot.comTask) = \
        createCenterOfMassFeatureAndTask(robot,
        '{0}_feature_com'.format(robot.name),
        '{0}_feature_ref_com'.format(robot.name),
        '{0}_task_com'.format(robot.name))

    # --- operational points tasks -----
    robot.features = dict()
    robot.tasks = dict()
    for op in robot.OperationalPoints:
        (robot.features[op], robot.tasks[op]) = \
            createOperationalPointFeatureAndTask(robot,
            op, '{0}_feature_{1}'.format(robot.name, op),
            '{0}_task_{1}'.format(robot.name, op))
        # define a member for each operational point
        w = op.split('-')
        memberName = w[0]
        for i in w[1:]:
            memberName += i.capitalize()
        setattr(robot, memberName, robot.features[op])

    initializeSignals(robot)

    # --- create solver --- #
    solver = Solver(robot, solverType)

    # --- push balance task --- #
    metaContact = Pr2ContactTask(robot)
    robot.tasks['contact'] = metaContact.task
    robot.features['contact'] = metaContact.feature
    metaContact.feature.selec.value = '011100'
    metaContact.featureDes.position.value = \
      array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]])
    solver.push(robot.tasks['contact'])
    #    solver.sot.addContact(robot.tasks['contact'])

    return solver
Exemplo n.º 3
0
# been loaded in robot_description parameter
# on the Ros Parameter Server
from dynamic_graph.sot.pr2.robot import Pr2
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = youbot('youbot', device=RobotSimu('youbot'))
plug(robot.device.state, robot.dynamic.position)

# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)

# 3. Create a solver
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)

# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple

taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_joint_5')
Pr2handMgrip = eye(4)
Pr2handMgrip[0:3, 3] = (0.337, 0, 0.275)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
taskRH.feature.frame('desired')
targetR = (0.65, 0.2, 0.9)
selec = '111'
gain = (4.9, 0.9, 0.01, 0.9)
gotoNd(taskRH, targetR, selec, gain)
Exemplo n.º 4
0
# been loaded in robot_description parameter
# on the Ros Parameter Server
from dynamic_graph.sot.pr2.robot import Pr2
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = youbot('youbot', device=RobotSimu('youbot'))
plug(robot.device.state, robot.dynamic.position)

# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)

# 3. Create a solver
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)

# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple

taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','arm_joint_5')
Pr2handMgrip = eye(4); Pr2handMgrip[0:3,3] = (0.337,0,0.275)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
taskRH.feature.frame('desired')
targetR=(0.65,0.2,0.9)
selec='111'
gain=(4.9,0.9,0.01,0.9)
gotoNd(taskRH,targetR,selec,gain)
        res.append( tuple(al[i]) )
    return tuple(res)

# Create the robot.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('romeo', device=RobotSimu('romeo'))
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 Solver

solver = Solver(robot)

# Alternate visualization tool
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)


#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------

dt=5e-3

@loopInThread
def inc():
    robot.device.increment(dt)
    updateComDisplay(robot.device,robot.dynamic.com)