The goal of this task is to solve the inverse kinematic task for a general 6R mechanism using Newton's method.

a. Implement function jac_r(mechanism, joints) that returns the rotational Jacobian for the given mechanism and its configuration joints.

I/O specifications for jac_r:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”.
2. joints : list of 6 float numbers that defines the configuration of the mechanism.
3. Return value: 9×6 matrix of type np.ndarray that defines the rotational Jacobian matrix evaluated at the given configuration, i.e.

$$\frac{\partial \mathrm{vec}(\mathbf{R})}{\partial \boldsymbol{\theta}}_{\big|\boldsymbol{\theta}_0}, \quad \text{where} \; \mathrm{vec}(\mathbf{R}) = \begin{bmatrix} r_{11} & r_{21} & r_{31} & r_{12} & r_{22} & r_{32} & r_{13} & r_{23} & r_{33} \end{bmatrix}^\top$$

b. Implement function jac_t(mechanism, joints) that returns the positional Jacobian for the given mechanism and its configuration joints.

I/O specifications for jac_t:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”.
2. joints : list of 6 float numbers that defines the configuration of the mechanism.
3. Return value: 3×6 matrix of type np.ndarray that defines the positional Jacobian matrix evaluated at the given configuration, i.e.

$$\frac{\partial \mathbf{t}}{\partial \boldsymbol{\theta}}_{\big|\boldsymbol{\theta}_0}$$

c. Implement function jac(mechanism, joints) that returns the full Jacobian for the given mechanism and its configuration joints.

I/O specifications for jac:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”.
2. joints : list of 6 float numbers that defines the configuration of the mechanism.
3. Return value: 12×6 matrix of type np.ndarray that defines the full Jacobian matrix evaluated at the given configuration, i.e.

$$ \begin{bmatrix} \frac{\partial \mathrm{vec}(\mathbf{R})}{\partial \boldsymbol{\theta}} \\ \frac{\partial \mathbf{t}}{\partial \boldsymbol{\theta}} \end{bmatrix}_{\big|\boldsymbol{\theta}_0} $$

d. Implement function solve(mechanism, pose, joints, n_iter, tol) that applies Newton's method to solve IKT.

I/O specifications for solve:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”
2. pose: dictionary with 2 keys “q” and “t” that defines the desired pose of the mechanism's end effector
3. joints : list of 6 float numbers that defines the initial configuration of the mechanism
4. n_iter : number of iterations
5. tol : tolerance
6. Return value: tuple consisting of the state of convergence and a list of 6 float numbers that defines the configuration of the mechanism that generates the given pose with tolerance tol or the algorithm failed to converge in n_iter steps.

Upload a zip archive hw08.zip containing

1. hw08.py - python script containing the implemented functions jac_r, jac_t, jac, solve