Skip to content

Commit 6d93433

Browse files
bjack205bjack205
committed
Update examples
1 parent 117315e commit 6d93433

File tree

3 files changed

+282
-0
lines changed

3 files changed

+282
-0
lines changed

‎examples/benchmark_problems.jl

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
using TrajectoryOptimization
2+
using BenchmarkTools
3+
using TrajOptPlots
4+
using Plots
5+
using LinearAlgebra
6+
using MeshCat
7+
8+
vis = Visualizer()
9+
open(vis)
10+
11+
solver = DIRCOLSolver(Problems.Quadrotor()...,integration=HermiteSimpson)
12+
solve!(solver)
13+
cost(solver)
14+
set_mesh!(vis, get_model(solver))
15+
visualize!(vis, solver)
16+
17+
# Double Integrator
18+
solver = ALTROSolver(Problems.DoubleIntegrator()...)
19+
benchmark_solve!(solver)
20+
iterations(solver) # 8
21+
plot(solver)
22+
23+
# Pendulum
24+
solver = ALTROSolver(Problems.Pendulum()...)
25+
benchmark_solve!(solver)
26+
iterations(solver) # 19
27+
delete!(vis)
28+
set_mesh!(vis, get_model(solver))
29+
visualize!(vis, solver)
30+
31+
# Cartpole
32+
solver = ALTROSolver(Problems.Cartpole()...)
33+
benchmark_solve!(solver)
34+
iterations(solver) # 40
35+
delete!(vis)
36+
set_mesh!(vis, get_model(solver))
37+
visualize!(vis, solver)
38+
39+
# Acrobot
40+
solver = ALTROSolver(Problems.Acrobot()...)
41+
benchmark_solve!(solver)
42+
iterations(solver) # 50
43+
delete!(vis)
44+
set_mesh!(vis, get_model(solver))
45+
visualize!(vis, solver)
46+
47+
# Parallel Park
48+
solver = ALTROSolver(Problems.DubinsCar(:parallel_park)...)
49+
benchmark_solve!(solver)
50+
iterations(solver) # 13
51+
delete!(vis)
52+
set_mesh!(vis, get_model(solver))
53+
visualize!(vis, solver)
54+
55+
# Three Obstacles
56+
solver = ALTROSolver(Problems.DubinsCar(:three_obstacles)...)
57+
benchmark_solve!(solver)
58+
iterations(solver) # 20
59+
delete!(vis)
60+
add_cylinders!(vis, solver, robot_radius=get_model(solver).radius)
61+
set_mesh!(vis, get_model(solver))
62+
visualize!(vis, solver)
63+
64+
# Escape
65+
solver = ALTROSolver(Problems.DubinsCar(:escape)..., infeasible=true, R_inf=0.1)
66+
benchmark_solve!(solver, samples=1, evals=1)
67+
iterations(solver) # 13
68+
delete!(vis)
69+
set_mesh!(vis, get_model(solver))
70+
add_cylinders!(vis, solver, robot_radius=get_model(solver).model.radius, height=0.2)
71+
visualize!(vis, solver)
72+
A = rand(10,10)
73+
typeof(view(A,1:0,1:0))
74+
75+
# Zig-zag
76+
solver = ALTROSolver(Problems.Quadrotor(:zigzag)...)
77+
benchmark_solve!(solver)
78+
iterations(solver) # 15
79+
delete!(vis)
80+
set_mesh!(vis, get_model(solver))
81+
visualize!(vis, solver)
82+
83+
# Barrell Roll
84+
solver = ALTROSolver(Problems.YakProblems()...)
85+
benchmark_solve!(solver)
86+
iterations(solver) # 17
87+
max_violation(solver)
88+
delete!(vis)
89+
set_mesh!(vis, get_model(solver))
90+
visualize!(vis, solver)

