class Solver: def __init__(self, robot): self.robot = robot # Make sure control does not exceed joint limits. self.jointLimitator = JointLimitator("joint_limitator") plug(self.robot.dynamic.position, self.jointLimitator.joint) plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl) plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl) # Create the solver. self.sot = SolverKine("solver") self.sot.signal("damping").value = 1e-6 self.sot.setSize(self.robot.dimension) # Plug the solver control into the filter. plug(self.sot.control, self.jointLimitator.controlIN) # Important: always use 'jointLimitator.control' # and NOT 'sot.control'! if robot.device: plug(self.jointLimitator.control, robot.device.control) def push(self, task): """ Proxy method to push a task (not a MetaTask) in the sot """ self.sot.push(task.name) if task.name != "taskposture" and "taskposture" in self.toList(): self.sot.down("taskposture") def rm(self, task): """ Proxy method to remove a task from the sot """ self.sot.rm(task.name) def pop(self): """ Proxy method to remove the last (usually posture) task from the sot """ self.sot.pop() def __str__(self): return self.sot.display() def toList(self): """ Creates the list of the tasks in the sot """ return map(lambda x: x[1:-1], self.sot.dispStack().split("|")[1:]) def clear(self): """ Proxy method to remove all tasks from the sot """ self.sot.clear()
class Solver: def __init__(self, robot): self.robot = robot # Make sure control does not exceed joint limits. self.jointLimitator = JointLimitator('joint_limitator') plug(self.robot.dynamic.position, self.jointLimitator.joint) plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl) plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl) # Create the solver. self.sot = SolverKine('solver') self.sot.signal('damping').value = 1e-6 self.sot.setSize(self.robot.dimension) # Plug the solver control into the filter. plug(self.sot.control, self.jointLimitator.controlIN) # Important: always use 'jointLimitator.control' # and NOT 'sot.control'! if robot.device: plug(self.jointLimitator.control, robot.device.control) def push(self, task): """ Proxy method to push a task (not a MetaTask) in the sot """ self.sot.push(task.name) if task.name != "taskposture" and "taskposture" in self.toList(): self.sot.down("taskposture") def rm(self, task): """ Proxy method to remove a task from the sot """ self.sot.rm(task.name) def pop(self): """ Proxy method to remove the last (usually posture) task from the sot """ self.sot.pop() def __str__(self): return self.sot.display() def toList(self): """ Creates the list of the tasks in the sot """ return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:]) def clear(self): """ Proxy method to remove all tasks from the sot """ self.sot.clear()
class RobotCollisionModel: def __init__(self): # Marker array for link collision model self.linkcollisionmodel = MarkerArray() # Define marker properties for link self.rcm_pub = rospy.Publisher("collision_model", MarkerArray) self.r = rospy.Rate(1) self.nolinks = 2 self.dt = 0 self.buildurrobot() def buildurrobot(self): # collision components self.robot = Pr2(name="robot", device=RobotSimu("Pr2")) self.dimension = self.robot.dynamic.getDimension() plug(self.robot.device.state, self.robot.dynamic.position) self.ros = Ros(self.robot) self.a = sc.SotCollision("sc") # create links for collision check # self.a.createcollisionlink("wall1","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0)) self.a.createcollisionlink("base_link", "box", "internal", (0.65, 0.65, 0.25, 0.0, 0.0, 0.175, 0, 0, 0)) self.a.createcollisionlink( "shoulder_right_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0) ) self.a.createcollisionlink( "shoulder_left_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0) ) self.a.createcollisionlink("upper_left_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0)) self.a.createcollisionlink("upper_right_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0)) self.a.createcollisionlink( "upper_left_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0) ) self.a.createcollisionlink( "upper_right_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0) ) self.a.createcollisionlink("upper_right_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0)) self.a.createcollisionlink("upper_left_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0)) # self.a.createcollisionlink("platform1","box","external",(0.5,0.8,0.11,0.0,0.0,0.0,0,0.0,0)) # plug joint position and joint jacobian to collision checker entity # self.a.wall1.value = ((1,0,0,0),(0,1,0,1.22),(0,0,1,0.12),(0,0,0,1)) # self.a.platform1.value = ((1,0,0,12),(0,1,0,0.82),(0,0,1,0.025),(0,0,0,1)) # self.a.platform2.value = ((1,0,0,0.77),(0,1,0,-0.82),(0,0,1,0.025),(0,0,0,1)) # print self.robot.dynamic plug(self.robot.dynamic.base_joint, self.a.base_link) plug(self.robot.dynamic.r_shoulder_pan_joint, self.a.shoulder_right_link) plug(self.robot.dynamic.l_shoulder_pan_joint, self.a.shoulder_left_link) plug(self.robot.dynamic.l_upper_arm_roll_joint, self.a.upper_left_arm) plug(self.robot.dynamic.r_upper_arm_roll_joint, self.a.upper_right_arm) plug(self.robot.dynamic.l_forearm_roll_joint, self.a.upper_left_forearm) plug(self.robot.dynamic.r_forearm_roll_joint, self.a.upper_right_forearm) plug(self.robot.dynamic.r_wrist_roll_joint, self.a.upper_right_wrist) plug(self.robot.dynamic.l_wrist_roll_joint, self.a.upper_left_wrist) # plug(self.robot.dynamic.base_joint,self.a.base_link_2) # plug(self.robot.dynamic.base_joint,self.a.base_link_3) # plug(self.robot.dynamic.base_joint,self.a.base_link_4) # l_shoulder_pan_joint # l_shoulder_lift_joint # r_shoulder_pan_joint # r_shoulder_lift_joint # self.a.Jwall1.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)) # self.a.Jwall2.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)) # self.a.Jwall3.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)) # self.a.Jplatform1.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)) # self.a.Jplatform2.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)) plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link) plug(self.robot.dynamic.Jr_shoulder_pan_joint, self.a.Jshoulder_right_link) plug(self.robot.dynamic.Jl_shoulder_pan_joint, self.a.Jshoulder_left_link) plug(self.robot.dynamic.Jl_upper_arm_roll_joint, self.a.Jupper_left_arm) plug(self.robot.dynamic.Jr_upper_arm_roll_joint, self.a.Jupper_right_arm) plug(self.robot.dynamic.Jl_forearm_roll_joint, self.a.Jupper_left_forearm) plug(self.robot.dynamic.Jr_forearm_roll_joint, self.a.Jupper_right_forearm) plug(self.robot.dynamic.Jr_wrist_roll_joint, self.a.Jupper_right_wrist) plug(self.robot.dynamic.Jl_wrist_roll_joint, self.a.Jupper_left_wrist) # plug(self.robot.dynamic.Jbase_joint,self.a.Jbase_link_4) self.a.createcollisionpair("base_link", "upper_left_arm") self.a.createcollisionpair("base_link", "upper_right_arm") self.a.createcollisionpair("base_link", "upper_left_forearm") self.a.createcollisionpair("base_link", "upper_right_forearm") self.a.createcollisionpair("shoulder_left_link", "upper_left_forearm") self.a.createcollisionpair("shoulder_right_link", "upper_right_forearm") self.a.createcollisionpair("upper_left_forearm", "upper_right_forearm") self.a.createcollisionpair("upper_left_arm", "upper_right_arm") self.a.createcollisionpair("upper_left_arm", "upper_right_forearm") self.a.createcollisionpair("upper_left_forearm", "upper_right_arm") self.a.createcollisionpair("upper_left_wrist", "upper_right_wrist") self.a.createcollisionpair("upper_left_forearm", "upper_right_wrist") self.a.createcollisionpair("upper_left_arm", "upper_right_wrist") self.a.createcollisionpair("upper_right_wrist", "upper_left_wrist") self.a.createcollisionpair("upper_right_forearm", "upper_left_wrist") self.a.createcollisionpair("upper_right_arm", "upper_left_wrist") # joint limits self.robot.dynamic.upperJl.recompute(0) self.robot.dynamic.lowerJl.recompute(0) self.taskjl = TaskJointLimits("taskJL") plug(self.robot.dynamic.position, self.taskjl.position) self.taskjl.controlGain.value = 5 self.taskjl.referenceInf.value = self.robot.dynamic.lowerJl.value # print self.robot.dynamic.lowerJl.value self.taskjl.referenceSup.value = self.robot.dynamic.upperJl.value # print self.robot.dynamic.upperJl.value self.taskjl.dt.value = 1 # torsotask self.task_torso_metakine = MetaTaskKine6d( "task_torso_metakine", self.robot.dynamic, "torso_lift_joint", "torso_lift_joint" ) goal_torso = ((1.0, 0, 0, -0.05), (0, 1.0, 0, -0.0), (0, 0, 1.0, 0.790675), (0, 0, 0, 1.0)) self.task_torso_metakine.feature.frame("desired") # task_torso_metakine.feature.selec.value = '000100'#RzRyRxTzTyTx self.task_torso_metakine.gain.setConstant(10) self.task_torso_metakine.featureDes.position.value = goal_torso goal = ((1.0, 0, 0, 0.0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0)) self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint") # task_waist_metakine.opmodif = goal # task_waist_metakine.feature.frame('desired') self.task_waist_metakine.featureDes.position.value = goal self.task_waist_metakine.gain.setConstant(1) self.task_l_wrist_metakine = MetaTaskKine6d( "task_l_wrist_metakine", self.robot.dynamic, "l_wrist_roll_joint", "l_wrist_roll_joint" ) # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),) self.goal_l_wrist = ((1.0, 0, 0, 0.649), (0, 1.0, 0, -0.042), (0, 0, 1.0, 0.845), (0, 0, 0, 1.0)) # self.goal_l_wrist = ((1.,0,0,0.486),(0,1.,0,-0.251),(0,0,1.,0.826),(0,0,0,1.),) self.task_l_wrist_metakine.feature.frame("desired") self.task_l_wrist_metakine.feature.selec.value = "000111" # RzRyRxTzTyTx self.task_l_wrist_metakine.gain.setConstant(6) self.task_l_wrist_metakine.featureDes.position.value = self.goal_l_wrist self.task_r_wrist_metakine = MetaTaskKine6d( "task_r_wrist_metakine", self.robot.dynamic, "r_wrist_roll_joint", "r_wrist_roll_joint" ) # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),) # self.goal_r_wrist = ((1.,0,0,0.455),(0,1.,0,-0.835),(0,0,1.,0.780),(0,0,0,1.),) self.goal_r_wrist = ((1.0, 0, 0, 0.590), (0, 1.0, 0, -0.188), (0, 0, 1.0, 1.107), (0, 0, 0, 1.0)) self.task_r_wrist_metakine.feature.frame("desired") self.task_r_wrist_metakine.feature.selec.value = "000100" # RzRyRxTzTyTx self.task_r_wrist_metakine.gain.setConstant(6) self.task_r_wrist_metakine.featureDes.position.value = self.goal_r_wrist self.task_collision_avoidance = TaskInequality("taskcollision") self.collision_feature = FeatureGeneric("collisionfeature") plug(self.a.collisionJacobian, self.collision_feature.jacobianIN) plug(self.a.collisionDistance, self.collision_feature.errorIN) self.task_collision_avoidance.add(self.collision_feature.name) self.task_collision_avoidance.referenceInf.value = ( 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, ) self.task_collision_avoidance.referenceSup.value = ( 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, ) self.task_collision_avoidance.dt.value = 1 self.task_collision_avoidance.controlGain.value = 50.0 self.solver = SolverKine("sot") self.solver.setSize(self.dimension) self.solver.damping.value = 3e-2 self.solver.push(self.taskjl.name) self.solver.push(self.task_collision_avoidance.name) self.solver.push(self.task_waist_metakine.task.name) self.solver.push(self.task_torso_metakine.task.name) self.solver.push(self.task_r_wrist_metakine.task.name) self.solver.push(self.task_l_wrist_metakine.task.name) # self.solver.push (self.task_collision_avoidance.name) # self.solver.push (self.task_wrist_metakine.task.name) # self.solver.damping.value =3e-2 plug(self.solver.control, self.robot.device.control) def publish(self): # self.robot.dynamic.Jwrist_2_joint.recompute(self.dt) # self.a.collisionDistance.recompute(self.dt) # print self.a.collisionDistance.value # self.a.collisionJacobian.recompute(self.dt) # print self.a.collisionDistance.value self.dt = self.dt + 1 # self.r.sleep() # print self.robot.dynamic.base_joint.value tstart = dt.datetime.now() self.robot.device.increment(0.01) self.getstate() self.rcm_pub.publish(self.linkcollisionmodel) tend = dt.datetime.now() print(tend - tstart).microseconds self.robot.dynamic.l_upper_arm_roll_joint.recompute(self.dt) # print self.robot.dynamic.l_upper_arm_roll_joint.value def getstate(self): state = rcm.a.getfclstate() # print state # a = self.robot.dynamic.wrist_2_joint.value for i in range(9): self.link = Marker() self.link.type = Marker().CUBE # self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae" self.link.header.frame_id = "/odom" self.link.header.stamp = rospy.Time.now() # self.link.ns = "basicshapes" self.link.id = i self.link.action = self.link.ADD # print state self.link.pose.position.x = state[i][0] self.link.pose.position.y = state[i][1] self.link.pose.position.z = state[i][2] self.link.pose.orientation.x = state[i][3] self.link.pose.orientation.y = state[i][4] self.link.pose.orientation.z = state[i][5] self.link.pose.orientation.w = state[i][6] self.link.scale.x = state[i][7] self.link.scale.y = state[i][8] self.link.scale.z = state[i][9] self.link.color.r = 0.0 self.link.color.g = 1.0 self.link.color.b = 0.0 self.link.color.a = 1.0 # pself.link.pose.orientation # self.link.lifetime = rospy.Duration(); self.linkcollisionmodel.markers.append(self.link)
# --- TASK RIGHT FOOT # Task right hand taskRF = MetaTask6d('rf', dyn, 'rf', 'right-ankle') taskLF = MetaTask6d('lf', dyn, 'lf', 'left-ankle') plug(pg.rightfootref, taskRF.featureDes.position) taskRF.task.controlGain.value = 5 plug(pg.leftfootref, taskLF.featureDes.position) taskLF.task.controlGain.value = 5 # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskComPD.name) plug(sot.control, robot.control) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.parseCmd(':SetAlgoForZmpTrajectory Herdt') pg.parseCmd(':doublesupporttime 0.1') pg.parseCmd(':singlesupporttime 0.7') # When velocity reference is at zero, the robot stops all motion after n steps pg.parseCmd(':numberstepsbeforestop 4') # Set constraints on XY pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
tr.add('featureComDes.errorIN', 'comref') tr.add('dyn.com', 'com') tr.add(taskWaist.gain.name + '.gain', 'gainwaist') history = History(dyn, 1) # --- RUN ----------------------------------------------------------------- featurePosture.selec.value = toFlags(range(6, 36)) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8) attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'), (lambda: pg.initState(), 'Init PG'), (lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'), (lambda: sot.push(taskCom.name), 'Push COM'), (lambda: plug(pg.rightfootref, taskRF.featureDes.position), 'Plug RF'), (lambda: plug(pg.leftfootref, taskLF.featureDes.position), 'plug LF')) #attime(700,lambda: sot.push(taskPosture.name),'Add Posture') #attime(900,lambda: sot.rm(taskCom.name),'rm com') #attime(1200,lambda: dump(),'dump!')
# --- TASK RIGHT FOOT # Task right hand taskRF=MetaTask6d('rf',dyn,'rf','right-ankle') taskLF=MetaTask6d('lf',dyn,'lf','left-ankle') plug(pg.rightfootref,taskRF.featureDes.position) taskRF.task.controlGain.value = 5 plug(pg.leftfootref,taskLF.featureDes.position) taskLF.task.controlGain.value = 5 # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskComPD.name) plug(sot.control,robot.control) # --- TRACER ----------------------------------------------------------------- from dynamic_graph.tracer import * from dynamic_graph.tracer_real_time import * tr = Tracer('tr') tr.open('/tmp/','','.dat') tr.start() robot.after.addSignal('tr.triger') #tr.add(dyn.name+'.ffposition','ff')
taskPosition = Task('taskPosition') taskPosition.add('featurePosition') # featurePosition.selec.value = toFlags((6,24)) gainPosition = GainAdaptive('gainPosition') gainPosition.set(0.1,0.1,125e3) gainPosition.gain.value = 5 plug(taskPosition.error,gainPosition.error) plug(gainPosition.gain,taskPosition.controlGain) featurePosition.selec.value = '000000000000001111111111111100000000000' # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskCom.task.name) sot.push(taskWaistOr.task.name) # Stun the upper part of the body. sot.push(taskHead.task.name) # constraint the head orientation: look straight ahead sot.push('taskPosition') # stun the arms. # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.startHerdt(False) # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
class RobotCollisionModel: def __init__(self): # Marker array for link collision model self.linkcollisionmodel = MarkerArray() # Define marker properties for link self.rcm_pub = rospy.Publisher("collision_model", MarkerArray) self.r = rospy.Rate(100) self.nolinks = 6 self.dt = 0 self.buildurrobot() def buildurrobot(self): self.robot = Ur("ur5", device=RobotSimu("ur5")) robot_dimension = self.robot.dynamic.getDimension() self.robot.device.resize(robot_dimension) # Ros binding # roscore must be running ros = Ros(self.robot) # collision components self.a = sc.SotCollision("sc") # self.a.createlinkmodel(((0,0.08,0.01,0,0,0.0,0.0,0,0),(1,0.08,0.04,0,0,0,0,0,0),(2,0.09,0.15,0.2,0.0,0.0,0,1.57,0),(3,0.07,0.14,0.2,0.0,0.0,-3.1416,1.5706,-3.1416),(4,0.05,0.03,0.0,0.093,0,-3.14,0,-3.14),(5,0.057,0.02,0.0,0,-0.095,-3.14,0,-3.14),(6,0.04,0.01,0.0,0.065,0,1.57,0,0))) # create links for collision check self.a.createcollisionlink("base_link", "capsule", "internal", (0.08, 0.01, 0, 0, 0, 0.0, 0.0, 0, 0)) self.a.createcollisionlink("shoulder_pan_link", "capsule", "internal", (0.08, 0.04, 0, 0, 0, 0, 0, 0, 0)) self.a.createcollisionlink( "shoulder_lift_link", "capsule", "internal", (0.09, 0.15, 0, 0.2, 0.0, 0.0, 0, 1.57, 0) ) self.a.createcollisionlink( "elbow_link", "capsule", "internal", (0.07, 0.14, 0, 0.2, 0.0, 0.0, -3.1416, 1.5706, -3.1416) ) self.a.createcollisionlink( "wrist_1_link", "capsule", "internal", (0.05, 0.03, 0, 0.0, 0.093, 0, -3.14, 0, -3.14) ) self.a.createcollisionlink( "wrist_2_link", "capsule", "internal", (0.057, 0.02, 0, 0.0, 0, -0.095, -3.14, 0, -3.14) ) self.a.createcollisionlink("wrist_3_link", "capsule", "internal", (0.04, 0.01, 0, 0.0, 0.065, 0, 1.57, 0, 0)) # plug joint position and joint jacobian to collision checker entity plug(self.robot.dynamic.base_joint, self.a.base_link) plug(self.robot.dynamic.shoulder_pan_joint, self.a.shoulder_pan_link) plug(self.robot.dynamic.shoulder_lift_joint, self.a.shoulder_lift_link) plug(self.robot.dynamic.elbow_joint, self.a.elbow_link) plug(self.robot.dynamic.wrist_1_joint, self.a.wrist_1_link) plug(self.robot.dynamic.wrist_2_joint, self.a.wrist_2_link) plug(self.robot.dynamic.wrist_3_joint, self.a.wrist_3_link) plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link) plug(self.robot.dynamic.Jshoulder_pan_joint, self.a.Jshoulder_pan_link) plug(self.robot.dynamic.Jshoulder_lift_joint, self.a.Jshoulder_lift_link) plug(self.robot.dynamic.Jelbow_joint, self.a.Jelbow_link) plug(self.robot.dynamic.Jwrist_1_joint, self.a.Jwrist_1_link) plug(self.robot.dynamic.Jwrist_2_joint, self.a.Jwrist_2_link) plug(self.robot.dynamic.Jwrist_3_joint, self.a.Jwrist_3_link) # create collision pairs # base link self.a.createcollisionpair("base_link", "shoulder_lift_link") self.a.createcollisionpair("base_link", "elbow_link") self.a.createcollisionpair("base_link", "wrist_1_link") self.a.createcollisionpair("base_link", "wrist_2_link") self.a.createcollisionpair("base_link", "wrist_3_link") # shoulder pan link self.a.createcollisionpair("shoulder_pan_link", "wrist_1_link") self.a.createcollisionpair("shoulder_pan_link", "wrist_2_link") self.a.createcollisionpair("shoulder_pan_link", "wrist_3_link") self.a.createcollisionpair("shoulder_pan_link", "elbow_link") # shoulder lift link self.a.createcollisionpair("shoulder_lift_link", "wrist_1_link") self.a.createcollisionpair("shoulder_lift_link", "wrist_2_link") self.a.createcollisionpair("shoulder_lift_link", "wrist_3_link") # shoulder elbow link self.a.createcollisionpair("elbow_link", "wrist_2_link") self.a.createcollisionpair("elbow_link", "wrist_3_link") # shoulder wrist1 link self.a.createcollisionpair("wrist_1_link", "wrist_3_link") ######### task description for old type of solver############ # Create task for the waist robot_pose = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0)) feature_base = FeaturePosition( "position_base", self.robot.dynamic.base_joint, self.robot.dynamic.Jbase_joint, robot_pose ) task_base = Task("waist_task") task_base.controlGain.value = 100.0 task_base.add(feature_base.name) # Create task for the wrist3 I4 = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0)) feature_wrist = FeaturePosition( "position_wrist", self.robot.dynamic.wrist_3_joint, self.robot.dynamic.Jwrist_3_joint, I4 ) task_wrist = Task("wrist_task") task_wrist.controlGain.value = 1 task_wrist.add(feature_wrist.name) ######### task description for new type of solver############ # waist position task goal = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0)) self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint") # task_waist_metakine.opmodif = goal # task_waist_metakine.feature.frame('desired') self.task_waist_metakine.featureDes.position.value = goal self.task_waist_metakine.gain.setConstant(100) # wrist_3 task goal = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0)) self.task_wrist_metakine = MetaTaskKine6d( "task_wrist_metakine", self.robot.dynamic, "wrist_3_joint", "wrist_3_joint" ) # task_wrist_metakine.opmodif = goal # task_wrist_metakine.feature.frame('desired') self.task_wrist_metakine.featureDes.position.value = goal self.task_wrist_metakine.gain.setConstant(1) ######### task description for collision task ############ self.task_collision_avoidance = TaskInequality("taskcollision") self.collision_feature = FeatureGeneric("collisionfeature") plug(self.a.collisionJacobian, self.collision_feature.jacobianIN) plug(self.a.collisionDistance, self.collision_feature.errorIN) # self.a.collisionDistance self.task_collision_avoidance.add(self.collision_feature.name) self.task_collision_avoidance.referenceInf.value = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0) # min self.task_collision_avoidance.referenceSup.value = ( 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, 2e10, ) # max self.task_collision_avoidance.dt.value = self.dt self.task_collision_avoidance.controlGain.value = 0.01 # task.featureDes.xy.value = (0,0) """ #task_wrist_metakine.opmodif = goal #task_wrist_metakine.feature.frame('desired') task_wrist_metakine.featureDes.position.value = goal task_wrist_metakine.gain.setConstant(1) """ ######### solver########################################################### ##### solver old############ """ self.solver = SOT ('solver') self.solver.setSize (robot_dimension) self.solver.push (task_base.name) self.solver.push (task_wrist.name) plug (self.solver.control, self.robot.device.control) """ self.solver = SolverKine("sot") self.solver.setSize(robot_dimension) self.solver.push(self.task_waist_metakine.task.name) self.solver.push(self.task_collision_avoidance.name) self.solver.push(self.task_wrist_metakine.task.name) self.solver.damping.value = 3e-2 plug(self.solver.control, self.robot.device.control) def publish(self): self.a.collisionJacobian.recompute(self.dt) self.a.collisionDistance.recompute(self.dt) print "colljac" print self.a.collisionJacobian.value print "colldist" print self.a.collisionDistance.value self.robot.device.increment(0.01) """ rcm.robot.device.set((8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,,8E-58,8E-58)) rcm.robot.device.increment(0.01) rcm.robot.dynamic.shoulder_pan_joint.recompute(dt) rcm.robot.dynamic.shoulder_lift_joint.recompute(dt) rcm.robot.dynamic.elbow_joint.recompute(dt) rcm.robot.dynamic.wrist_1_joint.recompute(dt) rcm.robot.dynamic.wrist_2_joint.recompute(dt) rcm.robot.dynamic.wrist_3_joint.recompute(dt) rcm.robot.dynamic.Jshoulder_pan_joint.recompute(dt) rcm.robot.dynamic.Jshoulder_lift_joint.recompute(dt) rcm.robot.dynamic.Jelbow_joint.recompute(dt) rcm.robot.dynamic.Jwrist_1_joint.recompute(dt) rcm.robot.dynamic.Jwrist_2_joint.recompute(dt) rcm.a.collisionJacobian.recompute(dt) print rcm.a.collisionJpair1.value dt = dt +1 """ self.robot.dynamic.shoulder_pan_joint.recompute(self.dt) self.robot.dynamic.shoulder_lift_joint.recompute(self.dt) self.robot.dynamic.elbow_joint.recompute(self.dt) self.robot.dynamic.wrist_1_joint.recompute(self.dt) self.robot.dynamic.wrist_2_joint.recompute(self.dt) self.robot.dynamic.wrist_3_joint.recompute(self.dt) self.robot.dynamic.Jshoulder_pan_joint.recompute(self.dt) self.robot.dynamic.Jshoulder_lift_joint.recompute(self.dt) self.robot.dynamic.Jelbow_joint.recompute(self.dt) self.robot.dynamic.Jwrist_1_joint.recompute(self.dt) self.robot.dynamic.Jwrist_2_joint.recompute(self.dt) # recompute pair # self.a.collisionJacobian.recompute(self.dt) # self.a.imdpair1.recompute(self.dt) # print self.a.imdpair1.value # print 'im here' self.dt = self.dt + 1 # self.a.updatefclmodel(1) self.getstate() # print 'wrist_3' # print self.robot.dynamic.wrist_3_joint.value self.rcm_pub.publish(self.linkcollisionmodel) self.r.sleep() # self.linkcollisionmodel = MarkerArray() # 0.07 0.07 0.05 # def getstate(self): state = rcm.a.getfclstate() # print state # a = self.robot.dynamic.wrist_2_joint.value for i in range(7): self.link = Marker() self.link.type = self.link.MESH_RESOURCE # Cylinder self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae" self.link.header.frame_id = "/odom" self.link.header.stamp = rospy.Time.now() # self.link.ns = "basicshapes" self.link.id = i self.link.action = self.link.ADD # print state self.link.pose.position.x = state[i][0] self.link.pose.position.y = state[i][1] self.link.pose.position.z = state[i][2] self.link.pose.orientation.x = state[i][3] self.link.pose.orientation.y = state[i][4] self.link.pose.orientation.z = state[i][5] self.link.pose.orientation.w = state[i][6] self.link.scale.x = state[i][7] self.link.scale.y = state[i][7] self.link.scale.z = state[i][8] self.link.color.r = 0.0 self.link.color.g = 1.0 self.link.color.b = 0.0 self.link.color.a = 1.0 # pself.link.pose.orientation # self.link.lifetime = rospy.Duration(); self.linkcollisionmodel.markers.append(self.link)
#taskCom0.task.controlGain.value = 10 #sot.push(taskCom0.task.name) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.startHerdt() # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw) pg.pg.velocitydes.value = (0.1, 0.0, 0.0) #ball0 = (2.5,-0.3,0.5) ball0 = (0.5, -2.3, 0.5) w = (1 / 1.3, 1 / 1.8, 1 / 3.9) h = (0.1, 0.07, 0.3) sot.damping.value = 5e-2 sot.push(taskJL.name) sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskCom.task.name) taskPosture.gotoq((5, 1, 0.05, 0.9), rhand=[ 1, ]) sot.push(taskPosture.task.name) gotoNd(taskGaze, ball0, '011', (10, 1, 0.01, 0.9)) sot.push(taskGaze.task.name) gotoNd(taskRH, ball0, '011', (10, 1, 0.01, 0.9)) sot.push(taskRH.task.name) # --- SCRIPT EVENTS ---------------------------------------------------------- #from random import random
#taskCom0.task.controlGain.value = 10 #sot.push(taskCom0.task.name) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.startHerdt() # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw) pg.pg.velocitydes.value =(0.1,0.0,0.0) #ball0 = (2.5,-0.3,0.5) ball0 = (0.5,-2.3,0.5) w=(1/1.3,1/1.8,1/3.9) h=(0.1,0.07,0.3) sot.damping.value = 5e-2 sot.push(taskJL.name) sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskCom.task.name) taskPosture.gotoq((5,1,0.05,0.9),rhand=[1,]) sot.push(taskPosture.task.name) gotoNd(taskGaze,ball0,'011',(10,1,0.01,0.9)) sot.push(taskGaze.task.name) gotoNd(taskRH,ball0,'011',(10,1,0.01,0.9)) sot.push(taskRH.task.name) # --- SCRIPT EVENTS ---------------------------------------------------------- #from random import random import numpy.random as nprand
robot.after.addSignal(taskJL.name+".normalizedPosition") robot.after.addSignal(taskSupportSmall.name+".normalizedPosition") robot.after.addSignal(taskFoV.task.name+".normalizedPosition") robot.after.addSignal(taskFoV.feature.name+'.error') robot.after.addSignal(taskSupport.name+".normalizedPosition") # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- taskCom.featureDes.errorIN.value = dyn.com.value taskCom.task.controlGain.value = 10 taskPosture.gotoq(1,array(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[]) sot.push(taskJL.name) sot.addContact(contactRF) sot.addContact(contactLF) sot.push(taskSupport.name) ball = BallPosition((0,-1.1,0.9)) tw=TaskWeight('tw') ''' taskFoV.task.controlGain.value=0.05 taskSupportSmall.controlGain.value=0.02 tw.add(taskSupportSmall.name,1) tw.add(taskFoV.task.name,10) sot.damping.value = 0.01 '''
tr.add('featureComDes.errorIN','comref') tr.add('dyn.com','com') tr.add(taskWaist.gain.name+'.gain','gainwaist') history = History(dyn,1) # --- RUN ----------------------------------------------------------------- featurePosture.selec.value = toFlags( range(6,36) ) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18,0.1,0.005,0.8) attime(200 ,(lambda : sot.rm(taskWaist.task.name),'Rm waist') ,(lambda : pg.initState(),'Init PG') ,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list') ,(lambda : sot.push(taskCom.name),'Push COM') ,(lambda : plug(pg.rightfootref,taskRF.featureDes.position),'Plug RF') ,(lambda : plug(pg.leftfootref,taskLF.featureDes.position),'plug LF' ) )
taskComPD.setBeta(-1) # --- CONSTRAINT SUPPORT FOOT featureSupport = FeatureGeneric('featureSupport') featureSupport.errorIN.value = 6*(0,) plug(footSelection.Jfoot,featureSupport.jacobianIN) constraintSupport = Task('constraintSupport') constraintSupport.add(featureSupport.name) constraintSupport.task.value = 6*(0,) # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) sot.push(constraintSupport.name) sot.push(taskTwoFeet.name) sot.push(taskWaist.task.name) sot.push(taskComPD.name) plug(sot.control,robot.control) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.parseCmd(':SetAlgoForZmpTrajectory Herdt') pg.parseCmd(':doublesupporttime 0.1') pg.parseCmd(':singlesupporttime 0.7') # When velocity reference is at zero, the robot stops all motion after n steps pg.parseCmd(':numberstepsbeforestop 4') # Set constraints on XY pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
ip = (0.200, -0.006, 0.377,-0.155, 0.414, -0.555) zp = (0.198, 0.001, 0.581 ,-0.000, 0.001, 0.010) prl = (0.606, -0.001, 0.425,-3.115, 0.228, -3.029) aov = (-0.136, -0.005, 0.389,0.005, -0.575, 0.010) pp = (0.567, -0.013, 0.260,0,0,0) pr = (0.008,0.04,0.515,0,0,0) goal = matrixToTuple(generic6dReference(zp)) print goal #solver.damping.value =3e-2 task_wrist_metakine.gain.setConstant(1) task_wrist_metakine.feature.selec.value = '000111' task_wrist_metakine.featureDes.position.value = goal #solver #solver.push (task_waist.name) solver.push (taskjl.name) solver.push (task_waist_metakine.task.name) solver.push (task_wrist_metakine.task.name) plug (solver.control,robot.device.control) dt = 0.01 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)
taskComPD.setBeta(-1) # --- CONSTRAINT SUPPORT FOOT featureSupport = FeatureGeneric('featureSupport') featureSupport.errorIN.value = 6 * (0, ) plug(footSelection.Jfoot, featureSupport.jacobianIN) constraintSupport = Task('constraintSupport') constraintSupport.add(featureSupport.name) constraintSupport.task.value = 6 * (0, ) # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) sot.push(constraintSupport.name) sot.push(taskTwoFeet.name) sot.push(taskWaist.task.name) sot.push(taskComPD.name) plug(sot.control, robot.control) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.parseCmd(':SetAlgoForZmpTrajectory Herdt') pg.parseCmd(':doublesupporttime 0.1') pg.parseCmd(':singlesupporttime 0.7') # When velocity reference is at zero, the robot stops all motion after n steps pg.parseCmd(':numberstepsbeforestop 4') # Set constraints on XY pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
class Solver: def __init__(self, robot): self.robot = robot """ # Make sure control does not exceed joint limits. self.jointLimitator = JointLimitator('joint_limitator') plug(self.robot.dynamic.position, self.jointLimitator.joint) plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl) plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl) """ # Create the solver. self.sot = SolverKine('solver') self.sot.signal('damping').value = 1e-6 self.sot.setSize(self.robot.dimension) # Set second order computation self.robot.device.setSecondOrderIntegration() self.sot.setSecondOrderKinematics() plug(self.robot.device.velocity,self.robot.dynamic.velocity) plug(self.robot.dynamic.velocity,self.sot.velocity) # Plug the solver control into the robot. plug(self.sot.control, robot.device.control) def push (self, task): """ Proxy method to push a task (not a MetaTask) in the sot """ self.sot.push (task.name) if task.name!="taskposture" and "taskposture" in self.toList(): self.sot.down("taskposture") def rm (self, task): """ Proxy method to remove a task from the sot """ self.sot.rm (task.name) def pop (self): """ Proxy method to remove the last (usually posture) task from the sot """ self.sot.pop () def __str__ (self): return self.sot.display () def toList(self): """ Creates the list of the tasks in the sot """ return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:]) def clear(self): """ Proxy method to remove all tasks from the sot """ self.sot.clear ()
taskPosition = Task('taskPosition') taskPosition.add('featurePosition') # featurePosition.selec.value = toFlags((6,24)) gainPosition = GainAdaptive('gainPosition') gainPosition.set(0.1, 0.1, 125e3) gainPosition.gain.value = 5 plug(taskPosition.error, gainPosition.error) plug(gainPosition.gain, taskPosition.controlGain) featurePosition.selec.value = '000000000000001111111111111100000000000' # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskCom.task.name) sot.push(taskWaistOr.task.name) # Stun the upper part of the body. sot.push( taskHead.task.name) # constraint the head orientation: look straight ahead sot.push('taskPosition') # stun the arms. # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.startHerdt(False) # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw) pg.pg.velocitydes.value = (0.1, 0.0, 0.0)