Lab 3 Forward Kinematics/Coordinate Transformations Solution

Goals




By the end of this lab you should be able to:




Compute the forward kinematics map for a robotic manipulator




Compare your own forward kinematics implementation to the functionality provided by ROS Use the powerful functionality of tf2 in your own ROS node.




Make Baxter/Sawyer move to simple joint position goals




View the sensor and state data published by Baxter using RViz













Relevant Tutorials and Documentation:




Baxter SDK: https://github.com/RethinkRobotics/sdk-docs/wiki/API-Reference




Sawyer SDK: http://sdk.rethinkrobotics.com/intera/API_Reference Baxter Joint Position Control Examples :

https://github.com/RethinkRobotics/sdk-docs/wiki/Joint-Position-Example




Sawyer Joint Position Control Examples : http://sdk.rethinkrobotics.com/intera/Joint_Position_Example




tf2 Tutorials: http://wiki.ros.org/tf2/Tutorials




Contents




1
Forward kinematics
2


1.1
Kinematic functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2


1.2
Writing the forward kinematics map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2


1.3
Compare with built-in ROS functionality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3


1.4
Writing a tf Listener . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
2
Make Baxter/Sawyer move
5






Developed by Aaron Bestick, Austin Buchan, Fall 2014. Modi ed by Victor Shia and Jaime Fisac, Fall 2015; Dexter Scobee and Oladapo Afolabi, Fall 2016; David Fridovich-Keil and Laura Hallock, Fall 2017. Ravi Pandya, Nandita Iyer, Phillip Wu, and Valmik Prabhu, Fall 2018




























1
Introduction




Coordinate transformations are one of the fundamental mathematical tools of robotics. One of the most common applications of coordinate transformations is the forward kinematics problem. Given a robotic manipulator, forward kinematics answers the following question: Given a speci ed angle for each joint in the manipulator, can we compute the orientation of a selected link of the manipulator relative to a xed world coordinate frame or a frame attached to another point on the robot?




This lab will explore this question in two parts, which need not be done in order. In Part 1, you’ll use the code you wrote as part of the prelab to write the forward kinematics map for one of Baxter’s arms, then you’ll compare your results against some of ROS’s built-in tools. You’ll also learn a bit more about tf2, a useful package for computing transforms. In Part 2, you’ll explore Baxter/Sawyer’s basic joint position control functions, and take a quick look at how ROS helps you manage the coordinate transformations associated with all of Baxter/Sawyer’s moving parts.




Forward kinematics



As discussed in lecture, the forward kinematics problem involves nding the con guration of a speci ed link in a robotic manipulator relative to some other reference frame, given the angles of each of the joints in the manipulator. In this exercise, you’ll write your own code to compute the forward kinematics map for one of the Baxter robot’s arms.




1.1 Kinematic functions




Work on the relevant parts of Section 4 in the block2_supplement.pdf le. You may leverage code that you have already written, provided both partners can explain the code.




1.2 Writing the forward kinematics map




Writing the forward kinematics map for a serial chain manipulator involves the following steps:




De ne a reference \zero" con guration for the manipulator at which we’ll say = 0, where = [ 1; : : : ; n] is the vector of joint angles for an n-degree-of-freedom manipulator



Choose where on the robot to attach the xed base frame and the moving tool frame



Write the coordinate transformation from the base to the tool frame when the manipulator is in the zero con guration (gst(0))



Find the axis of rotation (!i) for each joint as well as a single point qi on each axis of rotation (all in the base frame)



Write the twist i for each joint in the manipulator



Write the product of exponentials map for the complete manipulator



Multiply the map by the original base-to-tool coordinate transformation to get the new transformation between the base and tool frames (gst( ), now as a function of the joint angles)



Task 1: Using the code from the Block 2 supplement and referring to the textbook (available on bCourses) if necessary, write a Python function that computes the coordinate transformation between the base and tool frames for the Baxter arm pictured below (steps 3-7 above). Your function should take an array of 7 joint angles as its only argument and return the 4x4 homogeneous transformation matrix gst( ). Refer to Figure 1 for the parameters of the Baxter arm. The only other parameter you should need is the rotation matrix




R =
2
0:0001
0:7102


0:7040
3


