amino  1.0-beta2
Lightweight Robot Utility Library
Kinematics

\[ \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.

Robot Model

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.

  1. We use the following scene file:

    /* 7dof.roboray
    * ===============
    *
    * 7-Degree-of-Freedom serial robot.
    */
    /* Parameters for the robot */
    def r 1.618; // a ratio
    def rad 0.05; // Link radius
    def len0 1; // upper arm length
    def len1 1; // forearm length
    /* Define classes for robot parts */
    class link {
    shape cylinder;
    radius rad;
    color [.5, .5, .5];
    specular [.75, .75, .75];
    }
    class joint_style {
    color [0, 0, 0];
    specular [.5, .5, .5];
    }
    /* Draw the grid */
    frame grid {
    geometry {
    shape grid;
    delta [.1, .1];
    thickness .005;
    dimension [1, 1];
    }
    }
    /* Draw the robot */
    // Shoulder
    frame s0 {
    type revolute; # frame type: revolute joint
    axis [1, 0, 0]; # joint axis
    geometry {
    isa joint_style; # instance of joint_style class
    shape sphere;
    radius r*rad;
    }
    }
    frame s1 {
    parent s0;
    type revolute;
    axis [0, 1, 0];
    }
    frame s2 {
    parent s1;
    type revolute;
    axis [1, 0, 0];
    // nested frame is a child of s2
    frame upper-link {
    rpy [0, pi/2, 0];
    geometry {
    isa link;
    height len0;
    }
    }
    }
    // Elbow
    frame e {
    parent s2;
    type revolute;
    axis [0,1,0];
    translation [len0, 0, 0];
    frame e-link {
    rpy [pi/2, 0, 0];
    translation [0, rad, 0];
    geometry {
    isa joint_style;
    shape cylinder;
    height 2*rad;
    radius r*rad;
    }
    }
    frame lower-link {
    rpy [0, pi/2, 0];
    geometry {
    isa link;
    height len1;
    }
    }
    }
    // Wrist
    frame w0 {
    parent e;
    type revolute;
    axis [1, 0, 0];
    translation [len1, 0, 0];
    }
    frame w1 {
    parent w0;
    type revolute;
    axis [0, 1, 0];
    }
    frame w2 {
    parent w1;
    type revolute;
    axis [0, 0, 1];
    geometry {
    isa joint_style;
    shape sphere;
    radius r*rad;
    }
    }
    // Hand (Palm)
    def hand_rad r**2 * rad;
    def hand_height r*rad;
    frame hand {
    parent w2;
    type fixed;
    rpy [0, pi/2, 0];
    translation [hand_height, 0, 0];
    geometry {
    isa link;
    shape cone;
    height -hand_height;
    start_radius hand_rad;
    end_radius r*rad;
    }
    }
    // Fingers
    def finger_rad rad / r**2;
    def finger_len finger_rad * r**4;
    def finger_base_rad finger_rad*r;
    def finger_off hand_rad - finger_base_rad;
    class finger_base {
    isa joint_style;
    shape sphere;
    radius finger_base_rad;
    }
    class finger_link {
    isa link;
    shape cylinder;
    radius finger_rad;
    height finger_len;
    }
    class finger_tip {
    isa link;
    shape sphere;
    radius finger_rad;
    }
    frame f0 {
    parent hand;
    translation [finger_off, 0, finger_base_rad];
    type revolute;
    axis [0, -1, 0];
    geometry { isa finger_base; }
    geometry { isa finger_link; }
    frame f0tip {
    translation [0, 0, finger_len];
    geometry { isa finger_tip; }
    }
    }
    frame f1base {
    parent hand;
    rpy [0, 0, 2/3*pi];
    frame f1 {
    axis [0, -1, 0];
    type revolute;
    translation [finger_off, 0, finger_base_rad];
    geometry { isa finger_base; }
    geometry { isa finger_link; }
    frame f1tip {
    translation [0, 0, finger_len];
    geometry { isa finger_tip; }
    }
    }
    }
    frame f2base {
    parent hand;
    rpy [0, 0, -2/3*pi];
    frame f2 {
    axis [0, -1, 0];
    type revolute;
    translation [finger_off, 0, finger_base_rad];
    geometry { isa finger_base; }
    geometry { isa finger_link; }
    frame f2tip {
    translation [0, 0, finger_len];
    geometry { isa finger_tip; }
    }
    }
    }

  2. Compile the scene using the following build script:

    cmake_minimum_required (VERSION 2.6)
    project (AminoTutorials)
    # pkg-config setup
    find_package(PkgConfig)
    # Search for the pkg-config packages
    pkg_search_module(SDL2 REQUIRED sdl2)
    pkg_search_module(GL REQUIRED gl)
    pkg_search_module(AMINO REQUIRED amino)
    pkg_search_module(AMINO_GL REQUIRED amino-gl)
    # Compile scene graph to C
    add_custom_command(
    OUTPUT 7dof-scene.c 7dof-scene.c.h
    DEPENDS 7dof.robray
    COMMAND aarxc "${CMAKE_SOURCE_DIR}/7dof.robray" -o 7dof-scene.c -n "7dof"
    )
    # Compile plugin (shared library)
    add_library( scene MODULE 7dof-scene.c )
    target_include_directories( scene PUBLIC
    ${AMINO_INCLUDE_DIRS}
    )
    target_link_libraries( scene
    ${AMINO_GL_LIBRARIES}
    ${AMINO_LIBRARIES}
    )
    amino namespace
    Definition: amino.hpp:60

Forward Kinematics

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. Frame Structure of the Simple Scene

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} \]

