2 个四元数之间的后向欧拉优化，涉及灵活的 dt 和角速度成本

问题描述

这是对Why does this simple MP for finding angular velocity between 2 quaternions fail?的又一次跟进

在 answer given by Hongkai Dai 之后，我做了以下修改：

使dt成为优化变量
在 w_mag 上增加了成本，因此它应该更喜欢最小且均匀的角速度
添加了正 dt 约束
添加了 dt 的总和 = 1 个约束（即最终时间 = 1）

# Taken from https://stackoverflow.com/a/64778960/3177701
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

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)

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,dt = np.split(q_qprev_v_dt,[
            4,4 + 4,4 + 4 + 3,4 + 4 + 3 + 1])
    return q - apply_angular_veLocity_to_quaternion(qprev,dt)

N = 4
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/(N-1)] * (N-1)
dt = prog.NewContinuousVariables(rows=N,name="dt")

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 : 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],dt[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"))
(prog.AddLinearConstraint(ge(dt,0.0))
        .evaluator().set_description("timestep greater than 0 constraint"))
(prog.AddConstraint(np.sum(dt) == 1.0)
        .evaluator().set_description("Total time constriant"))

for k in range(N):
    prog.SetinitialGuess(w_axis[k],[0,1])
    prog.SetinitialGuess(w_mag[k],[0])
    prog.SetinitialGuess(q[k],[1.,0.,0.])
    prog.SetinitialGuess(dt[k],[1.0/N])
    prog.Addcost((w_mag[k]*w_mag[k])[0])
    # prog.AddQuadraticCost(Q=np.identity(1),b=np.zeros((1,)),c = 0.0,vars=np.reshape(w_mag[k],(1,1)))
solver = SnoptSolver()
result = solver.solve(prog)
print(result.is_success())
if not result.is_success():
    print("---------- INFEASIBLE ----------")
    print(result.GetInfeasibleConstraintNames(prog))
    print("--------------------")
print(f"Cost = {result.get_optimal_cost()}")
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}")
dt_sol = result.GetSolution(dt)
print(f"dt_sol = {dt_sol}")

优化器返回不可行。（奇怪的是，如果不涉及成本，这是可行的）。

我之前看过 NLP 涉及具有灵活时间步长 dt 的后向欧拉的论文，所以我相信这一定是可行的，我错过了什么？

详细变化

@@ -1,10 +1,11 @@
 # Taken from https://stackoverflow.com/a/64778960/3177701
 import numpy as np
 from pydrake.all import Quaternion
 from pydrake.all import MathematicalProgram,SnoptSolver
 from pydrake.all import Quaternion_,autodiffXd
+import pdb
 
 epsilon = 1e-9
 quaternion_epsilon = 1e-9
 
 def quat_multiply(q0,q1):
@@ -17,51 +18,65 @@ def quat_multiply(q0,q1):
 
 def apply_angular_veLocity_to_quaternion(q,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,w_mag = np.split(q_qprev_v,[
+def backward_euler(q_qprev_v_dt):
+    q,[
             4,-            4+4,8 + 3])
+            4 + 4,+            4 + 4 + 3,+            4 + 4 + 3 + 1])
     return q - apply_angular_veLocity_to_quaternion(qprev,dt)
 
 N = 4
 prog = MathematicalProgram()
 q = prog.NewContinuousVariables(rows=N,name='q')
 w_axis = prog.NewContinuousVariables(rows=N,name="w_axis")
 w_mag = prog.NewContinuousVariables(rows=N,name="w_mag")
-dt = [0.0] + [1.0/(N-1)] * (N-1)
+# dt = [0.0] + [1.0/(N-1)] * (N-1)
+dt = prog.NewContinuousVariables(rows=N,name="dt")
+
 for k in range(N):
     (prog.AddConstraint(lambda x: [x @ x],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),+    (prog.AddConstraint(lambda q_qprev_v_dt : backward_euler(q_qprev_v_dt),-            vars=np.concatenate([q[k],w_mag[k]]))
+            vars=np.concatenate([q[k],dt[k]]))
         .evaluator().set_description(f"q[{k}] backward euler constraint"))
 
 (prog.AddLinearConstraint(eq(q[0],0.0])))
         .evaluator().set_description("Initial orientation constraint"))
 (prog.AddLinearConstraint(eq(q[-1],0.7659655809383968])))
         .evaluator().set_description("Final orientation constraint"))
+(prog.AddLinearConstraint(ge(dt,0.0))
+        .evaluator().set_description("timestep greater than 0 constraint"))
+(prog.AddConstraint(np.sum(dt) == 1.0)
+        .evaluator().set_description("Total time constriant"))
 
 for k in range(N):
     prog.SetinitialGuess(w_axis[k],1])
     prog.SetinitialGuess(w_mag[k],[0])
     prog.SetinitialGuess(q[k],0.])
+    prog.SetinitialGuess(dt[k],[1.0/N])
+    prog.Addcost((w_mag[k]*w_mag[k])[0])
+    # prog.AddQuadraticCost(Q=np.identity(1),1)))
 solver = SnoptSolver()
 result = solver.solve(prog)
 print(result.is_success())
 if not result.is_success():
     print("---------- INFEASIBLE ----------")
     print(result.GetInfeasibleConstraintNames(prog))
     print("--------------------")
+print(f"Cost = {result.get_optimal_cost()}")
 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}")
+dt_sol = result.GetSolution(dt)
+print(f"dt_sol = {dt_sol}")

解决方法

我试过你的代码，但是先无代价解决问题，然后将变量初始化为无代价解，再用代价求解，这次就可以找到解了。

当非线性优化失败时，并不意味着不存在解，只是在求解器搜索过的局部邻域中，它没有找到解。其他地方可能有解决方案，但求解器尚未搜索。

由于您想找到将物体从一个方向旋转到另一个方向的最小角速度，您可能已经知道，这个问题有解析解。也就是说，您找到连接这两个四元数的测地距离的弧，表示以恒定角速度从初始方向旋转到最终方向的主体。在数学上找到这样的弧（和角速度），你可以做

# We know quat_multiply(q_initial,delta_q) = q_final
delta_q = quat_multiply(conjugate(q_initial),q_final)
# Now convert the quaternion delta_q to angle-axis representation
angular_vel_axis = norm(delta_q[1:])
angular_vel_magnitude = arccos(delta_q[0]) * 2 / t_total

然后您无需解决优化问题即可获得最佳角速度。

drake python-3.x

2 个四元数之间的后向欧拉优化，涉及灵活的 dt 和角速度成本

问题描述

详细变化

解决方法

相关问答