Modern Robotics - Course Notes

这是美国西北大学 Modern Robotics 课程的个人笔记，主要内容包括机器人运动基础、正向运动学（FK）、逆向运动学（IK）等。

Chapter 2&3: Foundations of Robot Motion

2.1 Degrees of Freedom of a Rigid Body

• Configuration: A specification of the position of all points of a robot.
• C-space: The space of all configurations.
• Degrees of freedom (dof): The dimension of the C-space.
  • $\text{dof} = \sum(\text{freedoms of points}) - \text{\# of independent constraints}$
  • $\text{dof} = \sum(\text{freedoms of bodies}) - \text{\# of independent constraints}$

2.2 Degrees of Freedom of a Robot

• Grubler’s formula: (all constraints independent)

  $$\begin{aligned} \text{dof} &=\, \underbrace{m(N-1)}_{\text{rigid body freedoms}} - \underbrace{\sum_{i=1}^{j} c_i}_{\text{joint constraints}} \\ &=\, m(N - 1) - \sum_{i=1}^J(m - f_i) \\ &=\, m(N - 1 - J) + \sum_{i=1}^J f_i \end{aligned}$$

  where

  • $N = \text{\# of bodies, including ground}$
  • $J = \text{\# of joints}$
  • $m = 6 \text{ for spatial bodies}, 3 \text{ for planar}$

• Example 1: 3R serial “open-chain” robot

  • $N = 4, m = 3, J = 3$
  • $\text{dof} = 3(4 - 1 - 3) + 3 = 3$

• Example 2: Four-bar “closed-chain” mechanism
  • $N = 4, m = 3, J = 4$
  • $\text{dof} = 3(4 - 1 - 4) + 4 = 1$

• Example 3: Stewart platform 6*UPS
  • $N = 14, m = 6, J = 18$
  • $\text{dof} = 6(14 - 1 - 18) + 36 = 6$

2.3.1 Configuration Space Topology

• Two spaces are topological equivalent if one can be smoothly deformed to the other without cutting and gluing.
• C-spaces of the same dimension can have different topologies.

2.3.2 Configuration Space Representation

• The topology of the space is independent of our presentation of the spce.
• Take a sphere as an example:
  • Explicit Parametrization: minimum number of coordinate, e.g., latitude, longtitude. May have sigularities.
  • Implicit Parametrization: a surface embedded in a higher-dimensional space, e.g., $(x, y, z)$ such that $x^2 + y^2 + z^2 = 1$.

2.4 Configuration and Velocity Constraints

Holonomic constraints

By Grubler’s formula, this mechanism has one degree of freedom. We view the C-space as a one dimensional space embedded in the four dimensional space of joint angles, defined by three loop closure equations.

$$\begin{aligned} L_1\cos\theta_1 + L_2 \cos(\theta_1+\theta_2) + \cdots + L_4 \cos(\theta_1 + \cdots + \theta_4) =&\, 0 \\ L_1\sin\theta_1 + L_2 \sin(\theta_1+\theta_2) + \cdots + L_4 \sin(\theta_1 + \cdots + \theta_4) =&\, 0 \\ \theta_1 + \theta_2 + \theta_3 + \theta_4 - 2\pi =&\, 0 \end{aligned}$$

Let us define

$$\theta = [\theta_1, \dots, \theta_n]^T,$$

then we can rewrite the loop closure equations in the vector form

$$\mathbf{g}(\theta) = \begin{bmatrix} g_1(\theta_1, \dots, \theta_n) \\ \vdots \\ g_k(\theta_1, \dots, \theta_n) \end{bmatrix} = \mathbf{0}.$$

Configuration constraints are called holonomic constraints, which are constraints that reduce the dimension of the C-space.

• If $\theta \in \mathbb{R}^n$, and $\mathbf{g}(\theta) \in \mathbb{R}^k$, then $\mathrm{dof} = n - k$.

This means that if the robot’s configuration is defined by $n$ varibles, subject to $k$ independent polynomimal constraints, then the dimension of the C-space (dof) is $n-k$.

Pfaffian & Nonholonomic constraints

When the robot is moving, how the holonomic constraints restrict the velocity of the robot?

Since $\mathbf{g}(\theta)$ has to be zero at all times, the time rate of change of $\mathbf{g}$ must also be zero at all times. So we have

$$\begin{bmatrix} \dfrac{\partial g_1}{\partial \theta_1}(\theta)\dot{\theta_1}+\cdots+\dfrac{\partial g_1}{\partial \theta_n}(\theta)\dot{\theta_n} \\ \vdots \\ \dfrac{\partial g_k}{\partial \theta_1}(\theta)\dot{\theta_1}+\cdots+\dfrac{\partial g_k}{\partial \theta_n}(\theta)\dot{\theta_n} \\ \end{bmatrix} = 0,$$

which can be rewritten as

$$\begin{bmatrix} \dfrac{\partial g_1}{\partial \theta_1}(\theta) & \ldots & \dfrac{\partial g_1}{\partial \theta_n}(\theta) \\ \vdots & \ddots & \vdots \\ \dfrac{\partial g_k}{\partial \theta_1}(\theta) & \ldots & \dfrac{\partial g_k}{\partial \theta_n}(\theta) \\ \end{bmatrix} \begin{bmatrix} \dot{\theta_1} \\ \vdots \\ \dot{\theta_n} \\ \end{bmatrix} = 0.$$

Let

$$\mathbf{A}(\theta) = \begin{bmatrix} \dfrac{\partial g_1}{\partial \theta_1}(\theta) & \ldots & \dfrac{\partial g_1}{\partial \theta_n}(\theta) \\ \vdots & \ddots & \vdots \\ \dfrac{\partial g_k}{\partial \theta_1}(\theta) & \ldots & \dfrac{\partial g_k}{\partial \theta_n}(\theta) \\ \end{bmatrix},$$

then we have

$$\mathbf{A}(\theta)\dot{\theta} = 0.$$

Velocity constraints like this are called Pfaffian constraints.

If the velocity constraints cannot be intergated to given an equivalent configuration constraint, then these are nonholonomic constraints. A nonholonomic constraint reduces the space of possible velocities of the robot, but it does not reduce the space of configurations.

In conclusion, holonomic constraints are constraints on configuartion. Nonholonomic constraints are constraints on velocity. Pfaffian constraints can be holonomic configuration constraints or only nonholonomic velocity constraints.

2.5 Task Space and Workspace

• Task space: The space in which the robot’s task is naturally expressed.
• Workspace: A specification of the reachable configurations of the end-effector.

3.2.1 Rotation Matrices

Consider the following space frame $s$ and body frame $b$.

We can express the orientation of the frame $b$ relative to $s$ by writing the unit coordinate axes of frame $b$ in the coordinates of frame $s$

$$\mathbf{\hat{x}_b} = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} \quad \mathbf{\hat{y}_b} = \begin{bmatrix} -1 \\ 0 \\ 0 \end{bmatrix} \quad \mathbf{\hat{z}_b} = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}$$

Then write the column vectors side by side to obtain the rotation matrix $R_{sb}$.

$$R_{sb} = \begin{bmatrix}\mathbf{\hat{x}_b} & \mathbf{\hat{y}_b} & \mathbf{\hat{z}_b}\end{bmatrix} = \begin{bmatrix}0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1\end{bmatrix}$$

The rotation matrix must be subject to

$$R^T R = I, \quad I = \begin{bmatrix}1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix},$$

which ensures the determinant of $R$ is $1$ since we only use right handed frames.

The set of all rotation matrices is called the special orthogonal group. The special orthogonal group $SO(3)$ is the set of all $3 \times 3$ real matrices $R$ satisifying:

• $R^T R = I$ and
• $\det R = 1$.

Some properties of rotation matrices:

• inverse: $R^{-1} = R^T \in SO(3)$
• closure: $R_1R_2 \in SO(3)$
• associative: $(R_1R_2)R_3 = R_1(R_2R_3)$
• not commutative: $R_1R_2 \neq R_2R_1$
• $x \in \mathbb{R}^3, \Vert Rx \Vert = \Vert x \Vert$

Common uses of rotation matrices:

• Represent an orientation:

$$R_{cs} = R_{sc}^T = R_{sc}^{-1}$$

• Change reference frame:

$$R_{sc} = R_{sb}R_{bc}$$

$$p_s = R_{sb}p_b$$

• Rotate a vector or frame:

$$R_{sb} = R = \mathrm{Rot}(\hat{\mathbf{z}}, 90^\circ)$$

$$p_s' = Rp_s$$

$$R_{sc'} = RR_{sc}\quad (\text{rotation about } \hat{\mathbf{z}}_s)$$

$$R_{sc''} = R_{sc}R\quad (\text{rotation about } \hat{\mathbf{z}}_c)$$

3.2.2 Angular Velocities

Any angular velocity can be represented by a rotation axis and the speed of rotation about it.

• Rotation axis: $\hat{\omega}_s$
• Rate of rotation: $\dot{\theta}$

The angular velocity vector in the $s$ frame is obtained by

$$\omega_s = \hat{\omega}_s \dot{\theta}$$

The linear velocity of the x-axis is calculated as

$$\dot{\hat{\mathbf{x}}}_b = \omega_s \times \mathbf{\hat{x}_b}$$

Similarly, we have

$$\dot{\hat{\mathbf{y}}}_b = \omega_s \times \mathbf{\hat{y}_b}$$

$$\dot{\hat{\mathbf{z}}}_b = \omega_s \times \mathbf{\hat{z}_b}$$

Define a bracket notation

$$x \times y = [x]y$$

where

$$x = \begin{bmatrix}x_1 \\ x_2 \\ x_3\end{bmatrix} \in \mathbb{R}^3, \quad [x] = \begin{bmatrix}0 & -x_3 & x_2 \\ x_3 & 0 & -x1 \\ -x_2 & x_1 & 0 \end{bmatrix}.$$

Since $[x] = -[x]^T$, $[x]$ is called a skew-symmetric matrix. The set of all $3 \times 3$ skew-symmetric real matrices is called $so(3)$.

With the bracket notation, we have

$$\dot{R}_{sb} = \begin{bmatrix}\dot{\hat{\mathbf{x}}}_b & \dot{\hat{\mathbf{y}}}_b & \dot{\hat{\mathbf{z}}}_b\end{bmatrix} = [\omega_s] R_{sb}$$

The angular velocity vector can also be expressed in the body frame $b$.

$$\omega_b = R_{bs} \omega_s = R_{sb}^{-1} \omega_s = R_{sb}^T \omega_s$$

Or put simply,

$$\omega_b = R^{-1}\omega_s = R^T \omega_s, \quad \omega_s = R \omega_b,$$

$$[\omega_b] = R^{-1}\dot{R} = R^T \dot{R}, \quad [\omega_s] = \dot{R}R^{-1} = \dot{R}R^T$$

3.2.3 Exponential Coordinates of Rotation

Besides the rotation matrix, oritentation has an alternative representation, the 3-parameter representation.

Any orientation can be achieved from an initial orientation aligned with the space frame by rotating about some unit axis $\hat{\omega}$ by a particular angle $\theta$. The 3-vector $\hat{\omega}\theta$ is called the exponential coordinates, representing the orientation of one frame relative to another.

Consider only one coordinate axis $p$. We want to determine the final location of the vector $p(\theta)$ if it rotates an angle $\theta$ about the rotation axis $\hat{\omega}$.

$$\dot{p}(t) = \hat{\omega} \times p(t) = [\hat{\omega}]p(t)$$

$$x \in \mathbb{R}: \dot{x}(t) = ax(t) \to x(t) = e^{at}x(0)$$

$$e^{at} = 1 + at + \frac{(at)^2}{2!} + \frac{(at)^3}{3!} + \cdots$$

Generalize to vector linear differential equation:

$$x \in \mathbb{R}^n: \dot{x}(t) = Ax(t) \to x(t) = e^{At}x(0)$$

$$e^{At} = 1 + At + \frac{(At)^2}{2!} + \frac{(At)^3}{3!} + \cdots$$

$$p(t) = e^{[\hat{\omega}]\theta}p(0) = e^{[\hat{\omega}\theta] }p(0)$$

Since $[\hat{\omega}\theta] \in so(3)$, the series expansion has a simple closed form:

$$\mathrm{Rot}(\hat{\omega}, \theta) = e^{[\hat{\omega}]\theta} = I + \sin\theta[\hat{\omega}] + (1 - \cos\theta) [\hat{\omega}]^2 \in SO(3)$$

The matrix exponentiation integrates the angular velocity $\hat{\omega}$ for time $\theta$ seconds, going from the identity matrix to the final rotation matrix $R$.

$$\exp: \quad [\hat{\omega}]\theta \in so(3) \quad \to \quad R \in SO(3)$$

Inversely, the matrix logarithm takes a rotation matrix and returns the skew-symmetric matrix representation of the exponential coordinates that achieve it, starting from the identity orientation.

$$\log: \quad R \in SO(3) \quad \to \quad [\hat{\omega}]\theta \in so(3)$$

For a revolute joint, the unit angular velocity $\hat{\omega}$ represents the axis of rotation of the joint, and $\theta$ represents how far that joint has been rotated.

3.3.1 Homogeneous Transformation Matrices

The configuration of a body frame $b$ in the fixed space frame $s$ can be represented by specifying the position $p$ of the frame $b$ and the rotation matrix $R$ specifying the orientation of $b$, both in the $s$ coordinates.

We gather $p$ and $R$ to get a $4 \times 4$ matrix $T$, called a homogeneous transformation matrix, or transformation matrix.

$$T = \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix} = \begin{bmatrix}r_{11} & r_{12} & r_{13} & p_1 \\ r_{21} & r_{22} & r_{23} & p_2 \\ r_{31} & r_{32} & r_{33} & p_3 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

The set of all transformation matrices is called the special Euclidean group $SE(3)$. The special Euclidean group
$SE(3)$ is the set of all $4\times4$ real matrices $T$ of the form

$$T = \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix} = \begin{bmatrix}r_{11} & r_{12} & r_{13} & p_1 \\ r_{21} & r_{22} & r_{23} & p_2 \\ r_{31} & r_{32} & r_{33} & p_3 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

