Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 2806a28c authored by Negre Philippe's avatar Negre Philippe
Browse files

added N=4

parent 4a658b0d
No related branches found
No related tags found
2 merge requests!231Develop,!230Feature/nav julia
Expand all Hide whitespace changes
Inline Side-by-side
examples/nav/nav-julia-N3.ipynb
+ 2
− 2
View file @ 2806a28c
%% Cell type:code id: tags:
 
``` julia
using JuMP,Plots,Ipopt
```
 
%% Cell type:code id: tags:
 
``` julia
#JuMP model, Ipopt solver
sys = Model(optimizer_with_attributes(Ipopt.Optimizer,"print_level"=> 5))
set_optimizer_attribute(sys,"tol",1e-14)
set_optimizer_attribute(sys,"max_iter",1000)
 
#Parameters
 
# constants
w = 0.8
l = 0.99
 
# initial data
x0 = 0
y0 = 0
θ0 = pi/7
β1_0 = 0
β2_0 = 0
β3_0 = 0
 
# final data
xf = 4
yf = 7
θf = -pi/2
θf = pi/2
β1_f = 0
β2_f = 0
β3_f = 0
 
N = 200
# Bounds for variables
 
JuMP.@variables(sys,begin
x[1:N] # x
y[1:N] # y
θ[1:N] # theta
β1[1:N] # beta1
β2[1:N] # beta2
β3[1:N] # beta3
-1 ≤ u[1:N] ≤ 1 # u, control
0 ≤ Δt ≤ 1
end)
 
# Objective
@objective(sys,Min,Δt)
 
# Constraints
 
@constraints(sys,begin
x[1] == x0
y[1] == y0
θ[1] == θ0
β1[1]== β1_0
β2[1] == β2_0
β3[1] == β3_0
x[N] == xf
y[N] == yf
θ[N] == θf
β1[N] == β1_f
β2[N] == β2_f
β3[N] == β3_f
end)
 
# Dynamics : Crank-Nicolson scheme
 
for j in 1 : N-1
@NLconstraint(sys, # x' = w + cos(theta)
x[j+1] == x[j] + 0.5 * Δt * (w + cos(θ[j]) + w + cos(θ[j+1])))
@NLconstraint(sys, # y' = sin(theta)
y[j+1] == y[j] + 0.5* Δt *(sin(θ[j])+ sin(θ[j+1])))
@NLconstraint(sys, # theta ' = u
θ[j+1] == θ[j] + 0.5 * Δt * (u[j] + u[j+1]) )
@NLconstraint(sys, #beta1 ' = -u- 1/l * sin(beta1)
β1[j+1] == β1[j] + 0.5 * Δt * (-u[j]-(1/l)*sin(β1[j]) - u[j+1] - (1/l)*sin(β1[j+1])))
@NLconstraint(sys, #beta2' = -u - cos(beta1)*sin(beta2 - beta1)/l
β2[j+1] == β2[j] + 0.5 * Δt * (-u[j] - cos(β1[j])*sin(β2[j] - β1[j])/l - u[j+1] - cos(β1[j+1])*sin(β2[j+1] - β1[j+1])/l ))
@NLconstraint(sys, #beta3' = -u - cos(beta1) * cos(beta2-beta1)*sin(beta3 - beta2)
β3[j+1] == β3[j] +0.5 * Δt * (-u[j] - cos(β1[j])*cos(β2[j] - β1[j]) * sin(β3[j] - β2[j])/l -u[j+1] - cos(β1[j+1])*cos(β2[j+1] - β1[j+1]) * sin(β3[j+1] - β2[j+1])/l ))
end
 
#Solve for the control and state
println("Solving...")
status = optimize!(sys)
println("Solver status : ",status)
x1 = value.(x)
y1 = value.(y)
θ1 = value.(θ)
β_1 = value.(β1)
β_2 = value.(β2)
β_3 = value.(β3)
u1 = value.(u)
println("Cost : " , objective_value(sys))
 
println("tf = ", value.(Δt)*N)
 
#plots : states
 
Δt1 = value.(Δt)
t = (1 : N)*Δt1
#h = (1 : N)
 
x_plot = plot(t,x1,xlabel = "t", ylabel = "position x", legend = false, fmt = :png)
y_plot = plot(t,y1,xlabel = "t", ylabel = "position y", legend = false, fmt = :png)
θ_plot = plot(t,θ1,xlabel = "t", ylabel = "θ", legend = false, fmt = :png)
β1_plot = plot(t,β_1,xlabel = "t", ylabel = "β1", legend = false, fmt = :png)
β2_plot = plot(t,β_2,xlabel = "t", ylabel = "β2", legend = false, fmt = :png)
β3_plot = plot(t,β_3,xlabel = "t", ylabel = "β3", legend = false, fmt = :png)
display(plot(x_plot,y_plot,θ_plot,β1_plot,β2_plot,β3_plot ,layout = (3,2)))
u_plot = plot(t,u1,xlabel = "t", ylabel = "control", legend = false, fmt = :png)
display(u_plot)
 
 
#plots : trajectory
 
p = plot(x1,y1, c = :blue, lw = 7)
plot!(size=(2000,2000))
 
for i = 1 : N
z = [x1[i] y1[i]]
θ_p = θ1[i]
β1_p = β_1[i]
β2_p = β_2[i]
β3_p = β_3[i]
θ_1 = β1_p + θ_p
θ_2 = β2_p + θ_p
θ_3 = β3_p + θ_p
z1 = z - l*[cos(θ_1) sin(θ_1)]
z2 = z1 - l*[cos(θ_2) sin(θ_2)]
z3 = z2 - l*[cos(θ_3) sin(θ_3)]
plot!([z[1],z1[1]], [z[2],z1[2]] ,color=:black, legend = false)
plot!([z1[1],z2[1]], [z1[2],z2[2]], color=:black, legend = false)
plot!([z2[1],z3[1]], [z2[2],z3[2]], color=:black, legend = false)
plot!([z[1]],[z[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z1[1]],[z1[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z2[1]],[z2[2]],seriestype = :scatter, color =:black , legend = false)
plot!(size=(2000,2000))
end
current()
```
 
%% Output
 
Solving...
 
 
 
