Skip to content

Commit 314cc51

Browse files
ishaanamahajanishaanamahajan
authored
Merge pull request #10 from ishaanamahajan/main
Adding quadrotor examples and adapt functionality
2 parents 958472f + 1af5c3b commit 314cc51

File tree

417 files changed

+171516
-16
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

417 files changed

+171516
-16
lines changed

‎CMakeLists.txt‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ list(APPEND CMAKE_MESSAGE_INDENT " ")
1515
FetchContent_Declare(
1616
tinympc
1717
GIT_REPOSITORY https://github.com/TinyMPC/TinyMPC.git
18-
GIT_TAG 50bfaf3883621cec005c3e3b35ebabc22bb9ed55
18+
GIT_TAG main
1919
)
2020
list(POP_BACK CMAKE_MESSAGE_INDENT)
2121
FetchContent_MakeAvailable(tinympc)
@@ -25,6 +25,7 @@ FetchContent_MakeAvailable(tinympc)
2525
pybind11_add_module(ext_tinympc src/bindings.cpp)
2626
install(TARGETS ext_tinympc DESTINATION . COMPONENT python)
2727

28-
28+
install(TARGETS ext_tinympc DESTINATION . COMPONENT python)
2929
target_link_libraries(ext_tinympc PUBLIC pybind11::module tinympcstatic)
3030

31+

‎MANIFEST.in‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
recursive-include src/tinympc/codegen/codegen_src *
2+
recursive-include src/tinympc/codegen/pywrapper *

