plug(robot.device.state,robot.features['feature_jl'].state) robotDim = len(robot.dynamic.velocity.value) robot.features['feature_jl'].posture.value = robot.halfSitting # Right Leg, Left leg, chest+neck, right arm, left arm postureTaskDofs = \ [False]*3 + [False]*3 + [False]*3 + [False]*3 \ + [True] *1 + [False]*3 \ + [False]*1 + [True] *1 + [False]*5 + [False]*2 \ + [False]*1 + [True] *1 + [False]*5 + [False]*2 for dof,isEnabled in enumerate(postureTaskDofs): robot.features['feature_jl'].selectDof(dof+6,isEnabled) taskJointLimit=TaskInequality('taskJL') taskJointLimit.add('feature_jl') # Limits on these joints: Shoulder Roll, Thumb, Fingers (Left and Right) #taskJointLimit.referenceInf.value = (r_hip_yaw, r_hip_roll, r_hip_pitch, l_hip_yaw, l_hip_roll, l_hip_pitch, -1.55 ,1.57, 1.57,-0.005, -1.57,-1.57) #taskJointLimit.referenceSup.value = (r_hip_yaw, r_hip_roll, r_hip_pitch, l_hip_yaw, l_hip_roll, l_hip_pitch, 0.005,1.57, 1.57, 1.55 , -1.57,-1.57) taskJointLimit.referenceInf.value = (0.00,-1.550,-0.005) taskJointLimit.referenceSup.value = (0.10, 0.005, 1.550) taskJointLimit.dt.value= 0.005 taskJointLimit.controlGain.value = 0.009 ######################################################### bottle = createBottle() estimateBottleFrameInHand(robot) estimateCupFrameInHand(robot)
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)
from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph import plug, writeGraph from dynamic_graph.sot.core.meta_task_6d import toFlags from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d from dynamic_graph.sot.dyninv import SolverKine import dynamic_graph.sotcollision as sc from numpy import * # JOINT LIMIT INEQUALITY joint_limit_task=TaskInequality('joint_inequality') joint_feature = FeatureGeneric('joint_state_feature') plug(robot.dynamic.position,joint_feature.errorIN) joint_jacobian = matrixToTuple(eye(39)) joint_feature.jacobianIN.value = joint_jacobian joint_limit_task.add(joint_feature.name) joint_limit_task.referenceInf.value = (0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.007, -0.471238, -0.714601836603, -0.5236, -0.8, -2.3213, -1.7976931348623157e+308, -2.18, -1.7976931348623157e+308, 0.0, 0.0, -0.1, -1.7976931348623157e+308, 0.0, 0.0, 0.0, -0.7854, -2.2853981634, -0.5236, -3.9, -2.3213, -1.7976931348623157e+308, -2.18, -1.7976931348623157e+308, 0.0, 0.0, -0.1, -1.7976931348623157e+308, 0.0, 0.0, 0.0, -1.7976931348623157e+308) joint_limit_task.referenceSup.value = (1.0, 1.0, 0.0, 0, 0.0, 3.14, 0.33, 3.007, 1.39626, 2.2853981634, 1.3963, 3.9, 0.0, 1.7976931348623157e+308, 0.0, 1.7976931348623157e+308, 0.548, 0.548, 0.1, 1.7976931348623157e+308, 0.548, 0.548, 0.09, 1.48353, 0.714601836603, 1.3963, 0.8, 0.0, 1.7976931348623157e+308, 0.0, 1.7976931348623157e+308, 0.548, 0.548, 0.1, 1.7976931348623157e+308, 0.548, 0.548, 0.088, 1.7976931348623157e+308) joint_limit_task.dt.value=1 joint_limit_task.controlGain.value=1.0 #WAIST TASK## task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint') goal_waist = ((1.,0,0,0.0),(0,1.,0,-0.0),(0,0,1.,0.0),(0,0,0,1.),) task_waist_metakine.feature.frame('desired') #task_waist_metakine.feature.selec.value = '111101'#RzRyRxTzTyTx task_waist_metakine.gain.setConstant(1) task_waist_metakine.featureDes.position.value = goal_waist ##POSTURE TASK### posture_feature = FeaturePosture('featurePosition')
#a.createcollisionpair("base_link","wall1") #a.createcollisionpair("base_link","wall2") #a.createcollisionpair("base_link","wall3") a.createcollisionpair("base_link_1","platform1") a.createcollisionpair("base_link_1","platform2") a.createcollisionpair("base_link_2","platform1") a.createcollisionpair("base_link_2","platform2") a.createcollisionpair("base_link_3","platform1") a.createcollisionpair("base_link_3","platform2") a.createcollisionpair("base_link_4","platform1") a.createcollisionpair("base_link_4","platform2") task_collision_avoidance=TaskInequality('taskcollision') collision_feature = FeatureGeneric('collisionfeature') plug(a.collisionJacobian,collision_feature.jacobianIN) plug(a.collisionDistance,collision_feature.errorIN) task_collision_avoidance.add(collision_feature.name) task_collision_avoidance.referenceInf.value = (0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1) task_collision_avoidance.referenceSup.value = (2e10,2e10,2e10,2e10,2e10,2e10,2e10,2e10) task_collision_avoidance.dt.value=1 task_collision_avoidance.controlGain.value=50.0 #task_collision_avoidance.selec.value='110001' task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint') goal = ((1.,0,0,0.0),(0,1.,0,0.0),(0,0,1.,0),(0,0,0,1.),) task_waist_metakine.gain.setConstant(8) task_waist_metakine.featureDes.position.value = goal a.collisionJacobian.recompute(0) print a.collisionJacobian a.collisionDistance.recompute(0) print a.collisionDistance #robot.traceDefaultSignals() robot.addTrace(task_waist_metakine.task.name,'error')
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)