where $R \in SO(3), p \in \mathbb{R}^3$.

Some properties of transformation matrices:

• inverse: $T^{-1} = \begin{bmatrix}R^T & -R^Tp \\ 0 & 1\end{bmatrix} \in SE(3)$
• closure: $T_1T_2 \in SE(3)$
• associative: $(T_1T_2)T_3 = T_1(T_2T_3)$
• not commutative: $T_1T_2 \neq T_2T_1$

Common uses of transformation matrices:

• Represent a configuration:

$$T_{sb} = \begin{bmatrix}R_{sb} & p \\ 0 & 1\end{bmatrix}$$

$$T_{bs} = T_{sb}^{-1} = \begin{bmatrix}R_{sb}^T & -R_{sb}^Tp \\ 0 & 1\end{bmatrix}$$

• Change reference frame:

$$T_{sc} = T_{sb}T_{bc}$$

$$\begin{bmatrix}p_s \\ 1\end{bmatrix} = T_{sb} \begin{bmatrix}p_b \\ 1\end{bmatrix}$$

• Displace a vector or frame:

$$T = \mathrm{Trans}(p)\mathrm{Rot}(\hat{\omega},\theta) = \begin{bmatrix}1 & 0 & 0 & p_x \\ 0 & 1 & 0 & p_y \\ 0 & 0 & 1 & p_z \\ 0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix} & & & 0 \\ & e^{[\hat{\omega}]\theta} & & 0 \\ & & & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