This is Ipopt version 3.13.4, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).
Number of nonzeros in equality constraint Jacobian...: 7176
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 21094
Total number of variables............................: 1401
variables with only lower bounds: 0
variables with lower and upper bounds: 201
variables with only upper bounds: 0
Total number of equality constraints.................: 1206
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 9.9999900e-03 7.00e+00 2.89e-15 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.0000169e-02 7.00e+00 2.46e+04 -6.0 6.45e+03 - 1.54e-04 1.54e-04h 1
2 1.0010671e-02 7.00e+00 3.56e+04 1.9 4.82e+03 - 2.93e-04 7.53e-05h 1
3 1.0015556e-02 7.00e+00 3.84e+04 1.9 8.56e+03 - 1.46e-04 1.96e-05h 3
4r 1.0015556e-02 7.00e+00 9.82e+02 2.1 0.00e+00 - 0.00e+00 2.88e-07R 9
5r 7.7544007e-03 5.54e+00 4.10e+03 0.1 4.95e+00 - 7.90e-01 3.42e-01f 1
6r 7.7544007e-03 5.54e+00 9.99e+02 0.7 0.00e+00 - 0.00e+00 4.46e-07R 5
7r 8.0469975e-03 5.34e+00 9.86e+02 0.4 5.57e+00 - 1.00e+00 1.06e-01f 1
8r 1.0827472e-02 4.02e+00 8.81e+02 1.0 9.47e+00 - 1.00e+00 1.39e-01f 1
9 1.0832142e-02 4.02e+00 2.46e+02 -6.0 1.03e+04 - 1.44e-04 4.84e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10r 1.0832142e-02 4.02e+00 9.99e+02 0.6 0.00e+00 - 0.00e+00 3.07e-07R 4
11r 1.1394250e-02 3.93e+00 9.72e+02 0.6 7.17e+01 - 5.60e-01 2.75e-02f 1
12r 1.3107039e-02 3.51e+00 3.39e+03 0.3 4.79e+00 - 9.84e-01 1.66e-01f 1
13 1.3117240e-02 3.51e+00 5.57e+02 1.6 1.35e+04 - 1.91e-05 1.35e-04h 1
14r 1.3117240e-02 3.51e+00 9.99e+02 0.5 0.00e+00 - 0.00e+00 3.29e-07R 3
15r 1.3112415e-02 3.41e+00 9.71e+02 0.4 6.74e+01 - 6.05e-01 2.91e-02f 1
16r 1.2740613e-02 1.75e+00 5.72e+04 0.1 2.61e+00 - 8.50e-01 6.36e-01f 1
17 1.2746611e-02 1.75e+00 1.79e+04 -5.7 2.41e+03 - 5.04e-04 2.42e-04h 1
18 1.2754484e-02 1.75e+00 3.94e+04 2.3 7.54e+03 - 1.57e-04 7.16e-05h 1
19 1.2756627e-02 1.75e+00 6.39e+04 2.3 1.55e+04 - 5.99e-05 2.89e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20r 1.2756627e-02 1.75e+00 9.94e+02 1.0 0.00e+00 - 0.00e+00 3.12e-07R 6
21r 1.6418013e-02 1.11e+00 7.70e+03 0.9 3.85e+00 - 9.75e-01 4.54e-01f 1
22 1.6420453e-02 1.11e+00 2.13e+02 -0.1 1.64e+03 - 6.73e-04 1.46e-04h 1
23 1.6431469e-02 1.11e+00 2.45e+03 -0.1 2.09e+03 - 6.68e-04 3.06e-04h 1
24 1.6447383e-02 1.11e+00 6.16e+03 -0.1 2.73e+03 - 3.37e-04 1.41e-04h 1
25 1.6460984e-02 1.11e+00 1.44e+04 -0.1 1.02e+04 - 1.29e-04 6.48e-05h 1
26r 1.6460984e-02 1.11e+00 9.99e+02 0.0 0.00e+00 - 0.00e+00 3.85e-07R 6
27r 1.8140875e-02 1.11e+00 1.52e+03 0.8 1.16e+01 - 7.28e-01 6.60e-02f 1
28r 2.5116945e-02 1.98e+00 2.97e+04 0.2 2.24e+00 - 3.87e-01 4.47e-01f 1
29r 2.7411067e-02 1.97e+00 2.74e+04 0.9 3.44e+00 - 9.78e-01 7.17e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30r 3.5467112e-02 2.07e+00 4.39e+03 0.4 1.40e+00 - 5.80e-01 1.00e+00f 1
31r 3.7291118e-02 2.08e+00 1.34e+03 0.4 3.41e-01 - 1.00e+00 6.39e-01f 1
32r 3.8291612e-02 2.00e+00 2.17e+02 0.4 9.59e-01 - 1.00e+00 8.17e-01f 1
33r 3.8599724e-02 1.98e+00 1.21e+01 0.4 2.24e-01 - 1.00e+00 1.00e+00f 1
34r 3.9617726e-02 3.01e+00 8.63e+01 -0.3 1.27e+00 - 6.86e-01 8.09e-01f 1
35r 4.0365489e-02 3.06e+00 3.29e+02 -0.2 5.02e-01 - 9.77e-01 8.03e-01f 1
36r 4.0591962e-02 3.32e+00 1.69e+02 -1.1 3.13e-01 - 1.00e+00 8.26e-01f 1
37r 4.1182234e-02 3.25e+00 3.70e+01 -0.5 4.53e-01 - 9.76e-01 1.00e+00f 1
38r 4.0790133e-02 3.30e+00 4.39e+01 -0.6 3.17e-01 - 1.00e+00 1.00e+00f 1
39r 4.1048826e-02 3.42e+00 1.52e+01 -1.2 1.61e-01 - 9.78e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40r 4.1895950e-02 3.43e+00 2.45e+02 -1.7 2.14e-01 - 1.00e+00 1.00e+00f 1
41r 4.3028936e-02 3.42e+00 1.17e+02 -2.3 2.76e-01 - 1.00e+00 1.00e+00f 1
42r 4.4018647e-02 3.41e+00 7.74e+01 -2.8 5.44e-01 - 1.00e+00 7.09e-01f 1
43r 4.5093681e-02 3.40e+00 5.39e+01 -3.1 5.43e-01 - 1.00e+00 6.58e-01f 1
44r 4.6512014e-02 3.38e+00 4.38e+01 -3.4 2.90e-01 - 1.00e+00 5.39e-01f 1
45r 4.7117275e-02 3.38e+00 1.12e+01 -3.2 4.13e-02 0.0 1.00e+00 1.00e+00h 1
46r 4.8910653e-02 3.36e+00 3.24e+00 -3.9 1.47e-01 -0.5 1.00e+00 1.00e+00f 1
47r 5.1126301e-02 3.35e+00 6.33e+01 -2.5 1.55e-01 -0.1 1.00e+00 1.00e+00f 1
48r 5.2036787e-02 3.34e+00 2.45e+00 -2.6 9.61e-02 0.4 8.58e-01 1.00e+00f 1
49r 6.0823149e-02 3.28e+00 4.94e+02 -2.6 6.71e-01 -0.1 1.03e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50r 6.4612592e-02 3.26e+00 1.06e+02 -2.6 5.81e-01 0.3 1.46e-01 1.00e+00f 1
51r 7.7478725e-02 3.02e+00 1.80e+02 -0.5 2.45e+00 -0.2 3.29e-01 7.62e-01f 1
52r 9.6605288e-02 2.91e+00 2.15e+02 -0.8 9.93e+00 - 3.14e-02 1.65e-01f 1
53r 1.2686412e-01 2.72e+00 3.15e+02 -0.8 8.91e+00 -0.6 5.10e-02 1.93e-01f 1
54r 1.6653266e-01 2.45e+00 5.11e+02 -0.8 4.92e+00 -0.2 1.37e-01 4.19e-01f 1
55r 2.0089918e-01 2.18e+00 5.03e+02 -0.8 1.63e+00 0.2 4.16e-01 1.00e+00f 1
56r 2.2847846e-01 1.93e+00 5.44e+02 0.3 7.93e+00 -0.3 8.81e-01 1.30e-01f 1
57r 2.5014197e-01 1.61e+00 2.34e+02 -0.3 1.11e+00 0.2 9.14e-01 1.00e+00f 1
58r 2.9283726e-01 1.27e+00 5.20e+01 -0.3 1.09e+00 0.6 9.00e-01 1.00e+00f 1
59r 3.0349789e-01 1.16e+00 4.09e+01 -0.3 8.22e-01 1.0 1.88e-01 3.87e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60r 3.0403946e-01 1.14e+00 3.67e+01 -0.3 3.12e+00 1.4 2.48e-01 4.32e-02f 1
61r 3.0933317e-01 1.14e+00 2.52e+01 -0.3 1.31e-01 1.9 1.00e+00 1.00e+00f 1
62r 3.1875136e-01 1.06e+00 4.28e+01 -1.0 2.77e-01 1.4 5.53e-01 9.86e-01f 1
63r 3.3603790e-01 8.35e-01 1.76e+01 -2.4 1.72e+00 0.9 3.84e-01 3.23e-01f 1
64 3.0698396e-01 5.19e-01 3.17e+01 0.5 2.03e+01 - 7.41e-01 3.77e-01f 2
65 3.7470241e-01 2.25e-01 8.88e+01 0.4 6.94e+00 - 1.44e-01 6.54e-01f 1
66 4.8643629e-01 1.51e-01 5.65e+01 0.4 5.49e+00 - 7.32e-01 1.00e+00f 1
67 6.6578745e-01 2.12e-01 2.53e+01 0.4 4.88e+00 - 8.32e-01 1.00e+00f 1
68 7.1138166e-01 5.61e-02 2.16e+00 -0.3 5.00e+00 -4.0 1.00e+00 1.00e+00h 1
69 7.1252388e-01 2.97e-03 1.99e-01 -1.0 9.80e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 5.5087944e-01 1.74e-01 1.71e-01 -7.0 4.27e+00 - 6.34e-01 1.00e+00h 1
71 4.8723201e-01 6.08e-02 3.72e-02 -1.5 2.96e+00 - 1.00e+00 1.00e+00h 1
72 3.9507162e-01 9.13e-02 2.23e-02 -2.3 3.33e+00 - 9.66e-01 1.00e+00h 1
73 3.3571823e-01 1.54e-01 1.03e-01 -2.3 2.96e+00 - 1.00e+00 1.00e+00h 1
74 3.1161765e-01 6.96e-02 6.67e-02 -2.8 2.21e+00 - 9.85e-01 1.00e+00h 1
75 3.0086818e-01 5.28e-02 2.39e-02 -3.5 1.52e+00 - 9.97e-01 1.00e+00h 1
76 2.9920654e-01 3.41e-03 4.78e-03 -4.2 4.90e-01 - 9.93e-01 1.00e+00h 1
77 2.9851696e-01 9.35e-04 2.39e-03 -4.9 2.48e-01 - 9.97e-01 1.00e+00h 1
78 2.9826725e-01 2.27e-04 4.64e-04 -5.7 3.08e-01 - 9.90e-01 1.00e+00h 1
79 2.9823112e-01 4.86e-05 8.38e-05 -6.4 1.78e-01 - 9.95e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 2.9822560e-01 9.23e-06 1.50e-05 -6.8 9.15e-02 - 9.90e-01 1.00e+00h 1
81 2.9822298e-01 2.06e-06 3.48e-06 -8.0 6.68e-02 - 1.00e+00 1.00e+00h 1
82 2.9822280e-01 2.78e-07 5.28e-07 -8.3 1.16e-01 - 9.92e-01 1.00e+00h 1
83 2.9822272e-01 1.32e-08 4.10e-08 -9.5 6.04e-02 - 1.00e+00 1.00e+00h 1
84 2.9822271e-01 1.13e-09 1.60e-08 -10.9 5.15e-02 - 1.00e+00 1.00e+00h 1
85 2.9822272e-01 1.17e-11 1.33e-10 -12.8 4.54e-03 - 1.00e+00 1.00e+00h 1
86 2.9822272e-01 3.55e-15 1.45e-14 -14.3 4.33e-05 - 1.00e+00 1.00e+00h 1
87 2.9822272e-01 1.78e-15 1.35e-15 -14.3 7.10e-09 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 87
(scaled) (unscaled)
Objective...............: 2.9822271551276286e-01 2.9822271551276286e-01
Dual infeasibility......: 1.3499827888011194e-15 1.3499827888011194e-15
Constraint violation....: 1.7763568394002505e-15 1.7763568394002505e-15
Complementarity.........: 5.0008835746513821e-15 5.0008835746513821e-15
Overall NLP error.......: 5.0008835746513821e-15 5.0008835746513821e-15
Number of objective function evaluations = 129
Number of objective gradient evaluations = 49
Number of equality constraint evaluations = 129
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 94
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 87
Total CPU secs in IPOPT (w/o function evaluations) = 0.799
Total CPU secs in NLP function evaluations = 0.548
EXIT: Optimal Solution Found.
Solver status : nothing
Cost : 0.29822271551276286
tf = 59.644543102552575
 
 
%% Cell type:code id: tags:
 
