Why does this simple MP for finding angular velocity between 2 quaternions fail?

Question

This is a follow up to What is the recommended way of constraining floating base quaternion position and angular velocity?

As a sample problem, I am trying to find the angular velocity given a starting quaternion, ending quaternion, and a specific dt using a mathematical program, following the suggestions linked.

import numpy as np
from pydrake.all import Quaternion
from pydrake.all import MathematicalProgram, Solve, eq, le, ge, SolverOptions, SnoptSolver
from pydrake.all import Quaternion_, AutoDiffXd
import pdb

epsilon = 1e-9
quaternion_epsilon = 1e-9

# Following the suggestion from:
# https://stackoverflow.com/a/63510131/3177701
def apply_angular_velocity_to_quaternion(q, w, t):
    # This currently returns a runtime warning of division by zero
    # https://github.com/RobotLocomotion/drake/issues/10451
    norm_w = np.linalg.norm(w)
    if norm_w <= epsilon:
        return q
    norm_q = np.linalg.norm(q)
    if abs(norm_q - 1.0) > quaternion_epsilon:
        print(f"WARNING: Quaternion {q} with norm {norm_q} not normalized!")
    a = w / norm_w
    if q.dtype == AutoDiffXd:
        delta_q = Quaternion_[AutoDiffXd](np.hstack([np.cos(norm_w * t/2.0), a*np.sin(norm_w * t/2.0)]).reshape((4,1)))
        return Quaternion_[AutoDiffXd](q/norm_q).multiply(delta_q).wxyz()
    else:
        delta_q = Quaternion(np.hstack([np.cos(norm_w * t/2.0), a*np.sin(norm_w * t/2.0)]).reshape((4,1)))
        return Quaternion(q/norm_q).multiply(delta_q).wxyz()

def backward_euler(q_qprev_v, dt):
    q, qprev, v = np.split(q_qprev_v, [
            4,
            4+4])
    return q - apply_angular_velocity_to_quaternion(qprev, v, dt)

N = 2
prog = MathematicalProgram()
q = prog.NewContinuousVariables(rows=N, cols=4, name='q')
v = prog.NewContinuousVariables(rows=N, cols=3, name='v')
dt = [0.0, 1.0] # dt[0] is unused
for k in range(N):
    (prog.AddConstraint(np.linalg.norm(q[k][0:4]) == 1.)
            .evaluator().set_description(f"q[{k}] unit quaternion constraint"))
for k in range(1, N):
    (prog.AddConstraint(lambda q_qprev_v, dt=dt[k] : backward_euler(q_qprev_v, dt),
            lb=[0.0]*4, ub=[0.0]*4,
            vars=np.concatenate([q[k], q[k-1], v[k]]))
        .evaluator().set_description(f"q[{k}] backward euler constraint"))

(prog.AddLinearConstraint(eq(q[0], np.array([1.0, 0.0, 0.0, 0.0])))
        .evaluator().set_description("Initial orientation constraint"))
(prog.AddLinearConstraint(eq(q[-1], np.array([-0.2955511242573139, 0.25532186031279896, 0.5106437206255979, 0.7659655809383968])))
        .evaluator().set_description("Final orientation constraint"))

initial_guess = np.empty(prog.num_vars())
q_guess = [[1.0, 0.0, 0.0, 0.0]]*N
prog.SetDecisionVariableValueInVector(q, q_guess, initial_guess)
v_guess = [[0., 0., 0.], [0.0, 0.0, 0.0]]
# v_guess = [[0., 0., 0.], [1., 2., 3.]] # Uncomment this for the correct guess
prog.SetDecisionVariableValueInVector(v, v_guess, initial_guess)
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
print(result.is_success())
if not result.is_success():
    print("---------- INFEASIBLE ----------")
    print(result.GetInfeasibleConstraintNames(prog))
    print("--------------------")
q_sol = result.GetSolution(q)
print(f"q_sol = {q_sol}")
v_sol = result.GetSolution(v)
print(f"v_sol = {v_sol}")
pdb.set_trace()

This program returns infeasible immediately with the following infeasible constraints

['q[1] backward euler constraint[0]: 0.000000 <= -1.295551 <= 0.000000', 'q[1] backward euler constraint[1]: 0.000000 <= 0.255322 <= 0.000000', 'q[1] backward euler constraint[2]: 0.000000 <= 0.510644 <= 0.000000', 'q[1] backward euler constraint[3]: 0.000000 <= 0.765966 <= 0.000000']

The program solves successfully when I provide it with the correct answer as guess by uncommenting the line

# v_guess = [[0., 0., 0.], [1., 2., 3.]] # Uncomment this for the correct guess

I am not 100% sure my interpretation and implementation of the suggestion in the linked answer is correct. I would assume direct transcription involving quaternions is done quite often but I can't quite find any examples...

Is my method of constraining correct or are there better methods?

Side question: Why does snopt claim my problem is infeasible?

Answer 1

I modified your code, and it runs now

import numpy as np
from pydrake.all import Quaternion
from pydrake.all import MathematicalProgram, Solve, eq, le, ge, SolverOptions, SnoptSolver
from pydrake.all import Quaternion_, AutoDiffXd

epsilon = 1e-9
quaternion_epsilon = 1e-9

def quat_multiply(q0, q1):
    w0, x0, y0, z0 = q0
    w1, x1, y1, z1 = q1
    return np.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0,
                   x1*w0 + y1*z0 - z1*y0 + w1*x0,
                  -x1*z0 + y1*w0 + z1*x0 + w1*y0,
                   x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=q0.dtype)

# Following the suggestion from:
# https://stackoverflow.com/a/63510131/3177701
def apply_angular_velocity_to_quaternion(q, w_axis, w_mag, t):
    delta_q = np.hstack([np.cos(w_mag* t/2.0), w_axis*np.sin(w_mag* t/2.0)])
    return  quat_multiply(q, delta_q)

def backward_euler(q_qprev_v, dt):
    q, qprev, w_axis, w_mag = np.split(q_qprev_v, [
            4,
            4+4, 8 + 3])
    return q - apply_angular_velocity_to_quaternion(qprev, w_axis, w_mag, dt)

N = 2
prog = MathematicalProgram()
q = prog.NewContinuousVariables(rows=N, cols=4, name='q')
w_axis = prog.NewContinuousVariables(rows=N, cols=3, name="w_axis")
w_mag = prog.NewContinuousVariables(rows=N, cols=1, name="w_mag")
dt = [0.0, 1.0] # dt[0] is unused
for k in range(N):
    (prog.AddConstraint(lambda x: [x @ x], [1], [1], q[k])
            .evaluator().set_description(f"q[{k}] unit quaternion constraint"))
    # Impose unit length constraint on the rotation axis.
    (prog.AddConstraint(lambda x: [x @ x], [1], [1], w_axis[k])
            .evaluator().set_description(f"w_axis[{k}] unit axis constraint"))
for k in range(1, N):
    (prog.AddConstraint(lambda q_qprev_v, dt=dt[k] : backward_euler(q_qprev_v, dt),
            lb=[0.0]*4, ub=[0.0]*4,
            vars=np.concatenate([q[k], q[k-1], w_axis[k], w_mag[k]]))
        .evaluator().set_description(f"q[{k}] backward euler constraint"))

(prog.AddLinearConstraint(eq(q[0], np.array([1.0, 0.0, 0.0, 0.0])))
        .evaluator().set_description("Initial orientation constraint"))
(prog.AddLinearConstraint(eq(q[-1], np.array([-0.2955511242573139, 0.25532186031279896, 0.5106437206255979, 0.7659655809383968])))
        .evaluator().set_description("Final orientation constraint"))

w_axis_guess = [[0., 0., 1.], [0., 0., 1.]]
w_mag_guess = [[0], [0.]]
# v_guess = [[0., 0., 0.], [1., 2., 3.]] # Uncomment this for the correct guess
for k in range(N):
    prog.SetInitialGuess(w_axis[k], [0, 0, 1])
    prog.SetInitialGuess(w_mag[k], [0])
    prog.SetInitialGuess(q[k], [1., 0., 0., 0.])
solver = SnoptSolver()
result = solver.Solve(prog)
print(result.is_success())
if not result.is_success():
    print("---------- INFEASIBLE ----------")
    print(result.GetInfeasibleConstraintNames(prog))
    print("--------------------")
q_sol = result.GetSolution(q)
print(f"q_sol = {q_sol}")
w_axis_sol = result.GetSolution(w_axis)
print(f"w_axis_sol = {w_axis_sol}")
w_mag_sol = result.GetSolution(w_mag)
print(f"w_mag_sol = {w_mag_sol}")

Some suggestions on this problem:

1. Using [0, 0, 0] as the initial guess of the angular velocity could be bad. Notice that in your original code, you have w / np.linalg.norm(w) where w is the angular velocity. First this causes division by zero, and second, whenever your divisor is small, the gradient w.r.t the divisor is huge, such large gradient can cause problems in gradient based nonlinear optimization solver.
2. Instead of using angular velocity w as the decision variable, I changed the decision variable to the axis and magnitude of the angular velocity, hence there is no need do the division w / np.linalg.norm(w) any more. Notice that I impose the unit length constraint on the axis.
3. When there is a unit length constraint x' * x = 1 (like the unit length constraint on quaternion and rotation axis), it is better not to use x=0 as the initial guess. The reason is that the gradient of this constraint x' * x is 0 when x is 0, hence the gradient based solver might not know how to move x.

Side question: Why does snopt claim my problem is infeasible?

When snopt claims the problem is infeasible, it doesn't mean the problem is globally infeasible, just means that in the neighbourhood where it get stuck, it cannot find the solution, while there might be a solution far away from where it gets stuck. Usually if you change the initial guess, and start from the vicinity of the solution, snopt will more likely find a solution.