问题
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
回答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.
来源：https://stackoverflow.com/questions/63503764/what-is-the-recommended-way-of-constraining-floating-base-quaternion-position-an