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.

1 Math
- 1.1 Gradient
- 1.2 Lagrange Multipliers
2 Kinematics
- 2.1 Transformations
  - 2.1.1 Rotation Matrix
- 2.2 Jacobian
  - 2.2.1 Spatial Jacobian
3 Dynamics
4 Velocity Profiles
- 4.1 Trapezoidal Velocity
  - 4.1.1 Normal Case
  - 4.1.2 Fixed Time

1 Math

1.1 Gradient

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

let \(f({\bf x}): \Re^n \mapsto \Re\)

\begin{equation} \nabla f = \left[\begin{array}{ccc} \frac{\partial f}{\partial x_1} & \cdots & \frac{\partial f}{\partial x_n} \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 \(\Lambda({\bf x}, \lambda)\).

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

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

\begin{equation} \nabla \Lambda({\bf x}, \lambda) = {\bf 0} \end{equation}

taking the gradient across all elements of \({\bf x}\) and also \(\lambda\).

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. transform.
\[ \left[\begin{array}{c} b_1 \\ b_2 \\ b_3 \\ 1 \end{array}\right]^{(i)} = \left\{\begin{array}{cccc} r_{11} & r_{12} & r_{13} & v_1 \\ r_{21} & r_{22} & r_{23} & v_2 \\ r_{31} & r_{32} & r_{33} & v_3 \\ 0 & 0 & 0 & 1 \end{array}\right\} \left[\begin{array}{c} a_1 \\ a_2 \\ a_3 \\ 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} r_{11} \\ r_{12} \\ r_{13} \\ v_1 \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}: \Re^n \mapsto \Re^m\)
let \(y_i = f_i\left({\bf x}\right) = \left({\bf f}\left({\bf x}\right)\right)_i\)

then

\begin{equation} {\bf J} = \nabla{\bf f} = \left[ \begin{array}{ccc} \frac{\partial y_1}{\partial x_1} & \cdots & \frac{\partial y_1}{\partial x_n} \\ \vdots & \ddots & \vdots\\ \frac{\partial y_m}{\partial x_1} & \cdots & \frac{\partial y_m}{\partial x_n} \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}) : \Re^n \mapsto \left[\begin{array}{cccccc} x & y & z & \theta_r & \theta_p & \theta_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}_{p_1} \\ {\bf j}_{R_1} \end{array}\right) & \begin{array}{c} \cdots \\ \cdots \end{array} & \left(\begin{array}{c} {\bf j}_{p_n} \\ {\bf j}_{R_n} \end{array}\right) \end{array}\right] \end{equation}

where \[ {\bf j}_{p_i} = \left[\begin{array}{ccc} \frac{\partial x}{\partial w_i} & \frac{\partial y}{\partial w_i} & \frac{\partial z}{\partial w_i} \end{array}\right]^T \] and \[ {\bf j}_{R_i} = \left[\begin{array}{ccc} \frac{\partial \theta_r}{\partial w_i} & \frac{\partial \theta_p}{\partial w_i} & \frac{\partial \theta_y}{\partial w_i} \end{array}\right]^T . \] We can easily calculate \({\bf j}_{p_i}\) and \({\bf j}_{R_i}\) for rotational joints according to

\begin{eqnarray} {\bf j}_{p_i} = {\bf a}_{i-1} \times ({\bf p}_e - {\bf q}_{i-1})\\ {\bf j}_{R_i} = {\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 \tau} \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 \({\tau}\) are the generalized forces along \({\bf q}\).

\begin{equation} {\bf M} = \sum_{i=0}^n \left( m_i {J_{v_i}}^TJ_{v_i} + {J_{\omega_i}}^T I_{C,C_i} J_{\omega_i} \right) \end{equation}

Where \(I_{C,C_i}\) is the inertia tensor of link \(i\) WRT the links center of mass at \(C_i\), \(J_{vi}\) is the Jacobian Matrix corresponding to the linear motion of the center of mass of link \(i\), \(J_{\omega i}\) is the Jacobian corresponding to the angular velocity of link \(i\), and \(m_i\) 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} m_{ijk} = \frac{\partial M_{ij}}{\partial q_k} \\ b_{ijk} = \frac{1}{2} \left( m_{ijk} + m_{ikj} - m_{jki} \right) \\ {\bf C}(\bf {q}) = \left[ \begin{array}{cccc} b_{111} & b_{122} & \cdots & b_{1nn} \\ b_{211} & b_{222} & \cdots & b_{2nn} \\ \vdots & \vdots & \ddots & \vdots \\ b_{n11} & b_{n22} & \cdots & b_{nnn} \\ \end{array}\right] \\ {\bf B}({\bf q}) = \left[ \begin{array}{ccccccccc} 2b_{112} & 2b_{113} & \cdots & 2b_{11n} & 2b_{123} & \cdots & 2b_{12n} & \cdots & 2b_{1\left(n-1\right)n} \\ 2b_{212} & 2b_{213} & \cdots & 2b_{21n} & 2b_{223} & \cdots & 2b_{22n} & \cdots & 2b_{2\left(n-1\right)n} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 2b_{212} & 2b_{213} & \cdots & 2b_{21n} & 2b_{223} & \cdots & 2b_{22n} & \cdots & 2b_{2\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}_{v_1}}^T\left(m_1 {\bf g} \right) + {{\bf J}_{v_2}}^T\left(m_2 {\bf g} \right) + \cdots + {{\bf J}_{v_n}}^T\left(m_n {\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, \(a_M\)
Max Velocity, \(v_M\)
Distance, \(d\)
Fastest way to reach set point under those constraints
Using Switching times \(t_1, t_2, t_3\)

4.1.1 Normal Case

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

Trapezoidal
\begin{eqnarray} \nonumber t_1 = v_M / a_M \\ \nonumber t_2 = d / v_M \\ t_3 = t_1 + t_2 \end{eqnarray}

Triangular

When \(t_1 v_m > d\),
\begin{eqnarray} \nonumber t_3 = \sqrt{\frac{4d}{a_M}} \\ \nonumber t_1 = \frac{t_3 }{ 2 } \\ t_1 = t_1 \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, \(t_3\).

Triangular

Try this first since a smaller acceleration is probably more desirable that a smaller final velocity.
\begin{eqnarray} \nonumber t_1 = \frac{t_3}{2} \\ \nonumber t_2 = t_1 \\ \nonumber v_m = \frac{d}{t_1} \\ a = \frac{v_m}{t_1} \end{eqnarray}

Trapezoidal When v_m > v_M,
\begin{eqnarray} \nonumber v_m = v_M \\ \nonumber t_m = \frac{2d}{v_m} - t_3 \\ \nonumber t_1 = \frac{t_3 - t_m}{2}\\ \nonumber t_2 = t_3 - t_1 \\ a = \frac{v_m}{t_1} \end{eqnarray}
If \(a > a_M\), then you've given too small a time \(t_3\).