TrajectoryOptimization.jl minimal time problem

Specific Domains Optimization (Mathematical)
1

Maybe some of authors of the package are around here to answer the question. Is it possible to formulate minimal time trajectory optimization problem with TrajectoryOptimization.jl? How can I go about if I don’t know in advance the time trajectory might take? Maybe some other packages offering that capabilities can be suggested?

2

You can accomplish this with InfiniteOpt.jl: Minimum time manouevering problem + boundaries · Discussion #219 · infiniteopt/InfiniteOpt.jl · GitHub

2 Likes
3

You can also use JuMP directly by modifying these examples:
https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/rocket_control/
https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/space_shuttle_reentry_trajectory/

I had modified these examples for time optimal control of Dubins vehicle. See if you find them useful.

# Dubins Car Time Optimal Control using JuMP
using JuMP
import Ipopt
import Plots

# Create JuMP model, using Ipopt as the solver
dubins = Model(Ipopt.Optimizer)
n = 100    # Time steps

# Dubins vehicle parameters
u_max = 0.3  # Maximum Angular Input

# Decision variables
@variables(dubins, begin
    Δt ≥ 0  # Time step
    # State variables
    x[1:n]           # x-coordinate
    y[1:n]           # y-coordinate
    θ[1:n]           # Orientation
    # Control variables
    -u_max ≤ u[1:n] ≤ u_max    # Thrust
end)

# Objective: Minimize time
@objective(dubins, Min, Δt)

# Initial conditions
@NLconstraint(dubins, x[1] == 0.0)
@NLconstraint(dubins, y[1] == 0.0)
@NLconstraint(dubins, θ[1] == 0.0)


# Constraints for satisfying Dynamics
for j in 2:n
    # x' = cos(θ)
    @NLconstraint(dubins, x[j] == x[j-1] + Δt * (cos(θ[j-1]) + cos(θ[j])) / 2)
    # y' = sin(θ)
    @NLconstraint(dubins, y[j] == y[j-1] + Δt * (sin(θ[j-1]) + sin(θ[j])) / 2)
    # θ' = u
    @NLconstraint(dubins, θ[j] == θ[j-1] + Δt * (u[j-1] + u[j]) / 2)
end

# Constraints for not passing through circles
rc = 1.0
xc = 5.0
yc = [5 + 0.5 * i for i = 1:5]
for i = 1:n
    @NLconstraint(dubins, (x[i] - xc)^2 + (y[i] - yc[1])^2 ≥ rc^2)
    @NLconstraint(dubins, (x[i] - xc)^2 + (y[i] - yc[2])^2 ≥ rc^2)
    @NLconstraint(dubins, (x[i] - xc)^2 + (y[i] - yc[3])^2 ≥ rc^2)
    @NLconstraint(dubins, (x[i] - xc)^2 + (y[i] - yc[4])^2 ≥ rc^2)
    @NLconstraint(dubins, (x[i] - xc)^2 + (y[i] - yc[5])^2 ≥ rc^2)
end


# Final time constraints
xf = 7.0
yf = 8.0
@NLconstraint(dubins, (x[n] - xf)^2 + (y[n] - yf)^2 ≤ 0.01)

# Solve for the control and state
println("Solving...")
optimize!(dubins)
# solution_summary(dubins)

# ## Display results

println("Time required: ", n * objective_value(dubins))
plt1 = Plots.plot(value.(x), value.(y), aspect_ratio = 1, legend = false, framestyle = :box)

# Circles
xt = zeros(n)
yt = zeros(n)
θt = LinRange(0, 2π, n)
for j = 1:5
	global plt1
    for i = 1:n
        xt[i] = xc + rc * cos(θt[i])
        yt[i] = yc[j] + rc * sin(θt[i])
    end
    Plots.plot!(plt1, xt, yt)
end

# Plotting final and initial locations
Plots.scatter!(plt1, [0, xf], [0, yf])
Plots.savefig(plt1, "dubins_obstacle.png")
display(fig1)
1 Like
4

I suppose it transcribe “infinite” parameters under the hood using some direct methods? So it does something similar to what @A_C suggested below hiding some machinery behind API?

5

I’ve tested many direct methods in JuMP like single/multiple shooting as well as orthogonal collocation / pseudospectral methods… TrajectoryOptimization.jl uses indirect methods and iterative solver. Not only it proved to be order of magnitude faster on my tasks. It is also more embedded systems friendly due to it’s iterative structure. So looking forward implementing something on real hardware in some C language I definitely would opt to something like TrajectoryOptimization.jl uses. But I can’t figure out how it is possible to formulate minimal time problems with it…

6

InfiniteOpt.jl transforms the model into a tractable representation. Currently, only direct methods are implemented (so yes something similar to the suggestion of @A_C), though it would be possible to add on indirect methods. With that said, it was not designed for use in high frequency control on embedded systems.

1 Like
7

Were you able to use TrajectoryOptimization.jl to solve minimum time problems? In the documentation it has been mentioned that you need to augment time as a state variable.

8

Well you can augment your dynamics with some state T:

z = [x;T]. 
g(z, u) = [T*f(x,u), 0]
z' = g(z,u)

So time interval would be rescaled to [0, 1].

Then consider multiphase minimal time optimization problem like flying drone through several waypoints. Because each segment of flight will take different amount of time you have different augmented system for each segment i.e. different dynamics for such approach. And this is not something supported by TrajecotryOptimization.jl again. As far as I understand. It only allows for one model dynamics.