``` julia
#JuMP model, Ipopt solver
sys2 = Model(optimizer_with_attributes(Ipopt.Optimizer,"print_level"=> 5))
set_optimizer_attribute(sys2,"tol",1e-14)
set_optimizer_attribute(sys2,"max_iter",1000)
 
#Parameters
 
# constants
w = 0.8
l = 0.99
 
# initial data
x0 = 0
y0 = 0
θ0 = pi/7
β1_0 = 0
β2_0 = 0
β3_0 = 0
 
# final data
xf = 4
yf = 7
θf = pi/2
θf = -pi/2
β1_f = 0
β2_f = 0
β3_f = 0
 
N = 200
# Bounds for variables
 
JuMP.@variables(sys2,begin
x[1:N] # x
y[1:N] # y
θ[1:N] # theta
β1[1:N] # beta1
β2[1:N] # beta2
β3[1:N] # beta3
-1 ≤ u[1:N] ≤ 1 # u, control
0 ≤ Δt ≤ 1
end)
 
# Objective
@objective(sys2,Min,Δt)
 
# Constraints
 
@constraints(sys2,begin
x[1] == x0
y[1] == y0
θ[1] == θ0
β1[1]== β1_0
β2[1] == β2_0
β3[1] == β3_0
x[N] == xf
y[N] == yf
θ[N] == θf
β1[N] == β1_f
β2[N] == β2_f
β3[N] == β2_f
end)
 
# Dynamics : Crank-Nicolson scheme
 
for j in 1 : N-1
@NLconstraint(sys2, # x' = w + cos(theta)
x[j+1] == x[j] + 0.5 * Δt * (w + cos(θ[j]) + w + cos(θ[j+1])))
@NLconstraint(sys2, # y' = sin(theta)
y[j+1] == y[j] + 0.5* Δt *(sin(θ[j])+ sin(θ[j+1])))
@NLconstraint(sys2, # theta ' = u
θ[j+1] == θ[j] + 0.5 * Δt * (u[j] + u[j+1]) )
@NLconstraint(sys2, #beta1 ' = -u- 1/l * sin(beta1)
β1[j+1] == β1[j] + 0.5 * Δt * (-u[j]-(1/l)*sin(β1[j]) - u[j+1] - (1/l)*sin(β1[j+1])))
@NLconstraint(sys2, #beta2' = -u - cos(beta1)*sin(beta2 - beta1)/l
β2[j+1] == β2[j] + 0.5 * Δt * (-u[j] - cos(β1[j])*sin(β2[j] - β1[j])/l - u[j+1] - cos(β1[j+1])*sin(β2[j+1] - β1[j+1])/l ))
@NLconstraint(sys2, #beta3' = -u - cos(beta1) * cos(beta2-beta1)*sin(beta3 - beta2)
β3[j+1] == β3[j] +0.5 * Δt * (-u[j] - cos(β1[j])*cos(β2[j] - β1[j]) * sin(β3[j] - β2[j])/l -u[j+1] - cos(β1[j+1])*cos(β2[j+1] - β1[j+1]) * sin(β3[j+1] - β2[j+1])/l ))
end
 
#Solve for the control and state
println("Solving...")
status = optimize!(sys2)
println("Solver status : ",status)
x1 = value.(x)
y1 = value.(y)
θ1 = value.(θ)
β_1 = value.(β1)
β_2 = value.(β2)
β_3 = value.(β3)
u1 = value.(u)
println("Cost : " , objective_value(sys2))
 
println("tf = ", value.(Δt)*N)
 
#plots : states
 
Δt1 = value.(Δt)
t = (1 : N)*Δt1
#h = (1 : N)
 
x_plot = plot(t,x1,xlabel = "t", ylabel = "position x", legend = false, fmt = :png)
y_plot = plot(t,y1,xlabel = "t", ylabel = "position y", legend = false, fmt = :png)
θ_plot = plot(t,θ1,xlabel = "t", ylabel = "θ", legend = false, fmt = :png)
β1_plot = plot(t,β_1,xlabel = "t", ylabel = "β1", legend = false, fmt = :png)
β2_plot = plot(t,β_2,xlabel = "t", ylabel = "β2", legend = false, fmt = :png)
β3_plot = plot(t,β_3,xlabel = "t", ylabel = "β3", legend = false, fmt = :png)
display(plot(x_plot,y_plot,θ_plot,β1_plot,β2_plot,β3_plot ,layout = (3,2)))
u_plot = plot(t,u1,xlabel = "t", ylabel = "control", legend = false, fmt = :png)
display(u_plot)
 
#plots : trajectory
 
p = plot(x1,y1, c = :blue, lw = 7)
plot!(size=(2000,2000))
 
for i = 1 : N
z = [x1[i] y1[i]]
θ_p = θ1[i]
β1_p = β_1[i]
β2_p = β_2[i]
β3_p = β_3[i]
θ_1 = β1_p + θ_p
θ_2 = β2_p + θ_p
θ_3 = β3_p + θ_p
z1 = z - l*[cos(θ_1) sin(θ_1)]
z2 = z1 - l*[cos(θ_2) sin(θ_2)]
z3 = z2 - l*[cos(θ_3) sin(θ_3)]
plot!([z[1],z1[1]], [z[2],z1[2]] ,color=:black, legend = false)
plot!([z1[1],z2[1]], [z1[2],z2[2]], color=:black, legend = false)
plot!([z2[1],z3[1]], [z2[2],z3[2]], color=:black, legend = false)
plot!([z[1]],[z[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z1[1]],[z1[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z2[1]],[z2[2]],seriestype = :scatter, color =:black , legend = false)
plot!(size=(2000,2000))
end
current()
```
 
%% Output
 
