Warning

## Homework 07 - Inverse Kinematics by Newton's Method

Allowed libraries: numpy

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, niter, 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. niter : integer that defines number of iterations
5. tol : positive float 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 niter steps.

Upload a zip archive hw07.zip containing
1. hw07.py - python script containing the implemented functions jac_r, jac_t, jac, solve