Skip to content

Commit 24d5239

Browse files
ishaanamahajanishaanamahajan
authored
Merge pull request #11 from ishaanamahajan/main
fixing bindings to match current C++ API
2 parents 32d3861 + c374907 commit 24d5239

File tree

2 files changed

+36
-11
lines changed

2 files changed

+36
-11
lines changed

‎src/bindings.cpp‎

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -11,21 +11,23 @@ using namespace pybind11::literals;
1111

1212
class PyTinySolver {
1313
public:
14-
PyTinySolver(Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, // A, B
14+
PyTinySolver(Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, // A, B, fdyn
1515
Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, // Q, R
16-
float, int, int, int, // rho, nx, nu, N
16+
double, int, int, int, // rho, nx, nu, N
1717
Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, // x_min, x_max
1818
Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, // u_min, u_max
1919
TinySettings*, int); // settings, verbosity
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_bound_constraints(Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>,
24+
Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>);
2325
void set_sensitivity_matrices(
2426
Eigen::Ref<tinyMatrix> dK,
2527
Eigen::Ref<tinyMatrix> dP,
2628
Eigen::Ref<tinyMatrix> dC1,
2729
Eigen::Ref<tinyMatrix> dC2,
28-
float rho = 0.0,
30+
double rho = 0.0, // CHANGED: float -> double
2931
int verbose = 0);
3032
void set_cache_terms(
3133
Eigen::Ref<tinyMatrix> Kinf,
@@ -55,9 +57,10 @@ class PyTinySolver {
5557
PyTinySolver::PyTinySolver(
5658
Eigen::Ref<tinyMatrix> A,
5759
Eigen::Ref<tinyMatrix> B,
60+
Eigen::Ref<tinyMatrix> fdyn,
5861
Eigen::Ref<tinyMatrix> Q,
5962
Eigen::Ref<tinyMatrix> R,
60-
float rho,
63+
double rho,
6164
int nx,
6265
int nu,
6366
int N,
@@ -68,7 +71,14 @@ PyTinySolver::PyTinySolver(
6871
TinySettings *settings,
6972
int verbose) {
7073

71-
int status = tiny_setup(&this->_solver, A, B, Q, R, rho, nx, nu, N, x_min, x_max, u_min, u_max, verbose);
74+
75+
int status = tiny_setup(&this->_solver, A, B, fdyn, Q, R, rho, nx, nu, N, verbose);
76+
77+
78+
if (status == 0) {
79+
status = tiny_set_bound_constraints(this->_solver, x_min, x_max, u_min, u_max);
80+
}
81+
7282
this->_solver->settings = settings;
7383
if (status) {
7484
std::string message = "Error during setup";
@@ -88,12 +98,20 @@ void PyTinySolver::set_u_ref(Eigen::Ref<tinyMatrix> u_ref) {
8898
tiny_set_u_ref(this->_solver, u_ref);
8999
}
90100

101+
void PyTinySolver::set_bound_constraints(Eigen::Ref<tinyMatrix> x_min, Eigen::Ref<tinyMatrix> x_max,
102+
Eigen::Ref<tinyMatrix> u_min, Eigen::Ref<tinyMatrix> u_max) {
103+
int status = tiny_set_bound_constraints(this->_solver, x_min, x_max, u_min, u_max);
104+
if (status) {
105+
throw py::value_error("Error setting bound constraints");
106+
}
107+
}
108+
91109
void PyTinySolver::set_sensitivity_matrices(
92110
Eigen::Ref<tinyMatrix> dK,
93111
Eigen::Ref<tinyMatrix> dP,
94112
Eigen::Ref<tinyMatrix> dC1,
95113
Eigen::Ref<tinyMatrix> dC2,
96-
float rho,
114+
double rho,
97115
int verbose) {
98116
// Create copies of the matrices to ensure they remain valid
99117
tinyMatrix dK_copy = dK;
@@ -293,12 +311,13 @@ PYBIND11_MODULE(ext_tinympc, m) {
293311

294312
// Solver
295313
py::class_<PyTinySolver>(m, "TinySolver", py::module_local())
296-
.def(py::init<Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, float, int, int, int, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, TinySettings*, int>(),
297-
"A"_a.noconvert(), "B"_a.noconvert(), "Q"_a.noconvert(), "R"_a.noconvert(), "rho"_a, "nx"_a, "nu"_a, "N"_a, "x_min"_a.noconvert(), "x_max"_a.noconvert(), "u_min"_a.noconvert(), "u_max"_a.noconvert(), "settings"_a, "verbose"_a)
314+
.def(py::init<Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, double, int, int, int, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, Eigen::Ref<tinyMatrix>, TinySettings*, int>(),
315+
"A"_a.noconvert(), "B"_a.noconvert(), "fdyn"_a.noconvert(), "Q"_a.noconvert(), "R"_a.noconvert(), "rho"_a, "nx"_a, "nu"_a, "N"_a, "x_min"_a.noconvert(), "x_max"_a.noconvert(), "u_min"_a.noconvert(), "u_max"_a.noconvert(), "settings"_a, "verbose"_a)
298316
.def_property_readonly("solution", &PyTinySolver::get_solution)
299317
.def("set_x0", &PyTinySolver::set_x0)
300318
.def("set_x_ref", &PyTinySolver::set_x_ref)
301319
.def("set_u_ref", &PyTinySolver::set_u_ref)
320+
.def("set_bound_constraints", &PyTinySolver::set_bound_constraints)
302321
.def("update_settings", &PyTinySolver::update_settings)
303322
.def("set_sensitivity_matrices", &PyTinySolver::set_sensitivity_matrices,
304323
"dK"_a.noconvert(), "dP"_a.noconvert(),

‎src/tinympc/interface.py‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -73,15 +73,16 @@ def expand_ndarray(self, array_, expected_rows, expected_cols, fallback):
7373
return array_
7474

7575
# Setup the problem data and solver options
76-
def setup(self, A, B, Q, R, N, rho=1.0,
76+
def setup(self, A, B, Q, R, N, rho=1.0, fdyn=None,
7777
x_min=None, x_max=None, u_min=None, u_max=None, verbose=False, **settings):
7878
"""Instantiate necessary algorithm variables and parameters
7979
8080
:param A (np.ndarray): State transition matrix of the linear system, size nx x nx
8181
:param B (np.ndarray): Input matrix of the linear system, size nx x nu
8282
:param Q (np.ndarray): Stage cost for state, must be diagonal and positive semi-definite, size nx x nx
8383
:param R (np.ndarray): Stage cost for input, must be diagonal and positive definite, size nu x nu
84-
:param rho (int, optional): Penalty term used in ADMM, default 1
84+
:param rho (float, optional): Penalty term used in ADMM, default 1.0
85+
:param fdyn (np.ndarray or None, optional): Affine offset vector for dynamics, size nx x 1. If None, defaults to zeros (linear system), default None
8586
:param x_min (list[float] or None, optional): Lower bound state constraints of the same length as nx, default None
8687
:param x_max (list[float] or None, optional): Upper bound state constraints of the same length as nx, default None
8788
:param u_min (list[float] or None, optional): Lower bound input constraints of the same length as nu, default None
@@ -110,6 +111,11 @@ def setup(self, A, B, Q, R, N, rho=1.0,
110111
self.nx = A.shape[0]
111112
self.nu = B.shape[1]
112113

114+
# Handle fdyn parameter - default to zeros for linear systems
115+
if fdyn is None:
116+
fdyn = np.zeros((self.nx, 1))
117+
self.fdyn = np.array(fdyn, order="F")
118+
113119
assert N > 1
114120
self.N = N
115121

@@ -149,7 +155,7 @@ def setup(self, A, B, Q, R, N, rho=1.0,
149155
if 'adaptive_rho_enable_clipping' in settings:
150156
self.settings.adaptive_rho_enable_clipping = 1 if settings.pop('adaptive_rho_enable_clipping') else 0
151157

152-
self._solver = self.ext.TinySolver(self.A, self.B, self.Q, self.R, self.rho,
158+
self._solver = self.ext.TinySolver(self.A, self.B, self.fdyn, self.Q, self.R, self.rho,
153159
self.nx, self.nu, self.N,
154160
self.x_min, self.x_max, self.u_min, self.u_max,
155161
self.settings, self.verbose

0 commit comments

Comments
 (0)