Search code examples
drake

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

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

enter image description here

Solution

  • 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.