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
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)
# 5. Add a contact constraint with the robot and the floor
contact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
contact.feature.frame('desired')
contact.feature.selec.value = '011100'
contact.gain.setConstant(10)
contact.keep()
locals()['contactBase'] = contact
# 6. Push tasks in the solver
solver.push(taskRH.task)
solver.push(contactBase.task)
# Main loop
dt = 3e-3
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner = inc()
runner.once()
[go, stop, next, n] = loopShortcuts(runner)
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)
# 5. Add a contact constraint with the robot and the floor
contact = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle')
contact.feature.frame('desired')
contact.feature.selec.value = '011100'
contact.gain.setConstant(10)
contact.keep()
locals()['contactBase'] = contact
# 6. Push tasks in the solver
solver.push(taskRH.task)
solver.push(contactBase.task)
# Main loop
dt=3e-3
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
runner.once()
[go,stop,next,n]=loopShortcuts(runner)
print 'Type go to run the solver loop'
# 3\ (Optional) attach an adaptive gain (entity GainAdaptive) to the task created.
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1,0.1,125e3)
gainPosition.gain.value = 5
plug(taskPosition.error,gainPosition.error)
plug(gainPosition.gain,taskPosition.controlGain)
featurePosition.selec.value = '000000000000001111111111111100000000000'
############################################################################
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
solver.push(taskWaist.task)
solver.push(taskRF.task)
solver.push(taskLF.task)
solver.push(taskCom.task)
# Stun the upper part of the body.
solver.push(taskHead.task) # constraint the head orientation: look straight ahead
solver.push(taskPosition) # stun the arms.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
print('You can now modifiy the speed of the robot by setting pg.pg.velocitydes')
print('example : pg.pg.velocitydes.value =(0.1,0.0,0.0)\n')
