Implement Forward Kinematics and Coordinate Transforms

In this part of the workshop you will implement a python program that computes the forward kinematics of a robot and visualises the computed position of the end effector in the 3D space.

1. Write the elementary homogeneous transformations matrices

• Open the file week4/scripts/transformations.py and fill in all the missing symbolic transformations (find the #TODOs in the code). Do not forget to execute rebuild after every change to code.

Note that for defining these transformations, we are using the library sympy which allows defining symbolic expressions and can perform symbolic computations, also on matrices.

• Once you have finished filling all the missing parts, simply executing the script will test that the results obtained by performing various transformations are the ones we expect: python3 src/week4/scripts/transformations.py. Check that all the computed vectors match with the expected values and, if not, fix any error.

2. Implement the forward kinematics of a 6 DOF robot

• Open the file week4/scripts/forward_kinematics.py and implement the function symbolic_FK() which combines the elementary operations defined in step 1 to model the forward kinematics of the CR7iA 6DOF robot. The transformation should represent the homogeneuos transformation from the base_link to the tool_link: ${}^{base}T_{tool}$. The definition of links and joints can be derived from the URDF created in week2: src/week2/description/urdf/6dof.urdf.xacro.

Note that the forward_kinematics.py script also contains another function that converts a symbolic matrix into a numeric one by substituting all the joint angle values and axes translations.

• Once you have finished implementing the symbolic transformation, execute the script and look at the output printed. Does it give the expected values? Try changing the values for the q angles, does the tool_link translation w.r.t the base_link correspond to your expectation?

3. Compare your transformations with the ones from tf2

Now, you are going to write a ROS node that computes in real-time the forward kinematics for the robot based on the configuration of the angles at any given time.

• Open the script forward_kinematics_node.py and fill in all the missing parts to complete the code (look for the #TODOs). You will need to subscribe to the /joint_states topic to get the configuration of the joints and plug the numbers in the symbolic matrix. Finally, you will need to evaluate the numeric transformation matrix and find the centre of the tool link position w.r.t. the base.

• Once you have finished, launch the visualisation window for the robot: ros2 launch week2 view_robot.launch.py description_file:=6dof.urdf.xacro, and open the VNC window.

• Open a new terminal and compute the transformation between the base and the tool links with the tf2 functionality: ros2 run tf2_ros tf2_echo base_link tool_link. Compare the values with the ones generated your code; are they the same?

• Click the "Randomize" button in the Joint State Publisher window and compare the results from your code to the one computed by tf2.