Robot Mechanics

These notes summarize some useful information about robot mechanics, particularly WRT arms. Many formulas are based on the lecture notes of Oussama Khatib. Accuracy is believed, but not in any fashion guaranteed.

Table of Contents

1 Math

1.1 Gradient

The gradient, $∇ f$, is a vector of partial derivatives of a vector function $f({\bf x})$.

  • let $f({\bf x}): \Ren \mapsto \Re$

\begin{equation} ∇ f = \left[\begin{array}{ccc} \frac{∂ f}{∂ x1} & \cdots & \frac{∂ f}{∂ xn} \end{array}\right] \end{equation}

1.2 Lagrange Multipliers

Lagrange multipliers are a technique to find the maximum or minimum of some function, $f({\bf x})$, subject to some constraint, $g({\bf x}) = c$. To solve the problem, consider the Lagrange function $Λ({\bf x}, λ)$.

\begin{equation} Λ({\bf x}, λ) = f({\bf x}) - λ ( g({\bf x}) - c) \end{equation}

Then, to find the points on ${\bf x}$ with max and min values, solve

\begin{equation} ∇ Λ({\bf x}, λ) = {\bf 0} \end{equation}

taking the gradient across all elements of ${\bf x}$ and also $λ$.

2 Kinematics

2.1 Transformations

2.1.1 Rotation Matrix

A rotation matrix ${\bf R}A/B$ describes the orientation of frame $A$ relative to frame $B$. The columns of ${\bf R}A/B$ are the unit vectors of frame $A$ in the coordinates of frame $B$. One can combine rotation matrices as

\begin{equation} {\bf R}A/C = {\bf R}B/C{\bf R}A/B \end{equation}

  • Pure Rotation

    For some vector ${\bf p}$ expressed in frame $A$ as ${\bf p}A$, we can express that vector in frame $B$ as ${\bf p}B$ by:

    \begin{equation} {\bf p}B = {\bf R}A/B{\bf p}A \end{equation}

  • Translation

    Translation can be expressed by simple addition, provided the vectors are in the same coordinate frame:

    \begin{equation} {\bf r}A = {\bf p}A + {\bf q}A \end{equation}

  • Homogeneous Transformation

    The homogeneous transformation combines a translation and a rotation in \begin{equation} {\bf r}B = {\bf p}B + {\bf R}A/B {\bf q}A \end{equation}

    This can be written as

    \begin{eqnarray} \nonumber \left[\begin{array}{c} {\bf r}B\\ 1 \end{array}\right] = \left[\begin{array}{cc} {\bf R}A/B & {\bf p}B
    0 & 1 \end{array} \right] \left[\begin{array}{c} {\bf q}A
    1 \end{array} \right]
    % \label{eq:transform} \left[\begin{array}{c} {\bf r}B\\ 1 \end{array}\right] = {\bf T}A/B \left[\begin{array}{c} {\bf q}A
    1 \end{array} \right] \end{eqnarray}

    where the transformation matrix ${\bf T}A/B$ encoding the position and orientation of the first vector is given by

    \begin{equation} {\bf T}A/B = \left[\begin{array}{cc} {\bf R}A/B & {\bf p}B
    0 & 1 \end{array} \right] \end{equation}

    Note also that \begin{equation} {{\bf T}A/B}-1 = {\bf T}B/A \end{equation}

  • Fitting Transforms

    Given corresponding points $\{{\bf a}(1)\ldots{\bf a}(n) \}$ and $\{{\bf b}(1)\ldots{\bf b}(n) \}$ in frames $A$ and $B$, we can find the best-fit transform between them.

    • Expand the homogeneous transform from Eq. \ref{eq:transform}.

    \[ \left[\begin{array}{c} b1 \\ b2 \\ b3 \\ 1 \end{array}\right](i) = \left\{\begin{array}{cccc} r11 & r12 & r13 & v1
    r21 & r22 & r23 & v2
    r31 & r32 & r33 & v3
    0 & 0 & 0 & 1 \end{array}\right\} \left[\begin{array}{c} a1 \\ a2 \\ a3 \\ 1 \end{array}\right](i) \]

    • Take the first line of this matrix equation, and reorder, including all points.

    \[ \left[\begin{array}{c} {b(1)}1 \\ \vdots \\ {b(n)}1 \end{array}\right] = \left\{\begin{array}{cccc} {a(1)}1 & {a(1)}2 & {a(1)}3 & 1
    \vdots & \vdots & \vdots & \vdots
    {a(n)}1 & {a(n)}2 & {a(n)}3 & 1 \end{array}\right\} \left[\begin{array}{c} r11 \\ r12 \\ r13 \\ v1 \end{array}\right] \]

    • This system is now in the standard ${\bf b} = {\bf A}{\bf x}$ form, and we can easily use SVD via Lapack dgelss to find each row of ${\bf T}A/B$
    • Each row found this man need to be normalized so the magnitude of the r components is 1.