Solving...
This is Ipopt version 3.13.4, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).
Number of nonzeros in equality constraint Jacobian...: 7176
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 21094
Total number of variables............................: 1401
variables with only lower bounds: 0
variables with lower and upper bounds: 201
variables with only upper bounds: 0
Total number of equality constraints.................: 1206
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 9.9999900e-03 7.00e+00 2.89e-15 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.0001381e-02 6.99e+00 2.60e+04 -6.0 8.30e+02 - 1.19e-03 1.19e-03h 1
2 1.0012238e-02 6.99e+00 4.15e+04 1.9 6.06e+02 - 2.91e-03 6.70e-04h 1
3 1.0030996e-02 6.98e+00 1.01e+05 2.0 1.07e+03 - 1.32e-03 6.25e-04h 1
4 1.0042708e-02 6.98e+00 1.50e+05 2.0 2.94e+03 - 3.92e-04 2.33e-04h 1
5 1.0048519e-02 6.98e+00 1.82e+05 2.0 6.58e+03 - 1.66e-04 9.29e-05h 1
6 1.0051608e-02 6.98e+00 2.07e+05 2.0 1.13e+04 - 1.25e-04 4.48e-05h 1
7 1.0054163e-02 6.98e+00 2.59e+05 2.1 2.35e+04 - 4.57e-05 3.51e-05h 1
8r 1.0054163e-02 6.98e+00 9.99e+02 0.8 0.00e+00 - 0.00e+00 2.80e-07R 6
9r 1.1570751e-02 6.12e+00 1.58e+03 1.3 7.87e+00 - 9.92e-01 1.31e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 1.1564043e-02 6.12e+00 2.84e+04 2.3 4.87e+03 - 1.23e-04 2.30e-04f 1
11 1.1558910e-02 6.12e+00 2.84e+04 2.3 4.86e+03 - 2.52e-04 1.63e-04f 1
12 1.1551481e-02 6.12e+00 1.72e+04 2.2 3.30e+03 - 2.74e-04 2.21e-04f 1
13 1.1550329e-02 6.12e+00 1.66e+04 -5.8 2.55e+03 - 1.46e-04 3.00e-05h 1
14 1.1547505e-02 6.12e+00 1.30e+04 2.1 4.78e+03 - 2.17e-04 7.08e-05f 1
15 1.1537152e-02 6.11e+00 5.05e+04 2.1 4.86e+03 - 1.78e-04 2.45e-04f 1
16 1.1535363e-02 6.11e+00 5.55e+04 -5.9 4.16e+03 - 8.51e-05 3.15e-05h 1
17 1.1527433e-02 6.11e+00 1.81e+05 2.1 7.82e+03 - 4.50e-05 1.30e-04f 1
18r 1.1527433e-02 6.11e+00 1.00e+03 0.8 0.00e+00 - 0.00e+00 2.88e-07R 4
19r 1.2502990e-02 5.78e+00 1.08e+03 1.0 3.08e+01 - 9.91e-01 6.39e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20r 1.5190126e-02 4.64e+00 1.57e+03 0.6 4.99e+00 - 6.82e-01 2.39e-01f 1
21 1.5205305e-02 4.63e+00 5.38e+02 -6.0 6.34e+01 - 1.49e-02 1.06e-03h 1
22 1.5236441e-02 4.63e+00 1.21e+04 2.0 1.29e+03 - 2.48e-03 1.31e-03h 1
23 1.5248131e-02 4.62e+00 2.43e+04 2.1 1.25e+03 - 3.50e-03 7.46e-04h 1
24 1.5257730e-02 4.62e+00 7.17e+04 2.2 4.31e+03 - 3.01e-04 1.85e-04h 1
25 1.5254771e-02 4.62e+00 2.09e+03 2.2 3.73e+03 - 5.02e-04 1.41e-04h 1
26 1.5259275e-02 4.62e+00 1.51e+05 2.2 5.69e+03 - 1.43e-04 6.91e-05h 1
27 1.5258119e-02 4.62e+00 1.72e+04 2.2 1.21e+04 - 6.81e-05 1.67e-05h 1
28 1.5259380e-02 4.62e+00 7.20e+05 2.2 4.03e+04 - 5.42e-06 1.13e-05h 1
29 1.5259224e-02 4.62e+00 7.21e+04 2.2 2.99e+04 - 3.43e-05 1.15e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30r 1.5259224e-02 4.62e+00 1.00e+03 2.2 0.00e+00 - 0.00e+00 3.60e-07R 4
31r 1.8242880e-02 2.81e+00 6.86e+02 0.0 6.45e+00 - 9.88e-01 2.84e-01f 1
32 1.8244462e-02 2.81e+00 2.72e+01 -0.1 5.51e+02 - 4.87e-04 1.38e-04h 1
33 1.8260150e-02 2.81e+00 1.10e+03 -0.8 7.90e+02 - 1.37e-05 1.01e-03h 1
34 1.8273864e-02 2.81e+00 2.01e+03 -0.8 8.31e+02 - 1.49e-04 6.11e-04h 1
35 1.8285119e-02 2.81e+00 2.99e+03 -0.8 6.58e+02 - 1.75e-04 3.81e-04h 1
36 1.8304468e-02 2.80e+00 7.45e+03 -0.8 1.10e+03 - 1.01e-04 6.12e-04h 1
37 1.8307956e-02 2.80e+00 7.96e+03 -0.8 2.44e+03 - 4.31e-04 7.38e-05h 1
38 1.8312398e-02 2.80e+00 9.49e+03 -0.8 2.64e+03 - 1.67e-03 7.45e-05h 1
39 1.8326161e-02 2.80e+00 7.09e+04 -0.8 3.00e+03 - 4.76e-04 1.48e-04h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.8322270e-02 2.80e+00 5.29e+04 -0.8 4.29e+03 - 1.25e-04 4.62e-05h 1
41 1.8322200e-02 2.80e+00 5.29e+04 -0.8 1.51e+04 - 9.02e-07 9.02e-07s 2
42 1.8322114e-02 2.80e+00 5.28e+04 -0.8 1.54e+04 - 1.23e-06 1.23e-06s 2
43 1.8320638e-02 2.80e+00 4.06e+04 -0.8 1.53e+04 - 3.61e-05 0.00e+00S 2
44 1.8323742e-02 2.80e+00 1.58e+05 -0.8 2.16e+04 - 2.77e-05 1.47e-05h 1
45 1.8320476e-02 2.80e+00 1.30e+05 -0.8 3.10e+04 - 1.59e-05 1.75e-05h 1
46 1.8323172e-02 2.80e+00 3.79e+05 -0.8 3.27e+04 - 2.90e-05 1.10e-05h 1
47r 1.8323172e-02 2.80e+00 1.00e+03 0.4 0.00e+00 - 0.00e+00 3.25e-07R 4
48r 2.1166667e-02 1.76e+00 1.04e+03 1.2 1.62e+01 - 9.89e-01 6.41e-02f 1
49 2.1170245e-02 1.76e+00 2.17e+01 -6.0 2.50e+02 - 2.06e-03 4.65e-04h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 2.1177052e-02 1.76e+00 8.07e+01 2.0 7.22e+03 - 1.11e-04 1.64e-04f 1
51 2.1183050e-02 1.76e+00 6.72e+01 -6.0 1.83e+01 - 1.77e-02 7.17e-04h 1
52 2.1251325e-02 1.75e+00 4.35e+03 -0.1 3.03e+02 - 5.64e-05 6.50e-03h 1
53 2.1252109e-02 1.75e+00 4.35e+03 -6.0 6.50e+02 - 5.91e-04 3.03e-05h 1
54 2.1281175e-02 1.75e+00 8.16e+03 0.5 3.53e+02 - 1.53e-03 1.10e-03h 1
55 2.1297986e-02 1.74e+00 1.41e+04 2.0 2.38e+03 - 1.66e-03 3.35e-04f 1
56 2.1298702e-02 1.74e+00 1.52e+04 -5.9 7.25e+02 - 1.39e-03 2.34e-04h 1
57 2.1313097e-02 1.74e+00 1.63e+05 2.1 1.71e+03 - 5.16e-04 1.92e-04h 1
58 2.1292561e-02 1.74e+00 4.77e+05 2.1 2.43e+03 - 3.51e-04 2.23e-04h 1
59 2.1312241e-02 1.74e+00 9.54e+05 2.0 1.79e+03 - 3.69e-06 3.15e-04h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 2.1306805e-02 1.74e+00 4.39e+05 2.0 2.24e+03 - 5.19e-04 5.79e-05h 1
61 2.1301706e-02 1.74e+00 4.53e+06 2.1 5.23e+04 - 1.02e-05 9.83e-06h 1
62 2.1309327e-02 1.74e+00 1.21e+07 2.0 1.18e+04 - 5.75e-05 4.24e-05f 1
63 2.1303907e-02 1.74e+00 1.59e+07 2.0 1.29e+04 - 3.46e-05 2.53e-05h 1
64r 2.1303907e-02 1.74e+00 1.00e+03 2.0 0.00e+00 - 0.00e+00 4.67e-07R 5
65r 2.2523630e-02 1.43e-01 6.54e+02 -0.1 4.66e+00 - 9.88e-01 3.82e-01f 1
66 2.2523961e-02 1.43e-01 7.02e+01 -6.0 1.67e+02 - 2.50e-03 3.48e-05h 1
67 2.2595721e-02 1.43e-01 4.94e+04 1.0 9.65e+02 - 1.10e-03 1.37e-03f 1
68 2.2522202e-02 1.42e-01 2.11e+03 1.0 1.26e+02 - 4.35e-03 7.74e-03f 1
69 2.2539900e-02 1.42e-01 1.36e+04 2.0 1.21e+03 - 5.97e-04 8.53e-04h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 2.2541504e-02 1.42e-01 1.84e+04 0.6 3.94e+03 - 4.58e-04 1.03e-04h 1
71 2.2542148e-02 1.42e-01 2.64e+04 2.0 1.69e+04 - 3.96e-05 1.77e-05h 1
72r 2.2542148e-02 1.42e-01 9.99e+02 2.0 0.00e+00 - 0.00e+00 3.18e-07R 5
73r 2.3468102e-02 6.36e-02 1.46e+03 -0.2 1.50e+00 - 9.83e-01 6.22e-01f 1
74 2.3468202e-02 6.36e-02 7.02e+01 -6.0 5.73e+01 - 5.49e-03 7.63e-05h 1
75 2.3396586e-02 6.34e-02 5.36e+03 0.4 5.73e+02 - 2.49e-04 3.19e-03f 1
76 2.3403947e-02 6.34e-02 5.23e+03 -6.0 5.23e+02 - 3.43e-03 3.42e-04h 1
77r 2.3403947e-02 6.34e-02 9.99e+02 -1.2 0.00e+00 - 0.00e+00 4.96e-07R 5
78r 2.4007021e-02 4.42e-02 9.79e+02 0.9 3.93e+02 - 8.32e-02 5.04e-03f 1
79r 2.2882634e-02 4.36e-02 1.20e+03 -0.4 6.67e+00 - 3.48e-02 6.86e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 2.2938519e-02 4.35e-02 1.27e+03 1.1 1.13e+03 - 5.23e-03 1.66e-03f 1
81 2.2885779e-02 4.35e-02 3.17e+03 2.0 2.90e+03 - 2.52e-03 6.76e-04f 1
82 2.2927942e-02 4.34e-02 6.56e+03 2.1 9.71e+02 - 2.12e-04 9.63e-04f 1
83 2.2894070e-02 4.34e-02 3.62e+04 2.1 5.46e+02 - 1.77e-03 1.47e-03h 1
84 2.2933482e-02 4.33e-02 1.85e+04 2.1 1.01e+03 - 1.65e-03 1.15e-03f 1
85 2.2933421e-02 4.33e-02 1.82e+04 2.2 7.63e+02 - 5.38e-04 1.40e-04h 1
86 2.2933080e-02 4.33e-02 1.98e+04 2.2 6.56e+03 - 8.50e-05 2.30e-05h 1
87 2.2934238e-02 4.33e-02 3.51e+04 2.2 1.31e+04 - 9.55e-06 1.80e-05h 1
88 2.2933909e-02 4.33e-02 2.03e+04 2.2 1.58e+04 - 6.93e-06 6.93e-06s 5
89r 2.2933909e-02 4.33e-02 9.99e+02 2.2 0.00e+00 - 0.00e+00 2.75e-07R 4
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
90r 2.9056609e-02 2.52e-02 7.59e+02 0.4 7.26e-01 - 9.89e-01 9.46e-01f 1
91 2.9095672e-02 2.50e-02 2.47e+02 -0.1 4.57e+01 - 8.08e-03 7.20e-03f 1
92 2.9138676e-02 2.48e-02 4.62e+02 -0.1 3.42e+01 - 4.67e-03 6.39e-03h 1
93 2.9433658e-02 2.41e-02 5.42e+03 -0.1 6.01e+01 - 5.60e-03 2.98e-02f 1
94 2.9469978e-02 2.40e-02 5.79e+03 -0.1 7.74e+01 - 8.61e-03 3.17e-03h 1
95 2.9504369e-02 2.40e-02 6.39e+03 -0.1 1.04e+02 - 7.84e-03 1.68e-03h 1
96 2.9567829e-02 2.39e-02 1.15e+04 -0.1 1.57e+02 - 7.87e-03 2.39e-03h 1
97 2.9569224e-02 2.39e-02 1.23e+04 -0.1 5.91e+02 - 2.71e-03 5.50e-04h 1
98 2.9577479e-02 2.39e-02 1.71e+04 -0.1 9.61e+02 - 1.31e-03 7.94e-05h 1
99 2.9578573e-02 2.39e-02 2.83e+04 -0.1 6.38e+03 - 2.32e-04 7.87e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
100r 2.9578573e-02 2.39e-02 9.99e+02 -0.1 0.00e+00 - 0.00e+00 4.44e-07R 3
101r 3.0104328e-02 2.13e-02 9.76e+02 0.9 1.51e+02 - 1.65e-01 1.31e-02f 1
102 3.0105277e-02 2.13e-02 3.86e+02 -0.8 5.64e+01 - 1.51e-02 4.08e-05h 1
103 3.0314551e-02 2.13e-02 3.54e+04 -0.8 5.80e+02 - 3.97e-05 1.22e-03h 1
104 3.0247370e-02 2.12e-02 3.15e+04 -0.8 5.07e+01 - 1.35e-02 3.67e-03f 1
105 3.0176236e-02 2.11e-02 2.17e+04 -0.8 2.19e+02 - 2.02e-03 2.46e-03h 1
106 3.0175514e-02 2.11e-02 2.17e+04 -0.8 4.90e+02 - 2.39e-03 1.10e-05h 1
107 3.0035010e-02 2.11e-02 1.50e+05 -0.8 4.99e+02 - 3.05e-05 2.02e-03h 1
108 3.0115472e-02 2.11e-02 2.66e+04 -0.8 1.63e+02 - 2.65e-03 2.18e-03h 1
109 3.0125298e-02 2.11e-02 1.07e+04 -0.8 2.80e+03 - 1.95e-05 3.32e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
110 3.0123941e-02 2.11e-02 2.69e+03 -0.8 5.47e+04 - 9.24e-07 9.24e-07s 2
111 3.0123741e-02 2.11e-02 1.91e+03 -1.5 8.43e+05 - 7.36e-08 7.36e-08s 2
112r 3.0123741e-02 2.11e-02 9.99e+02 -1.7 0.00e+00 - 0.00e+00 2.34e-08R 2
113r 3.4592716e-02 2.14e-02 9.96e+02 1.1 1.61e+02 - 2.13e-02 3.15e-03f 1
114r 3.4398532e-02 2.33e-02 9.94e+02 -0.7 9.98e+00 - 2.08e-02 2.34e-03f 1
115r 3.4421036e-02 1.66e-01 9.84e+02 -0.7 2.04e+01 - 7.80e-03 1.04e-02f 1
116r 3.5428102e-02 1.88e-01 1.01e+03 -0.7 1.68e+01 - 1.33e-03 3.84e-02f 1
117r 3.5215711e-02 1.54e-01 1.07e+03 0.9 1.12e+02 - 2.70e-01 1.66e-03f 1
118r 3.5346222e-02 1.58e-01 1.05e+03 -0.3 4.76e+00 - 2.03e-01 3.23e-02f 1
119r 3.5528134e-02 8.83e-02 9.29e+02 0.4 3.36e+01 - 9.10e-01 5.81e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
120r 3.6543324e-02 8.01e-02 9.17e+02 -0.0 4.97e+00 - 3.81e-01 3.66e-01f 1
121r 3.6497739e-02 2.30e-02 7.22e+02 0.8 2.64e+00 - 6.04e-01 1.34e-01f 1
122r 3.6381587e-02 1.80e-02 1.10e+03 0.8 5.59e+00 - 9.48e-01 1.61e-01f 1
123 3.6470868e-02 1.74e-02 9.82e+01 -6.0 2.20e+00 - 1.31e-01 3.07e-02h 1
124 3.7014435e-02 1.57e-02 2.72e+03 -0.4 1.14e+01 - 5.80e-02 9.54e-02h 1
125 3.7033359e-02 1.56e-02 2.99e+03 0.5 5.45e+01 - 8.48e-02 8.99e-03f 1
126 3.7019655e-02 1.55e-02 3.61e+03 0.3 5.28e+01 - 2.88e-02 8.29e-03h 1
127 3.7124498e-02 1.54e-02 5.77e+03 -6.0 1.30e+02 - 9.22e-03 3.48e-03h 1
128 3.7125216e-02 1.54e-02 5.18e+04 1.8 2.90e+02 - 4.77e-01 1.11e-03h 1
129 3.7121150e-02 1.54e-02 6.38e+04 3.0 2.11e+02 - 2.58e-03 1.65e-03h 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
130 3.7196382e-02 1.53e-02 7.61e+05 3.0 2.43e+02 - 3.73e-03 1.77e-03h 2
131 3.7143439e-02 1.53e-02 1.00e+06 3.0 4.55e+02 - 2.37e-03 1.77e-03h 1
132 3.7185840e-02 1.53e-02 1.20e+06 3.0 4.60e+02 - 2.90e-03 4.89e-04h 2
133 3.7198399e-02 1.53e-02 6.36e+06 3.0 1.04e+04 - 7.92e-05 7.66e-05h 1
134 3.7192256e-02 1.53e-02 3.75e+06 3.0 1.42e+04 - 2.06e-05 2.38e-05h 2
135 3.7192015e-02 1.53e-02 3.69e+06 3.0 4.48e+04 - 6.94e-07 6.94e-07s 5
136 3.7191451e-02 1.53e-02 3.20e+06 1.4 5.21e+04 - 1.70e-06 1.70e-06s 4
137 3.7191227e-02 1.53e-02 2.59e+06 0.7 9.62e+04 - 1.13e-06 1.13e-06s 3
138r 3.7191227e-02 1.53e-02 9.84e+02 0.0 0.00e+00 - 0.00e+00 5.99e-08R 2
139r 3.8025960e-02 9.36e-03 5.87e+02 0.3 6.55e+00 - 7.83e-01 2.89e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
140 3.8037126e-02 9.36e-03 8.07e+01 -6.0 1.83e+01 - 2.80e-02 3.39e-04h 1
141 3.9370874e-02 9.32e-03 2.65e+04 1.0 3.88e+02 - 6.60e-04 4.68e-03f 1
142 3.8329436e-02 8.98e-03 1.61e+04 -0.1 1.08e+01 - 5.06e-02 3.57e-02f 2
143 3.7896292e-02 8.90e-03 9.22e+03 -0.1 5.84e+01 - 4.32e-02 9.71e-03h 2
144 3.7728931e-02 8.88e-03 8.56e+04 1.0 1.18e+02 - 1.00e+00 2.01e-03h 4
145 3.6271434e-02 8.79e-03 1.18e+06 3.0 1.44e+02 - 1.43e-02 9.80e-03f 1
146 3.6825110e-02 8.28e-03 2.90e+05 3.0 3.24e+01 - 3.83e-02 5.76e-02f 1
147 3.6687721e-02 8.26e-03 4.48e+05 1.7 5.69e+01 - 1.56e-02 2.66e-03h 1
148 3.6645700e-02 8.26e-03 5.49e+05 1.0 4.40e+02 - 1.65e-03 7.26e-04h 3
149 3.6649385e-02 8.24e-03 3.16e+05 0.3 4.82e+02 - 4.24e-03 1.87e-03h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
150 3.6671686e-02 8.24e-03 5.36e+05 -0.4 1.33e+03 - 6.57e-04 4.13e-04h 2
151 3.6676663e-02 8.24e-03 1.62e+06 -0.4 2.52e+03 - 1.65e-04 2.21e-04h 1
152 3.6672576e-02 8.24e-03 2.06e+06 -0.4 3.82e+03 - 1.79e-04 6.88e-05h 1
153r 3.6672576e-02 8.24e-03 9.99e+02 -0.4 0.00e+00 - 0.00e+00 3.46e-07R 5
154r 3.7313494e-02 5.23e-03 4.41e+02 0.5 2.42e+00 - 9.25e-01 1.14e-01f 1
155r 3.9796528e-02 9.80e-03 5.68e+02 0.2 1.75e+00 - 7.13e-01 1.00e+00f 1
156r 4.0386299e-02 3.13e-02 1.11e+03 0.2 5.95e-01 - 4.75e-01 1.00e+00f 1
157r 4.0445106e-02 1.69e-02 6.20e+02 0.2 1.10e+00 - 5.35e-01 8.45e-01f 1
158r 4.0502704e-02 1.78e-02 6.16e+02 0.2 7.18e-01 - 1.00e+00 1.00e+00f 1
159r 4.0551119e-02 1.73e-02 2.41e+02 0.2 5.83e-01 - 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
160r 4.0558604e-02 1.75e-02 1.24e+01 0.2 2.14e-01 - 1.00e+00 1.00e+00f 1
161r 4.1261930e-02 3.44e-02 7.11e+01 -0.5 1.85e-01 - 5.87e-01 1.00e+00f 1
162r 4.1637866e-02 1.81e-01 8.18e+01 -0.8 1.73e-01 - 1.00e+00 8.57e-01f 1
163r 4.1906428e-02 1.30e-01 3.77e+01 -0.5 2.84e-01 - 1.00e+00 1.00e+00f 1
164r 4.1861352e-02 2.14e-01 3.20e+01 -0.9 2.89e-01 - 9.55e-01 1.00e+00f 1
165r 4.2171003e-02 2.26e-01 1.60e+01 -0.9 7.98e-02 - 9.56e-01 1.00e+00f 1
166r 4.2500738e-02 2.78e-01 1.77e+01 -1.5 1.91e-01 - 9.80e-01 1.00e+00f 1
167r 4.3259290e-02 2.60e-01 6.96e+01 -1.5 3.17e-01 - 8.69e-01 1.00e+00f 1
168r 4.3561561e-02 2.74e-01 2.94e+00 -2.3 1.70e-01 - 1.00e+00 1.00e+00f 1
169r 4.4530811e-02 2.58e-01 4.40e+01 -2.5 6.56e-01 - 9.91e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
170r 4.5083577e-02 2.55e-01 9.84e+00 -3.6 1.22e-01 - 1.00e+00 8.52e-01f 1
171r 4.5557990e-02 2.49e-01 1.40e+01 -3.3 9.07e-01 - 1.00e+00 2.69e-01f 1
172r 4.6957762e-02 2.33e-01 1.26e+01 -3.3 2.75e-01 - 1.00e+00 6.32e-01f 1
173r 4.7315746e-02 2.32e-01 2.65e+00 -3.3 3.79e-02 0.0 1.00e+00 1.00e+00f 1
174r 4.8757752e-02 2.20e-01 1.51e+00 -3.0 1.15e-01 -0.5 1.00e+00 1.00e+00f 1
175r 4.9959064e-02 2.12e-01 1.41e+01 -3.1 9.13e-02 -0.1 6.87e-01 1.00e+00f 1
176r 5.0570576e-02 2.08e-01 5.33e+00 -3.1 5.58e-02 0.4 1.00e+00 1.00e+00f 1
177r 5.5162876e-02 1.74e-01 1.24e+02 -2.1 4.75e-01 -0.1 1.00e+00 1.00e+00f 1
178r 5.7792897e-02 1.64e-01 9.11e+01 -2.2 2.74e-01 0.3 3.15e-01 1.00e+00f 1
179r 7.3786328e-02 1.62e-01 1.47e+03 -2.2 2.28e+00 -0.2 5.30e-02 8.22e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
180r 7.4009441e-02 1.63e-01 1.46e+03 -8.2 1.12e+01 -0.6 3.13e-02 3.72e-03f 1
181r 7.7038519e-02 1.62e-01 1.32e+03 -0.2 1.76e+01 -0.2 2.16e-01 9.90e-02f 1
182r 7.8775184e-02 1.63e-01 1.19e+03 -1.2 2.48e+00 - 8.64e-02 9.70e-02f 1
183r 7.9499945e-02 1.62e-01 1.18e+03 -1.2 5.66e+00 -0.7 3.99e-02 1.35e-02f 1
184r 8.2421297e-02 1.51e-01 1.17e+03 -1.2 1.64e+01 - 1.36e-02 2.20e-02f 1
185r 8.7978433e-02 1.24e-01 1.13e+03 -1.2 9.20e+00 - 3.18e-02 7.29e-02f 1
186r 9.2465549e-02 1.03e-01 1.09e+03 -1.2 7.62e+00 - 4.29e-02 5.02e-02f 1
187r 1.0163786e-01 6.59e-02 1.08e+03 -1.2 1.16e+01 - 2.72e-02 6.57e-02f 1
188r 1.1195153e-01 3.07e-02 1.03e+03 -1.2 5.33e+00 -1.2 7.05e-02 1.06e-01f 1
189r 1.2220063e-01 1.17e-02 8.58e+02 -0.4 1.80e+00 -0.7 4.79e-01 2.20e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
190r 1.0986595e-01 1.50e-02 3.86e+02 -0.9 8.51e-01 - 7.04e-02 6.68e-01h 1
191r 1.2035290e-01 1.15e-02 1.89e+02 -0.9 5.96e-01 -0.3 1.00e+00 6.41e-01f 1
192r 1.2471169e-01 5.26e-03 1.81e+02 -0.9 2.49e+00 - 8.77e-02 7.03e-02f 1
193 1.3098173e-01 5.26e-04 7.54e-01 -1.7 2.18e-01 - 9.86e-01 1.00e+00h 1
194 1.3106884e-01 2.01e-06 6.33e-03 -3.5 2.42e-02 - 9.92e-01 1.00e+00h 1
195 1.2341990e-01 3.59e-03 8.72e-03 -3.9 5.17e-01 - 9.41e-01 1.00e+00h 1
196 1.1985298e-01 1.30e-03 8.62e-03 -4.7 4.13e-01 - 9.99e-01 1.00e+00h 1
197 1.1937034e-01 2.18e-04 2.04e-03 -5.0 4.25e-01 - 1.00e+00 9.94e-01h 1
198 1.1940068e-01 2.11e-05 7.63e-05 -5.0 1.52e-01 - 9.48e-01 1.00e+00h 1
199 1.1904345e-01 7.62e-05 4.55e-04 -7.1 2.53e-01 - 8.60e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
200 1.1903499e-01 1.10e-05 6.75e-05 -6.6 1.84e-01 - 1.00e+00 1.00e+00h 1
201 1.1903052e-01 3.13e-06 1.70e-05 -6.7 1.15e-01 - 1.00e+00 8.48e-01h 1
202 1.1902670e-01 8.46e-07 2.27e-06 -7.0 6.93e-02 - 8.87e-01 1.00e+00h 1
203 1.1902283e-01 4.64e-07 1.71e-06 -7.9 5.34e-02 - 9.95e-01 1.00e+00h 1
204 1.1902232e-01 7.37e-08 2.47e-07 -9.0 1.01e-01 - 1.00e+00 1.00e+00h 1
205 1.1902228e-01 9.83e-09 5.88e-08 -10.0 1.31e-01 - 1.00e+00 1.00e+00h 1
206 1.1902227e-01 1.16e-09 2.28e-08 -11.0 1.31e-01 - 1.00e+00 1.00e+00h 1
207 1.1902227e-01 5.17e-11 1.66e-09 -12.3 3.79e-02 - 1.00e+00 1.00e+00h 1
208 1.1902227e-01 2.47e-13 6.75e-12 -14.3 2.36e-03 - 1.00e+00 1.00e+00h 1
209 1.1902227e-01 8.88e-16 3.04e-16 -14.3 1.02e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 209
(scaled) (unscaled)
Objective...............: 1.1902227406488765e-01 1.1902227406488765e-01
Dual infeasibility......: 3.0393205502123122e-16 3.0393205502123122e-16
Constraint violation....: 8.8817841970012523e-16 8.8817841970012523e-16
Complementarity.........: 5.0007227086743682e-15 5.0007227086743682e-15
Overall NLP error.......: 5.0007227086743682e-15 5.0007227086743682e-15
Number of objective function evaluations = 296
Number of objective gradient evaluations = 161
Number of equality constraint evaluations = 296
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 222
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 209
Total CPU secs in IPOPT (w/o function evaluations) = 1.873
Total CPU secs in NLP function evaluations = 1.622
EXIT: Optimal Solution Found.
Solver status : nothing
 
 
 
