Implement Inverse Kinematics - LCAS/RBT1001 GitHub Wiki

Based on the robot definition: image

We can derive the following direct kinematics transformations:

$T_{0123} =$ image

$R_{456} =$ image

:arrow_right: 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)

:arrow_forward: :technologist: 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!).