~~NOTOC~~

===== Homework 08 - Inverse Kinematics by Newton's Method =====

The goal of this task is to solve [[https://cw.fel.cvut.cz/b221/_media/courses/pkr/labs/hw08.pdf|the inverse kinematic task for a general 6R mechanism using Newton's method]].

=== Task ===

<note warning>
**Allowed libraries**: numpy
</note>

**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'':
  - ''mechanism'': dictionary with 4 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"alpha"''.
  - ''joints'' : list of 6 float numbers that defines the configuration of the mechanism.
  - **Return value**: 9x6 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'':
  - ''mechanism'': dictionary with 4 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"alpha"''.
  - ''joints'' : list of 6 float numbers that defines the configuration of the mechanism.
  - **Return value**: 3x6 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'':
  - ''mechanism'': dictionary with 4 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"alpha"''.
  - ''joints'' : list of 6 float numbers that defines the configuration of the mechanism.
  - **Return value**: 12x6 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'':
  - ''mechanism'': dictionary with 4 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"alpha"''
  - ''pose'': dictionary with 2 keys ''"q"'' and ''"t"'' that defines the desired pose of the mechanism's end effector
  - ''joints'' : list of 6 float numbers that defines the initial configuration of the mechanism
  - ''n_iter'' : number of iterations
  - ''tol'' : tolerance
  - **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 ===

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