How to control end-effector orientation with vector part of quaternion?

Question

I was reading this paper: Closed-Loop Manipulator Control Using Quaternion Feedback, and it mentioned that you could control the end-effector orientation using a resolved rate controller where the orientation part of that controller is: $$\tau_\theta = \mathbf{K}_d(\mathbf{J}_o^\dagger(\omega_d - \mathbf{K}_o\mathbf{e}_o) - \dot{\theta}).$$

See Eq.(20) of the linked paper. My understanding of the terms is that $\omega_d$ is the desired angular velocity, $\mathbf{J}_o$ is the orientation part of the geometric Jacobian, $\dot{\theta}$ is the angular velocities of the joints, $\mathbf{e}_o$ is the orientation error, and both $\mathbf{K}_d$ and $\mathbf{K}_o$ are gain matrices.

In that paper, it mentioned that $\mathbf{e}_o$ could be represented by $\delta{\mathbf{q}}$, which is the vector part of the error quaternion. I did try this. However, it did not work. Instead, if I set $\mathbf{e}_o$ to be the orientation error in the axis-angle form, it was able to give me a controller that works.

So I was wondering if I did something wrong when using the vector part of the error quaternion as $\delta{\mathbf{q}}$. I guess that one possible place I might have gotten wrong is $\mathbf{J}_o$, I am using the rotation part of the geometric Jacobian, is this the right choice?

The portion of my code that computes the control is

import pinocchio as pin
from scipy.spatial.transform import Rotation as R

# get joint states
q, dq = get_state()

# Get end-effector position
ee_position = get_ee_position()

# Get end-effector orientation in quaternions
ee_orientation = get_ee_orientation()

# [x, y, z, w]
ee_quaternion = R.from_matrix(ee_orientation).as_quat()

# Orientation error in quaternion form
quat_err = compute_quat_vec_error(target_quaternion, ee_quaternion)

# Get frame ID for grasp target
jacobian_frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED

# Get Jacobian of grasp target frame
jacobian = robot.getFrameJacobian(FRAME_ID, jacobian_frame)

# Get pseudo-inverse of frame Jacobian
pinv_jac = np.linalg.pinv(jacobian)

# Compute Gravitational terms
G = robot.gravity(q)

# Compute controller
target_dx = np.zeros((6, 1))
target_dx[:3] = 1.0 * (target_position - ee_position)
target_dx[3:] = np.diag([3.0, 3.0, 3.0]) @ quat_err[:, np.newaxis]

# Compute torque commands
tau = (pinv_jac @ target_dx - dq[:, np.newaxis]) + G[:, np.newaxis]

the function that I used to compute the quaternion error is

def compute_quat_vec_error(quat_desired, quat_measured):
    eta_d = quat_desired[-1]
    eta = quat_measured[-1]

    q_d = quat_desired[:3]
    q = quat_measured[:3]

    delta_quat_vec = eta_d * q - eta * q_d - np.cross(q_d, q)
    return delta_quat_vec

Answer 1

The error with the above code when compared to the axis-angle representation method is that this quaternion method returns its values still in a quaternion representation. In other words, we have:

$$ \mathbf{e}_{q} = [q_{1}\text{sin}(\frac{\psi}{2}), \ q_{2}\text{sin}(\frac{\psi}{2}), \ q_{3}\text{sin}(\frac{\psi}{2})]$$

whereas the angle axis representation appears to return (from our discussions in chat):

$$ \mathbf{e}_{a} = [q_{1}\psi, \ q_{2}\psi, \ q_{3}\psi]$$

So, this leaves us with two different values for what conceptually represents the same information. Now, we should keep in mind that $\mathbf{J}_{o}$ is formulated (as far as I am aware) to represent the axis-angle change in orientation with respect to joint velocities. Thus, the pseudo-inverse of $\mathbf{J}_{o}$ will return proper joint velocities when multiplied with the axis-angle representation of angular change. To accomplish this given $\mathbf{e}_{q}$, one could apply the following:

$$ \text{let} \ r_{i} \triangleq q_{i}\text{sin}(\frac{\psi}{2}), \ \text{for} \ i \in \{1,2,3\} \\ ||\mathbf{e}_q|| = \sqrt{r_{1}^{2} + r_{2}^{2} + r_{3}^{2}} \\ $$

Then, as $||\mathbf{q}|| = \sqrt{q_{1}^{2} + q_{2}^{2} + q_{3}^{2}} = 1$ by the definition of a quaternion (versor), we can find the rotation angle from the following result:

$$ ||\mathbf{e}_{q}|| = ||\mathbf{q} \cdot \text{sin}(\frac{\psi}{2})|| = | \text{sin}(\frac{\psi}{2})| \cdot ||\mathbf{q}|| = | \text{sin}(\frac{\psi}{2}) | \cdot 1$$

Where we can pull the $| \sin({\frac{\psi}{2}}) |$ out of the norm by the definition of the Euclidian norm. Now, we need to deal with the absolute value portion of the $| \sin(\frac{\psi}{2}) |$ term. So, choose an arbitrary $r_{j}, \text{where} \ 1 \le j \le 3$, and we can now define:

$$\psi \triangleq 2 \sin^{-1}(||\mathbf{e}_{q}||) \cdot \text{sgn}(r_{j})$$

In other words, we assume $q_{j}$ is always positive and determine the correct sign of our angle with the direction of $r_{j}$. Note that this should not cause any issues as we will be multiplying $q_j$ and $\psi$ resulting in the same original sign (we know this is true as $\sin(x) \gt 0 \ \text{for} \ 0 \lt x \lt \frac{\pi}{2}$ and $\sin(x) < 0 \ \text{for} \ -\frac{\pi}{2} \lt x \lt 0$). The final step of converting the error in the quaternion representation to error in the angle-axis representation is:

$$\mathbf{e}_{a} = \mathbf{e}_{q} \cdot \frac{\psi}{\sin(\frac{\psi}{2})} $$

Note that a check should be done to see if $|| \mathbf{e}_{q} ||$ is close to zero to avoid division by zero - as $\psi$ would then also be close to zero.

---

As a final point not really related to solving your problem here, I would recommend sticking with angle-axis representations instead of quaternions. The only clear advantage of quaternions is there computational efficiency, but given the speed of modern computer hardware you are unlikely to see any real benefit there unless you are simulating many, many objects simultaneously. Angle-axis representation has the same ability to represent rotations without representational singularities just as quaternions (they are isomorphic). Personally, I find angle-axis more intuituve as well - given the exponential map from a unit rotation vector and scalar angle to a rotation matrix here and the rotation of a vector about an arbitrary vector by a scalar angle here. Quaternions do have simple equations, but I have found the headaches associated with their nuances to be much greater than the utility gained from slightly faster computations.

Finally, note that most arguments around quaternions vs. rotation matrices are based on compositions of Euler angles - which are inferior to both quaternions and axis-angle.