4
0:0076
0:7040
0:7102
5
where
1:0000
0:0053
0:0055


gst(0) =


0
1














R
q















2
for the appropriate value of q.




Note: Copying the information into Python from the diagram below can take a while, so we have done it for you




in lab3_skeleton.py.

























z’
left_s1
left_e0
left_w0


left_w2


y’
q2=[.1106,.3116,.3885]












q3=[.1827,.3838,.3881]
q =[.4417,.6420,.3177]


q7=[.7152,.9158,.3063]
T
ω2=[-.7077,.7065,-.0122]


5


ω =[.7065,.7077,-.0038]
x’
ω =[.7065,.7077,-.0038]
ω =[.7065,.7077,-.0038]


3
7










5














left_e1
left_w1


left_hand






























q=[.7957,.9965,.3058]




q4=[.3682,.5684,.3181]
q6=[.6332,.8337,.3067]










left_s0


ω4=[-.7077,.7065,-.0122]
ω6=[-.7077,.7065,-.0122]






q1=[.0635,.2598,.1188]












ω1=[-.0059,.0113,.9999]






























base

q=[0,0,0]




z




y




S

x










Figure 1: Baxter arm parameters.







1.3 Compare with built-in ROS functionality




Once you think you have your forward kinematics map nished, you’ll compare with with some built-in functions o ered by ROS.




To do this, we’ll use a new tool called rosbag, which allows you to record and play back all the messages published on a set of topics, in order to test pieces of your software. I recorded a set of data from Baxter while I moved its left arm around. Find the baxter.bag le (from lab3_resources.zip), start roscore, and play the le with







rosbag play baxter.bag







Notice how you can pause playback with the space bar and view the published messages with the usual tools like rostopic list and rostopic echo.




Try rostopic echo-ing the robot/joint_states topic, which gives the current joint angles of all joints in Baxter’s left and right arms, as well as those of the head and torso. Using knowledge from rostopic echo, you can gure out what joint angles correspond to Baxter’s left arm. (Hint: names starting with ’left ’ correspond to the left arm.)







Next, try running the command







rosrun tf tf_echo base left_hand







while the bag le is playing. Any ideas about the data that’s displayed?




Task 2: Write a subscriber node forward_kinematics.py that receives the messages from the robot/joint_states topic, plugs the appropriate joint angles from each message into your forward kinematics map from the last task, and displays the resulting transformation matrix on the terminal. Display this in another window alongside the tf data discussed above. Do you notice any similarities? What do you think the \RPY" portion of the tf message is?









1.4 Writing a tf Listener




tf is more than just a command line utility. It’s a powerful set of libraries that you can use to nd transforms between di erent frames on your robot. You’ll be writing a listener node using tf2, which is the newer, supported version of tf. Here are some commands you will nd useful:







import tf2_ros







The tf2 package is ROS independent, so you need to import tf2_ros, which contain ROS bindings of the various tf2 functionalities.







tfBuffer = tf2_ros.Buffer()







A Buffer is the core of tf2 and stores a bu er of previous transforms.







tfListener = tf2_ros.TransformListener(tfBuffer)







A TransformListener subscribes to the tf topic and maintains the tf graph inside the bu er.







trans = tfBuffer.lookup_transform(target_frame, source_frame, rospy.Time())







Looks up the transform of the target frame in the source frame. The output is of type geometry_msgs/TransformStamped.




Here are some exceptions you might want to catch:







tf2_ros.LookupException




tf2_ros.ConnectivityException




tf2_ros.ExtrapolationException







Task 3: Write a tf listener node tf_echo.py that duplicates the functionality of the tf_echo command line utility. Display this in another window alongside the tf data discussed above and ensure that the output is the same. Note: you do not have to format your output the same way, but the position and orientation should be the same.

























Checkpoint 1




Get a TA to check your work. At this point you should be able to:




Explain how you constructed your forward kinematics function




Explain the functionality of your forward_kinematics node and demonstrate how it works Demonstrate that your forward_kinematics node and tf produce the same output




Demonstrate that your tf_echo node and tf produce the same output














Make Baxter/Sawyer move



In this section, you’ll explore some of Baxter/Sawyer’s basic position control functionality. Close all running ROS nodes and terminals from the previous part, including the one running roscore, before you begin. Additionally, ensure that you have been trained by the course instructors in the proper safety procedures (including use of the e-stop button) and etiquette for running Baxter/Sawyer.




