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)
Beispiel #2
0
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')
Beispiel #5
0
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)