‎README.md‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,25 @@ Python wrapper for [TinyMPC](https://tinympc.org/).
66
pip install tinympc
77
```
88

9+
10+
For development installation:
11+
```bash
12+
git clone --recursive https://github.com/TinyMPC/tinympc-python.git
13+
cd tinympc-python
14+
pip install -e .
15+
```
16+
17+
## Examples
18+
19+
The `examples/` directory contains:
20+
- `cartpole_example_one_solve.py`
21+
- `cartpole_example_mpc_constrained.py`
22+
- `cartpole_example_mpc.py`
23+
- `cartpole_example_code_generation.py`
24+
- `quadrotor_hover_code_generation.py` - For online hyperparameter tuning of rho set ```ENABLE_ADAPTIVE_RHO``` to ```True```
25+
26+
27+
928
## Documentation
1029

1130
Documentation and examples can be found [here](https://tinympc.org/get-started/installation/).

‎examples/quadrotor_hover_code_generation.py‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
import tinympc
2+
import numpy as np
3+
from autograd import jacobian
4+
import autograd.numpy as anp
5+
6+
# Toggle switch for adaptive rho
7+
ENABLE_ADAPTIVE_RHO = False # Set to True to enable adaptive rho
8+
9+
# Quadrotor system matrices (12 states, 4 inputs)
10+
rho_value = 5.0
11+
Adyn = np.array([
12+
1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0245250, 0.0000000, 0.0500000, 0.0000000, 0.0000000, 0.0000000, 0.0002044, 0.0000000,
13+
0.0000000, 1.0000000, 0.0000000, -0.0245250, 0.0000000, 0.0000000, 0.0000000, 0.0500000, 0.0000000, -0.0002044, 0.0000000, 0.0000000,
14+
0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0500000, 0.0000000, 0.0000000, 0.0000000,
15+
0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0250000, 0.0000000, 0.0000000,
16+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0250000, 0.0000000,
17+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0250000,
18+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.9810000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0122625, 0.0000000,
19+
0.0000000, 0.0000000, 0.0000000, -0.9810000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, -0.0122625, 0.0000000, 0.0000000,
20+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000, 0.0000000,
21+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000, 0.0000000,
22+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000, 0.0000000,
23+
0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 0.0000000, 1.0000000
24+
]).reshape(12, 12)
25+
26+
# Input/control matrix
27+
Bdyn = np.array([
28+
-0.0007069, 0.0007773, 0.0007091, -0.0007795,
29+
0.0007034, 0.0007747, -0.0007042, -0.0007739,
30+
0.0052554, 0.0052554, 0.0052554, 0.0052554,
31+
-0.1720966, -0.1895213, 0.1722891, 0.1893288,
32+
-0.1729419, 0.1901740, 0.1734809, -0.1907131,
33+
0.0123423, -0.0045148, -0.0174024, 0.0095748,
34+
-0.0565520, 0.0621869, 0.0567283, -0.0623632,
35+
0.0562756, 0.0619735, -0.0563386, -0.0619105,
36+
0.2102143, 0.2102143, 0.2102143, 0.2102143,
37+
-13.7677303, -15.1617018, 13.7831318, 15.1463003,
38+
-13.8353509, 15.2139209, 13.8784751, -15.2570451,
39+
0.9873856, -0.3611820, -1.3921880, 0.7659845
40+
]).reshape(12, 4)
41+
42+
43+
Q_diag = np.array([100.0000000, 100.0000000, 100.0000000, 4.0000000, 4.0000000, 400.0000000,
44+
4.0000000, 4.0000000, 4.0000000, 2.0408163, 2.0408163, 4.0000000])
45+
R_diag = np.array([4.0000000, 4.0000000, 4.0000000, 4.0000000])
46+
Q = np.diag(Q_diag)
47+
R = np.diag(R_diag)
48+
49+
N = 20
50+
51+
prob = tinympc.TinyMPC()
52+
53+
u_min = -np.ones(4) * 2.0
54+
u_max = np.ones(4) * 2.0
55+
56+
# Setup with adaptive rho based on toggle
57+
prob.setup(Adyn, Bdyn, Q, R, N, rho=rho_value, max_iter=100, u_min=u_min, u_max=u_max,
58+
adaptive_rho=1 if ENABLE_ADAPTIVE_RHO else 0)
59+
60+
if ENABLE_ADAPTIVE_RHO:
61+
print("Enabled adaptive rho - generating code with sensitivity matrices...")
62+
63+
# First compute the cache terms (this will compute K, P, etc.)
64+
Kinf, Pinf, Quu_inv, AmBKt = prob.compute_cache_terms()
65+
66+
# Compute derivatives with respect to rho via Autograd's Jacobian
67+
def lqr_direct(rho):
68+
R_rho = anp.array(R) + rho * anp.eye(4)
69+
Q_rho = anp.array(Q) + rho * anp.eye(12)
70+
P = Q_rho
71+
for _ in range(50):
72+
K = anp.linalg.solve(
73+
R_rho + Bdyn.T @ P @ Bdyn + 1e-4*anp.eye(4),
74+
Bdyn.T @ P @ Adyn
75+
)
76+
P = Q_rho + Adyn.T @ P @ (Adyn - Bdyn @ K)
77+
# Final gain and cache matrices
78+
K = anp.linalg.solve(
79+
R_rho + Bdyn.T @ P @ Bdyn + 1e-4*anp.eye(4),
80+
Bdyn.T @ P @ Adyn
81+
)
82+
C1 = anp.linalg.inv(R_rho + Bdyn.T @ P @ Bdyn)
83+
C2 = (Adyn - Bdyn @ K).T
84+
return anp.concatenate([K.flatten(), P.flatten(), C1.flatten(), C2.flatten()])
85+
86+
derivs = jacobian(lqr_direct)(rho_value)
87+
# Dynamically split the derivative vector based on matrix sizes
88+
deriv_array = np.array(derivs)
89+
nu, nx = Bdyn.shape[1], Adyn.shape[0]
90+
idx = 0
91+
dK = deriv_array[idx:idx + nu * nx].reshape(nu, nx); idx += nu * nx
92+
dP = deriv_array[idx:idx + nx * nx].reshape(nx, nx); idx += nx * nx
93+
dC1 = deriv_array[idx:idx + nu * nu].reshape(nu, nu); idx += nu * nu
94+
dC2 = deriv_array[idx:idx + nx * nx].reshape(nx, nx)
95+
96+
# Generate code with sensitivity matrices
97+
prob.codegen_with_sensitivity("out", dK, dP, dC1, dC2, verbose=1)
98+
else:
99+
print("Running without adaptive rho - generating code without sensitivity matrices...")
100+
prob.codegen("out", verbose=1)

‎pyproject.toml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,11 @@ build-backend = "scikit_build_core.build"
44

55
[project]
66
name = "tinympc"
7-
version = "0.0.3"
7+
version = "0.0.4"
88
authors = [
99
{ name="Sam Schoedel", email="seschoedel@gmail.com" },
1010
{ name="Khai Nguyen", email="khai.nx1201@gmail.com" },
11+
{ name="Ishaan Mahajan", email="ishaanamahajan@gmail.com" },
1112
]
1213
description = "Python wrapper for TinyMPC"
1314
readme = "README.md"

‎src/bindings.cpp‎

Lines changed: 133 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,34 @@ class PyTinySolver {
2020
void set_x0(Eigen::Ref<tinyVector>);
2121
void set_x_ref(Eigen::Ref<tinyMatrix>);
2222
void set_u_ref(Eigen::Ref<tinyMatrix>);
23+
void set_sensitivity_matrices(
24+
Eigen::Ref<tinyMatrix> dK,
25+
Eigen::Ref<tinyMatrix> dP,
26+
Eigen::Ref<tinyMatrix> dC1,
27+
Eigen::Ref<tinyMatrix> dC2,
28+
float rho = 0.0,
29+
int verbose = 0);
30+
void set_cache_terms(
31+
Eigen::Ref<tinyMatrix> Kinf,
32+
Eigen::Ref<tinyMatrix> Pinf,
33+
Eigen::Ref<tinyMatrix> Quu_inv,
34+
Eigen::Ref<tinyMatrix> AmBKt,
35+
int verbose = 0);
36+
void update_settings(TinySettings* settings);
2337

2438
int solve();
2539
TinySolution* get_solution();
2640

2741
void print_problem_data();
2842

2943
int codegen(const char*, int); // output_dir, verbosity
44+
45+
int codegen_with_sensitivity(const char *output_dir,
46+
Eigen::Ref<tinyMatrix> dK,
47+
Eigen::Ref<tinyMatrix> dP,
48+
Eigen::Ref<tinyMatrix> dC1,
49+
Eigen::Ref<tinyMatrix> dC2,
50+
int verbose);
3051
private:
3152
TinySolver *_solver;
3253
};
@@ -67,6 +88,37 @@ void PyTinySolver::set_u_ref(Eigen::Ref<tinyMatrix> u_ref) {
6788
tiny_set_u_ref(this->_solver, u_ref);
6889
}
6990

91+
void PyTinySolver::set_sensitivity_matrices(
92+
Eigen::Ref<tinyMatrix> dK,
93+
Eigen::Ref<tinyMatrix> dP,
94+
Eigen::Ref<tinyMatrix> dC1,
95+
Eigen::Ref<tinyMatrix> dC2,
96+
float rho,
97+
int verbose) {
98+
// Create copies of the matrices to ensure they remain valid
99+
tinyMatrix dK_copy = dK;
100+
tinyMatrix dP_copy = dP;
101+
tinyMatrix dC1_copy = dC1;
102+
tinyMatrix dC2_copy = dC2;
103+
104+
// Store the sensitivity matrices in the solver's cache
105+
if (this->_solver->cache != nullptr) {
106+
107+
// For now, we'll just store them for code generation
108+
if (verbose) {
109+
std::cout << "Sensitivity matrices set for code generation" << std::endl;
110+
std::cout << "dK norm: " << dK_copy.norm() << std::endl;
111+
std::cout << "dP norm: " << dP_copy.norm() << std::endl;
112+
std::cout << "dC1 norm: " << dC1_copy.norm() << std::endl;
113+
std::cout << "dC2 norm: " << dC2_copy.norm() << std::endl;
114+
}
115+
} else {
116+
if (verbose) {
117+
std::cout << "Warning: Cache not initialized, sensitivity matrices will only be used for code generation" << std::endl;
118+
}
119+
}
120+
}
121+
70122
int PyTinySolver::solve() {
71123
py::gil_scoped_release release;
72124
int status = tiny_solve(this->_solver);
@@ -134,6 +186,69 @@ int PyTinySolver::codegen(const char *output_dir, int verbose) {
134186
return tiny_codegen(this->_solver, output_dir, verbose);
135187
}
136188

189+
int PyTinySolver::codegen_with_sensitivity(const char *output_dir,
190+
Eigen::Ref<tinyMatrix> dK_ref,
191+
Eigen::Ref<tinyMatrix> dP_ref,
192+
Eigen::Ref<tinyMatrix> dC1_ref,
193+
Eigen::Ref<tinyMatrix> dC2_ref,
194+
int verbose) {
195+
tinyMatrix dK_copy = dK_ref;
196+
tinyMatrix dP_copy = dP_ref;
197+
tinyMatrix dC1_copy = dC1_ref;
198+
tinyMatrix dC2_copy = dC2_ref;
199+
200+
return tiny_codegen_with_sensitivity(this->_solver, output_dir,
201+
&dK_copy, &dP_copy, &dC1_copy, &dC2_copy, verbose);
202+
}
203+
204+
void PyTinySolver::set_cache_terms(
205+
Eigen::Ref<tinyMatrix> Kinf,
206+
Eigen::Ref<tinyMatrix> Pinf,
207+
Eigen::Ref<tinyMatrix> Quu_inv,
208+
Eigen::Ref<tinyMatrix> AmBKt,
209+
int verbose) {
210+
if (!this->_solver) {
211+
throw py::value_error("Solver not initialized");
212+
}
213+
if (!this->_solver->cache) {
214+
throw py::value_error("Solver cache not initialized");
215+
}
216+
217+
// Create copies of the matrices to ensure they remain valid
218+
this->_solver->cache->Kinf = Kinf;
219+
this->_solver->cache->Pinf = Pinf;
220+
this->_solver->cache->Quu_inv = Quu_inv;
221+
this->_solver->cache->AmBKt = AmBKt;
222+
this->_solver->cache->C1 = Quu_inv; // Cache terms
223+
this->_solver->cache->C2 = AmBKt; // Cache terms
224+
225+
if (verbose) {
226+
std::cout << "Cache terms set with norms:" << std::endl;
227+
std::cout << "Kinf norm: " << Kinf.norm() << std::endl;
228+
std::cout << "Pinf norm: " << Pinf.norm() << std::endl;
229+
std::cout << "Quu_inv norm: " << Quu_inv.norm() << std::endl;
230+
std::cout << "AmBKt norm: " << AmBKt.norm() << std::endl;
231+
}
232+
}
233+
234+
void PyTinySolver::update_settings(TinySettings* settings) {
235+
if (this->_solver && this->_solver->settings && settings) {
236+
// Copy settings to the solver's settings
237+
this->_solver->settings->abs_pri_tol = settings->abs_pri_tol;
238+
this->_solver->settings->abs_dua_tol = settings->abs_dua_tol;
239+
this->_solver->settings->max_iter = settings->max_iter;
240+
this->_solver->settings->check_termination = settings->check_termination;
241+
this->_solver->settings->en_state_bound = settings->en_state_bound;
242+
this->_solver->settings->en_input_bound = settings->en_input_bound;
243+
244+
// Copy adaptive rho settings
245+
this->_solver->settings->adaptive_rho = settings->adaptive_rho;
246+
this->_solver->settings->adaptive_rho_min = settings->adaptive_rho_min;
247+
this->_solver->settings->adaptive_rho_max = settings->adaptive_rho_max;
248+
this->_solver->settings->adaptive_rho_enable_clipping = settings->adaptive_rho_enable_clipping;
249+
}
250+
}
251+
137252
PYBIND11_MODULE(ext_tinympc, m) {
138253
// // PYBIND11_MODULE(@TINYMPC_EXT_MODULE_NAME@, m) {
139254

@@ -168,7 +283,11 @@ PYBIND11_MODULE(ext_tinympc, m) {
168283
.def_readwrite("max_iter", &TinySettings::max_iter)
169284
.def_readwrite("check_termination", &TinySettings::check_termination)
170285
.def_readwrite("en_state_bound", &TinySettings::en_state_bound)
171-
.def_readwrite("en_input_bound", &TinySettings::en_input_bound);
286+
.def_readwrite("en_input_bound", &TinySettings::en_input_bound)
287+
.def_readwrite("adaptive_rho", &TinySettings::adaptive_rho)
288+
.def_readwrite("adaptive_rho_min", &TinySettings::adaptive_rho_min)
289+
.def_readwrite("adaptive_rho_max", &TinySettings::adaptive_rho_max)
290+
.def_readwrite("adaptive_rho_enable_clipping", &TinySettings::adaptive_rho_enable_clipping);
172291

173292
m.def("tiny_set_default_settings", &tiny_set_default_settings);
174293

@@ -180,8 +299,19 @@ PYBIND11_MODULE(ext_tinympc, m) {
180299
.def("set_x0", &PyTinySolver::set_x0)
181300
.def("set_x_ref", &PyTinySolver::set_x_ref)
182301
.def("set_u_ref", &PyTinySolver::set_u_ref)
302+
.def("update_settings", &PyTinySolver::update_settings)
303+
.def("set_sensitivity_matrices", &PyTinySolver::set_sensitivity_matrices,
304+
"dK"_a.noconvert(), "dP"_a.noconvert(),
305+
"dC1"_a.noconvert(), "dC2"_a.noconvert(),
306+
"rho"_a=0.0, "verbose"_a=0)
307+
.def("set_cache_terms", &PyTinySolver::set_cache_terms,
308+
py::arg("Kinf"), py::arg("Pinf"), py::arg("Quu_inv"), py::arg("AmBKt"),
309+
py::arg("verbose") = 0)
183310
.def("solve", &PyTinySolver::solve)
184311
.def("print_problem_data", &PyTinySolver::print_problem_data)
185-
.def("codegen", &PyTinySolver::codegen);
312+
.def("codegen", &PyTinySolver::codegen, "output_dir"_a, "verbose"_a=0)
313+
.def("codegen_with_sensitivity", &PyTinySolver::codegen_with_sensitivity,
314+
"output_dir"_a, "dK"_a.noconvert(), "dP"_a.noconvert(),
315+
"dC1"_a.noconvert(), "dC2"_a.noconvert(), "verbose"_a);
186316

187-
}
317+
}

0 commit comments

Comments
 (0)