Skip to content

Commit 92a2e60

Browse files
thowellthowell
committed
more examples run_ci
1 parent 3b941f8 commit 92a2e60

39 files changed

+1723
-745
lines changed

‎deps/build.jl

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,3 +4,5 @@ exampledir = joinpath(@__DIR__, "..", "examples")
44
Pkg.activate(exampledir)
55
Pkg.add("Literate")
66
include(joinpath(exampledir, "generate_notebooks.jl"))
7+
8+

‎examples/README.md

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,17 @@ You can also generate Jupyter notebooks and run them locally by performing the f
1414
using Pkg
1515
Pkg.build("ContactImplicitMPC")
1616
using IJulia, ContactImplicitMPC
17+
```
18+
4. generate dynamics (do once after installation)
19+
```
20+
include(joinpath(dirname(pathof(ContactImplicitMPC)), "..", "src/dynamics/generate_dynamics.jl"))
21+
```
22+
5. generate simulations (do once after installation)
23+
```
24+
include(joinpath(dirname(pathof(ContactImplicitMPC)), "..", "src/simulation/generate_simulation.jl"))
25+
```
26+
6. interact with notebooks
27+
```
1728
notebook(dir=joinpath(dirname(pathof(ContactImplicitMPC)), "..", "examples"))
1829
```
1930

20-

‎examples/flamingo/flat.jl

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,6 @@ open(vis)
7575

7676
# ## Visualize
7777
anim = visualize_meshrobot!(vis, model, sim.traj, sample=10);
78-
anim = visualize_force!(vis, model, env, sim.traj, anim=anim, h=h_sim, sample=10);
7978

8079
# ## Timing result
8180
# Julia is [JIT-ed](https://en.wikipedia.org/wiki/Just-in-time_compilation) so re-run the MPC setup through Simulate for correct timing results.

‎examples/flamingo/piecewise.jl

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
# PREAMBLE
2+
3+
# PKG_SETUP
4+
5+
# ## Setup
6+
7+
using ContactImplicitMPC
8+
using LinearAlgebra
9+
using StaticArrays
10+
11+
# ## Simulation
12+
s_sim = get_simulation("flamingo", "piecewise1_2D_lc", "piecewise", approx = true)
13+
s = get_simulation("flamingo", "flat_2D_lc", "flat");
14+
model = s.model
15+
env = s.env
16+
17+
# ## Reference Trajectory
18+
ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
19+
joinpath(module_dir(), "src/dynamics/flamingo/gaits/gait_forward_36_4.jld2"),
20+
load_type = :split_traj_alt));
21+
H = ref_traj.H
22+
h = ref_traj.h
23+
24+
# ## MPC setup
25+
N_sample = 5
26+
H_mpc = 15
27+
h_sim = h / N_sample
28+
H_sim = 2500 #15000
29+
κ_mpc = 1.0e-4
30+
31+
obj = TrackingVelocityObjective(model, env, H_mpc,
32+
v = [Diagonal(1e-3 * [1e0,1,1e4,1,1,1,1,1e4,1e4]) for t = 1:H_mpc],
33+
q = [Diagonal(1e-1 * [3e2, 1e-6, 3e2, 1, 1, 1, 1, 0.1, 0.1]) for t = 1:H_mpc],
34+
u = [Diagonal(3e-1 * [0.1; 0.1; 0.3; 0.3; ones(model.dim.u-6); 2; 2]) for t = 1:H_mpc],
35+
γ = [Diagonal(1.0e-100 * ones(model.dim.c)) for t = 1:H_mpc],
36+
b = [Diagonal(1.0e-100 * ones(model.dim.c * friction_dim(env))) for t = 1:H_mpc]);
37+
38+
p = linearized_mpc_policy(ref_traj, s, obj,
39+
H_mpc = H_mpc,
40+
N_sample = N_sample,
41+
κ_mpc = κ_mpc,
42+
n_opts = NewtonOptions(
43+
r_tol = 3e-4,
44+
max_iter = 5),
45+
mpc_opts = LinearizedMPCOptions(
46+
altitude_update = true,
47+
altitude_impact_threshold = 0.02,
48+
altitude_verbose = true,
49+
)
50+
);
51+
52+
# ## Initial conditions
53+
q1_sim = SVector{model.dim.q}(copy(ref_traj.q[2]))
54+
q0_sim = SVector{model.dim.q}(copy(q1_sim - (copy(ref_traj.q[2]) - copy(ref_traj.q[1])) / N_sample))
55+
56+
# ## Simulator
57+
sim = simulator(s_sim, q0_sim, q1_sim, h_sim, H_sim,
58+
p = p,
59+
ip_opts = InteriorPointOptions(
60+
undercut = Inf,
61+
γ_reg = 0.0,
62+
r_tol = 1.0e-8,
63+
κ_tol = 1.0e-8),
64+
sim_opts = SimulatorOptions(warmstart = true),
65+
);
66+
67+
# ## Simulate
68+
@time status = simulate!(sim);
69+
70+
# ## Visualizer
71+
vis = ContactImplicitMPC.Visualizer()
72+
open(vis)
73+
74+
# ## Visualize
75+
plot_surface!(vis, s_sim.env)
76+
anim = visualize_meshrobot!(vis, model, sim.traj, sample=10)
77+
78+
# ## Timing result
79+
# Julia is [JIT-ed](https://en.wikipedia.org/wiki/Just-in-time_compilation) so re-run the MPC setup through Simulate for correct timing results.
80+
process!(sim) # Time budget
81+
H_sim * h_sim / sum(sim.stats.dt) # Speed ratio

‎examples/flamingo/sine.jl

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
# PREAMBLE
2+
3+
# PKG_SETUP
4+
5+
# ## Setup
6+
7+
using ContactImplicitMPC
8+
using LinearAlgebra
9+
using StaticArrays
10+
11+
# ## Simulation
12+
s_sim = get_simulation("flamingo", "sine3_2D_lc", "sinusoidal");
13+
s = get_simulation("flamingo", "flat_2D_lc", "flat");
14+
model = s.model
15+
env = s.env
16+
17+
# ## Reference Trajectory
18+
ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
19+
joinpath(module_dir(), "src/dynamics/flamingo/gaits/gait_forward_36_4.jld2"),
20+
load_type = :split_traj_alt));
21+
22+
H = ref_traj.H
23+
h = ref_traj.h
24+
25+
# ## MPC setup
26+
N_sample = 5
27+
H_mpc = 15
28+
h_sim = h / N_sample
29+
H_sim = 1000 #35000
30+
κ_mpc = 2.0e-4
31+
32+
obj = TrackingVelocityObjective(model, env, H_mpc,
33+
v = [Diagonal(1e-3 * [1e0,1,1e4,1,1,1,1,1e4,1e4]) for t = 1:H_mpc],
34+
q = [Diagonal(1e-1 * [3e2, 1e-6, 3e2, 1, 1, 1, 1, 0.1, 0.1]) for t = 1:H_mpc],
35+
u = [Diagonal(3e-1 * [0.1; 0.1; 0.3; 0.3; ones(model.dim.u-6); 2; 2]) for t = 1:H_mpc],
36+
γ = [Diagonal(1.0e-100 * ones(model.dim.c)) for t = 1:H_mpc],
37+
b = [Diagonal(1.0e-100 * ones(model.dim.c * friction_dim(env))) for t = 1:H_mpc]);
38+
39+
p = linearized_mpc_policy(ref_traj, s, obj,
40+
H_mpc = H_mpc,
41+
N_sample = N_sample,
42+
κ_mpc = κ_mpc,
43+
mode = :configuration,
44+
ip_opts = InteriorPointOptions(
45+
undercut = 5.0,
46+
κ_tol = κ_mpc,
47+
r_tol = 1.0e-8,
48+
diff_sol = true,
49+
solver = :empty_solver,
50+
max_time = 1e5,),
51+
n_opts = NewtonOptions(
52+
r_tol = 3e-4,
53+
max_iter = 5),
54+
mpc_opts = LinearizedMPCOptions(
55+
altitude_update = true,
56+
altitude_impact_threshold = 0.02,
57+
)
58+
);
59+
60+
# ## Initial conditions
61+
q1_sim = SVector{model.dim.q}(copy(ref_traj.q[2]))
62+
q0_sim = SVector{model.dim.q}(copy(q1_sim - (copy(ref_traj.q[2]) - copy(ref_traj.q[1])) / N_sample))
63+
64+
# ## Simulator
65+
sim = simulator(s_sim, q0_sim, q1_sim, h_sim, H_sim,
66+
p = p,
67+
ip_opts = InteriorPointOptions(
68+
undercut = Inf,
69+
γ_reg = 0.0,
70+
r_tol = 1.0e-8,
71+
κ_tol = 1.0e-8),
72+
sim_opts = SimulatorOptions(warmstart = true),
73+
)
74+
75+
# ## Simulate
76+
@time status = simulate!(sim, verbose = true)
77+
78+
# ## Visualizer
79+
vis = ContactImplicitMPC.Visualizer()
80+
open(vis)
81+
82+
# ## Visualize
83+
plot_surface!(vis, s_sim.env)
84+
anim = visualize_meshrobot!(vis, s_sim.model, sim.traj, sample=10)
85+
anim = visualize_force!(vis, s_sim.model, s_sim.env, sim.traj, anim=anim, h=h_sim, sample=10)
86+
87+
# ## Timing result
88+
# Julia is [JIT-ed](https://en.wikipedia.org/wiki/Just-in-time_compilation) so re-run the MPC setup through Simulate for correct timing results.
89+
process!(sim) # Time budget
90+
H_sim * h_sim / sum(sim.stats.dt) # Speed ratio

‎examples/flamingo/slope.jl

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
# PREAMBLE
2+
3+
# PKG_SETUP
4+
5+
# ## Setup
6+
7+
using ContactImplicitMPC
8+
using LinearAlgebra
9+
using StaticArrays
10+
11+
# ## Simulation
12+
s_sim = get_simulation("flamingo", "slope_smooth_2D_lc", "slope");
13+
s = get_simulation("flamingo", "flat_2D_lc", "flat");
14+
model = s.model
15+
env = s.env
16+
17+
# ## Reference Trajectory
18+
ref_traj = deepcopy(ContactImplicitMPC.get_trajectory(s.model, s.env,
19+
joinpath(module_dir(), "src/dynamics/flamingo/gaits/gait_forward_36_4.jld2"),
20+
load_type = :split_traj_alt));
21+
22+
H = ref_traj.H
23+
h = ref_traj.h
24+
25+
# ## MPC setup
26+
N_sample = 5
27+
H_mpc = 15
28+
h_sim = h / N_sample
29+
H_sim = 1000 #35000
30+
κ_mpc = 2.0e-4
31+
32+
obj = TrackingVelocityObjective(model, env, H_mpc,
33+
v = [Diagonal(1e-3 * [1e0,1,1e4,1,1,1,1,1e4,1e4]) for t = 1:H_mpc],
34+
q = [Diagonal(1e-1 * [3e2, 1e-6, 3e2, 1, 1, 1, 1, 0.1, 0.1]) for t = 1:H_mpc],
35+
u = [Diagonal(3e-1 * [0.1; 0.1; 0.3; 0.3; ones(model.dim.u-6); 2; 2]) for t = 1:H_mpc],
36+
γ = [Diagonal(1.0e-100 * ones(model.dim.c)) for t = 1:H_mpc],
37+
b = [Diagonal(1.0e-100 * ones(model.dim.c * friction_dim(env))) for t = 1:H_mpc]);
38+
39+
p = linearized_mpc_policy(ref_traj, s, obj,
40+
H_mpc = H_mpc,
41+
N_sample = N_sample,
42+
κ_mpc = κ_mpc,
43+
mode = :configuration,
44+
ip_opts = InteriorPointOptions(
45+
undercut = 5.0,
46+
κ_tol = κ_mpc,
47+
r_tol = 1.0e-8,
48+
diff_sol = true,
49+
solver = :empty_solver,
50+
max_time = 1e5,),
51+
n_opts = NewtonOptions(
52+
r_tol = 3e-4,
53+
max_iter = 5),
54+
mpc_opts = LinearizedMPCOptions(
55+
altitude_update = true,
56+
altitude_impact_threshold = 0.02,
57+
)
58+
);
59+
60+
# ## Initial conditions
61+
q1_sim = SVector{model.dim.q}(copy(ref_traj.q[2]))
62+
q0_sim = SVector{model.dim.q}(copy(q1_sim - (copy(ref_traj.q[2]) - copy(ref_traj.q[1])) / N_sample))
63+
64+
# ## Simulator
65+
sim = simulator(s_sim, q0_sim, q1_sim, h_sim, H_sim,
66+
p = p,
67+
ip_opts = InteriorPointOptions(
68+
undercut = Inf,
69+
γ_reg = 0.0,
70+
r_tol = 1.0e-8,
71+
κ_tol = 1.0e-8),
72+
sim_opts = SimulatorOptions(warmstart = true),
73+
);
74+
75+
# ## Simulate
76+
@time status = simulate!(sim, verbose = true)
77+
78+
# ## Visualizer
79+
vis = ContactImplicitMPC.Visualizer()
80+
open(vis)
81+
82+
# ## Visualize
83+
plot_surface!(vis, s_sim.env)
84+
anim = visualize_meshrobot!(vis, s_sim.model, sim.traj, sample=10)
85+
86+
# ## Timing result
87+
# Julia is [JIT-ed](https://en.wikipedia.org/wiki/Just-in-time_compilation) so re-run the MPC setup through Simulate for correct timing results.
88+
process!(sim) # Time budget
89+
H_sim * h_sim / sum(sim.stats.dt) # Speed ratio

0 commit comments

Comments
 (0)