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