9

I have not solved multi phase problems. But why would the time interval be scaled to [0, 1]? Also, the derivative of time as state variable with respect to time will be 1, right?

10

Introducing new time tau in [0, 1] and parameter T for trajectory total time.
then t = tau * T
introducing new state z as described above we get

z = [x; T]
dz/d(tau) = [dx/dt*dt/dtau; dT/dtau] = [T*f; 0]

so we have n+1 state dynamical system (augmented). We getting zero as T is a constant and its derivative is zero.

Maybe there are some other augmentation methods to get things done?

11

There is still an issue though.
TrajectoryOptimization.jl requires x0 to be provided. And I can’t see how I can provide partial initial state i.e. define only some states and leave others to figure out optimal values by solver.

Thus I have to define augmented state i.e. my total time T constant parameter for initial state x0. And because it has zero rate of change it will remain the same along all trajectory. Effectively I have to define trajectory time again.

So problem still not solved unfortunately. If only there is some another method of augmenting system. Or possibility to not define part of initial state.

12

I looked into this package about a year ago and found the same thing. I believe they wrote the documentation intending to allow for this, but haven’t implemented it yet. On GitHub, there are open issues for minimum time problems and hybrid dynamics, either of which should address this. It’s possible this is pretty easy to implement, because continuous-time problems just get discretized. At that conversion, code could be added to allow for unusual discrete dynamics. For example, a discrete model might have trivial dynamics e.g. x[1] = u[0] where x[1] is the final time (used to scale the continuous-time dynamics as others have said above), and u[0] is a control input that acts as the single decision variable for final time. That is trivial, but an exception to the package’s rule of discretizing a continuous-time model.

13

You must introduce time t as your additional state variable (not final time T_f). Then the derivative of that state becomes one. The initial value then becomes the instant from which you start your system (or 0 for convenience WLOG). See the book by Issacs (2nd Chapter) if you need a reference.

14

I don’t see how this will help avoid providing t_f as required by the library… You can augment you state with time if you wish the way you described (and as described in the book). But then you still have to provide some terminal time t_f. This sort of augmentation will not change the time scale.

What I did above is scaled differential equation by factor of time to make sure terminal time is 1. So at least we can provide terminal time now. But we can’t provide initial value for augmented state then as it equals to terminal time now and should become a decision variable.

1 Like
15

Yes, I believe you still need access to the discrete-time system even with the @A_C’s method, which is another good approach. Again, minimum time is an open issue in TrajectoryOptimization.jl, so they are aware of it.

Also, I forgot to put in a good word for InfiniteOpt.jl, which is a really extraordinary package. Aside from the infinite variables part, it makes a lot of things in JuMP easier. It mixes nonlinear and linear constraints/expressions very comfortably.

@a6a3uh if you prefer iterative methods, I am guessing you have a problem suited for DDP or iLQR. As far as I know, TrajectoryOptimization.jl is very promising for those problems, but the package is still young and doesn’t implement everything yet.

I used to prefer “indirect” methods such as forward-backward sweep. But I think the momentum is swinging toward “direct” methods, because they can leverage the huge improvements in static optimizers (e.g. IPopt), which can be quite good at handling sparsity. iLQR can get you second-order convergence, but it helps to know the ins and outs of linear solves. For problems of a certain size, just typing in "" is not good enough. On the other hand, in SciML there is LinearSolve.jl which may make it easier to set up systems so they solve quickly.

16

The paper for Altro.jl mentions time penalized problems. Also, to quote the documentation:

any coupling between adjacent time-steps can be resolved by augmenting the state and defining the appropriate dynamics (this is the method we use to solve minimum time problems).

So let us assume that the problem is solvable. I have not used TrajectoryOptimization.jl so I am not sure.

Assume that the dynamics is given by \dot(x) = f(x, u). Discretize it by assuming different dt at each instant i.e.
x(k+1) = x(k) + f(x(k), u(k))*dt(k).
Now define another state
t(k+1) = t(k) + dt(k), t(0) = t_0
Now let dt be a pseudo input and constrain it to be in 0<dt(k)<dt_max where dt_max is the maximum step size that you want.
The objective function then becomes J = t(N).

Another way would be to use RL like approach. Add time t as augmented state and discretize with a fixed time step. Say that you want to reach x_f. Then at each time step add a penalty L(x(k),u(k)) = t*||x-xf||_2. This will make the system reach x_f as soon as possible.

1 Like
17

Thank you @A_C for more detailed explanation. It was not a big deal for me to imagine how to penalize time in continuous time sense or in discrete dynamics. But it was unclear how to make terminal time free variable. The key here is to make dt which comes after discretization a pseudo control. This is something @artkuo has mentioned already. But now you made it precisely clear.

1 Like
18

For the record, note that this idea did not wait for the advent of RL techniques. It has been used by control engineers for many decades to asses the performance of (and even tune) industrial controllers such as PID, PI, … It is know as ITSE (Integral of Time times Squared Error) criterion. But there are also ITAE (Integral of Time times Absolute Error), ISE (and the related LQ), IAE, …

Indeed, you can use these to indirectly speed up the response of the system, but the fact is that there are more tailored formulations for minimum-time (or time-optimal) control.

2 Likes