def publish_joint_states(self):
# Construct message & publish joint states
msg = JointStatePR2()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for i, joint in enumerate(self.motor_states):
#linear fit into torque in Nm
effort_nm = joint.load * 0.00336 * 1.083775
if i == 6:
#finger 1
msg.name.append(self.joints[i])
msg.position.append(-1 * joint.position * self.finger_divisor)
msg.velocity.append(-1 * joint.speed)
msg.effort.append(-1 * effort_nm)
#finger 2
msg.name.append(self.joints[i+1])
msg.position.append(1 * joint.position * self.finger_divisor)
msg.velocity.append(1 * joint.speed)
msg.effort.append(1 * effort_nm)
else:
msg.name.append(self.joints[i])
msg.position.append(joint.position)
msg.velocity.append(joint.speed)
msg.effort.append(effort_nm)
msg.header.stamp = rospy.Time.now()
self.joint_states_pub.publish(msg)
def publish_joint_states(self):
# Construct message & publish joint states
msg = JointStatePR2()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for joint in self.joint_states.values():
msg.name.append(joint.name)
msg.position.append(joint.position)
msg.velocity.append(joint.velocity)
msg.effort.append(joint.effort)
msg.header.stamp = rospy.Time.now()
self.joint_states_pub.publish(msg)
def publish_joint_states(self):
# Construct message & publish joint states
msg = JointStatePR2()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for joint in self.joint_states.values():
msg.name.append(joint.name)
#if joint.name == "left_finger_joint" or joint.name == "right_finger_joint":
msg.position.append(joint.position)
msg.velocity.append(joint.velocity)
msg.effort.append(joint.effort)
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
self.joint_states_pub.publish(msg)
def publish_joint_states(self):
# Construct message & publish joint states
msg = JointStatePR2()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for joint in self.joint_states.values():
msg.name.append(joint.name)
fudge_value = rospy.get_param(
'~fudge_factor/' + joint.name + '/value', 0.0)
j_pos = joint.position - fudge_value
# rospy.loginfo("fudge " + str(joint.name) + ': ' + str(j_pos) + ' = ' + str(joint.position) + ' - ' + str(fudge_value))
msg.position.append(j_pos)
msg.velocity.append(joint.velocity)
msg.effort.append(joint.effort)
msg.header.stamp = rospy.Time.now()
self.joint_states_pub.publish(msg)
def publish_joint_states(self):
# Construct message & publish joint states
msg = JointStatePR2()
msg.name = [
'right_gripper_finger_joint', 'left_gripper_finger_joint',
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
msg.position = ast.literal_eval(sys.argv[1])
msg.velocity = []
msg.effort = []
for joint in self.joint_states.values():
msg.name.append(joint.name)
msg.position.append(joint.position)
msg.velocity.append(joint.velocity)
msg.effort.append(joint.effort)
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_link'
self.joint_states_pub.publish(msg)