为什么这个用于查找 2 个四元数之间的 angular 速度的简单 MP 会失败？

Question

这是

的跟进

作为示例问题，我正在尝试根据链接的建议，使用数学程序在给定起始四元数、结束四元数和特定 dt 的情况下找到 angular 速度。

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:
# 
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()

此程序returns立即不可行，具有以下不可行约束

['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']

当我通过取消注释行

提供正确答案作为猜测时，程序成功解决

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

我不能 100% 确定我对链接答案中建议的解释和实施是正确的。我假设涉及四元数的直接转录经常进行，但我找不到任何例子......

我的约束方法正确还是有更好的方法？

附带问题：为什么 snopt 声称我的问题不可行？

Answer 1

我修改了你的代码，现在可以运行了

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:
# 
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}")

关于这个问题的一些建议：

使用 [0, 0, 0] 作为 angular 速度的初始猜测可能很糟糕。请注意，在您的原始代码中，您有 w / np.linalg.norm(w)，其中 w 是 angular 速度。首先，这会导致被零除，其次，只要你的除数很小，梯度 w.r.t 除数就会很大，如此大的梯度会导致基于梯度的非线性优化求解器出现问题。
我没有使用 angular 速度 w 作为决策变量，而是将决策变量更改为 angular 速度的轴和大小，因此无需执行除法 w / np.linalg.norm(w) 了。请注意，我在轴上施加了单位长度约束。
当有单位长度约束x' * x = 1时（如四元数和旋转轴上的单位长度约束），最好不要使用x=0作为初始猜测。原因是当 x 为 0 时此约束 x' * x 的梯度为 0，因此基于梯度的求解器可能不知道如何移动 x.

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

当snopt声称问题不可行时，并不意味着问题是全局不可行的，只是意味着在它被卡住的邻域中，它找不到解决方案，而在远离它的地方可能有解决方案它被卡住的地方。通常如果你改变最初的猜测，并从解决方案的附近开始，snopt 将更有可能找到解决方案。

为什么这个用于查找 2 个四元数之间的 angular 速度的简单 MP 会失败？

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

python-3.x

drake