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

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [as 别名]
 def joint_goal(self):
     rospy.loginfo(rospy.get_caller_id() + " -> starting joint goal")
     # set in position joint mode
     self.set_state_block("DVRK_POSITION_GOAL_JOINT")
     # get current position
     initial_joint_position = []
     initial_joint_position[:] = self._position_joint_desired
     rospy.loginfo(
         rospy.get_caller_id() + " -> testing goal joint position for 2 joints of %i", len(initial_joint_position)
     )
     amplitude = math.radians(10.0)
     # create a new goal starting with current position
     goal = JointState()
     goal.position[:] = initial_joint_position
     # first motion
     goal.position[0] = initial_joint_position[0] + amplitude
     goal.position[1] = initial_joint_position[1] - amplitude
     self.set_position_goal_joint_publish_and_wait(goal)
     # second motion
     goal.position[0] = initial_joint_position[0] - amplitude
     goal.position[1] = initial_joint_position[1] + amplitude
     self.set_position_goal_joint_publish_and_wait(goal)
     # back to initial position
     goal.position[:] = initial_joint_position
     self.set_position_goal_joint_publish_and_wait(goal)
     rospy.loginfo(rospy.get_caller_id() + " <- joint goal complete")

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [as 别名]
def main():
    print "INITIALIZING HEAD NODE IN SIMULATION MODE BY MARCOSOFT..."
    ###Connection with ROS
    rospy.init_node("head")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["pan_connect", "tilt_connect"]
    jointStates.position = [0 ,0]

    subPosition = rospy.Subscriber("head/goal_pose", Float32MultiArray, callbackPosHead)
    pubHeadPose = rospy.Publisher("head/current_pose", Float32MultiArray, queue_size = 1);
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubHeadBattery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size=1)
    
    loop = rospy.Rate(10)

    global goalPan
    global goalTilt
    goalPan = 0
    goalTilt = 0
    pan = 0;
    tilt = 0;
    speedPan = 0.1 #These values should represent the Dynamixel's moving_speed 
    speedTilt = 0.1
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0, 0]
    while not rospy.is_shutdown():
        deltaPan = goalPan - pan;
        deltaTilt = goalTilt - tilt;
        if deltaPan > speedPan:
            deltaPan = speedPan;
        if deltaPan < -speedPan:
            deltaPan = -speedPan;
        if deltaTilt > speedTilt:
            deltaTilt = speedTilt;
        if deltaTilt < -speedTilt:
            deltaTilt = -speedTilt;
        pan += deltaPan
        tilt += deltaTilt
        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pan
        jointStates.position[1] = -tilt #A tilt > 0 goes upwards, but to keep a dextereous system, positive tilt should go downwards
        pubJointStates.publish(jointStates)
        #print "Poses: " + str(panPose) + "   " + str(tiltPose)
        msgCurrentPose.data = [pan, tilt]
        pubHeadPose.publish(msgCurrentPose)
        msgBattery = Float32()
        msgBattery.data = 19.0
        pubHeadBattery.publish(msgBattery);
        loop.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [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[1] [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[1] [as 别名]
 def joint_direct(self):
     rospy.loginfo(rospy.get_caller_id() + ' -> starting joint direct')
     # set in position joint mode
     self.set_state_block('DVRK_POSITION_JOINT')
     # get current position
     initial_joint_position = []
     initial_joint_position[:] = self._position_joint_desired
     rospy.loginfo(rospy.get_caller_id() + " -> testing direct joint position for 2 joints of %i", len(initial_joint_position))
     amplitude = math.radians(10.0) # +/- 10 degrees
     duration = 5  # seconds
     rate = 200 # aiming for 200 Hz
     samples = duration * rate
     # create a new goal starting with current position
     goal = JointState()
     goal.position[:] = initial_joint_position
     for i in xrange(samples):
         goal.position[0] = initial_joint_position[0] + amplitude *  math.sin(i * math.radians(360.0) / samples)
         goal.position[1] = initial_joint_position[1] + amplitude *  math.sin(i * math.radians(360.0) / samples)
         self.set_position_joint.publish(goal)
         rospy.sleep(1.0 / rate)
     rospy.loginfo(rospy.get_caller_id() + ' <- joint direct complete')

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [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[1] [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[1] [as 别名]

#.........这里部分代码省略.........
        dynMan1.SetTorqueEnable(0, 1)
        dynMan1.SetTorqueEnable(1, 1)
     
        dynMan1.SetMovingSpeed(0, 90)
        dynMan1.SetMovingSpeed(1, 90)

    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["pan_connect", "tilt_connect"]
    jointStates.position = [0, 0]

    jointStatesSimul = JointState()
    jointStatesSimul.name = ["pan_connect", "tilt_connect"]
    jointStatesSimul.position = [0, 0]
    
    ## Subscribers
    subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead)
    subSimul = rospy.Subscriber("/simulated", Bool, callback_simulated)
    pubCurrentPose = rospy.Publisher("/hardware/head/current_pose", Float32MultiArray, queue_size = 1)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size = 1)
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0, 0]
    msgCurrentPosesimul = Float32MultiArray()
    msgCurrentPosesimul.data = [0, 0]
    
    loop = rospy.Rate(50)

    #lastPan = 0.0
    #lastTilt = 0.0
    goalPan = 0
    goalTilt = 0
    speedPan = 0.1 #These values should represent the Dynamixel's moving_speed 
    speedTilt = 0.1
    goalPansimul = 0
    goalTiltsimul= 0
    speedPansimul = 0.1 #These values should represent the Dynamixel's moving_speed 
    speedTiltsimul = 0.1

    while not rospy.is_shutdown():

        if simul == True:
            deltaPansimul = goalPansimul - pansimul
            deltaTiltsimul = goalTiltsimul - tiltsimul
            if deltaPansimul > speedPansimul:
                deltaPansimul = speedPansimul
            if deltaPansimul < -speedPansimul:
                deltaPansimul = -speedPansimul
            if deltaTiltsimul > speedTiltsimul:
                deltaTiltsimul = speedTiltsimul
            if deltaTiltsimul < -speedTiltsimul:
                deltaTiltsimul = -speedTiltsimul
            pansimul += deltaPansimul
            tiltsimul += deltaTiltsimul

            #print "HardwareHead.->pan " + str(pans)
            #print "HardwareHead.->tilt " + str(tilts)
        
            jointStatesSimul.header.stamp = rospy.Time.now()
            jointStatesSimul.position[0] = pansimul
            jointStatesSimul.position[1] = -tiltsimul

            pubJointStates.publish(jointStatesSimul)  #We substract 0.1 to correct an offset error due to the real head position
            msgCurrentPosesimul.data = [pansimul, tiltsimul]
            pubCurrentPose.publish(msgCurrentPosesimul)

            msgBattery = Float32()
            msgBattery.data = 12.0
            pubBatery.publish(msgBattery)

        else:
            deltaPan = goalPan - pan
            deltaTilt = goalTilt - tilt
            if deltaPan > speedPan:
                deltaPan = speedPan
            if deltaPan < -speedPan:
                deltaPan = -speedPan
            if deltaTilt > speedTilt:
                deltaTilt = speedTilt
            if deltaTilt < -speedTilt:
                deltaTilt = -speedTilt
            pan += deltaPan
            tilt += deltaTilt
            #print "HardwareHead.->pan " + str(pan)
            #print "HardwareHead.->tilt " + str(tilt)
        
            jointStates.header.stamp = rospy.Time.now()
            jointStates.position[0] = pan
            jointStates.position[1] = -tilt - 0.04#goes upwards, but to keep a dextereous system, positive tilt should go downwards
            pubJointStates.publish(jointStates)  #We substract 0.1 to correct an offset error due to the real head position
            msgCurrentPose.data = [pan, tilt]
            pubCurrentPose.publish(msgCurrentPose)

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

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [as 别名]
def mover():

    l1 = 1
    
    l2 = 1
    
    T = rospy.get_param("T")
    
    w = 2*math.pi/T
     
    t0 = time.time()
    
    pub = rospy.Publisher('joint_states',JointState,queue_size=10)
    
    frequency = rospy.get_param("control_frequency")
   
    rate = rospy.Rate(frequency)
    
    joint_state = JointState()
    
    joint_state.name.append("joint1")
    
    joint_state.name.append("joint2")
    
    joint_state.name.append("joint3")

    joint_state.position.append(0) 
    
    joint_state.position.append(0)
    
    joint_state.position.append(0)
    
    listener = tf.TransformListener()
    
    br = tf.TransformBroadcaster()
    
    trans1 = (0,0,0)
    
    trans2 = (0,0,0)
    
    trans3 = (0,0,0)
    
    rot1 = (0,0,0)
    
    rot2 = (0,0,0)
    
    rot3 = (0,0,0)
    
    rospy.sleep(0.5)
    
    while not rospy.is_shutdown():
    
        t = time.time() - t0
        
        x = 0.5*math.cos(w*t)+1.25

        y = 0.5*math.sin(w*t)
        
        r = math.sqrt(x**2+y**2)
        
        a = (l1**2+l2**2-r**2)/(2*l1*l2)
        
        b = (r**2+l1**2-l2**2)/(2*l1*r)
        
        theta1 = math.acos(a)
        
        theta2 = math.acos(b)
        
        joint_state.position[0] = math.atan(y/x)-theta2
        joint_state.position[1] = math.pi - theta1
    
        stamp = rospy.get_rostime()            
           
        br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0, 0, joint_state.position[0]),stamp, "link1","base_link")
        br.sendTransform((1,0,0),tf.transformations.quaternion_from_euler(0, 0, joint_state.position[1]),stamp, "link2","link1")
        br.sendTransform((1,0,0),tf.transformations.quaternion_from_euler(0, 0, 0),stamp, "link3","link2")
        
        pub.publish(joint_state)   

        rate.sleep()   

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [as 别名]
    pub = rospy.Publisher('/joint_states', JointState, queue_size=100)

    msg = JointState()
    msg.name = ['head_pan_joint', 'head_tilt_joint', 'l_eyebrow_joint', 'r_eyebrow_joint']
    msg.position = [0.0, 0.0, 0.0, 0.0]

    rate = rospy.Rate(1.0)

    while not rospy.is_shutdown():

        try:
            msg.position[0] = get_servo_state.call(0).position
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

        try:
            msg.position[1] = get_servo_state.call(1).position
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

        try:
            msg.position[2] = msg.position[3] = get_servo_state.call(2).position
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

        print msg

        msg.header.stamp = rospy.Time.now()
        pub.publish(msg)
        rate.sleep()
#        rospy.spinOnce()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [as 别名]
def main(portName, portBaud):
    print "INITIALIZING HEAD NODE..."

    ###Communication with dynamixels:
    global dynMan1
    dynMan1 = Dynamixel.DynamixelMan(portName, portBaud)
    pan = 0;
    tilt = 0;
    i = 0

    ### Set controller parameters
    dynMan1.SetDGain(1, 25)
    dynMan1.SetPGain(1, 16)
    dynMan1.SetIGain(1, 1)
    dynMan1.SetDGain(5, 25)
    dynMan1.SetPGain(5, 16)
    dynMan1.SetIGain(5, 1)


    ### Set servos features
    dynMan1.SetMaxTorque(1, 1024)
    dynMan1.SetTorqueLimit(1, 512)
    dynMan1.SetHighestLimitTemperature(1, 80)
    dynMan1.SetMaxTorque(5, 1024)
    dynMan1.SetTorqueLimit(5, 512)
    dynMan1.SetHighestLimitTemperature(5, 80)
    
    ###Connection with ROS
    rospy.init_node("head")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["pan_connect", "tilt_connect"]
    jointStates.position = [0, 0]
    
    ## Subscribers
    subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead)
    #subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size = 1)
    
    bitsPerRadian = (1023)/((300)*(3.14159265358979323846/180))

    dynMan1.SetCWAngleLimit(5, 0)
    dynMan1.SetCCWAngleLimit(5, 1023)

    dynMan1.SetCWAngleLimit(1, 0)
    dynMan1.SetCCWAngleLimit(1, 1023)
    #dynMan1.SetGoalPosition(5, 512)
    #dynMan1.SetGoalPosition(1, 674)
 
    dynMan1.SetTorqueEnable(5, 1)
    dynMan1.SetTorqueEnable(1, 1)
     
    dynMan1.SetMovingSpeed(5, 50)
    dynMan1.SetMovingSpeed(1, 50)
    loop = rospy.Rate(10)
    
    while not rospy.is_shutdown():
        panPose = float((512-dynMan1.GetPresentPosition(5))/bitsPerRadian)
        tiltPose = float((674-dynMan1.GetPresentPosition(1))/bitsPerRadian)

        # Pose in bits
        panPose = dynMan1.GetPresentPosition(5)
        tiltPose = dynMan1.GetPresentPosition(1)
        
        # Pose in rad
        pan = (panPose - 512)*300/1023*3.14159265358979323846/180
        tilt = (tiltPose - 674)*300/1023*3.14159265358979323846/180
        
        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pan
        jointStates.position[1] = -tilt #A tilt > 0 goes upwards, but to keep a dextereous system, positive tilt should go downwards
        pubJointStates.publish(jointStates)

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

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

#.........这里部分代码省略.........
    dynMan1.SetIGain(5, 1)


    ### Set servos features
    #dynMan1.SetMaxTorque(1, 1023)
    dynMan1.SetTorqueLimit(1, 512)
    #dynMan1.SetHighestLimitTemperature(1, 80)
    #dynMan1.SetMaxTorque(5, 1023)
    dynMan1.SetTorqueLimit(5, 512)
    #dynMan1.SetHighestLimitTemperature(5, 80)
    
    ###Connection with ROS
    rospy.init_node("head")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["pan_connect", "tilt_connect"]
    jointStates.position = [0, 0]
    
    ## Subscribers
    subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead)
    pubCurrentPose = rospy.Publisher("/hardware/head/current_pose", Float32MultiArray, queue_size = 1);
    #subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size = 1)
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0, 0]
    

    dynMan1.SetCWAngleLimit(5, 1023)
    dynMan1.SetCCWAngleLimit(5, 3069)

    dynMan1.SetCWAngleLimit(1, 0)
    dynMan1.SetCCWAngleLimit(1, 4095)
    
    dynMan1.SetGoalPosition(5, 1750)
    dynMan1.SetGoalPosition(1, 3550)
 
    dynMan1.SetTorqueEnable(5, 1)
    dynMan1.SetTorqueEnable(1, 1)
     
    dynMan1.SetMovingSpeed(5, 90)
    dynMan1.SetMovingSpeed(1, 90)
    loop = rospy.Rate(30)

    lastPan = 0.0;
    lastTilt = 0.0;
    goalPan = 0;
    goalTilt = 0;
    speedPan = 0.1 #These values should represent the Dynamixel's moving_speed 
    speedTilt = 0.1
    while not rospy.is_shutdown():
        # Pose in bits
        #panPose = dynMan1.GetPresentPosition(5)
        #tiltPose = dynMan1.GetPresentPosition(1)
        

        # Pose in rad
        #if panPose != None:
        #    pan = (panPose - 1750)*360/4095*3.14159265358979323846/180
        #    if abs(lastPan-pan) > 0.78539816339:
        #        pan = lastPan
        #else:
        #    pan = lastPan

        #if tiltPose != None:
        #    tilt = (tiltPose - 970)*360/4095*3.14159265358979323846/180
        #    if abs(lastTilt-tilt) > 0.78539816339:
        #        tilt = lastTilt
        #else:
        #    tilt = lastTilt
        #SINCE READING IS NOT WORKING, WE ARE FAKING THE REAL SERVO POSE
        deltaPan = goalPan - pan;
        deltaTilt = goalTilt - tilt;
        if deltaPan > speedPan:
            deltaPan = speedPan;
        if deltaPan < -speedPan:
            deltaPan = -speedPan;
        if deltaTilt > speedTilt:
            deltaTilt = speedTilt;
        if deltaTilt < -speedTilt:
            deltaTilt = -speedTilt;
        pan += deltaPan
        tilt += deltaTilt
        
        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pan
        jointStates.position[1] = -tilt - 0.08 #goes upwards, but to keep a dextereous system, positive tilt should go downwards
        pubJointStates.publish(jointStates)  #We substract 0.1 to correct an offset error due to the real head position
        msgCurrentPose.data = [pan, tilt]
        pubCurrentPose.publish(msgCurrentPose)

        if i == 10:
            msgBatery = float(dynMan1.GetPresentVoltage(5)/10.0)
            pubBatery.publish(msgBatery)
            i=0
        i+=1
        
        lastPan = pan
        lastTilt = tilt 
        loop.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[1] [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[1] [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[1] [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()