-Optimization Engine (OpEn) is a solver for Fast & Accurate Embedded Optimization for next-generation Robotics and Autonomous Systems.
+
+Optimization Engine (OpEn) is a solver for Fast & Accurate Embedded Optimization for next-generation Robotics and Autonomous Systems. Read on to see how you can use OpEn on your robot today.
**Documentation available at** [**alphaville.github.io/optimization-engine**](https://alphaville.github.io/optimization-engine/)
-## Table of contents
-
-- [Features](#features)
-- [Demos](#demos)
- - [Code generation](#code-generation)
- - [Embedded applications](#embedded-applications)
-- [Parametric optimization problems](#parametric-problems)
-- [Code generation example](#code-generation-example)
-- [Getting started](#getting-started)
-- [Contact](#contact-us)
-- [Show us your love](#do-you-like-open)
-- [Licence](#license)
-- [Core team](#core-team)
-- [Contributions](#contributions)
-- [Cite OpEn](#citing-open)
## Features
@@ -67,11 +42,9 @@ Optimization Engine (OpEn) is a solver for Fast & Accurate Embedded Optimization
**OpEn** is ideal for:
-- Embedded **Nonlinear Model Predictive Control**,
-- Embedded **Nonlinear Moving Horizon Estimation** and their applications in
-- Robotics and Advanced Manufacturing Systems
-- Autonomous vehicles
-- Aerial Vehicles and Aerospace
+- Embedded **nonlinear nonlinear model predictive control** and **moving horizon estimation**
+- Robotics and advanced manufacturing
+- Autonomous (aerial/ground) vehicles
## Demos
@@ -82,9 +55,7 @@ Code generation? Piece of cake!
**OpEn** generates parametric optimizer modules in Rust - it's blazingly fast - it's safe - it can run on embedded devices.
-You can use the [MATLAB](https://alphaville.github.io/optimization-engine/docs/matlab-interface) or [Python interface](https://alphaville.github.io/optimization-engine/docs/python-interface) of OpEn to generate Rust code for your parametric optimizer.
-
-This can then be called directly, using Rust, or, it can be consumed as a service over a socket.
+You can use the [Python interface](https://alphaville.github.io/optimization-engine/docs/python-interface) of OpEn, `opengen`, to generate Rust code for your parametric optimizer. The optimizer can then be called from Python, used as a solver in Rust, consumed as a service over TCP (even remotely), or used in ROS on your robot.
@@ -98,18 +69,71 @@ OpEn can run on embedded devices; here we see it running on an intel Atom for th
-## Parametric Problems
+## Optimal Control
+
+Optimal control problems can now be set up directly from their natural ingredients: dynamics, prediction horizon, stage cost, terminal cost, and constraints. You can easily choose between a single or multiple shooting formulation. The solver is parametrizable, so you don't have to recompile whenever a parameter changes.
+
+OpEn allows to solve optimal control problems of the form
+
+
+
+Here is a minimal Python example (try it in [Google Colab](https://colab.research.google.com/drive/17vbVUbqcah9seIg17aN6bW0-T15FWrBo?usp=sharing)):
+
+
+
+
+
+
+```python
+import opengen as og
+import casadi.casadi as cs
+
+ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=20)
+ocp.add_parameter("x0", 2)
+ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+
+ocp.with_dynamics(
+ lambda x, u, p:
+ cs.vertcat(x[1], -x[0] + u[0]))
+
+ocp.with_stage_cost(
+ lambda x, u, p, t:
+ cs.dot(x - p["xref"], x - p["xref"])
+ + 0.1 * cs.dot(u, u))
+
+ocp.with_terminal_cost(
+ lambda x, p:
+ 10.0 * cs.dot(x - p["xref"], x - p["xref"]))
+
+ocp.with_input_constraints(og.constraints.BallInf(radius=1.))
+
+optimizer = og.ocp.OCPBuilder(
+ ocp,
+ meta=...,
+ build_config=...,
+ solver_config=...).build()
+
+result = optimizer.solve(x0=[1.0, 0.0], xref=[0.0, 0.0])
+```
+
+See the dedicated [OCP documentation](https://alphaville.github.io/optimization-engine/docs/python-ocp-1) for more details and examples.
+
+## General Parametric Optimization
**OpEn** can solve nonconvex parametric optimization problems of the general form
-
+
+
where *f* is a smooth cost, *U* is a simple - possibly nonconvex - set, *F1* and *F2* are nonlinear smooth mappings and *C* is a convex set ([read more](https://alphaville.github.io/optimization-engine/docs/open-intro)).
-## Code Generation Example
Code generation in **Python** in just a few lines of code (read the [docs](https://alphaville.github.io/optimization-engine/docs/python-examples) for details)
+
+
+
+
```python
import opengen as og
import casadi.casadi as cs
@@ -143,39 +167,44 @@ builder = og.builder.OpEnOptimizerBuilder(problem, meta,
builder.build()
```
-Code generation in a few lines of **MATLAB** code (read the [docs](https://alphaville.github.io/optimization-engine/docs/matlab-interface) for details)
+## Use the Solver in Rust
+
+You can also use OpEn directly in Rust by defining a cost function, its gradient, and a set of constraints, then solving the problem.
-```matlab
-% Define variables
-% ------------------------------------
-u = casadi.SX.sym('u', 5);
-p = casadi.SX.sym('p', 2);
+Here is a minimal Rust example:
-% Define cost function and constraints
-% ------------------------------------
-phi = rosenbrock(u, p);
-f2 = [1.5*u(1) - u(2);
- max(0, u(3)-u(4)+0.1)];
+```rust
+use optimization_engine::{constraints, panoc::*, Problem, SolverError};
-bounds = OpEnConstraints.make_ball_at_origin(5.0);
+let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> {
+ *c = (1.0 - u[0]).powi(2) + 200.0 * (u[1] - u[0].powi(2)).powi(2);
+ Ok(())
+};
-opEnBuilder = OpEnOptimizerBuilder()...
- .with_problem(u, p, phi, bounds)...
- .with_build_name('penalty_new')...
- .with_fpr_tolerance(1e-5)...
- .with_constraints_as_penalties(f2);
+let df = |u: &[f64], grad: &mut [f64]| -> Result<(), SolverError> {
+ grad[0] = 2.0 * (u[0] - 1.0) - 800.0 * u[0] * (u[1] - u[0].powi(2));
+ grad[1] = 400.0 * (u[1] - u[0].powi(2));
+ Ok(())
+};
-opEnOptimizer = opEnBuilder.build();
+let bounds = constraints::Ball2::new(None, 1.0);
+let problem = Problem::new(&bounds, df, f);
+let mut cache = PANOCCache::new(2, 1e-8, 10);
+let mut optimizer = PANOCOptimizer::new(problem, &mut cache).with_max_iter(100);
+let mut u = [-1.5, 0.9];
+let status = optimizer.solve(&mut u)?;
```
+See the dedicated [Rust documentation](https://alphaville.github.io/optimization-engine/docs/openrust-basic) for a full introduction and more complete examples.
+See more Rust examples [here](examples).
-## Getting started
+## Check out next...
- [More information about OpEn](https://alphaville.github.io/optimization-engine/docs/open-intro)
- [Quick installation guide](https://alphaville.github.io/optimization-engine/docs/installation)
- [OpEn in Rust](https://alphaville.github.io/optimization-engine/docs/openrust-basic)
- [OpEn in Python](https://alphaville.github.io/optimization-engine/docs/python-interface) ([Examples](https://alphaville.github.io/optimization-engine/docs/python-examples))
-- [OpEn in MATLAB](https://alphaville.github.io/optimization-engine/docs/matlab-interface) ([Examples](https://alphaville.github.io/optimization-engine/docs/matlab-examples))
+- [Optimal control](https://alphaville.github.io/optimization-engine/docs/python-ocp-1)
- [OpEn+Jupyter in Docker](https://alphaville.github.io/optimization-engine/docs/docker)
- [Generation of ROS packages](https://alphaville.github.io/optimization-engine/docs/python-ros)
- [Call OpEn in C/C++](https://alphaville.github.io/optimization-engine/docs/python-c)
diff --git a/ci/script.sh b/ci/script.sh
index f9e68a43..35599d7e 100755
--- a/ci/script.sh
+++ b/ci/script.sh
@@ -52,6 +52,7 @@ regular_test() {
export PYTHONPATH=.
python -W ignore test/test_constraints.py -v
python -W ignore test/test.py -v
+ python -W ignore test/test_ocp.py -v
python -W ignore test/test_raspberry_pi.py -v
diff --git a/icasadi/.gitignore b/design/icasadi_test/.gitignore
similarity index 100%
rename from icasadi/.gitignore
rename to design/icasadi_test/.gitignore
diff --git a/test/icasadi_test/Cargo.toml b/design/icasadi_test/Cargo.toml
similarity index 100%
rename from test/icasadi_test/Cargo.toml
rename to design/icasadi_test/Cargo.toml
diff --git a/test/icasadi_test/README.md b/design/icasadi_test/README.md
similarity index 100%
rename from test/icasadi_test/README.md
rename to design/icasadi_test/README.md
diff --git a/test/icasadi_test/build.rs b/design/icasadi_test/build.rs
similarity index 100%
rename from test/icasadi_test/build.rs
rename to design/icasadi_test/build.rs
diff --git a/icasadi/extern/Makefile b/design/icasadi_test/extern/Makefile
similarity index 100%
rename from icasadi/extern/Makefile
rename to design/icasadi_test/extern/Makefile
diff --git a/test/icasadi_test/extern/auto_casadi_constraints_type_penalty.c b/design/icasadi_test/extern/auto_casadi_constraints_type_penalty.c
similarity index 100%
rename from test/icasadi_test/extern/auto_casadi_constraints_type_penalty.c
rename to design/icasadi_test/extern/auto_casadi_constraints_type_penalty.c
diff --git a/test/icasadi_test/extern/auto_casadi_cost.c b/design/icasadi_test/extern/auto_casadi_cost.c
similarity index 100%
rename from test/icasadi_test/extern/auto_casadi_cost.c
rename to design/icasadi_test/extern/auto_casadi_cost.c
diff --git a/test/icasadi_test/extern/auto_casadi_grad.c b/design/icasadi_test/extern/auto_casadi_grad.c
similarity index 100%
rename from test/icasadi_test/extern/auto_casadi_grad.c
rename to design/icasadi_test/extern/auto_casadi_grad.c
diff --git a/icasadi/extern/icasadi.c b/design/icasadi_test/extern/icasadi.c
similarity index 100%
rename from icasadi/extern/icasadi.c
rename to design/icasadi_test/extern/icasadi.c
diff --git a/icasadi/extern/icasadi.h b/design/icasadi_test/extern/icasadi.h
similarity index 100%
rename from icasadi/extern/icasadi.h
rename to design/icasadi_test/extern/icasadi.h
diff --git a/test/icasadi_test/extern/icasadi_config.h b/design/icasadi_test/extern/icasadi_config.h
similarity index 100%
rename from test/icasadi_test/extern/icasadi_config.h
rename to design/icasadi_test/extern/icasadi_config.h
diff --git a/icasadi/extern/main.c b/design/icasadi_test/extern/main.c
similarity index 100%
rename from icasadi/extern/main.c
rename to design/icasadi_test/extern/main.c
diff --git a/icasadi/src/lib.rs b/design/icasadi_test/src/lib.rs
similarity index 100%
rename from icasadi/src/lib.rs
rename to design/icasadi_test/src/lib.rs
diff --git a/docs/python-advanced.md b/docs/python-advanced.md
index 9f679158..8b384533 100644
--- a/docs/python-advanced.md
+++ b/docs/python-advanced.md
@@ -1,6 +1,6 @@
---
id: python-advanced
-title: Advanced options
+title: Additional options
description: Advanced options of opengen, OpEn's Python interface
---
diff --git a/docs/python-ocp-1.md b/docs/python-ocp-1.md
new file mode 100644
index 00000000..dfc91ac9
--- /dev/null
+++ b/docs/python-ocp-1.md
@@ -0,0 +1,214 @@
+---
+id: python-ocp-1
+title: Getting started
+description: Optimal Control with OpEn/opengen
+---
+
+
+
+
+
+
+
+Info: The functionality presented here was introduced in opengen version 0.10.0a1. The API is still young and is likely to change in version 0.11.
+
+
+Opegen now comes with a new module that facilitates the construction
+of optimal control problems. In an intuitive and straightforward way,
+the user defines the problem ingredients (stage/terminal costs,
+state/input constraints, dynamics).
+
+
+
+## In a nutshell: quick overview
+
+Suppose you want to solve the optimal control problem
+
+Lastly, we have the state constraint $x_t \geq x_{\rm min}$, where $x_{\rm min}$ is a parameter,
+and the hard input constraints $|u_t| \leq 0.2$.
+
+
+
+
+
+
+This optimal control problem can be constructed as follows:
+
+
+
+
+```python
+import opengen as og
+import casadi.casadi as cs
+import numpy as np
+import matplotlib.pyplot as plt
+
+optimizer_name = "ocp_alm"
+
+# Construct the OCP
+ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=20)
+
+# Define the parameters
+ocp.add_parameter("x0", 2)
+ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ocp.add_parameter("q", 1, default=1)
+ocp.add_parameter("r", 1, default=0.1)
+ocp.add_parameter("a", 1, default=0.8)
+ocp.add_parameter("xmin", 1, default=-1)
+
+# System dynamics
+ocp.with_dynamics(lambda x, u, param:
+ cs.vertcat(0.98 * cs.sin(x[0]) + x[1],
+ 0.1 * x[0]**2 - 0.5 * x[0] + param["a"] * x[1] + u[0]))
+# Stage cost
+ocp.with_stage_cost(
+ lambda x, u, param, _t:
+ param["q"] * cs.dot(x - param["xref"], x - param["xref"])
+ + param["r"] * cs.dot(u, u)
+)
+
+# Terminal cost
+ocp.with_terminal_cost(
+ lambda x, param: 100 * cs.dot(x - param["xref"], x - param["xref"])
+)
+
+# State constraint: x1 <= xmax, imposed with ALM
+ocp.with_path_constraint(
+ lambda x, u, param, _t: x[1] - param["xmin"],
+ kind="alm",
+ set_c=og.constraints.Rectangle([0.], [np.inf]),
+)
+
+# Input constraints
+ocp.with_input_constraints(og.constraints.BallInf(radius=0.2))
+```
+
+Having defined the above OCP, we can build the optimizer...
+
+
+
+
+```python
+ocp_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name(optimizer_name),
+ build_configuration=og.config.BuildConfiguration()
+ .with_build_python_bindings().with_rebuild(True),
+ solver_configuration=og.config.SolverConfiguration()
+ .with_tolerance(1e-5)
+ .with_delta_tolerance(1e-5)
+ .with_preconditioning(True)
+ .with_penalty_weight_update_factor(1.8)
+ .with_max_inner_iterations(2000)
+ .with_max_outer_iterations(40),
+).build()
+```
+
+
+```python
+ocp_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name(optimizer_name),
+ build_configuration=og.config.BuildConfiguration()
+ .with_tcp_interface_config(
+ tcp_interface_config=og.config.TcpServerConfiguration(bind_port=3391)
+ ).with_rebuild(True),
+ solver_configuration=og.config.SolverConfiguration()
+ .with_tolerance(1e-5)
+ .with_delta_tolerance(1e-5)
+ .with_preconditioning(True)
+ .with_penalty_weight_update_factor(1.8)
+ .with_max_inner_iterations(2000)
+ .with_max_outer_iterations(40),
+).build()
+```
+
+
+
+The optimizer can then be called as follows:
+
+```python
+result = ocp_optimizer.solve(x0=[0.4, 0.2], q=30, r=1, xmin=-0.2)
+```
+
+and note that all parameters except `x0` are optional; if not specified,
+their default values will be used (the defaults were set when we constructed the
+OCP). We can now plot the optimal sequence of inputs (`result.inputs`)
+
+
+
+and the corresponding sequence of states (`result.states`)
+
+
+
+The object `result` contains the above sequences of inputs and states and additional
+information about the solution, solver time, Lagrange multipliers, etc.
diff --git a/docs/python-ocp-2.md b/docs/python-ocp-2.md
new file mode 100644
index 00000000..d2c2a0e0
--- /dev/null
+++ b/docs/python-ocp-2.md
@@ -0,0 +1,508 @@
+---
+id: python-ocp-2
+title: Constructing OCPs
+description: Optimal Control with OpEn/opengen
+---
+
+
+
+
+
+
+
+Info: The functionality presented here was introduced in opengen version 0.10.0a1. The API is still young and is likely to change in version 0.11.
+
+
+Here we will look at how we can construct an optimal control problem (OCP)
+by defining its state and terminal cost functions, input and state
+constraints, prediction horizon and other options.
+
+
+Generally, we want to solve problems of the form
+
+
+
+## OCP Formulations
+
+### Single shooting
+
+There are two main ways to formulate an optimal control
+problem. The **single shooting** approach consists in
+eliminating the sequence of states; the decision variables
+are the control actions $u_0, \ldots, u_{N-1}$, i.e., the OCP looks
+like (simplified version — additional constraints can be present)
+
+
+
+where $x_t$ is a function of the initial state, $x_0$, and
+the sequence of inputs $u_0, \ldots, u_{t-1}$. For example,
+$x_1 = F(x_0, u_0)$, and $x_2 = F(x_1, u_1) = F(F(x_0, u_0), u_1)$.
+The single shooting formulation is the default one as we have
+observed that it leads to better performance. To construct a
+single shooting OCP do
+
+```python
+import opengen as og
+import casadi.casadi as cs
+
+ocp = og.ocp.OptimalControlProblem(
+ nx=2, nu=1,
+ horizon=20,
+ shooting=og.ocp.ShootingMethod.SINGLE)
+```
+
+Since $\mathbf{u} = (u_0, \ldots, u_{N-1})$ is the decision
+variable, we can impose hard constraints of the form
+$u_t \in U$ for all $t$. For example, to impose the constraint
+$\Vert u_t \Vert_\infty \leq 0.2$ we can do
+
+```python
+set_U = og.constraints.BallInf(radius=0.2)
+ocp.with_input_constraints(set_U)
+```
+
+### Multiple shooting
+
+Alternatively, we can formulate the problem using the
+**multiple shooting** approach, where the sequence of states
+is not eliminated. The OCP now has the form (simplified version — additional constraints can be present)
+
+
+
+
+A multiple shooting problem can be constructed as follows
+
+```python
+ocp = og.ocp.OptimalControlProblem(
+ nx=2, nu=1,
+ horizon=20,
+ shooting=og.ocp.ShootingMethod.MULTIPLE)
+```
+
+Here the decision variable is the vector $\mathbf{z} = (u_0, x_0, \ldots, u_{N-1}, x_{N-1}, x_N)$
+and the system dynamics become equality constraints, which can be imposed via ALM (default)
+or PM; this can be configured using `with_dynamics_constraints` and either `"alm"` or `"penalty"`.
+
+```python
+ocp.with_dynamics_constraints("alm")
+```
+
+When using the multiple shooting formulation, we can impose constraints of the form $(x_t, u_t)\in Z$
+using `with_hard_stage_state_input_constraints`.
+
+```python
+set_Z = og.constraints.BallInf(radius=1.0)
+ocp.with_hard_stage_state_input_constraints(set_Z)
+```
+
+Likewise, since $x_N$ is part of the decision variable vector, $\mathbf{z}$,
+we can impose hard constraints. This can be done as follows
+
+```
+set_XN = og.constraints.Ball1(radius=0.1)
+ocp.with_hard_terminal_state_constraints(set_XN)
+```
+
+## Parameters
+
+The OCP API supports **named parameters**. Parameters are packed internally
+into the flat vector expected by the low-level OpEn builder, but the user does
+not need to keep track of slices manually. Instead, parameters are declared by
+name and are later accessed by name inside the callback functions.
+
+Parameters are added using `add_parameter(name, size, default=None)`.
+
+```python
+ocp.add_parameter("x0", 2)
+ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ocp.add_parameter("q", 1, default=10.0)
+```
+
+In the above example:
+
+- `x0` is required because no default value is provided
+- `xref` and `q` are optional when calling the optimizer
+- scalar defaults such as `10.0` are allowed for one-dimensional parameters
+
+If the same parameter name is declared twice, `add_parameter` raises a
+`ValueError`. Additional checks are in place to validate parameter dimensions.
+
+Parameters can be used in callback functions (dynamics, cost functions,
+constraints); more on this later.
+
+
+Important: The parameter x0 should always be declared because it defines the initial
+state of the OCP.
+
+
+
+
+## Dynamics
+
+The system dynamics is specified using `with_dynamics`. The callback must
+return the next state, that is, it must implement the map
+
+
+\[
+ x^+ = F(x, u; p)
+\]
+
+where `x` is the current state, `u` is the current input, and `p` is the
+parameter view introduced earlier. In other words, the OCP layer expects
+**discrete-time** dynamics.
+
+A typical discrete-time model can be provided as follows:
+
+```python
+ocp.with_dynamics(
+ lambda x, u, p:
+ cs.vertcat(
+ 0.98 * cs.sin(x[0]) + x[1],
+ 0.1 * x[0]**2 - 0.5 * x[0] + p["a"] * x[1] + u[0]
+ )
+)
+```
+
+Note that the parameter argument behaves like a dictionary of CasADi
+symbols, so named parameters can be accessed with expressions such as
+`p["a"]`, `p["xref"]`, and so on.
+
+### Continuous-time dynamics
+
+If your model is given in continuous time as
+
+
+\[
+ \dot{x} = f(x, u; p),
+\]
+
+you can discretize it using `og.ocp.DynamicsDiscretizer`. This helper
+constructs a discrete-time callback that is directly compatible with
+`with_dynamics`.
+
+For example, suppose
+
+
+
+Then we can discretize it as follows:
+
+```python
+continuous_dynamics = lambda x, u, p: cs.vertcat(
+ x[1],
+ -x[0] + u[0]
+)
+
+discretizer = og.ocp.DynamicsDiscretizer(
+ continuous_dynamics,
+ sampling_time=0.1,
+)
+
+ocp.with_dynamics(discretizer.rk4())
+```
+
+### Available discretization methods
+
+The following discretization methods are currently available:
+
+- `euler()`
+- `midpoint()`
+- `heun()`
+- `rk4()`
+- `multistep(method=..., num_steps=...)`
+
+For example, if a higher-accuracy internal integration is needed over one
+sampling interval, we can use a multistep method:
+
+```python
+ocp.with_dynamics(
+ discretizer.multistep(method="rk4", num_steps=4)
+)
+```
+
+This subdivides the sampling interval into four smaller substeps and applies
+RK4 on each one.
+
+### Choosing a discretization
+
+As a rule of thumb:
+
+- `euler()` is the simplest and cheapest option
+- `midpoint()` and `heun()` offer better accuracy than Euler at modest cost
+- `rk4()` is a good default when accuracy matters
+- `multistep(...)` is useful when the controller sampling time is relatively
+ large compared to the time scale of the plant
+
+In all cases, the result of the discretizer is a standard discrete-time
+callback, so once it is passed to `with_dynamics`, the remainder of the OCP
+construction is unchanged.
+
+## Stage cost function
+
+The stage cost is specified using `with_stage_cost`. The callback must accept
+`(x, u, p, stage_idx)` and return the scalar quantity $\ell_t(x_t, u_t; p)$.
+Here `x` is the current state, $x_t$, `u` is the current input, $u_t$,
+`p` is the parameter view, `stage_idx` is the stage index, $t$.
+
+### Typical quadratic stage cost
+
+A common choice in optimal control is a quadratic tracking term plus a control
+effort penalty:
+
+
+
+This can be written as:
+
+```python
+def my_stage_cost(x, u, p, _t):
+ xref = p["xref"]
+ uref = p["uref"]
+ ex = x - xref
+ eu = u - uref
+ return p["q"] * cs.dot(ex, ex) + p["r"] * cs.dot(eu, eu)
+
+ocp.with_stage_cost(my_stage_cost)
+```
+
+The stage index is written as `_t` above because it is not used in this
+example.
+
+### Time-varying stage costs
+
+If the cost depends explicitly on the stage, the argument `stage_idx` can be
+used directly. For example, a discounted quadratic stage cost can be written
+as
+
+$$\ell_t(x_t, u_t; p) = \gamma^t \left( q \Vert x_t - x^{\mathrm{ref}} \Vert^2 + r \Vert u_t - u^{\rm ref} \Vert^2 \right),$$
+
+where $\gamma \in (0, 1]$ is a discount factor. This can be implemented
+as follows (this time using a lambda):
+
+```python
+ocp.with_stage_cost(
+ lambda x, u, p, t:
+ (p["gamma"][0] ** t) * (
+ p["q"] * cs.dot(x - p["xref"], x - p["xref"])
+ + p["r"] * cs.dot(u - p["uref"], u - p["uref"])
+ )
+)
+```
+
+In that case, `gamma`, `q`, and `r` should be declared as parameters, for example:
+
+```python
+ocp.add_parameter("gamma", 1, default=0.95)
+ocp.add_parameter("q", 1, default=1.0)
+ocp.add_parameter("r", 1, default=0.5)
+```
+
+More generally, `stage_idx` can be used to encode stage-dependent references,
+weights, or economic costs.
+
+Note that the return value must be a scalar CasADi expression.
+
+## Terminal cost function
+
+The terminal cost is specified using `with_terminal_cost`. The callback must
+accept `(x, p)` and return the scalar quantity $V_f(x_N; p)$.
+Here `x` is the terminal state $x_N$ and `p` is the parameter view.
+
+A common choice is a quadratic tracking penalty of the form
+
+
+
+where $P$ is a symmetric positive definite matrix. This can be
+implemented as follows:
+
+```python
+# Terminal cost matrix
+P = cs.DM([
+ [10.0, 0.0],
+ [0.0, 2.0],
+])
+
+# Terminal cost function
+ocp.with_terminal_cost(
+ lambda x, p:
+ cs.mtimes([
+ (x - p["xref"]).T,
+ P,
+ (x - p["xref"]),
+ ])
+)
+```
+
+## General symbolic constraints
+
+In addition to hard constraints on the decision variables, the OCP API supports
+general **symbolic constraints**. These are constraints defined by CasADi
+expressions and handled either by the penalty method (PM) or by the augmented
+Lagrangian method (ALM).
+
+There are two variants:
+
+- `with_path_constraint` for stage-wise constraints, evaluated at every
+ stage $t = 0, \ldots, N-1$
+- `with_terminal_constraint` for terminal constraints, evaluated at $x_N$
+
+### Penalty-type symbolic constraints
+
+Penalty constraints are appended to the mapping $F_2$ (see [PM docs](/optimization-engine/docs/python-interface#penalty-method)).
+Such sonstraints are treated through the penalty method.
+For example, to impose the state inequality $x_{2,t} \geq x_{\min}$
+we may write
+
+```python
+ocp.with_path_constraint(
+ lambda x, u, p, _t: cs.fmax(0.0, -x[1] + p["xmin"]),
+ kind="penalty",
+)
+```
+
+Likewise, a terminal penalty constraint can be used to encode an inequality
+such as
+
+$$
+x_{1,N} + x_{2,N} \leq 0.1.
+$$
+
+This can be written as
+
+```python
+ocp.with_terminal_constraint(
+ lambda x, p: cs.fmax(0.0, x[0] + x[1] - 0.1),
+ kind="penalty",
+)
+```
+
+### ALM-type symbolic constraints
+
+ALM constraints are appended to the mapping $F_1$ and must be combined
+with a set :math:`C`, so that the constraint has the form
+
+
+\[
+ F_1(x_t, u_t; p) \in C
+\]
+
+or, for terminal constraints,
+
+
+\[
+ F_1(x_N; p) \in C.
+\]
+
+In that case, $C$ (`set_c`) must be provided.
+For example, the inequality
+
+
+\[
+ x_{2,t} - x_{\min} \geq 0
+\]
+
+can be written as an ALM constraint by taking $C = [0, \infty)$, that is,
+
+```python
+set_c = og.constraints.Rectangle([0.0], [float("inf")])
+ocp.with_path_constraint(
+ lambda x, u, p, _t: x[1] - p["xmin"],
+ kind="alm",
+ set_c=set_c,
+)
+```
+
+A terminal ALM constraint can be defined similarly:
+
+```python
+set_c = set_c=og.constraints.Rectangle([-0.1], [0.1])
+ocp.with_terminal_constraint(
+ lambda x, p: x[0],
+ kind="alm",
+ set_c=set_c,
+)
+```
+
+An optional dual set `set_y` may also be provided, but in many cases it is
+best to leave it unspecified and let the lower-level OpEn machinery choose it
+(see the [documentation](/optimization-engine/docs/python-interface#augmented-lagrangian-method)).
+
+## Next steps
+
+Once you have defined your optimal control problem, the next step is to
+[build an optimizer](/optimization-engine/docs/python-ocp-3).
\ No newline at end of file
diff --git a/docs/python-ocp-3.md b/docs/python-ocp-3.md
new file mode 100644
index 00000000..da5487e9
--- /dev/null
+++ b/docs/python-ocp-3.md
@@ -0,0 +1,92 @@
+---
+id: python-ocp-3
+title: Building OCPs
+description: Optimal Control with OpEn/opengen
+---
+
+
+
+
+
+
+
+Info: The functionality presented here was introduced in opengen version 0.10.0a1. The API is still young and is likely to change in version 0.11.
+
+
+Once an OCP has been defined, it can be turned into a generated optimizer
+using `og.ocp.OCPBuilder`. This builder lowers the high-level OCP to the
+standard OpEn problem format and then invokes the usual code-generation
+pipeline.
+
+At a minimum, you need:
+
+- the OCP definition,
+- optimizer metadata,
+- a build configuration, and
+- a solver configuration.
+
+For example:
+
+```python
+ocp_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("my_ocp"),
+ build_configuration=og.config.BuildConfiguration()
+ .with_build_python_bindings(),
+ solver_configuration=og.config.SolverConfiguration()
+ .with_tolerance(1e-5)
+ .with_delta_tolerance(1e-5),
+).build()
+```
+
+The call to `.build()` generates the solver and returns a
+`GeneratedOptimizer`, which can then be used directly:
+
+```python
+result = ocp_optimizer.solve(x0=[0.4, 0.2], xref=[0.0, 0.0])
+```
+
+If you prefer to access the generated solver over TCP instead of direct Python
+bindings, use `with_tcp_interface_config(...)` in the build configuration:
+
+```python
+ocp_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("my_ocp_tcp"),
+ build_configuration=og.config.BuildConfiguration()
+ .with_tcp_interface_config(),
+ solver_configuration=og.config.SolverConfiguration(),
+).build()
+```
+
+In both cases, the high-level interface remains the same:
+
+```python
+result = ocp_optimizer.solve(x0=[0.4, 0.2], xref=[0.0, 0.0])
+```
+
+For more details on build configurations, TCP servers, Python bindings, and
+the low-level code-generation pipeline, see the main
+[Python interface documentation](https://alphaville.github.io/optimization-engine/docs/python-interface).
diff --git a/docs/python-ocp-4.md b/docs/python-ocp-4.md
new file mode 100644
index 00000000..dde21e53
--- /dev/null
+++ b/docs/python-ocp-4.md
@@ -0,0 +1,138 @@
+---
+id: python-ocp-4
+title: Solving OCPs
+description: Optimal Control with OpEn/opengen
+---
+
+
+
+
+
+
+
+Info: The functionality presented here was introduced in opengen version 0.10.0a1. The API is still young and is likely to change in version 0.11.
+
+
+After an OCP optimizer has been built, it can be called using the method
+`solve(...)`. The main difference compared to the low-level interface is that
+the OCP optimizer accepts **named parameters** rather than a manually packed
+parameter vector.
+
+For example, if the OCP declares the parameters `x0`, `xref`, `q`, and `r`,
+the optimizer can be called as
+
+```python
+result = ocp_optimizer.solve(
+ x0=[0.4, 0.2],
+ xref=[0.0, 0.0],
+ q=30.0,
+ r=1.0,
+)
+```
+
+Any parameter with a default value may be omitted. For example, if `xref`,
+`q`, and `r` have defaults, then the following is also valid:
+
+```python
+result = ocp_optimizer.solve(x0=[0.4, 0.2])
+```
+
+Optional warm-start information may also be provided:
+
+```python
+result = ocp_optimizer.solve(
+ x0=[0.4, 0.2],
+ initial_guess=[0.0] * 20,
+ initial_lagrange_multipliers=[0.0] * 5,
+ initial_penalty=10.0,
+)
+```
+
+## Solution object
+
+The method `solve(...)` returns an object of type `OcpSolution`. This object
+contains both the raw low-level solver status and OCP-oriented views such as
+the stage-wise inputs and the state trajectory.
+
+The most commonly used fields are:
+
+- `result.inputs`: sequence of optimal inputs
+- `result.states`: sequence of states
+- `result.cost`: value of the objective at the solution
+- `result.exit_status`: solver exit status
+- `result.solve_time_ms`: total solver time in milliseconds
+
+Additional solver diagnostics are also available:
+
+- `result.penalty`
+- `result.num_outer_iterations`
+- `result.num_inner_iterations`
+- `result.last_problem_norm_fpr`
+- `result.f1_infeasibility`
+- `result.f2_norm`
+- `result.lagrange_multipliers`
+
+For convenience, the solution object can be printed directly:
+
+```python
+print(result)
+```
+
+This displays a compact summary of the solution, including inputs, states, and
+solver diagnostics.
+
+## Direct bindings and TCP
+
+The same high-level `solve(...)` interface is used regardless of whether the
+optimizer was built with direct Python bindings or with the TCP interface.
+When TCP is used, the underlying server is managed automatically by the
+generated optimizer wrapper.
+
+When the optimizer is accessed over TCP, the server is started automatically
+the first time `solve(...)` is called and remains alive for subsequent calls.
+When you are done with the optimizer, you should stop the TCP server
+explicitly by calling:
+
+```python
+ocp_optimizer.kill()
+```
+
+This has no effect for optimizers using direct Python bindings.
+
+## Reloading a generated optimizer
+
+Generated OCP optimizers save a manifest automatically when they are built.
+They can later be reloaded without rebuilding:
+
+```python
+optimizer = og.ocp.GeneratedOptimizer.load(
+ "path/to/optimizer_manifest.json"
+)
+
+result = optimizer.solve(x0=[0.4, 0.2], xref=[0.0, 0.0])
+```
+
+This is useful when you want to reuse a previously generated solver across
+Python sessions.
diff --git a/examples/README.md b/examples/README.md
new file mode 100644
index 00000000..dca42530
--- /dev/null
+++ b/examples/README.md
@@ -0,0 +1,6 @@
+## Rust examples
+
+1. [`panoc_ex1.rs`](panoc_ex1.rs): constrained minimization of the Rosenbrock function
+2. [`panoc_ex2.rs`](panoc_ex2.rs): another similar example
+3. [`pm.rs`](pm.rs): constrained optimization with the penalty method
+4. [`alm.rs`](alm.rs): constrained optimization with the augmented Lagrangian method
\ No newline at end of file
diff --git a/icasadi/Cargo.toml b/icasadi/Cargo.toml
deleted file mode 100644
index 9531df4b..00000000
--- a/icasadi/Cargo.toml
+++ /dev/null
@@ -1,25 +0,0 @@
-[package]
-authors = ["Pantelis Sopasakis"]
-name = "icasadi"
-categories = ["api-bindings", "no-std", "science", "science::robotics", "simulation"]
-keywords = ["casadi", "optimization", "interface"]
-description = "Rust interface to CasADi functions"
-documentation = "https://docs.rs/icasadi"
-license = "MIT"
-readme = "README.md"
-version = "0.2.0"
-edition = "2018"
-
-[dependencies]
-libc = { version = "0.2.0", default-features = false }
-
-[build-dependencies]
-cc = "1.0"
-bindgen = "0.49.0"
-
-[profile.dev]
-opt-level = 0
-
-
-[profile.release]
-opt-level = 3
diff --git a/icasadi/README.md b/icasadi/README.md
deleted file mode 100644
index 7d751870..00000000
--- a/icasadi/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# MOVED!
-
-Moved [here](../open-codegen/icasadi/)
\ No newline at end of file
diff --git a/icasadi/build.rs b/icasadi/build.rs
deleted file mode 100644
index b205c50e..00000000
--- a/icasadi/build.rs
+++ /dev/null
@@ -1,65 +0,0 @@
-use bindgen;
-use cc;
-
-use std::env;
-use std::path::{Path, PathBuf};
-
-fn main() {
- let out_path = PathBuf::from(env::var("OUT_DIR").unwrap());
-
- // Sanity checks to get better error messages
- assert!(
- Path::new("extern/auto_casadi_constraints_type_penalty.c").exists(),
- "extern/auto_casadi_constraints_type_penalty.c is missing"
- );
- assert!(
- Path::new("extern/auto_casadi_cost.c").exists(),
- "extern/auto_casadi_cost.c is missing"
- );
- assert!(
- Path::new("extern/auto_casadi_grad.c").exists(),
- "extern/auto_casadi_grad.c is missing"
- );
- assert!(
- Path::new("extern/icasadi.c").exists(),
- "extern/icasadi.c is missing"
- );
- assert!(
- Path::new("extern/icasadi.h").exists(),
- "extern/icasadi.h is missing"
- );
- assert!(
- Path::new("extern/icasadi_config.h").exists(),
- "extern/icasadi_config.h is missing"
- );
-
- cc::Build::new()
- .flag_if_supported("-Wall")
- .flag_if_supported("-Wpedantic")
- .flag_if_supported("-Wno-long-long")
- .flag_if_supported("-Wno-unused-parameter")
- .pic(true)
- .include("src")
- .file("extern/auto_casadi_cost.c")
- .file("extern/auto_casadi_grad.c")
- .file("extern/auto_casadi_constraints_type_penalty.c")
- .file("extern/icasadi.c")
- .compile("icasadi");
-
- // Extract the problem size parameter size constants from the
- // icasadi_config.h file
- bindgen::Builder::default()
- .header("extern/icasadi_config.h")
- .generate()
- .expect("Unable to generate bindings")
- .write_to_file(out_path.join("bindings.rs"))
- .expect("Couldn't write bindings!");
-
- // Rerun if these autogenerated files change
- println!("cargo:rerun-if-changed=extern/auto_casadi_cost.c");
- println!("cargo:rerun-if-changed=extern/auto_casadi_grad.c");
- println!("cargo:rerun-if-changed=extern/auto_casadi_constraints_type_penalty.c");
- println!("cargo:rerun-if-changed=extern/icasadi.c");
- println!("cargo:rerun-if-changed=extern/icasadi.h");
- println!("cargo:rerun-if-changed=extern/icasadi_config.h");
-}
diff --git a/icasadi/extern/auto_casadi_constraints_type_penalty.c b/icasadi/extern/auto_casadi_constraints_type_penalty.c
deleted file mode 100644
index e33b003b..00000000
--- a/icasadi/extern/auto_casadi_constraints_type_penalty.c
+++ /dev/null
@@ -1,112 +0,0 @@
-/* This file was automatically generated by CasADi.
- The CasADi copyright holders make no ownership claim of its contents. */
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* How to prefix internal symbols */
-#ifdef CODEGEN_PREFIX
- #define NAMESPACE_CONCAT(NS, ID) _NAMESPACE_CONCAT(NS, ID)
- #define _NAMESPACE_CONCAT(NS, ID) NS ## ID
- #define CASADI_PREFIX(ID) NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
-#else
- #define CASADI_PREFIX(ID) auto_casadi_constraints_type_penalty_ ## ID
-#endif
-
-#include
-
-#ifndef casadi_real
-#define casadi_real double
-#endif
-
-#ifndef casadi_int
-#define casadi_int long long int
-#endif
-
-/* Add prefix to internal symbols */
-#define casadi_f0 CASADI_PREFIX(f0)
-#define casadi_s0 CASADI_PREFIX(s0)
-#define casadi_s1 CASADI_PREFIX(s1)
-
-/* Symbol visibility in DLLs */
-#ifndef CASADI_SYMBOL_EXPORT
- #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
- #if defined(STATIC_LINKED)
- #define CASADI_SYMBOL_EXPORT
- #else
- #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
- #endif
- #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
- #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
- #else
- #define CASADI_SYMBOL_EXPORT
- #endif
-#endif
-
-static const casadi_int casadi_s0[24] = {20, 1, 0, 20, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19};
-static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
-
-/* constraints_as_penalty:(i0[20],i1)->(o0) */
-static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem) {
- casadi_real a0;
- a0=0.;
- if (res[0]!=0) res[0][0]=a0;
- return 0;
-}
-
-CASADI_SYMBOL_EXPORT int constraints_as_penalty(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem){
- return casadi_f0(arg, res, iw, w, mem);
-}
-
-CASADI_SYMBOL_EXPORT void constraints_as_penalty_incref(void) {
-}
-
-CASADI_SYMBOL_EXPORT void constraints_as_penalty_decref(void) {
-}
-
-CASADI_SYMBOL_EXPORT casadi_int constraints_as_penalty_n_in(void) { return 2;}
-
-CASADI_SYMBOL_EXPORT casadi_int constraints_as_penalty_n_out(void) { return 1;}
-
-CASADI_SYMBOL_EXPORT const char* constraints_as_penalty_name_in(casadi_int i){
- switch (i) {
- case 0: return "i0";
- case 1: return "i1";
- default: return 0;
- }
-}
-
-CASADI_SYMBOL_EXPORT const char* constraints_as_penalty_name_out(casadi_int i){
- switch (i) {
- case 0: return "o0";
- default: return 0;
- }
-}
-
-CASADI_SYMBOL_EXPORT const casadi_int* constraints_as_penalty_sparsity_in(casadi_int i) {
- switch (i) {
- case 0: return casadi_s0;
- case 1: return casadi_s1;
- default: return 0;
- }
-}
-
-CASADI_SYMBOL_EXPORT const casadi_int* constraints_as_penalty_sparsity_out(casadi_int i) {
- switch (i) {
- case 0: return casadi_s1;
- default: return 0;
- }
-}
-
-CASADI_SYMBOL_EXPORT int constraints_as_penalty_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
- if (sz_arg) *sz_arg = 2;
- if (sz_res) *sz_res = 1;
- if (sz_iw) *sz_iw = 0;
- if (sz_w) *sz_w = 0;
- return 0;
-}
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
diff --git a/open-codegen/CHANGELOG.md b/open-codegen/CHANGELOG.md
index 0782a291..751bb5ba 100644
--- a/open-codegen/CHANGELOG.md
+++ b/open-codegen/CHANGELOG.md
@@ -8,6 +8,31 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
Note: This is the Changelog file of `opengen` - the Python interface of OpEn
+## [0.10.0] - 2026-03-19
+
+
+### Added
+
+- New high-level `opengen.ocp` package for constructing optimal control problems from dynamics, stage/terminal costs, named parameters, and OCP-oriented constraints
+- Support for both single-shooting and multiple-shooting formulations, including penalty- and ALM-based constraint lowering
+- `GeneratedOptimizer` wrapper with `solve(x0=..., xref=...)` API and persistent optimizer metadata via `optimizer_manifest.json` plus `rollout.casadi`
+- Continuous-time dynamics discretization helpers in `opengen.ocp.DynamicsDiscretizer`, including Euler, midpoint, Heun, RK4, and multistep discretizations
+- Extended `OcpSolution` with solver diagnostics such as cost, penalty, iteration counts, infeasibility, fixed-point residual, and Lagrange multipliers
+- Detailed documentation available [here](https://alphaville.github.io/optimization-engine/docs/python-ocp-1), incl. Google Colab notebooks, and updated [API docs](https://alphaville.github.io/optimization-engine/api-dox/html/index.html)
+
+### Changed
+
+- Changed the default penalty weight update factor to `1.75`
+- Added and refined OCP documentation, docstrings, notebook examples, and automated tests
+- Improved readability of printed OCP solver results
+
+### Fixed
+
+- Avoid duplicate builder log messages by disabling propagation to the root logger
+- Use reliable package version lookup when recording the installed CasADi version in optimizer manifests
+- Check valid version in `with_version`
+
+
## [0.9.6] - 2026-03-14
### Fixed
@@ -255,7 +280,8 @@ Note: This is the Changelog file of `opengen` - the Python interface of OpEn
* Fixed `lbfgs` typo
-[0.9.5]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.5...opengen-0.9.6
+[0.10.0]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.6...opengen-0.10.0
+[0.9.6]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.5...opengen-0.9.6
[0.9.5]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.4...opengen-0.9.5
[0.9.4]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.3...opengen-0.9.4
[0.9.3]: https://github.com/alphaville/optimization-engine/compare/opengen-0.9.2...opengen-0.9.3
diff --git a/open-codegen/VERSION b/open-codegen/VERSION
index 9cf03868..2774f858 100644
--- a/open-codegen/VERSION
+++ b/open-codegen/VERSION
@@ -1 +1 @@
-0.9.6
\ No newline at end of file
+0.10.0
\ No newline at end of file
diff --git a/open-codegen/opengen/__init__.py b/open-codegen/opengen/__init__.py
index 8e50b150..d7cedb79 100644
--- a/open-codegen/opengen/__init__.py
+++ b/open-codegen/opengen/__init__.py
@@ -4,3 +4,4 @@
import opengen.functions
import opengen.constraints
import opengen.tcp
+import opengen.ocp
diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py
index 0ff153e7..316f5422 100644
--- a/open-codegen/opengen/builder/optimizer_builder.py
+++ b/open-codegen/opengen/builder/optimizer_builder.py
@@ -86,6 +86,9 @@ def with_verbosity_level(self, verbosity_level):
self.__logger.setLevel(verbosity_level)
self.__logger.handlers.clear()
self.__logger.addHandler(stream_handler)
+ # Keep logs on this named logger only; otherwise callers that configure
+ # the root logger can end up seeing every builder message twice.
+ self.__logger.propagate = False
return self
@@ -656,7 +659,6 @@ def __check_user_provided_parameters(self):
"For now we only support orthans (e.g., F1(u, p) <= 0).")
def __generate_code_python_bindings(self):
- self.__logger.info("Generating code for Python bindings")
target_dir = self.__target_dir()
python_bindings_dir_name = _PYTHON_BINDINGS_PREFIX + self.__meta.optimizer_name
python_bindings_dir = os.path.join(
diff --git a/open-codegen/opengen/config/meta.py b/open-codegen/opengen/config/meta.py
index c1400239..5d742a1c 100644
--- a/open-codegen/opengen/config/meta.py
+++ b/open-codegen/opengen/config/meta.py
@@ -1,6 +1,18 @@
import re # regular expressions
+SEMVER_PATTERN = re.compile(
+ r"^(0|[1-9]\d*)\."
+ r"(0|[1-9]\d*)\."
+ r"(0|[1-9]\d*)"
+ r"(?:-"
+ r"(?:0|[1-9]\d*|\d*[A-Za-z-][0-9A-Za-z-]*)"
+ r"(?:\.(?:0|[1-9]\d*|\d*[A-Za-z-][0-9A-Za-z-]*))*"
+ r")?"
+ r"(?:\+[0-9A-Za-z-]+(?:\.[0-9A-Za-z-]+)*)?$"
+)
+
+
class OptimizerMeta:
"""Metadata of auto-generated parametric optimizer
@@ -107,8 +119,20 @@ def with_version(self, optimizer_version):
:param optimizer_version: version of auto-generated optimizer
+ :raises ValueError: if ``optimizer_version`` is not a valid Cargo
+ package version following Semantic Versioning, for example
+ ``0.1.0`` or ``1.2.3-alpha.1``
+
:returns: The current instance of OptimizerMeta
"""
+ if not isinstance(optimizer_version, str) or not SEMVER_PATTERN.match(optimizer_version):
+ raise ValueError(
+ "invalid optimizer version {!r}; expected a valid Cargo package "
+ "version following Semantic Versioning, for example "
+ "'0.1.0', '1.2.3-alpha.1', or '1.0.0+build.5'".format(
+ optimizer_version
+ )
+ )
self.__optimizer_version = optimizer_version
return self
diff --git a/open-codegen/opengen/config/solver_config.py b/open-codegen/opengen/config/solver_config.py
index abed19fb..8958e66f 100644
--- a/open-codegen/opengen/config/solver_config.py
+++ b/open-codegen/opengen/config/solver_config.py
@@ -19,7 +19,7 @@ def __init__(self):
# For the initial penalty, None means that the actual value will be computed
# in Rust (see templates/optimizer.rs)
self.__initial_penalty = None
- self.__penalty_weight_update_factor = 5.0
+ self.__penalty_weight_update_factor = 1.75
self.__max_duration_micros = 5000000
self.__inner_tolerance_update_factor = 0.1
self.__sufficient_decrease_coefficient = 0.1
diff --git a/open-codegen/opengen/ocp/__init__.py b/open-codegen/opengen/ocp/__init__.py
new file mode 100644
index 00000000..3cdfa0cb
--- /dev/null
+++ b/open-codegen/opengen/ocp/__init__.py
@@ -0,0 +1,19 @@
+"""High-level OCP interface for building optimizers with ``opengen``."""
+
+from .builder import GeneratedOptimizer, OCPBuilder
+from .dynamics import DynamicsDiscretizer
+from .parameter import ParameterDefinition, ParameterPack, ParameterView
+from .problem import OptimalControlProblem, ShootingMethod
+from .solution import OcpSolution
+
+__all__ = [
+ "GeneratedOptimizer",
+ "DynamicsDiscretizer",
+ "OCPBuilder",
+ "OcpSolution",
+ "OptimalControlProblem",
+ "ParameterDefinition",
+ "ParameterPack",
+ "ParameterView",
+ "ShootingMethod",
+]
diff --git a/open-codegen/opengen/ocp/builder.py b/open-codegen/opengen/ocp/builder.py
new file mode 100644
index 00000000..8a44286c
--- /dev/null
+++ b/open-codegen/opengen/ocp/builder.py
@@ -0,0 +1,564 @@
+"""Builders and runtime wrappers for OCP-generated optimizers."""
+
+import importlib
+import json
+import os
+import sys
+import hashlib
+import platform
+from datetime import datetime, timezone
+import casadi
+import casadi.casadi as cs
+from importlib.metadata import version, PackageNotFoundError
+
+from opengen.builder.optimizer_builder import OpEnOptimizerBuilder
+from opengen.builder.problem import Problem
+from opengen.config.build_config import BuildConfiguration
+from opengen.config.solver_config import SolverConfiguration
+from opengen.constraints.no_constraints import NoConstraints
+from opengen.tcp.optimizer_tcp_manager import OptimizerTcpManager
+
+from .constraint_utils import make_constraint_product
+from .parameter import ParameterPack
+from .problem import ShootingMethod
+from .solution import OcpSolution
+
+
+class GeneratedOptimizer:
+ """High-level runtime wrapper around a generated optimizer.
+
+ This class hides whether the underlying solver is consumed through direct
+ Python bindings or through the TCP interface and exposes a uniform
+ ``solve(x0=..., xref=...)`` API based on named parameters.
+ """
+
+ def __init__(
+ self,
+ optimizer_name,
+ target_dir,
+ backend,
+ backend_kind,
+ ocp=None,
+ symbolic_model=None,
+ metadata=None,
+ ):
+ """Construct a generated optimizer wrapper.
+
+ :param optimizer_name: generated optimizer name
+ :param target_dir: generated optimizer directory
+ :param backend: low-level backend object
+ :param backend_kind: backend type, e.g. ``"direct"`` or ``"tcp"``
+ :param ocp: source OCP definition
+ :param symbolic_model: precomputed symbolic model of the OCP
+ :param metadata: serialized optimizer metadata used when reloading an
+ optimizer from disk
+ """
+ self.__optimizer_name = optimizer_name
+ self.__target_dir = os.path.abspath(target_dir)
+ self.__backend = backend
+ self.__backend_kind = backend_kind
+ self.__started = False
+ self.__rollout_function = None
+ self.__input_slices = None
+ self.__state_slices = None
+
+ if metadata is not None:
+ self.__initialize_from_metadata(metadata)
+ elif ocp is not None:
+ self.__initialize_from_ocp(ocp, symbolic_model=symbolic_model)
+ else:
+ raise ValueError("either ocp or metadata must be provided")
+
+ def __initialize_from_ocp(self, ocp, symbolic_model=None):
+ """Populate runtime metadata from an in-memory OCP object."""
+ self.__shooting = ocp.shooting
+ self.__nx = ocp.nx
+ self.__nu = ocp.nu
+ self.__horizon = ocp.horizon
+ self.__parameters = ocp.parameters
+ model = symbolic_model if symbolic_model is not None else ocp.build_symbolic_model()
+ self.__input_slices = model.get("input_slices")
+ self.__state_slices = model.get("state_slices")
+ self.__rollout_function = self.__make_rollout_function(model)
+
+ def __initialize_from_metadata(self, metadata):
+ """Populate runtime metadata from a saved JSON manifest."""
+ self.__shooting = ShootingMethod(metadata["shooting"])
+ self.__nx = metadata["nx"]
+ self.__nu = metadata["nu"]
+ self.__horizon = metadata["horizon"]
+ self.__parameters = ParameterPack()
+ for definition in metadata["parameters"]:
+ self.__parameters.add(
+ definition["name"],
+ definition["size"],
+ default=definition["default"],
+ )
+ self.__input_slices = metadata.get("input_slices")
+ self.__state_slices = metadata.get("state_slices")
+ self.__rollout_function = metadata.get("rollout_function")
+
+ @property
+ def target_dir(self):
+ """Directory of the generated optimizer project."""
+ return self.__target_dir
+
+ @property
+ def optimizer_name(self):
+ """Name of the generated optimizer."""
+ return self.__optimizer_name
+
+ @property
+ def backend_kind(self):
+ """Backend kind used by this optimizer wrapper."""
+ return self.__backend_kind
+
+ def start(self):
+ """Start the backend if it is a local TCP server.
+
+ :return: current instance
+ """
+ if self.__backend_kind == "tcp" and not self.__started:
+ self.__backend.start()
+ self.__started = True
+ return self
+
+ def kill(self):
+ """Stop the backend if it is a local TCP server."""
+ if self.__backend_kind == "tcp" and self.__started:
+ self.__backend.kill()
+ self.__started = False
+
+ def __make_rollout_function(self, symbolic_model):
+ """
+ Create the state rollout function for single shooting
+
+ The function is used in single shooting formualations only;
+ a function is constructed to compute the sequence of states
+ from the sequence of inputs.
+ """
+ if self.__shooting == ShootingMethod.MULTIPLE:
+ return None
+ states = cs.horzcat(*symbolic_model["state_trajectory"])
+ return cs.Function("ocp_rollout", [symbolic_model["u"], symbolic_model["p"]], [states])
+
+ def __pack_parameters(self, solve_kwargs):
+ """Pack named keyword arguments into the flat solver parameter vector."""
+ return self.__parameters.pack(solve_kwargs)
+
+ def __extract_inputs(self, flat_solution):
+ r"""Extract stage-wise inputs :math:`u_0, \ldots, u_{N-1}` from the flat solution."""
+ if self.__shooting == ShootingMethod.SINGLE:
+ nu = self.__nu
+ return [
+ list(flat_solution[stage_idx * nu:(stage_idx + 1) * nu])
+ for stage_idx in range(self.__horizon)
+ ]
+
+ return [
+ list(flat_solution[start:stop])
+ for start, stop in self.__input_slices
+ ]
+
+ def __extract_states(self, flat_solution, packed_parameters):
+ r"""Extract or reconstruct the state trajectory :math:`x_0, \ldots, x_N`."""
+ if self.__shooting == ShootingMethod.MULTIPLE:
+ x0_start, x0_stop = self.__parameters.slices()["x0"]
+ x0 = packed_parameters[x0_start:x0_stop]
+ states = [x0]
+ states.extend(
+ list(flat_solution[start:stop])
+ for start, stop in self.__state_slices
+ )
+ return states
+
+ state_matrix = self.__rollout_function(flat_solution, packed_parameters).full()
+ return [state_matrix[:, idx].reshape((-1,)).tolist() for idx in range(state_matrix.shape[1])]
+
+ def __metadata_dict(self):
+ """Return the JSON-serializable optimizer manifest."""
+ rollout_sha256 = None
+ if self.__rollout_function is not None:
+ rollout_sha256 = None
+
+ return {
+ "manifest_version": 1,
+ "optimizer_name": self.__optimizer_name,
+ "target_dir": self.__target_dir,
+ "backend_kind": self.__backend_kind,
+ "created_at": datetime.now(timezone.utc).isoformat(),
+ "platform": platform.platform(),
+ "python_version": sys.version,
+ "opengen_version": self.__safe_package_version("opengen"),
+ "casadi_version": self.__casadi_version(),
+ "shooting": self.__shooting.value,
+ "nx": self.__nx,
+ "nu": self.__nu,
+ "horizon": self.__horizon,
+ "decision_dimension": self.__nu * self.__horizon
+ if self.__shooting == ShootingMethod.SINGLE
+ else self.__horizon * (self.__nu + self.__nx),
+ "parameter_dimension": self.__parameters.total_size(),
+ "parameters": [
+ {
+ "name": definition.name,
+ "size": definition.size,
+ "default": definition.default,
+ }
+ for definition in self.__parameters.definitions()
+ ],
+ "input_slices": self.__input_slices,
+ "state_slices": self.__state_slices,
+ "rollout_file": "rollout.casadi" if self.__rollout_function is not None else None,
+ "rollout_sha256": rollout_sha256,
+ }
+
+ @staticmethod
+ def __safe_package_version(package_name):
+ """Return the installed version of a package if available."""
+ try:
+ return version(package_name)
+ except PackageNotFoundError:
+ return None
+
+ @staticmethod
+ def __casadi_version():
+ """Return the installed CasADi version if available."""
+ casadi_version = getattr(casadi, "__version__", None)
+ if casadi_version is not None:
+ return casadi_version
+ return GeneratedOptimizer.__safe_package_version("casadi")
+
+ def save(self, json_path=None):
+ """Save a manifest that can later recreate this optimizer.
+
+ The manifest is stored in the generated optimizer directory by default.
+ For single-shooting optimizers, the rollout function is stored in a
+ separate ``rollout.casadi`` file next to the manifest.
+
+ :param json_path: optional destination manifest path
+ :return: current instance
+ """
+ if json_path is None:
+ json_path = os.path.join(self.__target_dir, "optimizer_manifest.json")
+
+ json_path = os.path.abspath(json_path)
+ manifest_dir = os.path.dirname(json_path)
+ os.makedirs(manifest_dir, exist_ok=True)
+
+ metadata = self.__metadata_dict()
+ rollout_file = metadata.get("rollout_file")
+ if rollout_file is not None:
+ rollout_path = os.path.join(manifest_dir, rollout_file)
+ self.__rollout_function.save(rollout_path)
+ with open(rollout_path, "rb") as fh:
+ metadata["rollout_sha256"] = hashlib.sha256(fh.read()).hexdigest()
+
+ with open(json_path, "w") as fh:
+ json.dump(metadata, fh, indent=2)
+ return self
+
+ @staticmethod
+ def __load_backend(target_dir, optimizer_name, backend_kind):
+ """Create a backend object from saved metadata."""
+ if backend_kind == "direct":
+ if target_dir not in sys.path:
+ sys.path.insert(0, target_dir)
+ module = importlib.import_module(optimizer_name)
+ return module.solver()
+ if backend_kind == "tcp":
+ return OptimizerTcpManager(target_dir)
+ if backend_kind == "none":
+ return None
+ raise ValueError(f"unknown backend kind '{backend_kind}'")
+
+ @classmethod
+ def load(cls, json_path):
+ """Load a previously saved optimizer manifest.
+
+ :param json_path: path to a JSON manifest created by :meth:`save`
+ :return: reconstructed :class:`GeneratedOptimizer`
+ """
+ json_path = os.path.abspath(json_path)
+ with open(json_path, "r") as fh:
+ metadata = json.load(fh)
+ rollout_file = metadata.get("rollout_file")
+ if rollout_file is not None:
+ rollout_path = os.path.join(os.path.dirname(json_path), rollout_file)
+ metadata["rollout_function"] = cs.Function.load(rollout_path)
+ rollout_sha256 = metadata.get("rollout_sha256")
+ if rollout_sha256 is not None:
+ with open(rollout_path, "rb") as fh:
+ current_sha256 = hashlib.sha256(fh.read()).hexdigest()
+ if current_sha256 != rollout_sha256:
+ raise ValueError("rollout.casadi checksum mismatch")
+ else:
+ raise ValueError("manifest is missing rollout_sha256")
+ backend = cls.__load_backend(
+ metadata["target_dir"],
+ metadata["optimizer_name"],
+ metadata["backend_kind"],
+ )
+ return cls(
+ optimizer_name=metadata["optimizer_name"],
+ target_dir=metadata["target_dir"],
+ backend=backend,
+ backend_kind=metadata["backend_kind"],
+ metadata=metadata,
+ )
+
+ def solve(
+ self,
+ initial_guess=None,
+ initial_lagrange_multipliers=None,
+ initial_penalty=None,
+ **parameter_values,
+ ):
+ r"""Solve the generated OCP optimizer.
+
+ Named keyword arguments are packed according to the declared OCP
+ parameters. For example, if the OCP declares ``x0`` and ``xref``, the
+ solver can be called as ``optimizer.solve(x0=x0, xref=xref)`` to solve
+ the OCP for a given initial condition :math:`x_0` and reference
+ :math:`x^{\mathrm{ref}}`.
+
+ :param initial_guess: optional initial decision-variable guess
+ :param initial_lagrange_multipliers: optional initial ALM multipliers
+ :param initial_penalty: optional initial penalty parameter
+ :param parameter_values: named parameter values
+ :return: :class:`OcpSolution`
+ :raises RuntimeError: if the backend is unavailable or the low-level
+ solve call fails
+ """
+ packed_parameters = self.__pack_parameters(parameter_values)
+
+ if self.__backend_kind == "direct":
+ raw = self.__backend.run(
+ p=packed_parameters,
+ initial_guess=initial_guess,
+ initial_lagrange_multipliers=initial_lagrange_multipliers,
+ initial_penalty=initial_penalty,
+ )
+ if raw is None:
+ raise RuntimeError("solver failed")
+ elif self.__backend_kind == "tcp":
+ self.start()
+ response = self.__backend.call(
+ packed_parameters,
+ initial_guess=initial_guess,
+ initial_y=initial_lagrange_multipliers,
+ initial_penalty=initial_penalty,
+ )
+ if not response.is_ok():
+ raise RuntimeError(str(response.get()))
+ raw = response.get()
+ else:
+ raise RuntimeError("optimizer backend is not available")
+
+ inputs = self.__extract_inputs(raw.solution)
+ states = self.__extract_states(raw.solution, packed_parameters)
+ return OcpSolution(raw=raw, inputs=inputs, states=states)
+
+
+class OCPBuilder:
+ """Builder that lowers an OCP to the low-level OpEn builder stack."""
+
+ def __init__(
+ self,
+ problem,
+ metadata,
+ build_configuration=BuildConfiguration(),
+ solver_configuration=None,
+ ):
+ """Construct an OCP builder.
+
+ :param problem: instance of :class:`OptimalControlProblem`
+ :param metadata: optimizer metadata
+ :param build_configuration: OpEn build configuration
+ :param solver_configuration: OpEn solver configuration
+ """
+ self.__ocp = problem
+ self.__metadata = metadata
+ self.__build_configuration = build_configuration
+ self.__solver_configuration = (
+ solver_configuration
+ if solver_configuration is not None
+ else SolverConfiguration()
+ )
+ self.__generate_not_build = False
+
+ def with_generate_not_build_flag(self, flag):
+ """Generate code without compiling it.
+
+ :param flag: whether to generate only
+ :return: current instance
+ """
+ self.__generate_not_build = flag
+ return self
+
+ def __make_input_constraints(self):
+ r"""Build the hard input constraint set for single shooting.
+
+ This produces a stage-wise product set for the control inputs
+ :math:`u_0, \ldots, u_{N-1}`.
+ """
+ stage_constraints = self.__ocp.input_constraints
+ if stage_constraints is None:
+ return NoConstraints()
+
+ if self.__ocp.horizon == 1:
+ return stage_constraints
+
+ segments = [((idx + 1) * self.__ocp.nu) - 1 for idx in range(self.__ocp.horizon)]
+ constraints = [stage_constraints] * self.__ocp.horizon
+ return make_constraint_product(segments, constraints)
+
+ def __make_multiple_shooting_constraints(self):
+ """Build the hard decision-variable set for multiple shooting.
+
+ Depending on the selected OCP options, this acts on input blocks
+ :math:`u_t`, stage-wise state-input blocks :math:`(x_t, u_t)`, or the
+ terminal state :math:`x_N`.
+ """
+ stage_constraints = self.__ocp.input_constraints
+ if stage_constraints is None:
+ stage_constraints = NoConstraints()
+
+ hard_stage_constraints = self.__ocp.hard_stage_state_input_constraints
+ hard_terminal_constraints = self.__ocp.hard_terminal_state_constraints
+
+ if hard_stage_constraints is not None and self.__ocp.input_constraints is not None:
+ raise ValueError(
+ "cannot combine with_input_constraints with "
+ "with_hard_stage_state_input_constraints automatically; "
+ "encode the input bounds directly in the stage state-input set"
+ )
+
+ if hard_stage_constraints is not None and hard_terminal_constraints is not None:
+ raise ValueError(
+ "cannot combine with_hard_stage_state_input_constraints with "
+ "with_hard_terminal_state_constraints automatically; "
+ "their intersection on the terminal stage is not supported"
+ )
+
+ if hard_stage_constraints is not None:
+ segments = [
+ ((idx + 1) * (self.__ocp.nu + self.__ocp.nx)) - 1
+ for idx in range(self.__ocp.horizon)
+ ]
+ constraints = [hard_stage_constraints] * self.__ocp.horizon
+ return make_constraint_product(segments, constraints)
+
+ if hard_terminal_constraints is not None:
+ segments = []
+ constraints = []
+ offset = -1
+
+ for _ in range(self.__ocp.horizon - 1):
+ offset += self.__ocp.nu
+ segments.append(offset)
+ constraints.append(stage_constraints)
+
+ offset += self.__ocp.nx
+ segments.append(offset)
+ constraints.append(NoConstraints())
+
+ offset += self.__ocp.nu
+ segments.append(offset)
+ constraints.append(stage_constraints)
+
+ offset += self.__ocp.nx
+ segments.append(offset)
+ constraints.append(hard_terminal_constraints)
+
+ return make_constraint_product(segments, constraints)
+
+ segments = []
+ constraints = []
+ offset = -1
+
+ for _ in range(self.__ocp.horizon):
+ offset += self.__ocp.nu
+ segments.append(offset)
+ constraints.append(stage_constraints)
+
+ offset += self.__ocp.nx
+ segments.append(offset)
+ constraints.append(NoConstraints())
+
+ return make_constraint_product(segments, constraints)
+
+ def build_problem(self, symbolic_model=None):
+ """Lower the OCP to a low-level :class:`opengen.builder.problem.Problem`.
+
+ :return: low-level OpEn problem
+ """
+ model = symbolic_model if symbolic_model is not None else self.__ocp.build_symbolic_model()
+ low_level_problem = Problem(model["u"], model["p"], model["cost"])
+
+ if self.__ocp.shooting == ShootingMethod.SINGLE:
+ constraints = self.__make_input_constraints()
+ elif self.__ocp.shooting == ShootingMethod.MULTIPLE:
+ constraints = self.__make_multiple_shooting_constraints()
+ else:
+ raise NotImplementedError("unsupported shooting method")
+
+ low_level_problem = low_level_problem.with_constraints(constraints)
+
+ if model["alm_mapping"] is not None:
+ low_level_problem = low_level_problem.with_aug_lagrangian_constraints(
+ model["alm_mapping"],
+ model["alm_set_c"],
+ model["alm_set_y"],
+ )
+
+ if model["penalty_mapping"] is not None:
+ low_level_problem = low_level_problem.with_penalty_constraints(
+ model["penalty_mapping"]
+ )
+
+ return low_level_problem
+
+ def __make_backend(self, target_dir):
+ """Create the runtime backend associated with the generated optimizer."""
+ optimizer_name = self.__metadata.optimizer_name
+
+ if self.__build_configuration.build_python_bindings:
+ if target_dir not in sys.path:
+ sys.path.insert(0, target_dir)
+ module = importlib.import_module(optimizer_name)
+ return module.solver(), "direct"
+
+ if self.__build_configuration.tcp_interface_config is not None:
+ return OptimizerTcpManager(target_dir), "tcp"
+
+ return None, "none"
+
+ def build(self):
+ """Generate, compile, and wrap the optimizer.
+
+ :return: :class:`GeneratedOptimizer`
+ """
+ symbolic_model = self.__ocp.build_symbolic_model()
+ low_level_problem = self.build_problem(symbolic_model=symbolic_model)
+ builder = OpEnOptimizerBuilder(
+ low_level_problem,
+ metadata=self.__metadata,
+ build_configuration=self.__build_configuration,
+ solver_configuration=self.__solver_configuration,
+ )
+ if self.__generate_not_build:
+ builder.with_generate_not_build_flag(True)
+ info = builder.build()
+ target_dir = os.path.abspath(info["paths"]["target"])
+ backend, backend_kind = self.__make_backend(target_dir)
+ optimizer = GeneratedOptimizer(
+ optimizer_name=self.__metadata.optimizer_name,
+ target_dir=target_dir,
+ backend=backend,
+ backend_kind=backend_kind,
+ ocp=self.__ocp,
+ symbolic_model=symbolic_model,
+ )
+ optimizer.save()
+ return optimizer
diff --git a/open-codegen/opengen/ocp/constraint_utils.py b/open-codegen/opengen/ocp/constraint_utils.py
new file mode 100644
index 00000000..0a22892c
--- /dev/null
+++ b/open-codegen/opengen/ocp/constraint_utils.py
@@ -0,0 +1,65 @@
+"""Helpers for simplifying products of OCP constraint sets."""
+
+from opengen.constraints.cartesian import CartesianProduct
+from opengen.constraints.no_constraints import NoConstraints
+from opengen.constraints.rectangle import Rectangle
+from opengen.constraints.zero import Zero
+
+
+def segment_dimensions(segments):
+ """Compute segment dimensions from Cartesian-product end indices."""
+ dimensions = []
+ previous = -1
+ for segment_end in segments:
+ dimensions.append(segment_end - previous)
+ previous = segment_end
+ return dimensions
+
+
+def rectangle_bounds(constraint, dimension):
+ """Return explicit box bounds when a set admits a rectangle representation."""
+ if isinstance(constraint, NoConstraints):
+ return [float("-inf")] * dimension, [float("inf")] * dimension
+
+ if isinstance(constraint, Zero):
+ return [0.0] * dimension, [0.0] * dimension
+
+ if isinstance(constraint, Rectangle):
+ if constraint.dimension() != dimension:
+ raise ValueError("constraint dimension does not match its segment length")
+ xmin = constraint.xmin if constraint.xmin is not None else [float("-inf")] * dimension
+ xmax = constraint.xmax if constraint.xmax is not None else [float("inf")] * dimension
+ return list(xmin), list(xmax)
+
+ return None
+
+
+def make_constraint_product(segments, constraints):
+ """Build the most specific set representing a block product of constraints."""
+ if not constraints:
+ return NoConstraints()
+
+ if len(constraints) == 1:
+ return constraints[0]
+
+ if all(isinstance(constraint, NoConstraints) for constraint in constraints):
+ return NoConstraints()
+
+ if all(isinstance(constraint, Zero) for constraint in constraints):
+ return Zero()
+
+ dimensions = segment_dimensions(segments)
+ rectangle_data = [
+ rectangle_bounds(constraint, dimension)
+ for constraint, dimension in zip(constraints, dimensions)
+ ]
+
+ if all(bounds is not None for bounds in rectangle_data):
+ xmin = []
+ xmax = []
+ for current_xmin, current_xmax in rectangle_data:
+ xmin.extend(current_xmin)
+ xmax.extend(current_xmax)
+ return Rectangle(xmin, xmax)
+
+ return CartesianProduct(segments, constraints)
diff --git a/open-codegen/opengen/ocp/dynamics.py b/open-codegen/opengen/ocp/dynamics.py
new file mode 100644
index 00000000..9f9ca3c7
--- /dev/null
+++ b/open-codegen/opengen/ocp/dynamics.py
@@ -0,0 +1,167 @@
+"""Continuous-time dynamics discretization helpers for OCP problems."""
+
+import casadi.casadi as cs
+
+
+class DynamicsDiscretizer:
+ r"""Discretizer for continuous-time dynamics.
+
+ This helper wraps a continuous-time model of the form
+ :math:`\dot{x} = f(x, u, p)` and produces a discrete-time callback
+ compatible with :meth:`opengen.ocp.problem.OptimalControlProblem.with_dynamics`.
+ """
+
+ def __init__(self, continuous_dynamics, sampling_time):
+ r"""Construct a dynamics discretizer.
+
+ :param continuous_dynamics: callable implementing
+ :math:`\dot{x} = f(x, u, p)`
+ :param sampling_time: sampling time :math:`T_s`
+ :raises ValueError: if the sampling time is not positive
+ """
+ if sampling_time <= 0:
+ raise ValueError("sampling_time must be positive")
+
+ self.__continuous_dynamics = continuous_dynamics
+ self.__sampling_time = float(sampling_time)
+
+ @property
+ def sampling_time(self):
+ """Sampling time :math:`T_s`."""
+ return self.__sampling_time
+
+ @property
+ def continuous_dynamics(self):
+ """Continuous-time dynamics callback."""
+ return self.__continuous_dynamics
+
+ def euler(self):
+ r"""Return an explicit Euler discretization callback.
+
+ The returned callback implements:
+
+ :math:`x^+ = x + T_s f(x, u, p)`.
+ """
+
+ def discrete_dynamics(x, u, param):
+ return x + self.__sampling_time * self.__continuous_dynamics(x, u, param)
+
+ return discrete_dynamics
+
+ def midpoint(self):
+ r"""Return an explicit midpoint discretization callback.
+
+ The returned callback implements the second-order Runge-Kutta rule:
+
+ :math:`x^+ = x + T_s f(x + \tfrac{T_s}{2} k_1, u, p)`,
+
+ where :math:`k_1 = f(x, u, p)`.
+ """
+ ts = self.__sampling_time
+ f = self.__continuous_dynamics
+
+ def discrete_dynamics(x, u, param):
+ k1 = f(x, u, param)
+ midpoint_state = x + 0.5 * ts * k1
+ return x + ts * f(midpoint_state, u, param)
+
+ return discrete_dynamics
+
+ def heun(self):
+ r"""Return a Heun discretization callback.
+
+ The returned callback implements the explicit trapezoidal method:
+
+ :math:`x^+ = x + \tfrac{T_s}{2}(k_1 + k_2)`,
+
+ where :math:`k_1 = f(x, u, p)` and
+ :math:`k_2 = f(x + T_s k_1, u, p)`.
+ """
+ ts = self.__sampling_time
+ f = self.__continuous_dynamics
+
+ def discrete_dynamics(x, u, param):
+ k1 = f(x, u, param)
+ k2 = f(x + ts * k1, u, param)
+ return x + 0.5 * ts * (k1 + k2)
+
+ return discrete_dynamics
+
+ def rk4(self):
+ r"""Return a classical fourth-order Runge-Kutta discretization callback.
+
+ The returned callback implements:
+
+ :math:`x^+ = x + \frac{T_s}{6}(k_1 + 2k_2 + 2k_3 + k_4)`,
+
+ where the intermediate slopes are computed from the continuous-time
+ dynamics.
+ """
+ ts = self.__sampling_time
+ f = self.__continuous_dynamics
+
+ def discrete_dynamics(x, u, param):
+ k1 = f(x, u, param)
+ k2 = f(x + 0.5 * ts * k1, u, param)
+ k3 = f(x + 0.5 * ts * k2, u, param)
+ k4 = f(x + ts * k3, u, param)
+ return x + (ts / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
+
+ return discrete_dynamics
+
+ def multistep(self, method="rk4", num_steps=1):
+ """Return a discrete-time callback with internal substeps.
+
+ This helper subdivides the sampling interval :math:`T_s` into
+ ``num_steps`` smaller steps and applies the chosen explicit method on
+ each subinterval.
+
+ Supported methods are ``"euler"``, ``"midpoint"``, ``"heun"``, and
+ ``"rk4"``.
+
+ :param method: base discretization method
+ :param num_steps: number of internal substeps
+ :return: discrete-time dynamics callback
+ :raises ValueError: if ``num_steps`` is not a positive integer or the
+ method is unknown
+ """
+ if not isinstance(num_steps, int) or num_steps <= 0:
+ raise ValueError("num_steps must be a positive integer")
+
+ if num_steps == 1:
+ return self.discretize(method)
+
+ substep = DynamicsDiscretizer(
+ self.__continuous_dynamics,
+ self.__sampling_time / float(num_steps),
+ )
+ base_step = substep.discretize(method)
+
+ def discrete_dynamics(x, u, param):
+ x_next = x
+ for _ in range(num_steps):
+ x_next = base_step(x_next, u, param)
+ return x_next
+
+ return discrete_dynamics
+
+ def discretize(self, method="euler"):
+ """Return a discrete-time dynamics callback for a chosen method.
+
+ Supported methods are ``"euler"``, ``"midpoint"``, ``"heun"``, and
+ ``"rk4"``.
+
+ :param method: discretization method
+ :return: discrete-time dynamics callback
+ :raises ValueError: if the method is unknown
+ """
+ method = method.lower()
+ if method == "euler":
+ return self.euler()
+ if method == "midpoint":
+ return self.midpoint()
+ if method == "heun":
+ return self.heun()
+ if method == "rk4":
+ return self.rk4()
+ raise ValueError(f"unknown discretization method '{method}'")
diff --git a/open-codegen/opengen/ocp/parameter.py b/open-codegen/opengen/ocp/parameter.py
new file mode 100644
index 00000000..1f0a91f2
--- /dev/null
+++ b/open-codegen/opengen/ocp/parameter.py
@@ -0,0 +1,209 @@
+"""Utilities for named OCP parameters and flat OpEn parameter vectors."""
+
+import casadi.casadi as cs
+
+
+class ParameterDefinition:
+ """Definition of a named parameter block.
+
+ A parameter block has a name, a dimension and, optionally, a default
+ numeric value. Multiple parameter blocks are later packed into the flat
+ parameter vector expected by the low-level OpEn builder.
+ """
+
+ def __init__(self, name, size, default=None):
+ """Construct a parameter definition.
+
+ :param name: parameter name
+ :param size: block dimension
+ :param default: optional default numeric value
+ :raises ValueError: if the name, size or default are invalid
+ """
+ if not isinstance(name, str) or not name:
+ raise ValueError("parameter name must be a non-empty string")
+ if not isinstance(size, int) or size <= 0:
+ raise ValueError("parameter size must be a positive integer")
+
+ self.__name = name
+ self.__size = size
+ self.__default = ParameterDefinition._normalize_default(default, size)
+
+ @staticmethod
+ def _normalize_default(default, size):
+ """Validate and normalize a default value to a list of floats."""
+ if default is None:
+ return None
+
+ if size == 1 and not isinstance(default, (list, tuple)):
+ return [float(default)]
+
+ if len(default) != size:
+ raise ValueError("default value has incompatible dimension")
+
+ return [float(value) for value in default]
+
+ @property
+ def name(self):
+ """Name of the parameter block."""
+ return self.__name
+
+ @property
+ def size(self):
+ """Dimension of the parameter block."""
+ return self.__size
+
+ @property
+ def default(self):
+ """Default value as a list of floats, or ``None`` if absent."""
+ return None if self.__default is None else list(self.__default)
+
+ def has_default(self):
+ """Whether this parameter block has a default value."""
+ return self.__default is not None
+
+
+class ParameterView:
+ """Dictionary-like accessor for symbolic parameter slices.
+
+ Instances of this class are passed to user-defined callbacks so they can
+ access named symbolic parameters using expressions such as
+ ``param["xref"]``.
+ """
+
+ def __init__(self, packed_symbol, slices):
+ """Construct a parameter view.
+
+ :param packed_symbol: flat symbolic parameter vector
+ :param slices: dictionary mapping parameter names to index pairs
+ """
+ self.__packed_symbol = packed_symbol
+ self.__slices = dict(slices)
+
+ def __getitem__(self, name):
+ """Return the symbolic slice associated with a named parameter."""
+ if name not in self.__slices:
+ raise KeyError(f"unknown parameter '{name}'")
+ start, end = self.__slices[name]
+ return self.__packed_symbol[start:end]
+
+ def get(self, name, default=None):
+ """Return the symbolic slice associated with ``name`` if it exists."""
+ if name not in self.__slices:
+ return default
+ return self[name]
+
+ def keys(self):
+ """Return all known parameter names."""
+ return self.__slices.keys()
+
+ @property
+ def packed(self):
+ """The underlying flat symbolic parameter vector."""
+ return self.__packed_symbol
+
+
+class ParameterPack:
+ """Registry and packer for named parameters.
+
+ This class stores the declared parameter blocks of an OCP and provides
+ helpers to:
+
+ - construct a flat symbolic parameter vector,
+ - create named symbolic views into that vector, and
+ - pack concrete numeric values for calls to the generated solver.
+ """
+
+ def __init__(self, symbol_type=cs.SX.sym):
+ """Construct an empty parameter registry.
+
+ :param symbol_type: CasADi symbol constructor, typically ``cs.SX.sym``
+ or ``cs.MX.sym``
+ """
+ self.__definitions = []
+ self.__definitions_by_name = {}
+ self.__symbol_type = symbol_type
+
+ def add(self, name, size, default=None):
+ """Add a named parameter block.
+
+ :param name: parameter name
+ :param size: block dimension
+ :param default: optional default numeric value
+ :return: current instance
+ :raises ValueError: if a parameter with the same name already exists
+ """
+ if name in self.__definitions_by_name:
+ raise ValueError(f"parameter '{name}' already exists")
+
+ definition = ParameterDefinition(name, size, default=default)
+ self.__definitions.append(definition)
+ self.__definitions_by_name[name] = definition
+ return self
+
+ def total_size(self):
+ """Return the total dimension of the packed parameter vector."""
+ return sum(definition.size for definition in self.__definitions)
+
+ def symbol(self, name="p"):
+ """Construct the packed symbolic parameter vector.
+
+ :param name: symbolic variable name
+ :return: CasADi symbolic vector containing all parameter blocks
+ """
+ return self.__symbol_type(name, self.total_size())
+
+ def slices(self):
+ """Return the slice map of all parameter blocks.
+
+ :return: dictionary ``name -> (start, stop)``
+ """
+ offset = 0
+ slices = {}
+ for definition in self.__definitions:
+ next_offset = offset + definition.size
+ slices[definition.name] = (offset, next_offset)
+ offset = next_offset
+ return slices
+
+ def view(self, packed_symbol):
+ """Create a named symbolic view of a packed parameter vector."""
+ return ParameterView(packed_symbol, self.slices())
+
+ def pack(self, values=None):
+ """Pack numeric values into the flat parameter vector.
+
+ :param values: dictionary mapping parameter names to values
+ :return: flat list of floats
+ :raises ValueError: if a required parameter is missing or dimensions are
+ inconsistent
+ """
+ values = {} if values is None else dict(values)
+ packed = []
+ missing = []
+
+ for definition in self.__definitions:
+ value = values.get(definition.name, definition.default)
+ if value is None:
+ missing.append(definition.name)
+ continue
+
+ if definition.size == 1 and not isinstance(value, (list, tuple)):
+ value = [value]
+
+ if len(value) != definition.size:
+ raise ValueError(
+ f"parameter '{definition.name}' has incompatible dimension"
+ )
+
+ packed.extend(float(item) for item in value)
+
+ if missing:
+ raise ValueError(
+ "missing values for parameters: " + ", ".join(sorted(missing))
+ )
+
+ return packed
+
+ def definitions(self):
+ """Return the declared parameter definitions."""
+ return list(self.__definitions)
diff --git a/open-codegen/opengen/ocp/problem.py b/open-codegen/opengen/ocp/problem.py
new file mode 100644
index 00000000..fd1a5db2
--- /dev/null
+++ b/open-codegen/opengen/ocp/problem.py
@@ -0,0 +1,532 @@
+"""High-level optimal control problem definitions for ``opengen``."""
+
+from enum import Enum
+
+import casadi.casadi as cs
+
+from opengen.constraints.zero import Zero
+
+from .constraint_utils import make_constraint_product
+from .parameter import ParameterPack
+
+
+class ShootingMethod(Enum):
+ """Available OCP formulations."""
+
+ SINGLE = "single"
+ MULTIPLE = "multiple"
+
+
+class OptimalControlProblem:
+ """High-level specification of a finite-horizon optimal control problem.
+
+ The OCP layer collects dynamics, costs, parameters and constraints in an
+ OCP-oriented format and later lowers them to the low-level
+ :class:`opengen.builder.problem.Problem` abstraction.
+ """
+
+ def __init__(self, nx, nu, horizon, shooting=ShootingMethod.SINGLE):
+ """Construct a new optimal control problem.
+
+ :param nx: state dimension
+ :param nu: input dimension
+ :param horizon: prediction horizon
+ :param shooting: shooting formulation, either single or multiple
+ :raises ValueError: if dimensions are invalid
+ """
+ if not isinstance(nx, int) or nx <= 0:
+ raise ValueError("nx must be a positive integer")
+ if not isinstance(nu, int) or nu <= 0:
+ raise ValueError("nu must be a positive integer")
+ if not isinstance(horizon, int) or horizon <= 0:
+ raise ValueError("horizon must be a positive integer")
+
+ if isinstance(shooting, str):
+ shooting = ShootingMethod(shooting)
+
+ self.__nx = nx
+ self.__nu = nu
+ self.__horizon = horizon
+ self.__shooting = shooting
+ self.__symbol_type = cs.SX.sym
+ self.__parameters = ParameterPack(symbol_type=self.__symbol_type)
+
+ self.__dynamics = None
+ self.__dynamics_constraint_kind = "penalty"
+ self.__stage_cost = None
+ self.__terminal_cost = None
+ self.__input_constraints = None
+ self.__hard_stage_state_input_constraints = None
+ self.__hard_terminal_state_constraints = None
+ self.__path_constraints = []
+ self.__terminal_constraints = []
+
+ @property
+ def nx(self):
+ """State dimension."""
+ return self.__nx
+
+ @property
+ def nu(self):
+ """Input dimension."""
+ return self.__nu
+
+ @property
+ def horizon(self):
+ """Prediction horizon."""
+ return self.__horizon
+
+ @property
+ def shooting(self):
+ """Selected shooting formulation."""
+ return self.__shooting
+
+ @property
+ def symbol_type(self):
+ """CasADi symbol constructor used internally."""
+ return self.__symbol_type
+
+ @property
+ def parameters(self):
+ """Declared named parameters."""
+ return self.__parameters
+
+ @property
+ def dynamics(self):
+ """User-defined dynamics callback."""
+ return self.__dynamics
+
+ @property
+ def input_constraints(self):
+ """Hard stage-wise input constraints."""
+ return self.__input_constraints
+
+ @property
+ def hard_stage_state_input_constraints(self):
+ """Hard multiple-shooting constraints on stage-wise ``(x_t, u_t)``."""
+ return self.__hard_stage_state_input_constraints
+
+ @property
+ def hard_terminal_state_constraints(self):
+ """Hard multiple-shooting constraints on the terminal state ``x_N``."""
+ return self.__hard_terminal_state_constraints
+
+ @property
+ def path_constraints(self):
+ """Stage-wise symbolic PM/ALM constraints."""
+ return list(self.__path_constraints)
+
+ @property
+ def terminal_constraints(self):
+ """Terminal symbolic PM/ALM constraints."""
+ return list(self.__terminal_constraints)
+
+ def add_parameter(self, name, size, default=None):
+ """Add a named parameter block to the OCP.
+
+ :param name: parameter name
+ :param size: parameter dimension
+ :param default: optional default numeric value
+ :return: current instance
+
+ :raises ValueError: if the parameter definition is invalid or a
+ parameter with the same name has already been declared
+ """
+ self.__parameters.add(name, size, default=default)
+ return self
+
+ def with_dynamics(self, dynamics):
+ """Specify the system dynamics callback.
+
+ The callback must accept ``(x, u, param)`` and return the next state
+ :math:`x^+ = F(x, u, p)`.
+
+ Note that the callback must be a function involving
+ symbolic computations using CasADi functions.
+
+ :param dynamics: dynamics callback
+ :return: current instance
+ """
+ self.__dynamics = dynamics
+ return self
+
+ def with_dynamics_constraints(self, kind="penalty"):
+ """Choose how multiple-shooting defect constraints are imposed.
+
+ Supported values are ``"penalty"`` and ``"alm"``. This option is only
+ relevant in multiple shooting. The corresponding defect constraints are
+ of the form :math:`x_{t+1} - F(x_t, u_t, p)`.
+
+ :param kind: constraint treatment for dynamics defects
+ :return: current instance
+ :raises ValueError: if ``kind`` is invalid
+ """
+ if kind not in ("penalty", "alm"):
+ raise ValueError("dynamics constraint kind must be either 'penalty' or 'alm'")
+ self.__dynamics_constraint_kind = kind
+ return self
+
+ def with_stage_cost(self, stage_cost):
+ """Specify the stage cost callback.
+
+ The callback must accept ``(x, u, param, stage_idx)``.
+
+ :param stage_cost: stage cost callback
+ :return: current instance
+ """
+ self.__stage_cost = stage_cost
+ return self
+
+ def with_terminal_cost(self, terminal_cost):
+ """Specify the terminal cost callback.
+
+ The callback must accept ``(x, param)``.
+
+ :param terminal_cost: terminal cost callback
+ :return: current instance
+ """
+ self.__terminal_cost = terminal_cost
+ return self
+
+ def with_input_constraints(self, constraints):
+ """Specify hard constraints on the stage-wise control inputs.
+
+ In single shooting these are the only hard constraints that act
+ directly on decision variables. In multiple shooting they are applied
+ to every input block :math:`u_t`.
+
+ :param constraints: instance of ``opengen.constraints.Constraint``
+ :return: current instance
+ """
+ self.__input_constraints = constraints
+ return self
+
+ def with_hard_stage_state_input_constraints(self, constraints):
+ r"""Specify hard stage-wise constraints on :math:`(x_t, u_t)`.
+
+ This method is only meaningful in the multiple-shooting formulation.
+
+ :param constraints: set acting on the stacked vector
+ :math:`\operatorname{vertcat}(x_t, u_t)`
+ :return: current instance
+ """
+ self.__hard_stage_state_input_constraints = constraints
+ return self
+
+ def with_hard_terminal_state_constraints(self, constraints):
+ """Specify hard constraints on the terminal state :math:`x_N`.
+
+ This method is only meaningful in the multiple-shooting formulation.
+
+ :param constraints: set acting on :math:`x_N`
+ :return: current instance
+ """
+ self.__hard_terminal_state_constraints = constraints
+ return self
+
+ @staticmethod
+ def __constraint_dimension(mapping):
+ """Return the scalar dimension of a CasADi mapping."""
+ return mapping.size1() * mapping.size2()
+
+ @staticmethod
+ def __append_constraint_definition(container, kind, constraint, set_c=None, set_y=None):
+ """Validate and append a symbolic PM/ALM constraint definition."""
+ if kind not in ("penalty", "alm"):
+ raise ValueError("constraint kind must be either 'penalty' or 'alm'")
+ if kind == "alm" and set_c is None:
+ raise ValueError("set_c must be provided for ALM constraints")
+ container.append({
+ "kind": kind,
+ "constraint": constraint,
+ "set_c": set_c,
+ "set_y": set_y,
+ })
+
+ @staticmethod
+ def __assemble_constraint_set(blocks, key):
+ """Assemble repeated constraint blocks into a set or Cartesian product."""
+ if not blocks:
+ return None
+ if len(blocks) == 1:
+ return blocks[0][key]
+
+ segments = []
+ constraints = []
+ offset = -1
+ for block in blocks:
+ offset += block["dimension"]
+ segments.append(offset)
+ constraints.append(block[key])
+ return make_constraint_product(segments, constraints)
+
+ def with_path_constraint(self, constraint, kind="penalty", set_c=None, set_y=None):
+ r"""Add a stage-wise symbolic constraint.
+
+ The callback must accept ``(x, u, param, stage_idx)``. When
+ ``kind="penalty"``, its output is appended to the penalty mapping
+ :math:`F_2`. When ``kind="alm"``, its output is appended to the ALM
+ mapping :math:`F_1` and the user must also provide ``set_c`` so that
+ the constraints take the form :math:`F_1(x_t, u_t, p) \in C`.
+
+ :param constraint: stage constraint callback
+ :param kind: either ``"penalty"`` or ``"alm"``
+ :param set_c: set :math:`C` for ALM constraints
+ :param set_y: optional dual set :math:`Y` for ALM constraints
+ :return: current instance
+ """
+ self.__append_constraint_definition(
+ self.__path_constraints,
+ kind,
+ constraint,
+ set_c=set_c,
+ set_y=set_y,
+ )
+ return self
+
+ def with_terminal_constraint(self, constraint, kind="penalty", set_c=None, set_y=None):
+ """Add a terminal symbolic constraint.
+
+ The callback must accept ``(x, param)``. PM and ALM semantics are the
+ same as in :meth:`with_path_constraint`.
+
+ :param constraint: terminal constraint callback
+ :param kind: either ``"penalty"`` or ``"alm"``
+ :param set_c: set :math:`C` for ALM constraints
+ :param set_y: optional dual set :math:`Y` for ALM constraints
+ :return: current instance
+ """
+ self.__append_constraint_definition(
+ self.__terminal_constraints,
+ kind,
+ constraint,
+ set_c=set_c,
+ set_y=set_y,
+ )
+ return self
+
+ def validate(self):
+ """Validate the OCP definition before lowering it.
+
+ :return: current instance
+ :raises ValueError: if mandatory components are missing or a hard
+ state-based constraint is used in single shooting
+ """
+ if self.__dynamics is None:
+ raise ValueError("dynamics must be specified")
+ if self.__stage_cost is None:
+ raise ValueError("stage cost must be specified")
+ if self.__shooting == ShootingMethod.SINGLE:
+ if self.__hard_stage_state_input_constraints is not None:
+ raise ValueError(
+ "with_hard_stage_state_input_constraints is only available in multiple shooting"
+ )
+ if self.__hard_terminal_state_constraints is not None:
+ raise ValueError(
+ "with_hard_terminal_state_constraints is only available in multiple shooting"
+ )
+ return self
+
+ def __build_single_shooting_model(self):
+ """Build the symbolic low-level model for single shooting."""
+ u = self.__symbol_type("u", self.__nu * self.__horizon)
+ p = self.__parameters.symbol()
+ param = self.__parameters.view(p)
+
+ x0 = param.get("x0")
+ if x0 is None:
+ raise ValueError("parameter 'x0' must be declared for OCP problems")
+ if x0.size1() * x0.size2() != self.__nx:
+ raise ValueError("parameter 'x0' has incompatible dimension")
+
+ cost = 0
+ x = x0
+ states = [x0]
+ penalty_terms = []
+ alm_blocks = []
+
+ for stage_idx in range(self.__horizon):
+ start = stage_idx * self.__nu
+ stop = start + self.__nu
+ u_t = u[start:stop]
+ cost += self.__stage_cost(x, u_t, param, stage_idx)
+
+ for definition in self.__path_constraints:
+ mapping = definition["constraint"](x, u_t, param, stage_idx)
+ if definition["kind"] == "penalty":
+ penalty_terms.append(mapping)
+ else:
+ alm_blocks.append({
+ "mapping": mapping,
+ "dimension": self.__constraint_dimension(mapping),
+ "set_c": definition["set_c"],
+ "set_y": definition["set_y"],
+ })
+
+ x = self.__dynamics(x, u_t, param)
+ states.append(x)
+
+ if self.__terminal_cost is not None:
+ cost += self.__terminal_cost(x, param)
+
+ for definition in self.__terminal_constraints:
+ mapping = definition["constraint"](x, param)
+ if definition["kind"] == "penalty":
+ penalty_terms.append(mapping)
+ else:
+ alm_blocks.append({
+ "mapping": mapping,
+ "dimension": self.__constraint_dimension(mapping),
+ "set_c": definition["set_c"],
+ "set_y": definition["set_y"],
+ })
+
+ penalty_mapping = None
+ if penalty_terms:
+ penalty_mapping = cs.vertcat(*penalty_terms)
+
+ alm_mapping = None
+ alm_set_c = None
+ alm_set_y = None
+ if alm_blocks:
+ alm_mapping = cs.vertcat(*[block["mapping"] for block in alm_blocks])
+ alm_set_c = self.__assemble_constraint_set(alm_blocks, "set_c")
+ if all(block["set_y"] is not None for block in alm_blocks):
+ alm_set_y = self.__assemble_constraint_set(alm_blocks, "set_y")
+
+ return {
+ "shooting": self.__shooting.value,
+ "u": u,
+ "p": p,
+ "param": param,
+ "cost": cost,
+ "alm_mapping": alm_mapping,
+ "alm_set_c": alm_set_c,
+ "alm_set_y": alm_set_y,
+ "penalty_mapping": penalty_mapping,
+ "state_trajectory": states,
+ }
+
+ def __build_multiple_shooting_model(self):
+ """Build the symbolic low-level model for multiple shooting."""
+ p = self.__parameters.symbol()
+ param = self.__parameters.view(p)
+
+ x0 = param.get("x0")
+ if x0 is None:
+ raise ValueError("parameter 'x0' must be declared for OCP problems")
+ if x0.size1() * x0.size2() != self.__nx:
+ raise ValueError("parameter 'x0' has incompatible dimension")
+
+ decision_blocks = []
+ input_slices = []
+ state_slices = []
+ states = [x0]
+ cost = 0
+ penalty_terms = []
+ alm_blocks = []
+ dynamics_alm_terms = []
+
+ x_current = x0
+ offset = 0
+
+ for stage_idx in range(self.__horizon):
+ u_t = self.__symbol_type(f"u_{stage_idx}", self.__nu)
+ x_next = self.__symbol_type(f"x_{stage_idx + 1}", self.__nx)
+
+ decision_blocks.extend([u_t, x_next])
+ input_slices.append((offset, offset + self.__nu))
+ offset += self.__nu
+ state_slices.append((offset, offset + self.__nx))
+ offset += self.__nx
+
+ cost += self.__stage_cost(x_current, u_t, param, stage_idx)
+
+ for definition in self.__path_constraints:
+ mapping = definition["constraint"](x_current, u_t, param, stage_idx)
+ if definition["kind"] == "penalty":
+ penalty_terms.append(mapping)
+ else:
+ alm_blocks.append({
+ "mapping": mapping,
+ "dimension": self.__constraint_dimension(mapping),
+ "set_c": definition["set_c"],
+ "set_y": definition["set_y"],
+ })
+
+ dynamics_defect = x_next - self.__dynamics(x_current, u_t, param)
+ if self.__dynamics_constraint_kind == "penalty":
+ penalty_terms.append(dynamics_defect)
+ else:
+ dynamics_alm_terms.append(dynamics_defect)
+
+ x_current = x_next
+ states.append(x_current)
+
+ if self.__terminal_cost is not None:
+ cost += self.__terminal_cost(x_current, param)
+
+ if dynamics_alm_terms:
+ dynamics_mapping = cs.vertcat(*dynamics_alm_terms)
+ alm_blocks.append({
+ "mapping": dynamics_mapping,
+ "dimension": self.__constraint_dimension(dynamics_mapping),
+ "set_c": Zero(),
+ "set_y": None,
+ })
+
+ for definition in self.__terminal_constraints:
+ mapping = definition["constraint"](x_current, param)
+ if definition["kind"] == "penalty":
+ penalty_terms.append(mapping)
+ else:
+ alm_blocks.append({
+ "mapping": mapping,
+ "dimension": self.__constraint_dimension(mapping),
+ "set_c": definition["set_c"],
+ "set_y": definition["set_y"],
+ })
+
+ u = cs.vertcat(*decision_blocks)
+ penalty_mapping = cs.vertcat(*penalty_terms) if penalty_terms else None
+ alm_mapping = None
+ alm_set_c = None
+ alm_set_y = None
+ if alm_blocks:
+ alm_mapping = cs.vertcat(*[block["mapping"] for block in alm_blocks])
+ alm_set_c = self.__assemble_constraint_set(alm_blocks, "set_c")
+ if all(block["set_y"] is not None for block in alm_blocks):
+ alm_set_y = self.__assemble_constraint_set(alm_blocks, "set_y")
+
+ return {
+ "shooting": self.__shooting.value,
+ "u": u,
+ "p": p,
+ "param": param,
+ "cost": cost,
+ "alm_mapping": alm_mapping,
+ "alm_set_c": alm_set_c,
+ "alm_set_y": alm_set_y,
+ "penalty_mapping": penalty_mapping,
+ "state_trajectory": states,
+ "input_slices": input_slices,
+ "state_slices": state_slices,
+ }
+
+ def build_symbolic_model(self):
+ r"""Lower the OCP to a symbolic model understood by the OCP builder.
+
+ The returned dictionary contains the decision variables, packed
+ parameter vector, symbolic cost, optional ALM/penalty mappings and the
+ symbolic state trajectory. In particular, the returned mappings are the
+ low-level objects later used to define constraints of the form
+ :math:`F_1(u, p) \in C` and :math:`F_2(u, p) = 0`.
+
+ :return: symbolic model dictionary
+ """
+ self.validate()
+ if self.__shooting == ShootingMethod.SINGLE:
+ return self.__build_single_shooting_model()
+ if self.__shooting == ShootingMethod.MULTIPLE:
+ return self.__build_multiple_shooting_model()
+ raise NotImplementedError("unsupported shooting method")
diff --git a/open-codegen/opengen/ocp/solution.py b/open-codegen/opengen/ocp/solution.py
new file mode 100644
index 00000000..8555d1b1
--- /dev/null
+++ b/open-codegen/opengen/ocp/solution.py
@@ -0,0 +1,68 @@
+"""High-level solution objects returned by OCP-generated optimizers."""
+
+from numbers import Real
+
+class OcpSolution:
+ """High-level solution returned by :meth:`GeneratedOptimizer.solve`.
+
+ The object wraps the raw low-level solver status and adds OCP-oriented
+ views such as stage-wise inputs and the reconstructed state trajectory.
+ """
+
+ def __init__(self, raw, inputs, states):
+ """Construct an OCP solution object.
+
+ :param raw: raw low-level solver status
+ :param inputs: stage-wise input sequence
+ :param states: state trajectory
+ """
+ self.raw = raw
+ self.inputs = inputs
+ self.states = states
+ self.solution = getattr(raw, "solution", [])
+ self.cost = getattr(raw, "cost", None)
+ self.exit_status = getattr(raw, "exit_status", None)
+ self.solve_time_ms = getattr(raw, "solve_time_ms", None)
+ self.penalty = getattr(raw, "penalty", None)
+ self.num_outer_iterations = getattr(raw, "num_outer_iterations", None)
+ self.num_inner_iterations = getattr(raw, "num_inner_iterations", None)
+ self.last_problem_norm_fpr = getattr(raw, "last_problem_norm_fpr", None)
+ self.f1_infeasibility = getattr(raw, "f1_infeasibility", None)
+ self.f2_norm = getattr(raw, "f2_norm", None)
+ self.lagrange_multipliers = getattr(raw, "lagrange_multipliers", None)
+
+ def __repr__(self):
+ """Return a readable multi-line summary of the solution."""
+ def fmt(value):
+ if isinstance(value, bool):
+ return str(value)
+ if isinstance(value, Real):
+ numeric = float(value)
+ if numeric == 0.0:
+ return "0.0"
+ text = f"{numeric:.4g}"
+ return "0.0" if text == "-0" else text
+ if isinstance(value, list):
+ return "[" + ", ".join(fmt(item) for item in value) + "]"
+ if isinstance(value, tuple):
+ return "(" + ", ".join(fmt(item) for item in value) + ")"
+ return str(value)
+
+ return "\n".join([
+ "OCP Solution:",
+ f" Exit status.......... {self.exit_status}",
+ f" Cost................. {fmt(self.cost)}",
+ f" Solve time [ms]...... {fmt(self.solve_time_ms)}",
+ f" Penalty.............. {fmt(self.penalty)}",
+ f" Outer iterations..... {fmt(self.num_outer_iterations)}",
+ f" Inner iterations..... {fmt(self.num_inner_iterations)}",
+ f" FPR.................. {fmt(self.last_problem_norm_fpr)}",
+ f" ALM infeasibility.... {fmt(self.f1_infeasibility)}",
+ f" PM infeasibility..... {fmt(self.f2_norm)}",
+ f" Decision variables... {fmt(self.solution)}",
+ f" Inputs............... {fmt(self.inputs)}",
+ f" States............... {fmt(self.states)}",
+ f" Lagrange multipliers. {fmt(self.lagrange_multipliers)}",
+ ])
+
+ __str__ = __repr__
diff --git a/open-codegen/opengen/tcp/optimizer_tcp_manager.py b/open-codegen/opengen/tcp/optimizer_tcp_manager.py
index 68df889f..77c2297c 100644
--- a/open-codegen/opengen/tcp/optimizer_tcp_manager.py
+++ b/open-codegen/opengen/tcp/optimizer_tcp_manager.py
@@ -11,6 +11,9 @@
from .solver_response import SolverResponse
from importlib.metadata import version
+# A bit of warning suppressing for the retry module
+logging.getLogger("retry").setLevel(logging.ERROR)
+
class OptimizerTcpManager:
"""Client for TCP interface of parametric optimizers
diff --git a/open-codegen/test/test.py b/open-codegen/test/test.py
index 21a7d13f..d56f3075 100644
--- a/open-codegen/test/test.py
+++ b/open-codegen/test/test.py
@@ -195,7 +195,8 @@ def setUpOnlyParametricF2(cls):
solver_config = og.config.SolverConfiguration() \
.with_tolerance(1e-6) \
.with_initial_tolerance(1e-4) \
- .with_delta_tolerance(1e-5)
+ .with_delta_tolerance(1e-5) \
+ .with_penalty_weight_update_factor(5)
og.builder.OpEnOptimizerBuilder(
problem, meta, build_config, solver_config).build()
@@ -548,6 +549,22 @@ def test_squared_norm(self):
y = fun([3, 4])
self.assertAlmostEqual(25., y, places=12)
+ def test_optimizer_meta_valid_version(self):
+ meta = og.config.OptimizerMeta().with_version("1.2.3-alpha.1+build.5")
+ self.assertEqual("1.2.3-alpha.1+build.5", meta.version)
+
+ def test_optimizer_meta_invalid_version1(self):
+ with self.assertRaises(ValueError) as context:
+ og.config.OptimizerMeta().with_version("^1.2")
+
+ self.assertIn("Cargo package version", str(context.exception))
+
+ def test_optimizer_meta_invalid_version2(self):
+ with self.assertRaises(ValueError) as context:
+ og.config.OptimizerMeta().with_version("0.1")
+
+ self.assertIn("Cargo package version", str(context.exception))
+
if __name__ == '__main__':
logging.getLogger('retry').setLevel(logging.ERROR)
diff --git a/open-codegen/test/test_ocp.py b/open-codegen/test/test_ocp.py
new file mode 100644
index 00000000..5407fbd8
--- /dev/null
+++ b/open-codegen/test/test_ocp.py
@@ -0,0 +1,737 @@
+import json
+import os
+import unittest
+
+import casadi.casadi as cs
+import opengen as og
+
+
+class DummySolverStatus:
+ def __init__(self, solution):
+ self.solution = solution
+ self.cost = 1.23
+ self.exit_status = "Converged"
+ self.solve_time_ms = 0.7
+ self.penalty = 12.5
+ self.num_outer_iterations = 3
+ self.num_inner_iterations = 27
+ self.last_problem_norm_fpr = 1e-6
+ self.f1_infeasibility = 2e-5
+ self.f2_norm = 3e-5
+ self.lagrange_multipliers = []
+
+
+class DummyDirectSolver:
+ def __init__(self, solution):
+ self.solution = solution
+ self.last_call = None
+
+ def run(self, p, initial_guess=None, initial_lagrange_multipliers=None, initial_penalty=None):
+ self.last_call = {
+ "p": p,
+ "initial_guess": initial_guess,
+ "initial_lagrange_multipliers": initial_lagrange_multipliers,
+ "initial_penalty": initial_penalty,
+ }
+ return DummySolverStatus(self.solution)
+
+
+class OcpTestCase(unittest.TestCase):
+ TEST_DIR = ".python_test_build_ocp"
+
+ @staticmethod
+ def get_open_local_absolute_path():
+ return os.path.abspath(os.path.join(os.path.dirname(__file__), "..", ".."))
+
+ @classmethod
+ def setUpOCP1(cls):
+ optimizer_name = "ocp_manifest_bindings"
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=3)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ ocp.add_parameter("q", 1, default=1)
+ ocp.add_parameter("r", 1, default=0.1)
+ ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(
+ lambda x, u, param, _t:
+ param["q"] * cs.dot(x - param["xref"], x - param["xref"]) + param["r"] * cs.dot(u, u)
+ )
+ ocp.with_terminal_cost(
+ lambda x, param: cs.dot(x - param["xref"], x - param["xref"])
+ )
+ ocp.with_input_constraints(og.constraints.Rectangle([-1.0], [1.0]))
+
+ cls.ocp1_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name(optimizer_name),
+ build_configuration=og.config.BuildConfiguration()
+ .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+ .with_build_directory(OcpTestCase.TEST_DIR)
+ .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+ .with_build_python_bindings(),
+ solver_configuration=og.config.SolverConfiguration()
+ .with_tolerance(1e-5)
+ .with_delta_tolerance(1e-5)
+ .with_initial_penalty(10.0)
+ .with_penalty_weight_update_factor(1.2)
+ .with_max_inner_iterations(500)
+ .with_max_outer_iterations(10),
+ ).build()
+ cls.ocp1_manifest_path = os.path.join(cls.ocp1_optimizer.target_dir, "optimizer_manifest.json")
+ cls.ocp1_rollout_path = os.path.join(cls.ocp1_optimizer.target_dir, "rollout.casadi")
+
+ @classmethod
+ def setUpOCP2(cls):
+ def make_problem(shooting):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=5, shooting=shooting)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ ocp.with_dynamics(lambda x, u, param:
+ cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(
+ lambda x, u, param, _t:
+ cs.dot(x - param["xref"], x - param["xref"]) + 0.01 * cs.dot(u, u)
+ )
+ ocp.with_terminal_cost(
+ lambda x, param:
+ 2.0 * cs.dot(x - param["xref"], x - param["xref"])
+ )
+ ocp.with_input_constraints(og.constraints.Rectangle([-0.4], [0.4]))
+ ocp.with_path_constraint(
+ lambda x, u, param, _t: cs.fmax(0.0, x[0] - 1.5)
+ )
+ if shooting == og.ocp.ShootingMethod.MULTIPLE:
+ ocp.with_dynamics_constraints("alm")
+ return ocp
+
+ solver_config = og.config.SolverConfiguration() \
+ .with_tolerance(1e-5) \
+ .with_delta_tolerance(1e-5) \
+ .with_initial_penalty(10.0) \
+ .with_penalty_weight_update_factor(1.2) \
+ .with_max_inner_iterations(5000) \
+ .with_max_outer_iterations(20)
+
+ cls.ocp2_single_optimizer = og.ocp.OCPBuilder(
+ make_problem(og.ocp.ShootingMethod.SINGLE),
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_single_tcp"),
+ build_configuration=og.config.BuildConfiguration()
+ .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+ .with_build_directory(OcpTestCase.TEST_DIR)
+ .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+ .with_tcp_interface_config(
+ tcp_interface_config=og.config.TcpServerConfiguration(bind_port=3391)
+ ),
+ solver_configuration=solver_config,
+ ).build()
+ cls.ocp2_multiple_optimizer = og.ocp.OCPBuilder(
+ make_problem(og.ocp.ShootingMethod.MULTIPLE),
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_multiple_tcp"),
+ build_configuration=og.config.BuildConfiguration()
+ .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+ .with_build_directory(OcpTestCase.TEST_DIR)
+ .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+ .with_tcp_interface_config(
+ tcp_interface_config=og.config.TcpServerConfiguration(bind_port=3392)
+ ),
+ solver_configuration=solver_config,
+ ).build()
+
+ cls.ocp2_single_manifest_path = os.path.join(
+ cls.ocp2_single_optimizer.target_dir, "optimizer_manifest.json")
+ cls.ocp2_multiple_manifest_path = os.path.join(
+ cls.ocp2_multiple_optimizer.target_dir, "optimizer_manifest.json")
+
+ @classmethod
+ def setUpOCP3(cls):
+ nx, nu, ts = 2, 1, 0.1
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: cs.vertcat(x[1], -x[0] + u[0]),
+ sampling_time=ts,
+ )
+
+ ocp = og.ocp.OptimalControlProblem(nx=nx, nu=nu, horizon=5)
+ ocp.add_parameter("x0", nx)
+ ocp.add_parameter("xref", nx, default=[0.0, 0.0])
+ ocp.with_dynamics(discretizer.multistep(method="rk4", num_steps=3))
+ ocp.with_stage_cost(
+ lambda xk, uk, param, _t: cs.dot(xk - param["xref"], xk - param["xref"]) + cs.dot(uk, uk)
+ )
+ ocp.with_terminal_cost(
+ lambda xk, param: cs.dot(xk - param["xref"], xk - param["xref"])
+ )
+ ocp.with_input_constraints(og.constraints.Rectangle([-1.0], [1.0]))
+
+ cls.ocp3_optimizer = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_multistep_rk4_bindings"),
+ build_configuration=og.config.BuildConfiguration()
+ .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+ .with_build_directory(OcpTestCase.TEST_DIR)
+ .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+ .with_build_python_bindings(),
+ solver_configuration=og.config.SolverConfiguration()
+ .with_tolerance(1e-5)
+ .with_delta_tolerance(1e-5)
+ .with_initial_penalty(10.0)
+ .with_penalty_weight_update_factor(1.2)
+ .with_max_inner_iterations(500)
+ .with_max_outer_iterations(10),
+ ).build()
+
+ @classmethod
+ def setUpClass(cls):
+ cls.setUpOCP1()
+ cls.setUpOCP2()
+ cls.setUpOCP3()
+
+ def make_ocp(self, shooting=og.ocp.ShootingMethod.SINGLE):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=3, shooting=shooting)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[1.0, -1.0])
+
+ def dynamics(x, u, param):
+ return cs.vertcat(x[0] + u[0], x[1] - u[0])
+
+ def stage_cost(x, u, param, _t):
+ err = x - param["xref"]
+ return cs.dot(err, err) + 0.5 * cs.dot(u, u)
+
+ def terminal_cost(x, param):
+ err = x - param["xref"]
+ return 2.0 * cs.dot(err, err)
+
+ ocp.with_dynamics(dynamics)
+ ocp.with_stage_cost(stage_cost)
+ ocp.with_terminal_cost(terminal_cost)
+ ocp.with_input_constraints(og.constraints.Rectangle([-2.0], [2.0]))
+ ocp.with_path_constraint(lambda x, u, param, _t: x[0] + u[0] - param["xref"][0])
+ return ocp
+
+ def test_parameter_defaults(self):
+ ocp = self.make_ocp()
+ packed = ocp.parameters.pack({"x0": [0.5, -0.25]})
+ self.assertEqual(packed, [0.5, -0.25, 1.0, -1.0])
+
+ def test_duplicate_parameters(self):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=3)
+ ocp.add_parameter("x0", 2)
+ with self.assertRaises(ValueError):
+ ocp.add_parameter("x0", 2)
+
+ def test_missing_parameter(self):
+ ocp = self.make_ocp()
+ with self.assertRaises(ValueError):
+ ocp.parameters.pack({"xref": [0.0, 0.0]})
+
+ def test_parameter_dimension_mismatch(self):
+ ocp = self.make_ocp()
+ with self.assertRaises(ValueError):
+ ocp.parameters.pack({"x0": [1.0], "xref": [0.0, 0.0]})
+
+ with self.assertRaises(ValueError):
+ ocp.parameters.pack({"x0": [1.0, 2.0], "xref": [0.0]})
+
+ def test_parameter_default_shapes(self):
+ pack = og.ocp.ParameterPack()
+ pack.add("alpha", 1, default=2.5)
+ pack.add("beta", 2, default=[1.0, -1.0])
+
+ self.assertEqual(pack.pack(), [2.5, 1.0, -1.0])
+
+ def test_invalid_default_dimension(self):
+ with self.assertRaises(ValueError):
+ og.ocp.ParameterDefinition("xref", 2, default=[0.0])
+
+ def test_matrix_weighted_terminal_cost(self):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=2)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(lambda x, u, param, _t: cs.dot(u, u))
+
+ p_matrix = cs.DM([
+ [10.0, 0.0],
+ [0.0, 2.0],
+ ])
+ ocp.with_terminal_cost(
+ lambda x, param: cs.mtimes([
+ (x - param["xref"]).T,
+ p_matrix,
+ (x - param["xref"]),
+ ])
+ )
+
+ model = ocp.build_symbolic_model()
+ cost_fun = cs.Function("terminal_cost_test", [model["u"], model["p"]], [model["cost"]])
+
+ value = float(cost_fun([0.0, 0.0], [1.0, -2.0, 0.0, 0.0]).full().reshape(-1)[0])
+ self.assertAlmostEqual(value, 18.0)
+
+ def test_discretizer_euler(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: -x + u,
+ sampling_time=0.1,
+ )
+ x = cs.SX.sym("x", 1)
+ u = cs.SX.sym("u", 1)
+ p = cs.SX.sym("p", 0)
+ discrete = discretizer.euler()
+ fun = cs.Function("euler_discrete", [x, u, p], [discrete(x, u, p)])
+
+ y = fun([2.0], [1.0], []).full().reshape(-1).tolist()
+ self.assertAlmostEqual(y[0], 1.9)
+
+ def test_discretizer_rk4(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: -x,
+ sampling_time=0.1,
+ )
+ x = cs.SX.sym("x", 1)
+ u = cs.SX.sym("u", 1)
+ p = cs.SX.sym("p", 0)
+ discrete = discretizer.rk4()
+ fun = cs.Function("rk4_discrete", [x, u, p], [discrete(x, u, p)])
+
+ y = fun([1.0], [0.0], []).full().reshape(-1).tolist()
+ self.assertAlmostEqual(y[0], 0.9048375, places=6)
+
+ def test_discretizer_midpoint(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: -x,
+ sampling_time=0.1,
+ )
+ x = cs.SX.sym("x", 1)
+ u = cs.SX.sym("u", 1)
+ p = cs.SX.sym("p", 0)
+ discrete = discretizer.midpoint()
+ fun = cs.Function("midpoint_discrete", [x, u, p], [discrete(x, u, p)])
+
+ y = fun([1.0], [0.0], []).full().reshape(-1).tolist()
+ self.assertAlmostEqual(y[0], 0.905)
+
+ def test_discretizer_heun(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: -x,
+ sampling_time=0.1,
+ )
+ x = cs.SX.sym("x", 1)
+ u = cs.SX.sym("u", 1)
+ p = cs.SX.sym("p", 0)
+ discrete = discretizer.heun()
+ fun = cs.Function("heun_discrete", [x, u, p], [discrete(x, u, p)])
+
+ y = fun([1.0], [0.0], []).full().reshape(-1).tolist()
+ self.assertAlmostEqual(y[0], 0.905)
+
+ def test_discretizer_multistep(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: -x,
+ sampling_time=0.1,
+ )
+ x = cs.SX.sym("x", 1)
+ u = cs.SX.sym("u", 1)
+ p = cs.SX.sym("p", 0)
+ discrete = discretizer.multistep(method="euler", num_steps=2)
+ fun = cs.Function("multistep_discrete", [x, u, p], [discrete(x, u, p)])
+
+ y = fun([1.0], [0.0], []).full().reshape(-1).tolist()
+ self.assertAlmostEqual(y[0], 0.9025)
+
+ def test_discretizer_invalid_multistep(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: x,
+ sampling_time=0.1,
+ )
+ with self.assertRaises(ValueError):
+ discretizer.multistep(method="euler", num_steps=0)
+
+ def test_multistep_rk4_codegen(self):
+ result = self.ocp3_optimizer.solve(
+ x0=[1.0, 0.0],
+ xref=[0.0, 0.0],
+ initial_guess=[0.0] * 5,
+ )
+
+ # Check the result has all necessary attributes
+ self.assertTrue(hasattr(result, "raw"))
+ self.assertTrue(hasattr(result, "solution"))
+ self.assertTrue(hasattr(result, "inputs"))
+ self.assertTrue(hasattr(result, "states"))
+ self.assertTrue(hasattr(result, "cost"))
+ self.assertTrue(hasattr(result, "exit_status"))
+ self.assertTrue(hasattr(result, "solve_time_ms"))
+ self.assertTrue(hasattr(result, "penalty"))
+ self.assertTrue(hasattr(result, "num_outer_iterations"))
+ self.assertTrue(hasattr(result, "num_inner_iterations"))
+ self.assertTrue(hasattr(result, "last_problem_norm_fpr"))
+ self.assertTrue(hasattr(result, "f1_infeasibility"))
+ self.assertTrue(hasattr(result, "f2_norm"))
+ self.assertTrue(hasattr(result, "lagrange_multipliers"))
+
+ # Check all attributes of `result`
+ self.assertEqual("Converged", result.exit_status)
+ self.assertIsInstance(result.solution, list)
+ self.assertIsInstance(result.inputs, list)
+ self.assertIsInstance(result.states, list)
+ self.assertEqual(5, len(result.solution))
+ self.assertEqual(5, len(result.inputs))
+ self.assertEqual(6, len(result.states))
+ self.assertIsInstance(result.cost, float)
+ self.assertGreaterEqual(result.cost, 0.0)
+ self.assertIsInstance(result.solve_time_ms, float)
+ self.assertGreater(result.solve_time_ms, 0.0)
+ self.assertIsInstance(result.penalty, float)
+ self.assertIsInstance(result.num_outer_iterations, int)
+ self.assertGreaterEqual(result.num_outer_iterations, 1)
+ self.assertIsInstance(result.num_inner_iterations, int)
+ self.assertGreaterEqual(result.num_inner_iterations, 1)
+ self.assertIsInstance(result.last_problem_norm_fpr, float)
+ self.assertGreaterEqual(result.last_problem_norm_fpr, 0.0)
+ self.assertIsInstance(result.f1_infeasibility, float)
+ self.assertGreaterEqual(result.f1_infeasibility, 0.0)
+ self.assertIsInstance(result.f2_norm, float)
+ self.assertGreaterEqual(result.f2_norm, 0.0)
+ self.assertIsInstance(result.lagrange_multipliers, list)
+ self.assertAlmostEqual(result.states[0][0], 1.0)
+ self.assertAlmostEqual(result.states[0][1], 0.0)
+
+ def test_discretizer_invalid_method(self):
+ discretizer = og.ocp.DynamicsDiscretizer(
+ lambda x, u, param: x,
+ sampling_time=0.1,
+ )
+ with self.assertRaises(ValueError):
+ discretizer.discretize("unknown")
+
+ def test_single_shooting_hard_constraints(self):
+ ocp_stage = self.make_ocp()
+ ocp_stage.with_hard_stage_state_input_constraints(
+ og.constraints.Rectangle([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0])
+ )
+ with self.assertRaises(ValueError):
+ ocp_stage.build_symbolic_model()
+
+ ocp_terminal = self.make_ocp()
+ ocp_terminal.with_hard_terminal_state_constraints(
+ og.constraints.Rectangle([-1.0, -1.0], [1.0, 1.0])
+ )
+ with self.assertRaises(ValueError):
+ ocp_terminal.build_symbolic_model()
+
+ def test_lowering_penalty_constraints(self):
+ ocp = self.make_ocp()
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_test"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertEqual(low_level.dim_decision_variables(), 3)
+ self.assertEqual(low_level.dim_parameters(), 4)
+ self.assertEqual(low_level.dim_constraints_penalty(), 3)
+ self.assertIsInstance(low_level.constraints, og.constraints.Rectangle)
+ self.assertEqual(low_level.constraints.xmin, [-2.0, -2.0, -2.0])
+ self.assertEqual(low_level.constraints.xmax, [2.0, 2.0, 2.0])
+
+ def test_lowering_alm_constraints(self):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=3)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(lambda x, u, param, _t: cs.dot(x - param["xref"], x - param["xref"]) + cs.dot(u, u))
+ ocp.with_terminal_cost(lambda x, param: cs.dot(x - param["xref"], x - param["xref"]))
+ ocp.with_path_constraint(
+ lambda x, u, param, _t: x[0] + u[0],
+ kind="alm",
+ set_c=og.constraints.Ball2(None, 5.),
+ )
+ ocp.with_terminal_constraint(
+ lambda x, param: x[1] - 0.5,
+ kind="alm",
+ set_c=og.constraints.Ball2(None, 0.1),
+ )
+ ocp.with_terminal_constraint(
+ lambda x, param: x[0] + x[1] - 2.0,
+ kind="penalty",
+ )
+
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_alm_test"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertEqual(low_level.dim_constraints_aug_lagrangian(), 4)
+ self.assertEqual(low_level.dim_constraints_penalty(), 1)
+ self.assertIsInstance(low_level.alm_set_c, og.constraints.CartesianProduct)
+ self.assertEqual(low_level.alm_set_c.segments, [0, 1, 2, 3])
+
+ def test_lowering_alm_rectangle_constraints(self):
+ ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=3)
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xmin", 1, default=[-0.5])
+ ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(lambda x, u, param, _t: cs.dot(x, x) + cs.dot(u, u))
+ ocp.with_terminal_cost(lambda x, param: cs.dot(x, x))
+ ocp.with_path_constraint(
+ lambda x, u, param, _t: x[1] - param["xmin"],
+ kind="alm",
+ set_c=og.constraints.Rectangle([0.0], [float("inf")]),
+ )
+
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_alm_rect_test"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertEqual(low_level.dim_constraints_aug_lagrangian(), 3)
+ self.assertIsInstance(low_level.alm_set_c, og.constraints.Rectangle)
+ self.assertEqual(low_level.alm_set_c.xmin, [0.0, 0.0, 0.0])
+ self.assertEqual(low_level.alm_set_c.xmax, [float("inf"), float("inf"), float("inf")])
+
+ def test_generated_optimizer_defaults(self):
+ ocp = self.make_ocp()
+ backend = DummyDirectSolver(solution=[0.1, 0.2, 0.3])
+ optimizer = og.ocp.GeneratedOptimizer(
+ ocp=ocp,
+ optimizer_name="dummy",
+ target_dir=".",
+ backend=backend,
+ backend_kind="direct",
+ )
+
+ result = optimizer.solve(x0=[0.0, 0.0])
+ # print(result)
+
+ self.assertEqual(backend.last_call["p"], [0.0, 0.0, 1.0, -1.0])
+ self.assertEqual(result.inputs, [[0.1], [0.2], [0.3]])
+ self.assertEqual(result.states[0], [0.0, 0.0])
+ self.assertAlmostEqual(result.states[-1][0], 0.6)
+ self.assertAlmostEqual(result.states[-1][1], -0.6)
+ self.assertEqual(result.penalty, 12.5)
+ self.assertEqual(result.num_outer_iterations, 3)
+ self.assertEqual(result.num_inner_iterations, 27)
+ self.assertEqual(result.last_problem_norm_fpr, 1e-6)
+ self.assertEqual(result.f1_infeasibility, 2e-5)
+ self.assertEqual(result.f2_norm, 3e-5)
+
+ def test_optimizer_manifest_roundtrip(self):
+ manifest_path = self.ocp1_manifest_path
+ rollout_path = self.ocp1_rollout_path
+
+ self.assertTrue(os.path.exists(manifest_path))
+ self.assertTrue(os.path.exists(rollout_path))
+
+ with open(manifest_path, "r") as fh:
+ manifest = json.load(fh)
+
+ self.assertEqual(1, manifest["manifest_version"])
+ self.assertEqual("ocp_manifest_bindings", manifest["optimizer_name"])
+ self.assertEqual("direct", manifest["backend_kind"])
+ self.assertEqual("single", manifest["shooting"])
+ self.assertEqual(2, manifest["nx"])
+ self.assertEqual(1, manifest["nu"])
+ self.assertEqual(3, manifest["horizon"])
+ self.assertEqual(3, manifest["decision_dimension"])
+ self.assertEqual(6, manifest["parameter_dimension"]) # p = (x0, xref, q, r)
+ self.assertEqual("rollout.casadi", manifest["rollout_file"])
+ self.assertIsNotNone(manifest["rollout_sha256"])
+ self.assertIn("created_at", manifest)
+
+ loaded_optimizer = og.ocp.GeneratedOptimizer.load(manifest_path)
+
+ result = loaded_optimizer.solve(
+ x0=[1.0, 0.0]
+ )
+
+ self.assertEqual("ocp_manifest_bindings", loaded_optimizer.optimizer_name)
+ self.assertEqual("direct", loaded_optimizer.backend_kind)
+ self.assertEqual("Converged", result.exit_status)
+ self.assertEqual(3, len(result.inputs))
+ self.assertEqual(4, len(result.states))
+
+ def test_multiple_shooting_penalty(self):
+ ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_ms_test"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertEqual(low_level.dim_decision_variables(), 9)
+ self.assertEqual(low_level.dim_parameters(), 4)
+ self.assertEqual(low_level.dim_constraints_penalty(), 9)
+ self.assertIsInstance(low_level.constraints, og.constraints.Rectangle)
+ self.assertEqual(low_level.constraints.xmin, [-2.0, float("-inf"), float("-inf"),
+ -2.0, float("-inf"), float("-inf"),
+ -2.0, float("-inf"), float("-inf")])
+ self.assertEqual(low_level.constraints.xmax, [2.0, float("inf"), float("inf"),
+ 2.0, float("inf"), float("inf"),
+ 2.0, float("inf"), float("inf")])
+
+ def test_multiple_shooting_alm(self):
+ ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
+ ocp.with_dynamics_constraints("alm")
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_ms_alm_test"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertEqual(low_level.dim_decision_variables(), 9)
+ self.assertEqual(low_level.dim_constraints_penalty(), 3)
+ self.assertEqual(low_level.dim_constraints_aug_lagrangian(), 6)
+ self.assertIsInstance(low_level.alm_set_c, og.constraints.Zero)
+ self.assertIsInstance(low_level.alm_set_y, og.constraints.BallInf)
+
+ def test_multiple_shooting_terminal_constraints(self):
+ ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
+ ocp.with_hard_terminal_state_constraints(
+ og.constraints.Rectangle([-10.0, -10.0], [10.0, 10.0])
+ )
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_ms_terminal_hard"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertIsInstance(low_level.constraints, og.constraints.Rectangle)
+ self.assertEqual(low_level.constraints.xmin, [-2.0, float("-inf"), float("-inf"),
+ -2.0, float("-inf"), float("-inf"),
+ -2.0, -10.0, -10.0])
+ self.assertEqual(low_level.constraints.xmax, [2.0, float("inf"), float("inf"),
+ 2.0, float("inf"), float("inf"),
+ 2.0, 10.0, 10.0])
+
+ def test_multiple_shooting_zero_constraints(self):
+ ocp = og.ocp.OptimalControlProblem(
+ nx=2,
+ nu=1,
+ horizon=3,
+ shooting=og.ocp.ShootingMethod.MULTIPLE,
+ )
+ ocp.add_parameter("x0", 2)
+ ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+ ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+ ocp.with_stage_cost(lambda x, u, param, _t: cs.dot(x - param["xref"], x - param["xref"]))
+ ocp.with_terminal_cost(lambda x, param: cs.dot(x - param["xref"], x - param["xref"]))
+ ocp.with_hard_stage_state_input_constraints(og.constraints.Zero())
+
+ builder = og.ocp.OCPBuilder(
+ ocp,
+ metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_ms_zero"),
+ )
+ low_level = builder.build_problem()
+
+ self.assertIsInstance(low_level.constraints, og.constraints.Zero)
+
+ def test_multiple_shooting_state_extraction(self):
+ ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
+ backend = DummyDirectSolver(solution=[
+ 0.1, 0.1, -0.1,
+ 0.2, 0.3, -0.3,
+ 0.4, 0.7, -0.7,
+ ])
+ optimizer = og.ocp.GeneratedOptimizer(
+ ocp=ocp,
+ optimizer_name="dummy_ms",
+ target_dir=".",
+ backend=backend,
+ backend_kind="direct",
+ )
+
+ result = optimizer.solve(x0=[0.0, 0.0])
+
+ self.assertEqual(result.inputs, [[0.1], [0.2], [0.4]])
+ self.assertEqual(result.states, [[0.0, 0.0], [0.1, -0.1], [0.3, -0.3], [0.7, -0.7]])
+
+ def test_tcp_optimizer_result(self):
+ optimizer = og.ocp.GeneratedOptimizer.load(self.ocp2_single_manifest_path)
+
+ try:
+ result = optimizer.solve(
+ x0=[1.0, -1.0],
+ xref=[0.0, 0.0],
+ )
+
+ self.assertTrue(hasattr(result, "raw"))
+ self.assertTrue(hasattr(result, "solution"))
+ self.assertTrue(hasattr(result, "inputs"))
+ self.assertTrue(hasattr(result, "states"))
+ self.assertTrue(hasattr(result, "cost"))
+ self.assertTrue(hasattr(result, "exit_status"))
+ self.assertTrue(hasattr(result, "solve_time_ms"))
+ self.assertTrue(hasattr(result, "penalty"))
+ self.assertTrue(hasattr(result, "num_outer_iterations"))
+ self.assertTrue(hasattr(result, "num_inner_iterations"))
+ self.assertTrue(hasattr(result, "last_problem_norm_fpr"))
+ self.assertTrue(hasattr(result, "f1_infeasibility"))
+ self.assertTrue(hasattr(result, "f2_norm"))
+ self.assertTrue(hasattr(result, "lagrange_multipliers"))
+
+ self.assertEqual("Converged", result.exit_status)
+ self.assertIsInstance(result.solution, list)
+ self.assertIsInstance(result.inputs, list)
+ self.assertIsInstance(result.states, list)
+ self.assertEqual(5, len(result.inputs))
+ self.assertEqual(6, len(result.states))
+ self.assertIsInstance(result.cost, float)
+ self.assertGreaterEqual(result.cost, 0.0)
+ self.assertIsInstance(result.solve_time_ms, float)
+ self.assertGreater(result.solve_time_ms, 0.0)
+ self.assertIsInstance(result.penalty, float)
+ self.assertGreater(result.penalty, 0.0)
+ self.assertIsInstance(result.num_outer_iterations, int)
+ self.assertGreaterEqual(result.num_outer_iterations, 1)
+ self.assertIsInstance(result.num_inner_iterations, int)
+ self.assertGreaterEqual(result.num_inner_iterations, 1)
+ self.assertIsInstance(result.last_problem_norm_fpr, float)
+ self.assertGreaterEqual(result.last_problem_norm_fpr, 0.0)
+ self.assertIsInstance(result.f1_infeasibility, float)
+ self.assertGreaterEqual(result.f1_infeasibility, 0.0)
+ self.assertIsInstance(result.f2_norm, float)
+ self.assertGreaterEqual(result.f2_norm, 0.0)
+ self.assertIsInstance(result.lagrange_multipliers, list)
+ self.assertAlmostEqual(result.states[0][0], 1.0)
+ self.assertAlmostEqual(result.states[0][1], -1.0)
+ finally:
+ optimizer.kill()
+
+ def test_single_vs_multiple(self):
+ single_optimizer = og.ocp.GeneratedOptimizer.load(self.ocp2_single_manifest_path)
+ multiple_optimizer = og.ocp.GeneratedOptimizer.load(self.ocp2_multiple_manifest_path)
+
+ try:
+ x0 = [1.0, -1.0]
+ xref = [0.0, 0.0]
+ single_result = single_optimizer.solve(x0=x0, xref=xref)
+ multiple_result = multiple_optimizer.solve(x0=x0, xref=xref)
+ # print("SINGLE\n------------")
+ # print(single_result)
+ # print("MULTIPLE\n------------")
+ # print(multiple_result)
+
+ self.assertEqual("Converged", single_result.exit_status)
+ self.assertEqual("Converged", multiple_result.exit_status)
+ self.assertEqual(5, len(single_result.inputs))
+ self.assertEqual(5, len(multiple_result.inputs))
+ self.assertEqual(6, len(single_result.states))
+ self.assertEqual(6, len(multiple_result.states))
+
+ for u_single, u_multiple in zip(single_result.inputs, multiple_result.inputs):
+ self.assertAlmostEqual(u_single[0], u_multiple[0], delta=1e-3)
+
+ for x_single, x_multiple in zip(single_result.states, multiple_result.states):
+ self.assertAlmostEqual(x_single[0], x_multiple[0], delta=1e-3)
+ self.assertAlmostEqual(x_single[1], x_multiple[1], delta=1e-3)
+
+ self.assertAlmostEqual(single_result.cost, multiple_result.cost, delta=1e-3)
+ finally:
+ single_optimizer.kill()
+ multiple_optimizer.kill()
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/test/icasadi_test/.gitignore b/test/icasadi_test/.gitignore
deleted file mode 100644
index 046d2998..00000000
--- a/test/icasadi_test/.gitignore
+++ /dev/null
@@ -1,7 +0,0 @@
-/target
-**/*.rs.bk
-Cargo.lock
-*.o
-*.a
-icasadirunner
-*~
diff --git a/test/icasadi_test/extern/Makefile b/test/icasadi_test/extern/Makefile
deleted file mode 100644
index 1a308308..00000000
--- a/test/icasadi_test/extern/Makefile
+++ /dev/null
@@ -1,18 +0,0 @@
-all:
- gcc -Wall -fPIC -c -o auto_casadi_cost.o auto_casadi_cost.c;
- gcc -Wall -fPIC -c -o auto_casadi_grad.o auto_casadi_grad.c;
- gcc -Wall -fPIC -c -o icasadi.o icasadi.c;
- gcc -Wall -fPIC -c -o auto_casadi_constraints_type_penalty.o auto_casadi_constraints_type_penalty.c;
- gcc -Wall -o icasadirunner main.c auto_casadi_grad.o auto_casadi_cost.o auto_casadi_constraints_type_penalty.o icasadi.o -lm;
-
-
-run: all
- ./icasadirunner
-
-
-clean:
- rm -rf *.o
- rm -f ./icasadirunner
-
-
-.PHONY: run, clean, all, icasadirunner
diff --git a/test/icasadi_test/extern/icasadi.c b/test/icasadi_test/extern/icasadi.c
deleted file mode 100644
index 892693d6..00000000
--- a/test/icasadi_test/extern/icasadi.c
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "icasadi.h"
-
-int icasadi_cost_(
- const double *u,
- const double *casadi_static_params,
- double *cost_value)
-{
- const double *casadi_arguments[2];
- casadi_arguments[0] = u;
- casadi_arguments[1] = casadi_static_params;
- return CASADI_COST_NAME(casadi_arguments, &cost_value, 0, 0, 0);
-}
-
-int icasadi_grad_(
- const double *u,
- const double *casadi_static_params,
- double *gradient)
-{
- const double *casadi_arguments[2];
- casadi_arguments[0] = u;
- casadi_arguments[1] = casadi_static_params;
- return CASADI_GRAD_NAME(casadi_arguments, &gradient, 0, 0, 0);
-}
-
-int icasadi_constraints_as_penalty_(
- const double *u,
- const double *casadi_static_params,
- double *constraints)
-{
- const double *casadi_arguments[2];
- casadi_arguments[0] = u;
- casadi_arguments[1] = casadi_static_params;
- return CASADI_CONSTRAINTS_AS_PENALTY_NAME(casadi_arguments, &constraints, 0, 0, 0);
-}
diff --git a/test/icasadi_test/extern/icasadi.h b/test/icasadi_test/extern/icasadi.h
deleted file mode 100644
index e32e1a5a..00000000
--- a/test/icasadi_test/extern/icasadi.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef ICASADI_HEADER_SENTINEL
-#define ICASADI_HEADER_SENTINEL
-
-#include "icasadi_config.h"
-
-extern int CASADI_COST_NAME(
- const double **arg,
- double **casadi_results,
- long long int *iw,
- double *w,
- void *mem);
-
-extern int CASADI_GRAD_NAME(
- const double **arg,
- double **casadi_results,
- long long int *iw,
- double *w,
- void *mem);
-
-extern int CASADI_CONSTRAINTS_AS_PENALTY_NAME(
- const double **arg,
- double **res,
- long long int *iw,
- double *w,
- void *mem);
-
-int icasadi_cost_(
- const double *u,
- const double *casadi_static_params,
- double *cost_value);
-
-int icasadi_grad_(
- const double *u,
- const double *casadi_static_params,
- double *gradient);
-
-int icasadi_constraints_as_penalty_(
- const double *u,
- const double *casadi_static_params,
- double *constraints);
-
-#endif
diff --git a/test/icasadi_test/extern/main.c b/test/icasadi_test/extern/main.c
deleted file mode 100644
index a3c3f8e3..00000000
--- a/test/icasadi_test/extern/main.c
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "icasadi.h"
-#include
-
-int main() {
- // long long int n = phi_n_in();
- double u[CASADI_NU] = {1.0, 2.0, 3.0, -5.0, 1.0};
- double p[CASADI_NP] = {1.0,-1.0,3.0};
-
- double phival = 0;
- double cost_jacobian[CASADI_NU] = {0};
- double c[CASADI_NUM_CONSTAINTS_TYPE_PENALTY] = {0};
-
- icasadi_cost_(u, p, &phival);
- icasadi_grad_(u, p, cost_jacobian);
- icasadi_constraints_as_penalty_(u, p, c);
-
- printf("cost value = %g\n", phival);
- printf("jacobian of cost =\n");
- printf("[\n");
- int i;
- for (i = 0; i < CASADI_NU; ++i){
- printf(" %g\n", cost_jacobian[i]);
- }
- printf("]\n\n");
-
- printf("c(x) =\n");
- printf("[\n");
- for (i = 0; i < CASADI_NUM_CONSTAINTS_TYPE_PENALTY; ++i){
- printf(" %g\n", c[i]);
- }
- printf("]\n");
-
-
- return 0;
-}
\ No newline at end of file
diff --git a/test/icasadi_test/src/lib.rs b/test/icasadi_test/src/lib.rs
deleted file mode 100644
index e07fe37b..00000000
--- a/test/icasadi_test/src/lib.rs
+++ /dev/null
@@ -1,128 +0,0 @@
-//! # CasADi Rust interface
-//!
-//! This is a Rust interface to C functions generated by Rust
-//!
-//! This is a `no-std` library (however, mind that the CasADi-generated code
-//! requires `libm` to call functions such as `sqrt`, `sin`, etc...)
-
-#![no_std]
-
-// Get the CasADi problem size and parameter size and re-export them, this is necessary to have
-// them available as compile time constants
-include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
-
-/// Number of static parameters
-pub use CASADI_NP as NUM_STATIC_PARAMETERS;
-
-/// Number of decision variables
-pub use CASADI_NU as NUM_DECISION_VARIABLES;
-
-use libc::{c_double, c_int};
-
-extern "C" {
- fn icasadi_cost_(
- u: *const c_double,
- casadi_static_params: *const c_double,
- cost_value: *mut c_double,
- ) -> c_int;
-
- fn icasadi_grad_(
- u: *const c_double,
- casadi_static_params: *const c_double,
- cost_jacobian: *mut c_double,
- ) -> c_int;
-
- fn icasadi_constraints_as_penalty_(
- u: *const c_double,
- casadi_static_params: *const c_double,
- constraints_as_penalty: *mut c_double,
- ) -> c_int;
-
-}
-
-///
-/// Consume the cost function written in C
-///
-/// # Example
-/// ```
-/// fn tst_call_casadi_cost() {
-/// let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
-/// let p = [1.0, -1.0];
-/// let mut cost_value = 0.0;
-/// icasadi::icasadi_cost(&u, &p, &mut cost_value);
-/// }
-/// ```
-///
-/// # Panics
-/// This method does not panic (on purpose). However, users need to be
-/// careful when providing the arguments `u` and `casadi_static_params`
-/// as they must be arrays of appropriate size.
-///
-/// As a safety measure, you may check whether
-///
-/// - `u.len() >= NUM_DECISION_VARIABLES`
-/// - `casadi_static_params.len() >= NUM_STATIC_PARAMETERS`
-///
-pub fn icasadi_cost(u: &[f64], casadi_static_params: &[f64], cost_value: &mut f64) -> c_int {
- unsafe { icasadi_cost_(u.as_ptr(), casadi_static_params.as_ptr(), cost_value) }
-}
-
-///
-/// Consume the Jacobian function written in C
-///
-/// # Example
-/// ```
-/// fn tst_call_casadi_cost() {
-/// let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
-/// let p = [1.0, -1.0];
-/// let mut jac = [0.0; 10];
-/// icasadi::icasadi_grad(&u, &p, &mut jac);
-/// }
-/// ```
-///
-/// # Panics
-/// This method does not panic (on purpose). However, users need to be
-/// careful when providing the arguments `u` and `casadi_static_params`
-/// as they must be arrays of appropriate size.
-///
-/// As a safety measure, you may check whether
-///
-/// - `u.len() >= icasadi::num_decision_variables()`
-/// - `casadi_static_params.len() >= icasadi::num_static_parameters()`
-/// - `cost_jacobian.len() >= icasadi::num_decision_variables()`
-///
-pub fn icasadi_grad(u: &[f64], casadi_static_params: &[f64], cost_jacobian: &mut [f64]) -> c_int {
- unsafe {
- icasadi_grad_(
- u.as_ptr(),
- casadi_static_params.as_ptr(),
- cost_jacobian.as_mut_ptr(),
- )
- }
-}
-
-pub fn icasadi_constraints_as_penalty(u: &[f64], casadi_static_params: &[f64], constraints_as_penalty: &mut [f64]) -> c_int {
- unsafe {
- icasadi_constraints_as_penalty_(
- u.as_ptr(),
- casadi_static_params.as_ptr(),
- constraints_as_penalty.as_mut_ptr(),
- )
- }
-}
-
-#[cfg(test)]
-mod tests {
- use super::*;
-
- #[test]
- fn tst_num_static() {
- let _np = NUM_STATIC_PARAMETERS;
- }
-
- #[test]
- fn tst_num_decision_var() {
- let _nu = NUM_DECISION_VARIABLES;
- }
-
-}
diff --git a/website/sidebars.json b/website/sidebars.json
index 68f637c4..dd3e3973 100644
--- a/website/sidebars.json
+++ b/website/sidebars.json
@@ -6,22 +6,28 @@
],
"Python": [
"python-interface",
- "python-advanced",
+ "python-advanced",
"python-c",
"python-bindings",
"python-tcp-ip",
"python-ros",
"python-examples"
],
- "MATLAB": [
- "matlab-interface",
- "matlab-examples"
- ],
+ "Optimal Control": [
+ "python-ocp-1",
+ "python-ocp-2",
+ "python-ocp-3",
+ "python-ocp-4"
+ ],
"Rust": [
"openrust-basic",
"openrust-alm",
"openrust-features"
],
+ "MATLAB": [
+ "matlab-interface",
+ "matlab-examples"
+ ],
"Docker": [
"docker"
],
diff --git a/website/static/img/ocp-inputs.png b/website/static/img/ocp-inputs.png
new file mode 100644
index 00000000..b914f3fd
Binary files /dev/null and b/website/static/img/ocp-inputs.png differ
diff --git a/website/static/img/ocp-states.png b/website/static/img/ocp-states.png
new file mode 100644
index 00000000..719beccf
Binary files /dev/null and b/website/static/img/ocp-states.png differ
+ 
-
@@ -98,18 +69,71 @@ OpEn can run on embedded devices; here we see it running on an intel Atom for th
-## Parametric Problems
+## Optimal Control
+
+Optimal control problems can now be set up directly from their natural ingredients: dynamics, prediction horizon, stage cost, terminal cost, and constraints. You can easily choose between a single or multiple shooting formulation. The solver is parametrizable, so you don't have to recompile whenever a parameter changes.
+
+OpEn allows to solve optimal control problems of the form
+
+
+
+
+
+```python
+import opengen as og
+import casadi.casadi as cs
+
+ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=20)
+ocp.add_parameter("x0", 2)
+ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+
+ocp.with_dynamics(
+ lambda x, u, p:
+ cs.vertcat(x[1], -x[0] + u[0]))
+
+ocp.with_stage_cost(
+ lambda x, u, p, t:
+ cs.dot(x - p["xref"], x - p["xref"])
+ + 0.1 * cs.dot(u, u))
+
+ocp.with_terminal_cost(
+ lambda x, p:
+ 10.0 * cs.dot(x - p["xref"], x - p["xref"]))
+
+ocp.with_input_constraints(og.constraints.BallInf(radius=1.))
+
+optimizer = og.ocp.OCPBuilder(
+ ocp,
+ meta=...,
+ build_config=...,
+ solver_config=...).build()
+
+result = optimizer.solve(x0=[1.0, 0.0], xref=[0.0, 0.0])
+```
+
+See the dedicated [OCP documentation](https://alphaville.github.io/optimization-engine/docs/python-ocp-1) for more details and examples.
+
+## General Parametric Optimization
**OpEn** can solve nonconvex parametric optimization problems of the general form
-
+
- 
- 
+
+and the corresponding sequence of states (`result.states`)
+
+
+
+The object `result` contains the above sequences of inputs and states and additional
+information about the solution, solver time, Lagrange multipliers, etc.
diff --git a/docs/python-ocp-2.md b/docs/python-ocp-2.md
new file mode 100644
index 00000000..d2c2a0e0
--- /dev/null
+++ b/docs/python-ocp-2.md
@@ -0,0 +1,508 @@
+---
+id: python-ocp-2
+title: Constructing OCPs
+description: Optimal Control with OpEn/opengen
+---
+
+
+
+
+
+
+