What is the recommended way of constraining floating base quaternion position and angular velocity?

Question

For objects with floating base, rotation in the generalized position is expressed as a 4D quaternion. However, the rotation in the generalized velocity is still expressed as a 3D spatial rotation vector.

Is there a recommended way of constraining them for, e.g. backward euler?

prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))

Is it appropriate to convert the angular velocity into quaternion form, e.g. here?

Or as John T. Betts puts it in Practical Methods for Optimal Control and Estimation Using Nonlinear Programming

Answer 1

I think for your use case, you want to impose the integration constraint between adjacent waypoints in planning. In this case, I wouldn't suggest to use the Euler integration q[n] = q[n+1] - qdot[n+1] * dt directly. The reason is that the quaternion should always satisfy the unit length constraint (i.e., q[n] and q[n+1] are both on the surface of the unit sphere in 4D). Hence the time derivative qdot is along the tangent surface of this unit sphere, namely q[n+1] - qdot[n+1] * dt is on that tangent surface. No point (other than q[n+1]) on the tangent surface is also on the surface of the unit circle. Hence the constraint q[n+1] = q[n] + qdot[n+1] * dt can only be satisfied when dt=0, when you couple it with the constraint |q[n]| = |q[n+1]|=1. Pictorially, you could use the figure in the "tangent to a circle" section of https://www.toppr.com/guides/maths/circles/tangents-to-the-circle/. Notice that q[n+1] - qdot[n+1] * dt is on the tangent line of the sphere, and q[n] is on the circle, so you can't have q[n] = q[n+1] - qdot[n+1] * dt.

Instead, I would suggest to consider the following condition

q[n+1] = q[n] ⊗ Δq

where q[n] is the quaternion of the floating base at the n'th waypoint, q[n+1] is the quaternion of the floating base at the n+1'th waypoint. ⊗ is the Hamiltonian product between two quaternions q[n] and Δq. So here you need to compute Δq from the angular velocity.

Suppose that the average angular velocity between the n'th and n+1'th waypoints is ω ∈ ℝ³. This means that the floating base rotates about an axis a = ω/|ω| at rate |ω|, hence

Δq = [cos(|ω|Δt/2), a*sin(|ω|Δt/2)]

See https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotations for an explanation on the equation above.

If you use ω[n+1] as the average angular velocity from the n'th waypoint to n+1'th waypoint (analogous to using v[n+1] in backward Euler), then your integration constraint is

q[n+1] = q[n] ⊗ [cos(|ω[n+1]|Δt/2), a*sin(|ω[n+1]|Δt/2)]

You could also use the other quantities as the average angular velocity, for example (ω[n] + ω[n+1])/2.

Answer 2

There is another way to look at the quaternion integration as given in the book by Betts [1] but this has two caveats to look out for. As given in the book, we cannot use the ODEs directly (6.123a - 6.123d) for quaternion integration. Rather, we can use the DAEs given in the book, equations 6.126a - 6.126c and 6.126g. These work as we can view quaternions as a version of axis angle representation:

quat = (cos(phi/2), unit_vec * sin(phi/2))

Once we integrate the vector part (using either Euler or Range-Kutta Higher-Order Methods), the scalar part is determined by the unit quaternion constraint of rotation quaternions. The vector part can be integrated using the quaternion derivative formulation from the angular velocity as given in Betts [1].

This process has the following two caveats:

• This method is only good for very small delta Ts
• There is a singularity in this method. Every time, we use only 3 numbers to represent rotations (here, we use the vector part of the quaternion only for integration), we introduce a singularity. The scalar part of the axis-angle quaternion is indeed determined by unit length constraint except at -pi or pi. At these points, this mapping does not work. For an in-depth derivation/understanding of why this happens check the determinant in equation 8 in [2].

The singularity occurs when the angle in the axis-angle representation is at pi/-pi. It is important to note that this does not necessarily coincide with crossing pi/-pi in the individual x/y/z axis. Although this can also happen. Run the Betts example 6.8 for rotation of more than pi about the x-axis to see this in action.

Hence, if you can ensure the angle in your axis-angle representation during the trajectory will not cross pi/-pi, you can use the integration method using DAEs given in Betts for simpler implementation. But as soon as you cross pi/-pi, this will not work. Then you will have to use proper quaternion integration. as given by Hongkai Dai with certain care in the implementation. You can check his other answers on this topic to find the implementation details. Also check the UnitQuaternionConstraint in Drake for this.

[1] - Betts, J. T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. In Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. Society for Industrial and Applied Mathematics.

[2] - Yang, Y. (2010). Quaternion based model for momentum biased nadir pointing spacecraft. Aerospace Science and Technology, 14(3), 199–202. https://doi.org/10.1016/j.ast.2009.12.006