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)