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)