Cost : 0.11902227406488765
tf = 23.80445481297753
 
 
%% Cell type:code id: tags:
 
``` julia
#JuMP model, Ipopt solver
sys3 = Model(optimizer_with_attributes(Ipopt.Optimizer,"print_level"=> 5))
set_optimizer_attribute(sys3,"tol",1e-14)
set_optimizer_attribute(sys3,"max_iter",1000)
 
#Parameters
 
# constants
w = 0.8
l = 0.99
 
# initial data
x0 = 0
y0 = 0
θ0 = pi/7
β1_0 = 0
β2_0 = 0
β3_0 = 0
 
# final data
xf = 4
yf = 7
θf = 3*pi/2
β1_f = 0
β2_f = 0
β3_f = 0
 
N = 200
# Bounds for variables
 
JuMP.@variables(sys3,begin
x[1:N] # x
y[1:N] # y
θ[1:N] # theta
β1[1:N] # beta1
β2[1:N] # beta2
β3[1:N] # beta3
-1 ≤ u[1:N] ≤ 1 # u, control
0 ≤ Δt ≤ 1
end)
 
# Objective
@objective(sys3,Min,Δt)
 
# Constraints
 
@constraints(sys3,begin
x[1] == x0
y[1] == y0
θ[1] == θ0
β1[1]== β1_0
β2[1] == β2_0
β3[1] == β3_0
x[N] == xf
y[N] == yf
θ[N] == θf
β1[N] == β1_f
β2[N] == β2_f
β3[N] == β3_f
end)
 
# Dynamics : Crank-Nicolson scheme
 
for j in 1 : N-1
@NLconstraint(sys3, # x' = w + cos(theta)
x[j+1] == x[j] + 0.5 * Δt * (w + cos(θ[j]) + w + cos(θ[j+1])))
@NLconstraint(sys3, # y' = sin(theta)
y[j+1] == y[j] + 0.5* Δt *(sin(θ[j])+ sin(θ[j+1])))
@NLconstraint(sys3, # theta ' = u
θ[j+1] == θ[j] + 0.5 * Δt * (u[j] + u[j+1]) )
@NLconstraint(sys3, #beta1 ' = -u- 1/l * sin(beta1)
β1[j+1] == β1[j] + 0.5 * Δt * (-u[j]-(1/l)*sin(β1[j]) - u[j+1] - (1/l)*sin(β1[j+1])))
@NLconstraint(sys3, #beta2' = -u - cos(beta1)*sin(beta2 - beta1)/l
β2[j+1] == β2[j] + 0.5 * Δt * (-u[j] - cos(β1[j])*sin(β2[j] - β1[j])/l - u[j+1] - cos(β1[j+1])*sin(β2[j+1] - β1[j+1])/l ))
@NLconstraint(sys3, #beta3' = -u - cos(beta1) * cos(beta2-beta1)*sin(beta3 - beta2)
β3[j+1] == β3[j] +0.5 * Δt * (-u[j] - cos(β1[j])*cos(β2[j] - β1[j]) * sin(β3[j] - β2[j])/l -u[j+1] - cos(β1[j+1])*cos(β2[j+1] - β1[j+1]) * sin(β3[j+1] - β2[j+1])/l ))
end
 
#Solve for the control and state
println("Solving...")
status = optimize!(sys3)
println("Solver status : ",status)
x1 = value.(x)
y1 = value.(y)
θ1 = value.(θ)
β_1 = value.(β1)
β_2 = value.(β2)
β_3 = value.(β3)
u1 = value.(u)
println("Cost : " , objective_value(sys3))
 
println("tf = ", value.(Δt)*N)
 
#plots : states
 
Δt1 = value.(Δt)
t = (1 : N)*Δt1
#h = (1 : N)
 
x_plot = plot(t,x1,xlabel = "t", ylabel = "position x", legend = false, fmt = :png)
y_plot = plot(t,y1,xlabel = "t", ylabel = "position y", legend = false, fmt = :png)
θ_plot = plot(t,θ1,xlabel = "t", ylabel = "θ", legend = false, fmt = :png)
β1_plot = plot(t,β_1,xlabel = "t", ylabel = "β1", legend = false, fmt = :png)
β2_plot = plot(t,β_2,xlabel = "t", ylabel = "β2", legend = false, fmt = :png)
β3_plot = plot(t,β_3,xlabel = "t", ylabel = "β3", legend = false, fmt = :png)
display(plot(x_plot,y_plot,θ_plot,β1_plot,β2_plot,β3_plot ,layout = (3,2)))
u_plot = plot(t,u1,xlabel = "t", ylabel = "control", legend = false, fmt = :png)
display(u_plot)
 
 
#plots : trajectory
 
p = plot(x1,y1, c = :blue, lw = 7)
plot!(size=(2000,2000))
 
for i = 1 : N
z = [x1[i] y1[i]]
θ_p = θ1[i]
β1_p = β_1[i]
β2_p = β_2[i]
β3_p = β_3[i]
θ_1 = β1_p + θ_p
θ_2 = β2_p + θ_p
θ_3 = β3_p + θ_p
z1 = z - l*[cos(θ_1) sin(θ_1)]
z2 = z1 - l*[cos(θ_2) sin(θ_2)]
z3 = z2 - l*[cos(θ_3) sin(θ_3)]
plot!([z[1],z1[1]], [z[2],z1[2]] ,color=:black, legend = false)
plot!([z1[1],z2[1]], [z1[2],z2[2]], color=:black, legend = false)
plot!([z2[1],z3[1]], [z2[2],z3[2]], color=:black, legend = false)
plot!([z[1]],[z[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z1[1]],[z1[2]],seriestype = :scatter, color =:black , legend = false)
plot!([z2[1]],[z2[2]],seriestype = :scatter, color =:black , legend = false)
plot!(size=(2000,2000))
end
current()
```
 
