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
:
mechanism
: dictionary with 4 keys “theta offset”
, “d”
, “a”
, “alpha”
.
joints
: list of 6 float numbers that defines the configuration of the mechanism.
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.
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.
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
:
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
niter
: integer that defines number of iterations
tol
: positive float tolerance
pose
with tolerance tol
or the algorithm failed to converge in niter
steps.
Upload a zip archive hw07.zip
containing
hw07.py
- python script containing the implemented functions jac_r
, jac_t
, jac
, solve