本文整理汇总了Python中sensor_msgs.msg.JointState.position[7]方法的典型用法代码示例。如果您正苦于以下问题：Python JointState.position[7]方法的具体用法？Python JointState.position[7]怎么用？Python JointState.position[7]使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.JointState的用法示例。
在下文中一共展示了JointState.position[7]方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: main
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[7] [as 别名]
def main():
print "INITIALIZING RIGHT ARM NODE IN SIMULATION BY MARCOSOFT..."
###Connection with ROS
rospy.init_node("right_arm")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["ra_1_joint", "ra_2_joint", "ra_3_joint", "ra_4_joint", "ra_5_joint", "ra_6_joint", "ra_7_joint", "ra_grip_left", "ra_grip_right"]
jointStates.position = [0, 0, 0, 0, 0, 0, 0, 0, 0]
subPosition = rospy.Subscriber("right_arm/goal_pose", Float32MultiArray, callbackPos)
subGripper = rospy.Subscriber("/hardware/right_arm/goal_gripper", Float32, callbackGripper)
subTorqueGripper = rospy.Subscriber("/hardware/right_arm/torque_gripper", Float32, callbackTorqueGripper)
pubArmPose = rospy.Publisher("right_arm/current_pose", Float32MultiArray, queue_size = 1)
pubGripper = rospy.Publisher("right_arm/current_gripper", Float32, queue_size = 1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery", Float32, queue_size=1)
loop = rospy.Rate(10)
global goalAngles;
global goalGripper
global speeds
goalAngles = [0, 0, 0, 0, 0, 0, 0]
angles = [0, 0, 0, 0, 0, 0, 0]
speeds = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
goalGripper = 0
gripper = 0
gripperSpeed = 0.1
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0, 0, 0, 0, 0, 0]
msgCurrentGripper = Float32()
msgCurrentGripper.data = 0
deltaAngles = [0, 0, 0, 0, 0, 0, 0]
deltaGripper = 0
while not rospy.is_shutdown():
for i in range(len(deltaAngles)):
deltaAngles[i] = goalAngles[i] - angles[i]
if deltaAngles[i] > speeds[i]:
deltaAngles[i] = speeds[i]
if deltaAngles[i] < -speeds[i]:
deltaAngles[i] = -speeds[i]
angles[i] += deltaAngles[i]
jointStates.position[i] = angles[i]
msgCurrentPose.data[i] = angles[i]
deltaGripper = goalGripper - gripper
if deltaGripper > gripperSpeed:
deltaGripper = gripperSpeed
if deltaGripper < -gripperSpeed:
deltaGripper = -gripperSpeed
gripper += deltaGripper
jointStates.position[7] = gripper
jointStates.position[8] = gripper
msgCurrentGripper.data = gripper
jointStates.header.stamp = rospy.Time.now()
pubJointStates.publish(jointStates)
pubArmPose.publish(msgCurrentPose)
pubGripper.publish(msgCurrentGripper);
msgBattery = Float32()
msgBattery.data = 11.6
pubArmBattery.publish(msgBattery);
loop.sleep()示例2: main
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[7] [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()