时间戳

首页 > 解决方案 > 为什么这个简单的 MP 用于查找 2 个四元数之间的角速度会失败？

python-3.x - 为什么这个简单的 MP 用于查找 2 个四元数之间的角速度会失败？

问题描述

这是对约束浮动基础四元数位置和角速度的推荐方法是什么的后续行动？

作为一个示例问题，我试图按照链接的建议，使用数学程序找到给定起始四元数、结束四元数和特定 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), 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()

这个程序立即返回不可行的，具有以下不可行的约束

['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 声称我的问题不可行？

标签： python-3.xdrake

解决方案

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

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

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

  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 将更有可能找到一个解。

推荐阅读