Example Code

The following example code computes the forward kinematics for the 7-DoF robot:

#!/usr/bin/env python3
# File: t4.1-fk.py
# =======================
#
# Compute Forward Kinematics
from math import pi, cos
from time import sleep
import os
from amino import SceneWin, SceneGraph, SceneFK
# Scene Parameters
scene_plugin = ("{}/../plugin/7dof/libscene.so".format(os.path.dirname(__file__)))
scene_name = "7dof"
# Create an (empty) scene graph
sg = SceneGraph()
# Load the scene plugin
sg.load(scene_plugin, scene_name)
# Initialize the scene graph
sg.init()
# Create a window, pass the scenegraph, and start
win = SceneWin(scenegraph=sg, start=True, background=True)
# Create the FK context
fk = SceneFK(sg)
# Do a simple wave
dt = 1.0 / 60
t = 0
config_dict = {
"s1": -.75 * pi,
"e": .75 * pi,
"f0": .125 * pi,
"f1": .125 * pi,
"f2": .125 * pi
}
while win.is_runnining():
# update forward kinematics
config_dict['e'] = (120 + 15 * cos(t)) * (pi / 180)
fk.config = config_dict
# update window
win.fk = fk
# absolute hand pose is changing
TF_g_h = fk["hand"]
print("g->hand: %s" % TF_g_h)
# Relative pose between elbow and hand does not change; there are
# only fixed transforms and joints with unchanging configuration
# between these two frames.
TF_e_h = fk["e", "hand"]
print("e->hand: %s" % TF_e_h)
# sleep till next cycle
sleep(dt)
t += dt

Differential Kinematics

Differential kinematics relates change (e.g., velocities) in configuration (joint positions) and pose. We will construct and solve a differential equation for this relationship.

Representing Pose Change

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.

Velocity

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 \; . \]

Transform Derivative

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.

Twist

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.

Manipulator Jacobian

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.

Velocity Jacobian

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.

Twist Jacobian

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.

Derivative Jacobian

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*}

Jacobian Pseudoinverse Methods

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.

Proportional Position Control

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.

Nullspace Projection

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.

Singularity-robust methods

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:

  • \(\MAT{U}\) is an \(m \times m\) orthogonal matrix whose columns are the left-singular vectors of \(\MAT{J}\)
  • \(\MAT{\Sigma}\) is an \(m\times n\) diagonal matrix containing the singular values of \(\MAT{J}\)
  • \(\MAT{V}\) is an \(n \times n\) orthogonal matrix whose columns are the right-singular vectors of \(\MAT{J}\)

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} \]

Example Code

The following example code computes the differential kinematics for the 7-DoF robot:

