Implement Inverse Kinematics Part 2

We continue with (the same) task 2 from last week

1. ➡️ 🧑‍💻 Launch the rviz with ros2 launch mecharm_moveit_config launch_ik.launch.py to visualise our robot and the marker to set the target pose of the end-effector (link6).
2. ℹ️ ➡️ 🧑‍💻 This week, I provide a new template for implementing the inverse kinematics problem which contains a few hints and parts of the solutions. Open and complete the script src/week6/inverse_kinematics_analytic_hints.py.

Based on the robot definition:

We can derive the following direct kinematics transformations:

$T_{0123} =$

$R_{456} =$

➡️ Remember that we have derived the forward kinematics for the robot in the shown configuration in week4 as follows:

 T = self.HT('z', 0.136) @ self.HR('z', q1) @ self.HR('x', -np.pi/2) @ self.HR('z', -np.pi/2) @ \
            self.HR('z', q2) @ self.HT('x', 0.1) @ self.HR('z', q3) @ self.HT('y', 0.107) @ \
            self.HR('x', -np.pi/2) @ self.HR('z', q4) @ self.HR('x', np.pi/2) @ self.HR('z', q5) @ \
            self.HR('z', np.pi/2) @ self.HR('y', np.pi/2) @ self.HR('z', q6) @ \
            self.HT('z', 0.065)

▶️ 🧑‍💻 Use the above information to find the angles of the robot joints using the approaches seen in the previous weeks (lectures and workshop materials are going to be useful!).