2.2 Jacobian

The jacobian is a matrix of partial derivatives of vector valued function. It is a generalization of the gradient, allowing ${\bf f}$ to map to a vector rather than just a scalar.

  • let ${\bf f}: \Ren \mapsto \Rem$
  • let $yi = fi\left({\bf x}\right) = \left({\bf f}\left({\bf x}\right)\right)i$


\begin{equation} {\bf J} = ∇{\bf f} = \left[ \begin{array}{ccc} \frac{∂ y1}{∂ x1} & \cdots & \frac{∂ y1}{∂ xn}
\vdots & \ddots & \vdots
\frac{∂ ym}{∂ x1} & \cdots & \frac{∂ ym}{∂ xn} \end{array} \right] \end{equation}

2.2.1 Spatial Jacobian

The Jacobian for 3-dimensional (and 3-rotational) space can be directly calculated from the transformation matrices. Assume we have some function ${\bf f}({\bf w}) : \Ren \mapsto \left[\begin{array}{cccccc} x & y & z & θr & θp & θy \end{array}\right]$ where ${\bf w}$ represents rotational joint angles or prismatic extensions and ${\bf f}$ is defined by a product series of transformation matrices. This is given by

\begin{equation} {\bf J} = \left[\begin{array}{ccc} \left(\begin{array}{c} {\bf j}p1
{\bf j}R1 \end{array}\right) & \begin{array}{c} \cdots
\cdots \end{array} & \left(\begin{array}{c} {\bf j}pn
{\bf j}Rn \end{array}\right) \end{array}\right] \end{equation}

where \[ {\bf j}pi = \left[\begin{array}{ccc} \frac{∂ x}{∂ wi} & \frac{∂ y}{∂ wi} & \frac{∂ z}{∂ wi} \end{array}\right]T \] and \[ {\bf j}Ri = \left[\begin{array}{ccc} \frac{∂ θr}{∂ wi} & \frac{∂ θp}{∂ wi} & \frac{∂ θy}{∂ wi} \end{array}\right]T . \]

We can easily calculate ${\bf j}pi$ and ${\bf j}Ri$ for rotational joints according to

\begin{eqnarray} {\bf j}pi = {\bf a}i-1 × ({\bf p}e - {\bf q}i-1)
{\bf j}Ri = {\bf a}i-1 \end{eqnarray}

where ${\bf a}i$ is the axis through which Frame i moves, ${\bf q}i$ is the position vector of the origin of Frame i, and ${\bf p}e$ is the point for which the Jacobian is being calculated, all in the global frame.

3 Dynamics

The general dynamics of a robot arm can be expressed according to

\begin{equation} {\bf M}\ddot{\bf q} + {\bf v}({\bf q}, \dot{\bf q}) + {\bf G}({\bf q}) = {\bf τ} \end{equation}

Where ${\bf q}$ are the generalized coordinates of the Lagrange equation, ${\bf M}$ is the ``Mass Matrix,'' ${\bf v}$ represents the centrifugal and coriolis forces, ${\bf G}$ represents the gravity forces, and ${τ}$ are the generalized forces along ${\bf q}$.

\begin{equation} {\bf M} = ∑i=0n \left( mi {Jvi}TJvi + {Jωi}T IC,Ci Jωi \right) \end{equation}

