from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY from dynamic_graph.sot.core.meta_tasks_kine import * from numpy import * # Create the robot romeo. from dynamic_graph.sot.romeo.robot import * robot = Robot('romeo_small', device=RobotSimu('romeo_small')) # Binds with ROS. assert that roscore is running. from dynamic_graph.ros import * ros = Ros(robot) # Create a simple kinematic solver. from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize ( robot ) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- # define the macro allowing to run the simulation. from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts dt=5e-3 @loopInThread def inc(): robot.device.increment(dt) runner=inc() [go,stop,next,n]=loopShortcuts(runner) # ---- TASKS -------------------------------------------------------------------
from dynamic_graph.sot.expression_graph.types import BaseElement from dynamic_graph.sot.expression_graph.types import * from dynamic_graph.sot.expression_graph.expression_graph import * from dynamic_graph.sot.expression_graph.functions import * from dynamic_graph.sot.robohow.superviser import * # Binds with ROS. assert that roscore is running. from dynamic_graph.ros import * ros = Ros(robot) # Create a simple kinematic solver. from dynamic_graph.sot.dyninv import SolverKine from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize ( robot, 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) # Creates the superviser, that will handle the stack of task update superviser = Superviser(robot, solver, ros.rosPublish) # Additional frames. robot.frames['l_gripper'] = robot.frames['leftGripper'] robot.frames['r_gripper'] = robot.frames['rightGripper'] robot.expressions={}
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize #deprecated solver=initialize(robot) #deprecated from dynamic_graph.sot.application.velocity.precomputed_tasks import Application appli =Application(robot) robot.initializeTracer() #launch service start_dynamic_graph #walk few steps from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPG, walkFewSteps, walkFewStepsCircular CreateEverythingForPG(robot,solver) walkFewSteps(robot) #-----------creation of a task--------------- from dynamic_graph.sot.core.meta_tasks import generic6dReference, Task, GainAdaptive from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph import plug from numpy import * #show tasks in solver controling the robot print solver #deprecated print appli.solver #show library of precomputed tasks robot.tasks # appli.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8) trunktask = Task ("trunk-task")
#----------------PINOCCHIO----------------------------# import pinocchio as se3 from pinocchio.robot_wrapper import RobotWrapper pinocchioRobot = RobotWrapper(_urdfPath, _urdfDir, se3.JointModelFreeFlyer()) pinocchioRobot.initDisplay(loadModel=True) #----------------ROBOT - DEVICE AND DYNAMICS----------# from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot robot = HumanoidRobot(_robotName, pinocchioRobot.model, pinocchioRobot.data, _initialConfig, _OperationalPointsMap) #-----------------------------------------------------# #----------------SOT (SOLVER)-------------------------# from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize(robot) #-----------------------------------------------------# #----------------PG--------------------------------# #from dynamic_graph.sot.pattern_generator.walking import initPg, initZMPRef, initWaistCoMTasks, initFeetTask, initPostureTask, pushTasks from dynamic_graph.sot.pattern_generator.walking import walkNaveau, CreateEverythingForPG, walkFewSteps, walkAndrei CreateEverythingForPG(robot, solver) #------------------------------------------------# #walkFewSteps ( robot ) walkNaveau(robot) #walkAndrei( robot ) robot.pg.velocitydes.value = (0.1, 0.0, 0.0) #------------------------------------------------------------------------------- #----- MAIN LOOP ---------------------------------------------------------------
def __init__(self, robot): ros = Ros(robot) solver = initialize ( robot, SolverKine ) self.solver = solver # --- CONTACTS # define contactLF and contactRF for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]: contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint) contact.feature.frame('desired') contact.gain.setConstant(10) contact.keep() locals()['contact'+name] = contact # ---- TASKS ------------------------------------------------------------------- # operational point used ground_z = VersorElement('ground_z', robot, 'ground', versor = (0,0,1)) r_gripper_x = VersorElement('r_gripper_x', robot, 'rightGripper', versor = (1,0,0) ) (taskAngleBottle, angleBottleZ) = createTaskAndFeature('bottleZ', r_gripper_x, ground_z, 'angle') angleBottleZ.reference.value = radians(90) l_gripper_x = VersorElement('r_gripper_x', robot, 'leftGripper', versor = (1,0,0) ) (taskAngleCup, angleBottleCup) = createTaskAndFeature('cupZ', l_gripper_x, ground_z, 'angle') angleBottleCup.reference.value = radians(90) # # # # # # # ground_plane = PlaneElement('ground_plane', robot, 'ground', normal = (0,0,1)) r_gripper_y = VersorElement('r_gripper_y', robot, 'rightGripper', versor = (0,1,0) ) (taskPlanBottleY, planBottleY) = createTaskAndFeature('bottleY', ground_plane, r_gripper_y, 'angle') planBottleY.reference.value = 0 ################################ #######################"" ## position right hand above target # heightZ = PointElement('heightZ', robot, 'ground', position = (0,0,1)) l_gripperZpos = PointElement('l_gripperZpos', robot, 'leftGripper') r_gripperZpos = PointElement('r_gripperZpos', robot, 'rightGripper') (taskGripperZ, positionZ) = createFeaturePointToPoint('positionZ', r_gripperZpos, l_gripperZpos) positionZ.selec.value ='100' positionZ.reference.value = (-0.1,) #######################################################"" ## position leftHand op above right hand ## -pi/8 << dot(bottle_normal, World_Z_axis) << pi/8 #posXY = PointElement('posXY', robot, 'ground') l_gripperXY = PointElement('l_gripperXY', robot, 'leftGripper') r_gripperXY = PointElement('r_gripperXY', robot, 'rightGripper') (taskGripperXY, positionXY) = createFeaturePointToPoint('positionXY', l_gripperXY, r_gripperXY) positionXY.selec.value = '011' positionXY.reference.value = (0,0) # define a task for the orientation of the fingertips : parallel to the handle # line / line constraint #tips = FeatureVersorToVersor('tips') ground_x = VersorElement('ground_x', robot, 'ground', versor = (1,0,0)) (taskTips, tips) = createTaskAndFeature('tips', ground_x, r_gripper_y, 'angle') tips.reference.value = 2.5 l_gripper_angle = Gripper('r_gripper_angle', robot, 38, 2) r_gripper_angle = Gripper('r_gripper_angle', robot, 29, 2) ## TODO ... self.angleBottleZ = angleBottleZ self.taskAngleBottle = taskAngleBottle self.taskAngleCup = taskAngleCup self.l_gripper_angle = l_gripper_angle self.r_gripper_angle = r_gripper_angle self.taskGripperXY =taskGripperXY self.taskGripperZ =taskGripperZ self.taskPlanBottleY =taskPlanBottleY self.taskTips =taskTips
from dynamic_graph.sot.core.meta_tasks_kine import * ## Create the robot romeo. from dynamic_graph.sot.romeo.robot import * robot = Robot('romeo', device=RobotSimu('romeo')) # Binds with ROS. assert that roscore is running. from dynamic_graph.ros import * ros = Ros(robot) # Create a simple kinematic solver. from dynamic_graph.sot.dyninv import SolverKine from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize ( robot, SolverKine ) from dynamic_graph.sot.core.meta_tasks_kine import * from dynamic_graph.sot.core.meta_task_generic import MetaTaskGeneric #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- # define the macro allowing to run the simulation. from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts dt=5e-3 @loopInThread def inc(): robot.device.increment(dt) runner=inc()