diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index b628193c..55c96f64 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -26,6 +26,11 @@ _ICASADI_PREFIX = 'icasadi_' _ROS_PREFIX = 'ros_node_' +# Template files +_OPTIMIZER_RS = "optimizer.rs.jinja" +_OPTIMIZER_CARGO_TOML = "optimizer_cargo.toml.jinja" +_OPTIMIZER_BUILD_RS = "optimizer_build.rs.jinja" + def make_dir_if_not_exists(directory): if not os.path.exists(directory): @@ -243,7 +248,7 @@ def __generate_cargo_toml(self): self.__logger.info("Generating Cargo.toml for target optimizer") target_dir = self.__target_dir() cargo_template = OpEnOptimizerBuilder.__get_template( - 'optimizer_cargo.toml') + _OPTIMIZER_CARGO_TOML) cargo_output_template = cargo_template.render( meta=self.__meta, build_config=self.__build_config, @@ -540,7 +545,7 @@ def __generate_main_project_code(self): "Generating main code for target optimizer (lib.rs)") target_dir = self.__target_dir() optimizer_rs_template = OpEnOptimizerBuilder.__get_template( - 'optimizer.rs') + _OPTIMIZER_RS) optimizer_rs_output_template = optimizer_rs_template.render( solver_config=self.__solver_config, meta=self.__meta, @@ -557,7 +562,7 @@ def __generate_build_rs(self): self.__logger.info("Generating build.rs for target optimizer") target_dir = self.__target_dir() build_rs_template = OpEnOptimizerBuilder.__get_template( - 'optimizer_build.rs') + _OPTIMIZER_BUILD_RS) build_rs_output_template = build_rs_template.render( meta=self.__meta, activate_clib_generation=self.__build_config.build_c_bindings) diff --git a/open-codegen/opengen/constraints/__init__.py b/open-codegen/opengen/constraints/__init__.py index 4fbf168b..e361c496 100644 --- a/open-codegen/opengen/constraints/__init__.py +++ b/open-codegen/opengen/constraints/__init__.py @@ -11,3 +11,4 @@ from .finite_set import * from .halfspace import * from .simplex import * +from .affine_space import * diff --git a/open-codegen/opengen/constraints/affine_space.py b/open-codegen/opengen/constraints/affine_space.py new file mode 100644 index 00000000..28879e23 --- /dev/null +++ b/open-codegen/opengen/constraints/affine_space.py @@ -0,0 +1,40 @@ +import casadi.casadi as cs +import numpy as np +from .constraint import Constraint +import opengen.functions as fn + + +class AffineSpace(Constraint): + """An affine constraint + + A constraint of the form :math:`Ax = b`, where :math:`A` and :math:`b` are + a matrix and a vector of appropriate dimensions + """ + + def __init__(self, A, b): + """Constructor for an affine space + + :return: new instance of AffineSpace + """ + self.__A = A.flatten('C') + self.__b = b + + @property + def matrix_a(self): + return self.__A + + @property + def vector_b(self): + return self.__b + + def distance_squared(self, u): + raise NotImplementedError() + + def project(self, u): + raise NotImplementedError() + + def is_convex(self): + return True + + def is_compact(self): + return False diff --git a/open-codegen/opengen/tcp/solver_status.py b/open-codegen/opengen/tcp/solver_status.py index 59dffa39..7267dabc 100644 --- a/open-codegen/opengen/tcp/solver_status.py +++ b/open-codegen/opengen/tcp/solver_status.py @@ -115,3 +115,15 @@ def cost(self): :return: Value of cost function at the solution """ return self.__dict__["__cost"] + + def __repr__(self): + return "Solver Status Report:\n" + \ + f"Exit status....... {self.exit_status}\n" + \ + f"Num Outer Iters... {self.num_outer_iterations}\n" + \ + f"Num Inner Iters... {self.num_inner_iterations}\n" + \ + f"FPR............... {self.last_problem_norm_fpr}\n" + \ + "Infeasibility\n" + \ + f" L F1............ {self.f1_infeasibility}\n" + \ + f" L Fw............ {self.f2_norm}\n" + \ + f"Penalty............ {self.penalty}\n" + \ + f"Time............... {self.solve_time_ms} ms\n" diff --git a/open-codegen/opengen/templates/c/optimizer_cinterface.rs b/open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja similarity index 100% rename from open-codegen/opengen/templates/c/optimizer_cinterface.rs rename to open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja diff --git a/open-codegen/opengen/templates/icasadi/icasadi_lib.rs b/open-codegen/opengen/templates/icasadi/icasadi_lib.rs index 3de21def..efd30433 100644 --- a/open-codegen/opengen/templates/icasadi/icasadi_lib.rs +++ b/open-codegen/opengen/templates/icasadi/icasadi_lib.rs @@ -268,7 +268,7 @@ pub fn precondition( /// Computes the initial penalty /// -/// Make sure that you have called init_ +/// Make sure that you have called init_{{ meta.optimizer_name }} pub fn initial_penalty( u: &[f64], static_params: &[f64], diff --git a/open-codegen/opengen/templates/optimizer.rs b/open-codegen/opengen/templates/optimizer.rs.jinja similarity index 96% rename from open-codegen/opengen/templates/optimizer.rs rename to open-codegen/opengen/templates/optimizer.rs.jinja index 912784db..3624bbe6 100644 --- a/open-codegen/opengen/templates/optimizer.rs +++ b/open-codegen/opengen/templates/optimizer.rs.jinja @@ -61,17 +61,18 @@ pub const {{meta.optimizer_name|upper}}_N1: usize = {{problem.dim_constraints_au /// Number of penalty constraints pub const {{meta.optimizer_name|upper}}_N2: usize = {{problem.dim_constraints_penalty() or 0}}; -{% include "c/optimizer_cinterface.rs" %} +{% include "c/optimizer_cinterface.rs.jinja" %} // ---Parameters of the constraints---------------------------------------------------------------------- - +{# CASE I: Ball* or Sphere #} {% if 'Ball1' == problem.constraints.__class__.__name__ or 'Ball2' == problem.constraints.__class__.__name__ or 'BallInf' == problem.constraints.__class__.__name__ or 'Sphere2' == problem.constraints.__class__.__name__ -%} /// Constraints: Centre of Ball const CONSTRAINTS_BALL_XC: Option<&[f64]> = {% if problem.constraints.center is not none %}Some(&[{{problem.constraints.center | join(', ')}}]){% else %}None{% endif %}; - /// Constraints: Radius of Ball const CONSTRAINTS_BALL_RADIUS : f64 = {{problem.constraints.radius}}; -{% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} +{% endif %} +{# CASE II: Rectangle #} +{% if 'Rectangle' == problem.constraints.__class__.__name__ -%} const CONSTRAINTS_XMIN :Option<&[f64]> = {% if problem.constraints.xmin is not none %}Some(&[ {%- for xmini in problem.constraints.xmin -%} {%- if float('-inf') == xmini -%}std::f64::NEG_INFINITY{%- else -%}{{xmini}}{%- endif -%}, @@ -85,7 +86,6 @@ const CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.constraints.xmax is not n {% endif %} - {% if problem.alm_set_c is not none %} // ---Parameters of ALM-type constraints (Set C)--------------------------------------------------------- {% if 'Ball2' == problem.alm_set_c.__class__.__name__ or 'BallInf' == problem.alm_set_c.__class__.__name__ -%} @@ -150,6 +150,10 @@ fn make_constraints() -> impl Constraint { {% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} // - Rectangle: Rectangle::new(CONSTRAINTS_XMIN, CONSTRAINTS_XMAX) + {% elif 'AffineSpace' == problem.constraints.__class__.__name__ -%} + let constraints_affine_a = vec![{{problem.constraints.matrix_a | join(', ')}}]; + let constraints_affine_b = vec![{{problem.constraints.vector_b | join(', ')}}]; + AffineSpace::new(constraints_affine_a, constraints_affine_b) {% elif 'FiniteSet' == problem.constraints.__class__.__name__ -%} // - Finite Set: let data: &[&[f64]] = &[ @@ -192,6 +196,11 @@ fn make_constraints() -> impl Constraint { let center_{{loop.index}}: Option<&[f64]> = {% if set_i.center is not none %}Some(&[{{set_i.center | join(', ')}}]){% else %}None{% endif %}; let set_{{loop.index}} = Sphere2::new(center_{{loop.index}}, radius_{{loop.index}}); let bounds = bounds.add_constraint(idx_{{loop.index}}, set_{{loop.index}}); + {% elif 'AffineSpace' == set_i.__class__.__name__ -%} + let constraints_affine_a = vec![{{problem.constraints.matrix_a | join(', ')}}]; + let constraints_affine_b = vec![{{problem.constraints.vector_b | join(', ')}}]; + let set_{{loop.index}} = AffineSpace::new(constraints_affine_a, constraints_affine_b) + let bounds = bounds.add_constraint(idx_{{loop.index}}, set_{{loop.index}}); {% elif 'Simplex' == set_i.__class__.__name__ -%} let alpha_{{loop.index}} = {{set_i.alpha}}; let set_{{loop.index}} = Simplex::new(alpha_{{loop.index}}); diff --git a/open-codegen/opengen/templates/optimizer_build.rs b/open-codegen/opengen/templates/optimizer_build.rs.jinja similarity index 100% rename from open-codegen/opengen/templates/optimizer_build.rs rename to open-codegen/opengen/templates/optimizer_build.rs.jinja diff --git a/open-codegen/opengen/templates/optimizer_cargo.toml b/open-codegen/opengen/templates/optimizer_cargo.toml.jinja similarity index 100% rename from open-codegen/opengen/templates/optimizer_cargo.toml rename to open-codegen/opengen/templates/optimizer_cargo.toml.jinja