$$TT_{sb} = \mathrm{Trans}(p)\mathrm{Rot}(\hat{\omega},\theta)T_{sb}\quad (p, \hat{\omega} \text{ in } \{s\})$$

$$T_{sb}T = T_{sb}\mathrm{Trans}(p)\mathrm{Rot}(\hat{\omega},\theta)\quad (p, \hat{\omega} \text{ in } \{b\})$$

3.3.2 Twists

Any rigid-body velocity consists of a linear component and an angular component, and is equivalent to the instantaneous velocity about some screw axis.

The screw axis is defined by a point $q$ on the axis, a unit vector $\hat{s}$ in the direction of the axis, and the pitch of the screw $h$.

$$h = \mathrm{pitch} = \frac{\text{linear speed}}{\text{angular speed}}$$

The screw axis defines the direction the body is moving, and $\dot{\theta}$ is a scalar indicating how fast the body rotates about the screw.

We choose a reference frame, and define the screw axis $\mathcal{S}$ as a 6-vector in that frame’s coordinates.

$$\mathcal{S} = \begin{bmatrix}\mathcal{S_w} \\ \mathcal{S_v}\end{bmatrix} = \begin{bmatrix}\text{angular velocity when } \dot{\theta} = 1 \\ \text{linear velocity of the origin when } \dot{\theta} = 1 \end{bmatrix}$$

