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