为什么这个用于查找两个四元数之间的角速度的简单MP失败了？

问题描述

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

的跟进

作为一个示例问题，我正在尝试根据链接的建议，使用数学程序找到给定起始四元数，终止四元数和特定dt的角速度。

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),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])))
        .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]]*N
prog.SetDecisionVariableValueInVector(q,q_guess,initial_guess)
v_guess = [[0.,0.,0.],[0.0,0.0]]
# v_guess = [[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()

该程序在不可行的情况下立即返回不可行

['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.,3.]] # Uncomment this for the correct guess

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

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

侧面问题：为什么snopt声称我的问题不可行？

解决方法

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

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_mag = np.split(q_qprev_v,[
            4,4+4,8 + 3])
    return q - apply_angular_velocity_to_quaternion(qprev,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],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],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])))
        .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.,1.]]
w_mag_guess = [[0],[0.]]
# v_guess = [[0.,0.],[1.,2.,3.]] # Uncomment this for the correct guess
for k in range(N):
    prog.SetInitialGuess(w_axis[k],[0,1])
    prog.SetInitialGuess(w_mag[k],[0])
    prog.SetInitialGuess(q[k],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}")

有关此问题的一些建议：

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

侧面问题：为什么snopt声称我的问题不可行？

当snopt声称该问题不可行时，这并不意味着该问题在全球范围内都是不可行的，仅意味着在该问题被卡住的附近，它无法找到解决方案，而可能有一个远离它的解决方案卡住。通常，如果您更改初始猜测，并从解决方案附近开始，则snopt更有可能找到解决方案。