def run_complex_mathematical_program():
print "\n\ncomplex mathematical program"
mp = MathematicalProgram()
x = mp.NewContinuousVariables(1, 'x')
mp.AddCost(cost, x)
mp.AddConstraint(quad_constraint, [1.0], [2.0], x)
mp.SetInitialGuess(x, [1.1])
print mp.Solve()
res = mp.GetSolution(x)
print res
print "(finished) complex mathematical program"
def trajopt_simulation(x0, xf, u_max=0.5): # numeric parameters time_interval = .1 time_steps = 400 # initialize optimization prog = MathematicalProgram() # optimization variables state = prog.NewContinuousVariables(time_steps + 1, 3, 'state') u = prog.NewContinuousVariables(time_steps, 1, 'u') # final position constraint # for residual in constraint_state_to_orbit(state[-2], state[-1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual == 0) # initial position constraint prog.AddConstraint(eq(state[0],x0)) # discretized dynamics for t in range(time_steps): residuals = dubins_discrete_dynamics(state[t], state[t+1], u[t], time_interval) for residual in residuals: prog.AddConstraint(residual == 0) # control limits prog.AddConstraint(u[t][0] <= u_max) prog.AddConstraint(-u_max <= u[t][0]) # cost - increases cost if off the orbit p = state[t][:2] - xf[:2] v = (state[t+1][:2] - state[t][:2]) / time_interval # prog.AddCost(time_interval) # prog.AddCost(u[t][0]*u[t][0]*time_interval) # prog.AddCost(p.dot(v)*time_interval) for residual in constraint_state_to_orbit(state[t], state[t+1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual >= 0) prog.AddQuadraticCost(residual) # prog.AddCost((p.dot(p))) # # initial guess state_guess = interpolate_dubins_state( np.array([0, 0, np.pi]), xf, time_steps, time_interval, 1/u_max ) prog.SetInitialGuess(state, state_guess) solver = SnoptSolver() result = solver.Solve(prog) assert result.is_success() # retrieve optimal solution u_opt = result.GetSolution(u).T state_opt = result.GetSolution(state).T return state_opt, u_opt
def test_Feedback_Linearization_controller_BS(x, t):
# Output feedback linearization 2
#
# y1= ||r-rf||^2/2
# y2= wx
# y3= wy
# y4= wz
#
# Analysis: The zero dynamics is unstable.
global g, xf, Aout, Bout, Mout, T_period, w_freq, radius
print("%%%%%%%%%%%%%%%%%%%%%")
print("%%CLF-QP %%%%%%%%%%%")
print("%%%%%%%%%%%%%%%%%%%%%")
print("t:", t)
prog = MathematicalProgram()
u_var = prog.NewContinuousVariables(4, "u_var")
c_var = prog.NewContinuousVariables(1, "c_var")
# # # Example 1 : Circle
x_f = radius * math.cos(w_freq * t)
y_f = radius * math.sin(w_freq * t)
# print("x_f:",x_f)
# print("y_f:",y_f)
dx_f = -radius * math.pow(w_freq, 1) * math.sin(w_freq * t)
dy_f = radius * math.pow(w_freq, 1) * math.cos(w_freq * t)
ddx_f = -radius * math.pow(w_freq, 2) * math.cos(w_freq * t)
ddy_f = -radius * math.pow(w_freq, 2) * math.sin(w_freq * t)
dddx_f = radius * math.pow(w_freq, 3) * math.sin(w_freq * t)
dddy_f = -radius * math.pow(w_freq, 3) * math.cos(w_freq * t)
ddddx_f = radius * math.pow(w_freq, 4) * math.cos(w_freq * t)
ddddy_f = radius * math.pow(w_freq, 4) * math.sin(w_freq * t)
# # Example 2 : Lissajous curve a=1 b=2
# ratio_ab=2;
# a=1;
# b=ratio_ab*a;
# delta_lissajous = 0 #math.pi/2;
# x_f = radius*math.sin(a*w_freq*t+delta_lissajous)
# y_f = radius*math.sin(b*w_freq*t)
# # print("x_f:",x_f)
# # print("y_f:",y_f)
# dx_f = radius*math.pow(a*w_freq,1)*math.cos(a*w_freq*t+delta_lissajous)
# dy_f = radius*math.pow(b*w_freq,1)*math.cos(b*w_freq*t)
# ddx_f = -radius*math.pow(a*w_freq,2)*math.sin(a*w_freq*t+delta_lissajous)
# ddy_f = -radius*math.pow(b*w_freq,2)*math.sin(b*w_freq*t)
# dddx_f = -radius*math.pow(a*w_freq,3)*math.cos(a*w_freq*t+delta_lissajous)
# dddy_f = -radius*math.pow(b*w_freq,3)*math.cos(b*w_freq*t)
# ddddx_f = radius*math.pow(a*w_freq,4)*math.sin(a*w_freq*t+delta_lissajous)
# ddddy_f = radius*math.pow(b*w_freq,4)*math.sin(b*w_freq*t)
e1 = np.array([1, 0, 0]) # e3 elementary vector
e2 = np.array([0, 1, 0]) # e3 elementary vector
e3 = np.array([0, 0, 1]) # e3 elementary vector
# epsilonn= 1e-0
# alpha = 100;
# kp1234=alpha*1/math.pow(epsilonn,4) # gain for y
# kd1=4/math.pow(epsilonn,3) # gain for y^(1)
# kd2=12/math.pow(epsilonn,2) # gain for y^(2)
# kd3=4/math.pow(epsilonn,1) # gain for y^(3)
# kp5= 10; # gain for y5
q = np.zeros(7)
qd = np.zeros(6)
q = x[0:8]
qd = x[8:15]
print("qnorm:", np.dot(q[3:7], q[3:7]))
q0 = q[3]
q1 = q[4]
q2 = q[5]
q3 = q[6]
xi1 = q[7]
v = qd[0:3]
w = qd[3:6]
xi2 = qd[6]
xd = xf[0]
yd = xf[1]
zd = xf[2]
wd = xf[11:14]
# Useful vectors and matrices
(Rq, Eq, wIw, I_inv) = robobee_plantBS.GetManipulatorDynamics(q, qd)
F1q = np.zeros((3, 4))
F1q[0, :] = np.array([q2, q3, q0, q1])
F1q[1, :] = np.array([-1 * q1, -1 * q0, q3, q2])
F1q[2, :] = np.array([q0, -1 * q1, -1 * q2, q3])
Rqe3 = np.dot(Rq, e3)
Rqe3_hat = np.zeros((3, 3))
Rqe3_hat[0, :] = np.array([0, -Rqe3[2], Rqe3[1]])
Rqe3_hat[1, :] = np.array([Rqe3[2], 0, -Rqe3[0]])
Rqe3_hat[2, :] = np.array([-Rqe3[1], Rqe3[0], 0])
Rqe1 = np.dot(Rq, e1)
Rqe1_hat = np.zeros((3, 3))
Rqe1_hat[0, :] = np.array([0, -Rqe1[2], Rqe1[1]])
Rqe1_hat[1, :] = np.array([Rqe1[2], 0, -Rqe1[0]])
Rqe1_hat[2, :] = np.array([-Rqe1[1], Rqe1[0], 0])
Rqe1_x = np.dot(e2.T, Rqe1)
Rqe1_y = np.dot(e1.T, Rqe1)
w_hat = np.zeros((3, 3))
w_hat[0, :] = np.array([0, -w[2], w[1]])
w_hat[1, :] = np.array([w[2], 0, -w[0]])
w_hat[2, :] = np.array([-w[1], w[0], 0])
Rw = np.dot(Rq, w)
Rw_hat = np.zeros((3, 3))
Rw_hat[0, :] = np.array([0, -Rw[2], Rw[1]])
Rw_hat[1, :] = np.array([Rw[2], 0, -Rw[0]])
Rw_hat[2, :] = np.array([-Rw[1], Rw[0], 0])
#- Checking the derivation
# print("F1qEqT-(-Rqe3_hat)",np.dot(F1q,Eq.T)-(-Rqe3_hat))
# Rqe3_cal = np.zeros(3)
# Rqe3_cal[0] = 2*(q3*q1+q0*q2)
# Rqe3_cal[1] = 2*(q3*q2-q0*q1)
# Rqe3_cal[2] = (q0*q0+q3*q3-q1*q1-q2*q2)
# print("Rqe3 - Rqe3_cal", Rqe3-Rqe3_cal)
# Four output
y1 = x[0] - x_f
y2 = x[1] - y_f
y3 = x[2] - zd - 0
y4 = math.atan2(Rqe1_x, Rqe1_y) - math.pi / 8
# print("Rqe1_x:", Rqe1_x)
eta1 = np.zeros(3)
eta1 = np.array([y1, y2, y3])
eta5 = y4
# print("y4", y4)
# First derivative of first three output and yaw output
eta2 = np.zeros(3)
eta2 = v - np.array([dx_f, dy_f, 0])
dy1 = eta2[0]
dy2 = eta2[1]
dy3 = eta2[2]
x2y2 = (math.pow(Rqe1_x, 2) + math.pow(Rqe1_y, 2)) # x^2+y^2
eta6_temp = np.zeros(3) #eta6_temp = (ye2T-xe1T)/(x^2+y^2)
eta6_temp = (Rqe1_y * e2.T - Rqe1_x * e1.T) / x2y2
# print("eta6_temp:", eta6_temp)
# Body frame w ( multiply R)
eta6 = np.dot(eta6_temp, np.dot(-Rqe1_hat, np.dot(Rq, w)))
# World frame w
# eta6 = np.dot(eta6_temp,np.dot(-Rqe1_hat,w))
# print("Rqe1_hat:", Rqe1_hat)
# Second derivative of first three output
eta3 = np.zeros(3)
eta3 = -g * e3 + Rqe3 * xi1 - np.array([ddx_f, ddy_f, 0])
ddy1 = eta3[0]
ddy2 = eta3[1]
ddy3 = eta3[2]
# Third derivative of first three output
eta4 = np.zeros(3)
# Body frame w ( multiply R)
eta4 = Rqe3 * xi2 + np.dot(-Rqe3_hat, np.dot(Rq, w)) * xi1 - np.array(
[dddx_f, dddy_f, 0])
# World frame w
# eta4 = Rqe3*xi2+np.dot(np.dot(F1q,Eq.T),w)*xi1 - np.array([dddx_f,dddy_f,0])
dddy1 = eta4[0]
dddy2 = eta4[1]
dddy3 = eta4[2]
# Fourth derivative of first three output
B_qw_temp = np.zeros(3)
# Body frame w
B_qw_temp = xi1 * (-np.dot(Rw_hat, np.dot(Rqe3_hat, Rw)) +
np.dot(Rqe3_hat, np.dot(Rq, np.dot(I_inv, wIw)))
) # np.dot(I_inv,wIw)*xi1-2*w*xi2
B_qw = B_qw_temp + xi2 * (-2 * np.dot(Rqe3_hat, Rw)) - np.array(
[ddddx_f, ddddy_f, 0]) #np.dot(Rqe3_hat,B_qw_temp)
# World frame w
# B_qw_temp = xi1*(-np.dot(w_hat,np.dot(Rqe3_hat,w))+np.dot(Rqe3_hat,np.dot(I_inv,wIw))) # np.dot(I_inv,wIw)*xi1-2*w*xi2
# B_qw = B_qw_temp+xi2*(-2*np.dot(Rqe3_hat,w)) - np.array([ddddx_f,ddddy_f,0]) #np.dot(Rqe3_hat,B_qw_temp)
# B_qw = B_qw_temp - np.dot(w_hat,np.dot(Rqe3_hat,w))*xi1
# Second derivative of yaw output
# Body frame w
dRqe1_x = np.dot(e2, np.dot(-Rqe1_hat, Rw)) # \dot{x}
dRqe1_y = np.dot(e1, np.dot(-Rqe1_hat, Rw)) # \dot{y}
alpha1 = 2 * (Rqe1_x * dRqe1_x +
Rqe1_y * dRqe1_y) / x2y2 # (2xdx +2ydy)/(x^2+y^2)
# World frame w
# dRqe1_x = np.dot(e2,np.dot(-Rqe1_hat,w)) # \dot{x}
# dRqe1_y = np.dot(e1,np.dot(-Rqe1_hat,w)) # \dot{y}
# alpha1 = 2*(Rqe1_x*dRqe1_x+Rqe1_y*dRqe1_y)/x2y2 # (2xdx +2ydy)/(x^2+y^2)
# # alpha2 = math.pow(dRqe1_y,2)-math.pow(dRqe1_x,2)
# Body frame w
B_yaw_temp3 = np.zeros(3)
B_yaw_temp3 = alpha1 * np.dot(Rqe1_hat, Rw) + np.dot(
Rqe1_hat, np.dot(Rq, np.dot(I_inv, wIw))) - np.dot(
Rw_hat, np.dot(Rqe1_hat, Rw))
B_yaw = np.dot(eta6_temp,
B_yaw_temp3) # +alpha2 :Could be an error in math.
g_yaw = np.zeros(3)
g_yaw = -np.dot(eta6_temp, np.dot(Rqe1_hat, np.dot(Rq, I_inv)))
# World frame w
# B_yaw_temp3 =np.zeros(3)
# B_yaw_temp3 = alpha1*np.dot(Rqe1_hat,w)+np.dot(Rqe1_hat,np.dot(I_inv,wIw))-np.dot(w_hat,np.dot(Rqe1_hat,w))
# B_yaw = np.dot(eta6_temp,B_yaw_temp3) # +alpha2 :Could be an error in math.
# g_yaw = np.zeros(3)
# g_yaw = -np.dot(eta6_temp,np.dot(Rqe1_hat,I_inv))
# print("g_yaw:", g_yaw)
# Decoupling matrix A(x)\in\mathbb{R}^4
A_fl = np.zeros((4, 4))
A_fl[0:3, 0] = Rqe3
# Body frame w
A_fl[0:3, 1:4] = -np.dot(Rqe3_hat, np.dot(Rq, I_inv)) * xi1
# World frame w
# A_fl[0:3,1:4] = -np.dot(Rqe3_hat,I_inv)*xi1
A_fl[3, 1:4] = g_yaw
A_fl_inv = np.linalg.inv(A_fl)
A_fl_det = np.linalg.det(A_fl)
# print("I_inv:", I_inv)
print("A_fl:", A_fl)
print("A_fl_det:", A_fl_det)
# Drift in the output dynamics
U_temp = np.zeros(4)
U_temp[0:3] = B_qw
U_temp[3] = B_yaw
# Output dyamics
eta = np.hstack([eta1, eta2, eta3, eta5, eta4, eta6])
eta_norm = np.dot(eta, eta)
# v-CLF QP controller
FP_PF = np.dot(Aout.T, Mout) + np.dot(Mout, Aout)
PG = np.dot(Mout, Bout)
L_FVx = np.dot(eta, np.dot(FP_PF, eta))
L_GVx = 2 * np.dot(eta.T, PG) # row vector
L_fhx_star = U_temp
Vx = np.dot(eta, np.dot(Mout, eta))
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+(min_e_Q/max_e_P)*Vx*1 # exponentially stabilizing
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+min_e_Q*eta_norm # more exact bound - exponentially stabilizing
phi0_exp = L_FVx + np.dot(
L_GVx, L_fhx_star) # more exact bound - exponentially stabilizing
phi1_decouple = np.dot(L_GVx, A_fl)
# # Solve QP
v_var = np.dot(A_fl, u_var) + L_fhx_star
Quadratic_Positive_def = np.matmul(A_fl.T, A_fl)
QP_det = np.linalg.det(Quadratic_Positive_def)
c_QP = 2 * np.dot(L_fhx_star.T, A_fl)
c_QP_extned = np.hstack([c_QP, -1])
u_var_extended = np.hstack([u_var, c_var])
# CLF_QP_cost_v = np.dot(v_var,v_var) // Exact quadratic cost
CLF_QP_cost_v_effective = np.dot(
u_var, np.dot(Quadratic_Positive_def, u_var)) + np.dot(
c_QP, u_var) - c_var[0] # Quadratic cost without constant term
# CLF_QP_cost_u = np.dot(u_var,u_var)
phi1 = np.dot(phi1_decouple, u_var) + c_var[0] * eta_norm
#----Printing intermediate states
# print("L_fhx_star: ",L_fhx_star)
# print("c_QP:", c_QP)
# print("Qp : ",Quadratic_Positive_def)
# print("Qp det: ", QP_det)
# print("c_QP", c_QP)
# print("phi0_exp: ", phi0_exp)
# print("PG:", PG)
# print("L_GVx:", L_GVx)
# print("eta6", eta6)
# print("d : ", phi1_decouple)
# print("Cost expression:", CLF_QP_cost_v)
#print("Const expression:", phi0_exp+phi1)
#----Different solver option // Gurobi did not work with python at this point (some binding issue 8/8/2018)
solver = IpoptSolver()
# solver = GurobiSolver()
# print solver.available()
# assert(solver.available()==True)
# assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
# solver.Solve(prog)
# assertEqual(result, mp.SolutionResult.kSolutionFound)
# mp.AddLinearConstraint()
# print("x:", x)
# print("phi_0_exp:", phi0_exp)
# print("phi1_decouple:", phi1_decouple)
# print("eta1:", eta1)
# print("eta2:", eta2)
# print("eta3:", eta3)
# print("eta4:", eta4)
# print("eta5:", eta5)
# print("eta6:", eta6)
# Output Feedback Linearization controller
mu = np.zeros(4)
k = np.zeros((4, 14))
k = np.matmul(Bout.T, Mout)
k = np.matmul(np.linalg.inv(R), k)
mu = -1 / 1 * np.matmul(k, eta)
v = -U_temp + mu
U_fl = np.matmul(
A_fl_inv, v
) # Output Feedback controller to comare the result with CLF-QP solver
# Set up the QP problem
prog.AddQuadraticCost(CLF_QP_cost_v_effective)
prog.AddConstraint(phi0_exp + phi1 <= 0)
prog.AddConstraint(c_var[0] >= 0)
prog.AddConstraint(c_var[0] <= 100)
prog.AddConstraint(u_var[0] <= 30)
prog.SetSolverOption(mp.SolverType.kIpopt, "print_level",
5) # CAUTION: Assuming that solver used Ipopt
print("CLF value:", Vx) # Current CLF value
prog.SetInitialGuess(u_var, U_fl)
prog.Solve() # Solve with default osqp
# solver.Solve(prog)
print("Optimal u : ", prog.GetSolution(u_var))
U_CLF_QP = prog.GetSolution(u_var)
C_CLF_QP = prog.GetSolution(c_var)
#---- Printing for debugging
# dx = robobee_plantBS.evaluate_f(U_fl,x)
# print("dx", dx)
# print("\n######################")
# # print("qe3:", A_fl[0,0])
# print("u:", u)
# print("\n####################33")
# deta4 = B_qw+Rqe3*U_fl_zero[0]+np.matmul(-np.matmul(Rqe3_hat,I_inv),U_fl_zero[1:4])*xi1
# deta6 = B_yaw+np.dot(g_yaw,U_fl_zero[1:4])
# print("deta4:",deta4)
# print("deta6:",deta6)
print(C_CLF_QP)
phi1_opt = np.dot(phi1_decouple, U_CLF_QP) + C_CLF_QP * eta_norm
phi1_opt_FL = np.dot(phi1_decouple, U_fl)
print("FL u: ", U_fl)
print("CLF u:", U_CLF_QP)
print("Cost FL: ", np.dot(mu, mu))
v_CLF = np.dot(A_fl, U_CLF_QP) + L_fhx_star
print("Cost CLF: ", np.dot(v_CLF, v_CLF))
print("Constraint FL : ", phi0_exp + phi1_opt_FL)
print("Constraint CLF : ", phi0_exp + phi1_opt)
u = U_CLF_QP
print("eigenvalues minQ maxP:", [min_e_Q, max_e_P])
return u
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}")
for residual in universe.constraint_state_to_orbit(state[-1], 'Mars'):
prog.AddConstraint(residual == 0)
# discretized dynamics
for t in range(time_steps):
residuals = universe.rocket_discrete_dynamics(state[t], state[t+1], thrust[t], time_interval)
for residual in residuals:
prog.AddConstraint(residual == 0)
# initial guess
state_guess = interpolate_rocket_state(
universe.get_planet('Earth').position,
universe.get_planet('Mars').position,
time_steps
)
prog.SetInitialGuess(state, state_guess)
"""Here is an empty cell for you to work in: detailed instructions are given below.
For the moment just run it as it is and continue to the plots.
**Troubleshooting:**
If you run the following cell multiple times, you keep adding more and more constraints to the optimization problem.
Hence, after you modify the code in the cell below, rerun the cell above to start with a fresh `MathematicalProgram`.
"""
# thrust limits, for all t:
# two norm of the rocket thrust
# lower or equal to the rocket thrust_limit
# modify here
for t in range(time_steps):
def optimal_trajectory(joints,
start_position,
end_position,
signed_dist_fn,
nc,
time=10,
knot_points=100):
assert len(joints) == len(start_position)
assert len(joints) == len(end_position)
h = time / (knot_points - 1)
nq = len(joints)
prog = MathematicalProgram()
q_var = []
v_var = []
for ii in range(knot_points):
q_var.append(prog.NewContinuousVariables(nq, "q[" + str(ii) + "]"))
v_var.append(prog.NewContinuousVariables(nq, "v[" + str(ii) + "]"))
# ---------------------------------------------------------------
# Initial & Final Pose Constraints ------------------------------
x_i = np.append(start_position, np.zeros(nq))
x_i_vars = np.append(q_var[0], v_var[0])
prog.AddLinearEqualityConstraint(np.eye(2 * nq), x_i, x_i_vars)
tol = 0.01 * np.ones(2 * nq)
x_f = np.append(end_position, np.zeros(nq))
x_f_vars = np.append(q_var[-1], v_var[-1])
prog.AddBoundingBoxConstraint(x_f - tol, x_f + tol, x_f_vars)
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points - 1):
dyn_con1 = np.hstack((np.eye(nq), np.eye(nq), -np.eye(nq)))
dyn_var1 = np.concatenate((q_var[ii], v_var[ii], q_var[ii + 1]))
prog.AddLinearEqualityConstraint(dyn_con1, np.zeros(nq), dyn_var1)
# ---------------------------------------------------------------
# Joint Limit Constraints ---------------------------------------
q_min = np.array([j.lower_limits() for j in joints])
q_max = np.array([j.upper_limits() for j in joints])
for ii in range(knot_points):
prog.AddBoundingBoxConstraint(q_min, q_max, q_var[ii])
# ---------------------------------------------------------------
# Collision Constraints -----------------------------------------
for ii in range(knot_points):
prog.AddConstraint(signed_dist_fn, np.zeros(nc), 1e8 * np.ones(nc),
q_var[ii])
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points):
prog.AddQuadraticErrorCost(np.eye(nq), np.zeros(nq), v_var[ii])
xi = np.array(start_position)
xf = np.array(end_position)
for ii in range(knot_points):
prog.SetInitialGuess(q_var[ii],
ii * (xf - xi) / (knot_points - 1) + xi)
prog.SetInitialGuess(v_var[ii], np.zeros(nq))
result = prog.Solve()
print prog.GetSolverId().name()
if result != SolutionResult.kSolutionFound:
print result
return None
q_path = []
# print signed_dist_fn(prog.GetSolution(q_var[0]))
for ii in range(knot_points):
q_path.append(prog.GetSolution(q_var[ii]))
return q_path
def compute_control(self, x_p1, x_p2, x_puck, p_goal, obstacles):
"""Solve for initial velocity for the puck to bounce the wall once and hit the goal."""
"""Currently does not work"""
# initialize program
N = self.mpc_params.N # length of receding horizon
prog = MathematicalProgram()
# State and input variables
x1 = prog.NewContinuousVariables(N + 1, 4,
name='p1_state') # state of player 1
u1 = prog.NewContinuousVariables(
N, 2, name='p1_input') # inputs for player 1
xp = prog.NewContinuousVariables(
N + 1, 4, name='puck_state') # state of the puck
# Slack variables
t1_kick = prog.NewContinuousVariables(
N + 1, name='kick_time'
) # slack variables that captures if player 1 is kicking or not at the given time step
# Defined as continous, but treated as mixed integer. 1 when kicking
v1_kick = prog.NewContinuousVariables(
N + 1, 2, name='v1_kick') # velocity of player after kick to puck
vp_kick = prog.NewContinuousVariables(
N + 1, 2, name='vp_kick'
) # velocity of puck after being kicked by the player
dist = prog.NewContinuousVariables(
N + 1, name='dist_puck_player') # distance between puck and player
cost = prog.NewContinuousVariables(
N + 1, name='cost') # slack variable to monitor cost
# Compute distance between puck and player as slack variable.
for k in range(N + 1):
r1 = self.sim_params.player_radius
rp = self.sim_params.puck_radius
prog.AddConstraint(
dist[k] == (x1[k, 0:2] - xp[k, 0:2]).dot(x1[k, 0:2] -
xp[k, 0:2]) -
(r1 + rp)**2)
#### COST and final states
# TODO: find adequate final velocity
x_puck_des = np.concatenate(
(p_goal, np.zeros(2)), axis=0) # desired position and vel for puck
for k in range(N + 1):
Q_dist_puck_goal = 10 * np.eye(2)
q_dist_puck_player = 0.1
e1 = x_puck_des[0:2] - xp[k, 0:2]
e2 = xp[k, 0:2] - x1[k, 0:2]
prog.AddConstraint(
cost[k] == e1.dot(np.matmul(Q_dist_puck_goal, e1)) +
q_dist_puck_player * dist[k])
prog.AddCost(cost[k])
#prog.AddQuadraticErrorCost(Q=self.mpc_params.Q_puck, x_desired=x_puck_des, vars=xp[k]) # puck in the goal
#prog.AddQuadraticErrorCost(Q=np.eye(2), x_desired=x_puck_des[0:2], vars=x1[k, 0:2])
#prog.AddQuadraticErrorCost(Q=10*np.eye(2), x_desired=np.array([2, 2]), vars=x1[k, 0:2]) # TEST: control position of the player instead of puck
#for k in range(N):
# prog.AddQuadraticCost(1e-2*u1[k].dot(u1[k])) # be wise on control effort
# Initial states for puck and player
prog.AddBoundingBoxConstraint(x_p1, x_p1,
x1[0]) # Intial state for player 1
prog.AddBoundingBoxConstraint(x_puck, x_puck,
xp[0]) # Initial state for puck
# Compute elastic collision for every possible timestep.
for k in range(N + 1):
v_puck_bfr = xp[k, 2:4]
v_player_bfr = x1[k, 2:4]
v_puck_aft, v_player_aft = self.get_reset_velocities(
v_puck_bfr, v_player_bfr)
prog.AddConstraint(eq(vp_kick[k], v_puck_aft))
prog.AddConstraint(eq(v1_kick[k], v_player_aft))
# Use slack variable to activate guard condition based on distance.
M = 15**2
for k in range(N + 1):
prog.AddLinearConstraint(dist[k] <= M * (1 - t1_kick[k]))
prog.AddLinearConstraint(dist[k] >= -t1_kick[k] * M)
# Hybrid dynamics for player
#for k in range(N):
# A = self.mpc_params.A_player
# B = self.mpc_params.B_player
# x1_kick = np.concatenate((x1[k][0:2], v1_kick[k]), axis=0) # The state of the player after it kicks the puck
# x1_next = np.matmul(A, (1 - t1_kick[k])*x1[k] + t1_kick[k]*x1_kick) + np.matmul(B, u1[k])
# prog.AddConstraint(eq(x1[k+1], x1_next))
# Assuming player dynamics are not affected by collision
for k in range(N):
A = self.mpc_params.A_player
B = self.mpc_params.B_player
x1_next = np.matmul(A, x1[k]) + np.matmul(B, u1[k])
prog.AddConstraint(eq(x1[k + 1], x1_next))
# Hybrid dynamics for puck_mass
for k in range(N):
A = self.mpc_params.A_puck
xp_kick = np.concatenate(
(xp[k][0:2], vp_kick[k]),
axis=0) # State of the puck after it gets kicked
xp_next = np.matmul(A, (1 - t1_kick[k]) * xp[k] +
t1_kick[k] * xp_kick)
prog.AddConstraint(eq(xp[k + 1], xp_next))
# Generate trajectory that is not in direct collision with the puck
for k in range(N + 1):
eps = 0.1
prog.AddConstraint(dist[k] >= -eps)
# Input and arena constraint
self.add_input_limits(prog, u1, N)
self.add_arena_limits(prog, x1, N)
self.add_arena_limits(prog, xp, N)
# fake mixed-integer constraint
#for k in range(N+1):
# prog.AddConstraint(t1_kick[k]*(1-t1_kick[k])==0)
# Hot-start
guess_u1, guess_x1 = self.get_initial_guess(x_p1, p_goal, x_puck[0:2])
prog.SetInitialGuess(x1, guess_x1)
prog.SetInitialGuess(u1, guess_u1)
if not self.prev_xp is None:
prog.SetInitialGuess(xp, self.prev_xp)
#prog.SetInitialGuess(t1_kick, np.ones_like(t1_kick))
# solve for the periods
# solver = SnoptSolver()
#result = solver.Solve(prog)
#if not result.is_success():
# print("Unable to find solution.")
# save for hot-start
#self.prev_xp = result.GetSolution(xp)
#if True:
# print("x1:{}".format(result.GetSolution(x1)))
# print("u1: {}".format( result.GetSolution(u1)))
# print("xp: {}".format( result.GetSolution(xp)))
# print('dist{}'.format(result.GetSolution(dist)))
# print("t1_kick:{}".format(result.GetSolution(t1_kick)))
# print("v1_kick:{}".format(result.GetSolution(v1_kick)))
# print("vp_kick:{}".format(result.GetSolution(vp_kick)))
# print("cost:{}".format(result.GetSolution(cost)))
# return whether successful, and the initial player velocity
#u1_opt = result.GetSolution(u1)
return True, guess_u1[0, :], np.zeros((2))
def generate_prog(self):
self.nn_conss = []
self.nn_costs = []
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def final_cost(x):
return 100.*(cos(.5*x[0])**2 + x[1]**2)
h = [];
u = [];
x = [];
xf = np.array([math.pi, 0.])
# Declare the MathematicalProgram vars up front in a good order, so that
# prog.decision_variables will be result of np.hstack(h.flatten(), u.flatten(), x.flatten(), T.flatten())
# for the following shapes: unfortunately, prog.decision_variables() will have these shapes:
# h = (num_trajectories, 1) | h = (num_trajectories, 1)
# u = (num_trajectories, num_inputs, num_samples) | u = (num_trajectories, num_samples, num_inputs)
# x = (num_trajectories, num_states, num_samples) | x = (num_trajectories, num_samples, num_states)
# T = (num_params) | T = (num_params)
for ti in range(self.num_trajectories):
h.append(prog.NewContinuousVariables(1,'h'+str(ti)))
for ti in range(self.num_trajectories):
u.append(prog.NewContinuousVariables(1, self.num_samples,'u'+str(ti)))
for ti in range(self.num_trajectories):
x.append(prog.NewContinuousVariables(2, self.num_samples,'x'+str(ti)))
# Add in constraints
for ti in range(self.num_trajectories):
prog.AddBoundingBoxConstraint(self.kMinimumTimeStep, self.kMaximumTimeStep, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
x0 = np.array(self.ic_list[ti]) # TODO: hopefully this isn't subtley bad...
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:,0])
# nudge = np.array([.2, .2])
# prog.AddBoundingBoxConstraint(xf-nudge, xf+nudge, x[ti][:,-1])
prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
# Do optional warm start here
if self.warm_start:
prog.SetInitialGuess(h[ti], [(self.kMinimumTimeStep+self.kMaximumTimeStep)/2])
for i in range(self.num_samples):
prog.SetInitialGuess(u[ti][:,i], [0.])
x_interp = (xf-x0)*i/self.num_samples + x0
prog.SetInitialGuess(x[ti][:,i], x_interp)
# prog.SetInitialGuess(u[ti][:,i], np.array(1.0))
for i in range(self.num_samples-1):
AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:,i], x[ti][:,i+1], u[ti][:,i], u[ti][:,i+1], prog)
for i in range(self.num_samples):
#prog.AddQuadraticCost([[2., 0.], [0., 2.]], [0., 0.], x[ti][:,i])
#prog.AddQuadraticCost([25.], [0.], u[ti][:,i])
u_var = u[ti][:,i]
x_var = x[ti][:,i] - xf
h_var = h[ti][0]
#print(u_var.shape, x_var.shape, xf.shape)
prog.AddCost( h_var * (2*x_var.dot(x_var) + 25*u_var.dot(u_var)) )
#prog.AddCost( 2*((x[0]-math.pi)*(x[0]-math.pi) + x[1]*x[1]) + 25*u.dot(u))
kTorqueLimit = 5
prog.AddBoundingBoxConstraint([-kTorqueLimit], [kTorqueLimit], u[ti][:,i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
# Setting solver options
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
#prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# Save references
self.prog = prog
self.h = h
self.u = u
self.x = x
self.T = []
class FourierCollocationProblem:
def __init__(self, system_dynamics, constraints):
start_time = time.time()
self.prog = MathematicalProgram()
self.N = 50 # Number of collocation points
self.M = 10 # Number of frequencies
self.system_dynamics = system_dynamics
self.psi = np.pi * (0.7) # TODO change
(
min_height,
max_height,
min_vel,
max_vel,
h0,
min_travelled_distance,
t_f_min,
t_f_max,
avg_vel_min,
avg_vel_max,
) = constraints
initial_pos = np.array([0, 0, h0])
# Add state trajectory parameters as decision variables
self.coeffs = self.prog.NewContinuousVariables(
3, self.M + 1, "c"
) # (x,y,z) for every frequency
self.phase_delays = self.prog.NewContinuousVariables(3, self.M, "eta")
self.t_f = self.prog.NewContinuousVariables(1, 1, "t_f")[0, 0]
self.avg_vel = self.prog.NewContinuousVariables(1, 1, "V_bar")[0, 0]
# Enforce initial conditions
residuals = self.get_pos_fourier(collocation_time=0) - initial_pos
for residual in residuals:
self.prog.AddConstraint(residual == 0)
# Enforce dynamics at collocation points
for n in range(self.N):
collocation_time = (n / self.N) * self.t_f
pos = self.get_pos_fourier(collocation_time)
vel = self.get_vel_fourier(collocation_time)
vel_dot = self.get_vel_dot_fourier(collocation_time)
residuals = self.continuous_dynamics(pos, vel, vel_dot)
for residual in residuals[3:6]: # TODO only these last three are residuals
self.prog.AddConstraint(residual == 0)
if True:
# Add velocity constraints
squared_vel_norm = vel.T.dot(vel)
self.prog.AddConstraint(min_vel ** 2 <= squared_vel_norm)
self.prog.AddConstraint(squared_vel_norm <= max_vel ** 2)
# Add height constraints
self.prog.AddConstraint(pos[2] <= max_height)
self.prog.AddConstraint(min_height <= pos[2])
# Add constraint on min travelled distance
self.prog.AddConstraint(min_travelled_distance <= self.avg_vel * self.t_f)
# Add constraints on coefficients
if False:
for coeff in self.coeffs.T:
lb = np.array([-500, -500, -500])
ub = np.array([500, 500, 500])
self.prog.AddBoundingBoxConstraint(lb, ub, coeff)
# Add constraints on phase delays
if False:
for etas in self.phase_delays.T:
lb = np.array([0, 0, 0])
ub = np.array([2 * np.pi, 2 * np.pi, 2 * np.pi])
self.prog.AddBoundingBoxConstraint(lb, ub, etas)
# Add constraints on final time and avg vel
self.prog.AddBoundingBoxConstraint(t_f_min, t_f_max, self.t_f)
self.prog.AddBoundingBoxConstraint(avg_vel_min, avg_vel_max, self.avg_vel)
# Add objective function
self.prog.AddCost(-self.avg_vel)
# Set initial guess
coeffs_guess = np.zeros((3, self.M + 1))
coeffs_guess += np.random.rand(*coeffs_guess.shape) * 100
self.prog.SetInitialGuess(self.coeffs, coeffs_guess)
phase_delays_guess = np.zeros((3, self.M))
phase_delays_guess += np.random.rand(*phase_delays_guess.shape) * 1e-1
self.prog.SetInitialGuess(self.phase_delays, phase_delays_guess)
self.prog.SetInitialGuess(self.avg_vel, (avg_vel_max - avg_vel_min) / 2)
self.prog.SetInitialGuess(self.t_f, (t_f_max - t_f_min) / 2)
print(
"Finished formulating the optimization problem in: {0} s".format(
time.time() - start_time
)
)
start_solve_time = time.time()
self.result = Solve(self.prog)
print("Found solution: {0}".format(self.result.is_success()))
print("Found a solution in: {0} s".format(time.time() - start_solve_time))
# TODO input costs
# assert self.result.is_success()
return
def get_solution(self):
coeffs_opt = self.result.GetSolution(self.coeffs)
phase_delays_opt = self.result.GetSolution(self.phase_delays)
t_f_opt = self.result.GetSolution(self.t_f)
avg_vel_opt = self.result.GetSolution(self.avg_vel)
sim_N = 100
dt = t_f_opt / sim_N
times = np.arange(0, t_f_opt, dt)
pos_traj = np.zeros((3, sim_N))
# TODO remove vel traj
vel_traj = np.zeros((3, sim_N))
for i in range(sim_N):
t = times[i]
pos = self.evaluate_pos_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
pos_traj[:, i] = pos
vel = self.evaluate_vel_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
vel_traj[:, i] = vel
# TODO move plotting out
plot_trj_3_wind(pos_traj.T, np.array([np.sin(self.psi), np.cos(self.psi), 0]))
plt.show()
breakpoint()
return
def evaluate_pos_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
pos_traj = np.copy(coeffs[:, 0])
for m in range(1, self.M):
sine_terms = np.array(
[
np.sin((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
pos_traj += coeffs[:, m] * sine_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi) * t,
avg_vel * np.cos(self.psi) * t,
0,
]
)
pos_traj += direction_term
return pos_traj
def evaluate_vel_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
cos_terms = np.array(
[
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
vel_traj += coeffs[:, m] * cos_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi),
avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_pos_fourier(self, collocation_time):
pos_traj = np.copy(self.coeffs[:, 0])
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
np.sin(a * collocation_time + self.phase_delays[0, m]),
np.sin(a * collocation_time + self.phase_delays[1, m]),
np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
pos_traj += self.coeffs[:, m] * sine_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi) * collocation_time,
self.avg_vel * np.cos(self.psi) * collocation_time,
0,
]
)
pos_traj += direction_term
return pos_traj
def get_vel_fourier(self, collocation_time):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
cos_terms = np.array(
[
a * np.cos(a * collocation_time + self.phase_delays[0, m]),
a * np.cos(a * collocation_time + self.phase_delays[1, m]),
a * np.cos(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_traj += self.coeffs[:, m] * cos_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi),
self.avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_vel_dot_fourier(self, collocation_time):
vel_dot_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[0, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[1, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_dot_traj += self.coeffs[:, m] * sine_terms
return vel_dot_traj
def continuous_dynamics(self, pos, vel, vel_dot):
x = np.concatenate((pos, vel))
x_dot = np.concatenate((vel, vel_dot))
# TODO need to somehow implement input to make this work
return x_dot - self.system_dynamics(x, u)
def optimize(self, vehs):
c = self.c
p = self.p
n_veh, L, N, dt, u_max = c.n_veh, c.circumference, c.n_opt, c.sim_step, c.u_max
L_veh = vehs[0].length
a, b, s0, T, v0, delta = p.a, p.b, p.s0, p.tau, p.v0, p.delta
vehs = vehs[::-1]
np.set_printoptions(linewidth=100)
# the controlled vehicle is now the first vehicle, positions are sorted from largest to smallest
print(f'Current positions {[veh.pos for veh in vehs]}')
v_init = [veh.speed for veh in vehs]
print(f'Current speeds {v_init}')
# spacing
s_init = [vehs[-1].pos - vehs[0].pos - L_veh] + [
veh_im1.pos - veh_i.pos - L_veh
for veh_im1, veh_i in zip(vehs[:-1], vehs[1:])
]
s_init = [s + L if s < 0 else s for s in s_init] # handle wrap
print(f'Current spacings {s_init}')
# can still follow current plan
if self.plan_index < len(self.plan):
accel = self.plan[self.plan_index]
self.plan_index += 1
return accel
print(f'Optimizing trajectory for {c.n_opt} steps')
## solve for equilibrium conditions (when all vehicles have same spacing and going at optimal speed)
import scipy.optimize
sf = L / n_veh - L_veh # equilibrium space
accel_fn = lambda v: a * (1 - (v / v0)**delta - ((s0 + v * T) / sf)**2)
sol = scipy.optimize.root(accel_fn, 0)
vf = sol.x.item() # equilibrium speed
sstarf = s0 + vf * T
print('Equilibrium speed', vf)
# nonconvex optimization
from pydrake.all import MathematicalProgram, SnoptSolver, eq, le, ge
# get guesses for solutions
v_guess = [np.mean(v_init)]
for _ in range(N):
v_guess.append(dt * accel_fn(v_guess[-1]))
s_guess = [sf] * (N + 1)
prog = MathematicalProgram()
v = prog.NewContinuousVariables(N + 1, n_veh, 'v') # speed
s = prog.NewContinuousVariables(N + 1, n_veh, 's') # spacing
flat = lambda x: x.reshape(-1)
# Guess
prog.SetInitialGuess(s, np.stack([s_guess] * n_veh, axis=1))
prog.SetInitialGuess(v, np.stack([v_guess] * n_veh, axis=1))
# initial conditions constraint
prog.AddLinearConstraint(eq(v[0], v_init))
prog.AddLinearConstraint(eq(s[0], s_init))
# velocity constraint
prog.AddLinearConstraint(ge(flat(v[1:]), 0))
prog.AddLinearConstraint(le(flat(v[1:]),
vf + 1)) # extra constraint to help solver
# spacing constraint
prog.AddLinearConstraint(ge(flat(s[1:]), s0))
prog.AddLinearConstraint(le(flat(s[1:]),
sf * 2)) # extra constraint to help solver
prog.AddLinearConstraint(eq(flat(s[1:].sum(axis=1)),
L - L_veh * n_veh))
# spacing update constraint
s_n = s[:-1, 1:] # s_i[n]
s_np1 = s[1:, 1:] # s_i[n + 1]
v_n = v[:-1, 1:] # v_i[n]
v_np1 = v[1:, 1:] # v_i[n + 1]
v_n_im1 = v[:-1, :-1] # v_{i - 1}[n]
v_np1_im1 = v[1:, :-1] # v_{i - 1}[n + 1]
prog.AddLinearConstraint(
eq(flat(s_np1 - s_n),
flat(0.5 * dt * (v_n_im1 + v_np1_im1 - v_n - v_np1))))
# handle position wrap for vehicle 1
prog.AddLinearConstraint(
eq(s[1:, 0] - s[:-1, 0],
0.5 * dt * (v[:-1, -1] + v[1:, -1] - v[:-1, 0] - v[1:, 0])))
# vehicle 0's action constraint
prog.AddLinearConstraint(ge(v[1:, 0], v[:-1, 0] - u_max * dt))
prog.AddLinearConstraint(le(v[1:, 0], v[:-1, 0] + u_max * dt))
# idm constraint
prog.AddConstraint(
eq((v_np1 - v_n - dt * a * (1 - (v_n / v0)**delta)) * s_n**2,
-dt * a * (s0 + v_n * T + v_n * (v_n - v_n_im1) /
(2 * np.sqrt(a * b)))**2))
if c.cost == 'mean':
prog.AddCost(-v.mean())
elif c.cost == 'target':
prog.AddCost(((v - vf)**2).mean() + ((s - sf)**2).mean())
solver = SnoptSolver()
result = solver.Solve(prog)
if not result.is_success():
accel = self.idm_backup.step(s_init[0], v_init[0], v_init[-1])
print(f'Optimization failed, using accel {accel} from IDM')
return accel
v_desired = result.GetSolution(v)
print('Planned speeds')
print(v_desired)
print('Planned spacings')
print(result.GetSolution(s))
a_desired = (v_desired[1:, 0] - v_desired[:-1, 0]
) / dt # we're optimizing the velocity of the 0th vehicle
self.plan = a_desired
self.plan_index = 1
return self.plan[0]
def direct_transcription():
prog = MathematicalProgram()
N = 500
dt = 0.01
# Create decision variables
n_x = 6
n_u = 2
x = np.empty((n_x, N), dtype=Variable)
u = np.empty((n_u, N - 1), dtype=Variable)
for n in range(N - 1):
x[:, n] = prog.NewContinuousVariables(n_x, "x" + str(n))
u[:, n] = prog.NewContinuousVariables(n_u, "u" + str(n))
x[:, N - 1] = prog.NewContinuousVariables(n_x, "x" + str(N))
T = N - 1
# Add constraints
# x0 = np.array([10, -np.pi / 2, 0, 40, 0, 0])
# Slotine dynamics: x = [airspeed, heading, flight_path_angle, z, x, y]
for n in range(N - 1):
# Dynamics
prog.AddConstraint(
eq(x[:, n + 1],
x[:, n] + dt * continuous_dynamics(x[:, n], u[:, n])))
# Never below sea level
prog.AddConstraint(x[3, n + 1] >= 0 + 0.5)
# Always positive velocity
prog.AddConstraint(x[0, n + 1] >= 0)
# TODO use residuals
# Add periodic constraints
prog.AddConstraint(x[0, 0] - x[0, T] == 0)
prog.AddConstraint(x[1, 0] - x[1, T] == 0)
prog.AddConstraint(x[2, 0] - x[2, T] == 0)
prog.AddConstraint(x[3, 0] - x[3, T] == 0)
# Start at 20 meter height
prog.AddConstraint(x[4, 0] == 0)
prog.AddConstraint(x[5, 0] == 0)
h0 = 20
prog.AddConstraint(x[3, 0] == h0)
# Add cost function
p0 = x[4:6, 0]
p1 = x[4:6, T]
travel_angle = np.pi # TODO start along y direction
dir_vector = np.array([np.sin(travel_angle), np.cos(travel_angle)])
prog.AddCost(dir_vector.T.dot(p1 - p0)) # Maximize distance travelled
print("Initialized opt problem")
# Initial guess
V0 = 10
x_guess = np.zeros((n_x, N))
x_guess[:, 0] = np.array([V0, travel_angle, 0, h0, 0, 0])
for n in range(N - 1):
# Constant airspeed, heading and height
x_guess[0, n + 1] = V0
x_guess[1, n + 1] = travel_angle
x_guess[3, n + 1] = h0
# Interpolate position
avg_speed = 10 # conservative estimate
x_guess[4:6, n + 1] = dir_vector * avg_speed * n * dt
# Let the rest of the variables be initialized to zero
prog.SetInitialGuess(x, x_guess)
# solve mathematical program
solver = SnoptSolver()
result = solver.Solve(prog)
# be sure that the solution is optimal
# assert result.is_success()
# retrieve optimal solution
thrust_opt = result.GetSolution(x)
state_opt = result.GetSolution(u)
result = Solve(prog)
x_sol = result.GetSolution(x)
u_sol = result.GetSolution(u)
breakpoint()
# Slotine dynamics: x = [airspeed, heading, flight_path_angle, z, x, y]
z = x_sol[:, 3]
x = x_sol[:, 4]
y = x_sol[:, 5]
plot_trj_3_wind(np.vstack((x, y, z)).T, get_wind_field)
return