Where $IC,Ci$ is the inertia tensor of link $i$ WRT the links center of mass at $Ci$, $Jvi$ is the Jacobian Matrix corresponding to the linear motion of the center of mass of link $i$, $Jω i$ is the Jacobian corresponding to the angular velocity of link $i$, and $mi$ is the mass of link $i$.

\begin{equation} {\bf v}({\bf q}, \dot{\bf q}) = {\bf C}(\bf q) \left[\dot{\bf q}2\right] + {\bf B}({\bf q})\left[ \dot{\bf q} \dot{\bf q} \right] \end{equation}

The matrices ${\bf C}$ and ${\bf B}$ can be computed directly from the mass matrix ${\bf M}$.

\begin{eqnarray} mijk = \frac{∂ Mij}{∂ qk}
bijk = \frac{1}{2} \left( mijk + mikj - mjki \right)
{\bf C}(\bf {q}) = \left[ \begin{array}{cccc} b111 & b122 & \cdots & b1nn
b211 & b222 & \cdots & b2nn
\vdots & \vdots & \ddots & \vdots
bn11 & bn22 & \cdots & bnnn
{\bf B}({\bf q}) = \left[ \begin{array}{ccccccccc} 2b112 & 2b113 & \cdots & 2b11n & 2b123 & \cdots & 2b12n & \cdots & 2b1\left(n-1\right)n
2b212 & 2b213 & \cdots & 2b21n & 2b223 & \cdots & 2b22n & \cdots & 2b2\left(n-1\right)n
\vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots
2b212 & 2b213 & \cdots & 2b21n & 2b223 & \cdots & 2b22n & \cdots & 2b2\left(n-1\right)n
\end{array}\right] \end{eqnarray}

Finally, ${\bf G}({\bf q})$ can be computed according to

\begin{equation} {\bf G}({\bf q}) = -\left( {{\bf J}v1}T\left(m1 {\bf g} \right) + {{\bf J}v2}T\left(m2 {\bf g} \right) + \cdots + {{\bf J}vn}T\left(mn {\bf g} \right) + \right) \end{equation}

where ${\bf g}$ represents the vector in space of the acceleration due to gravity (ie, $\left[\begin{array}{ccc}0 & 0 & -9.8\end{array}\right]T$).

4 Velocity Profiles

4.1 Trapezoidal Velocity

  • Constant acceleration, $aM$
  • Max Velocity, $vM$
  • Distance, $d$
  • Fastest way to reach set point under those constraints
  • Using Switching times $t1, t2, t3$

4.1.1 Normal Case

Useful when you want to reach a set point in the shortest possible time.

  • Trapezoidal

    \begin{eqnarray} \nonumber t1 = vM / aM
    \nonumber t2 = d / vM
    t3 = t1 + t2 \end{eqnarray}

  • Triangular

    When $t1 vm > d$,

    \begin{eqnarray} \nonumber t3 = \sqrt{\frac{4d}{aM}}
    \nonumber t1 = \frac{t3 }{ 2 }
    t1 = t1 \end{eqnarray}

4.1.2 Fixed Time

Useful when you have multiple joints and want them to all reach the endpoint at the same time, $t3$.

  • Triangular

    Try this first since a smaller acceleration is probably more desirable that a smaller final velocity.

    \begin{eqnarray} \nonumber t1 = \frac{t3}{2}
    \nonumber t2 = t1
    \nonumber vm = \frac{d}{t1}
    a = \frac{vm}{t1} \end{eqnarray}

  • Trapezoidal When vm > vM,

    \begin{eqnarray} \nonumber vm = vM
    \nonumber tm = \frac{2d}{vm} - t3
    \nonumber t1 = \frac{t3 - tm}{2}
    \nonumber t2 = t3 - t1
    a = \frac{vm}{t1} \end{eqnarray}

    If $a > aM$, then you've given too small a time $t3$.

Author: Neil T. Dantam <>

Date: 2014-07-01 17:01:01 EDT

HTML generated by org-mode 6.33x in emacs 23