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