 |
amino
1.0-beta2
Lightweight Robot Utility Library
|
|
amino
1.0-beta2
Lightweight Robot Utility Library
|
\[ \newcommand{\normtwo}[1]{\left| #1 \right|} \newcommand{\MAT}[1]{\boldsymbol{#1}} \newcommand{\VEC}[1]{\boldsymbol{#1}} \newcommand{\unitvec}[1]{\boldsymbol{\hat{#1}}} \newcommand{\ielt}[0]{\unitvec{\imath}} \newcommand{\jelt}[0]{\unitvec{\jmath}} \newcommand{\kelt}[0]{\unitvec{k}} \newcommand{\quat}[1]{\mathcal{#1}} \newcommand{\qmul}[0]{\otimes} \newcommand{\dotprod}{\boldsymbol{\cdot}} \newcommand{\tf}[3]{{^{#2}{#1}_{#3}}} \newcommand{\tfmat}[3]{{^{#2}\MAT{#1}_{#3}}} \newcommand{\tfquat}[3]{{^{#2}\quat{#1}_{#3}}} \newcommand{\qmul}[0]{\otimes} \newcommand{\dualelt}[0]{\boldsymbol{\varepsilon}} \newcommand{\confsym}[0]{\theta} \newcommand{\conf}[0]{\confsym} \newcommand{\confvec}[0]{\boldsymbol{\confsym}} \newcommand{\sequat}[0]{\quat{h}} \newcommand{\sequatdual}[0]{\quat{d}} \newcommand{\seduqu}[0]{\quat{S}} \newcommand{\serotvel}[0]{\omega} \newcommand{\setranssym}[0]{v} \newcommand{\setrans}[0]{\vec{\setranssym}} \newcommand{\setransvel}[0]{\dot{\setrans}} \newcommand{\setwist}[0]{{\Omega}} \newcommand{\jacobian}[0]{\boldsymbol{J}} \newcommand{\realset}[0]{\mathbb{R}} \newcommand{\overcmt}[2]{\overbrace{#1}^{#2}} \newcommand{\undercmt}[2]{\underbrace{#1}_{#2}} \newcommand{\setbuilder}[2]{\left\{\left.{#1}\ \right|\ {#2} \right\}} \]
This tutorial introduces forward, differential, and inverse robot kinematics.
First, we will extend our 4 degree-of-freedom (DoF) robot to a 7 DoF model so the robot can reach any orientation and translation.
We use the following scene file:
Compile the scene using the following build script:
Forward kinematics finds the absolute (root-coordinates) pose \(\tf{\quat{S}}{0}{c}\) of a target frame in the robot (scene) for a given configuration (joint positions) \(\confvec\). Each edge in the scene graph represents the relative transform between parent and child, which could be constant for fixed frames or varying based on configuration for joints. Generally, the absolute pose of frame c is defined recursively from the relative pose as:
\[ \tf{\quat{S}}{0}{c}(\confvec) = \begin{cases} \tf{\quat{S}}{p}{c}(\confvec), & \text{if } p = 0 \\ \tf{\quat{S}}{0}{p}(\confvec) \qmul \tf{\quat{S}}{p}{c}(\confvec), & \text{if } p \neq 0 \end{cases} \; , \]
where p is the parent of c and 0 is the root frame.
For example, consider the simple scene with a grid and three blocks. 
We compute the absolute poses of the blocks as follows:
\begin{align*} \tf{\seduqu}{0}{A} & = \tf{\seduqu}{0}{\rm grid} \qmul \tf{\seduqu}{\rm grid}{A} \\ \tf{\seduqu}{0}{B} & = \tf{\seduqu}{0}{\rm grid} \qmul \tf{\seduqu}{\rm grid}{A} \qmul \tf{\seduqu}{A}{B} \\ \tf{\seduqu}{0}{C} & = \tf{\seduqu}{0}{\rm grid} \qmul \tf{\seduqu}{\rm grid}{C} \end{align*}
Often, we want the absolute transform of every frame in the then scene, e.g., for visualization or collision checking. We efficiently compute every absolute transform by ordering the frames such that a parent frame always precedes its children. Then, a single scan through the ordered frames lets us compute every absolute pose since the absolute transform of the parent will always be available when we reach the child.
If we need the relative transforms between two frames, we compute this from the absolute transforms of each frame:
\[ \tf{\quat{S}}{a}{b} = \left(\tf{\quat{S}}{0}{a}\right)^* \qmul \tf{\quat{S}}{0}{b} \]
The following example code computes the forward kinematics for the 7-DoF robot:
Differential kinematics relates change (e.g., velocities) in configuration (joint positions) and pose. We will construct and solve a differential equation for this relationship.
Due to the special nature of rotations, there are multiple ways we may represent change of the target pose. We will describe pose changes represented as velocities, transform derivatives, and twists.
Though three-parameter representations of rotational position suffer from singularities, this is not a problem for rotational velocity. Thus, we may represent change in pose with the 6-vector consisting of rotational velocity \(\serotvel\) about the principal axes and translational velocity \(\dot{\vec{v}}\) along these axes:
\[ \begin{bmatrix} \serotvel \\ \dot{\vec{v}} \end{bmatrix} \in \realset^6 \; . \]
Another natural representation for pose change is the derivative of a transformation. For rotation quaternions, the derivative and velocity are related as follows :
\[ \dot{\sequat} = \frac{1}{2}\serotvel \qmul \sequat \quad\text{ and }\quad \serotvel = 2 \dot{\sequat} \qmul \sequat^* \; . \]
For dual quaternions representing transformation (rotation and translation) we differentiate as follows (using the sum and product derivative rules):
\begin{align*} & \seduqu = \sequat + \frac{1}{2} \setrans \qmul \sequat\dualelt \\ \text{derivative:}\qquad & \dot{\seduqu} = \frac{d}{dt} \left( \sequat + \frac{1}{2} \setrans \qmul \sequat\dualelt \right) \\ \text{sum rule:}\qquad & \phantom{\dot{\seduqu}} = \dot\sequat + \frac{1}{2} \left( \setransvel \qmul \sequat + \setrans \qmul \dot\sequat \right)\dualelt \\ \text{product rule:}\qquad & \phantom{\dot{\seduqu}} = \frac{1}{2}\left(\serotvel \qmul \sequat + \frac{1}{2} \left( \setransvel \qmul \sequat + \frac{1}{2}\setrans \qmul \serotvel \qmul \sequat \right)\dualelt \right) \; . \end{align*}
With a bit of algebra, we rearrange the dual quaternion derivative into a form that is structurally similar to the ordinary quaternion derivative:
\[ \dot{\seduqu} = \frac{1}{2}\setwist \qmul \seduqu\; , \quad \text{where} \quad \setwist = \serotvel + \left(\setransvel + \setrans \times \serotvel \right)\dualelt \; . \]
The value \(\setwist\) is the twist represented as a pure dual quaternion, i.e., zero scalar (w) in the real and dual parts.
The twist \(\setwist\) describes the coupling of rotation and translation: when a rigid body rotates about some point, all other points on that body will experience translation.
We write the twist as the following pure dual quaternion:
\[ \setwist = 2 \dot{\seduqu} \seduqu^* = \serotvel + \left(\setransvel + \setrans \times \serotvel \right)\dualelt \; . \]
Since the twist has only six non-zero elements, we may also write it as the following vector:
\[ \setwist = \begin{bmatrix} \serotvel \\ \setransvel + \setrans \times \serotvel \end{bmatrix} \in \realset^6 \; . \]
Computationally, twists offer useful properties for differential kinematics. Matrix operations are cheaper for both velocities and twists than for the dual quaternion derivative because the velocity and twist have six non-zero elements, while the dual quaternion derivative has eight. The specific advantages of the twist arise when computing and applying the manipulator Jacobian, where the twists inherent rotation-translation coupling simplifies Jacobian computation.
The manipulator Jacobian defines the differential equation relating change in pose and configuration.
Generally, the Jacobian of a function is a matrix of first-order partial derivatives. For function \(\VEC{f} : \realset^n \mapsto \realset^m\), the Jacobian is:
\[ \VEC{y} = \VEC{f}({\VEC{x}}) \\ \jacobian = \frac{\partial \VEC{f}}{\partial \VEC{x}} = \begin{bmatrix} \frac{\partial \VEC{f}}{\partial x_1} & \ldots & \frac{\partial \VEC{f}}{\partial x_n} \end{bmatrix} = \begin{bmatrix} \frac{\partial {f_1}}{\partial x_1} & \ldots & \frac{\partial {f_1}}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial {f_m}}{\partial x_1} & \ldots & \frac{\partial {f_m}}{\partial x_n} \end{bmatrix} \]
We use the Jacobian to relate change in the domain and range of a function via the chain rule:
\begin{align*} & \VEC{y} = \VEC{f}(\VEC{x}) \\ \text{derivative: } \quad & \frac{d \VEC{y}}{dt} = \frac{d}{dt}\left(\VEC{f}(\VEC{x})\right) \\ \text{chain rule: } \quad & \frac{d \VEC{y}}{dt} = \frac{\partial \VEC{f}}{\partial \VEC{x}} \frac{d \VEC{x}}{dt} \\ \text{Jacobian: } \quad & \dot{\VEC{y}} = \jacobian \dot{\VEC{x}} \end{align*}
Thus, the Jacobian matrix provides a mapping between \(\dot{\VEC{x}}\) and \(\dot{\VEC{y}}\) as a system of linear equations.
There are multiple formulations of the manipulator Jacobian due to the multiple ways we may represent pose change. In each formulation, the columns of the manipulator Jacobian correspond to the robot's joints and the rows correspond to the specific representation of pose change.
\[ \text{pose change} = \overcmt{ \begin{bmatrix} \begin{pmatrix} \VEC{j}_1 \end{pmatrix} & \ldots & \begin{pmatrix} \VEC{j}_n \end{pmatrix} \end{bmatrix} }{\MAT{J}} \overcmt{ \begin{bmatrix} \dot{\conf}_1 \\ \vdots \\ \strut\dot{\conf}_n \end{bmatrix} }{\dot\confvec} \]
We will discuss the Jacobians for the velocity, transform derivative, and twist.
The velocity Jacobian relates configuration and Cartesian velocities:
\[ \begin{bmatrix} \serotvel \\ \setransvel \end{bmatrix} = \jacobian_x \dot{\confvec} \]
Each column of the velocity Jacobian thus contains a rotational velocity part \(\VEC{j}_r\) and a translational velocity part \(\VEC{j}_p\).
\[ \begin{bmatrix} \omega \\ \dot v \end{bmatrix} = \overcmt{ \begin{bmatrix} \begin{pmatrix} \VEC{j}_{r_1}\\ \VEC{j}_{v_1} \end{pmatrix} & \begin{matrix} \ldots \\ \ldots \end{matrix} & \begin{pmatrix} \VEC{j}_{r_n}\\ \VEC{j}_{v_n} \end{pmatrix} \end{bmatrix} }{\MAT{J}} \overcmt{ \begin{bmatrix} \dot{\conf}_1 \\ \vdots \\ \strut\dot{\conf}_n \end{bmatrix} }{\dot\confvec} \]
The velocity Jacobian offers a direct geometric interpretation: we may consider the contribution of each joint to the end-effector velocity to construct the Jacobian column for that joint.
Prismatic Joint Columns: A prismatic joints creates linear motion of the end-effector along the prismatic joint axis and no rotational motion. The Jacobian column for a prismatic joint c is:
\[ \begin{pmatrix} \VEC{j}_{r_c}\\ \VEC{j}_{v_c} \end{pmatrix} = \begin{pmatrix} 0 \\ \tf{\unitvec{u}}{G}{c} \end{pmatrix} \]
where \(\tf{\unitvec{u}}{G}{c}\) is the joint axis in global coordinates.
Revolute Joint Columns: A revolute joint creates both rotational and translational motion. The rotational motion is about the joint axis. The translational motion is along the tangent of the circle centered at the joint axis and with a radius drawn to the end-effector; we may find this tangent motion using the cross product of the joint axis and the circle radius (from joint origin to end-effector). The Jacobian column for a revolute joint c is:
\[ \begin{pmatrix} \VEC{j}_{r_c}\\ \VEC{j}_{p_c} \end{pmatrix} = \begin{pmatrix} \tf{\unitvec{u}}{G}{c}\\ \tf{\unitvec{u}}{G}{c} \times \left( \tf{\vec{v}}{G}{e} - \tf{\vec{v}}{G}{c} \right) \end{pmatrix} \]
where \(\tf{\unitvec{u}}{G}{c}\) is the joint axis, \(\tf{\setrans}{G}{c}\) is the joint origin, and \(\tf{\setrans}{G}{e}\) is the end-effector translation, all in global coordinates.
With a bit of matrix algebra, we derive the twist Jacobian in terms of the velocity Jacobian. First, we will rewrite the twist as a matrix-vector product with the velocity vector. We rewrite the cross product in matrix form and factor:
\begin{align*} & \setwist = \begin{bmatrix} \serotvel \\ \setransvel + \setrans \times \serotvel \end{bmatrix} \\ \text{cross product matrix:}\quad & \phantom{\setwist} = \begin{bmatrix} \serotvel \\ \left[\setranssym\right] \serotvel + \setransvel \end{bmatrix} \;,\quad\text{where } [\setranssym] = \begin{bmatrix} 0 & -\setranssym_z & \setranssym_y \\ \setranssym_z & 0 & -\setranssym_x \\ -\setranssym_y & \setranssym_x & 0 \end{bmatrix} \\ \text{factor:}\quad & \phantom{\setwist} = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym]& \MAT{I} \end{bmatrix} \begin{bmatrix} \serotvel \\ \setransvel \end{bmatrix} \end{align*}
Next, we substitute the velocity Jacobian and configuration velocity for the Cartesian velocity vector:
\begin{align*} \setwist & = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \begin{bmatrix}\serotvel \\ \setransvel\end{bmatrix} \\ & = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \left( \jacobian_x\ \dot{\confvec} \right) \\ & = \undercmt{ \left( \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \jacobian_x\right) }{\text{twist Jacobian }\jacobian_\setwist} \dot{\confvec} \end{align*}
Thus, we may construct each column of the twist Jacobian as a matrix-vector product from the velocity Jacobian:
\begin{align*} \jacobian_\setwist &= \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \jacobian_x \\ \begin{bmatrix} \begin{pmatrix} \VEC{j}_{r_1}\\ \VEC{j}_{p_1} \end{pmatrix} & \begin{matrix} \ldots \\ \ldots \end{matrix} & \begin{pmatrix} \VEC{j}_{r_n}\\ \VEC{j}_{p_n} \end{pmatrix} \end{bmatrix} & = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \begin{bmatrix} \begin{pmatrix} \VEC{j}_{r_1}\\ \VEC{j}_{v_1} \end{pmatrix} & \begin{matrix} \ldots \\ \ldots \end{matrix} & \begin{pmatrix} \VEC{j}_{r_n}\\ \VEC{j}_{v_n} \end{pmatrix} \end{bmatrix} \\ \begin{pmatrix} \VEC{j}_{r_i}\\ \VEC{j}_{p_i} \end{pmatrix} & = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \begin{pmatrix} \VEC{j}_{r_i}\\ \VEC{j}_{v_i} \end{pmatrix} \end{align*}
We see that the rotational parts \(\VEC{j}_{r_i}\) of the twist and velocity Jacobians are identical. However, there may be differences in the translational parts \(\VEC{j}_{p_i}\) and \(\VEC{j}_{v_i}\).
Prismatic Joint Columns: Since prismatic joints do not create any rotational velocity, the additional \(\setrans \times \serotvel\) term of the twist is zero. Thus, the twist Jacobian column for prismatic joints is the same as the velocity Jacobian column:
\begin{align*} \begin{pmatrix} \VEC{j}_{r_c}\\ \VEC{j}_{p_c} \end{pmatrix} & = \begin{bmatrix} \MAT{I} & 0 \\ [\setranssym] & \MAT{I} \end{bmatrix} \begin{pmatrix} 0 \\ \tf{\unitvec{u}}{G}{c} \end{pmatrix} \\ & = \begin{pmatrix} 0 \\ \tf{\unitvec{u}}{G}{c} \end{pmatrix} \end{align*}
Revolute Joint Columns: In the case of revolute joints, the inclusion of end-effector translation in the twist cancels the end-effector translation in the velocity Jacobian column:
\begin{align*} \begin{pmatrix} \VEC{j}_{r_c}\\ \VEC{j}_{p_c} \end{pmatrix} & = \begin{bmatrix} \MAT{I} & 0 \\ [\tf{\setranssym}{G}{e}] & \MAT{I} \end{bmatrix} \begin{pmatrix} \tf{\unitvec{u}}{G}{c}\\ \tf{\unitvec{u}}{G}{c} \times \left( \tf{\vec{v}}{G}{e} - \tf{\vec{v}}{G}{c} \right) \end{pmatrix} \\ & = \begin{pmatrix} \tf{\unitvec{u}}{G}{c} \\ \tf{\vec{v}}{G}{e} \times \tf{\unitvec{u}}{G}{c} + \tf{\unitvec{u}}{G}{c} \times \left( \tf{\vec{v}}{G}{e} - \tf{\vec{v}}{G}{c} \right) \end{pmatrix} \\ & = \begin{pmatrix} \tf{\unitvec{u}}{G}{c} \\ \tf{\setrans}{G}{c} \times \tf{\unitvec{u}}{G}{c} \end{pmatrix} \end{align*}
Practically speaking, the twist Jacobian eliminates the need to include the end-effector position in the Jacobian. This can be computationally useful if we need to consider multiple "end-effectors," e.g., if we are considering velocities of multiple links in a chain instead of just the final link.
We will obtain the derivative Jacobian from the twist Jacobian. First we rewrite the derivative-twist relationship as a matrix multiplication.
Generally, we may rewrite ordinary quaternion multiplication as a matrix-vector product:
\begin{align*} \quat{q} \qmul \quat{p} & = \overcmt{ \begin{bmatrix} \quat{q}_w & - \quat{q}_z & \quat{q}_y & \quat{q}_x \\ \quat{q}_z & \quat{q}_w & - \quat{q}_x & \quat{q}_y \\ -\quat{q}_y & \quat{q}_x & \quat{q}_w & \quat{q}_z \\ -\quat{q}_x & - \quat{q}_y & - \quat{q}_z & \quat{q}_w \\ \end{bmatrix} }{[\quat{q}]_L} \begin{bmatrix} \quat{p}_x \\ \quat{p}_y \\ \quat{p}_z \\ \quat{p}_w \end{bmatrix} \nonumber = \left[\quat{q}\right]_L \VEC{p} \\ & = \undercmt{ \begin{bmatrix} {\quat{p}}_w & \quat{p}_z & -\quat{p}_y & \quat{p}_x \\ -{\quat{p}}_z & \quat{p}_w & \quat{p}_x & \quat{p}_y \\ {\quat{p}}_y & -\quat{p}_x & \quat{p}_w & \quat{p}_z \\ -{\quat{p}}_x & -\quat{p}_y & -\quat{p}_z & \quat{p}_w \end{bmatrix} }{[\quat{p}]_R} \begin{bmatrix} \quat{q}_x \\ \quat{q}_y \\ \quat{q}_z \\ \quat{q}_w \end{bmatrix} = \left[\quat{p}\right]_R \VEC{q} \; . \end{align*}
Similarly, we may view a dual quaternion as the 8-element vector,
\[ \seduqu = \sequat + \sequatdual\dualelt \quad\stackrel{\text{as vector}}{\leadsto}\quad \VEC{S} = \begin{bmatrix} \sequat_x & \sequat_y & \sequat_z & \sequat_w & \sequatdual_x & \sequatdual_y & \sequatdual_z & \sequatdual_w \end{bmatrix}^T \; , \]
and rewrite dual quaternion multiplication as a matrix-vector product:
\begin{align*} \quat{C} \qmul \quat{S} = \begin{bmatrix} \quat{C}_r \qmul \quat{S}_r \\ \quat{C}_d \qmul \quat{S}_r + \quat{C}_r \qmul \quat{S}_d \end{bmatrix} & = \overcmt{ \begin{bmatrix} \left([\quat{C}_r]_L\right) & \MAT{0}_{4\times 4} \\ \left([\quat{C}_d]_L\right) & \left([\quat{C}_r]_L\right) \end{bmatrix} }{[\quat{C}]_L} \VEC{S} = \left[\quat{C}\right]_L \VEC{S} \nonumber\\ & = \undercmt{ \begin{bmatrix} \left([\quat{S}_r]_R\right) & \MAT{0}_{4\times 4} \\ \left([\quat{S}_d]_R\right) & \left([\quat{S}_r]_R\right) \end{bmatrix} }{[\quat{S}]_R} \VEC{C} = \left[\quat{S}\right]_R \VEC{C} \; , \end{align*}
where blocks of the form \([\quat{S_r}]_R\) are the ordinary quaternion multiplication matrices constructed for the real ( \(r\)) or dual ( \(d\)) parts of \(\quat{S}\) and \(\quat{C}\).
Now, we rewrite the derivative-twist equation as a matrix-vector product:
\begin{align*} \dot{\seduqu} = \frac{1}{2}\setwist \qmul \seduqu = \frac{1}{2}\left[\seduqu\right]_R \setwist \;. \end{align*}
Then, we substitute the twist Jacobian and configuration velocity for the twist:
\begin{align*} \dot{\seduqu} & = \frac{1}{2}\left[\seduqu\right]_R \overcmt{ \left(\jacobian_\setwist \dot{\confvec}\right) }{\setwist} \\ & = \undercmt{\left( \frac{1}{2}\left[\seduqu\right]_R\jacobian_\setwist \right)}{\text{derivative Jacobian }\frac{\partial \seduqu}{\partial \confvec}} \dot{\confvec} \end{align*}
Once we have obtained the manipulator Jacobian, we use it to construct a system of linear equations relating configuration and Cartesian velocities. For example, in the case of twist, the equation is:
\[ \setwist = \jacobian_\setwist \dot{\confvec} \]
If we want to find the configuration velocity to achieve a desired end-effector velocity, we then solve this system of equations. One approach to find this solution is to use the pseudoinverse of the Jacobian:
\[ \setwist_{\rm ref} = \jacobian_\setwist \dot{\confvec}_{\rm cmd} \quad\leadsto\quad \left({\jacobian_\setwist}^+\right) \setwist_{\rm ref} = \dot{\confvec}_{\rm cmd} \]
Though there are more efficient methods for solving linear systems than explicitly computing the pseudoinverse \(\jacobian^+\), we are able to use the explicit \(\jacobian^+\) to further condition the robot's motion.
If we are attempting to track a Cartesian trajectory on a physical robot, we must correct any tracking error. One simple approach is to apply proportional control to the position error. First, we compute the position error based on the relative transform:
\[ \setwist_{\rm perr} = \ln \left( \seduqu_{\rm act} \qmul {\seduqu_{\rm ref}}^*\right) \; . \]
Then, we modify the above linear equation solution to apply a proportional gain on the position error:
\[ \dot{\confvec}_{\rm cmd} = \left({\jacobian_\setwist}^+\right) \setwist_{\rm ref} + \MAT{K}_p \setwist_{\rm perr} \; , \]
where \(\MAT{K}_p\) is the proportional gain matrix (typically, a diagonal matrix). This equation computes feed-forward control on a velocity reference and applies proportional control to position error.
For redundant manipulators (more than 7 DoF), there may be infinitely many solutions to the system of linear equations relating configuration and Cartesian velocities. Using only the pseudoinverse above, we will find the minimum-norm solution (smallest joint velocity magnitude). If we want a different solution, we can project a reference joint velocity into the nullspace of the manipulator Jacobian.
Generally, the nullspace of a matrix is the set of vectors whose product with the matrix is zero:
\[ \textrm{null}(\MAT{A}) = \setbuilder{\VEC{x}}{\MAT{A}\VEC{x} = \VEC{0}} \; . \]
In the case of the manipulator Jacobian, a vector in the nullspace will have no effect on Cartesian motion:
\[ \VEC{0} = \jacobian \dot{\confvec}_{\rm null} \; . \]
We project any configuration velocity into the nullspace using the following projection matrix:
\[ \dot{\confvec}_{\rm null} = \left(\MAT{I} - \jacobian^+\jacobian\right) \dot\confvec \]
A typical use-case of the nullspace projection is to move joints towards their center position, which helps to avoid reaching joint limits during robot motion,
\[ \dot{\confvec}_{\rm cmd} = \left(\jacobian^+\right) \setwist_{\rm ref} + \MAT{K}_\phi (\MAT{I} - \MAT{J}^+\MAT{J}) \left( {\confvec}_{\rm center} - {\confvec}_{\rm act} \right) \; , \]
where \(\MAT{K}_\phi\) is a proportional gain, typically a scalar or a diagonal matrix.
Singularities occur when the system of linear equations has no solution. For example, try stretching your arm straight out in front of you. Now, at what velocity must your elbow move to pull your hand straight back? There is no solution, since any elbow velocity will instantaneously move your hand to the side. When we try to solve the system of linear equations near the singularity, we end up with very large velocities than may produce "jerky" motion of the robot. However, we can apply singularity-robust methods to achieve acceptable performance near singularities.
Damped Least Squares: The damped least squares adds a small damping term to compute a damped psuedoinverse:
\[ \jacobian^\dagger = \left(\jacobian^T\jacobian + k \MAT{I}\right)^{-1}\jacobian^T = \jacobian^T\left(\jacobian\jacobian^{T} + k \MAT{I}\right)^{-1} \]
The damping term k introduces a small amount of error but ensures that we can always invert the matrix.
Singular Value Decomposition: We can also compute a singularity-robust pseudoinverse using the singular value decomposition.
Generally, the singular value decomposition (SVD) of an \(m \times n\) matrix \(\MAT{J}\) is the factorization:
\[ \MAT{J} = \MAT{U}\MAT{\Sigma}\MAT{V}^T = \begin{bmatrix} \VEC{u}_1 & \ldots \VEC{u}_m \end{bmatrix} \begin{bmatrix} \sigma_1 & \ldots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \ldots & \sigma_m \\ \end{bmatrix} \begin{bmatrix} \VEC{v}_1 & \ldots \VEC{v}_n \end{bmatrix}^T \]
where:
We may compute the pseudoinverse of the Jacobian by inverting the SVD:
\begin{align*} \jacobian^+ &= \MAT{V}\MAT{\Sigma}^+\MAT{U}^T \\ & = \begin{bmatrix} \VEC{v}_1 & \ldots \VEC{v}_n \end{bmatrix} \begin{bmatrix} \frac{1}{\sigma_1} & \ldots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \ldots & \frac{1}{\sigma_m} \\ \end{bmatrix} \begin{bmatrix} \VEC{u}_1 & \ldots \VEC{u}_m \end{bmatrix}^T \nonumber \\ & = \sum_{i=1}^m \frac{1}{\sigma_i} \VEC{v}_i \VEC{u}_i^T \; . \end{align*}
At singularities, some of the singular values \(\sigma_i\) go to zero, resulting in an undefined pseudoinverse.
We may compute the damped psuedoinverse by adding in the damping factor to the inverted SVD:
\[ \MAT{J}^\dagger = \sum_{i=1}^m \frac{\sigma_i^2}{\sigma_i^2 + k^2} \VEC{v}_i \VEC{u}_i^T \]
Finally, we may compute a singularity robust pseudo-inverse by simply omitting singular values below some threshold:
\[ \MAT{J}^\dagger = \sum_{i=1}^m \begin{cases} \frac{1}{\sigma_i} \VEC{v}_i \VEC{u}_i^T & \textrm{when } \sigma_i \geq \epsilon \\ \MAT{0} & \textrm{when } \sigma_i < \epsilon \end{cases} \]
The following example code computes the differential kinematics for the 7-DoF robot:
Inverse position kinematics (IK) finds the configuration (joint positions) for a given target frame pose. Generally, IK has a direct interpretation as constrained optimization to minimize error f between actual ( \(\tf{\quat{S}}{}{\rm act}\)) and reference ( \(\tf{\quat{S}}{}{\rm ref}\)) poses, subject to joint limits:
\[ \underset{\confvec}{\text{minimize}} \quad f\left( \tf{S}{}{\rm act}(\confvec),\ \tf{S}{}{\rm ref} \right) \nonumber \\ \text{subject to} \quad \confvec_{\rm min} \leq \confvec \leq \confvec_{\rm max} \; . \]
Classical approaches for inverse kinematics employed the Newton-Raphson method or Levenberg-Marquardt algorithm. However, such methods offer poor robustness regarding joint limits. Recent work has used sequential quadratic programming (SQP) to solve inverse kinematics problems. The SQP formulation directly handles joint limits, improving robustness. Moreover, careful implementations of the SQP formulation offer excellent performance: Amino's SQP-based IK solver handles 6-7 joint problems in an average of 0.1 milliseconds on a 2017 Intel CPU.
We may define the pose error f based on the distance between the reference and actual pose. The error as a transformation is:
\[ {\tf{\seduqu}{}{\rm act}} \qmul \tf{\seduqu}{}{\rm err} = \tf{\seduqu}{}{\rm ref} \qquad\leadsto\qquad \tf{\seduqu}{}{\rm err} = {\tf{\seduqu}{}{\rm act}}^* \qmul \tf{\seduqu}{}{\rm ref} \; . \]
The norm of the logarithm is a measure for both rotational and translational distances. Alternatively, we may separately find angle error as the norm of the rotation log and translation error as Euclidean distance of the relative. Thus, we may use the following as objective functions:
\begin{align*} f_{\rm dq} & = \normtwo{ \ln \left( {\tf{\seduqu}{}{\rm act}}^* \qmul \tf{\seduqu}{}{\rm ref} \right) }^2 \; , \\ f_{\rm qv} & = \normtwo{ \ln \left( {\tf{\sequat}{}{\rm act}}^* \qmul \tf{\sequat}{}{\rm ref} \right) }^2 + \normtwo{ \setrans_{\rm act} - \setrans_{\rm ref} }^2 \; . \end{align*}
To solve the optimization problem, we need a gradient and, depending on the specific method, the Hessian of the objective function. Solvers employing quasi-Newton methods approximate the Hessian, which both avoids the need to analytically derive the Hessian and is often faster than computing an exact Hessian. However, we must still find the gradient. It is possible to approximate the gradient via finite difference, but the repeated evaluation of the forward kinematics to find the finite difference gradient is slow. Instead, Amino includes analytic gradients for the above objective functions.