Product of Exponentials (PoE)

Product of Exponentials deliberately throws away the per-link frames. This method only requires 2 frames, the base frame and the end-effector frame.

Here are the 2 frames required:

• {s}: space frame (fixed, called base_frame in the other method)
• {b}: body frame (attached to the end-effector)

No frames in the middle arm. Instead:

• M is a single 4x4 matrix: the pose of {b} and {s} when all joints are at zero. Which is the home configuration.
• For each joint, we write down a screw axis $S_i$ describing its axis of motion in the space frame {s}, at home configuration. All six screw axes live in the same fixed frame.
• Forward Kinematics:

$$T(\theta )=e^{[S_1]{\theta }_1}\cdot e^{[S_2]{\theta }_2}\cdot \cdots \cdot e^{[S_6]{\theta }_6}\cdot M$$

• $T(\theta )$ is the output of the forward kinematics, it represents the 4x4 homogeneous transformation matrix describing the pose of the end-effector (TCP) in the space frame
• The 4x4 SE(3) matrix has this structure:

$$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$$

• where R is a 3x3 rotation matrix (TCP orientation) and p is a 3-vector (TCP position). So T packs both position and orientation of the TCP into one mathematical object.
• ${\theta }_1,{\theta }_2,...{\theta }_5,{\theta }_6$ are the joint angles, when we call rtde_r.getActualQ(), we get the 6 joint angles $[{\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4{\theta }_5,{\theta }_6]$. In the math we call it $\theta$, while in code we call it q
• Home Configuration is when all six angles are $0$: $\theta =(0,0,0,0,0,0)$
• Screw Axis is defined by a point q on the axis, a unit vector s in the direction of the axis, and the pitch h of the screw
• Screw axis is a fixed line in space, set at home configuration and never updated, even though the physical joint may have moved
• $h=\frac{v}{\omega }$, where $v$ is the linear speed, and $\omega$ is the angular speed
  • $h=0$ means object only twists, but doesn't slide
  • $h=Infinity$ means object slides, but doesn't twist
  • $0<h<Infinity$ means object gets true spiral motion

On UR5E, all joints are revolute (no helical screws), so h=0 for every joint

In my own words:

Step 0 - Apply $M$

Basically initially we are using the end-effector's home pose. The end-effector doesn't have a screw axis, only joints do. This is a pre-computed value. So its just a constant.

Step 1 - Apply $e^{[s_6]{\theta }_6}$

Next, we calculate how much the 6th joint has rotated about its own initial fixed screw axis. This relative rotation angle is substituted into ${\theta }_6$. The matrix $s_6$, screw axis, is fixed and already pre-calculated at the home pose, so are the other $s_5,s_4,s_3,s_2,s_1$. What the whole formula represent is a rotation of the whole pose by ${\theta }_i$ about joint i's screw axis.

Step 2 - Apply $e^{[s_5]{\theta }_5}$

Now, we rotate the current pose by $e^{[s_5]{\theta }_5}$ about joint 5's screw axis.

Step 3 - Apply $e^{[s_4]{\theta }_4}$

Now, we need to take into account the current pose of joint 4 about its screw axis. So again, we rotate the current pose by $e^{[s_4]{\theta }_4}$ about joint 4's screw axis.

This will continue until we multiply all of the rotations of joints between their poses and their respective screw axis. This results in obtaining the end-effector's TCP orientation and TCP position.