‎examples/ddl/contingency_car.jl

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
2+
include("path.jl")
3+
include("model.jl")
4+
using TrajectoryOptimization
5+
using TrajOptPlots
6+
import TrajectoryOptimization: set_state!
7+
using Plots
8+
9+
function gen_car_prob(U0, path, N; s0=0.0, L=10.0, δ0=0.0, integration=RK3)
10+
11+
# Discretization
12+
sf = L + s0
13+
ds = sf/(N-1)
14+
15+
s_traj = range(0,sf,step=ds)
16+
@assert N == length(s_traj)
17+
# k_traj = k_itp.(s_traj)
18+
# d_path = Dict(s_traj.=>k_traj)
19+
20+
# Model
21+
car = BicycleCar(path=path)
22+
n,m = size(car)
23+
p_c = 0.1 # probability of contingency plan
24+
25+
x,u = zeros(car)
26+
car.μ = 0.9
27+
fiala(a) = fiala_tire_model(car.μ, car.Cαf, a, 0.0, 10000)
28+
plot(fiala.(range(-0.2,0.2, length=100)))
29+
30+
# Objective
31+
Ux_des = 15.
32+
x0 = @SVector [δ0, # δ
33+
0, # fx
34+
0, # r
35+
0, # Uy
36+
Ux_des, # Ux
37+
0, # dpsi
38+
0, # e
39+
0] # t
40+
41+
xd = @SVector [0, # δ
42+
0, # fx
43+
0, # r
44+
0, # Uy
45+
Ux_des, # Ux
46+
0, # dpsi
47+
0, # e
48+
0] # t
49+
50+
Qd = @SVector [0.001, # δ
51+
0.001, # fx
52+
0.00, # r
53+
0.00, # Uy
54+
1.0, # Ux
55+
100.0, # dpsi
56+
100.0, # e
57+
0.0] # t
58+
59+
60+
Qf = @SVector [0.000, # δ
61+
0.000, # fx
62+
0.000, # r
63+
0.000, # Uy
64+
10.0, # Ux
65+
100.0, # dpsi
66+
100.0, # e
67+
0.00] # t
68+
69+
Rd = @SVector [0.1, # δ_dot
70+
0.1] # fx_dot
71+
Rd_c = @SVector [0.3282, # δ_dot
72+
4e-8] # fx_dot
73+
74+
# Q = Diagonal([(1-p_c)*Qd; p_c*Qd])
75+
# R = Diagonal([Rd; Rd_c])
76+
77+
# Qd = @SVector fill(0.1,n)
78+
# Rd = @SVector fill(0.01,m)
79+
Q = Diagonal(Qd)
80+
R = Diagonal(Rd)
81+
82+
obj = LQRObjective(Q,R,Q,xd,N)
83+
84+
# Bound Constraints
85+
δ_dot_bound = deg2rad(90) # deg/s
86+
δ_bound = deg2rad(27) # deg
87+
Fx_max = car.μ*car.mass*car.g
88+
Ux_min = 1.0 # m/s
89+
e_bnd = 1.0 # m
90+
91+
x_min = fill(-Inf,n)
92+
x_min[1] = -δ_bound
93+
x_min[5] = Ux_min
94+
x_min[7] = -e_bnd
95+
96+
x_max = fill(Inf,n)
97+
x_max[1] = δ_bound
98+
x_max[2] = Fx_max
99+
x_max[7] = e_bnd
100+
101+
u_min = [-δ_dot_bound; -Inf]
102+
u_max = [ δ_dot_bound; Inf]
103+
104+
bnd = BoundConstraint(n,m, x_max=x_max, x_min=x_min, u_min=u_min, u_max=u_max)
105+
106+
# Brake constraint
107+
brake = BrakeForceConstraint(car)
108+
109+
# Constraint Set
110+
conSet = ConstraintSet(n,m,N)
111+
add_constraint!(conSet, bnd, 1:N-1)
112+
add_constraint!(conSet, brake, 1:N)
113+
114+
# Problem
115+
prob = Problem(car, obj, xd, sf, constraints=conSet, x0=x0, t0=s0,
116+
integration=integration)
117+
initial_controls!(prob, U0)
118+
119+
return prob
120+
end

‎examples/multi_satellite.jl

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
using TrajectoryOptimization
2+
using StaticArrays
3+
using LinearAlgebra
4+
using Statistics
5+
using TrajOptPlots
6+
using GeometryTypes
7+
using CoordinateTransformations
8+
using BenchmarkTools
9+
using ForwardDiff
10+
using MeshCat
11+
using Test
12+
using InplaceOps
13+
const TO = TrajectoryOptimization
14+
import TrajectoryOptimization.Controllers: RBState
15+
16+
function gen_multisat_prob(Rot=MRP{Float64})
17+
# Test with rigid bodies
18+
model = Dynamics.FreeBody{Rot,Float64}()
19+
models = Dynamics.CopyModel(model, 2)
20+
21+
# Test solve
22+
N = 101
23+
tf = 5.0
24+
25+
x01 = Dynamics.build_state(model, [0, 1,0], UnitQuaternion(I), zeros(3), zeros(3))
26+
x02 = Dynamics.build_state(model, [0,-1,0], UnitQuaternion(I), zeros(3), zeros(3))
27+
x0 = [x01; x02]
28+
29+
xf1 = Dynamics.build_state(model, [ 1,0,0], expm(deg2rad( 45)*@SVector [1,0,0.]), zeros(3), zeros(3))
30+
xf2 = Dynamics.build_state(model, [-1,0,0], expm(deg2rad(-45)*@SVector [1,0,0.]), zeros(3), zeros(3))
31+
xf = [xf1; xf2]
32+
33+
Qd = Dynamics.fill_state(model, 1e-1, 1e-1, 1e-2, 1e-2)
34+
Q = Diagonal([Qd; Qd])
35+
Rd = @SVector fill(1e-2, 6)
36+
R = Diagonal([Rd; Rd])
37+
obj = LQRObjective(Q,R,Q*10,xf,N)
38+
39+
prob = Problem(models, obj, xf, tf, x0=x0)
40+
end
41+
prob = gen_multisat_prob()
42+
solver = iLQRSolver2(prob)
43+
solver.opts.verbose = false
44+
model = solver.model
45+
x,u = rand(model)
46+
47+
@time rollout!(solver)
48+
@time cost(solver)
49+
@time TO.state_diff_jacobian!(solver.G, solver.model, solver.Z)
50+
@time TO.dynamics_expansion!(solver.D, solver.model, solver.Z)
51+
@time TO.cost_expansion!(solver.Q, solver.obj, solver.Z)
52+
@time TO.error_expansion!(solver.D, solver.model, solver.G)
53+
@time TO.error_expansion!(solver.Q, solver.model, solver.Z, solver.G)
54+
solver.Q[1].xx
55+
solver.G[1]
56+
@time ΔV = TO.static_backwardpass!(solver)
57+
@time TO.forwardpass!(solver, ΔV, cost(solver))
58+
59+
prob = gen_multisat_prob()
60+
solver = iLQRSolver2(prob)
61+
solver.opts.verbose = true
62+
solve!(solver)
63+
cost(solver)
64+
65+
66+
67+
if !isdefined(Main,:vis)
68+
vis = Visualizer(); open(vis);
69+
set_mesh!(vis, model.model)
70+
end
71+
visualize!(vis, model, get_trajectory(solver))
72+
settransform!(vis["robot"]["copy2"], Translation(-1,1,1))

0 commit comments

Comments
 (0)