def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs.Substitute(dict(zip(prev_x, x))) Vdot = Vsdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] #mapping = dict(zip(x, np.ones(len(x)))) #V_norm = 0.0*V #for i in range(len(x)): # basis = np.ones(len(x)) # V_norm = V_norm + V.Substitute(dict(zip(x, basis))) #V = V/V_norm #Vdot = Vdot/V_norm #prog.AddConstraint(V_norm == 0) # in relative state (lambda(xbar) Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) #import pdb; pdb.set_trace() result = Solve(prog) if(not result.is_success()): print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) ) print('slack = %f' %(result.GetSolution(slack)) ) print('Rho = %f' %(rho)) #assert result.is_success() return -1.0 return result.GetSolution(slack)
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 CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): #import pdb; pdb.set_trace() prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() #print('V degree: %d' %(Polynomial(V).TotalDegree())) #print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree())) #print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree())) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) #assert result.is_success() return result.GetSolution(slack)
import manipulation.meshcat_cpp_utils as dut import numpy as np from pydrake.all import MathematicalProgram, Solve, Meshcat prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddCost(x.dot(x)) prog.AddBoundingBoxConstraint(-2, 2, x) result = Solve(prog) meshcat = Meshcat() X, Y = np.meshgrid(np.linspace(-3, 3, 35), np.linspace(-3, 3, 31)) dut.plot_mathematical_program(meshcat, "test", prog, X, Y, result)
x = v[0] y = v[1] # This is the famous "six-hump camel back function". It has six local # minima, two of them being global minima. p = 4*x**2+x*y - 4*y**2 - 2.1*x**4 + 4*y**4+x**6/3 # Find the minimum value by adding a sums of squares constraint, via # for all x, p(x) >= pmin # which we write as # p(x) - pmin is sos. pmin = prog.NewContinuousVariables(1, "pmin")[0] prog.AddSosConstraint(p-pmin) # Maximize pmin. prog.AddCost(-pmin) result = Solve(prog) assert(result.is_success()) print("Minimum value (lower bound): " + str(result.GetSolution(pmin))) # Now, let's plot it. fig = plt.figure(figsize=(10, 5)) ax0 = fig.add_subplot(121, projection='3d') ax1 = fig.add_subplot(122) xs = np.linspace(-2.2, 2.2, 51) ys = np.linspace(-1.2, 1.2, 51) [X, Y] = np.meshgrid(xs, ys) P = X.copy() for i in range(len(xs)):
# modify here for t in range(time_steps+1): for asteroid in asteroids: p = universe.position_wrt_planet(state[t], asteroid.name) prog.AddConstraint(np.linalg.norm(p) >= asteroid.orbit) # minimize fuel consumption, for all t: # add to the objective the two norm squared of the thrust # multiplied by the time_interval so that the optimal cost # approximates the time integral of the thrust squared # modify here cost = 0 for t in range(time_steps): cost += time_interval * np.dot(thrust[t], thrust[t]); prog.AddCost(cost) """Now that we have written the optimization problem, we can solve it using the nonlinear optimization solver `Snopt`. **Troubleshooting:** `Snopt` is a commercial solver and requires a licence to be used. `Snopt` is included in the precompiled binaries of Drake. Therefore, if you installed Drake using the precompiled binaries (or you are working in Colab or Binder), the following cell will run fine. But, if you built Drake from source, the following cell won't work unless you built Drake with the `Snopt` option enabled ([see here](https://drake.mit.edu/python_bindings.html)) and you have a `Snopt` licence on your machine. In that case, you can consider the alternative free solver `IPOPT` which can be imported running `from pydrake.all import IpoptSolver`. """ # solve mathematical program solver = SnoptSolver() result = solver.Solve(prog)
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))
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)
import numpy as np from pydrake.all import Quaternion from pydrake.all import MathematicalProgram, Solve, eq, le, ge, SolverOptions, SnoptSolver import pdb prog = MathematicalProgram() x = prog.NewContinuousVariables(rows=1, name='x') y = prog.NewContinuousVariables(rows=1, name='y') slack = prog.NewContinuousVariables(rows=1, name="slack") prog.AddConstraint(eq(x * y, slack)) prog.AddLinearConstraint(ge(x, 0)) prog.AddLinearConstraint(ge(y, 0)) prog.AddLinearConstraint(le(x, 1)) prog.AddLinearConstraint(le(y, 1)) prog.AddCost(1e5 * (slack**2)[0]) prog.AddCost(-(2 * x[0] + y[0])) solver = SnoptSolver() result = solver.Solve(prog) print( f"Success: {result.is_success()}, x = {result.GetSolution(x)}, y = {result.GetSolution(y)}, slack = {result.GetSolution(slack)}" )
def formulate_optimization(environment_name, obstacles): ''' Generate and return an optimization problem. All decision variables, constraints, and costs are added here. ''' def discrete_dynamics(state, state_next, thrust): '''Forward euler method to approximate discretized dynamics''' state_dot = generate_dynamics(state, thrust, env_name=environment_name) residuals = state_next - state - time_interval * state_dot return residuals def squared_distance_to_obstacle(boat_pos, obs_pos): '''Compute the squared distance from the boat to an obstacle''' vec = boat_pos - obs_pos return vec.dot(vec) # initialize optimization prog = MathematicalProgram() opt_state = prog.NewContinuousVariables(duration + 1, 6, 'optimal_state') opt_thrust = prog.NewContinuousVariables(duration, 2, 'optimal_thrust') if opt_switches[0]: # initial state constraint for i in range(len(start)): prog.AddConstraint(opt_state[0, i] - start[i], 0., 0.).evaluator().set_description("Initial State") if opt_switches[1]: # goal state constraint for i in range(len(start)): prog.AddConstraint( opt_state[-1, i] - target[i], -final_tolerances[i], final_tolerances[i]).evaluator().set_description( f"Final State {i}") if opt_switches[2]: # enforce dynamics for t in range(duration): residuals = discrete_dynamics(opt_state[t], opt_state[t + 1], opt_thrust[t]) for i, residual in enumerate(residuals): prog.AddConstraint(residual, -dynamics_tolerances[i], dynamics_tolerances[i]) if opt_switches[3]: # thrust limits for t in range(duration): prog.AddConstraint(opt_thrust[t][0], -thrust_lim[0], thrust_lim[0]) prog.AddConstraint(opt_thrust[t][1], -thrust_lim[1], thrust_lim[1]) if opt_switches[4]: # avoid collisions for i, obstacle in enumerate(obstacles): for t in range(duration + 1): d2 = squared_distance_to_obstacle(opt_state[t, 0:2], obstacle.traj[t]) prog.AddConstraint(d2 >= obstacle.radius**2 + (w / 2)**2) if opt_switches[5]: # fuel cost for t in range(duration): prog.AddCost(time_interval * opt_thrust[t, 0]**2) if opt_switches[6]: # distance cost for t in range(duration): prog.AddCost(time_interval * (opt_state[t, 3]**2 + opt_state[t, 4]**2)) return prog, opt_state, opt_thrust
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 __init__(self, start, goal, degree, n_segments, duration, regions, H): #sample_times, num_vars, continuity_degree): R = len(regions) N = n_segments M = 100 prog = MathematicalProgram() # Set H if H is None: H = prog.NewBinaryVariables(R, N) for n_idx in range(N): prog.AddLinearConstraint(np.sum(H[:, n_idx]) == 1) poly_xs, poly_ys, poly_zs = [], [], [] vars_ = np.zeros((N, )).astype(object) for n_idx in range(N): # for each segment t = prog.NewIndeterminates(1, "t" + str(n_idx)) vars_[n_idx] = t[0] poly_x = prog.NewFreePolynomial(Variables(t), degree, "c_x_" + str(n_idx)) poly_y = prog.NewFreePolynomial(Variables(t), degree, "c_y_" + str(n_idx)) poly_z = prog.NewFreePolynomial(Variables(t), degree, "c_z_" + str(n_idx)) for var_ in poly_x.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) for var_ in poly_y.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) for var_ in poly_z.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) poly_xs.append(poly_x) poly_ys.append(poly_y) poly_zs.append(poly_z) phi = np.array([poly_x, poly_y, poly_z]) for r_idx, region in enumerate(regions): # if r_idx == 0: # break A = region[0] b = region[1] b = b + (1 - H[r_idx, n_idx]) * M b = [Polynomial(this_b) for this_b in b] q = b - A.dot(phi) sigma = [] for q_idx in range(len(q)): sigma_1 = prog.NewFreePolynomial(Variables(t), degree - 1) prog.AddSosConstraint(sigma_1) sigma_2 = prog.NewFreePolynomial(Variables(t), degree - 1) prog.AddSosConstraint(sigma_2) sigma.append( Polynomial(t[0]) * sigma_1 + (1 - Polynomial(t[0])) * sigma_2) # for var_ in sigma[q_idx].decision_variables(): # prog.AddLinearConstraint(var_<=M) # prog.AddLinearConstraint(var_>=-M) q_coeffs = q[q_idx].monomial_to_coefficient_map() sigma_coeffs = sigma[q_idx].monomial_to_coefficient_map() both_coeffs = set(q_coeffs.keys()) & set( sigma_coeffs.keys()) for coeff in both_coeffs: # import pdb; pdb.set_trace() prog.AddConstraint( q_coeffs[coeff] == sigma_coeffs[coeff]) # res = Solve(prog) # print("x: " + str(res.GetSolution(poly_xs[0].ToExpression()))) # print("y: " + str(res.GetSolution(poly_ys[0].ToExpression()))) # print("z: " + str(res.GetSolution(poly_zs[0].ToExpression()))) # import pdb; pdb.set_trace() # for this_q in q: # prog.AddSosConstraint(this_q) # import pdb; pdb.set_trace() # cost = 0 print("Constraint: x0(0)=x0") prog.AddConstraint( poly_xs[0].ToExpression().Substitute(vars_[0], 0.0) == start[0]) prog.AddConstraint( poly_ys[0].ToExpression().Substitute(vars_[0], 0.0) == start[1]) prog.AddConstraint( poly_zs[0].ToExpression().Substitute(vars_[0], 0.0) == start[2]) for idx, poly_x, poly_y, poly_z in zip(range(N), poly_xs, poly_ys, poly_zs): if idx < N - 1: print("Constraint: x" + str(idx) + "(1)=x" + str(idx + 1) + "(0)") next_poly_x, next_poly_y, next_poly_z = poly_xs[ idx + 1], poly_ys[idx + 1], poly_zs[idx + 1] prog.AddConstraint( poly_x.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_x.ToExpression().Substitute(vars_[idx + 1], 0.0)) prog.AddConstraint( poly_y.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_y.ToExpression().Substitute(vars_[idx + 1], 0.0)) prog.AddConstraint( poly_z.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_z.ToExpression().Substitute(vars_[idx + 1], 0.0)) else: print("Constraint: x" + str(idx) + "(1)=xf") prog.AddConstraint(poly_x.ToExpression().Substitute( vars_[idx], 1.0) == goal[0]) prog.AddConstraint(poly_y.ToExpression().Substitute( vars_[idx], 1.0) == goal[1]) prog.AddConstraint(poly_z.ToExpression().Substitute( vars_[idx], 1.0) == goal[2]) # for cost = Expression() for var_, polys in zip(vars_, [poly_xs, poly_ys, poly_zs]): for poly in polys: for _ in range(2): #range(2): poly = poly.Differentiate(var_) poly = poly.ToExpression().Substitute({var_: 1.0}) cost += poly**2 # for dv in poly.decision_variables(): # cost += (Expression(dv))**2 prog.AddCost(cost) res = Solve(prog) print("x: " + str(res.GetSolution(poly_xs[0].ToExpression()))) print("y: " + str(res.GetSolution(poly_ys[0].ToExpression()))) print("z: " + str(res.GetSolution(poly_zs[0].ToExpression()))) self.poly_xs = [ res.GetSolution(poly_x.ToExpression()) for poly_x in poly_xs ] self.poly_ys = [ res.GetSolution(poly_y.ToExpression()) for poly_y in poly_ys ] self.poly_zs = [ res.GetSolution(poly_z.ToExpression()) for poly_z in poly_zs ] self.vars_ = vars_ self.degree = degree
def TimeVaryingLyapunovSearchRho(self, prev_x, Vs, Vdots, Ts, times, xtraj, utraj, \ rho_f, multiplier_degree=None): C = 1.0 #8.0 #rho_f = 3.0 tries = 40 prev_rhointegral = 0. N = len(times)-1 #Vmin = self.minimumV(prev_x, Vs) #0.05*np.ones((1, len(times))) # need to do something here instead of this!! dt = np.diff(times) #rho = np.flipud(rho_f*np.exp(-C*(np.array(times)-times[0])/(times[-1]-times[0])))# + np.max(Vmin) #rho = np.linspace(0.1, rho_f, N+1) #rho = np.linspace(rho_f/2.0, rho_f, N+1) rho = np.linspace(rho_f*3.0, rho_f, N+1) fig, ax = plt.subplots() fig.suptitle('Rho progression') ax.set(xlabel='index', ylabel='rho') ax.grid(True) plt.show(block = False) need_to_break = False for idx in range(tries): print('starting iteration #%d with rho=' %(idx)) print(rho) ax.plot(rho) plt.pause(0.05) # start of number of iterations if we want rhodot = np.diff(rho)/dt # sampleCheck() Lambda_vec = [] x_vec = [] #import pdb; pdb.set_trace() #fix rho, optimize Lagrange multipliers for i in range(N): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs[i].Substitute(dict(zip(prev_x, x))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, x))) x0 = xtraj.value(times[i]).transpose()[0] Ttrans = np.linalg.inv(Ts[i]) x0 = Ttrans.dot(x0) #xmin, vmin, vdmin = self.SampleCheck(x, V, Vdot) #if(vdmin > rhodot[i]): # print('Vdot is greater than rhodot!') #Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = prog.NewFreePolynomial(Variables(x), multiplier_degree).ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) gamma = prog.NewContinuousVariables(1,'g')[0] # Jdot-rhodot+Lambda*(rho-J) < -gamma prog.AddSosConstraint( -gamma*V - (Vdot-rhodot[i] + Lambda*(rho[i]-V)) ) prog.AddCost(-gamma) #maximize gamma result = Solve(prog) if result.is_success() == False: need_to_break = True print('Solver could not solve anymore') import pdb; pdb.set_trace() break else: Lambda_vec.append(result.GetSolution(Lambda)) slack = result.GetSolution(gamma) print('Slack #%d = %f' %(idx, slack)) x_vec.append(x) if(slack < 0.0): print('In iter#%d, found negative slack so going to end prematurely... :(' %(idx)) need_to_break = True if(need_to_break == True): break; # fix Lagrange multipliers, maximize rho rhointegral = 0.0 prog = MathematicalProgram() xx = prog.NewIndeterminates(len(x),'x') t = prog.NewContinuousVariables(N,'t') #import pdb; pdb.set_trace() #rho = np.concatenate((t,[rho_f])) + Vmin rho_x = np.concatenate((t,[rho[-1]])) #+ rho #import pdb; pdb.set_trace() for i in range(N): #prog.AddConstraint(t[i]>=0.0) # does this mean [prog,rho] = new(prog,N,'pos'); in matlab?? rhod_x = (rho_x[i+1]-rho_x[i])/dt[i] #prog.AddConstraint(rhod_x<=0.0) rhointegral = rhointegral+rho_x[i]*dt[i] + 0.5*rhod_x*(dt[i]**2) V = Vs[i].Substitute(dict(zip(prev_x, xx))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, xx))) #x0 = xtraj.value(times[i]).transpose()[0] L1 = Lambda_vec[i].Substitute(dict(zip(x_vec[i], xx))) #Vdot = Vdot*rho_x[i] - V*rhod_x prog.AddSosConstraint( -(Vdot - rhod_x + L1 * ( rho_x[i]-V ) ) ) prog.AddCost(-rhointegral) result = Solve(prog) assert result.is_success() rhos = result.GetSolution(rho_x) rho = [] for r in rhos: rho.append(r[0].Evaluate()) rhointegral = result.GetSolution(rhointegral).Evaluate() if( (rhointegral-prev_rhointegral)/rhointegral < 1E-5): # 0.1% print('Rho integral converged') need_to_break = True break; else: prev_rhointegral = rhointegral print('End of iteration #%d: rhointegral=%f' %(idx, rhointegral)) if(need_to_break == True): print('In iter#%d, found negative slack so ending prematurely... :(' %(idx)) break; # end of iterations if we want ax.plot(rho) plt.pause(0.05) print('done computing funnel.\nFinal rho= ') print(rho) #import pdb; pdb.set_trace() for i in range(len(rho)): Vs[i] = Vs[i]/rho[i] return Vs
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 Optimization: def __init__(self, config): self.physical_params = config["physical_params"] self.T = config["T"] self.dt = config["dt"] self.initial_state = config["xinit"] self.goal_state = config["xgoal"] self.ellipse_arc = config["ellipse_arc"] self.deviation_cost = config["deviation_cost"] self.Qf = config["Qf"] self.limits = config["limits"] self.n_state = 6 self.n_nominal_forces = 4 self.tire_model = config["tire_model"] self.initial_guess_config = config["initial_guess_config"] self.puddle_model = config["puddle_model"] self.force_constraint = config["force_constraint"] self.visualize_initial_guess = config["visualize_initial_guess"] self.dynamics = Dynamics(self.physical_params.lf, self.physical_params.lr, self.physical_params.m, self.physical_params.Iz, self.dt) def build_program(self): self.prog = MathematicalProgram() # Declare variables. state = self.prog.NewContinuousVariables(rows=self.T + 1, cols=self.n_state, name='state') nominal_forces = self.prog.NewContinuousVariables( rows=self.T, cols=self.n_nominal_forces, name='nominal_forces') steers = self.prog.NewContinuousVariables(rows=self.T, name="steers") slip_ratios = self.prog.NewContinuousVariables(rows=self.T, cols=2, name="slip_ratios") self.state = state self.nominal_forces = nominal_forces self.steers = steers self.slip_ratios = slip_ratios # Set the initial state. xinit = pack_state_vector(self.initial_state) for i, s in enumerate(xinit): self.prog.AddConstraint(state[0, i] == s) # Constrain xdot to always be at least some value to prevent numerical issues with optimizer. for i in range(self.T + 1): s = unpack_state_vector(state[i]) self.prog.AddConstraint(s["xdot"] >= self.limits.min_xdot) # Constrain slip ratio to be at least a certain magnitude. if i != self.T: self.prog.AddConstraint( slip_ratios[i, 0]**2.0 >= self.limits.min_slip_ratio_mag**2.0) self.prog.AddConstraint( slip_ratios[i, 1]**2.0 >= self.limits.min_slip_ratio_mag**2.0) # Control rate limits. for i in range(self.T - 1): ddelta = self.dt * (steers[i + 1] - steers[i]) f_dkappa = self.dt * (slip_ratios[i + 1, 0] - slip_ratios[i, 0]) r_dkappa = self.dt * (slip_ratios[i + 1, 1] - slip_ratios[i, 1]) self.prog.AddConstraint(ddelta <= self.limits.max_ddelta) self.prog.AddConstraint(ddelta >= -self.limits.max_ddelta) self.prog.AddConstraint(f_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(f_dkappa >= -self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa >= -self.limits.max_dkappa) # Control value limits. for i in range(self.T): self.prog.AddConstraint(steers[i] <= self.limits.max_delta) self.prog.AddConstraint(steers[i] >= -self.limits.max_delta) # Add dynamics constraints by constraining residuals to be zero. for i in range(self.T): residuals = self.dynamics.nominal_dynamics(state[i], state[i + 1], nominal_forces[i], steers[i]) for r in residuals: self.prog.AddConstraint(r == 0.0) # Add the cost function. self.add_cost(state) # Add a different force constraint depending on the configuration. if self.force_constraint == ForceConstraint.NO_PUDDLE: self.add_no_puddle_force_constraints( state, nominal_forces, steers, self.tire_model.get_deterministic_model(), slip_ratios) elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED: self.add_mean_constrained() elif self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: self.add_chance_constraints() else: raise NotImplementedError("ForceConstraint type invalid.") return def constant_input_initial_guess(self, state, steers, slip_ratios, nominal_forces): # Guess the slip ratio as the minimum allowed value. gslip_ratios = np.tile( np.array([ self.limits.min_slip_ratio_mag, self.limits.min_slip_ratio_mag ]), (self.T, 1)) # Guess the slip angle as a linearly ramping steer that then becomes constant. # This is done by creating an array of values corresponding to end_delta then # filling in the initial ramp up phase. gsteers = self.initial_guess_config["end_delta"] * np.ones(self.T) igc = self.initial_guess_config for i in range(igc["ramp_steps"]): gsteers[i] = (i / igc["ramp_steps"]) * (igc["end_delta"] - igc["start_delta"]) # Create empty arrays for state and forces. gstate = np.empty((self.T + 1, self.n_state)) gstate[0] = pack_state_vector(self.initial_state) gforces = np.empty((self.T, 4)) all_slips = self.T * [None] # Simulate the dynamics for the initial guess differently depending on the force # constraint being used. if self.force_constraint == ForceConstraint.NO_PUDDLE: tire_model = self.tire_model.get_deterministic_model() for i in range(self.T): s = unpack_state_vector(gstate[i]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED or self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Update the tire model based off the conditions of the world # at (x, y) s = unpack_state_vector(gstate[i]) tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips else: raise NotImplementedError( "Invalid value for self.force_constraint") # Declare array for the initial guess and set the values. initial_guess = np.empty(self.prog.num_vars()) self.prog.SetDecisionVariableValueInVector(state, gstate, initial_guess) self.prog.SetDecisionVariableValueInVector(steers, gsteers, initial_guess) self.prog.SetDecisionVariableValueInVector(slip_ratios, gslip_ratios, initial_guess) self.prog.SetDecisionVariableValueInVector(nominal_forces, gforces, initial_guess) if self.visualize_initial_guess: # TODO: reorganize visualizations psis = gstate[:, 2] xs = gstate[:, 4] ys = gstate[:, 5] fig, ax = plt.subplots() if self.force_constraint != ForceConstraint.NO_PUDDLE: plot_puddles(ax, self.puddle_model) plot_ellipse_arc(ax, self.ellipse_arc) plot_planned_trajectory(ax, xs, ys, psis, gsteers, self.physical_params) # Plot the slip ratios/angles plot_slips(all_slips) plot_forces(gforces) if self.force_constraint == ForceConstraint.NO_PUDDLE: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=None) else: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=self.puddle_model) return initial_guess def solve_program(self): # Generate initial guess initial_guess = self.constant_input_initial_guess( self.state, self.steers, self.slip_ratios, self.nominal_forces) # Solve the problem. solver = SnoptSolver() result = solver.Solve(self.prog, initial_guess) solver_details = result.get_solver_details() print("Exit flag: " + str(solver_details.info)) self.visualize_results(result) def visualize_results(self, result): state_res = result.GetSolution(self.state) psis = state_res[:, 2] xs = state_res[:, 4] ys = state_res[:, 5] steers = result.GetSolution(self.steers) fig, ax = plt.subplots() plot_ellipse_arc(ax, self.ellipse_arc) plot_puddles(ax, self.puddle_model) plot_planned_trajectory(ax, xs, ys, psis, steers, self.physical_params) generate_animation(xs, ys, psis, steers, self.physical_params, self.dt, puddle_model=self.puddle_model) plt.show() def add_cost(self, state): # Add the final state cost function. diff_state = pack_state_vector(self.goal_state) - state[-1] self.prog.AddQuadraticCost(diff_state.T @ self.Qf @ diff_state) # Get the approx distance function for the ellipse. fun = self.ellipse_arc.approx_dist_fun() for i in range(self.T): s = unpack_state_vector(state[i]) self.prog.AddCost(self.deviation_cost * fun(s["X"], s["Y"])) def add_mean_constrained(self): # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(self.state[i]) F = unpack_force_vector(self.nominal_forces[i]) # get the tire model at this position in space. tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Unpack values delta = self.steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = self.slip_ratios[i, 0] kappa_r = self.slip_ratios[i, 1] # Compute expected forces. E_Ffx, E_Ffy = tire_model(kappa_f, alpha_f) E_Frx, E_Fry = tire_model(kappa_r, alpha_r) # Constrain these expected force values to be equal to the nominal # forces in the optimization. self.prog.AddConstraint(E_Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(E_Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(E_Frx - F["r_long"] == 0.0) self.prog.AddConstraint(E_Fry - F["r_lat"] == 0.0) def add_chance_constrained(self): pass def add_no_puddle_force_constraints(self, state, forces, steers, pacejka_model, slip_ratios): """ Args: prog: state: forces: steers: pacejka_model: function with signature (slip_ratio, slip_angle) using pydrake.math """ for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(state[i]) F = unpack_force_vector(forces[i]) delta = steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = slip_ratios[i, 0] kappa_r = slip_ratios[i, 1] Ffx, Ffy = pacejka_model(kappa_f, alpha_f) Frx, Fry = pacejka_model(kappa_r, alpha_r) # Constrain the values from the pacejka model to be equal # to the desired values in the optimization. self.prog.AddConstraint(Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(Frx - F["r_long"] == 0.0) self.prog.AddConstraint(Fry - F["r_lat"] == 0.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