The linear velocity of the origin is a combination of two terms: the linear velocity due to translation along the screw axis, and the linear velocity due to rotation about the screw axis.

$$\text{linear velocity of the origin} = (h\hat{s} - \hat{s}\times q) \dot{\theta}$$

The twist, which is a full representation of angular and linear velocity, is obtained by multiplying the representation of the screw axis $\mathcal{S}$ by the scalar rate of rotation $\dot{\theta}$.

$$\mathcal{V} = \begin{bmatrix}\mathcal{w} \\ \mathcal{v}\end{bmatrix} = \mathcal{S}\dot{\theta}$$

• If pitch $h$ is infinite:

$$\mathcal{S_w} = 0, \Vert\mathcal{S_v}\Vert = 1, \dot{\theta} = \text{linear speed}$$

• If pitch $h$ is finite:

$$\Vert\mathcal{S_w}\Vert = 1, \dot{\theta} = \text{rotational speed}$$

If $\mathcal{S}$ is defined in $\{b\}$, $\mathcal{V_b} = (\mathcal{w_b}, \mathcal{v_b}) = \mathcal{S}\dot{\theta}$ is the body twist. If $\mathcal{S}$ is defined in $\{s\}$, $\mathcal{V_s} = (\mathcal{w_s}, \mathcal{v_s}) = \mathcal{S}\dot{\theta}$ is the spatial twist.

In conclusion, a twist is a 6-vector consisting of a 3-vector expressing the angular velocity and a 3-vector expressing the linear velocity. Both of these are written in coordinates of the same frame, and the linear velocity refers to the linear velocity of a point at the origin of that frame.

To change the frame of representation of a twist, we use a $6 \times 6$ adjoint representation of a transformation matrix.

$$T = \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix} \quad \Rightarrow \quad [A\mathrm{d}_T] = \begin{bmatrix}R & 0 \\ [p]R & R\end{bmatrix} \in \mathbb{R}^{6 \times 6}$$

$$\mathcal{V_a} = [A\mathrm{d}_{T_{ab}}] \mathcal{V_b}$$

Recall that

$$[\omega_b] = R^{-1}\dot{R} \in so(3)$$

$$[\omega_s] = \dot{R}R^{-1} \in so(3)$$

Similarly, we have

$$[\mathcal{V_b}] = T^{-1}\dot{T} = \begin{bmatrix}[\omega_b] & v_b \\ 0 & 0\end{bmatrix} \in se(3)$$

$$[\mathcal{V_s}] = \dot{T}T^{-1} = \begin{bmatrix}[\omega_s] & v_s \\ 0 & 0\end{bmatrix} \in se(3)$$

where $se(3)$ is the space of $4\times 4$ matrix representations of twists. The set of all $4\times 4$ real matrices with a $3\times3$ $so(3)$ matrix at the top left and four zeros in the bottom row is called $se(3)$.

3.3.3 Exponential Coordinates of Rigid-Body Motion

The matrix exponential for rigid-body motions also has closed-form solutions.

• If the screw axis is a pure translation with no rotation, $\mathcal{S_w} = 0, \Vert\mathcal{S_v}\Vert=1$, and $\theta$ refers to the linear distance traveled. The solution is:

$$e^{[\mathcal{S}]\theta} = \begin{bmatrix}I & \mathcal{S_v}\theta \\ 0 & 1\end{bmatrix}$$

• If the screw axis has rotation, $\Vert \mathcal{S_w} \Vert = 1$. The solution is:

$$e^{[\mathcal{S}]\theta} = \begin{bmatrix}e^{[\mathcal{S_w}]\theta} & (I\theta + (1 - \cos\theta)[\mathcal{S_w}] + (\theta - \sin\theta)[\mathcal{S_w}]^2)\mathcal{S_v} \\ 0 & 1\end{bmatrix}$$

If $\mathcal{S}$ is expressed in $\{b\}$:

