### Orientation Kinematics

In PART 1, I discussed how to control the position of a robot tool, and also how the orientation of a rigid body can be represented by a [3 x 3] rotation matrix. However, deriving an appropriate control method for a matrix is not always possible. The standard process for determining a control method is to look at the derivative, but that is

Since the matrix is an operator (i.e. mapping one space to another), it is not an appropriate way to represent the orientation state. The angular velocity at the tool, however, is surprisingly just as easy to compute as the linear velocity from part 1

Unfortunately, determining a control law with respect to angular velocity and rotation matrices is difficult. The relationship is not always straightforward between rotation matrices; state representations that have been shown to work are the classic yaw-pitch-roll, the Euler-Rodrigues axis angle, or my personal favorite: unit quaternions.

### Quaternions

*“I came later to see that, as far as the vector analysis I required was concerned, the quaternion was not only not required, but was a positive evil of no inconsiderable magnitude…” *Oliver Heaviside (famous mathematician, did not like quaternions)

The unit quaternion is a 4 dimensional hyper-complex representation of a rotation that sits in the SO(3) space. This is the terrifying product of a William Hamilton fever dream that very appropriately represents the SO(3) space in a single vector. One thing to note is that, because it’s a unit vector, it only has 3 degrees of freedom (since if we know 3 terms, we automatically know the 4th up to a sign ambiguity).

If we consider a rotation matrix as being the rotation about by , the unit quaternion is defined as

The relational aspect of rotation matrices where a single rotation can be decomposed into two rotations, i.e. continues into unit quaternions, except in this case we’re using the quaternion product. The popular notation in use, due to the hypercomplexity disallowing normal vector products, is

Here comes the useful part!. We can relate angular velocity to quaternion velocity as

Then we can modify the Jacobian to correspond from joint velocity to quaternion velocity

The inverse Jacobian is identical to the form described in Part 1, i.e.

Again, using standard PID control methods, we can use a control law

where is the quaternion complement

Example MATLAB code set up on Gist

### Advanced Task Space Control Concepts

For the full task space control, we can simply concatenate both Jacobians into a single matrix

to optimize

As an additional consideration, to improve singularity robustness, we can introduce a regularization term to minimize

and get the control law

Even further, if the number of joints is greater than 6, this is a redundant arm, and thus the internal degrees of freedom can be controlled using

where is a vector of terms corresponding to the nullspace of the jacobian that satisfies .