SE2 & Forward Kinematics

In our previous blog on robotic grasping, we briefly broached the topic of Inverse Kinematics (IK) and Jacobians. In this blog, we will establish some fundamentals about the mathematics behind Robot Arm Kinematics that will allow us to dive deeper.

The SE(2) Group 

The Special Euclidean group (SE2) [group being a mathematical set and operations that satisfy some laws] can be used to represent the rigid transformations of a planar object, a 2D arm, on a plane. We are going to first use a 2D arm, composed of a set of joints and links and see how we can make its end-effector reach certain parts of the 2D workspace, before going on to see if we can do the same in 3D (the space occupied by real-world robotic arms).

Concretely, each joint of the 2D arm has a 2D coordinate frame attached to it, which is described relative to the 2D coordinate frame of the previous joint, all the way down from the end effector to the base of the robot. These coordinate transforms in 2D space can be represented by an SE2 matrix, which describes how a particular coordinate frame is rotated (by $\theta$) and translated (by a 2D vector $[x, y]$) relative to a previous coordinate frame.

The SE2 matrix is a 3x3 homogeneous transformation matrix of the form:

 $T = \begin{bmatrix}\cos(\theta) & -\sin(\theta) & x \\ \sin(\theta) & \cos(\theta) & y \\ 0 & 0 & 1 \\ \end{bmatrix} $

where $\theta$ is the rotation angle of the arm & $(x, y)$ represents the translation components in the 2D plane. 

The homogeneous matrix has rotation and translation components of the rigid body (in this case, a robotic arm) integrated into a single matrix. The additional bottom row $\begin{bmatrix}0 & 0 & 1 \end{bmatrix}$ represents the additional dimension that allows us to chain a sequence of poses (matrices) together by multiplication. 

To get more of an intuitive understanding of the SE2 matrix and how it can be used to transform a coordinate system, try playing around with the above app.

Pose Transformation Chain

We can now chain together a series of coordinate transforms (SE2 matrices) for a 3-link robot arm. We will now have 3 SE2 matrices - one describing the transform from the robot base to the first joint (between link 1 and link 2), a second SE2 matrix, describing the transform from the first joint to the second joint, and a third SE2 matrix describing the transform from the second joint to the third joint.

For a robotic arm with 3 joints:

1. The first joint rotates by $\theta_1$ and translates by $l_1$ (length of the first link).
2. The second joint rotates by $\theta_2$ and translates by $l_2$ (length of the second link)
3. The third joint rotates by $\theta_3$ and translates by $l_3$ (length of the third link)

The transformation for each joint can be represented as:

$T_1(\theta_1, l_1) = \begin{bmatrix} \cos(\theta_1) & -\sin(\theta_1) & l_1 \cos(\theta_1) \\ \sin(\theta_1) & \cos(\theta_1) & l_1 \sin(\theta_1) \\ 0 & 0 & 1 \\ \end{bmatrix}$

$T_2(\theta_2, l_2) = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) & l_2 \cos(\theta_2) \\ \sin(\theta_2) & \cos(\theta_2) & l_2 \sin(\theta_2) \\ 0 & 0 & 1 \\ \end{bmatrix}$

$T_3(\theta_3, l_3) = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) & l_3 \cos(\theta_3) \\ \sin(\theta_3) & \cos(\theta_3) & l_3 \sin(\theta_3) \\ 0 & 0 & 1 \\ \end{bmatrix}$

To find the position and orientation of the coordinate frame on the end-effector attached to the end of link 3 relative to the coordinate of the base, these transformations are multiplied together:

$T_{\text{end}} = T_1(\theta_1, l_1) \cdot T_2(\theta_2, l_2) \cdot T_3(\theta_3, l_3)$.

Play around with the parameters of animation below to get a more intuitive feel for this.

Forward Kinematics (FK) is the process of determining the position and orientation of a robot's end-effector given the joint parameters (angles and link lengths). So when we chain together SE(2) matrices, we effectively combine the individual transformations from each joint to express the final pose of the end-effector relative to the base. 

This is the crux of how a robot arm moves to get to a certain point in the space around it. We have discussed this with SE2 matrices for an animated 2D robot arm. For a real robot arm operating in 3D space, SE(2) matrices would need to be replaced by SE(3) matrices, which extend these concepts to three-dimensional transformations involving both rotation and translation. These are 4x4 homogeneous transformation matrices that include rotations and translations along all three axes, allowing for the representation of full 3D poses.