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