$$T_{sb'} = T_{sb} e^{[\mathcal{S_b}]\theta}$$

If $\mathcal{S}$ is expressed in $\{s\}$:

$$T_{sb'} = e^{[\mathcal{S_s}]\theta} T_{sb}$$

Each single-degree-of-freedom joint of a robot, such as a revolute joint, a prismatic joint, or a helical joint, has a joint axis defined by a screw axis.

3.4 Wrenches

A line of force $f_b$ acts at the point $r_b$, both represented in the $\{b\}$ frame. This force induces a 3-vector torque, or moment, about the frame $\{b\}$.

$$m_b = r_b \times f_b$$

We can package the moment and the force together in a single 6-vector called the wrench.

$$\mathcal{F_b} = \begin{bmatrix}m_b \\ f_b\end{bmatrix}$$

The dot product of a twist and a wrench is power, which must be the same whether the wrench and twist are represented in the $\{b\}$ frame or in the $\{s\}$ frame.

$$\begin{aligned} &\mathcal{V_s}^T\mathcal{F}_s = \mathcal{V_b}^T\mathcal{F_b} = \mathrm{power} \\ \Rightarrow \quad&\mathcal{V_s}^T\mathcal{F}_s = ([A\mathrm{d}_{T_{bs}}]\mathcal{V_s})^T\mathcal{F_b} \\ \Rightarrow \quad & \mathcal{V_s}^T\mathcal{F}_s = \mathcal{V_s}^T[A\mathrm{d}_{T_{bs}}]^T\mathcal{F_b} \\ \Rightarrow \quad & \mathcal{F}_s = [A\mathrm{d}_{T_{bs}}]^T\mathcal{F_b} \end{aligned}$$

We have changed the coordinate frame of the wrench from the $\{b\}$ frame to the $\{s\}$ frame.

An example:

$$\mathcal{F_a} = \begin{bmatrix}m_a \\ f_a\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ -mg \\ 0\end{bmatrix}$$

$$\mathcal{T_af} = \begin{bmatrix}1 & 0 & 0 & -L \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

$$\mathcal{F_f} = \begin{bmatrix}m_f \\ f_f\end{bmatrix} = [A\mathrm{d}_{T_{af}}]^T\mathcal{F_a} = \begin{bmatrix}0 \\ 0 \\ -mgL \\ 0 \\ -mg \\ 0\end{bmatrix}$$

Chapter 4: Forward Kinematics

The forward kinematics problem is to find the configuration of the $\{b\}$ frame relative to the $\{s\}$ frame given the vector of joint angles $\theta$. Or put simply: Given $\theta$, find $T(\theta)$.

4.1.1 Product of Exponentials Formula in the Space Frame

This robot has three joints: a revolute joint, a prismatic joint, and another revolute joint. The 3-vector $\theta$ is a list of the three joint variables.

If we set all of the joint variables equal to zero, the robot is in its home position.

$$\theta = \begin{bmatrix}\theta_1 \\ \theta_2 \\ \theta_3\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$$

$$T(\theta) = M = \begin{bmatrix}1 & 0 & 0 & 3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

Now we rotate joint 3 by $\pi/4$ radians.

$$\theta = \begin{bmatrix}\theta_1 \\ \theta_2 \\ \theta_3\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ \pi/4\end{bmatrix}$$

Since it is a revolute joint with no translational motion, the screw axis has zero pitch. We represent the axis in the $\{s\}$ frame.

$$\mathcal{S_3} = \begin{bmatrix}\omega_3 \\ v_3\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ -2 \\ 0\end{bmatrix}$$

$$T(\theta) = e^{[\mathcal{S_3}]\theta_3}M = \begin{bmatrix}0.71 & -0.71 & 0 & 2.71 \\ 0.71 & -0.71 & 0 & 0.71 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

Now suppose we change joint 2, extending it by $0.5$ units of distance.

$$\theta = \begin{bmatrix}\theta_1 \\ \theta_2 \\ \theta_3\end{bmatrix} = \begin{bmatrix}0 \\ 0.5 \\ \pi/4\end{bmatrix}$$

The screw axis $\mathcal{S_2}$ is defined as:

$$\mathcal{S_2} = \begin{bmatrix}\omega_2 \\ v_2\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 1 \\ 0 \\ 0\end{bmatrix}$$

$$T(\theta) = e^{[\mathcal{S_2}]\theta_2}e^{[\mathcal{S_3}]\theta_3}M = \begin{bmatrix}0.71 & -0.71 & 0 & 3.21 \\ 0.71 & 0.71 & 0 & 0.71 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

Finally, we rotate joint 1 by $\pi/6$.

$$\theta = \begin{bmatrix}\theta_1 \\ \theta_2 \\ \theta_3\end{bmatrix} = \begin{bmatrix}\pi/6 \\ 0.5 \\ \pi/4\end{bmatrix}$$

$$\mathcal{S_1} = \begin{bmatrix}\omega_1 \\ v_1\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

$$T(\theta) = e^{[\mathcal{S_1}]\theta_1}e^{[\mathcal{S_2}]\theta_2}e^{[\mathcal{S_3}]\theta_3}M = \begin{bmatrix}0.26 & -0.97 & 0 & 2.42 \\ 0.97 & 0.26 & 0 & 2.22 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

Forward kinetics:

1. Let $M$ be the transformation matrix of the end-effector frame $\{b\}$ when $\theta = 0$.
2. Find the $\{s\}$-frame screw axes $\mathcal{S_1}, \dots, \mathcal{S_n}$ for each of the $n$ joint axes when $\theta = 0$.
3. Given $\theta$, evaluate the product of exponentials (PoE) formula in the space frame:

$$T(\theta) = e^{[\mathcal{S_1}]\theta_1}e^{[\mathcal{S_2}]\theta_2}\dots e^{[\mathcal{S_n}]\theta_n}M$$

4.1.2 Product of Exponentials Formula in the End-Effector Frame

We use the same example in 4.1.1.

1. Zero configuration

$$T(\theta) = M = \begin{bmatrix}1 & 0 & 0 & 3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

2. Rotate joint 1 by an angle $\theta_1$

We represent the screw axis in the $\{b\}$-frame as $\mathcal{B_1}$.

$$\mathcal{B_1} = \begin{bmatrix}\omega_1 \\ v_1\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ 3 \\ 0\end{bmatrix}$$

$$T(\theta) = Me^{[\mathcal{B_1}]\theta_1}$$

3. Extend joint 2 by $\theta_2$ units of distance

$$\mathcal{B_2} = \begin{bmatrix}\omega_2 \\ v_2\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 1 \\ 0 \\ 0\end{bmatrix}$$

$$T(\theta) = Me^{[\mathcal{B_1}]\theta_1}e^{[\mathcal{B_2}]\theta_2}$$

4. Rotate joint 3 by $\theta_3$

$$\mathcal{B_3} = \begin{bmatrix}\omega_3 \\ v_3\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ 1 \\ 0\end{bmatrix}$$

$$T(\theta) = Me^{[\mathcal{B_1}]\theta_1}e^{[\mathcal{B_2}]\theta_2}e^{[\mathcal{B_3}]\theta_3}$$

Forward kinetics:

1. Let $M$ be the transformation matrix of the end-effector frame $\{b\}$ when $\theta = 0$.
2. Find the $\{b\}$-frame screw axes $\mathcal{B_1}, \dots, \mathcal{B_n}$ for each of the $n$ joint axes when $\theta = 0$.
3. Given $\theta$, evaluate the product of exponentials (PoE) formula in the end-effector frame:

$$T(\theta) = Me^{[\mathcal{B_1}]\theta_1}e^{[\mathcal{B_2}]\theta_2}\dots e^{[\mathcal{B_n}]\theta_n}$$

Forward Kinematics Example

To solve the forward kinematics, we need to find $M$, the configuration of the $\{b\}$-frame, and the joint screw axes when the arm is at its zero configuration. Then we can use the product of exponentials formulas.

$$M = \begin{bmatrix}0 & -1 & 0 & 19 \\ -1 & 0 & 0 & 0 \\ 0 & 0 & -1 & -3 \\ 0 & 0 & 0 & 1\end{bmatrix}$$

• joint 1:

$$\mathcal{S_1} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0\end{bmatrix} \quad \mathcal{B_1} = \begin{bmatrix}0 \\ 0 \\ -1 \\ -19 \\ 0 \\ 0\end{bmatrix}$$

• joint 2:

$$\mathcal{S_2} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ -10 \\ 0\end{bmatrix} \quad \mathcal{B_2} = \begin{bmatrix}0 \\ 0 \\ -1 \\ -9 \\ 0 \\ 0\end{bmatrix}$$

• joint 3:

$$\mathcal{S_2} = \begin{bmatrix}0 \\ 0 \\ 1 \\ 0 \\ -19 \\ 0\end{bmatrix} \quad \mathcal{B_2} = \begin{bmatrix}0 \\ 0 \\ -1 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

• joint 4:

$$\mathcal{S_2} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 1\end{bmatrix} \quad \mathcal{B_2} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ 0 \\ -1\end{bmatrix}$$

Chapter 6: Inverse Kinematics of Open Chains

• FK: Given $\theta$, find $T(\theta) \in SE(3)$.

• IK: Given $X \in SE(3)$, find $\theta$ such that $T(\theta) = X$.

  • May be zero, one, or multiple solutions $\theta$.

• Solution methods of IK:

  • analytic closed-form
  • iterative numerical methods

6.2 Numerical Inverse Kinematics

For simplicity, we will start with a coordinate-based forward kinematics, rather than a transformation matrix-based.

$$\begin{aligned} \text{FK: }\quad& \theta \to T(\theta) \in SE(3) \\ \text{coord FK: }\quad& \theta \to f(\theta) \in \mathbb{R}^m \\ \text{coord IK: }\quad& \text{find } \theta_d \text{ such that} \\ \quad& x_d - f(\theta_d) = 0 \end{aligned}$$

where $f(\theta)$ is a minimal set of coordinates describing the end-effector configuration.

Write the Taylor expansion of the function $f(\theta)$ around $\theta_d$.

$$x_d = f(\theta_d) = f(\theta_i) + \underbrace{\frac{\partial f}{\partial \theta}\vert_{\theta^i}}_{J(\theta^i)}\underbrace{(\theta_d - \theta^i)}_{\Delta \theta} + \mathrm{h.o.t.}$$

Ignore the higher-order terms, then we have:

$$\begin{equation} x_d - f(\theta^i) = J(\theta^i) \Delta \theta \end{equation}$$

If $J$ is invertible, we can solve for $\Delta\theta$:

$$J^{-1}(\theta^i)(x_d - f(\theta^i)) = \Delta\theta$$

If $J$ is not invertible, because it is not square or because the robot is at a singularity, we premultiply (1) by the pseudoinverse of $J$.

$$J^\dagger(\theta^i)(x_d - f(\theta^i)) = \Delta\theta^*$$

• If there exists a $\Delta\theta$ exactly satisfying (1), then $\Delta\theta^*$ has the smallest 2-norm among all solutions.
• If there is no $\Delta\theta$ exactly satisfying (1), then $\Delta\theta^*$ comes closest in the 2-norm sense.

Newton-Raphson numerical inverse kinematics:

(a) Initialization: Given $x_d \in \mathbb{R}^m$ and an initial guess $\theta^0 \in \mathbb{R}^n$, set $i = 0$.

(b) Set $e = x_d - f(\theta^i)$. While $\Vert e \Vert > \epsilon$ for some small $\epsilon$:

• Set $\theta^{i+1} = \theta^i + J^\dagger(\theta^i)e$.
• Increment $i$.

This is the Newton-Raphson numerical algorithm for inverse kinematics when the end-effector configuration is represented by a minimum set of coordinates $x_d$. Now, we will modify the algorithm so that the desired end-effector configuration is described by the transformation matrix $T_{sd}$.

$$T_{bd}(\theta^i) = T_{sb}^{-1}(\theta^i) T_{sd} = T_{bs}(\theta^i)T_{sd}$$

$$[\mathcal{V_b}] = \log T_{bd}(\theta^i)$$

Newton-Raphson numerical inverse kinematics:

(a) Initialization: Given $T_{sd} \in \mathbb{R}^m$ and an initial guess $\theta^0 \in \mathbb{R}^n$, set $i = 0$.

(b) Set $[\mathcal{V_b}] = \log(T_{sb}^{-1}(\theta^i)T_{sd})$. While $\Vert \omega_b \Vert > \epsilon_\omega$ or $\Vert v_b \Vert > \epsilon_v$ for some small $\epsilon_\omega$, $\epsilon_v$:

• Set $\theta^{i+1} = \theta^i + J_b^\dagger(\theta^i)\mathcal{V_b}$.
• Increment $i$.

Chapter 11: Control System

11.1 Control System Overview

Control objetives:

• Motion control
• Force control
• Hybrid motion-force control
• Impedance control

A simplified diagram of the control system:

11.2.1 Error Response

• desired motion: $\theta_d(t)$
• actual motion: $\theta(t)$
• error: $\theta_e(t) = \theta_d(t) - \theta(t)$

The error dynamics are the equations that describe the evolution of $\theta_e$ of the control system. A good controller would control error dynamics that drive any intial error to zero, or nearly zero, as quickly as possible.

Unit step error response: the evolution of the error when

$$\theta_e(0) = 1$$

$$\dot{\theta}_e(0) = \ddot{\theta}_e(0) = \cdots = 0$$

• steady-state error response: $e_{ss}$
• transient error response: overshoot, settling time

11.2.2 Linear Error Dynamics

• Nonhomogeneous differential equation:

$$a_p\theta_e^{(p)} + a_{p-1}\theta_d^{(p-1)} + \cdots + a_2\ddot{\theta}_e + a_1\dot{\theta}_e + a_0\theta_e = c$$

• Homogeneous differential equation:

$$a_p\theta_e^{(p)} + a_{p-1}\theta_d^{(p-1)} + \cdots + a_2\ddot{\theta}_e + a_1\dot{\theta}_e + a_0\theta_e = 0$$

Some equivalent forms:

$$\theta_e^{(p)} + a_{p-1}'\theta_d^{(p-1)} + \cdots + a_2'\ddot{\theta}_e + a_1'\dot{\theta}_e + a_0'\theta_e = 0$$

$$\theta_e^{(p)} = -a_{p-1}'\theta_d^{(p-1)} - \cdots - a_2'\ddot{\theta}_e - a_1'\dot{\theta}_e - a_0'\theta_e$$

This single $p$th-order linear differential equation can be expressed as $p$ first-order differential equations. Define state vector $x = (x_1, x_2, \dots, x_p)$ and

$$x_1 = \theta_e \\ x_2 = \dot{x}_1 = \dot{\theta}_e \\ \dots \\ x_p = \dot{x}_{p-1} = \theta_e^{(p-1)}$$

So that we have

$$\dot{x}_p = -a_0'x_1 - a_1'x_2 - \cdots - a_{p-1}'x_p$$

We can write the $p$th-order differential equation as a first-order vector differential equation.

$$\dot{x}(t) = Ax(t)$$

where

$$A = \begin{bmatrix} 0 & 1 & 0 & \cdots & 0 & 0 \\ 0 & 0 & 1 & \cdots & 0 & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ 0 & 0 & 0 & \cdots & 1 & 0 \\ 0 & 0 & 0 & \cdots & 0 & 1 \\ -a_0' & -a_1' & -a_2' & \ldots & -a_{p-2}' & -a_{p-1}' \end{bmatrix} \in \mathbb{R}^{p \times p}$$

The solution is given by

$$x(t) = e^{At}x(0)$$

To understand the character of error response, we will study the eigenvalues of $A$. These eigenvalues will determine the initial error $\theta_e(0)$ grows or shrinks with time.

If $\mathrm{Re}(s) < 0$ for all eigenvalues $s$ of $A$, then the error dynamics are stable., i.e., the error decays to zero.

The eigenvalues $s$ are the roots of the characteristic equation:

$$\det (sI - A) = s^p + a_{p-1}s^{p-1} + \cdots + a_2's^2 + a_1's + a_0' = 0$$

A necessary condition for stability is that all $a'_i$ are positive. This condition is also sufficient for stability for first- and second-order error differential equations, but not for third-order or higher.

11.2.2.1 First-Order Error Dynamics

$$\dot{\theta}_e(t) + \frac{k}{b}\theta_e(t) = 0$$

Set $\mathfrak{m} = 0$ gives us this first-order differential equation, which means the force due to the spring and the force due to the damper always sum to zero.

Define time constant $\mathfrak{t} = b / k$, and we have

$$\dot{\theta}_e(t) + \frac{1}{\mathfrak{t}}\theta_e(t) = 0$$

This is the standard first-order form of error dynamics.

The error differential equation is stable if the time constant $\mathfrak{t}$ is positive. The solution to the equation is

$$\theta_e(t) = e^{-t/\mathfrak{t}}\theta_e(0)$$

The figure shows the unit step error reponse, which is a exponential decaying of time.

11.2.2.2 Second-Order Error Dynamics

Follow the mass-spring-damper analogy for the error dynamics of a controlled single joint robot.

$$\ddot{\theta}_e(t) + \frac{b}{\mathfrak{m}}\dot{\theta}_e(t) + \frac{k}{\mathfrak{m}}\theta_e(t) = 0$$

Assuming $\mathfrak{m}, b, k > 0$, the error dynamics are stable and the error will decay to zero.

Define the natural frequency

$$\omega_n = \sqrt{k / \mathfrak{m}}$$

and the damping ratio

$$\zeta = b / (2\sqrt{k\mathfrak{m}})$$

Then we can rewrite the differential equation as

$$\ddot{\theta}_e(t) + 2\zeta\omega_n\dot{\theta}_e(t) + \omega_n^2\theta_e(t) = 0$$

This is the standard second-order form of error dynamics.

The characteristic equation of this differential equation is

$$s^2 + + 2\zeta\omega_ns + \omega_n^2 = 0$$

The roots of the quadratic equation is

$$s_{1,2} = -\zeta\omega_n \pm \omega_n \sqrt{\zeta^2-1}$$

• If $\zeta \ge 1$, then $s_1$ and $s_2$ are real numbers.
• If $\zeta < 1$, then the roots are complex conjugates.

1. $\zeta > 1$: Overdamped

$$\theta_e(t) = c_1 e^{s_1t} + c_2 e^{s_2t}$$

where

$$s_1 = -\zeta\omega_n + \omega_n\sqrt{\zeta^2-1} \\ s_2 = -\zeta\omega_n - \omega_n\sqrt{\zeta^2-1}$$

• $\zeta = 1$: Critically damped

$$\theta_e(t) = (c_1 + c_2t) e^{-\omega_nt}$$

and

$$s_{1,2} = -\omega_n$$

• $\zeta < 1$: Underdamped

$$\theta_e(t) = (c_1 \cos \omega_dt + c_2 \sin \omega_dt) e^{-\zeta\omega_nt}$$

The frequency of the sinusoid is damped natural frequency.

$$\omega_d = \omega_n \sqrt{1 - \zeta^2}$$

$$s_{1,2} = -\zeta\omega_n \pm j \omega_d$$

11.3 Motion Control with Velocity Inputs

When we model a robot, we usually assume that we have control of the forces and torques at the joints, and the resulting motion of the robot is determined by its dynamics.

But it’s simpler to ignore the dynamics and assume that we have control of the joint velocities.

P control

• Open-loop control:

$$\dot{\theta}(t) = \dot{\theta}_d(t)$$

• Closed-loop control:

$$\dot{\theta}(t) = K_p(\theta_d(t) - \theta(t)) = K_p \theta_e(t)$$

where $K_p > 0$ is the proportional gain. This type of control is called Proportional (P) control.

Setpoint control

$$\dot{\theta}_d(t) = 0$$

$$\Rightarrow \quad \dot{\theta}_e(t) = \dot{\theta}_d(t) - \dot{\theta}(t) = - \dot{\theta}(t)$$

$$\Rightarrow \quad \dot{\theta}_e(t) = -K_p \theta_e(t)$$

$$\Rightarrow \quad \dot{\theta}_e(t) + K_p \theta_e(t) = 0$$

$$\Rightarrow \quad \theta_e(t) = e^{-t/\mathfrak{t}}\theta_e(0) \, \text{ where } \mathfrak{t} = 1 / K_p$$

The larger $K_p$, the faster the error converges to zero.

Constant desired velocity

$$\dot{\theta}_d(t) = c$$

$$\Rightarrow \quad \dot{\theta}_e(t) = \dot{\theta}_d(t) - \dot{\theta}(t) = c - K_p \theta_e(t)$$

$$\Rightarrow \quad \dot{\theta}_e(t) + K_p \theta_e(t) = c$$

$$\Rightarrow \quad \theta_e(t) = \frac{c}{K_p} + \left(\theta_e(0) - \frac{c}{K_p}\right) e^{-K_p t}$$

P controller needs error to command a non-zero velocity.

PI control

$$\dot{\theta}(t) = K_p \theta_e(t) + K_i \int_0^t \theta_e(t) \,\mathrm{d}t$$

This controller is called Proportional-Integral (PI) control.

$$\dot{\theta}_e(t) + K_p \theta_e(t) + K_i \int_0^t \theta_e(t) \,\mathrm{d}t = c$$

$$\Rightarrow \quad \ddot{\theta}_e(t) + K_p \dot{\theta}_e(t) + K_i \theta_e(t) = 0$$

Now it is a standard second-order homogeneous differential equation, where

$$\zeta = K_p / (2\sqrt{K_i}) \\ \omega_n = \sqrt{K_i}$$

P control eliminates steady state error for setpoint control, and PI control eliminates steady state error for any trajectory with constant velocity.

$$\dot{\theta}(t) = \dot{\theta}_d(t) + K_p \theta_e(t) + K_i \int_0^t \theta_e(t) \,\mathrm{d}t$$

Task-space control with velocity inputs

Now we move to the task-space version of the PI controller.

The desired motion is $X_d \in SE(3)$, $[\mathcal{V_d}] = X_d^{-1}\dot{X}_d$, and the actual motion is $X \in SE(3)$, $[\mathcal{V_b}] = X^{-1}\dot{X}$.

$$\mathcal{V_b}(t) = [A\mathrm{d}_{X_{bd}}]\mathcal{V}_d(t) + K_p X_e(t) + K_i \int_0^t X_e(t) \,\mathrm{d}t$$

where

$$X_{bd} = X_{bs}X_{sd} = X^{-1}X_d \\ [X_e] = \log X_{bd} = \log(X^{-1}X_d)$$

and

$$\dot{\theta} = J_b^\dagger(\theta) \mathcal{V_b}$$

Decoupled task-space control with velocity inputs

We can also decouple the rotation error and the linear error.

$$X = (R, p):$$

$$\begin{bmatrix}\omega_b(t) \\ \dot{p}(t)\end{bmatrix} = \begin{bmatrix}R^T(t)R_d(t) & 0 \\ 0 & I\end{bmatrix} \begin{bmatrix}\omega_d(t) \\ \dot{p}_d(t)\end{bmatrix} + K_p X_e(t) + K_i \int_0^t X_e(t) \,\mathrm{d}t$$

where

$$X_e(t) = \begin{bmatrix}\omega_e(t) \\ p_d(t) - p(t)\end{bmatrix} \quad [\omega_e] = \log(R^T R_d)$$