Minimum time manouevering problem + boundaries #219
-
|
HI @pulsipher, thank you for developing this library it's great. I've been looking into using InifiniteOpt for solving a problem similar to those discussed #188 and #192. The idea is to solve the trajectory optimization problem for a model of a car going around the track and find the controls that give the shortest lap time. The catch is that the controls must be chosen such that the car never leaves the track, so the problem has non-linear constraints on the trajectory too (the model is also non linear). Here's some references: Massaro, Anderson, Kapania. Is this something that InfiniteOpt could be used for? Would would be a good way to model the constraints? Thank you! |
Beta Was this translation helpful? Give feedback.
Replies: 9 comments 7 replies
-
|
Hi @FedeClaudi and welcome! This is certainly an interesting research area. As I discussed in #192, how to go about formulating and solving this problem is an open question. Now because you don't have explicit waypoints you might be able to derive a formulation where you seek to minimize the final time and constrain the position to remain in the track bounds. The trick here will be finding a clever way to force it to do one lap around the track (avoiding the trivial solution of remaining at the start/finish line to obtain a final time of 0s). I have seen some approaches do a change of variables such that the problem is parameterized by progress along the track and the formulation then seeks to maximize the progress (the MPCC approach used here might be of interest). Finding a closed form constraint for the boundary will also be challenging, but you may be able to use our function registration syntax to help. If you can come up with a single level optimization formulation, then we can try to implement it in InfiniteOpt. Nonlinearity shouldn't be a problem. I would recommend starting with simple point mass equations and a track with a separate start and finish line. I should also note that InfiniteOpt.jl will probably be a good sandbox for developing this, but ultimately it probably has too much overhead to be used in real-time vehicle control. In which case, an application specific software implementation can be developed. InfiniteOpt should be helpful quickly testing a variety of formulations and solution techniques. |
Beta Was this translation helpful? Give feedback.
-
|
Thank you so much for the quick and insightful reply. The idea of starting with a simple point mass model is very clever. Our actual model would be simpler than what most people in the field use anyway, but starting easy is always good. Indeed most of the research I've seen re-casts the problem by expressing the vehicles in the track's 'curvilinear' coordinates and using an objective function aimed at maximizing track progress. I'm comfortable with the maths behind this, but haven't managed to implement a working solution in code yet. I can do non-linear constrainted finite time problems with JuMP, but I'm not sure how to write the code for a problem parametrized in terms of track distance instead of time. I will keep experimenting for now, but it's great to know that in principle InfiniteOpt should work. The MPCC approach in the link you've sent seems great. I need to look at it in more details, but they seem to be using a finite horizon approach? For my needs something similar in Julia would be perfect, perhaps I'll try to translate their approach in InfiniteOpt. P.s.: I don't need to do actual vehicle control, the idea is to compare a simple model + optimal control solution to the behavior of animals running in an experimental arena with the same shape (I'm doing a PhD in neuroscience). So overhead/running latency are not a huge issue, and the model doesn't need to be super realistic. |
Beta Was this translation helpful? Give feedback.
-
|
That sounds like a very cool project. Yes I think InfiniteOpt will be a good fit. The MPCC approach is probably a little more involved than you need, it is over a fixed amount of time (i.e., a few seconds) but it tries to go as far as possible in that amount of time. However, based on your description you can probably just do the following:
Registering like this won't be the most efficient approach, but it might just work for your purposes. |
Beta Was this translation helpful? Give feedback.
-
|
Thanks!
If the time interval is small and the process is repeated iteratively along the entire track that should give a minimum time trajectory (or close enough)? I've been making good progress with the suggested approach, but I have a couple questions: One
I'm not sure what you mean by register that function? So far I've been using TwoI can, as you suggest, set a waypoint at the end of the track and ask that the model reaches it as quickly as possible while staying within the track's boundary. Preliminary experiments suggest that this should work, but it seems very slow (i.e. IPoPT not converging within 3000 iterations for a very simple problem). On the other hand, most papers take a different approach. They define a variable However, when I try to do this with Infinite opt I can't set up the integral. Given the If I define cost = (ẋ * cos(e_ψ) + ẏ * sin(e_ψ))/(1 - e_y * κ)
@objective(model, Min, ∫(cost, τ))works, but cost = (ẋ * cos(e_ψ) + ẏ * sin(e_ψ))/(1 - e_y * κ)
@objective(model, Min, ∫(cost, s))doesn't: Aren't both Thank you so much with all the help, it's very much appreciated. |
Beta Was this translation helpful? Give feedback.
-
|
The error above occurs because the 2nd objective function integrates over Also, you could break the problem up as you suggest via MPCC, but be warned that it can be tricky to implement and that there is no guarantee the overall result would be close to optimal (but it could be). The registration I was referring to is described here. I recommended it since I figured you would likely not have an algebraic representation of the path you could differentiate as you show above, but having an explicit algebraic form is better. Now let's take a step back here. You are completely right that most of the studies in the literature parameterize the problem in terms of a progress parameter (also called an independent variable, but this should amount to an infinite parameter in InfiniteOpt). The |
Beta Was this translation helpful? Give feedback.
-
|
I had some time and put together a modification of the minimum time hovercraft problem that implements basic path planning. For simplicity I let the track boundaries follow a semi-circle shape. using InfiniteOpt, Ipopt, Plots
# Define the model
m = InfiniteModel(Ipopt.Optimizer)
# Define the scaled time
@infinite_parameter(m, τ ∈ [0, 1], num_supports = 101)
# guess trajectory
guess_xs = [t -> 5 - 5 * cos(t * pi), t -> 5 * sin(t * pi)]
# Add the variables
@variables(m, begin
# state variables
x[i = 1:2], Infinite(τ), (start = guess_xs[i])
v[1:2], Infinite(τ)
# control variables
-1 <= u[1:2] <= 1, Infinite(τ), (start = 0) # enforce control limits
0 <= tf <= 500, (start = 60)
end)
# Set the objective
@objective(m, Min, tf)
# Set the constraints
@constraint(m, [i = 1:2], v[i](0) == 0)
@constraint(m, [i = 1:2], ∂(x[i], τ) == tf * v[i])
@constraint(m, [i = 1:2], ∂(v[i], τ) == tf * u[i])
# Set the initial and final conditions
@constraint(m, [i = 1:2], x[i](0) == 0)
@constraint(m, x[1](1) == 10)
@constraint(m, x[2](1) == 0)
# Set the track constraints
@constraint(m, (x[1] - 5)^2 + x[2]^2 >= 9)
@constraint(m, (x[1] - 5)^2 + x[2]^2 <= 36)
# Solve and get the results
optimize!(m)
opt_tf = value(tf)
opt_x = value.(x)
opt_u = value.(u)
# Plot the results
function circle_shape(h, k, r)
θ = LinRange(0, 2 * pi, 500)
return h .+ r * sin.(θ), k .+ r * cos.(θ)
end
plot(opt_x[1], opt_x[2], label = "Trajectory", ylims = (0, 8))
plot!(circle_shape(5, 0, 3), linecolor = :red, label = "Boundary")
plot!(circle_shape(5, 0, 6), linecolor = :red, label = "")
xlabel!("x_1")
ylabel!("x_2")
Note that this approach like many other path planning formulations is very sensitive to initial guesses. This is why I give a pretty good trajectory guess. For more complex tracks, one could define a piece wise linear center path. Then a registered function that assesses the distance of a point to the center path can be used to constrain the distance away from it. And the centerline function could be used to as a good initial guess for the trajectory. |
Beta Was this translation helpful? Give feedback.
-
|
That's awesome. Thank you so much for all the help and for looking into this further. That solution looks great, I didn't realize you could do something like I was about to actually post an update here, because I've made some good progress with this and I thought you might be curious about it. I ended up formulating the problem in terms of track creationusing InfiniteOpt, Ipopt, Plots
import MyterialColors: black, white, salmon
import InfiniteOpt: set_optimizer_attribute
"""
Model:
A mass particle moving in a 2D plane.
It has a mass (m), position (XY), an orientation (θ),
a speed (v, velocity towards θ) and an angular velocity (ω).
There are two controls:
- F_v: force acting on speed such that v̇ = F_v/m
- F_ω: force making the particle rotate: θ̇ = F_θ/m
Vairables meaning
- s: track progress
- n: distance from center line
- ψ: angular error between vehicle and track
- v: speed
- β: velocity vector splip angle
- ω: angular velocity
(for results computation only)
- x: x position
- y: y position
- θ: vehicle orientation angle
"""
# ---------------------------------------------------------------------------- #
# TRACK #
# ---------------------------------------------------------------------------- #
# define the half circle
δT = .15
T = -π/2:δT:π/2
N = length(T)
X, Y = T, cos.(T)
track = [X Y]
S_f = sum(sqrt.(diff(X).^2 .+ diff(Y).^2))
Define a function to compute the curvature of the track at mag(vec) = sqrt(sum(vec.^2))
"""
Track curvature at point closest to model
`s` is a value ∈ [0 S_f], to compute the curvature we need to get a corresponding value t ∈ T.
With that we get two vectors track[t-1 -> t] and track[t -> t+1] and look at the Δ angle between the two.
"""
function κ(s)
# get t̂ ∈ 0:N
t̂ = (Int ∘ round)(N * s/S_f)
if t̂ ∈ [0 1]
t̂ = 2
elseif t̂ == N
t̂ = N-1
end
# get two vectors
v1 = [X[t̂] - X[t̂ - 1] Y[t̂] - Y[t̂ - 1]]
v2 = [X[t̂ + 1] - X[t̂] Y[t̂ + 1] - Y[t̂]]
# get vectors angles
θ_1 = atan(v1[2]/v1[1])
θ_2 = atan(v2[2]/v2[1])
return (θ_2 - θ_1)/(mag(1)+mag(v2))
endModel definitionThe model's state is given by # ---------------------------------------------------------------------------- #
# Model #
# ---------------------------------------------------------------------------- #
model = InfiniteModel(Ipopt.Optimizer)
set_optimizer_attribute(model, "max_iter", 500)
# --------------------------------- variabls --------------------------------- #
@infinite_parameter(model, s ∈ [0, S_f], num_supports=51)
@variables(model, begin
# CONTROLS
-4 ≤ F_v ≤ 4, Infinite(s) # longitudinal force
-4 ≤ F_ω ≤ 4, Infinite(s) # turning force
# STATE
-.01 ≤ n ≤ .01, Infinite(s), (start=0) # model->track lateral deviation
-π/2 ≤ ψ ≤ π/2, Infinite(s), (start=0) # model->track angular deviation
0 ≤ v ≤ 10, Infinite(s), (start=0) # speed
β == 0 # slip angle, constant for now
-π ≤ ω ≤ π, Infinite(s), (start=0) # angular velocity
# constants
m == 5 # mass
end)
# register curvature function
@register(model, κ(s))
# --------------------------------- Dynamics --------------------------------- #
SF = (1 - n * κ(s))/(v * cos(ψ + β) + eps())
@constraints(model, begin
∂(n, s) == SF * v * sin(ψ + β)
∂(ψ, s) == SF * ω - κ(s)
∂(v, s) == SF * F_v / m
∂(ω, s) == SF * F_ω / m
end)fit# ------------------------- initial/final conditions ------------------------- #
@constraints(model, begin
# initial conditions
n(0) == 0
ψ(0) == 0
v(0) == 0
ω(0) == 0
# final conditions
v(S_f) == 0
ω(S_f) == 0
end)
# ------------------------------- Objective fn ------------------------------- #
@objective(model, Min, ∫(SF, s))
# ------------------------------------ FIT ----------------------------------- #
print("Optimizing!")
optimize!(model)Computing trajectoryOnce a solution is found in curvilinear coordinates, we have F_v and F_ω defined as functions of # ---------------------------------------------------------------------------- #
# Analysis #
# ---------------------------------------------------------------------------- #
"""
Given the results of the optimization, integrate the model's dynamics
to get the trajectory in Cartesian coordinates.
"""
function closest_track_point_s_value(x, y)
# get index of closest track waypoint
Δx, Δy = X.-x, Y.-y
Δ = @. sqrt(Δx^2 + Δy^2) # distance from track waypoints
waypoint_idx = argmin(Δ) - 1
return waypoint_idx/length(X) * S_f
end
lerp(x, y, p) = (1-p)*x + p * y
function simulate_Cartesian_dinamics(;s_thresh=.9)
# prep for simulation
δt = .1
sim_time = 0:δt:25
nT = length(sim_time)
x̂, ŷ, θ̂, v̂, ω̂, β̂ = zeros(nT), zeros(nT), zeros(nT), zeros(nT), zeros(nT), zeros(nT)
F̂_v, F̂_ω = zeros(nT), zeros(nT)
m̂ = value(m)
# initial conditions
x̂[1] = X[1]
ŷ[1] = Y[1]
θ̂[1] = atan((Y[2]-Y[1])/(X[2]-X[1]))
# get values from solution
opt_F_v = value(F_v)
opt_F_ω = value(F_ω)
svalues = supports(s)
# simulate
for (i, t) in enumerate(sim_time) if i > 1
# check if at end of track
if sqrt((X[end]-x̂[i-1])^2 + (Y[end]-ŷ[i-1])^2) ≤ (1-s_thresh)
println("Simulation time: $t (idx: $i)")
x̂ = x̂[1:i-1]
ŷ = ŷ[1:i-1]
θ̂ = θ̂[1:i-1]
v̂ = v̂[1:i-1]
ω̂ = ω̂[1:i-1]
β̂ = β̂[1:i-1]
sim_time = sim_time[1:i-1]
break
end
# get controls through interpolation
ŝ = closest_track_point_s_value(x̂[i-1], ŷ[i-1])
s₀ = findlast(svalues .<= ŝ) # last smaller s value
s₁ = findfirst(svalues .> ŝ) # next larger
p = (ŝ - svalues[s₀]) / (svalues[s₁] - svalues[s₀])
p = p == 0 ? .1 : p
F̂_v_t = lerp(opt_F_v[s₀], opt_F_v[s₁], p)
F̂_ω_t = lerp(opt_F_ω[s₀], opt_F_ω[s₁], p)
# compute kinematics
F̂_v[i] = F̂_v_t
F̂_ω[i] = F̂_ω_t
v̂[i] = v̂[i-1] + F̂_v_t / m̂ * δt
ω̂[i] = ω̂[i-1] + F̂_ω_t / m̂
θ̂[i] = θ̂[i-1] + ω̂[i] * δt
x̂[i] = x̂[i-1] + v̂[i] * cos(β̂[i] + θ̂[i]) * δt
ŷ[i] = ŷ[i-1] + v̂[i] * sin(β̂[i] + θ̂[i]) * δt
end end
return x̂, ŷ, θ̂, v̂, ω̂, β̂, opt_F_v, opt_F_ω, svalues, sim_time, F̂_v, F̂_ω
end
x̂, ŷ, θ̂ , v̂, ω̂, β̂, opt_F_v, opt_F_ω, svalues, sim_time, F̂_v, F̂_ω = simulate_Cartesian_dinamics(;s_thresh=.7)
κ̂ = κ.(range(0, stop=S_f, length=N))
# ---------------------------------------------------------------------------- #
# Plotting #
# ---------------------------------------------------------------------------- #
# plot track
p1 = plot(
X, Y,
label="track",
lw=3,
lc=black,
xlim=[min(X...)-.5, max(X...)+.5],
ylim=[min(Y...)-.5, max(Y...)+.5],
xlabel="X",
ylabel="Y")
scatter!(p1, X, Y, marker_z=κ̂ , label=nothing, color=:bwr, msc=black, msw=0, markersize=6, cmap="bwr")
# plot reconstructed XY trajectory
plot!(p1, x̂, ŷ, lw=2, color=salmon, label="model traj.", title="Sim. time: $(sim_time[end])s")
scatter!(p1, x̂[1:4:end], ŷ[1:4:end], label=nothing, markercolor=white, msc=salmon, msw=2)
# show all
display(
p1
)
@info "Done" |
Beta Was this translation helpful? Give feedback.
-
|
It's a bit of code since I set up and solve the problem in curvilinear coordinates and then "simulate" the dynamics in cartesian coordinates from there. I need to see if there's a neater way to get the controls over time from the controls over Again, outstanding library, it's such a nice way to build/fit models. |
Beta Was this translation helpful? Give feedback.
-
|
Thank you for the encouragement and good work. You put that all together fast. Very cool work. If and when you publish your work please consider citing our paper on InfiniteOpt. |
Beta Was this translation helpful? Give feedback.
-
|
Of course! I would also like to aknowledge your help. It's rare to find open source developers that actively try to help their users, so we should encourage this behavior (I try to do the same with my projects). Btw, is there a way to save a model to file? JuMP has a |
Beta Was this translation helpful? Give feedback.
-
|
Happy to help. InfiniteOpt.jl does not currently have a file writer. Feel free to make a feature request via opening an issue. |
Beta Was this translation helpful? Give feedback.
-
|
As a follow-up, #231 now tracks the creation of a native file writer for InfiniteOpt. |
Beta Was this translation helpful? Give feedback.
-
|
Hi, I have a couple more questions, I hope it's okay if I bother you a bit more. Let's say I have an infinite model with infinite variable Second question, is there a way to do forward integration with infinite opt? Third question! I know that there's options to generate a uniformly spaced set of supports or to specify the supports manually, but is there a way to have the position of the supports themselves be optimized? For my vehicle modelling stuff, it would make sense to have more densely spaced supports in parts of the track where a lot of changes to the controls are required (e.g. around bends) and fewer where less so (straights). Thank you, |
Beta Was this translation helpful? Give feedback.
-
|
1.) InfiniteOpt does not provide any built-in interpolation capabilities. Your options are to explicitly add the supports you need (e.g., via 2.) InfiniteOpt does not currently support forward integration (see #44). You can also solve an InfiniteOpt model with a fixed control policy and no objective to effectively simulate the system (though this is not the same as forward integration). However, with #44 we plan on making an add-on package that will convert InfiniteOpt models to a ModelingToolkit model such that it can be used with the SciML environment (e.g., with DifferentialEquations.jl) 3.) Optimizing support placement would correspond to some sort of active sampling method for selecting time points which is a current topic of research. I am not an expert in this particular topic and I am not aware of a way to accomplish this for arbitrary infinite-dimensional optimization problems. The few approaches that I have seen have involved iteratively solving the problem using some sort of rule to update the supports and assess the quality of the current set (it should be possible to manually setup such an approach with InfiniteOpt). If you have a particular approach in mind please post it in the forum and we can check it out. |
Beta Was this translation helpful? Give feedback.
-
|
Thank you for the reply.
|
Beta Was this translation helpful? Give feedback.
-
|
If you invoke For your use case, interpolating the solution trajectory via Interpolations.jl is probably the best approach (this is what DIfferentialEquations.jl does behind the scenes when you query the solution). Also, note that InfiniteOpt.jl is a very flexible interface, but as a result it makes no guarantees for the accuracy of the solution (relative to simulating the states). Hence, if increased accuracy is needed (based on your simulation results) you may need to try adding more supports and/or trying other derivative approximation methods. |
Beta Was this translation helpful? Give feedback.
That's awesome. Thank you so much for all the help and for looking into this further.
That solution looks great, I didn't realize you could do something like
(start = guess_xs[i]), that's brilliant. InfiniteOpt is truly a remarkable library, outstanding job. I've truly enjoyed learning to use it and I hope I'll keep having projects that drive me back to me.I was about to actually post an update here, because I've made some good progress with this and I thought you might be curious about it. I ended up formulating the problem in terms of
s, but as you suggested I had to go back to the papers and formulate the problem more cleanly. I'll post the code below with some comments.track creation
…