This is yet another follow up to Why does this simple MP for finding angular velocity between 2 quaternions fail?
Following the answer given by Hongkai Dai, I made the following modifications:
dt an optimization variablew_mag so it should prefer a minimal and uniform angular velocitydt constraintdt = 1 constraint (i.e. final time = 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, w_axis, w_mag, 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, w_axis, w_mag, 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, cols=1, name="dt")
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 : 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, 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, 0, 1])
prog.SetInitialGuess(w_mag[k], [0])
prog.SetInitialGuess(q[k], [1., 0., 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}")
The optimizer returns infeasible. (Curiously, it is feasible if there is no cost involved).
I've seen papers with NLPs involving backward euler with flexible timestep dt before, so I'm led to believe that this must be doable, what am I missing?
@@ -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, 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):
@@ -17,51 +18,65 @@ def quat_multiply(q0, q1):
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, [
+def backward_euler(q_qprev_v_dt):
+ q, qprev, w_axis, w_mag, dt = np.split(q_qprev_v_dt, [
4,
- 4+4, 8 + 3])
+ 4 + 4,
+ 4 + 4 + 3,
+ 4 + 4 + 3 + 1])
return q - apply_angular_velocity_to_quaternion(qprev, w_axis, w_mag, 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 = [0.0] + [1.0/(N-1)] * (N-1)
+dt = prog.NewContinuousVariables(rows=N, cols=1, name="dt")
+
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),
+ (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]]))
+ 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, 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, 0, 1])
prog.SetInitialGuess(w_mag[k], [0])
prog.SetInitialGuess(q[k], [1., 0., 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}")
I tried your code, but first solve the problem without the cost, and then initialize the variables to the no-cost solution, and solve it again with the cost, this time it can find the solution.
When nonlinear optimization fails, it doesn't mean there exist no solution, but just in the local neighbourhood where the solver has searched, it doesn't find a solution. There could be a solution elsewhere but the solver hasn't searched.
Since you want to find the minimal angular velocity to rotate the object from one orientation to another orientation, you probably know it already, that this problem has an analytical solution. Namely you find the arc with the geodesic distance connecting this two quaternions, that represents rotating the body with a constant angular velocity from the initial orientation to the final orientation. Mathematically to find such an arc (and angular velocity), you could do
# 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
Then you get the optimal angular velocity without solving the optimization problem.