Python JointState.position[2]方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def talker():
    joint1 = radians(90)
    pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10000) # 10hz
    msg = JointState()
    msg.position = [0,joint1,0,0,0,0,0]
    pub.publish(msg)
    r.sleep()
    time.sleep(.4)
    
    while True:
        for acc in range(600,601):
            print '************************* acceleratioin: %d *************************' % acc
            # acc = 200
            trjFile = '../trj/acc_new_' + str(acc) + '.dat'
            trj = np.genfromtxt(trjFile, delimiter="\t")
            done = True
            i = 1
            while not rospy.is_shutdown() and i < len(trj):
                print i
                msg = JointState()
                msg.position = [0,joint1,0,0,0,0,0]
                msg.position[0] = radians(trj[i][0])
                msg.position[2] = radians(trj[i][0])
                msg.position[3] = radians(trj[i][0])
                msg.position[4] = radians(trj[i][0])
                msg.position[5] = radians(trj[i][0])
                msg.position[6] = radians(trj[i][0])
                pub.publish(msg)
                r.sleep()
                time.sleep((trj[i][2] - trj[i-1][2])/10)
                i += 1

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def joint_state_publisher():
    pub = rospy.Publisher("joint_states", JointState, queue_size=5)
    rospy.init_node("sr_jnt_stt")

    r = rospy.Rate(300)
    while not rospy.is_shutdown():
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.position = num_joints * [0.0]
        msg.velocity = num_joints * [0.0]
        # msg.effort = num_joints * [0.0]
        msg.name = ["joint1", "joint2", "joint3", "gripper_con"]
        msg.position[0] = math.radians(herkulex.get_servo_angle(1))
        msg.position[1] = math.radians(herkulex.get_servo_angle(2))
        msg.position[2] = 0.0
        msg.position[3] = 0.0
        pub.publish(msg)
        r.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
 def prepare_cartesian(self):
     # make sure the camera is past the cannula and tool vertical
     initial_joint_position = self._position_joint_desired
     if ((self._robot_name == 'PSM1') or (self._robot_name == 'PSM2') or (self._robot_name == 'PSM3') or (self._robot_name == 'ECM')):
         # set in position joint mode
         self.set_state_block(state = 'DVRK_POSITION_GOAL_JOINT')
             # create a new goal starting with current position
         goal = JointState()
         goal.position[:] = initial_joint_position
         goal.position[0] = 0.0
         goal.position[1] = 0.0
         goal.position[2] = 0.12
         self._goal_reached_event.clear()
         self.set_position_goal_joint.publish(goal)
         self._goal_reached_event.wait(60) # 1 minute at most
         if not self._goal_reached:
             rospy.signal_shutdown('failed to reach goal')
             sys.exit(-1)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def publish_to_nxt(jointState):
    pub = rospy.Publisher('position_controller', JointState)
    rospy.sleep(0.5) 
    try:
        gear_ratios = [7, 5, 5]
        # Create msg
        jp = JointState()
        jp.name = [None]*3
        jp.position = [None]*3
        # Fill messag
        jp.name[0] = jointState.name[0]
        jp.position[0] = (jointState.position[0]) * gear_ratios[0]
        jp.name[1] = jointState.name[1]
        jp.position[1] = (jointState.position[1]) * gear_ratios[1]
        jp.name[2] = jointState.name[2]
        jp.position[2] =  (jointState.position[2]) * gear_ratios[2]
        r = rospy.Rate(500)
        for c in range(1,5):
            pub.publish(jp)
            r.sleep()
        return 'arm moved successfully'
    except Exception, e:
        print e
        return 'arm move failure'

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def main(ra,max_vel12,max_vel3):
    # Create Pose Publisher and Subscriber
    state_pub=rospy.Publisher('/dvrk/PSM1/set_robot_state',String,queue_size=10)
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_position_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) 
    rospy.sleep(1)
    #set robot state
    state_pub.publish("DVRK_POSITION_JOINT")
    rospy.sleep(1)

    #hardcode home to zero 
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[0]*7
    joint_msg.velocity = []  
    joint_msg.effort = []
    
    rospy.sleep(1)

    scale=0.6
    #d12=max_vel12/ra*scale
    #d3=max_vel3/ra*scale

    d=0.008
    thresh=0.008
    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
    
      if -thresh<dq1<thresh and -thresh<dq2<thresh  and -thresh<dq3<thresh: 
      	joint_msg.position=[]
      	print(joint_msg)
      	joint_pub.publish(joint_msg)
        break
      else:
        if -thresh<joint_sub.position[0]<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]<0: 
         dq1=joint_sub.position[0]+d
        else: 
          dq1=joint_sub.position[0]-d

        if -thresh<joint_sub.position[1]<thresh: 
         dq2=joint_sub.position[1]
        elif joint_sub.position[1]<0: 
          dq2=joint_sub.position[1]+d
        else: 
          dq2=joint_sub.position[1]-d

        if -thresh<joint_sub.position[2]<thresh:
          dq3=joint_sub.position[2]
        elif joint_sub.position[2]<0: 
          dq3=joint_sub.position[2]+d/5
        else: 
          dq3=joint_sub.position[2]-d/5
  

      #joint_msg.position = [dq1, dq2 , dq3 , None, None, None, None]
      
      joint_msg.position[0]=dq1
      joint_msg.position[1]=dq2
      joint_msg.position[2]=dq3
      joint_msg.position[3]=joint_sub.position[3]
      joint_msg.position[3]=joint_sub.position[4]
      joint_msg.position[3]=joint_sub.position[5]
      joint_msg.position[3]=joint_sub.position[6]

      print(joint_msg)

      joint_pub.publish(joint_msg)
      rospy.sleep(1/float(ra))

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def main(portName1, portBaud1):
    print "INITIALIZING LEFT ARM NODE BY MARCOSOFT..."
    
    ###Communication with dynamixels:
    global dynMan1 
    dynMan1 = Dynamixel.DynamixelMan(portName1, portBaud1)
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    msgCurrentGripper = Float32()
    msgBatery = Float32()
    msgBatery = 0.0
    curretPos = [0,0,0,0,0,0,0,0]
    bitsPerRadian_0 = (4095)/((251)*(3.14159265/180))
    bitsPerRadian = (4095)/((360)*(3.141592/180)) 
    i = 0

    ### Set controller parameters 
    dynMan1.SetCWComplianceSlope(0, 32)
    dynMan1.SetCCWComplianceSlope(0, 32)
    dynMan1.SetCWComplianceSlope(1, 32)
    dynMan1.SetCCWComplianceSlope(1, 32)

    for i in range(2, 6):
        dynMan1.SetDGain(i, 25)
        dynMan1.SetPGain(i, 16)
        dynMan1.SetIGain(i, 1)

    ### Set servos features
    for i in range(0, 6):
        dynMan1.SetMaxTorque(i, 1024)
        dynMan1.SetTorqueLimit(i, 512)
        dynMan1.SetHighestLimitTemperature(i, 80)

    
    ###Connection with ROS
    rospy.init_node("left_arm")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["la_1_joint", "la_2_joint", "la_3_joint", "la_4_joint", "la_5_joint", "la_6_joint", "la_7_joint"]
    jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


    subPos = rospy.Subscriber("/hardware/left_arm/goal_pose", Float32MultiArray, callbackPos)
    subGripper = rospy.Subscriber("/hardware/left_arm/gripper_pose", Float32, callbackGripper)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubArmPose = rospy.Publisher("left_arm/current_pose", Float32MultiArray, queue_size = 1)
    pubGripper = rospy.Publisher("left_arm/current_gripper", Float32, queue_size = 1)
    pubBatery = rospy.Publisher("/hardware/robot_state/left_arm_battery", Float32, queue_size = 1)
    


    loop = rospy.Rate(10)

    while not rospy.is_shutdown():
        
        pos0 = float(-(2054-dynMan1.GetPresentPosition(0))/bitsPerRadian_0)
        pos1 = float((2309-dynMan1.GetPresentPosition(1))/bitsPerRadian)
        pos2 = float(-(1787-dynMan1.GetPresentPosition(2))/bitsPerRadian)
        pos3 = float(-(1969-dynMan1.GetPresentPosition(3))/bitsPerRadian)
        pos4 = float(-(2048-dynMan1.GetPresentPosition(4))/bitsPerRadian)
        pos5 = float((1848-dynMan1.GetPresentPosition(5))/bitsPerRadian)
        pos6 = float(-(2048-dynMan1.GetPresentPosition(6))/bitsPerRadian)
        #posD21 = float((1400-dynMan1.GetPresentPosition(7))/bitsPerRadian)
        #posD22 = float((1295-dynMan1.GetPresentPosition(8))/bitsPerRadian)
        
        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pos0
        jointStates.position[1] = pos1
        jointStates.position[2] = pos2
        jointStates.position[3] = pos3
        jointStates.position[4] = pos4
        jointStates.position[5] = pos5
        jointStates.position[6] = pos6
        msgCurrentPose.data[0] = pos0
        msgCurrentPose.data[1] = pos1
        msgCurrentPose.data[2] = pos2
        msgCurrentPose.data[3] = pos3
        msgCurrentPose.data[4] = pos4
        msgCurrentPose.data[5] = pos5
        msgCurrentPose.data[6] = pos6
        #msgCurrentGripper.data = posD21
        pubJointStates.publish(jointStates)
        pubArmPose.publish(msgCurrentPose)
        pubGripper.publish(msgCurrentGripper)

        if i == 10:
            msgBatery = float(dynMan1.GetPresentVoltage(2)/10)
            pubBatery.publish(msgBatery)
            i=0
        i+=1
        
        loop.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]

#.........这里部分代码省略.........

        #### Refresh gripper_pos ####
        if attemps < 50:
            if gripperTorqueActive:
                dynMan1.SetCWAngleLimit(7, 0)
                dynMan1.SetCCWAngleLimit(7, 0)
                dynMan1.SetCWAngleLimit(8, 0)
                dynMan1.SetCCWAngleLimit(8, 0)
                dynMan1.SetTorqueLimit(7, torqueGripper)
                dynMan1.SetTorqueLimit(8, torqueGripper)
                dynMan1.SetTorqueVale(7, speedGripper, torqueGripperCCW1)
                dynMan1.SetTorqueVale(8, speedGripper, torqueGripperCCW2)
                currentLoad_D21 = dynMan1.GetPresentLoad(7)
                currentLoad_D22 = dynMan1.GetPresentLoad(8)
            else:
                dynMan1.SetCWAngleLimit(7, 0)
                dynMan1.SetCCWAngleLimit(7, 4095)
                dynMan1.SetCWAngleLimit(8, 0)
                dynMan1.SetCCWAngleLimit(8, 4095)
                dynMan1.SetTorqueLimit(7, 500)
                dynMan1.SetTorqueLimit(8, 500)
                dynMan1.SetMovingSpeed(7, 200)
                dynMan1.SetMovingSpeed(8, 200)
                dynMan1.SetGoalPosition(7, gripperGoal_1)
                dynMan1.SetGoalPosition(8, gripperGoal_2)
                objOnHand = False
            ## This counter is reseated in the callback
            attemps += 1

        #### Refresh arms_position's readings #####
        for i in range(9):
            bitValues[i] = dynMan1.GetPresentPosition(i)
            if(bitValues[i] == 0):
                bitValues[i] = lastValues[i]
            else:
                lastValues[i] = bitValues[i]

        # CurrentLoad > 1023 means the oposite direction of load
        if currentLoad_D21 > 1023:
            currentLoad_D21 -= 1023

        if currentLoad_D22 > 1023:
            currentLoad_D22 -= 1023

        if gripperTorqueActive:
            currentLoad_D21 = (currentLoad_D21 + currentLoad_D22)/2
            if currentLoad_D21 > 200 and posD21 > -0.2 :
                objOnHand = True
            else:
                objOnHand = False

        pos0 = float( (zero_arm[0]-bitValues[0])/bitsPerRadian)
        pos1 = float(-(zero_arm[1]-bitValues[1])/bitsPerRadian)
        pos2 = float(-(zero_arm[2]-bitValues[2])/bitsPerRadian)
        pos3 = float(-(zero_arm[3]-bitValues[3])/bitsPerRadian)
        pos4 = float(-(zero_arm[4]-bitValues[4])/bitsPerRadian)
        pos5 = float( (zero_arm[5]-bitValues[5])/bitsPerRadian)
        pos6 = float(-(zero_arm[6]-bitValues[6])/bitsPerRadian)
        posD21 = float( (zero_gripper[0]-bitValues[7])/bitsPerRadian)
        posD22 = float(-(zero_gripper[1]-bitValues[8])/bitsPerRadian)

        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pos0
        jointStates.position[1] = pos1
        jointStates.position[2] = pos2
        jointStates.position[3] = pos3
        jointStates.position[4] = pos4
        jointStates.position[5] = pos5
        jointStates.position[6] = pos6
        jointStates.position[7] = posD21
        jointStates.position[8] = posD22
        msgCurrentPose.data[0] = pos0
        msgCurrentPose.data[1] = pos1
        msgCurrentPose.data[2] = pos2
        msgCurrentPose.data[3] = pos3
        msgCurrentPose.data[4] = pos4
        msgCurrentPose.data[5] = pos5
        msgCurrentPose.data[6] = pos6
        msgCurrentGripper.data = posD21
        msgObjOnHand = objOnHand
        pubJointStates.publish(jointStates)
        pubArmPose.publish(msgCurrentPose)
        pubGripper.publish(msgCurrentGripper)
        pubObjOnHand.publish(msgObjOnHand)

        if i == 20:
            msgBatery = float(dynMan1.GetPresentVoltage(2)/10.0)
            pubBatery.publish(msgBatery)
            i=0
        i+=1
        loop.sleep()

    dynMan1.SetTorqueDisable(0)
    dynMan1.SetTorqueDisable(1)
    dynMan1.SetTorqueDisable(2)
    dynMan1.SetTorqueDisable(3)
    dynMan1.SetTorqueDisable(4)
    dynMan1.SetTorqueDisable(5)
    dynMan1.SetTorqueDisable(6)
    dynMan1.Close()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def main(ra,max_vel12,max_vel3):
    # Create Pose Publisher and Subscriber
    state_pub=rospy.Publisher('/dvrk/PSM1/set_robot_state',String,queue_size=10)
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_position_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) # 1hz
    rospy.sleep(1)
    
    #set robot state
    state_pub.publish("DVRK_POSITION_JOINT")
    rospy.sleep(1)

    #hardcode home to zero 
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[0]*7
    joint_msg.velocity = []  
    joint_msg.effort = []
    
    rospy.sleep(1)

    vel_scale=4
    #d12=max_vel12/ra*vel_scale
    #d3=max_vel3/ra*vel_scale
    d12=0.005
    d3=0.005/10 

    thresh=0.003
    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
    
      if -thresh<dq1<thresh and -thresh<dq2<thresh  and -thresh<dq3<thresh: 
        break
      else:
        if -thresh<joint_sub.position[0]<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]<0: 
         dq1=joint_sub.position[0]+d12
        else: 
          dq1=joint_sub.position[0]-d12

        if -thresh<joint_sub.position[1]<thresh: 
         dq2=joint_sub.position[1]
        elif joint_sub.position[1]<0: 
          dq2=joint_sub.position[1]+d12
        else: 
          dq2=joint_sub.position[1]-d12

        if -thresh<joint_sub.position[2]<thresh:
          dq3=joint_sub.position[2]
        elif joint_sub.position[2]<0: 
          dq3=joint_sub.position[2]+d3
        else: 
          dq3=joint_sub.position[2]-d3
  

      #joint_msg.position = [dq1, dq2 , dq3 , None, None, None, None]
      
      joint_msg.position[0]=dq1
      joint_msg.position[1]=dq2
      joint_msg.position[2]=dq3
      joint_msg.position[3]=joint_sub.position[3]
      joint_msg.position[3]=joint_sub.position[4]
      joint_msg.position[3]=joint_sub.position[5]
      joint_msg.position[3]=joint_sub.position[6]

      print(joint_msg)

      joint_pub.publish(joint_msg)
      rospy.sleep(1/float(ra))

    rospy.sleep(3)
    


    #Excitation Trajectory Position Coordinates
    i=0
    q_data1 = genfromtxt('Q_p1.csv', delimiter=',')
    q_data2 = genfromtxt('Q_p2.csv', delimiter=',')
    q_data3 = genfromtxt('Q_p3.csv', delimiter=',')
   
   
    q_output=[None]*len(q_data1)
    qd_output=[None]*len(q_data1)
    torque_output=[None]*len(q_data1)

    while not rospy.is_shutdown():
      if i>len(q_data1)-1:
        break
      joint_msg.position = [q_data1[i], q_data2[i], q_data3[i],joint_sub.position[3],joint_sub.position[4],joint_sub.position[5],joint_sub.position[6]]
      joint_msg.velocity = []  
      joint_msg.effort = []
      print(joint_msg)
      joint_pub.publish(joint_msg)

#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def main(ra,max_vel12,max_vel3):
    # Create Pose Publisher and Subscriber
    state_pub=rospy.Publisher('/dvrk/PSM1/set_robot_state',String,queue_size=10)
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_position_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) # 1hz
    rospy.sleep(1)
    
    #set robot state
    state_pub.publish("DVRK_POSITION_JOINT")
    rospy.sleep(1)

  
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[0]*7
    joint_msg.velocity = []  
    joint_msg.effort = []
    rospy.sleep(1)

    
    #Go to Home Position
    d12=0.007    #IF YOU WANT TO CHANGE THE SPEED OF ROBOT CHANGE THIS. Range of Values: 0.005-0.02.
    d3=0.005/10 
    thresh=0.01

    target_q1=0
    target_q2=-0.74
    target_q3=0.22

    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
      if -thresh<dq1-target_q1<thresh and -thresh<dq2-target_q2<thresh  and -thresh<dq3-target_q3<thresh: 
        break
      else:
        if -thresh<joint_sub.position[0]-target_q1<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]-target_q1<0: 
         dq1=joint_sub.position[0]+d12
        else: 
          dq1=joint_sub.position[0]-d12

        if -thresh<joint_sub.position[1]-target_q2<thresh: 
         dq2=joint_sub.position[1]
        elif joint_sub.position[1]-target_q2<0: 
          dq2=joint_sub.position[1]+d12
        else: 
          dq2=joint_sub.position[1]-d12

        if -thresh<joint_sub.position[2]-target_q3<thresh:
          dq3=joint_sub.position[2]
        elif joint_sub.position[2]-target_q3<0: 
          dq3=joint_sub.position[2]+d3
        else: 
          dq3=joint_sub.position[2]-d3
  

      #joint_msg.position = [dq1, dq2 , dq3 , None, None, None, None]
      
      joint_msg.position[0]=dq1
      joint_msg.position[1]=dq2
      joint_msg.position[2]=dq3
      joint_msg.position[3]=joint_sub.position[3]
      joint_msg.position[3]=joint_sub.position[4]
      joint_msg.position[3]=joint_sub.position[5]
      joint_msg.position[3]=joint_sub.position[6]

      print(joint_msg)

      joint_pub.publish(joint_msg)
      rospy.sleep(1/float(ra))

    rospy.sleep(3)


#Start Scan Sequence

    target_q1=-1.4
    target_q2=-0.74
    target_q3=0.22

    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
      if -thresh<dq1-target_q1<thresh and -thresh<dq2-target_q2<thresh  and -thresh<dq3-target_q3<thresh: 
        break
      else:
        if -thresh<joint_sub.position[0]-target_q1<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]-target_q1<0: 
         dq1=joint_sub.position[0]+d12
        else: 
          dq1=joint_sub.position[0]-d12

#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[2] [as 别名]
def main(ra,max_vel12,max_vel3):
    # Create Pose Publisher and Subscriber
    state_pub=rospy.Publisher('/dvrk/PSM1/set_robot_state',String,queue_size=10)
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_position_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) # 1hz
    rospy.sleep(1)
    
    #set robot state
    state_pub.publish("DVRK_POSITION_JOINT")
    rospy.sleep(1)

  
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[0]*7
    joint_msg.velocity = []  
    joint_msg.effort = []
    rospy.sleep(1)

    
    #Go to negative q2 position
    d12=0.02
    d3=0.005/10 
    thresh=0.01

    target=-0.6

    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
      if -thresh<dq1<thresh and -thresh<dq2-target<thresh  and -thresh<dq3<thresh: 
        break
      else:
        if -thresh<joint_sub.position[0]<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]<0: 
         dq1=joint_sub.position[0]+d12
        else: 
          dq1=joint_sub.position[0]-d12

        if -thresh<joint_sub.position[1]-target<thresh: 
         dq2=joint_sub.position[1]
        elif joint_sub.position[1]-target<0: 
          dq2=joint_sub.position[1]+d12
        else: 
          dq2=joint_sub.position[1]-d12

        if -thresh<joint_sub.position[2]<thresh:
          dq3=joint_sub.position[2]
        elif joint_sub.position[2]<0: 
          dq3=joint_sub.position[2]+d3
        else: 
          dq3=joint_sub.position[2]-d3
  

      #joint_msg.position = [dq1, dq2 , dq3 , None, None, None, None]
      
      joint_msg.position[0]=dq1
      joint_msg.position[1]=dq2
      joint_msg.position[2]=dq3
      joint_msg.position[3]=joint_sub.position[3]
      joint_msg.position[3]=joint_sub.position[4]
      joint_msg.position[3]=joint_sub.position[5]
      joint_msg.position[3]=joint_sub.position[6]

      print(joint_msg)

      joint_pub.publish(joint_msg)
      rospy.sleep(1/float(ra))

    rospy.sleep(3)
    
    #Go to positive q2 pos
    target=0.7

    dq1=thresh+1
    dq2=thresh+1
    dq3=thresh+1
    while not rospy.is_shutdown():
      if -thresh<dq1<thresh and -thresh<dq2-target<thresh  and -thresh<dq3<thresh: 
        break
      else:
        if -thresh<joint_sub.position[0]<thresh:
          dq1=joint_sub.position[0] 
        elif joint_sub.position[0]<0: 
         dq1=joint_sub.position[0]+d12
        else: 
          dq1=joint_sub.position[0]-d12

        if -thresh<joint_sub.position[1]-target<thresh: 
         dq2=joint_sub.position[1]
        elif joint_sub.position[1]-target<0: 
          dq2=joint_sub.position[1]+d12
        else: 
          dq2=joint_sub.position[1]-d12
#.........这里部分代码省略.........