[Documentation] [TitleIndex] [WordIndex

(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Link two joints [python]

Description: Reads the current position from one joint and publishes it to another joint.

Tutorial Level:

Reading and sending data from Python

This is a simple program that links one finger joint to another: it subscribes to the topic publishing a parent joints position data, and publishes the data of the selected parent joint as a target for it's child joint. This will simply make the child joint move together with the parent joint.

NB: To send a new target to a joint, you simply need to publish a std_msgs/Float64 message to the appropriate controllers command topic.

The full list of joints to send targets to the hand is: wrj1, wrj2, ffj4, ffj3, ffj0, mfj4, mfj3, mfj0, rfj4, rfj3, rfj0, lfj5, lfj4, lfj3, lfj0, thj5, thj4, thj3, thj2, thj1.

   1 import roslib; roslib.load_manifest('sr_hand')
   2 import rospy
   3 from pr2_controller_msgs.msg import JointControllerState
   4 from std_msgs.msg import Float64
   6 parent_name = "ffj3"
   7 child_name = "mfj3"
  10 def callback(data):
  11     """
  12     The callback function: called each time a message is received on the 
  13     topic parent joint controller state topic
  15     @param data: the message
  16     """
  17     # publish the message to the child joint controller command topic.
  18     # here we insert the joint name into the topic name
  19     pub = rospy.Publisher('/sh_'+child_name+'_position_controller/command', Float64)
  20     pub.publish(data.set_point)
  23 def listener():
  24     """
  25     The main function
  26     """
  27     # init the ros node
  28     rospy.init_node('joints_link_test', anonymous=True)
  30     # init the subscriber: subscribe to the
  31     # child joint controller topic, using the callback function
  32     # callback()
  33     rospy.Subscriber('/sh_'+parent_name+'_position_controller/state', JointControllerState, callback)
  34     # subscribe until interrupted
  35     rospy.spin()
  38 if __name__ == '__main__':
  39     listener()    

Let's look at this code.

   1 from pr2_controller_msgs.msg import JointControllerState
   2 from std_msgs.msg import Float64

   1 def listener():
   2   [...]
   3     rospy.Subscriber('/sh_'+parent_name+'_position_controller/state', JointControllerState, callback)

   1 def callback(data):
   2   [...]
   3     pub = rospy.Publisher('/sh_'+child_name+'_position_controller/command', Float64)
   4     pub.publish(data.set_point)

To test this example, you need to first start the simulator:

$ roslaunch sr_hand gazebo_arm_and_hand.launch

Then we run this code (which can be found in the sr_example package).

$ rosrun sr_example link_joints.py

Now if you send a target to FFJ3, MFJ3 will follow:

$ rostopic pub /sh_ffj3_position_controller/command std_msgs/Float64 1.5

Please note that you can find more examples in the sr_example package.

2020-02-22 13:16