To create your workspace, make a folder called lab3_baxter with a subfolder src. From within the new src, run catkin_init_workspace, then from within lab3_baxter, run catkin_make.




To set up your environment, make a shortcut (symbolic link) to the Baxter environment script /scratch/shared/baxter_ws/baxter.sh using the command







ln -s /scratch/shared/baxter_ws/baxter.sh ~/ros_workspaces/lab3_baxter/







Run ./baxter.sh [name-of-robot].local (where [name-of-robot] is either asimov, ayrton, archytas, ada, or alan) in your folder to set up your environment for interacting with Baxter/Sawyer, then run source devel/setup.bash so your new workspace is on the $ROS_PACKAGE_PATH.




Baxter and Sawyer have di erent interface packages (baxter_interface and intera_interface, respectively), but they are virtually identical. The main di erence is the obvious one: Sawyer only has one arm! This means that whenever you try to move an arm on Sawyer, it must be the right one. On Baxter, you may use either arm.




Open the baxter_examples package inside the baxter_ws workspace and examine the scripts/joint_position_ keyboard.py le, which allows you to move the Baxter’s limbs using the keyboard. If you are using a Baxter, run the program and test its commands. If you are on a Sawyer, run the corresponding example in the intera_examples package. Note that you don’t need to start roscore | it’s already running on the Baxter/Sawyer robot itself.




Instead of publishing directly to a topic to control Baxter/Sawyer’s arms (as with turtlesim), the respective SDKs provide a library of functions that take care of the publishing and subscribing for you.




Task 4: Create and open a new package called joint_ctrl in your lab3_baxter workspace. What depen-dencies will be needed? (Hint: Include baxter_examples/intera_examples as a dependency.) Make a copy of the joint_position_keyboard.py le (from the appropriate package, depending on which type of robot you are using) inside the joint_ctrl package, giving it a new name. Edit your copy so that instead of captur-ing keypresses, it prompts the user for a list of seven joint angles, then moves to the speci ed position. (Hint: You might have to call limb.set_joint_positions() repeatedly at some interval, say, 10ms, while the robot is in the process of moving to the new position.) The set_joint_positions() function takes a single argument, which should be a Python dictionary object mapping the names of each joint to the desired joint angles (e.g., {‘left_s0’: 0.0, ‘left_s1’: 0.53, ..., ‘left_w2’:1.20}). Dictionaries are used as follows:







Create an empty dictionary test_dict = {}



Add values to the dictionary test_dict[‘key1’] = ‘value1’ test_dict[‘a_number’] = 1.024



Read values from the dictionary print(test_dict[‘key1’]) print(test_dict[‘a_number’])



Output:



value1



1.024



You can also create a dictionary with a literal expression test_dict2 = {‘key1’: ‘value1’, ‘a_number’: 1.024}






Test your code with several di erent combinations of joint angles and observe the results. Once you get your code to work, run the command







rosrun tf tf_echo base left_hand









5
or







rosrun tf tf_echo base right_hand







as appropriate and observe the output as you move the robot around. Any ideas what the data represents? Finally, run







export ROS_MASTER_URI=http://[name-of-robot].local:11311 rosrun rviz rviz







for the appropriate value of [name-of-robot], as before. The rst line above tells RViz to connect to the remote master running on the robot.




Once RViz loads, ensure that Displays Global Options Fixed Frame is set to world. Next, click the Add button and add a RobotModel object to the window so you can see the robot move. Any thoughts as to where RViz gets the data on the robot’s position?




Next, add two copies of the Axes object to the display. In the Displays pane of the left side of the screen, set the Reference Frame of one Axes object to /base and the other to /right_hand. You should see both sets of axes displayed on Baxter. What do you think the axes represent?




Finally, remove both Axes objects and add a single TF object to the display. What happens?













Checkpoint 2




Get a TA to check your work at the end of lab. At this point you should be able to:




Demonstrate the code you wrote to set Baxter/Sawyer’s joint positions




Use RViz to display the di erent state and sensor data topics published by Baxter/Sawyer Explain what the Axes and TF displays in RViz represent










































































































6