#!/usr/bin/env python3
# File: t4.2-dk.py
# =======================
#
# Compute Differential Kinematics
#
# Slide the end-effector forward and backward in the X-direction
from math import pi, cos, sin
from time import sleep
import os
from amino import SceneWin, SceneGraph, Geom, SceneFK, SceneDK, TfVel, QuatTrans, YAngle
# Scene Parameters
scene_plugin = ("{}/../plugin/7dof/libscene.so".format(os.path.dirname(__file__)))
scene_name = "7dof"
# Create an (empty) scene graph
sg = SceneGraph()
# Load the scene plugin
sg.load(scene_plugin, scene_name)
# Initialize the scene graph
sg.init()
# Initial configuration
q_all = sg.config_vector({
"s1": -.75 * pi,
"e": .75 * pi,
"f0": .125 * pi,
"f1": .125 * pi,
"f2": .125 * pi
})
# Draw in the the path that the robot will follow
fk = SceneFK(sg) # temporary forward kinematics context
fk.config = q_all
tf0 = fk["hand"]
path_opts = {"color": (1, 0, 0), "alpha": .25}
path_h = .75
path_r = 5e-3
sg.add_frame_fixed(
"",
"path",
tf=QuatTrans((YAngle(pi / 2), tf0.translation)),
geom=(Geom.cylinder(path_opts, path_h, path_r),
Geom.cylinder(path_opts, -path_h, path_r)))
sg.init() # need to re-init after adding a frame
# Forward kinematics context
fk = SceneFK(sg)
# Initialize Window
win = SceneWin(scenegraph=sg, start=True, background=True)
# Create the sub-scenegraph from root to "hand"
ssg = sg[:"hand"]
# Create the differential kinematics context
dk = SceneDK(ssg, fk)
# Slide hand forward and back
dt = 1.0 / 60
t = 0
tfvel = TfVel()
while win.is_runnining():
# update forward kinematics
fk.config = q_all
# X-direction position and velocity
x = .25 * sin(t) # position
dx = .25 * cos(t) # velocity, the derivative of x
# Set the reference velocity.
tfvel.translational = (dx, 0, 0)
dk.ref_tf_vel = tfvel
# Set the reference pose. If we omit position feedback, the
# robot will drift over time due to floating point error.
dk.ref_tf = (tf0.rotation, (tf0.translation + (x, 0, 0)))
# Get config velocity
dq_sub = dk.solve_vel()
# Euler integration of configuration
if dq_sub:
dq_all = ssg.scatter_config(dq_sub)
q_all += dt * dq_all
else:
print("Error")
# update window
win.config = q_all
# sleep till next cycle
sleep(dt)
t += dt

Inverse Position Kinematics

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.

Example Code

#!/usr/bin/env python3
# File: t4.3-ik.py
# =======================
#
# Compute Inverse Kinematics
#
# Slide the end-effector forward and backward in the X-direction
from math import pi
from time import sleep
import os
from amino import SceneWin, SceneGraph, SceneIK, YAngle
# Scene Parameters
scene_plugin = ("{}/../plugin/7dof/libscene.so".format(os.path.dirname(__file__)))
scene_name = "7dof"
# Create an (empty) scene graph
sg = SceneGraph()
# Load the scene plugin
sg.load(scene_plugin, scene_name)
# Initialize the scene graph
sg.init()
# Create the sub-scenegraph from root to "hand"
ssg = sg[:"hand"]
# Initialize and Start Window
win = SceneWin(scenegraph=sg, start=True, background=True)
# Create the inverse kinematics context
ik = SceneIK(ssg)
# IK Parameters
ik.set_seed_center()
ik.restart_time = 100e-3
# Set the reference transform
ik.ref_tf = (YAngle(pi / 2), (0.2, 0, 0.7))
dt = .1
period = 3
while win.is_runnining():
# Solve IK
q_sub = ik.solve()
# Display in Window
if q_sub: # check for valid solution
print(q_sub)
win.config = ssg.scatter_config(q_sub)
else:
print("No IK Solution")
# reseed for next run
ik.set_seed_rand()
# Sleep a bit
t = 0
while win.is_runnining() and t < period:
sleep(dt)
t += dt

See Also

Python

C

References