本文整理汇总了Python中sensor_msgs.msg.JointState.position[5]方法的典型用法代码示例。如果您正苦于以下问题：Python JointState.position[5]方法的具体用法？Python JointState.position[5]怎么用？Python JointState.position[5]使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.JointState的用法示例。
在下文中一共展示了JointState.position[5]方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: talker
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[5] [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示例2: main
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[5] [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()示例3: main
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[5] [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()