%% Output
 
Solving...
This is Ipopt version 3.13.4, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).
Number of nonzeros in equality constraint Jacobian...: 7176
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 21094
Total number of variables............................: 1401
variables with only lower bounds: 0
variables with lower and upper bounds: 201
variables with only upper bounds: 0
Total number of equality constraints.................: 1206
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 9.9999900e-03 7.00e+00 2.89e-15 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.0000221e-02 7.00e+00 2.19e+04 -6.0 5.01e+03 - 1.98e-04 1.98e-04h 1
2 1.0010569e-02 7.00e+00 3.18e+04 1.9 3.75e+03 - 3.78e-04 9.86e-05h 1
3 1.0020246e-02 7.00e+00 4.17e+04 1.9 6.50e+03 - 1.89e-04 5.17e-05h 2
4 1.0024951e-02 7.00e+00 4.59e+04 2.0 1.02e+04 - 1.23e-04 1.92e-05h 3
5r 1.0024951e-02 7.00e+00 9.77e+02 2.2 0.00e+00 - 0.00e+00 4.62e-07R 8
6r 8.2382125e-03 4.89e+00 6.43e+02 0.1 5.28e+00 - 8.26e-01 4.05e-01f 1
7 8.2393855e-03 4.89e+00 5.58e+01 -6.0 3.10e+03 - 5.71e-04 1.65e-05h 1
8 8.2463688e-03 4.89e+00 6.43e+03 -6.0 9.95e+03 - 1.30e-04 6.05e-05h 1
9 8.2493351e-03 4.89e+00 9.23e+03 -6.0 4.43e+04 - 2.92e-05 1.54e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10r 8.2493351e-03 4.89e+00 9.99e+02 0.7 0.00e+00 - 0.00e+00 4.37e-07R 5
11r 1.0141994e-02 4.13e+00 2.27e+03 1.0 7.60e+00 - 9.96e-01 1.22e-01f 1
12r 1.0141994e-02 4.13e+00 9.99e+02 0.6 0.00e+00 - 0.00e+00 2.57e-07R 6
13r 1.3942359e-02 3.02e+00 1.61e+03 1.4 1.50e+01 - 9.93e-01 7.65e-02f 1
14r 1.3964690e-02 3.00e+00 9.08e+02 0.8 2.81e+00 - 8.80e-01 9.22e-03f 1
15r 1.4719787e-02 2.63e+00 8.70e+02 0.8 1.98e+01 - 6.48e-01 9.84e-02f 1
16 1.4743076e-02 2.63e+00 1.62e+03 -5.6 1.79e+03 - 4.33e-04 5.88e-04h 1
17 1.4776318e-02 2.63e+00 4.26e+03 1.3 2.10e+03 - 6.48e-04 7.40e-04h 1
18 1.4776987e-02 2.63e+00 4.26e+03 1.0 1.33e+03 - 3.01e-03 1.12e-05h 1
19 1.4847421e-02 2.63e+00 2.56e+04 1.9 8.32e+02 - 1.89e-03 1.01e-03h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 1.4894236e-02 2.62e+00 4.20e+04 2.6 1.13e+03 - 3.11e-03 5.75e-04h 1
21 1.4936635e-02 2.62e+00 8.78e+04 2.8 1.42e+03 - 4.84e-03 5.30e-04h 1
22 1.5007990e-02 2.62e+00 8.05e+05 3.0 1.28e+03 - 7.86e-04 8.79e-04h 1
23 1.5011001e-02 2.62e+00 8.13e+05 -5.0 2.90e+03 - 4.32e-04 3.38e-05h 1
24 1.5016713e-02 2.62e+00 9.00e+05 3.0 3.74e+03 - 1.19e-03 6.30e-05h 1
25 1.5021981e-02 2.62e+00 1.59e+06 3.0 3.48e+03 - 1.68e-04 5.71e-05h 1
26 1.5027184e-02 2.62e+00 4.12e+06 3.0 2.66e+03 - 4.15e-05 5.80e-05h 1
27 1.5029048e-02 2.62e+00 5.52e+06 3.0 3.65e+03 - 6.31e-05 2.08e-05h 1
28r 1.5029048e-02 2.62e+00 1.00e+03 3.0 0.00e+00 - 0.00e+00 4.24e-07R 5
29r 1.5980527e-02 5.09e-01 1.29e+03 0.8 5.28e+00 - 9.89e-01 6.02e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.5981576e-02 5.09e-01 1.76e+01 -0.1 1.70e+03 - 4.08e-04 2.18e-05h 1
31r 1.5981576e-02 5.09e-01 9.99e+02 -0.3 0.00e+00 - 0.00e+00 2.88e-07R 4
32r 1.5966466e-02 4.44e-01 9.98e+02 -1.2 2.26e+02 - 1.31e-02 1.09e-03f 1
33 1.5967118e-02 4.44e-01 3.23e+01 -6.0 1.34e+03 - 3.50e-04 1.04e-05h 1
34 1.5969277e-02 4.44e-01 9.38e+02 1.5 3.05e+03 - 6.88e-04 2.52e-05h 1
35r 1.5969277e-02 4.44e-01 9.73e+02 1.9 0.00e+00 - 0.00e+00 2.76e-07R 5
36r 1.7139099e-02 2.19e-01 4.82e+03 0.7 3.08e+00 - 9.16e-01 5.49e-01f 1
37 1.7146381e-02 2.19e-01 7.46e+01 -6.0 3.89e+02 - 1.54e-03 1.08e-04h 1
38 1.7155167e-02 2.19e-01 8.85e+02 0.4 9.15e+02 - 1.04e-03 9.78e-05h 1
39 1.7160838e-02 2.19e-01 5.89e+03 1.7 8.00e+03 - 3.64e-04 5.68e-05h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40r 1.7160838e-02 2.19e-01 9.23e+02 2.0 0.00e+00 - 0.00e+00 4.33e-07R 5
41r 2.0295498e-02 9.71e-02 8.02e+03 1.1 2.22e+00 - 9.76e-01 8.73e-01f 1
42 2.0330147e-02 9.70e-02 2.54e+02 -6.0 8.46e+01 - 7.28e-03 4.51e-04h 1
43 2.0358622e-02 9.70e-02 2.98e+03 0.5 5.78e+02 - 2.41e-03 3.26e-04h 1
44 2.0370435e-02 9.70e-02 1.12e+04 1.4 3.01e+03 - 3.29e-04 1.26e-04h 1
45 2.0372642e-02 9.70e-02 1.74e+04 1.9 4.83e+03 - 3.48e-04 2.45e-05h 1
46r 2.0372642e-02 9.70e-02 9.99e+02 1.7 0.00e+00 - 0.00e+00 2.97e-07R 3
47r 2.5892658e-02 7.31e-02 4.33e+03 1.0 1.87e+00 - 9.60e-01 1.00e+00f 1
48 2.6112439e-02 7.30e-02 1.22e+02 -0.8 1.42e+02 - 1.76e-03 2.27e-03h 1
49 2.6337700e-02 7.28e-02 4.29e+02 -0.6 1.92e+02 - 1.08e-03 2.42e-03h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 2.6773035e-02 7.24e-02 9.00e+02 -0.0 1.77e+02 - 1.31e-03 4.80e-03f 1
51 2.6845472e-02 7.24e-02 9.12e+02 0.5 1.08e+02 - 9.35e-03 8.13e-04f 1
52 2.7602405e-02 7.18e-02 2.28e+03 0.8 9.07e+01 - 2.25e-01 8.50e-03f 1
53 2.9310861e-02 7.03e-02 3.49e+04 2.1 8.73e+01 - 3.13e-02 1.94e-02f 1
54 3.1349453e-02 6.85e-02 5.36e+04 2.6 5.02e+01 - 8.31e-01 2.43e-02f 1
55 3.5392490e-02 6.43e-02 7.90e+05 3.0 2.08e+01 - 2.91e-02 4.97e-02f 1
56 3.8426146e-02 6.25e-02 7.26e+05 3.0 3.40e+01 - 2.85e-02 2.72e-02f 1
57 4.6579619e-02 5.77e-02 1.51e+05 3.0 1.29e+01 - 1.37e-01 7.82e-02f 1
58 6.0288081e-02 5.27e-02 6.79e+05 2.4 1.53e+01 - 1.25e-01 9.12e-02f 1
59 6.7126074e-02 4.88e-02 1.90e+05 2.4 1.44e+01 - 3.97e-02 6.96e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 7.3745301e-02 4.70e-02 1.89e+05 1.7 2.38e+01 - 5.79e-03 3.53e-02f 2
61 8.1311374e-02 4.73e-02 1.93e+05 1.7 1.92e+01 - 1.23e-01 2.71e-02h 2
62 9.1708446e-02 4.71e-02 1.96e+05 1.7 2.93e+01 - 1.33e-01 2.13e-02h 3
63 1.0044673e-01 4.71e-02 1.97e+05 1.7 2.13e+02 - 5.50e-03 2.33e-03f 4
64 1.2034125e-01 4.06e-02 2.17e+05 1.7 8.40e+00 2.0 4.12e-01 1.90e-01h 2
65 1.3527818e-01 4.24e-02 2.21e+05 -3.8 1.11e+01 1.5 3.02e-02 6.10e-02h 2
66 1.4499544e-01 4.32e-02 2.17e+05 1.9 4.64e+01 1.9 3.30e-02 1.98e-02h 2
67 1.5435295e-01 4.25e-02 1.82e+05 2.9 2.01e+01 2.4 9.37e-01 9.43e-02f 1
68 1.7116287e-01 4.27e-02 1.66e+05 3.0 5.63e+00 1.9 2.74e-01 1.01e-01h 1
69 2.0555551e-01 4.90e-02 4.65e+04 3.0 4.31e+00 2.3 8.23e-01 3.48e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 2.4157717e-01 3.84e-02 1.48e+05 2.8 2.16e+00 2.7 8.41e-01 6.03e-01h 1
71 2.8429299e-01 4.83e-02 1.17e+05 3.0 1.52e+00 2.3 7.90e-01 6.79e-01f 1
72 2.9134696e-01 1.32e-03 5.76e+03 2.3 2.33e-01 2.7 9.95e-01 1.00e+00f 1
73 2.9174781e-01 2.66e-06 2.61e+01 0.2 9.58e-03 2.2 9.90e-01 1.00e+00h 1
74 2.9174720e-01 7.46e-12 7.67e-02 -5.5 1.83e-05 1.7 9.90e-01 1.00e+00h 1
75 2.6201206e-01 2.01e-02 3.12e-02 -7.5 2.32e+00 - 8.09e-01 1.00e+00f 1
76 1.5923989e-01 2.69e-01 3.98e-01 -3.1 1.06e+01 - 6.76e-01 9.68e-01h 1
77 2.1283261e-01 2.09e-01 5.25e-01 -2.7 3.52e+00 - 7.81e-01 4.01e-01h 1
78 2.1755507e-01 1.97e-01 6.00e+01 -2.7 2.05e+00 1.3 2.02e-01 5.77e-02h 1
79 3.0030458e-01 2.30e-01 2.85e+03 -2.7 2.12e+00 0.8 5.70e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 3.1590161e-01 1.87e-02 1.71e+03 -2.7 7.31e-01 1.2 7.51e-02 1.00e+00h 1
81 3.1768700e-01 9.81e-03 1.08e+03 -2.7 3.35e-01 1.6 1.00e+00 4.77e-01h 1
82 3.2008023e-01 4.34e-04 3.86e+00 -2.7 1.97e-01 1.2 1.00e+00 1.00e+00h 1
83 3.2013258e-01 1.64e-07 3.74e-01 -2.7 2.46e-03 0.7 9.96e-01 1.00e+00h 1
84 3.2011724e-01 2.56e-07 2.37e-02 -2.7 1.46e-02 0.2 1.00e+00 1.00e+00h 1
85 1.4576524e-01 1.92e-01 1.33e-01 -2.7 2.60e+01 - 4.87e-01 3.34e-01f 1
86 2.2074725e-01 5.38e-02 1.71e-01 -2.7 2.87e+00 - 9.07e-01 1.00e+00h 1
87 2.0540306e-01 1.42e-02 3.07e-02 -2.7 1.36e+00 - 1.00e+00 1.00e+00h 1
88 1.9249139e-01 3.86e-03 3.17e-02 -3.7 8.34e-01 - 9.75e-01 1.00e+00h 1
89 1.8701439e-01 2.81e-03 2.91e-02 -4.4 3.44e-01 - 9.96e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
90 1.8656604e-01 8.13e-04 2.68e-03 -4.4 2.54e-01 - 1.00e+00 9.86e-01h 1
91 1.8607813e-01 2.14e-04 8.78e-04 -5.1 1.82e-01 - 9.94e-01 1.00e+00h 1
92 1.8588210e-01 9.66e-05 3.23e-04 -6.0 3.63e-01 - 1.00e+00 1.00e+00h 1
93 1.8585419e-01 2.29e-05 5.06e-05 -6.5 2.62e-01 - 9.98e-01 1.00e+00h 1
94 1.8584633e-01 4.46e-06 8.63e-06 -7.1 2.37e-01 - 9.93e-01 1.00e+00h 1
95 1.8584414e-01 1.91e-06 2.53e-06 -7.9 2.42e-01 - 1.00e+00 1.00e+00h 1
96 1.8584370e-01 7.50e-07 1.02e-06 -8.6 2.84e-01 - 1.00e+00 1.00e+00h 1
97 1.8584365e-01 1.20e-07 2.33e-07 -8.7 2.17e-01 - 1.00e+00 9.91e-01h 1
98 1.8584359e-01 5.69e-08 1.01e-07 -9.5 1.48e-01 - 9.96e-01 1.00e+00h 1
99 1.8584356e-01 2.23e-08 1.25e-07 -10.5 2.41e-01 - 1.00e+00 9.97e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
100 1.8584356e-01 2.12e-09 1.29e-08 -10.2 7.77e-02 - 1.00e+00 9.99e-01h 1
101 1.8584356e-01 1.07e-09 7.08e-09 -11.6 5.61e-02 - 1.00e+00 1.00e+00h 1
102 1.8584356e-01 5.59e-11 4.35e-10 -12.5 1.42e-02 - 1.00e+00 1.00e+00h 1
103 1.8584356e-01 2.92e-13 2.20e-12 -14.3 1.00e-03 - 1.00e+00 1.00e+00h 1
104 1.8584356e-01 1.78e-15 2.17e-11 -14.3 3.74e-06 - 1.00e+00 1.00e+00h 1
105 1.8584356e-01 1.78e-15 1.09e-16 -14.3 6.39e-11 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 105
(scaled) (unscaled)
Objective...............: 1.8584356195814677e-01 1.8584356195814677e-01
Dual infeasibility......: 1.0917138193012718e-16 1.0917138193012718e-16
Constraint violation....: 1.7763568394002505e-15 1.7763568394002505e-15
Complementarity.........: 5.0002935672481889e-15 5.0002935672481889e-15
Overall NLP error.......: 5.0002935672481889e-15 5.0002935672481889e-15
Number of objective function evaluations = 171
Number of objective gradient evaluations = 104
Number of equality constraint evaluations = 171
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 114
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 105
Total CPU secs in IPOPT (w/o function evaluations) = 1.107
Total CPU secs in NLP function evaluations = 0.745
EXIT: Optimal Solution Found.
Solver status : nothing
 
 
 
Cost : 0.18584356195814677
tf = 37.168712391629356
 
 
%% Cell type:code id: tags:
 
``` julia
```
examples/nav/nav-julia-N4.ipynb 0 → 100644
+ 11083
− 0
View file @ 2806a28c
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment