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

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[4] [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[4] [as 别名]
def jointStateConverter(vrep_joint_state):
    vrep_joint_names = {"Shoulder_Joint" : "Mico_joint1", "Arm_Joint": "Mico_joint2", "Forearm_Joint" : "Mico_joint3", "Wrist_1_Joint" : "Mico_joint4","Wrist_2_Joint":"Mico_joint5","Hand_Joint":"Mico_joint6"}
    joint_names = list(vrep_joint_state.name)
    position_offset = [3.14, -1.57, 1.57, 3.14, 3.14,3.14]
    output = JointState()
    output.name =["Shoulder_Joint", "Arm_Joint", "Forearm_Joint", "Wrist_1_Joint","Wrist_2_Joint","Hand_Joint"]
    output.position = [0] * len(output.name)
    output.velocity = [0] * len(output.name)
    output.effort = [0] * len(output.name)

    for i in range(0,len(joint_names)):
        if joint_names[i] in vrep_joint_names.values():
            vrep_joint_index = int(joint_names[i][-1]) - 1
            output.position[vrep_joint_index] = vrep_joint_state.position[i] - position_offset[vrep_joint_index]
            output.velocity[vrep_joint_index] = vrep_joint_state.velocity[i]
            output.effort[vrep_joint_index] = vrep_joint_state.effort[i]
    #output.position = list(vrep_joint_state.position[0:6])
    output.position[4] = -1*output.position[4]
    #for i in range(0,6):
        #output.position[i] = 1
     #   output.position[i] = output.position[i] - position_offset[i]
    #output.velocity = vrep_joint_state.velocity[0:6]
    #output.effort = vrep_joint_state.effort[0:6]
    pub1.publish(output)

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