Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
 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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 5
0
    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)