Implement direct kinematics for a 6DOF arm

Arm with spherical wrist

▶️ 🖥️ 1. In the editor, open the file RBT1001/src/week4/direct_kinematics.py. This is an uncomplete code for a ROS Node that computes the forward kinematics for the arm.

▶️ 🧑‍💻 2. Based on the series of homogeneous transformations that you have found in the previous task, complete the code. Find all the places where there is a ### TODO and add your code in there.

• ❗ We will use the Numpy (numerical python) library for computing the following values:
  • cos: np.cos(...)
  • sin: np.sin(...)
  • pi: np.pi(...)
• ❗ Note that the matrix multiplication (dot product) between numpy matrices can be obtained using the @ operator, e.g.

   self.HR('z', np.pi/2) @ self.HR('y', np.pi/2)

Once you have finished it's time to test it!

▶️ 🖥️ 3. Launch the visualiser for the same robot with ros2 launch week3 view_robot.launch.py description_file:=mecharm_270_m5.urdf.xacro

❗ You can reduce the dimension of the reference frames by setting the "Marker Scale" to 0.5 in the left bar options under TF.

▶️ 🖥️ 4. In a different terminal, launch your script: python3 src/week4/direct_kinematics.py

▶️ 🖥️ 5. In an adjacent terminal, launch the ROS TF echo functionality which automatically computes the transformation: ros2 run tf2_ros tf2_echo base link6. ❗ If your transformation is correct, you should see the same matrix - randomize the joints positions to check if the transformation is always accurate.