def setRootPosition(robot, root):
h**o = root[0:2] + (
0,
0,
0,
) + (root[2], )
print h**o
gotoNd(taskBase, h**o, '100011')
# --- CONTACTS
#define contactLF and contactRF
for name,joint in [ ['LF',robot.OperationalPointsMap['left-ankle']], ['RF',robot.OperationalPointsMap['right-ankle'] ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
target = (0.5,-0.2,1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskRH.task.name)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def runner(n):
for i in xrange(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
#runner(3000)
def setRootPosition(robot, root): h**o = root[0:2] + (0,0,0,) + (root[2],) print h**o gotoNd(taskBase, h**o, '100011')
ros = Ros(robot) # Create a simple kinematic solver. from dynamic_graph.sot.dyninv import SolverKine # load the initialization prototype for the pr2 from dynamic_graph.sot.pr2.pr2_tasks import * solver = initialize(robot, SolverKine) # allows the publication of the velocity in the JointState message plug(solver.jointLimitator.control, ros.rosJointState.velocity) #### Build the stack of tasks taskBase = Pr2BaseTask(robot) gotoNd(taskBase, (0,0,0,0,0,0), '100011') solver.push(taskBase.task) # 3. Init Tasks initPostureTask(robot) solver.push(robot.tasks['robot_task_position']) # set Zero to all the joints. def setZero(robot): robot.features['featurePosition'].posture.value = (0,)* len(robot.halfSitting) # set Zero to all the joints. def setInitial(robot): robot.features['featurePosition'].posture.value =robot.halfSitting
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)
# 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
taskContact = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle')
taskContact.feature.frame('desired')
taskContact.feature.selec.value = '011100'
taskContact.gain.setConstant(10)
taskFixed = MetaTaskKine6d('contactFixed',robot.dynamic,'contact','left-ankle')
taskFixed.feature.frame('desired')
taskFixed.gain.setConstant(10)
# Problem
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd
targetRH = (0.65,-0.2,0.9)
targetLH = (0.1, 0.0,1.2)
#targetLH = targetRH
gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9))
gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9))
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
return sot
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
# 3. Init Tasks taskRH = Pr2RightHandTask(robot) taskLH = Pr2LeftHandTask(robot) taskJL = Pr2JointLimitsTask(robot, dt) taskFov = Pr2FoVTask(robot, dt) taskBase = Pr2BaseTask(robot) taskChest = Pr2ChestTask(robot) (taskWeight, featureWeight) = Pr2Weight(robot) initPostureTask(robot) # 4. Formulate problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd # 4.0 chest gotoNd(taskChest, (-0.05, 0.0, 1), "111", (4.9, 0.9, 0.01, 0.9)) # 4.1 Right hand targetRH = (0.60, -0.2, 0.8) gotoNd(taskRH, targetRH, "111", (4.9, 0.9, 0.01, 0.9)) # 4.2 Left hand targetLH = (0.65, 0.2, 0.8) gotoNd(taskLH, targetLH, "111", (4.9, 0.9, 0.01, 0.9)) # 4.3 Look at the right hand target taskFov.goto3D(targetRH) # 4.4 Base position targetBase = (0, 0, 0, 0, 0, 0) gotoNd(taskBase, targetBase, "100011", (4.9, 0.9, 0.01, 0.9))
# Use kine solver (with inequalities)
solver = initialize(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','right-wrist')
Pr2handMgrip = eye(4); Pr2handMgrip[0:3,3] = (0.18,0,0)
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)
# 6. Push tasks in the solver
solver.push(taskRH.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)
contactLF.gain.setConstant(10)
contactLF.keep()
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
targetRH = (0.5, -0.2, 1.0)
targetLH = (0.5, 0.2, 1.0)
# addRobotViewer(robot, small=False)
# robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9))
gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskLH.task.name)
sot.push(taskRH.task.name)
toe = TOE("test_entity_toe")
id4 = ((1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.0),
(0.0, 0.0, 0.0, 1.0))
epsilon = 0.1
ref_offset = 0.1
tau_offset = np.asarray((ref_offset, ) * njoints).squeeze()
toe.init(urdfPath, id4, epsilon, OperationalPointsMap['waist'],
OperationalPointsMap['chest'])
# --- CONTACTS
#define contactLF and contactRF
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact' + name] = contact
target = (0.5, -0.2, 1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskRH.task.name)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
def runner(n):
for i in xrange(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
robot.device.control.unplug() plug(solver.sot.control,robot.device.control) dt = 0.001 taskRH = Pr2RightHandTask(robot) taskLH = Pr2LeftHandTask(robot) taskJL = Pr2JointLimitsTask(robot,dt) taskContact = Pr2ContactTask(robot) taskFov = Pr2FoVTask(robot,dt) taskBase = Pr2BaseTask(robot) rgrip = Pr2RightGripper(robot) lgrip = Pr2LeftGripper(robot) from dynamic_graph.sot.core.meta_tasks_kine import gotoNd targetRH = (0.60,-0.2,0.8) gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9)) targetLH = (0.65,0.3,0.6) gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9)) taskFov.goto3D(targetRH) targetBase = (0.2, -0.1, 0, 0, 0, 0.0) gotoNd(taskBase,targetBase,'100011',(4.9,0.9,0.01,0.9)) rgrip.open() lgrip.open() solver.push(taskRH.task) solver.push(taskLH.task) solver.push(rgrip.task) solver.push(lgrip.task) solver.push(taskBase.task)
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
dt = 3e-3
@loopInThread
def inc():
robot.device.increment(dt)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
#### Build the stack of tasks
taskBase = Pr2BaseTask(robot)
gotoNd(taskBase, (0, 0, 0, 0, 0, 0), '100011')
solver.push(taskBase.task)
# 3. Init Tasks
initPostureTask(robot)
solver.push(robot.tasks['robot_task_position'])
# set Zero to all the joints.
def setZero(robot):
robot.features['featurePosition'].posture.value = (0, ) * len(
robot.halfSitting)
# set Zero to all the joints.
def setInitial(robot):
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import *
ros = Ros(robot)
# Use kine solver (with inequalities)
from dynamic_graph.sot.dyninv import SolverKine
solver = initialize(robot, SolverKine)
#Define the main tasks
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, goto6d, MetaTaskKine6d
taskBase = BoxyBaseTask(robot)
currentWaistPos = robot.features['waist'].position.value
gotoNd(taskBase, currentWaistPos, '111111')
solver.push(taskBase.task)
#Add a task to immobilize the arms base
robot.features['featurePositionBase'] = FeaturePosture('featurePositionBase')
plug(robot.device.state, robot.features['featurePositionBase'].state)
robot.features['featurePositionBase'].posture.value = robot.halfSitting
postureTaskDofs = [False]*(len(robot.halfSitting))
postureTaskDofs[8] = True
for dof,isEnabled in enumerate(postureTaskDofs):
if dof > 5:
robot.features['featurePositionBase'].selectDof(dof,isEnabled)
robot.tasks['task_base_imo']=Task('task_base_imo')
robot.tasks['task_base_imo'].add('featurePositionBase')
gainPosition = GainAdaptive('gainPositionBase')
# 3. Init Tasks taskRH = Pr2RightHandTask(robot) taskLH = Pr2LeftHandTask(robot) taskJL = Pr2JointLimitsTask(robot, dt) taskFov = Pr2FoVTask(robot, dt) taskBase = Pr2BaseTask(robot) taskChest = Pr2ChestTask(robot) (taskWeight, featureWeight) = Pr2Weight(robot) initPostureTask(robot) # 4. Formulate problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd # 4.0 chest gotoNd(taskChest, (-0.05, 0.0, 1), '111', (4.9, 0.9, 0.01, 0.9)) # 4.1 Right hand targetRH = (0.60, -0.2, 0.8) gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9)) # 4.2 Left hand targetLH = (0.65, 0.2, 0.8) gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9)) # 4.3 Look at the right hand target taskFov.goto3D(targetRH) # 4.4 Base position targetBase = (0, 0, 0, 0, 0, 0) gotoNd(taskBase, targetBase, '100011', (4.9, 0.9, 0.01, 0.9))
