From 320cf6acc454797aa4e32b97605b65efc8615ebb Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 20 Sep 2019 02:46:11 +0100 Subject: [PATCH 01/42] An journey of a thousand miles begins with a single commit on github --- open-codegen/opengen/templates/optimizer_cargo.toml.template | 2 +- open-codegen/opengen/templates/tcp_server_cargo.toml.template | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/open-codegen/opengen/templates/optimizer_cargo.toml.template b/open-codegen/opengen/templates/optimizer_cargo.toml.template index f612f2a3..18a87ceb 100644 --- a/open-codegen/opengen/templates/optimizer_cargo.toml.template +++ b/open-codegen/opengen/templates/optimizer_cargo.toml.template @@ -20,7 +20,7 @@ publish=false [dependencies] #TODO: In production, change the following line #optimization_engine = { git = "https://github.com/alphaville/optimization-engine" } -optimization_engine = "{{open_version or '0.5.1-beta'}}" +optimization_engine = "{{open_version or '0.6.1-alpha.1'}}" icasadi = {path = "./icasadi/"} {% if activate_clib_generation -%} libc = "0.2.0" diff --git a/open-codegen/opengen/templates/tcp_server_cargo.toml.template b/open-codegen/opengen/templates/tcp_server_cargo.toml.template index 3564321e..faa72276 100644 --- a/open-codegen/opengen/templates/tcp_server_cargo.toml.template +++ b/open-codegen/opengen/templates/tcp_server_cargo.toml.template @@ -21,7 +21,7 @@ publish=false [dependencies] -optimization_engine = "0.5.1-beta" +optimization_engine = "0.6.1-alpha.1" #DO NOT USE THE FOLLOWING LINE IN PRODUCTION: #optimization_engine = { path = "../../../../../" } serde = { version = "1.0", features = ["derive"] } From d7ac76abbee48f69a683dad18010b27f1fc3a987 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 20 Sep 2019 18:53:11 +0100 Subject: [PATCH 02/42] problem.py: new problem setup --- open-codegen/opengen/builder/problem.py | 115 ++++++++++++++++-------- 1 file changed, 77 insertions(+), 38 deletions(-) diff --git a/open-codegen/opengen/builder/problem.py b/open-codegen/opengen/builder/problem.py index e7b3af50..761bc4b9 100644 --- a/open-codegen/opengen/builder/problem.py +++ b/open-codegen/opengen/builder/problem.py @@ -5,16 +5,15 @@ class Problem: """Definition of an optimization problem Provides the cost function, constraints and additional - penalty-type constraints. + ALM/penalty-type constraints. """ def __init__(self, u, p, cost): """Construct an optimization problem - Args: - u: decision variable (CasADi variable) - p: parameter (CasADi variable) - cost: cost function (CasADi function of u and p) + :param u: decision variable (CasADi variable) + :param p: parameter (CasADi variable) + :param cost: cost function (CasADi function of u and p) Example: >>> import casadi.casadi as cs @@ -28,13 +27,27 @@ def __init__(self, u, p, cost): >>> problem = og.builder.Problem(u, p, phi) """ + + # Decision variable: u self.__u = u + # Parameter: p self.__p = p + # Cost function: f(u, p) self.__cost = cost + # Constraints on u: u in U self.__u_constraints = None - self.__penalty_constraints = None + # ALM-type constraints: mapping F1(u, p) + self.__alm_mapping_f1 = None + # ALM-type constraints: set C + # Corresponds to the constraints: F1(u, p) in C + self.__alm_set_c = None + # Set Y of dual variables (compact subset of C*) + self.__alm_set_y = None + # Penalty-type constraints: mapping F2(u, p) + # Constraints: F1(u, p) = 0 + self.__penalty_mapping_f2 = None + # @deprecated - to be removed self.__penalty_function = None - self.__al_constraints = None # ---------- SETTERS ----------------------------------------------- @@ -43,9 +56,9 @@ def with_constraints(self, u_constraints): Args: u_constraints: constraints on the decision variable; must - be a Constraint object (such as - \link opengen.constraints.ball2.Ball2 Ball2 \endlink - and \link opengen.constraints.rectangle.Rectangle Rectangle \endlink) + be a Constraint object (such as + opengen.constraints.ball2.Ball2 + and opengen.constraints.rectangle.Rectangle) Returns: Current object @@ -61,23 +74,23 @@ def with_penalty_constraints(self, penalty_constraints, penalty_function=None): function c(u; p)) and the penalty function, g. If no penalty function is specified, the quadratic penalty will be used. - Args: - penalty_constraints: a function c(u, p), of the decision - variable u and the parameter vector p, which - corresponds to the constraints c(u, p) - penalty_function: a function g: R -> R, used to define the - penalty in the penalty method; the default is g(z) = z^2. - You typically will not need to change this, but if you very much want to, - you need to provide an instance of casadi.casadi.Function - (not a CasADi symbol such as SX). - Returns: - self + :param penalty_constraints: a function c(u, p), of the decision + variable u and the parameter vector p, which + corresponds to the constraints c(u, p) + + :param penalty_function: a function g: R -> R, used to define the + penalty in the penalty method; the default is g(z) = z^2. + You typically will not need to change this, but if you very much want to, + you need to provide an instance of casadi.casadi.Function + (not a CasADi symbol such as SX). + + :return: self """ if penalty_constraints is None: pass - self.__penalty_constraints = penalty_constraints + self.__penalty_mapping_f2 = penalty_constraints if penalty_function is None: # default penalty function: quadratic z = cs.SX.sym("z") @@ -86,14 +99,28 @@ def with_penalty_constraints(self, penalty_constraints, penalty_function=None): self.__penalty_function = penalty_function return self - def with_aug_lagrangian_constraints(self, al_constraints): - """Not implemented yet""" - raise NotImplementedError + def with_aug_lagrangian_constraints(self, mapping_f1, set_c, set_y): + """ + Constraints F1(u, p) in C + + :param mapping_f1: + :param set_c: + :param set_y: + + :return: self + """ + self.__alm_mapping_f1 = mapping_f1 + self.__alm_set_c = set_c + self.__alm_set_y = set_y + raise self # ---------- DIMENSIONS -------------------------------------------- def dim_decision_variables(self): - """Number of decision variables""" + """Number of decision variables + + :return: number of decision variables + """ return self.__u.size(1) def dim_parameters(self): @@ -102,11 +129,13 @@ def dim_parameters(self): def dim_constraints_penalty(self): """Number of penalty-type constraints""" - return 0 if self.__penalty_constraints is None \ - else self.__penalty_constraints.size(1) + return 0 if self.__penalty_mapping_f2 is None \ + else self.__penalty_mapping_f2.size(1) def dim_constraints_aug_lagrangian(self): """Not implemented yet""" + return 0 if self.__alm_mapping_f1 is None \ + else self.__alm_mapping_f1.size(1) return 0 # ---------- OTHER GETTERS ----------------------------------------- @@ -117,25 +146,35 @@ def cost_function(self): return self.__cost @property - def penalty_constraints(self): - """Penalty constraints as a CasADi symbol (function c)""" - return self.__penalty_constraints + def penalty_mapping_f2(self): + """Penalty-type mapping F2 as a CasADi symbol""" + return self.__penalty_mapping_f2 + + @property + def penalty_mapping_f1(self): + """ALM mapping F1 as a CasADi symbol""" + return self.__alm_mapping_f1 + + @property + def alm_set_c(self): + """Set C in the definition of constraints: F1(u, p) in C""" + return self.__alm_set_c + + @property + def alm_set_y(self): + """Set Y for the Lagrange multipliers""" + return self.__alm_set_y @property def penalty_function(self): - """Penalty function, g""" + """Penalty function, g""" return self.__penalty_function @property def constraints(self): - """Hard constraints""" + """Hard constraints; set U""" return self.__u_constraints - @property - def constraints_aug_lagrangian(self): - """Not implemented yet""" - raise NotImplementedError - @property def decision_variables(self): """Decision variables (CasADi symbol)""" From 747ddfc9efd4dabaa66b39e93ebc787b8241bc89 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 21 Sep 2019 13:02:56 +0100 Subject: [PATCH 03/42] python: squared distance to ball-2 support for lists, np.ndarray and SX --- open-codegen/opengen/constraints/ball2.py | 41 +++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index a6d7fa35..b7e6a40f 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -1,3 +1,7 @@ +import casadi.casadi as cs +import numpy as np + + class Ball2: """A Euclidean ball constraint @@ -37,3 +41,40 @@ def radius(self): """Returns the radius of the ball""" return self.__radius + def distance_squared(self, u): + """Computes the squared distance between a given point `u` and this ball + + :param u: given point; can be a list of float, a numpy + ndarray or a CasADi SX symbol + + :return: distance from set as a float or a CasADi symbol + """ + # Function `distance` can be applied to CasADi symbols and + # lists of numbers. However, if `u` is a symbol, we need to + # use appropriate CasADi functions like cs.sign and cs.norm_2 + if isinstance(u, cs.SX): + # Case I: `u` is a CasADi SX symbol + sign_fun = cs.sign + max_fun = cs.fmax + norm_fun = cs.norm_2 + v = u if self.__center is None else u - self.__center + elif isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)\ + or isinstance(u, np.ndarray): + # Case II: `u` is an array of numbers or an np.ndarray + sign_fun = np.sign + norm_fun = np.linalg.norm + max_fun = np.fmax + if self.__center is None: + v = u + else: + z = np.array(self.__center).reshape((len(u), 1)) + u = np.array(u).reshape((len(u), 1)) + v = np.subtract(u, z) + else: + raise Exception("u is of invalid type") + + # Compute distance + t = norm_fun(v) - self.radius + return max_fun(0.0, sign_fun(t) * t ** 2) + + From d33903a31259e54fd4eb794cf9d22f42872650bf Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 21 Sep 2019 20:18:09 +0100 Subject: [PATCH 04/42] squared distance to ball2 and ball-inf (wip) new nice formula for squared dist to box implementation is in progress --- .../opengen/builder/optimizer_builder.py | 10 +-- open-codegen/opengen/builder/problem.py | 2 +- open-codegen/opengen/constraints/__init__.py | 4 +- open-codegen/opengen/constraints/ball2.py | 37 ++++++--- open-codegen/opengen/constraints/ball_inf.py | 83 +++++++++++++++++++ .../opengen/constraints/constraint.py | 8 ++ open-codegen/opengen/constraints/rectangle.py | 10 ++- 7 files changed, 136 insertions(+), 18 deletions(-) create mode 100644 open-codegen/opengen/constraints/ball_inf.py create mode 100644 open-codegen/opengen/constraints/constraint.py diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index d43988b3..66ebdace 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -52,7 +52,7 @@ def __init__(self, self.__verbosity_level = 0 def with_verbosity_level(self, verbosity_level): - """Sepcify the verbosity level + """Specify the verbosity level Args: verbosity_level: level of verbosity (0,1,2,3) @@ -189,7 +189,7 @@ def __generate_casadi_code(self): penalty_function = self.__problem.penalty_function mu = cs.SX.sym("mu", self.__problem.dim_constraints_penalty()) p = cs.vertcat(p, mu) - phi += cs.dot(mu, penalty_function(self.__problem.penalty_constraints)) + phi += cs.dot(mu, penalty_function(self.__problem.penalty_mapping_f2)) # Define cost and its gradient as CasADi functions cost_fun = cs.Function(self.__build_config.cost_function_name, [u, p], [phi]) @@ -212,7 +212,7 @@ def __generate_casadi_code(self): # Lastly, we generate code for the penalty constraints; if there aren't # any, we generate the function c(u; p) = 0 (which will not be used) if ncp > 0: - penalty_constraints = self.__problem.penalty_constraints + penalty_constraints = self.__problem.penalty_mapping_f2 else: penalty_constraints = 0 @@ -246,7 +246,7 @@ def __generate_main_project_code(self): problem=self.__problem, timestamp_created=datetime.datetime.now(), activate_clib_generation=self.__build_config.build_c_bindings) - target_source_path = os.path.join(target_dir, "src"); + target_source_path = os.path.join(target_dir, "src") target_scr_lib_rs_path = os.path.join(target_source_path, "lib.rs") make_dir_if_not_exists(target_source_path) with open(target_scr_lib_rs_path, "w") as fh: @@ -399,7 +399,7 @@ def build(self): self.__check_user_provided_parameters() # check the provided parameters self.__prepare_target_project() # create folders; init cargo project self.__copy_icasadi_to_target() # copy icasadi/ files to target dir - self.__generate_cargo_toml() # generate Cargo.toml using tempalte + self.__generate_cargo_toml() # generate Cargo.toml using template self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) diff --git a/open-codegen/opengen/builder/problem.py b/open-codegen/opengen/builder/problem.py index 761bc4b9..c5f06d25 100644 --- a/open-codegen/opengen/builder/problem.py +++ b/open-codegen/opengen/builder/problem.py @@ -112,7 +112,7 @@ def with_aug_lagrangian_constraints(self, mapping_f1, set_c, set_y): self.__alm_mapping_f1 = mapping_f1 self.__alm_set_c = set_c self.__alm_set_y = set_y - raise self + return self # ---------- DIMENSIONS -------------------------------------------- diff --git a/open-codegen/opengen/constraints/__init__.py b/open-codegen/opengen/constraints/__init__.py index 5a275173..01a3b7c7 100644 --- a/open-codegen/opengen/constraints/__init__.py +++ b/open-codegen/opengen/constraints/__init__.py @@ -1,2 +1,4 @@ from .ball2 import * -from .rectangle import * \ No newline at end of file +from .rectangle import * +from .constraint import * +from .ball_inf import * \ No newline at end of file diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index b7e6a40f..48f5ebbc 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -1,8 +1,9 @@ import casadi.casadi as cs import numpy as np +from opengen.constraints.constraint import Constraint -class Ball2: +class Ball2(Constraint): """A Euclidean ball constraint A constraint of the form ||u-u0|| <= r, where u0 is the center @@ -10,7 +11,7 @@ class Ball2: """ - def __init__(self, center, radius): + def __init__(self, center, radius: float): """Constructor for a Euclidean ball constraint Args: @@ -25,10 +26,11 @@ def __init__(self, center, radius): if radius <= 0: raise Exception("The radius must be a positive number") - if center is not None and not isinstance(center, list): - raise Exception("center is neither None nor a list") + if center is not None and not (isinstance(center, list) + or isinstance(center, np.ndarray)): + raise Exception("center is neither None nor a list nor np.ndarray") - self.__center = None if center is None else [float(i) for i in center] + self.__center = None if center is None else np.array([float(i) for i in center]) self.__radius = float(radius) @property @@ -45,7 +47,7 @@ def distance_squared(self, u): """Computes the squared distance between a given point `u` and this ball :param u: given point; can be a list of float, a numpy - ndarray or a CasADi SX symbol + n-dim array (`ndarray`) or a CasADi SX symbol :return: distance from set as a float or a CasADi symbol """ @@ -58,7 +60,7 @@ def distance_squared(self, u): max_fun = cs.fmax norm_fun = cs.norm_2 v = u if self.__center is None else u - self.__center - elif isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)\ + elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u))\ or isinstance(u, np.ndarray): # Case II: `u` is an array of numbers or an np.ndarray sign_fun = np.sign @@ -67,14 +69,29 @@ def distance_squared(self, u): if self.__center is None: v = u else: - z = np.array(self.__center).reshape((len(u), 1)) + # Note: self.__center is np.ndarray (`u` might be a list) + z = self.__center.reshape((len(u), 1)) u = np.array(u).reshape((len(u), 1)) v = np.subtract(u, z) else: raise Exception("u is of invalid type") - # Compute distance + # Compute squared distance + # Let B = B(xc, r) be a Euclidean ball centered at xc with radius r + # + # dist_B^2(u) = max(0, sign(t(u))*t(u)^2), where + # t(u) = ||u - x_c|| - r + # + # Note: there is another way to take squared distances: + # d_B^2(u) = ||u||^2 * (1 - 1 / max{r, ||u||})^2, + # but this leads to slightly lengthier CasADi symbols for + # the Jacobian of the squared distance, so this approach was + # abandoned t = norm_fun(v) - self.radius return max_fun(0.0, sign_fun(t) * t ** 2) - + def project(self, u): + # Idea: Computes projection on Ball as follows + # Proj_B(u) = u / max{1, ||u||}, + # which avoids dividing by zero or defining the projections + raise NotImplementedError() diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py new file mode 100644 index 00000000..a55c6a00 --- /dev/null +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -0,0 +1,83 @@ +import casadi.casadi as cs +import numpy as np +from opengen.constraints.constraint import Constraint + + +class BallInf(Constraint): + """Norm-ball of norm infinity translated by given vector + + Centered inf-ball around given point + """ + + def __init__(self, center, radius: float): + """Constructor for an infinity ball constraint + + Args: + :param center: center of the ball; if this is equal to Null, the + ball is centered at the origin + + :param radius: radius of the ball + + :return: + New instance of Ballinf with given center and radius + """ + + if radius <= 0: + raise Exception("The radius must be a positive number") + + if center is not None and not (isinstance(center, list) + or isinstance(center, np.ndarray)): + raise Exception("center is neither None nor a list nor np.ndarray") + + self.__center = None if center is None else np.array([float(i) for i in center]) + self.__radius = float(radius) + + @property + def center(self): + """Returns the center of the ball""" + return self.__center + + @property + def radius(self): + """Returns the radius of the ball""" + return self.__radius + + def norm_inf_fun_np(a): + return np.linalg.norm(a, ord=np.inf) + + def distance_squared(self, u): + # Function `distance` can be applied to CasADi symbols and + # lists of numbers. However, if `u` is a symbol, we need to + # use appropriate CasADi functions like cs.sign and cs.norm_2 + if isinstance(u, cs.SX): + # Case I: `u` is a CasADi SX symbol + sign_fun = cs.sign + max_fun = cs.fmax + norm_fun = cs.norm_2 + norm_inf_fun = cs.norm_inf + v = u if self.__center is None else u - self.__center + elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ + or isinstance(u, np.ndarray): + # Case II: `u` is an array of numbers or an np.ndarray + sign_fun = np.sign + norm_fun = np.linalg.norm + norm_inf_fun = BallInf.norm_inf_fun_np + max_fun = np.fmax + if self.__center is None: + v = u + else: + # Note: self.__center is np.ndarray (`u` might be a list) + z = self.__center.reshape((len(u), 1)) + u = np.array(u).reshape((len(u), 1)) + v = np.subtract(u, z) + else: + raise Exception("u is of invalid type") + + # Compute distance to Ball infinity: + # dist^2(u) = norm(u)^2 + # + SUM_i min{ui^2, r^2} + # + SUM_i min{ui^2, r*|ui|} + + def project(self, u): + # Computes projection on Ball + raise NotImplementedError("Method `project` is not implemented") \ No newline at end of file diff --git a/open-codegen/opengen/constraints/constraint.py b/open-codegen/opengen/constraints/constraint.py new file mode 100644 index 00000000..b2aeb5ff --- /dev/null +++ b/open-codegen/opengen/constraints/constraint.py @@ -0,0 +1,8 @@ +class Constraint: + + def distance_squared(self, u): + raise NotImplementedError("Method `distance_squared` is not implemented") + + def project(self, u): + raise NotImplementedError("Method `project` is not implemented") + diff --git a/open-codegen/opengen/constraints/rectangle.py b/open-codegen/opengen/constraints/rectangle.py index 62043d5d..02f6b09f 100644 --- a/open-codegen/opengen/constraints/rectangle.py +++ b/open-codegen/opengen/constraints/rectangle.py @@ -1,4 +1,7 @@ -class Rectangle: +from opengen.constraints.constraint import Constraint + + +class Rectangle(Constraint): """A Rectangle (Box) constraint""" def __init__(self, xmin, xmax): @@ -50,3 +53,8 @@ def xmax(self): """Maximum bound""" return self.__xmax + def distance_squared(self, u): + 0 + + def project(self, u): + raise NotImplementedError() \ No newline at end of file From 3f42c66c509417ef22ebb361ee8c033d5de7b174 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 21 Sep 2019 22:24:56 +0100 Subject: [PATCH 05/42] squared distance to infinity ball --- open-codegen/opengen/constraints/ball2.py | 2 +- open-codegen/opengen/constraints/ball_inf.py | 29 ++++++++++++-------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index 48f5ebbc..9a11ae05 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -64,8 +64,8 @@ def distance_squared(self, u): or isinstance(u, np.ndarray): # Case II: `u` is an array of numbers or an np.ndarray sign_fun = np.sign - norm_fun = np.linalg.norm max_fun = np.fmax + norm_fun = np.linalg.norm if self.__center is None: v = u else: diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py index a55c6a00..6ed9011c 100644 --- a/open-codegen/opengen/constraints/ball_inf.py +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -51,32 +51,39 @@ def distance_squared(self, u): # use appropriate CasADi functions like cs.sign and cs.norm_2 if isinstance(u, cs.SX): # Case I: `u` is a CasADi SX symbol - sign_fun = cs.sign - max_fun = cs.fmax + nu = u.size(1) + min_fun = cs.fmin norm_fun = cs.norm_2 - norm_inf_fun = cs.norm_inf + abs_fun = cs.fabs v = u if self.__center is None else u - self.__center elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ or isinstance(u, np.ndarray): # Case II: `u` is an array of numbers or an np.ndarray - sign_fun = np.sign + nu = len(u) + min_fun = np.fmin norm_fun = np.linalg.norm - norm_inf_fun = BallInf.norm_inf_fun_np - max_fun = np.fmax + abs_fun = np.fabs if self.__center is None: v = u else: # Note: self.__center is np.ndarray (`u` might be a list) - z = self.__center.reshape((len(u), 1)) - u = np.array(u).reshape((len(u), 1)) + z = self.__center.reshape((nu, 1)) + u = np.array(u).reshape((nu, 1)) v = np.subtract(u, z) else: raise Exception("u is of invalid type") # Compute distance to Ball infinity: - # dist^2(u) = norm(u)^2 - # + SUM_i min{ui^2, r^2} - # + SUM_i min{ui^2, r*|ui|} + # dist^2(u) = norm(v)^2 + # + SUM_i min{vi^2, r^2} + # + SUM_i min{vi^2, r*|vi|} + # where v = u - xc + + squared_distance = norm_fun(v) + for i in range(nu): + squared_distance += min_fun(v[i]**2, self.radius**2) \ + + min_fun(v[i]**2, self.radius * abs_fun(v[i])) + return squared_distance def project(self, u): # Computes projection on Ball From b20c2f4e40143e83de4da9d528f6b9cd83493f4d Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 21 Sep 2019 23:29:50 +0100 Subject: [PATCH 06/42] python tests for new constraints unit tests for sq distance functions tests for ball inf and euclidean ball --- ci/script.sh | 20 +++-- open-codegen/opengen/constraints/ball2.py | 6 +- open-codegen/opengen/constraints/ball_inf.py | 16 ++-- open-codegen/opengen/test/test_constraints.py | 87 +++++++++++++++++++ 4 files changed, 110 insertions(+), 19 deletions(-) create mode 100644 open-codegen/opengen/test/test_constraints.py diff --git a/ci/script.sh b/ci/script.sh index d9e0968a..552028e3 100755 --- a/ci/script.sh +++ b/ci/script.sh @@ -15,19 +15,21 @@ main() { # TODO: Re-enable later # Create virtual environment - # cd open-codegen - # export PYTHONPATH=. - # virtualenv -p python$PYTHON_VERSION venv + cd open-codegen + export PYTHONPATH=. + virtualenv -p python$PYTHON_VERSION venv - # activate venv - # source venv/bin/activate + activate venv + source venv/bin/activate - # install opengen - # python setup.py install + install opengen + python setup.py install # run opengen main.py - # cd opengen - # export PYTHONPATH=. + + cd opengen + export PYTHONPATH=. + python -W ignore test/test_constraints.py -v # python -W ignore test/test.py -v } diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index 9a11ae05..d31299a8 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -1,6 +1,6 @@ import casadi.casadi as cs import numpy as np -from opengen.constraints.constraint import Constraint +from .constraint import Constraint class Ball2(Constraint): @@ -70,8 +70,8 @@ def distance_squared(self, u): v = u else: # Note: self.__center is np.ndarray (`u` might be a list) - z = self.__center.reshape((len(u), 1)) - u = np.array(u).reshape((len(u), 1)) + z = self.__center.reshape(len(u)) + u = np.array(u).reshape(len(u)) v = np.subtract(u, z) else: raise Exception("u is of invalid type") diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py index 6ed9011c..56a21c8d 100644 --- a/open-codegen/opengen/constraints/ball_inf.py +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -1,6 +1,6 @@ import casadi.casadi as cs import numpy as np -from opengen.constraints.constraint import Constraint +from .constraint import Constraint class BallInf(Constraint): @@ -67,22 +67,24 @@ def distance_squared(self, u): v = u else: # Note: self.__center is np.ndarray (`u` might be a list) - z = self.__center.reshape((nu, 1)) - u = np.array(u).reshape((nu, 1)) + z = self.__center.reshape(nu) + u = np.array(u).reshape(nu) v = np.subtract(u, z) else: raise Exception("u is of invalid type") # Compute distance to Ball infinity: # dist^2(u) = norm(v)^2 - # + SUM_i min{vi^2, r^2} - # + SUM_i min{vi^2, r*|vi|} + # + SUM_i [ + # min{vi^2, r^2} + # - 2*min{vi^2, r*|vi|} + # ] # where v = u - xc - squared_distance = norm_fun(v) + squared_distance = norm_fun(v)**2 for i in range(nu): squared_distance += min_fun(v[i]**2, self.radius**2) \ - + min_fun(v[i]**2, self.radius * abs_fun(v[i])) + - 2.0 * min_fun(v[i]**2, self.radius * abs_fun(v[i])) return squared_distance def project(self, u): diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py new file mode 100644 index 00000000..25c7d6f7 --- /dev/null +++ b/open-codegen/opengen/test/test_constraints.py @@ -0,0 +1,87 @@ +import unittest +import casadi.casadi as cs +import opengen as og +import numpy as np + + +class ConstraintsTestCase(unittest.TestCase): + + def test_ball_inf_origin(self): + ball = og.constraints.BallInf(None, 1) + x = np.array([3, 2]) + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = 5 + self.assertAlmostEqual(d_num, correct_squared_distance, + 8, "expected squared distance") + + def test_ball_inf_origin_inside(self): + ball = og.constraints.BallInf(None, 1) + x = np.array([0.1, -0.2]) + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) + self.assertAlmostEqual(d_sym, d_num, 10, "computation of distance") + correct_squared_distance = 0 + self.assertAlmostEqual(d_num, correct_squared_distance, + 10, "expected squared distance") + + def test_ball_inf_xc(self): + ball = og.constraints.BallInf([-1, -1], 0.5) + x = np.array([1, -2]) + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = 2.5 + self.assertAlmostEqual(d_num, correct_squared_distance, + 8, "expected squared distance") + + def test_ball_euclidean_origin(self): + ball = og.constraints.Ball2(None, 1) + x = np.array([2, 2]) + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = 8*(1-np.sqrt(2)/4)**2 + self.assertAlmostEqual(d_sym, correct_squared_distance, + 8, "expected squared distance") + + def test_ball_euclidean_origin_inside(self): + ball = og.constraints.Ball2(None, 1) + x = np.array([0.2, 0.8]) + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = 0.0 + self.assertAlmostEqual(d_sym, correct_squared_distance, + 8, "expected squared distance") + + def test_ball_euclidean_xc(self): + ball = og.constraints.Ball2([1, 2], 1) + x = [0, 1] + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = cs.substitute(ball.distance_squared(x_sym), x_sym, x) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = (np.sqrt(2) - 1) ** 2 + self.assertAlmostEqual(d_sym, correct_squared_distance, + 8, "expected squared distance") + + def test_ball_euclidean_xc_inside(self): + ball = og.constraints.Ball2([1, 2], 1) + x = [1.2, 1.55] + x_sym = cs.SX.sym("x", 2) + d_num = ball.distance_squared(x) + d_sym = cs.substitute(ball.distance_squared(x_sym), x_sym, x) + self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") + correct_squared_distance = 0.0 + self.assertAlmostEqual(d_sym, correct_squared_distance, + 8, "expected squared distance") + +if __name__ == '__main__': + unittest.main() From 455dec9db23f152112c2446e421a8a915c710989 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 21 Sep 2019 23:34:38 +0100 Subject: [PATCH 07/42] typo in ci/script is now fixed committing again to run CI tests --- ci/script.sh | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ci/script.sh b/ci/script.sh index 552028e3..248c0a38 100755 --- a/ci/script.sh +++ b/ci/script.sh @@ -19,17 +19,21 @@ main() { export PYTHONPATH=. virtualenv -p python$PYTHON_VERSION venv - activate venv + # --- activate venv source venv/bin/activate - install opengen + # --- install opengen python setup.py install + # --- uncomment to run main file # run opengen main.py + # --- run the tests cd opengen export PYTHONPATH=. python -W ignore test/test_constraints.py -v + + # --- temporarily commented out: # python -W ignore test/test.py -v } From 9456baa68cfaad44d775249c8a7da344cad0ee9b Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sun, 22 Sep 2019 01:36:33 +0100 Subject: [PATCH 08/42] rectangle: implementation of sq dist added simple tests in Python for sq dist from simple rectangles (more tests under preparation) --- open-codegen/opengen/constraints/rectangle.py | 78 ++++++++++++++++++- open-codegen/opengen/test/test_constraints.py | 34 ++++++++ 2 files changed, 110 insertions(+), 2 deletions(-) diff --git a/open-codegen/opengen/constraints/rectangle.py b/open-codegen/opengen/constraints/rectangle.py index 02f6b09f..080060a4 100644 --- a/open-codegen/opengen/constraints/rectangle.py +++ b/open-codegen/opengen/constraints/rectangle.py @@ -1,4 +1,5 @@ from opengen.constraints.constraint import Constraint +import numpy as np class Rectangle(Constraint): @@ -13,7 +14,7 @@ def __init__(self, xmin, xmax): Raises: Exception: if both xmin and xmax is None - Exception: if xmin/xmax is not None and not a list (wrong type) + Exception: if xmin/xmax is not None and not a list (wrong type) Exception: if xmin and xmax have incompatible lengths Exception: if xmin(i) > xmax(i) for some i (empty set) @@ -53,8 +54,81 @@ def xmax(self): """Maximum bound""" return self.__xmax + def dimension(self): + if self.__xmin is not None: + return len(self.__xmin) + elif self.__xmax is not None: + return len(self.__xmax) + else: + raise Exception("Absurd: both xmin and xmax are None!") + + def idx_bound_finite_all(self): + idx_both_finite = [] + + if self.__xmin is None or self.__xmax is None: + return idx_both_finite + + for i in range(self.dimension()): + xmini = self.__xmin[i] + xmaxi = self.__xmax[i] + if xmini > float('-inf') and xmaxi < float('inf'): + idx_both_finite += [i] + + return idx_both_finite + + def idx_infinite_only_xmin(self): + idx_xmin_infinite = [] + + if self.__xmax is None: + # If xmax is None (infinite), we should return + # the empty set + return idx_xmin_infinite + + # Hereafter, xmax is not None (but xmin can be None) + + for i in range(self.dimension()): + xmini = self.__xmin[i] if self.__xmin is not None else float('-inf') + xmaxi = self.__xmax[i] + if xmini == float('-inf') and xmaxi < float('inf'): + idx_xmin_infinite += [i] + + return idx_xmin_infinite + + def idx_infinite_only_xmax(self): + idx_xmin_infinite = [] + + if self.__xmin is None: + # If xmin is None (-infinity), we should return + # the empty set + return idx_xmin_infinite + + # Hereafter, xmin is not None (xmax might be) + for i in range(self.dimension()): + xmini = self.__xmin[i] + xmaxi = self.__xmax[i] if self.__xmax is not None else float('inf') + if xmaxi == float('inf') and xmini > float('-inf'): + idx_xmin_infinite += [i] + + return idx_xmin_infinite + def distance_squared(self, u): - 0 + I1 = self.idx_infinite_only_xmin() + I2 = self.idx_infinite_only_xmax() + I3 = self.idx_bound_finite_all() + + dist_sq = 0.0 + for i in I1: + dist_sq += np.fmax(0.0, u[i] - self.__xmax[i]) ** 2 + + for i in I2: + dist_sq += np.fmin(0.0, u[i] - self.__xmin[i]) ** 2 + + for i in I3: + dist_sq += np.fmin( + np.fmax(0.0, u[i] - self.__xmax[i]), + u[i] - self.__xmin[i]) ** 2 + + return dist_sq def project(self, u): raise NotImplementedError() \ No newline at end of file diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index 25c7d6f7..a57b4978 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -83,5 +83,39 @@ def test_ball_euclidean_xc_inside(self): self.assertAlmostEqual(d_sym, correct_squared_distance, 8, "expected squared distance") + def test_rectangle_simple(self): + rect = og.constraints.Rectangle([-1, -2], [4, -1]) + # some basic assertions + self.assertListEqual([0, 1], rect.idx_bound_finite_all()) + self.assert_(len(rect.idx_infinite_only_xmax()) == 0) + self.assertTrue(len(rect.idx_infinite_only_xmin()) == 0) + self.assertEqual(2, rect.dimension()) + # squared distance + self.assertAlmostEqual(1, rect.distance_squared([3, 0]), 8) + self.assertAlmostEqual(4, rect.distance_squared([0, 1]), 8) + self.assertAlmostEqual(1, rect.distance_squared([5, -1.5]), 8) + self.assertAlmostEqual(5, rect.distance_squared([5, 1]), 8) + + def test_rectangle_pos_quant(self): + n = 3 + rect = og.constraints.Rectangle([0.0]*n, None) + # some basic assertions + self.assertTrue(0 == len(rect.idx_bound_finite_all())) + self.assertTrue(0 == len(rect.idx_infinite_only_xmin())) + self.assertEqual([*range(n)], rect.idx_infinite_only_xmax()) + # some squared distances + self.assertAlmostEqual(0.0, rect.distance_squared([0.0]*n), 8) + self.assertAlmostEqual(0.0, rect.distance_squared([1.0] * n), 8) + self.assertAlmostEqual(1.0, rect.distance_squared([-1.0] + [1.0] * (n-1)), 8) + self.assertAlmostEqual(5.0, rect.distance_squared([-1.0, -2.0, 5.0]), 8) + + def test_rectangle_semiinf_corridor(self): + rect = og.constraints.Rectangle([-1.0, -2.0], [float('inf'), 3.0]) + self.assertEqual([0], rect.idx_infinite_only_xmax()) + self.assertAlmostEqual(0.0, rect.distance_squared([1e16, 1.5]), 8) + self.assertAlmostEqual(1.0, rect.distance_squared([1e16, 4.0]), 8) + self.assertAlmostEqual(4.0, rect.distance_squared([1e16, -4.0]), 8) + + if __name__ == '__main__': unittest.main() From ef2ac92cb24b1c228ac9ab19d9d06e911ebbbf78 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sun, 22 Sep 2019 02:23:46 +0100 Subject: [PATCH 09/42] neat design for NP and CasADi introduced stuff in opengen.functions updated unit tests --- open-codegen/opengen/constraints/ball2.py | 14 +++-------- open-codegen/opengen/constraints/ball_inf.py | 13 +++------- open-codegen/opengen/constraints/rectangle.py | 25 ++++++++++--------- open-codegen/opengen/functions/__init__.py | 7 ++++++ open-codegen/opengen/functions/fabs.py | 13 ++++++++++ open-codegen/opengen/functions/fmax.py | 13 ++++++++++ open-codegen/opengen/functions/fmin.py | 13 ++++++++++ open-codegen/opengen/functions/is_numeric.py | 6 +++++ open-codegen/opengen/functions/is_symbolic.py | 6 +++++ open-codegen/opengen/functions/norm2.py | 15 +++++++++++ open-codegen/opengen/functions/sign.py | 13 ++++++++++ open-codegen/opengen/test/test_constraints.py | 4 +++ 12 files changed, 110 insertions(+), 32 deletions(-) create mode 100644 open-codegen/opengen/functions/fabs.py create mode 100644 open-codegen/opengen/functions/fmax.py create mode 100644 open-codegen/opengen/functions/fmin.py create mode 100644 open-codegen/opengen/functions/is_numeric.py create mode 100644 open-codegen/opengen/functions/is_symbolic.py create mode 100644 open-codegen/opengen/functions/norm2.py create mode 100644 open-codegen/opengen/functions/sign.py diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index d31299a8..61917268 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -1,6 +1,7 @@ import casadi.casadi as cs import numpy as np from .constraint import Constraint +import opengen.functions as fn class Ball2(Constraint): @@ -51,21 +52,12 @@ def distance_squared(self, u): :return: distance from set as a float or a CasADi symbol """ - # Function `distance` can be applied to CasADi symbols and - # lists of numbers. However, if `u` is a symbol, we need to - # use appropriate CasADi functions like cs.sign and cs.norm_2 if isinstance(u, cs.SX): # Case I: `u` is a CasADi SX symbol - sign_fun = cs.sign - max_fun = cs.fmax - norm_fun = cs.norm_2 v = u if self.__center is None else u - self.__center elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u))\ or isinstance(u, np.ndarray): # Case II: `u` is an array of numbers or an np.ndarray - sign_fun = np.sign - max_fun = np.fmax - norm_fun = np.linalg.norm if self.__center is None: v = u else: @@ -87,8 +79,8 @@ def distance_squared(self, u): # but this leads to slightly lengthier CasADi symbols for # the Jacobian of the squared distance, so this approach was # abandoned - t = norm_fun(v) - self.radius - return max_fun(0.0, sign_fun(t) * t ** 2) + t = fn.norm2(v) - self.radius + return fn.fmax(0.0, fn.sign(t) * t ** 2) def project(self, u): # Idea: Computes projection on Ball as follows diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py index 56a21c8d..9c7d6fb5 100644 --- a/open-codegen/opengen/constraints/ball_inf.py +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -1,6 +1,7 @@ import casadi.casadi as cs import numpy as np from .constraint import Constraint +import opengen.functions as fn class BallInf(Constraint): @@ -52,17 +53,11 @@ def distance_squared(self, u): if isinstance(u, cs.SX): # Case I: `u` is a CasADi SX symbol nu = u.size(1) - min_fun = cs.fmin - norm_fun = cs.norm_2 - abs_fun = cs.fabs v = u if self.__center is None else u - self.__center elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ or isinstance(u, np.ndarray): # Case II: `u` is an array of numbers or an np.ndarray nu = len(u) - min_fun = np.fmin - norm_fun = np.linalg.norm - abs_fun = np.fabs if self.__center is None: v = u else: @@ -81,10 +76,10 @@ def distance_squared(self, u): # ] # where v = u - xc - squared_distance = norm_fun(v)**2 + squared_distance = fn.norm2(v)**2 for i in range(nu): - squared_distance += min_fun(v[i]**2, self.radius**2) \ - - 2.0 * min_fun(v[i]**2, self.radius * abs_fun(v[i])) + squared_distance += fn.fmin(v[i]**2, self.radius**2) \ + - 2.0 * fn.fmin(v[i]**2, self.radius * fn.fabs(v[i])) return squared_distance def project(self, u): diff --git a/open-codegen/opengen/constraints/rectangle.py b/open-codegen/opengen/constraints/rectangle.py index 080060a4..252b294f 100644 --- a/open-codegen/opengen/constraints/rectangle.py +++ b/open-codegen/opengen/constraints/rectangle.py @@ -1,5 +1,6 @@ -from opengen.constraints.constraint import Constraint +from .constraint import Constraint import numpy as np +import opengen.functions as fn class Rectangle(Constraint): @@ -112,23 +113,23 @@ def idx_infinite_only_xmax(self): return idx_xmin_infinite def distance_squared(self, u): - I1 = self.idx_infinite_only_xmin() - I2 = self.idx_infinite_only_xmax() - I3 = self.idx_bound_finite_all() + idx1 = self.idx_infinite_only_xmin() + idx2 = self.idx_infinite_only_xmax() + idx3 = self.idx_bound_finite_all() dist_sq = 0.0 - for i in I1: - dist_sq += np.fmax(0.0, u[i] - self.__xmax[i]) ** 2 + for i in idx1: + dist_sq += fn.fmax(0.0, u[i] - self.__xmax[i]) ** 2 - for i in I2: - dist_sq += np.fmin(0.0, u[i] - self.__xmin[i]) ** 2 + for i in idx2: + dist_sq += fn.fmin(0.0, u[i] - self.__xmin[i]) ** 2 - for i in I3: - dist_sq += np.fmin( - np.fmax(0.0, u[i] - self.__xmax[i]), + for i in idx3: + dist_sq += fn.fmin( + fn.fmax(0.0, u[i] - self.__xmax[i]), u[i] - self.__xmin[i]) ** 2 return dist_sq def project(self, u): - raise NotImplementedError() \ No newline at end of file + raise NotImplementedError() diff --git a/open-codegen/opengen/functions/__init__.py b/open-codegen/opengen/functions/__init__.py index dba70a2c..07d0d3ec 100644 --- a/open-codegen/opengen/functions/__init__.py +++ b/open-codegen/opengen/functions/__init__.py @@ -1 +1,8 @@ from .rosenbrock import * +from .fmin import * +from .fmax import * +from .is_symbolic import * +from .is_numeric import * +from .sign import * +from .norm2 import * +from .fabs import * diff --git a/open-codegen/opengen/functions/fabs.py b/open-codegen/opengen/functions/fabs.py new file mode 100644 index 00000000..587386e8 --- /dev/null +++ b/open-codegen/opengen/functions/fabs.py @@ -0,0 +1,13 @@ +import casadi.casadi as cs +import numpy as np +from .is_numeric import * +from .is_symbolic import * + + +def fabs(u): + if is_numeric(u): + return np.fabs(u) + elif is_symbolic(u): + return cs.fabs(u) + else: + raise Exception("Illegal argument") \ No newline at end of file diff --git a/open-codegen/opengen/functions/fmax.py b/open-codegen/opengen/functions/fmax.py new file mode 100644 index 00000000..a2b9471c --- /dev/null +++ b/open-codegen/opengen/functions/fmax.py @@ -0,0 +1,13 @@ +import casadi.casadi as cs +import numpy as np +from .is_numeric import * +from .is_symbolic import * + + +def fmax(u, v): + if is_numeric(u) and is_numeric(v): + return np.fmax(u, v) + elif is_symbolic(u) or is_symbolic(v): + return cs.fmax(u, v) + else: + raise Exception("Illegal argument") \ No newline at end of file diff --git a/open-codegen/opengen/functions/fmin.py b/open-codegen/opengen/functions/fmin.py new file mode 100644 index 00000000..dc7feb3b --- /dev/null +++ b/open-codegen/opengen/functions/fmin.py @@ -0,0 +1,13 @@ +import casadi.casadi as cs +import numpy as np +from .is_numeric import * +from .is_symbolic import * + + +def fmin(u, v): + if is_numeric(u) and is_numeric(v): + return np.fmin(u, v) + elif is_symbolic(u) or is_symbolic(v): + return cs.fmin(u, v) + else: + raise Exception("Illegal argument") \ No newline at end of file diff --git a/open-codegen/opengen/functions/is_numeric.py b/open-codegen/opengen/functions/is_numeric.py new file mode 100644 index 00000000..0dd20790 --- /dev/null +++ b/open-codegen/opengen/functions/is_numeric.py @@ -0,0 +1,6 @@ +import numpy as np + + +def is_numeric(u): + return isinstance(u, (int, float)) \ + or np.isscalar(u) diff --git a/open-codegen/opengen/functions/is_symbolic.py b/open-codegen/opengen/functions/is_symbolic.py new file mode 100644 index 00000000..1fa8bd88 --- /dev/null +++ b/open-codegen/opengen/functions/is_symbolic.py @@ -0,0 +1,6 @@ +import casadi.casadi as cs + + +def is_symbolic(u): + return isinstance(u, cs.SX) + diff --git a/open-codegen/opengen/functions/norm2.py b/open-codegen/opengen/functions/norm2.py new file mode 100644 index 00000000..1c314754 --- /dev/null +++ b/open-codegen/opengen/functions/norm2.py @@ -0,0 +1,15 @@ +import casadi.casadi as cs +import numpy as np +from .is_numeric import * +from .is_symbolic import * + + +def norm2(u): + if (isinstance(u, list) and all(is_numeric(x) for x in u))\ + or isinstance(u, np.ndarray): + # if `u` is a numeric vector + return np.linalg.norm(u) + elif is_symbolic(u): + return cs.norm_2(u) + else: + raise Exception("Illegal argument") \ No newline at end of file diff --git a/open-codegen/opengen/functions/sign.py b/open-codegen/opengen/functions/sign.py new file mode 100644 index 00000000..57846c71 --- /dev/null +++ b/open-codegen/opengen/functions/sign.py @@ -0,0 +1,13 @@ +import casadi.casadi as cs +import numpy as np +from .is_numeric import * +from .is_symbolic import * + + +def sign(u): + if is_numeric(u): + return np.sign(u) + elif is_symbolic(u): + return cs.sign(u) + else: + raise Exception("Illegal argument") \ No newline at end of file diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index a57b4978..fefb592e 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -95,6 +95,10 @@ def test_rectangle_simple(self): self.assertAlmostEqual(4, rect.distance_squared([0, 1]), 8) self.assertAlmostEqual(1, rect.distance_squared([5, -1.5]), 8) self.assertAlmostEqual(5, rect.distance_squared([5, 1]), 8) + # symbolic + x_sym = cs.SX.sym("x", 2) + d_sym = float(cs.substitute(rect.distance_squared(x_sym), x_sym, [5, 1])) + self.assertAlmostEqual(5, d_sym, 8) def test_rectangle_pos_quant(self): n = 3 From fc1d256547bf0bcc11053110b3841534ea21d1cb Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Mon, 23 Sep 2019 16:09:20 +0100 Subject: [PATCH 10/42] working towards sq-distance from SOC code to be modified (will introduce if_else) unit tests might fail --- open-codegen/opengen/constraints/__init__.py | 3 +- open-codegen/opengen/constraints/ball2.py | 2 +- open-codegen/opengen/constraints/ball_inf.py | 2 +- open-codegen/opengen/constraints/soc.py | 81 +++++++++++++++++++ open-codegen/opengen/functions/is_symbolic.py | 4 +- open-codegen/opengen/test/test_constraints.py | 27 ++++++- 6 files changed, 113 insertions(+), 6 deletions(-) create mode 100644 open-codegen/opengen/constraints/soc.py diff --git a/open-codegen/opengen/constraints/__init__.py b/open-codegen/opengen/constraints/__init__.py index 01a3b7c7..8d123be8 100644 --- a/open-codegen/opengen/constraints/__init__.py +++ b/open-codegen/opengen/constraints/__init__.py @@ -1,4 +1,5 @@ from .ball2 import * from .rectangle import * from .constraint import * -from .ball_inf import * \ No newline at end of file +from .ball_inf import * +from .soc import * diff --git a/open-codegen/opengen/constraints/ball2.py b/open-codegen/opengen/constraints/ball2.py index 61917268..8b8bf657 100644 --- a/open-codegen/opengen/constraints/ball2.py +++ b/open-codegen/opengen/constraints/ball2.py @@ -48,7 +48,7 @@ def distance_squared(self, u): """Computes the squared distance between a given point `u` and this ball :param u: given point; can be a list of float, a numpy - n-dim array (`ndarray`) or a CasADi SX symbol + n-dim array (`ndarray`) or a CasADi SX/MX symbol :return: distance from set as a float or a CasADi symbol """ diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py index 9c7d6fb5..aacdebb6 100644 --- a/open-codegen/opengen/constraints/ball_inf.py +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -50,7 +50,7 @@ def distance_squared(self, u): # Function `distance` can be applied to CasADi symbols and # lists of numbers. However, if `u` is a symbol, we need to # use appropriate CasADi functions like cs.sign and cs.norm_2 - if isinstance(u, cs.SX): + if fn.is_symbolic(u): # Case I: `u` is a CasADi SX symbol nu = u.size(1) v = u if self.__center is None else u - self.__center diff --git a/open-codegen/opengen/constraints/soc.py b/open-codegen/opengen/constraints/soc.py new file mode 100644 index 00000000..61f0cd13 --- /dev/null +++ b/open-codegen/opengen/constraints/soc.py @@ -0,0 +1,81 @@ +import casadi.casadi as cs +import numpy as np +from .constraint import Constraint +import opengen.functions as fn + + +class SecondOrderCone(Constraint): + """A Second-Order Cone given by C = {u = (x, r): a||x|| <= r} + + Second-order cones are used in conic optimisation to describe + inequalities that involve quadratic terms + + """ + + def __init__(self, a: float = 1.0): + """Constructor for a Second-Order Cone set + + Args: + :param a: parameter a + + Returns: + New instance of a SOC with given parameter `a` + """ + if a <= 0: + raise Exception("Parameter `a` must be a positive number") + + self.__a = a + + @property + def a(self): + """Returns the value of parameter `a`""" + return self.__a + + def distance_squared(self, u): + """Computes the squared distance between a given point `u` and this + second-order cone + + :param u: given point; can be a list of float, a numpy + n-dim array (`ndarray`) or a CasADi SX/MX symbol + + :return: distance from set as a float or a CasADi symbol + """ + + if fn.is_symbolic(u): + # Case I: `u` is a CasADi SX symbol + nu = u.size(1) + elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ + or isinstance(u, np.ndarray): + nu = len(u) + else: + raise Exception("Illegal Argument, `u`") + + # Partition `u = (x, r)`, where `r` is the last element of `u` + a = self.__a + x = u[0:nu-1] + r = u[nu-1] + # norm of x + norm_x = fn.norm2(x) + # The first branch is zero + # Second branch: + condition2 = r <= -norm_x + fun2 = cs.dot(x, x) + r ** 2 + # Third branch: + condition3 = (-norm_x < r) * (norm_x > r) + beta = a**2/(a**2 + 1.0) + fun3 = norm_x ** 2 + beta * (a * norm_x + r) ** 2 \ + - 2.0 * a * norm_x * (a*norm_x + r) \ + + (r - (a * norm_x + r)/(a**2 + 1.0)) ** 2 + + y = cs.if_else(condition2*(1-condition3), fun2, fun3) + + # Function defined piecewise + # y = condition2 * fun2 + condition3 * fun3 + return y + + def project(self, u): + # Idea: Computes projection on Ball as follows + # Proj_B(u) = u / max{1, ||u||}, + # which avoids dividing by zero or defining the projections + raise NotImplementedError() + diff --git a/open-codegen/opengen/functions/is_symbolic.py b/open-codegen/opengen/functions/is_symbolic.py index 1fa8bd88..77cd86da 100644 --- a/open-codegen/opengen/functions/is_symbolic.py +++ b/open-codegen/opengen/functions/is_symbolic.py @@ -2,5 +2,7 @@ def is_symbolic(u): - return isinstance(u, cs.SX) + return isinstance(u, cs.SX) \ + or isinstance(u, cs.MX) \ + or isinstance(u, cs.DM) diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index fefb592e..23d60c18 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -2,7 +2,7 @@ import casadi.casadi as cs import opengen as og import numpy as np - +import math class ConstraintsTestCase(unittest.TestCase): @@ -13,9 +13,14 @@ def test_ball_inf_origin(self): d_num = ball.distance_squared(x) d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x)) self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance") - correct_squared_distance = 5 + correct_squared_distance = 5.0 self.assertAlmostEqual(d_num, correct_squared_distance, 8, "expected squared distance") + # verify that it works with cs.MX + x_sym_mx = cs.MX.sym("xmx", 2) + sqdist_mx = ball.distance_squared(x_sym_mx) + sqdist_mx_fun = cs.Function('sqd', [x_sym_mx], [sqdist_mx]) + self.assertAlmostEqual(correct_squared_distance, sqdist_mx_fun(x)[0], 5) def test_ball_inf_origin_inside(self): ball = og.constraints.BallInf(None, 1) @@ -120,6 +125,24 @@ def test_rectangle_semiinf_corridor(self): self.assertAlmostEqual(1.0, rect.distance_squared([1e16, 4.0]), 8) self.assertAlmostEqual(4.0, rect.distance_squared([1e16, -4.0]), 8) + def test_second_order_cone(self): + soc = og.constraints.SecondOrderCone() + sq_dist = soc.distance_squared([1.0, 2.0, 1.0]) + sq_dist = soc.distance_squared([0.0, 0.0, 0.0]) + self.assertAlmostEqual(0.0, sq_dist, 8) + pass + + def test_second_order_cone_jacobian(self): + soc = og.constraints.SecondOrderCone() + u = cs.SX.sym('u', 3) + sq_dist = soc.distance_squared(u) + sq_dist_jac = cs.jacobian(sq_dist, u) + sq_dist_jac_fun = cs.Function('sq_dist_jac', [u], [sq_dist_jac]) + v = sq_dist_jac_fun([0., 0., 0.]) + for i in range(3): + print(v[i]) + self.assertFalse(math.isnan(v[i]), "v[i] is NaN") + if __name__ == '__main__': unittest.main() From e1d86d006cd787d50497da65321a0858ac598aec Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Mon, 23 Sep 2019 17:14:41 +0100 Subject: [PATCH 11/42] hallelujah: SOC works now (no NaNs) using cs.if_else to define squared dist to SOC --- open-codegen/opengen/constraints/soc.py | 45 +++++++++++-------- open-codegen/opengen/test/test_constraints.py | 5 ++- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/open-codegen/opengen/constraints/soc.py b/open-codegen/opengen/constraints/soc.py index 61f0cd13..a54add12 100644 --- a/open-codegen/opengen/constraints/soc.py +++ b/open-codegen/opengen/constraints/soc.py @@ -41,6 +41,9 @@ def distance_squared(self, u): :return: distance from set as a float or a CasADi symbol """ + if isinstance(u, cs.SX): + raise Exception("This function does not accept casadi.SX; use casadi.MX instead") + if fn.is_symbolic(u): # Case I: `u` is a CasADi SX symbol nu = u.size(1) @@ -54,24 +57,30 @@ def distance_squared(self, u): a = self.__a x = u[0:nu-1] r = u[nu-1] - # norm of x - norm_x = fn.norm2(x) - # The first branch is zero - # Second branch: - condition2 = r <= -norm_x - fun2 = cs.dot(x, x) + r ** 2 - # Third branch: - condition3 = (-norm_x < r) * (norm_x > r) - beta = a**2/(a**2 + 1.0) - fun3 = norm_x ** 2 + beta * (a * norm_x + r) ** 2 \ - - 2.0 * a * norm_x * (a*norm_x + r) \ - + (r - (a * norm_x + r)/(a**2 + 1.0)) ** 2 - - y = cs.if_else(condition2*(1-condition3), fun2, fun3) - - # Function defined piecewise - # y = condition2 * fun2 + condition3 * fun3 - return y + + eps = 1e-16 + + norm_x = fn.norm2(cs.fabs(x)) # norm of x + sq_norm_x = cs.dot(x, x) # squared norm of x + beta = a ** 2 / (a ** 2 + 1.0) + + fun1 = 0 + fun2 = sq_norm_x + r ** 2 + fun3 = norm_x ** 2 \ + + beta * (a * norm_x + r) ** 2 \ + - 2.0 * a * norm_x * (a * norm_x + r) \ + + (r - (a * norm_x + r)/(a ** 2 + 1.0)) ** 2 + + condition0 = norm_x + cs.fabs(r) < eps + condition1 = r >= norm_x/a + condition2 = r <= -a*norm_x + + f = cs.if_else(condition0, 0, cs.if_else(condition1, fun1, + cs.if_else(condition2, fun2, fun3, True), True), True) + + cs.Function + + return f def project(self, u): # Idea: Computes projection on Ball as follows diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index 23d60c18..d5e958cc 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -134,13 +134,14 @@ def test_second_order_cone(self): def test_second_order_cone_jacobian(self): soc = og.constraints.SecondOrderCone() - u = cs.SX.sym('u', 3) + # Important note: the second-order cone constraint does not work with cs.MX + # An exception will be raised if the user provides an SX + u = cs.MX.sym('u', 3) sq_dist = soc.distance_squared(u) sq_dist_jac = cs.jacobian(sq_dist, u) sq_dist_jac_fun = cs.Function('sq_dist_jac', [u], [sq_dist_jac]) v = sq_dist_jac_fun([0., 0., 0.]) for i in range(3): - print(v[i]) self.assertFalse(math.isnan(v[i]), "v[i] is NaN") From fbbbd32d5d9917dc96502c9cb869c32634a16fba Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Mon, 23 Sep 2019 19:45:42 +0100 Subject: [PATCH 12/42] unit tests for sq dist to SOC --- open-codegen/opengen/constraints/soc.py | 18 +++--- open-codegen/opengen/test/test_constraints.py | 59 +++++++++++++++++-- 2 files changed, 62 insertions(+), 15 deletions(-) diff --git a/open-codegen/opengen/constraints/soc.py b/open-codegen/opengen/constraints/soc.py index a54add12..56c6c632 100644 --- a/open-codegen/opengen/constraints/soc.py +++ b/open-codegen/opengen/constraints/soc.py @@ -2,6 +2,7 @@ import numpy as np from .constraint import Constraint import opengen.functions as fn +import warnings class SecondOrderCone(Constraint): @@ -42,7 +43,7 @@ def distance_squared(self, u): """ if isinstance(u, cs.SX): - raise Exception("This function does not accept casadi.SX; use casadi.MX instead") + warnings.warn("This function does not accept casadi.SX; use casadi.MX instead") if fn.is_symbolic(u): # Case I: `u` is a CasADi SX symbol @@ -60,26 +61,21 @@ def distance_squared(self, u): eps = 1e-16 - norm_x = fn.norm2(cs.fabs(x)) # norm of x + norm_x = cs.norm_2(x) # norm of x sq_norm_x = cs.dot(x, x) # squared norm of x - beta = a ** 2 / (a ** 2 + 1.0) + gamma = (a * norm_x + r)/(a**2 + 1) fun1 = 0 fun2 = sq_norm_x + r ** 2 - fun3 = norm_x ** 2 \ - + beta * (a * norm_x + r) ** 2 \ - - 2.0 * a * norm_x * (a * norm_x + r) \ - + (r - (a * norm_x + r)/(a ** 2 + 1.0)) ** 2 + fun3 = sq_norm_x * (1 - gamma * a / norm_x)**2 + (r - gamma)**2 condition0 = norm_x + cs.fabs(r) < eps - condition1 = r >= norm_x/a - condition2 = r <= -a*norm_x + condition1 = norm_x <= a*r + condition2 = a*norm_x <= -r f = cs.if_else(condition0, 0, cs.if_else(condition1, fun1, cs.if_else(condition2, fun2, fun3, True), True), True) - cs.Function - return f def project(self, u): diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index d5e958cc..59183785 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -4,8 +4,13 @@ import numpy as np import math + class ConstraintsTestCase(unittest.TestCase): + # ----------------------------------------------------------------------- + # Infinity Ball + # ----------------------------------------------------------------------- + def test_ball_inf_origin(self): ball = og.constraints.BallInf(None, 1) x = np.array([3, 2]) @@ -44,6 +49,10 @@ def test_ball_inf_xc(self): self.assertAlmostEqual(d_num, correct_squared_distance, 8, "expected squared distance") + # ----------------------------------------------------------------------- + # Euclidean Ball + # ----------------------------------------------------------------------- + def test_ball_euclidean_origin(self): ball = og.constraints.Ball2(None, 1) x = np.array([2, 2]) @@ -88,6 +97,10 @@ def test_ball_euclidean_xc_inside(self): self.assertAlmostEqual(d_sym, correct_squared_distance, 8, "expected squared distance") + # ----------------------------------------------------------------------- + # Rectangle + # ----------------------------------------------------------------------- + def test_rectangle_simple(self): rect = og.constraints.Rectangle([-1, -2], [4, -1]) # some basic assertions @@ -125,12 +138,48 @@ def test_rectangle_semiinf_corridor(self): self.assertAlmostEqual(1.0, rect.distance_squared([1e16, 4.0]), 8) self.assertAlmostEqual(4.0, rect.distance_squared([1e16, -4.0]), 8) + # ----------------------------------------------------------------------- + # Second-Order Cone (SOC) + # ----------------------------------------------------------------------- + def test_second_order_cone(self): - soc = og.constraints.SecondOrderCone() - sq_dist = soc.distance_squared([1.0, 2.0, 1.0]) + soc = og.constraints.SecondOrderCone(2.0) + + # dist_C^2(0, 0, 0) = 0 [origin is in the cone] sq_dist = soc.distance_squared([0.0, 0.0, 0.0]) - self.assertAlmostEqual(0.0, sq_dist, 8) - pass + self.assertAlmostEqual(0, sq_dist, 16) + + # dist_C^2(0, 0, 0) = 0 [close-origin] + sq_dist = soc.distance_squared([1e-12, 1e-12, 1e-12]) + self.assertAlmostEqual(0, sq_dist, 16) + + # dist_C^2(1, 1, 0.75) = 0 [case II] + sq_dist = soc.distance_squared([1.0, 1.0, 0.75]) + self.assertAlmostEqual(0, sq_dist, 16) + + # dist_C^2(3, 4, -11) = 146.0 [case II] + sq_dist = soc.distance_squared([3.0, 4.0, -11.0]) + self.assertAlmostEqual(146.0, sq_dist, 16) + sq_dist = soc.distance_squared([4.0, 3.0, -11.0]) + self.assertAlmostEqual(146.0, sq_dist, 16) + + # dist_C^2(2, 3, 0.5) = 1.357... [case III] + sq_dist = soc.distance_squared([2.0, 3.0, 0.5]) + self.assertAlmostEqual(1.35777948981440, sq_dist, 12) + + def test_second_order_cone_symbolic(self): + soc = og.constraints.SecondOrderCone(2.0) + u = cs.SX.sym('u', 3, 1) + sq_dist = soc.distance_squared(u) + u0 = [4.0, 3.0, -11.0] + + sq_dist_sx_fun = cs.Function('sqd1', [u], [sq_dist]) + self.assertAlmostEqual(146.0, sq_dist_sx_fun(u0), 16) + + umx = cs.MX.sym('u', 3, 1) + sq_dist_m2 = soc.distance_squared(u) + sq_dist_mx_fun = cs.Function('sqd2', [u], [sq_dist_m2]) + self.assertAlmostEqual(146.0, sq_dist_mx_fun(u0), 16) def test_second_order_cone_jacobian(self): soc = og.constraints.SecondOrderCone() @@ -143,6 +192,8 @@ def test_second_order_cone_jacobian(self): v = sq_dist_jac_fun([0., 0., 0.]) for i in range(3): self.assertFalse(math.isnan(v[i]), "v[i] is NaN") + self.assertAlmostEqual(0, cs.norm_2(v), 12) + if __name__ == '__main__': From dbea9fad2c19ba4e059bed6a82dda514bc56238a Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 13:09:58 +0100 Subject: [PATCH 13/42] implementation of more constraints experimental Callback for SOC --- open-codegen/opengen/builder/problem.py | 1 - open-codegen/opengen/constraints/__init__.py | 1 + open-codegen/opengen/constraints/ball_inf.py | 2 +- open-codegen/opengen/constraints/cartesian.py | 16 ++++++++++++++++ .../opengen/constraints/no_constraints.py | 13 +++++++++++++ open-codegen/opengen/constraints/soc.py | 6 +----- open-codegen/opengen/functions/__init__.py | 1 + open-codegen/opengen/test/test_constraints.py | 13 +++++++++++++ 8 files changed, 46 insertions(+), 7 deletions(-) create mode 100644 open-codegen/opengen/constraints/cartesian.py create mode 100644 open-codegen/opengen/constraints/no_constraints.py diff --git a/open-codegen/opengen/builder/problem.py b/open-codegen/opengen/builder/problem.py index c5f06d25..e4640e28 100644 --- a/open-codegen/opengen/builder/problem.py +++ b/open-codegen/opengen/builder/problem.py @@ -136,7 +136,6 @@ def dim_constraints_aug_lagrangian(self): """Not implemented yet""" return 0 if self.__alm_mapping_f1 is None \ else self.__alm_mapping_f1.size(1) - return 0 # ---------- OTHER GETTERS ----------------------------------------- diff --git a/open-codegen/opengen/constraints/__init__.py b/open-codegen/opengen/constraints/__init__.py index 8d123be8..642df483 100644 --- a/open-codegen/opengen/constraints/__init__.py +++ b/open-codegen/opengen/constraints/__init__.py @@ -3,3 +3,4 @@ from .constraint import * from .ball_inf import * from .soc import * +from .no_constraints import * \ No newline at end of file diff --git a/open-codegen/opengen/constraints/ball_inf.py b/open-codegen/opengen/constraints/ball_inf.py index aacdebb6..c2cd594b 100644 --- a/open-codegen/opengen/constraints/ball_inf.py +++ b/open-codegen/opengen/constraints/ball_inf.py @@ -83,5 +83,5 @@ def distance_squared(self, u): return squared_distance def project(self, u): - # Computes projection on Ball + # Computes projection on infinity Ball raise NotImplementedError("Method `project` is not implemented") \ No newline at end of file diff --git a/open-codegen/opengen/constraints/cartesian.py b/open-codegen/opengen/constraints/cartesian.py new file mode 100644 index 00000000..76c83291 --- /dev/null +++ b/open-codegen/opengen/constraints/cartesian.py @@ -0,0 +1,16 @@ +import casadi.casadi as cs +import numpy as np +from .constraint import Constraint +import opengen.functions as fn + + +class CartesianProduct(Constraint): + + def __init__(self): + pass + + def distance_squared(self, u): + raise NotImplementedError() + + def project(self, u): + raise NotImplementedError() diff --git a/open-codegen/opengen/constraints/no_constraints.py b/open-codegen/opengen/constraints/no_constraints.py new file mode 100644 index 00000000..e9626f98 --- /dev/null +++ b/open-codegen/opengen/constraints/no_constraints.py @@ -0,0 +1,13 @@ +from .constraint import Constraint + + +class NoConstraints(Constraint): + + def __init__(self): + pass + + def distance_squared(self, u): + return 0.0 + + def project(self, u): + return u diff --git a/open-codegen/opengen/constraints/soc.py b/open-codegen/opengen/constraints/soc.py index 56c6c632..b37ad812 100644 --- a/open-codegen/opengen/constraints/soc.py +++ b/open-codegen/opengen/constraints/soc.py @@ -46,7 +46,6 @@ def distance_squared(self, u): warnings.warn("This function does not accept casadi.SX; use casadi.MX instead") if fn.is_symbolic(u): - # Case I: `u` is a CasADi SX symbol nu = u.size(1) elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ or isinstance(u, np.ndarray): @@ -61,7 +60,7 @@ def distance_squared(self, u): eps = 1e-16 - norm_x = cs.norm_2(x) # norm of x + norm_x = fn.norm2(x) # norm of x sq_norm_x = cs.dot(x, x) # squared norm of x gamma = (a * norm_x + r)/(a**2 + 1) @@ -79,8 +78,5 @@ def distance_squared(self, u): return f def project(self, u): - # Idea: Computes projection on Ball as follows - # Proj_B(u) = u / max{1, ||u||}, - # which avoids dividing by zero or defining the projections raise NotImplementedError() diff --git a/open-codegen/opengen/functions/__init__.py b/open-codegen/opengen/functions/__init__.py index 07d0d3ec..e8f88d8c 100644 --- a/open-codegen/opengen/functions/__init__.py +++ b/open-codegen/opengen/functions/__init__.py @@ -6,3 +6,4 @@ from .sign import * from .norm2 import * from .fabs import * +from .sq_dist_to_soc import * diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index 59183785..af7c1a91 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -194,7 +194,20 @@ def test_second_order_cone_jacobian(self): self.assertFalse(math.isnan(v[i]), "v[i] is NaN") self.assertAlmostEqual(0, cs.norm_2(v), 12) + # ----------------------------------------------------------------------- + # No Constraints + # ----------------------------------------------------------------------- + + def test_no_constraints(self): + whole_rn = og.constraints.NoConstraints() + u = [1., 2., 3., 4.] + self.assertAlmostEqual(0.0, whole_rn.distance_squared(u), 16) + self.assertListEqual(u, whole_rn.project(u)) + # ----------------------------------------------------------------------- + # Cartesian product of constraints + # ----------------------------------------------------------------------- + if __name__ == '__main__': unittest.main() From 060ff9e8ecedc80a2056ac0bf540f3d5124a534f Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 13:58:46 +0100 Subject: [PATCH 14/42] cartesian constraints: unit tests --- open-codegen/opengen/constraints/__init__.py | 3 +- open-codegen/opengen/constraints/cartesian.py | 20 +++++++++-- open-codegen/opengen/test/test_constraints.py | 36 ++++++++++++++++++- 3 files changed, 54 insertions(+), 5 deletions(-) diff --git a/open-codegen/opengen/constraints/__init__.py b/open-codegen/opengen/constraints/__init__.py index 642df483..e7c7c8db 100644 --- a/open-codegen/opengen/constraints/__init__.py +++ b/open-codegen/opengen/constraints/__init__.py @@ -3,4 +3,5 @@ from .constraint import * from .ball_inf import * from .soc import * -from .no_constraints import * \ No newline at end of file +from .no_constraints import * +from .cartesian import * \ No newline at end of file diff --git a/open-codegen/opengen/constraints/cartesian.py b/open-codegen/opengen/constraints/cartesian.py index 76c83291..a0ab40d1 100644 --- a/open-codegen/opengen/constraints/cartesian.py +++ b/open-codegen/opengen/constraints/cartesian.py @@ -2,15 +2,29 @@ import numpy as np from .constraint import Constraint import opengen.functions as fn +from typing import List class CartesianProduct(Constraint): - def __init__(self): - pass + def __init__(self, dimension: int, segments: List[int], constraints: List[Constraint]): + # TODO: Check whether input arguments are valid (types, ranges, etc) + self.__dimension = dimension + self.__segments = segments + self.__constraints = constraints def distance_squared(self, u): - raise NotImplementedError() + squared_distance = 0.0 + num_segments = len(self.__segments) + idx_previous = -1 + for i in range(num_segments): + idx_current = self.__segments[i] + ui = u[idx_previous+1:idx_current+1] + current_sq_dist = self.__constraints[i].distance_squared(ui) + idx_previous = idx_current + squared_distance += current_sq_dist + + return squared_distance def project(self, u): raise NotImplementedError() diff --git a/open-codegen/opengen/test/test_constraints.py b/open-codegen/opengen/test/test_constraints.py index af7c1a91..fa3beb45 100644 --- a/open-codegen/opengen/test/test_constraints.py +++ b/open-codegen/opengen/test/test_constraints.py @@ -64,6 +64,13 @@ def test_ball_euclidean_origin(self): self.assertAlmostEqual(d_sym, correct_squared_distance, 8, "expected squared distance") + def test_ball_euclidean_origin_3d(self): + ball = og.constraints.Ball2(None, 1) + x = np.array([1, 1, 1]) + d_num = ball.distance_squared(x) + correct_squared_distance = 0.535898384862246 + self.assertAlmostEqual(correct_squared_distance, d_num, 12, "computation of distance") + def test_ball_euclidean_origin_inside(self): ball = og.constraints.Ball2(None, 1) x = np.array([0.2, 0.8]) @@ -208,6 +215,33 @@ def test_no_constraints(self): # Cartesian product of constraints # ----------------------------------------------------------------------- - + def test_cartesian(self): + inf = float('inf') + ball_inf = og.constraints.BallInf(None, 1) + ball_eucl = og.constraints.Ball2(None, 1) + rect = og.constraints.Rectangle(xmin=[0.0, 1.0, -inf, 2.0], + xmax=[1.0, inf, 10.0, 10.0]) + # Segments: + # [0, 1] + # [2, 3, 4] + # [5, 6, 7, 8] + cartesian = og.constraints.CartesianProduct(9, [1, 4, 8], [ball_inf, ball_eucl, rect]) + sq_dist = cartesian.distance_squared([5, 10, 1, 1, 1, 0.5, -1, 0, 11]) + correct_sq_distance = 102.0 + (math.sqrt(3)-1.0)**2 + self.assertAlmostEqual(correct_sq_distance, sq_dist, 12) + + def test_cartesian_sx(self): + inf = float('inf') + ball_inf = og.constraints.BallInf(None, 1) + ball_eucl = og.constraints.Ball2(None, 1) + rect = og.constraints.Rectangle(xmin=[0.0, 1.0, -inf, 2.0], + xmax=[1.0, inf, 10.0, 10.0]) + cartesian = og.constraints.CartesianProduct(9, [1, 4, 8], [ball_inf, ball_eucl, rect]) + u_sx = cs.SX.sym("u", 9, 1) + sqd_sx = cartesian.distance_squared(u_sx) + u_mx = cs.SX.sym("u", 9, 1) + sqd_mx = cartesian.distance_squared(u_mx) + + if __name__ == '__main__': unittest.main() From a69d78f5b99f26b4fdac422ede960859bcb5564c Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 14:07:23 +0100 Subject: [PATCH 15/42] add file sq_dist_to_soc.py --- .../opengen/functions/sq_dist_to_soc.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 open-codegen/opengen/functions/sq_dist_to_soc.py diff --git a/open-codegen/opengen/functions/sq_dist_to_soc.py b/open-codegen/opengen/functions/sq_dist_to_soc.py new file mode 100644 index 00000000..2e3a926a --- /dev/null +++ b/open-codegen/opengen/functions/sq_dist_to_soc.py @@ -0,0 +1,32 @@ +import casadi as cs + + +class SqDistSOC(cs.Callback): + + def __init__(self, name, opts={}): + cs.Callback.__init__(self) + self.construct(name, opts) + + def get_n_in(self): return 1 + + def get_n_out(self): return 1 + + def init(self): + pass + + def eval(self, arg): + x = arg[0] + f = 0.5 * cs.dot(x, x) + return [f] + + def has_jacobian(self, *_args): + return False + + def has_forward(self, *args): + return True + + def get_forward(self, *args): + x = cs.MX.sym('x', 1) + z = cs.MX.sym('x', 1) + return cs.Function('fw', [x, z], [1]) + From 335166020f2d3ac22ce51b28ccc5955f52bbc98f Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 15:35:22 +0100 Subject: [PATCH 16/42] code generation for F1 and F2 added docs in auto-generated file modified a few function names in Rust-to-C interface --- .../opengen/builder/optimizer_builder.py | 23 +++ open-codegen/opengen/config/build_config.py | 7 +- open-codegen/opengen/functions/rosenbrock.py | 6 +- open-codegen/opengen/icasadi/build.rs | 8 +- .../opengen/icasadi/extern/README.txt | 1 + .../opengen/templates/icasadi_lib.rs.template | 142 +++++++++++++++--- .../opengen/templates/optimizer.rs.template | 2 +- 7 files changed, 161 insertions(+), 28 deletions(-) create mode 100644 open-codegen/opengen/icasadi/extern/README.txt diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 66ebdace..5ea59ccc 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -14,6 +14,7 @@ _AUTOGEN_COST_FNAME = 'auto_casadi_cost.c' _AUTOGEN_GRAD_FNAME = 'auto_casadi_grad.c' _AUTOGEN_PNLT_CONSTRAINTS_FNAME = 'auto_casadi_constraints_type_penalty.c' +_AUTOGEN_ALM_MAPPING_F1_FNAME = 'auto_casadi_mapping_f1.c' def make_dir_if_not_exists(directory): @@ -181,6 +182,8 @@ def __generate_casadi_code(self): u = self.__problem.decision_variables p = self.__problem.parameter_variables ncp = self.__problem.dim_constraints_penalty() + nalm = self.__problem.dim_constraints_aug_lagrangian() + print("number of ALM constraints:", nalm) phi = self.__problem.cost_function # If there are penalty-type constraints, we need to define a modified @@ -209,6 +212,26 @@ def __generate_casadi_code(self): shutil.move(cost_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_COST_FNAME)) shutil.move(grad_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_GRAD_FNAME)) + # Next, we generate a CasADi function for the augmented Lagrangian constraints, + # that is, mapping F1(u, p) + if nalm > 0: + mapping_f1 = self.__problem.penalty_mapping_f1 + else: + mapping_f1 = 0 + + # Target C file name of mapping F1(u, p) + alm_mapping_f1_file_name = \ + self.__build_config.alm_mapping_f1_function_name + ".c" + # Define CasADi function F1(u, p) + alm_mapping_f1_fun = cs.Function( + self.__build_config.alm_mapping_f1_function_name, + [u, p], [mapping_f1]) + # Generate code for F1(u, p) + alm_mapping_f1_fun.generate(alm_mapping_f1_file_name) + # Move auto-generated file to target folder + shutil.move(alm_mapping_f1_file_name, + os.path.join(icasadi_extern_dir, _AUTOGEN_ALM_MAPPING_F1_FNAME)) + # Lastly, we generate code for the penalty constraints; if there aren't # any, we generate the function c(u; p) = 0 (which will not be used) if ncp > 0: diff --git a/open-codegen/opengen/config/build_config.py b/open-codegen/opengen/config/build_config.py index 867693f4..25e70f34 100644 --- a/open-codegen/opengen/config/build_config.py +++ b/open-codegen/opengen/config/build_config.py @@ -26,7 +26,8 @@ def __init__(self, build_dir="."): self.__build_mode = 'release' self.__cost_function_name = 'phi_' + random_string self.__grad_cost_function_name = 'grad_phi_' + random_string - self.__constraint_penalty_function = 'constraints_penalty_' + random_string + self.__constraint_penalty_function = 'mapping_f2_' + random_string + self.__alm_constraints_mapping_f1 = 'mapping_f1_' + random_string self.__rebuild = False self.__build_dir = build_dir self.__open_version = None @@ -52,6 +53,10 @@ def grad_function_name(self): def constraint_penalty_function_name(self): return self.__constraint_penalty_function + @property + def alm_mapping_f1_function_name(self): + return self.__alm_constraints_mapping_f1 + @property def target_system(self): """Target system""" diff --git a/open-codegen/opengen/functions/rosenbrock.py b/open-codegen/opengen/functions/rosenbrock.py index 9e8a29e7..cdbbc148 100644 --- a/open-codegen/opengen/functions/rosenbrock.py +++ b/open-codegen/opengen/functions/rosenbrock.py @@ -1,11 +1,11 @@ -import casadi.casadi as cs +from opengen.functions.is_symbolic import * def rosenbrock(u_, p_): """Rosenbrock functions with parameters p = [a, b]""" - if not isinstance(p_, cs.SX) or p_.size()[0] != 2: + if not is_symbolic(p_) or p_.size()[0] != 2: raise Exception('illegal parameter p_ (must be SX of size (2,1))') - if not isinstance(u_, cs.SX): + if not is_symbolic(u_): raise Exception('illegal parameter u_ (must be SX)') nu = u_.size()[0] a = p_[0] diff --git a/open-codegen/opengen/icasadi/build.rs b/open-codegen/opengen/icasadi/build.rs index 0afb4b70..0e56ac63 100644 --- a/open-codegen/opengen/icasadi/build.rs +++ b/open-codegen/opengen/icasadi/build.rs @@ -5,7 +5,11 @@ fn main() { // Sanity checks to get better error messages assert!( Path::new("extern/auto_casadi_constraints_type_penalty.c").exists(), - "extern/auto_casadi_cost.c is missing" + "extern/auto_casadi_mapping_f1.c is missing" + ); + 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(), @@ -26,10 +30,12 @@ fn main() { .file("extern/auto_casadi_cost.c") .file("extern/auto_casadi_grad.c") .file("extern/auto_casadi_constraints_type_penalty.c") + .file("extern/auto_casadi_mapping_f1.c") .compile("icasadi"); // 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/auto_casadi_mapping_f1.c"); } diff --git a/open-codegen/opengen/icasadi/extern/README.txt b/open-codegen/opengen/icasadi/extern/README.txt new file mode 100644 index 00000000..b6886b9a --- /dev/null +++ b/open-codegen/opengen/icasadi/extern/README.txt @@ -0,0 +1 @@ +C files auto-generated using CasADi are stored here diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 3593672a..74f8bdd2 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -17,17 +17,32 @@ #![no_std] /// Number of static parameters (this also includes penalty constraints) -pub const NUM_STATIC_PARAMETERS: usize = {{problem.dim_parameters()}} + {{problem.dim_constraints_penalty() or 0}}; +pub const NUM_STATIC_PARAMETERS: usize = {{problem.dim_parameters() + problem.dim_constraints_penalty() or 0}}; /// Number of decision variables pub const NUM_DECISION_VARIABLES: usize = {{problem.dim_decision_variables()}}; -/// Number of penalty constraints +/// Number of ALM-type constraints (dimension of F1, i.e., n1) +pub const NUM_CONSTRAINTS_TYPE_ALM: usize = {{problem.dim_constraints_aug_lagrangian() or 0}}; + +/// Number of penalty constraints (dimension of F2, i.e., n2) pub const NUM_CONSTAINTS_TYPE_PENALTY: usize = {{problem.dim_constraints_penalty() or 0}}; use libc::{c_double, c_int, c_longlong, c_void}; +/// C interface (Function API exactly as provided by CasADi) extern "C" { + + /// Cost function, f(u, p), generated by CasADi + /// + /// + /// ## Arguments + /// + /// - `arg`: function arguemnts (u and p) + /// - `casadi_results`: + /// - `iw`: integer workspace (here: empty) + /// - `w`: workspace (here: empty) + /// - `mem`: memory (here, 0) fn {{build_config.cost_function_name or 'phi'}}( arg: *const *const c_double, casadi_results: *mut *mut c_double, @@ -36,6 +51,17 @@ extern "C" { mem: *mut c_void, ) -> c_int; + + /// Gradient of the cost function, Df(u, p), generated by CasADi + /// + /// + /// ## Arguments + /// + /// - `arg`: function arguemnts (u and p) + /// - `casadi_results`: + /// - `iw`: integer workspace (here: empty) + /// - `w`: workspace (here: empty) + /// - `mem`: memory (here, 0) fn {{build_config.grad_function_name or 'grad_phi'}}( arg: *const *const c_double, casadi_results: *mut *mut c_double, @@ -44,7 +70,35 @@ extern "C" { mem: *mut c_void, ) -> c_int; - fn {{build_config.constraint_penalty_function_name or 'constraints_penalty'}}( + /// Penalty-related mapping, F1(u, p), generated by CasADi + /// + /// + /// ## Arguments + /// + /// - `arg`: function arguemnts (u and p) + /// - `casadi_results`: + /// - `iw`: integer workspace (here: empty) + /// - `w`: workspace (here: empty) + /// - `mem`: memory (here, 0) + fn {{build_config.alm_mapping_f1_function_name or 'mapping_f1'}}( + arg: *const *const c_double, + casadi_results: *mut *mut c_double, + iw: *mut c_longlong, + w: *mut c_double, + mem: *mut c_void, + ) -> c_int; + + /// Penalty-related mapping, F2(u, p), generated by CasADi + /// + /// + /// ## Arguments + /// + /// - `arg`: function arguemnts (u and p) + /// - `casadi_results`: + /// - `iw`: integer workspace (here: empty) + /// - `w`: workspace (here: empty) + /// - `mem`: memory (here, 0) + fn {{build_config.constraint_penalty_function_name or 'mapping_f2'}}( arg: *const *const c_double, casadi_results: *mut *mut c_double, iw: *mut c_longlong, @@ -67,14 +121,10 @@ extern "C" { /// ``` /// /// # Panics -/// This method does not panic (on purpose). However, users need to be -/// careful when providing the arguments `u` and `static_params` -/// as they must be arrays of appropriate size. -/// -/// As a safety measure, you may check whether +/// This method panics if the following conditions are not satisfied /// -/// - `u.len() >= NUM_DECISION_VARIABLES` -/// - `static_params.len() >= NUM_STATIC_PARAMETERS` +/// - `u.len() == NUM_DECISION_VARIABLES` +/// - `static_params.len() == NUM_STATIC_PARAMETERS` /// pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES); @@ -108,15 +158,11 @@ pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { /// ``` /// /// # Panics -/// This method does not panic (on purpose). However, users need to be -/// careful when providing the arguments `u` and `static_params` -/// as they must be arrays of appropriate size. +/// This method panics if the following conditions are not satisfied /// -/// As a safety measure, you may check whether -/// -/// - `u.len() >= icasadi::num_decision_variables()` -/// - `static_params.len() >= icasadi::num_static_parameters()` -/// - `cost_jacobian.len() >= icasadi::num_decision_variables()` +/// - `u.len() == icasadi::num_decision_variables()` +/// - `static_params.len() == icasadi::num_static_parameters()` +/// - `cost_jacobian.len() == icasadi::num_decision_variables()` /// pub fn grad(u: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES); @@ -137,18 +183,70 @@ pub fn grad(u: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 } } -pub fn constraints_as_penalty( + +/// Consume mapping F1, which has been generated by CasADi +/// +/// This is a wrapper function +/// +/// ## Arguments +/// +/// - `u`: (in) decision variables +/// - `p`: (in) vector of parameters +/// - `f1`: (out) value F2(u, p) +/// +/// ## Returns +/// +/// Returns `0` iff the computation is successful +/// +pub fn mapping_f1( + u: &[f64], + static_params: &[f64], + f1: &mut [f64], +) -> i32 { + assert_eq!(u.len(), NUM_DECISION_VARIABLES); + assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); + assert!(f1.len() == NUM_CONSTAINTS_TYPE_PENALTY || + NUM_CONSTAINTS_TYPE_PENALTY == 0); + + let arguments = &[u.as_ptr(), static_params.as_ptr()]; + let constraints = &mut [f1.as_mut_ptr()]; + + unsafe { + {{build_config.alm_mapping_f1_function_name or 'mapping_f1'}}( + arguments.as_ptr(), + constraints.as_mut_ptr(), + 0 as *mut c_longlong, + 0 as *mut c_double, + 0 as *mut c_void, + ) as i32 + } +} + +/// Consume mapping F2, which has been generated by CasADi +/// +/// This is a wrapper function +/// +/// ## Arguments +/// +/// - `u`: (in) decision variables +/// - `p`: (in) vector of parameters +/// - `f2`: (out) value F2(u, p) +/// +/// ## Returns +/// +/// Returns `0` iff the computation is successful +pub fn mapping_f2( u: &[f64], static_params: &[f64], - constraints_as_penalty: &mut [f64], + f2: &mut [f64], ) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES); assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); - assert!(constraints_as_penalty.len() == NUM_CONSTAINTS_TYPE_PENALTY || + assert!(f2.len() == NUM_CONSTAINTS_TYPE_PENALTY || NUM_CONSTAINTS_TYPE_PENALTY == 0); let arguments = &[u.as_ptr(), static_params.as_ptr()]; - let constraints = &mut [constraints_as_penalty.as_mut_ptr()]; + let constraints = &mut [f2.as_mut_ptr()]; unsafe { {{build_config.constraint_penalty_function_name or 'constraints_penalty'}}( diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index 4d347089..f17f0ae5 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -256,7 +256,7 @@ pub fn solve( /* penalty-type constraints: c(u; p) */ let penalty_constr_function = |u: &[f64], q: &[f64], constraints: &mut [f64]| -> Result<(), SolverError> { - if icasadi::constraints_as_penalty(u, q, constraints) == 0 { + if icasadi::mapping_f2(u, q, constraints) == 0 { Ok(()) } else { Err(SolverError::Cost) From b887c2ded95fa4d1ad074f50376649588a8d9384 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 22:32:00 +0100 Subject: [PATCH 17/42] added memory allocator for casadi introduced icallocator.c: icasadi functions may need to allocate some workspace once (wip) --- .../opengen/builder/optimizer_builder.py | 20 +++- open-codegen/opengen/config/build_config.py | 5 + open-codegen/opengen/icasadi/build.rs | 6 + open-codegen/opengen/icasadi/extern/main.c | 35 ------ .../opengen/templates/icallocator.c.template | 103 ++++++++++++++++++ .../opengen/templates/icasadi_lib.rs.template | 37 ++++++- 6 files changed, 168 insertions(+), 38 deletions(-) delete mode 100644 open-codegen/opengen/icasadi/extern/main.c create mode 100644 open-codegen/opengen/templates/icallocator.c.template diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 5ea59ccc..f0d49da4 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -142,6 +142,20 @@ def __copy_icasadi_to_target(self): ignore=shutil.ignore_patterns( '*.lock', 'ci*', 'target', 'auto*')) + def __generate_icasadi_memory_allocator(self): + logging.info("Generating icallocator.c") + file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) + env = jinja2.Environment(loader=file_loader) + template = env.get_template('icallocator.c.template') + output_template = template.render(meta=self.__meta, + problem=self.__problem, + build_config=self.__build_config, + timestamp_created=datetime.datetime.now()) + icallocator_path = os.path.abspath( + os.path.join(self.__icasadi_target_dir(), "extern/icallocator.c")) + with open(icallocator_path, "w") as fh: + fh.write(output_template) + def __generate_icasadi_lib(self): """Generates the Rust library file of icasadi @@ -185,12 +199,14 @@ def __generate_casadi_code(self): nalm = self.__problem.dim_constraints_aug_lagrangian() print("number of ALM constraints:", nalm) phi = self.__problem.cost_function + print("number of PM constraints:", ncp) # If there are penalty-type constraints, we need to define a modified # cost function if ncp > 0: penalty_function = self.__problem.penalty_function - mu = cs.SX.sym("mu", self.__problem.dim_constraints_penalty()) + mu = cs.SX.sym("mu", self.__problem.dim_constraints_penalty()) \ + if isinstance(p, cs.SX) else cs.MX.sym("mu", self.__problem.dim_constraints_penalty()) p = cs.vertcat(p, mu) phi += cs.dot(mu, penalty_function(self.__problem.penalty_mapping_f2)) @@ -423,11 +439,13 @@ def build(self): self.__prepare_target_project() # create folders; init cargo project self.__copy_icasadi_to_target() # copy icasadi/ files to target dir self.__generate_cargo_toml() # generate Cargo.toml using template + self.__generate_icasadi_memory_allocator() # generate icasadi/extern/icallocator.c self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) self.__generate_build_rs() # generate build.rs file self.__generate_yaml_data_file() + if not self.__generate_not_build: logging.info("Building optimizer") self.__build_optimizer() # build overall project diff --git a/open-codegen/opengen/config/build_config.py b/open-codegen/opengen/config/build_config.py index 25e70f34..7e06fc5e 100644 --- a/open-codegen/opengen/config/build_config.py +++ b/open-codegen/opengen/config/build_config.py @@ -24,6 +24,7 @@ def __init__(self, build_dir="."): self.__target_system = 'default' self.__build_mode = 'release' + self.__id = random_string self.__cost_function_name = 'phi_' + random_string self.__grad_cost_function_name = 'grad_phi_' + random_string self.__constraint_penalty_function = 'mapping_f2_' + random_string @@ -41,6 +42,10 @@ def rebuild(self): """Whether to re-build the optimizer from scratch""" return self.__rebuild + @property + def id(self): + return self.__id + @property def cost_function_name(self): return self.__cost_function_name diff --git a/open-codegen/opengen/icasadi/build.rs b/open-codegen/opengen/icasadi/build.rs index 0e56ac63..79360dea 100644 --- a/open-codegen/opengen/icasadi/build.rs +++ b/open-codegen/opengen/icasadi/build.rs @@ -19,6 +19,10 @@ fn main() { Path::new("extern/auto_casadi_grad.c").exists(), "extern/auto_casadi_grad.c is missing" ); + assert!( + Path::new("extern/icallocator.c").exists(), + "extern/icallocator.c is missing" + ); cc::Build::new() .flag_if_supported("-Wall") @@ -31,6 +35,7 @@ fn main() { .file("extern/auto_casadi_grad.c") .file("extern/auto_casadi_constraints_type_penalty.c") .file("extern/auto_casadi_mapping_f1.c") + .file("extern/icallocator.c") .compile("icasadi"); // Rerun if these autogenerated files change @@ -38,4 +43,5 @@ fn main() { 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/auto_casadi_mapping_f1.c"); + println!("cargo:rerun-if-changed=extern/icallocator.c"); } diff --git a/open-codegen/opengen/icasadi/extern/main.c b/open-codegen/opengen/icasadi/extern/main.c deleted file mode 100644 index a3c3f8e3..00000000 --- a/open-codegen/opengen/icasadi/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/open-codegen/opengen/templates/icallocator.c.template b/open-codegen/opengen/templates/icallocator.c.template new file mode 100644 index 00000000..0010e4df --- /dev/null +++ b/open-codegen/opengen/templates/icallocator.c.template @@ -0,0 +1,103 @@ +#include +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int long long int +#endif + +#define TRUE 1 +#define FALSE 0 + +extern int {{build_config.cost_function_name or 'phi'}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + +extern int {{build_config.grad_function_name or 'grad_phi'}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + +static char is_allocated = FALSE; +static casadi_int *allocated_i_workspace_cost; +static casadi_real *allocated_r_workspace_cost; +static casadi_int *allocated_i_workspace_grad; +static casadi_real *allocated_r_workspace_grad; + +/* returns 0 if the allocation of memory was successful */ +static int allocate_if_not_yet() { + + /* Sizes for cost function */ + casadi_int sz_arg_cost = 0; + casadi_int sz_res_cost = 0; + casadi_int sz_iw_cost = 0; + casadi_int sz_w_cost = 0; + + /* Sizes for gradient */ + casadi_int sz_arg_grad = 0; + casadi_int sz_res_grad = 0; + casadi_int sz_iw_grad = 0; + casadi_int sz_w_grad = 0; + + /* Obtain sizes */ + {{build_config.cost_function_name or 'phi'}}_work(&sz_arg_cost, &sz_res_cost, &sz_iw_cost, &sz_w_cost); + {{build_config.grad_function_name or 'grad_phi'}}_work(&sz_arg_grad, &sz_res_grad, &sz_iw_grad, &sz_w_grad); + + printf("cost = (%lld, %lld, %lld, %lld)", sz_arg_cost, sz_res_cost, sz_iw_cost, sz_w_cost); + + /* Allocate memory, if not allocated before */ + if (!is_allocated) { + + /* Allocate memory for cost function */ + allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); + if (allocated_i_workspace_cost == NULL) goto fail_1; + allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); + if (allocated_r_workspace_cost == NULL) goto fail_2; + + /* Allocate memory for gradient */ + allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); + if (allocated_i_workspace_grad == NULL) goto fail_3; + allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); + if (allocated_r_workspace_grad == NULL) goto fail_4; + + } + + return 0; + + /* Free memory that has been previously allocated (failure!) */ + fail_4: + free(allocated_i_workspace_grad); + fail_3: + free(allocated_r_workspace_cost); + fail_2: + free(allocated_i_workspace_cost); + fail_1: + return 1; +} + +int init_{{build_config.id or '0'}}() { + if (!is_allocated){ + return allocate_if_not_yet(); + } + return 0; +} + +int destroy_{{build_config.id or '0'}}() { + return 0; +} + +casadi_int * allocated_{{build_config.cost_function_name or 'phi'}}_iwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_i_workspace_cost; +} + +casadi_real * allocated_{{build_config.cost_function_name or 'phi'}}_rwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_r_workspace_cost; +} \ No newline at end of file diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 74f8bdd2..c1d5ec62 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -14,6 +14,7 @@ //! Generated at: {{timestamp_created}} //! +// We can't use no_std... #![no_std] /// Number of static parameters (this also includes penalty constraints) @@ -33,6 +34,10 @@ use libc::{c_double, c_int, c_longlong, c_void}; /// C interface (Function API exactly as provided by CasADi) extern "C" { + // ----------------------------------------------------------- + // Main External (C) functions + // ----------------------------------------------------------- + /// Cost function, f(u, p), generated by CasADi /// /// @@ -105,8 +110,36 @@ extern "C" { w: *mut c_double, mem: *mut c_void, ) -> c_int; + + + + // ----------------------------------------------------------- + // Workspace Length External (C) functions + // ----------------------------------------------------------- + + /// Workspace lengths of cost function + fn allocated_{{build_config.cost_function_name or 'phi'}}_iwork() -> *mut c_longlong; + fn allocated_{{build_config.cost_function_name or 'phi'}}_rwork() -> *mut c_double; + fn init_{{build_config.id or '0'}}() -> c_int; + +} // END of extern C + + + + +// Initialisation +pub fn init() -> i32 { + unsafe { + return init_{{build_config.id or '0'}}(); + } } + +// ----------------------------------------------------------- +// *MAIN* API Functions in Rust +// ----------------------------------------------------------- + + /// /// Consume the cost function written in C /// @@ -137,8 +170,8 @@ pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { {{build_config.cost_function_name or 'phi'}}( arguments.as_ptr(), cost.as_mut_ptr(), - 0 as *mut c_longlong, - 0 as *mut c_double, + allocated_{{build_config.cost_function_name or 'phi'}}_iwork() as *mut c_longlong, + allocated_{{build_config.cost_function_name or 'phi'}}_rwork() as *mut c_double, 0 as *mut c_void, ) as i32 } From c3d5f01386f47405eca9bdc1023802245bbd4f9b Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Tue, 24 Sep 2019 23:39:44 +0100 Subject: [PATCH 18/42] casadi allocator is complete --- .../opengen/builder/optimizer_builder.py | 2 +- open-codegen/opengen/icasadi/build.rs | 20 +- .../opengen/templates/icallocator.c.template | 192 ++++++++++++++++-- .../opengen/templates/icasadi_lib.rs.template | 48 +++-- 4 files changed, 224 insertions(+), 38 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index f0d49da4..4d4ed3e7 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -13,7 +13,7 @@ _AUTOGEN_COST_FNAME = 'auto_casadi_cost.c' _AUTOGEN_GRAD_FNAME = 'auto_casadi_grad.c' -_AUTOGEN_PNLT_CONSTRAINTS_FNAME = 'auto_casadi_constraints_type_penalty.c' +_AUTOGEN_PNLT_CONSTRAINTS_FNAME = 'auto_casadi_mapping_f2.c' _AUTOGEN_ALM_MAPPING_F1_FNAME = 'auto_casadi_mapping_f1.c' diff --git a/open-codegen/opengen/icasadi/build.rs b/open-codegen/opengen/icasadi/build.rs index 79360dea..9fc6a2e9 100644 --- a/open-codegen/opengen/icasadi/build.rs +++ b/open-codegen/opengen/icasadi/build.rs @@ -3,22 +3,32 @@ use std::path::Path; fn main() { // Sanity checks to get better error messages + + // Check for F1 assert!( - Path::new("extern/auto_casadi_constraints_type_penalty.c").exists(), + Path::new("extern/auto_casadi_mapping_f1.c").exists(), "extern/auto_casadi_mapping_f1.c is missing" ); + + // Check for F2 assert!( - Path::new("extern/auto_casadi_constraints_type_penalty.c").exists(), - "extern/auto_casadi_constraints_type_penalty.c is missing" + Path::new("extern/auto_casadi_mapping_f2.c").exists(), + "extern/auto_casadi_mapping_f2.c is missing" ); + + // Check for Cost assert!( Path::new("extern/auto_casadi_cost.c").exists(), "extern/auto_casadi_cost.c is missing" ); + + // Check for Grad assert!( Path::new("extern/auto_casadi_grad.c").exists(), "extern/auto_casadi_grad.c is missing" ); + + // Check for Memory Allocator assert!( Path::new("extern/icallocator.c").exists(), "extern/icallocator.c is missing" @@ -33,7 +43,7 @@ fn main() { .include("src") .file("extern/auto_casadi_cost.c") .file("extern/auto_casadi_grad.c") - .file("extern/auto_casadi_constraints_type_penalty.c") + .file("extern/auto_casadi_mapping_f2.c") .file("extern/auto_casadi_mapping_f1.c") .file("extern/icallocator.c") .compile("icasadi"); @@ -41,7 +51,7 @@ fn main() { // 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/auto_casadi_mapping_f2.c"); println!("cargo:rerun-if-changed=extern/auto_casadi_mapping_f1.c"); println!("cargo:rerun-if-changed=extern/icallocator.c"); } diff --git a/open-codegen/opengen/templates/icallocator.c.template b/open-codegen/opengen/templates/icallocator.c.template index 0010e4df..39a04c60 100644 --- a/open-codegen/opengen/templates/icallocator.c.template +++ b/open-codegen/opengen/templates/icallocator.c.template @@ -1,4 +1,3 @@ -#include #include #ifndef casadi_real @@ -12,65 +11,135 @@ #define TRUE 1 #define FALSE 0 +/** + * CasADi function: Workspace sizes for the COST function + */ extern int {{build_config.cost_function_name or 'phi'}}_work( casadi_int *sz_arg, casadi_int *sz_res, casadi_int *sz_iw, casadi_int *sz_w); - +/** + * CasADi function: Workspace sizes for the GRADIENT of the cost + */ extern int {{build_config.grad_function_name or 'grad_phi'}}_work( casadi_int *sz_arg, casadi_int *sz_res, casadi_int *sz_iw, casadi_int *sz_w); +/** + * CasADi function: Workspace sizes for the Mapping F1 + */ +extern int {{build_config.alm_mapping_f1_function_name}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + +/** + * CasADi function: Workspace sizes for the Mapping F2 + */ +extern int {{build_config.constraint_penalty_function_name}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + +/* Whether memory has been previously allocated */ static char is_allocated = FALSE; -static casadi_int *allocated_i_workspace_cost; -static casadi_real *allocated_r_workspace_cost; -static casadi_int *allocated_i_workspace_grad; -static casadi_real *allocated_r_workspace_grad; -/* returns 0 if the allocation of memory was successful */ +/* + * ALLOCATED MEMORY + */ +static casadi_int *allocated_i_workspace_cost = NULL; /* cost (int ) */ +static casadi_real *allocated_r_workspace_cost = NULL; /* cost (real) */ +static casadi_int *allocated_i_workspace_grad = NULL; /* grad (int ) */ +static casadi_real *allocated_r_workspace_grad = NULL; /* grad (real) */ +static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ +static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ +static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ +static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ + +/** + * Allocates memory only the first time it is called + * Returns 0 if the allocation of memory was successful + */ static int allocate_if_not_yet() { + /* + * Number of input/output arguments of CasADi args + * (but these are known already) + */ + casadi_int sz_arg = 0; + casadi_int sz_res = 0; + /* Sizes for cost function */ - casadi_int sz_arg_cost = 0; - casadi_int sz_res_cost = 0; casadi_int sz_iw_cost = 0; casadi_int sz_w_cost = 0; /* Sizes for gradient */ - casadi_int sz_arg_grad = 0; - casadi_int sz_res_grad = 0; casadi_int sz_iw_grad = 0; casadi_int sz_w_grad = 0; + /* Sizes for F1 */ + casadi_int sz_iw_f1 = 0; + casadi_int sz_w_f1 = 0; + + /* Sizes for F2 */ + casadi_int sz_iw_f2 = 0; + casadi_int sz_w_f2 = 0; + /* Obtain sizes */ - {{build_config.cost_function_name or 'phi'}}_work(&sz_arg_cost, &sz_res_cost, &sz_iw_cost, &sz_w_cost); - {{build_config.grad_function_name or 'grad_phi'}}_work(&sz_arg_grad, &sz_res_grad, &sz_iw_grad, &sz_w_grad); + {{build_config.cost_function_name or 'phi'}}_work(&sz_arg, &sz_res, &sz_iw_cost, &sz_w_cost); + {{build_config.grad_function_name or 'grad_phi'}}_work(&sz_arg, &sz_res, &sz_iw_grad, &sz_w_grad); + {{build_config.alm_mapping_f1_function_name}}_work(&sz_arg, &sz_res, &sz_iw_f1, &sz_w_f1); + {{build_config.constraint_penalty_function_name}}_work(&sz_arg, &sz_res, &sz_iw_f2, &sz_w_f2); - printf("cost = (%lld, %lld, %lld, %lld)", sz_arg_cost, sz_res_cost, sz_iw_cost, sz_w_cost); /* Allocate memory, if not allocated before */ if (!is_allocated) { /* Allocate memory for cost function */ allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); - if (allocated_i_workspace_cost == NULL) goto fail_1; + if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); - if (allocated_r_workspace_cost == NULL) goto fail_2; + if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; /* Allocate memory for gradient */ allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); - if (allocated_i_workspace_grad == NULL) goto fail_3; + if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); - if (allocated_r_workspace_grad == NULL) goto fail_4; + if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; + + /* Allocate memory for F1 */ + allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); + if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; + allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); + if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; + + /* Allocate memory for F2 */ + allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); + if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; + allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); + if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; } + /* All memory has been allocated; it shouldn't be re-allocated */ + is_allocated = TRUE; + return 0; /* Free memory that has been previously allocated (failure!) */ + fail_8: + free(allocated_i_workspace_f2); + fail_7: + free(allocated_r_workspace_f1); + fail_6: + free(allocated_i_workspace_grad); + fail_5: + free(allocated_r_workspace_grad); fail_4: free(allocated_i_workspace_grad); fail_3: @@ -81,23 +150,108 @@ static int allocate_if_not_yet() { return 1; } +/** + * Initialise the memory + */ int init_{{build_config.id or '0'}}() { + /* The first time we call this method, it will allocate memory */ if (!is_allocated){ return allocate_if_not_yet(); } + /* Otherwise, it will do nothing and will return 0 (success) */ return 0; } +/** + * Destroy all allocated memory + */ int destroy_{{build_config.id or '0'}}() { + /* If no memory has been allocated, return 0 (success) */ + if (!is_allocated) return 0; + if (allocated_i_workspace_cost != NULL) free(allocated_i_workspace_cost); + if (allocated_r_workspace_cost != NULL) free(allocated_r_workspace_cost); + if (allocated_i_workspace_grad != NULL) free(allocated_i_workspace_grad); + if (allocated_r_workspace_grad != NULL) free(allocated_r_workspace_grad); + if (allocated_i_workspace_f1 != NULL) free(allocated_i_workspace_f1); + if (allocated_r_workspace_f1 != NULL) free(allocated_r_workspace_f1); + if (allocated_i_workspace_f2 != NULL) free(allocated_i_workspace_f2); + if (allocated_r_workspace_f2 != NULL) free(allocated_r_workspace_f2); + return 0; } + +/* ------COST------------------------------------------------------------------- */ + +/** + * Integer-type workspace for COST + */ casadi_int * allocated_{{build_config.cost_function_name or 'phi'}}_iwork() { if (!is_allocated) allocate_if_not_yet(); return allocated_i_workspace_cost; } +/** + * Real-type workspace for COST + */ casadi_real * allocated_{{build_config.cost_function_name or 'phi'}}_rwork() { if (!is_allocated) allocate_if_not_yet(); return allocated_r_workspace_cost; -} \ No newline at end of file +} + + +/* ------GRADIENT--------------------------------------------------------------- */ + +/** + * Integer-type workspace for GRADient + */ +casadi_int * allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_i_workspace_grad; +} + +/** + * Real-type workspace for GRADient + */ +casadi_real * allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_r_workspace_grad; +} + + +/* ------MAPPING F1------------------------------------------------------------- */ + +/** + * Integer-type workspace for F1 + */ +casadi_int * allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_i_workspace_f1; +} + +/** + * Real-type workspace for F1 + */ +casadi_real * allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_r_workspace_f1; +} + + +/* ------MAPPING F2------------------------------------------------------------- */ + +/** + * Integer-type workspace for F2 + */ +casadi_int * allocated_{{build_config.constraint_penalty_function_name}}_iwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_i_workspace_f2; +} + +/** + * Real-type workspace for F2 + */ +casadi_real * allocated_{{build_config.constraint_penalty_function_name}}_rwork() { + if (!is_allocated) allocate_if_not_yet(); + return allocated_r_workspace_f2; +} diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index c1d5ec62..5f0f578b 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -3,7 +3,8 @@ //! This is a Rust interface to CasADi C functions. //! //! This is a `no-std` library (however, mind that the CasADi-generated code -//! requires `libm` to call math functions such as `sqrt`, `sin`, etc...) +//! requires `libm` to call math functions such as `sqrt`, `sin`, etc...) and +//! icallocator.c requires libstd to allocate memory using malloc //! //! --- //! @@ -14,7 +15,6 @@ //! Generated at: {{timestamp_created}} //! -// We can't use no_std... #![no_std] /// Number of static parameters (this also includes penalty constraints) @@ -118,9 +118,17 @@ extern "C" { // ----------------------------------------------------------- /// Workspace lengths of cost function - fn allocated_{{build_config.cost_function_name or 'phi'}}_iwork() -> *mut c_longlong; - fn allocated_{{build_config.cost_function_name or 'phi'}}_rwork() -> *mut c_double; - fn init_{{build_config.id or '0'}}() -> c_int; + fn allocated_{{build_config.cost_function_name or 'phi'}}_iwork() -> *mut c_longlong; + fn allocated_{{build_config.cost_function_name or 'phi'}}_rwork() -> *mut c_double; + fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() -> *mut c_longlong; + fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() -> *mut c_double; + fn allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() -> *mut c_longlong; + fn allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() -> *mut c_double; + fn allocated_{{build_config.constraint_penalty_function_name}}_iwork() -> *mut c_longlong; + fn allocated_{{build_config.constraint_penalty_function_name}}_rwork() -> *mut c_double; + + fn init_{{build_config.id or '0'}}() -> c_int; + fn destroy_{{build_config.id or '0'}}() -> c_int; } // END of extern C @@ -135,6 +143,13 @@ pub fn init() -> i32 { } +// Destruction +pub fn destroy() -> i32 { + unsafe { + return destroy_{{build_config.id or '0'}}(); + } +} + // ----------------------------------------------------------- // *MAIN* API Functions in Rust // ----------------------------------------------------------- @@ -170,8 +185,8 @@ pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { {{build_config.cost_function_name or 'phi'}}( arguments.as_ptr(), cost.as_mut_ptr(), - allocated_{{build_config.cost_function_name or 'phi'}}_iwork() as *mut c_longlong, - allocated_{{build_config.cost_function_name or 'phi'}}_rwork() as *mut c_double, + allocated_{{build_config.cost_function_name or 'phi'}}_iwork(), + allocated_{{build_config.cost_function_name or 'phi'}}_rwork(), 0 as *mut c_void, ) as i32 } @@ -209,8 +224,8 @@ pub fn grad(u: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 {{build_config.grad_function_name or 'grad_phi'}}( arguments.as_ptr(), grad.as_mut_ptr(), - 0 as *mut c_longlong, - 0 as *mut c_double, + allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork(), + allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork(), 0 as *mut c_void, ) as i32 } @@ -248,8 +263,8 @@ pub fn mapping_f1( {{build_config.alm_mapping_f1_function_name or 'mapping_f1'}}( arguments.as_ptr(), constraints.as_mut_ptr(), - 0 as *mut c_longlong, - 0 as *mut c_double, + allocated_{{build_config.alm_mapping_f1_function_name}}_iwork(), + allocated_{{build_config.alm_mapping_f1_function_name}}_rwork(), 0 as *mut c_void, ) as i32 } @@ -285,8 +300,8 @@ pub fn mapping_f2( {{build_config.constraint_penalty_function_name or 'constraints_penalty'}}( arguments.as_ptr(), constraints.as_mut_ptr(), - 0 as *mut c_longlong, - 0 as *mut c_double, + allocated_{{build_config.constraint_penalty_function_name}}_iwork(), + allocated_{{build_config.constraint_penalty_function_name}}_rwork(), 0 as *mut c_void, ) as i32 } @@ -306,4 +321,11 @@ mod tests { let _nu = NUM_DECISION_VARIABLES; } + #[test] + fn tst_initialise() { + assert_eq!(0, init()); + assert_eq!(0, init()); + assert_eq!(0, destroy()); + } + } From a2c2f4f552dfe1dfd59e7f36daadc536b815fac0 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 01:34:32 +0100 Subject: [PATCH 19/42] icallocator.c: simplification --- .../opengen/templates/icallocator.c.template | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/open-codegen/opengen/templates/icallocator.c.template b/open-codegen/opengen/templates/icallocator.c.template index 39a04c60..f70780fd 100644 --- a/open-codegen/opengen/templates/icallocator.c.template +++ b/open-codegen/opengen/templates/icallocator.c.template @@ -66,6 +66,7 @@ static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ * Returns 0 if the allocation of memory was successful */ static int allocate_if_not_yet() { + if (is_allocated) return 0; /* * Number of input/output arguments of CasADi args @@ -97,34 +98,34 @@ static int allocate_if_not_yet() { {{build_config.constraint_penalty_function_name}}_work(&sz_arg, &sz_res, &sz_iw_f2, &sz_w_f2); - /* Allocate memory, if not allocated before */ - if (!is_allocated) { + /* + * Allocate memory (if not allocated previously) + */ - /* Allocate memory for cost function */ - allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); - if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; - allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); - if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; + /* Allocate memory for cost function */ + allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); + if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; + allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); + if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; - /* Allocate memory for gradient */ - allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); - if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; - allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); - if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; + /* Allocate memory for gradient */ + allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); + if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; + allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); + if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; - /* Allocate memory for F1 */ - allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); - if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; - allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); - if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; + /* Allocate memory for F1 */ + allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); + if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; + allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); + if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; - /* Allocate memory for F2 */ - allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); - if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; - allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); - if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; + /* Allocate memory for F2 */ + allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); + if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; + allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); + if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; - } /* All memory has been allocated; it shouldn't be re-allocated */ is_allocated = TRUE; @@ -187,7 +188,7 @@ int destroy_{{build_config.id or '0'}}() { * Integer-type workspace for COST */ casadi_int * allocated_{{build_config.cost_function_name or 'phi'}}_iwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_i_workspace_cost; } @@ -195,7 +196,7 @@ casadi_int * allocated_{{build_config.cost_function_name or 'phi'}}_iwork() { * Real-type workspace for COST */ casadi_real * allocated_{{build_config.cost_function_name or 'phi'}}_rwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_r_workspace_cost; } @@ -206,7 +207,7 @@ casadi_real * allocated_{{build_config.cost_function_name or 'phi'}}_rwork() { * Integer-type workspace for GRADient */ casadi_int * allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_i_workspace_grad; } @@ -214,7 +215,7 @@ casadi_int * allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork( * Real-type workspace for GRADient */ casadi_real * allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_r_workspace_grad; } @@ -225,7 +226,7 @@ casadi_real * allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork * Integer-type workspace for F1 */ casadi_int * allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_i_workspace_f1; } @@ -233,7 +234,7 @@ casadi_int * allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() { * Real-type workspace for F1 */ casadi_real * allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_r_workspace_f1; } @@ -244,7 +245,7 @@ casadi_real * allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() { * Integer-type workspace for F2 */ casadi_int * allocated_{{build_config.constraint_penalty_function_name}}_iwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_i_workspace_f2; } @@ -252,6 +253,6 @@ casadi_int * allocated_{{build_config.constraint_penalty_function_name}}_iwork() * Real-type workspace for F2 */ casadi_real * allocated_{{build_config.constraint_penalty_function_name}}_rwork() { - if (!is_allocated) allocate_if_not_yet(); + allocate_if_not_yet(); return allocated_r_workspace_f2; } From 790b025c1000e42181f15e717ff6447bea8f9b71 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 15:29:25 +0100 Subject: [PATCH 20/42] proper memory mgmt in C interface --- .../opengen/builder/optimizer_builder.py | 10 +- open-codegen/opengen/icasadi/build.rs | 8 +- .../opengen/templates/icallocator.c.template | 258 ------------ .../opengen/templates/icasadi_lib.rs.template | 32 +- .../opengen/templates/interface.c.template | 379 ++++++++++++++++++ 5 files changed, 396 insertions(+), 291 deletions(-) delete mode 100644 open-codegen/opengen/templates/icallocator.c.template create mode 100644 open-codegen/opengen/templates/interface.c.template diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 4d4ed3e7..32b0fc16 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -142,17 +142,17 @@ def __copy_icasadi_to_target(self): ignore=shutil.ignore_patterns( '*.lock', 'ci*', 'target', 'auto*')) - def __generate_icasadi_memory_allocator(self): - logging.info("Generating icallocator.c") + def __generate_icasadi_c_interface(self): + logging.info("Generating intercafe.c (C interface)") file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) - template = env.get_template('icallocator.c.template') + template = env.get_template('interface.c.template') output_template = template.render(meta=self.__meta, problem=self.__problem, build_config=self.__build_config, timestamp_created=datetime.datetime.now()) icallocator_path = os.path.abspath( - os.path.join(self.__icasadi_target_dir(), "extern/icallocator.c")) + os.path.join(self.__icasadi_target_dir(), "extern/interface.c")) with open(icallocator_path, "w") as fh: fh.write(output_template) @@ -439,7 +439,7 @@ def build(self): self.__prepare_target_project() # create folders; init cargo project self.__copy_icasadi_to_target() # copy icasadi/ files to target dir self.__generate_cargo_toml() # generate Cargo.toml using template - self.__generate_icasadi_memory_allocator() # generate icasadi/extern/icallocator.c + self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) diff --git a/open-codegen/opengen/icasadi/build.rs b/open-codegen/opengen/icasadi/build.rs index 9fc6a2e9..aa831a64 100644 --- a/open-codegen/opengen/icasadi/build.rs +++ b/open-codegen/opengen/icasadi/build.rs @@ -30,8 +30,8 @@ fn main() { // Check for Memory Allocator assert!( - Path::new("extern/icallocator.c").exists(), - "extern/icallocator.c is missing" + Path::new("extern/interface.c").exists(), + "extern/interface.c is missing" ); cc::Build::new() @@ -45,7 +45,7 @@ fn main() { .file("extern/auto_casadi_grad.c") .file("extern/auto_casadi_mapping_f2.c") .file("extern/auto_casadi_mapping_f1.c") - .file("extern/icallocator.c") + .file("extern/interface.c") .compile("icasadi"); // Rerun if these autogenerated files change @@ -53,5 +53,5 @@ fn main() { println!("cargo:rerun-if-changed=extern/auto_casadi_grad.c"); println!("cargo:rerun-if-changed=extern/auto_casadi_mapping_f2.c"); println!("cargo:rerun-if-changed=extern/auto_casadi_mapping_f1.c"); - println!("cargo:rerun-if-changed=extern/icallocator.c"); + println!("cargo:rerun-if-changed=extern/interface.c"); } diff --git a/open-codegen/opengen/templates/icallocator.c.template b/open-codegen/opengen/templates/icallocator.c.template deleted file mode 100644 index f70780fd..00000000 --- a/open-codegen/opengen/templates/icallocator.c.template +++ /dev/null @@ -1,258 +0,0 @@ -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int long long int -#endif - -#define TRUE 1 -#define FALSE 0 - -/** - * CasADi function: Workspace sizes for the COST function - */ -extern int {{build_config.cost_function_name or 'phi'}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); -/** - * CasADi function: Workspace sizes for the GRADIENT of the cost - */ -extern int {{build_config.grad_function_name or 'grad_phi'}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); - -/** - * CasADi function: Workspace sizes for the Mapping F1 - */ -extern int {{build_config.alm_mapping_f1_function_name}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); - -/** - * CasADi function: Workspace sizes for the Mapping F2 - */ -extern int {{build_config.constraint_penalty_function_name}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); - -/* Whether memory has been previously allocated */ -static char is_allocated = FALSE; - -/* - * ALLOCATED MEMORY - */ -static casadi_int *allocated_i_workspace_cost = NULL; /* cost (int ) */ -static casadi_real *allocated_r_workspace_cost = NULL; /* cost (real) */ -static casadi_int *allocated_i_workspace_grad = NULL; /* grad (int ) */ -static casadi_real *allocated_r_workspace_grad = NULL; /* grad (real) */ -static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ -static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ -static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ -static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ - -/** - * Allocates memory only the first time it is called - * Returns 0 if the allocation of memory was successful - */ -static int allocate_if_not_yet() { - if (is_allocated) return 0; - - /* - * Number of input/output arguments of CasADi args - * (but these are known already) - */ - casadi_int sz_arg = 0; - casadi_int sz_res = 0; - - /* Sizes for cost function */ - casadi_int sz_iw_cost = 0; - casadi_int sz_w_cost = 0; - - /* Sizes for gradient */ - casadi_int sz_iw_grad = 0; - casadi_int sz_w_grad = 0; - - /* Sizes for F1 */ - casadi_int sz_iw_f1 = 0; - casadi_int sz_w_f1 = 0; - - /* Sizes for F2 */ - casadi_int sz_iw_f2 = 0; - casadi_int sz_w_f2 = 0; - - /* Obtain sizes */ - {{build_config.cost_function_name or 'phi'}}_work(&sz_arg, &sz_res, &sz_iw_cost, &sz_w_cost); - {{build_config.grad_function_name or 'grad_phi'}}_work(&sz_arg, &sz_res, &sz_iw_grad, &sz_w_grad); - {{build_config.alm_mapping_f1_function_name}}_work(&sz_arg, &sz_res, &sz_iw_f1, &sz_w_f1); - {{build_config.constraint_penalty_function_name}}_work(&sz_arg, &sz_res, &sz_iw_f2, &sz_w_f2); - - - /* - * Allocate memory (if not allocated previously) - */ - - /* Allocate memory for cost function */ - allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); - if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; - allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); - if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; - - /* Allocate memory for gradient */ - allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); - if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; - allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); - if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; - - /* Allocate memory for F1 */ - allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); - if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; - allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); - if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; - - /* Allocate memory for F2 */ - allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); - if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; - allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); - if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; - - - /* All memory has been allocated; it shouldn't be re-allocated */ - is_allocated = TRUE; - - return 0; - - /* Free memory that has been previously allocated (failure!) */ - fail_8: - free(allocated_i_workspace_f2); - fail_7: - free(allocated_r_workspace_f1); - fail_6: - free(allocated_i_workspace_grad); - fail_5: - free(allocated_r_workspace_grad); - fail_4: - free(allocated_i_workspace_grad); - fail_3: - free(allocated_r_workspace_cost); - fail_2: - free(allocated_i_workspace_cost); - fail_1: - return 1; -} - -/** - * Initialise the memory - */ -int init_{{build_config.id or '0'}}() { - /* The first time we call this method, it will allocate memory */ - if (!is_allocated){ - return allocate_if_not_yet(); - } - /* Otherwise, it will do nothing and will return 0 (success) */ - return 0; -} - -/** - * Destroy all allocated memory - */ -int destroy_{{build_config.id or '0'}}() { - /* If no memory has been allocated, return 0 (success) */ - if (!is_allocated) return 0; - if (allocated_i_workspace_cost != NULL) free(allocated_i_workspace_cost); - if (allocated_r_workspace_cost != NULL) free(allocated_r_workspace_cost); - if (allocated_i_workspace_grad != NULL) free(allocated_i_workspace_grad); - if (allocated_r_workspace_grad != NULL) free(allocated_r_workspace_grad); - if (allocated_i_workspace_f1 != NULL) free(allocated_i_workspace_f1); - if (allocated_r_workspace_f1 != NULL) free(allocated_r_workspace_f1); - if (allocated_i_workspace_f2 != NULL) free(allocated_i_workspace_f2); - if (allocated_r_workspace_f2 != NULL) free(allocated_r_workspace_f2); - - return 0; -} - - -/* ------COST------------------------------------------------------------------- */ - -/** - * Integer-type workspace for COST - */ -casadi_int * allocated_{{build_config.cost_function_name or 'phi'}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_cost; -} - -/** - * Real-type workspace for COST - */ -casadi_real * allocated_{{build_config.cost_function_name or 'phi'}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_cost; -} - - -/* ------GRADIENT--------------------------------------------------------------- */ - -/** - * Integer-type workspace for GRADient - */ -casadi_int * allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_grad; -} - -/** - * Real-type workspace for GRADient - */ -casadi_real * allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_grad; -} - - -/* ------MAPPING F1------------------------------------------------------------- */ - -/** - * Integer-type workspace for F1 - */ -casadi_int * allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_f1; -} - -/** - * Real-type workspace for F1 - */ -casadi_real * allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_f1; -} - - -/* ------MAPPING F2------------------------------------------------------------- */ - -/** - * Integer-type workspace for F2 - */ -casadi_int * allocated_{{build_config.constraint_penalty_function_name}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_f2; -} - -/** - * Real-type workspace for F2 - */ -casadi_real * allocated_{{build_config.constraint_penalty_function_name}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_f2; -} diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 5f0f578b..24a3f30b 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -38,24 +38,6 @@ extern "C" { // Main External (C) functions // ----------------------------------------------------------- - /// Cost function, f(u, p), generated by CasADi - /// - /// - /// ## Arguments - /// - /// - `arg`: function arguemnts (u and p) - /// - `casadi_results`: - /// - `iw`: integer workspace (here: empty) - /// - `w`: workspace (here: empty) - /// - `mem`: memory (here, 0) - fn {{build_config.cost_function_name or 'phi'}}( - arg: *const *const c_double, - casadi_results: *mut *mut c_double, - iw: *mut c_longlong, - w: *mut c_double, - mem: *mut c_void, - ) -> c_int; - /// Gradient of the cost function, Df(u, p), generated by CasADi /// @@ -118,8 +100,6 @@ extern "C" { // ----------------------------------------------------------- /// Workspace lengths of cost function - fn allocated_{{build_config.cost_function_name or 'phi'}}_iwork() -> *mut c_longlong; - fn allocated_{{build_config.cost_function_name or 'phi'}}_rwork() -> *mut c_double; fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() -> *mut c_longlong; fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() -> *mut c_double; fn allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() -> *mut c_longlong; @@ -130,6 +110,13 @@ extern "C" { fn init_{{build_config.id or '0'}}() -> c_int; fn destroy_{{build_config.id or '0'}}() -> c_int; + fn cost_function( + arg: *const *const c_double, + casadi_results: *mut *mut c_double + ) -> c_int; + + + } // END of extern C @@ -182,12 +169,9 @@ pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { let cost = &mut [cost_value as *mut c_double]; unsafe { - {{build_config.cost_function_name or 'phi'}}( + cost_function( arguments.as_ptr(), cost.as_mut_ptr(), - allocated_{{build_config.cost_function_name or 'phi'}}_iwork(), - allocated_{{build_config.cost_function_name or 'phi'}}_rwork(), - 0 as *mut c_void, ) as i32 } } diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template new file mode 100644 index 00000000..dfb13102 --- /dev/null +++ b/open-codegen/opengen/templates/interface.c.template @@ -0,0 +1,379 @@ +/* + * Interface/Wrapper for C functions generated by CasADi + * + * CasADi generated the following four files: + * - auto_casadi_cost.c + * - auto_casadi_grad.c + * - auto_casadi_mapping_f1.c + * - auto_casadi_mapping_f2.c + * + * This file is autogenerated by Optimization Engine + * See http://doc.optimization-engine.xyz + * + * + * Metadata: + * + Optimizer + * + name: {{ meta.optimizer_name }} + * + version: {{ meta.version }} + * + licence: {{ meta.licence }} + * + Problem + * + vars: {{ problem.dim_decision_variables() }} + * + parameters: {{ problem.dim_parameters() }} + * + n1: {{ problem.dim_constraints_aug_lagrangian() }} + * + n2: {{ problem.dim_constraints_penalty() }} + * + * Generated at: {{timestamp_created}} + * + */ +#include +#include + +#define TRUE 1 +#define FALSE 0 + +#define NU {{ problem.dim_decision_variables() }} +#define NP {{ problem.dim_parameters() + problem.dim_constraints_penalty() }} + +/* + * TODO: This is a temporary solution; we need to get the exact value + * from Python using cs.Function.sz_arg() ! + */ +#define MAX_ARG {{ 1 + problem.dim_decision_variables() + + problem.dim_parameters() + + problem.dim_constraints_aug_lagrangian() + + problem.dim_constraints_penalty() }} + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int long long int +#endif + +/* + * CasADi interface for the cost function + * and its workspace sizes + */ +extern int {{build_config.cost_function_name or 'phi'}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); +extern int {{build_config.cost_function_name or 'phi'}}( + const casadi_real** arg, + casadi_real** res, + casadi_int* iw, + casadi_real* w, + void* mem); + +/* + * CasADi interface for the gradient of the cost + * and its workspace sizes + */ +extern int {{build_config.grad_function_name or 'grad_phi'}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); +extern int {{build_config.grad_function_name or 'grad_phi'}}( + const casadi_real** arg, + casadi_real** res, + casadi_int* iw, + casadi_real* w, + void* mem); + +/* + * CasADi interface for the gradient of mapping F1 + * and its workspace sizes + */ +extern int {{build_config.alm_mapping_f1_function_name}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + +/* + * CasADi interface for the gradient of mapping F2 + * and its workspace sizes + */ +extern int {{build_config.constraint_penalty_function_name}}_work( + casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w); + + +/* Whether memory has been previously allocated */ +static char is_allocated = FALSE; + +/* + * ALLOCATED MEMORY + */ + +/* Integer workspaces */ +static casadi_int *allocated_i_workspace_cost = NULL; /* cost (int ) */ +static casadi_int *allocated_i_workspace_grad = NULL; /* grad (int ) */ +static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ +static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ + +/* Real workspaces */ +static casadi_real *allocated_r_workspace_cost = NULL; /* cost (real) */ +static casadi_real *allocated_r_workspace_grad = NULL; /* grad (real) */ +static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ +static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ + +/* Result workspaces */ +static casadi_real **result_space_cost = NULL; /* cost (real) */ +static casadi_real **result_space_grad = NULL; /* grad (real) */ +static casadi_real **result_space_f1 = NULL; /* F1 (real) */ +static casadi_real **result_space_f2 = NULL; /* F2 (real) */ + +/** + * Allocates memory only the first time it is called + * Returns 0 if the allocation of memory was successful + */ +static int allocate_if_not_yet() { + + /* Sizes for cost function */ + casadi_int sz_arg_cost = 0; + casadi_int sz_res_cost = 0; + casadi_int sz_iw_cost = 0; + casadi_int sz_w_cost = 0; + + /* Sizes for gradient */ + casadi_int sz_arg_grad = 0; + casadi_int sz_res_grad = 0; + casadi_int sz_iw_grad = 0; + casadi_int sz_w_grad = 0; + + /* Sizes for F1 */ + casadi_int sz_arg_f1 = 0; + casadi_int sz_res_f1 = 0; + casadi_int sz_iw_f1 = 0; + casadi_int sz_w_f1 = 0; + + /* Sizes for F2 */ + casadi_int sz_arg_f2 = 0; + casadi_int sz_res_f2 = 0; + casadi_int sz_iw_f2 = 0; + casadi_int sz_w_f2 = 0; + + if (is_allocated) return 0; + + /* Obtain sizes */ + {{build_config.cost_function_name or 'phi'}}_work( + &sz_arg_cost, + &sz_res_cost, + &sz_iw_cost, + &sz_w_cost); + {{build_config.grad_function_name or 'grad_phi'}}_work( + &sz_arg_grad, + &sz_res_grad, + &sz_iw_grad, + &sz_w_grad); + {{build_config.alm_mapping_f1_function_name}}_work( + &sz_arg_f1, + &sz_res_f1, + &sz_iw_f1, + &sz_w_f1); + {{build_config.constraint_penalty_function_name}}_work( + &sz_arg_f2, + &sz_res_f2, + &sz_iw_f2, + &sz_w_f2); + + + /* + * Allocate memory (if not allocated previously) + */ + + /* Allocate memory for cost function */ + allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); /* int work */ + if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; /* int work fail */ + allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); /* work */ + if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; /* work fail */ + + /* Allocate memory for gradient */ + allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); /* int work */ + if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; /* int work fail */ + allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); /* work */ + if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; /* work fail */ + + /* Allocate memory for F1 */ + allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); /* int work */ + if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; /* int work fail */ + allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); /* work */ + if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; /* work fail */ + + /* Allocate memory for F2 */ + allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); /* int work */ + if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; /* int work fail */ + allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); /* work */ + if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; /* work fail */ + + /* Allocate memory for result spaces */ + result_space_cost = (casadi_real**)malloc((sz_res_cost)*sizeof(casadi_real*)); /* res space: cost */ + if (sz_res_cost > 0 && result_space_cost == NULL) goto fail_9; /* res space fail */ + result_space_grad = (casadi_real**)malloc((sz_res_grad)*sizeof(casadi_real*)); /* res space: grad */ + if (sz_res_grad > 0 && result_space_grad == NULL) goto fail_10; /* res space fail */ + + /* All memory has been allocated; it shouldn't be re-allocated */ + is_allocated = TRUE; + + return 0; + + /* Free memory that has been previously allocated (failure!) */ + fail_10: + free(result_space_cost); + fail_9: + free(allocated_r_workspace_f2); + fail_8: + free(allocated_i_workspace_f2); + fail_7: + free(allocated_r_workspace_f1); + fail_6: + free(allocated_i_workspace_f1); + fail_5: + free(allocated_r_workspace_grad); + fail_4: + free(allocated_i_workspace_grad); + fail_3: + free(allocated_r_workspace_cost); + fail_2: + free(allocated_i_workspace_cost); + fail_1: + return 1; +} + +/** + * Initialise the memory + */ +int init_{{build_config.id or '0'}}() { + /* The first time we call this method, it will allocate memory */ + if (!is_allocated){ + return allocate_if_not_yet(); + } + /* Otherwise, it will do nothing and will return 0 (success) */ + return 0; +} + +/** + * Destroy all allocated memory + */ +int destroy_{{build_config.id or '0'}}() { + /* If no memory has been allocated, return 0 (success) */ + if (!is_allocated) return 0; + if (allocated_i_workspace_cost != NULL) free(allocated_i_workspace_cost); + if (allocated_r_workspace_cost != NULL) free(allocated_r_workspace_cost); + if (allocated_i_workspace_grad != NULL) free(allocated_i_workspace_grad); + if (allocated_r_workspace_grad != NULL) free(allocated_r_workspace_grad); + if (allocated_i_workspace_f1 != NULL) free(allocated_i_workspace_f1); + if (allocated_r_workspace_f1 != NULL) free(allocated_r_workspace_f1); + if (allocated_i_workspace_f2 != NULL) free(allocated_i_workspace_f2); + if (allocated_r_workspace_f2 != NULL) free(allocated_r_workspace_f2); + + return 0; +} + + + +/* ------U, P------------------------------------------------------------------- */ +static casadi_real up_space[NU+NP]; + +static void copy_args_into_up_space(const casadi_real** arg) { + int i; + for (i=0; i Date: Wed, 25 Sep 2019 16:08:42 +0100 Subject: [PATCH 21/42] better approch: heap allocation exporting exact sizes from python to header file gradually removing mallocs --- .../opengen/builder/optimizer_builder.py | 15 +- .../templates/casadi_memory.h.template | 19 ++ .../opengen/templates/icasadi_lib.rs.template | 33 +--- .../opengen/templates/interface.c.template | 162 ++++++------------ 4 files changed, 96 insertions(+), 133 deletions(-) create mode 100644 open-codegen/opengen/templates/casadi_memory.h.template diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 32b0fc16..df4a2c1c 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -190,6 +190,17 @@ def __generate_cargo_toml(self): with open(cargo_toml_path, "w") as fh: fh.write(output_template) + def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): + logging.info("Generating memory") + file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) + env = jinja2.Environment(loader=file_loader) + template = env.get_template('casadi_memory.h.template') + output_template = template.render(cost=cost, grad=grad) + memory_path = os.path.abspath( + os.path.join(self.__icasadi_target_dir(), "extern/casadi_memory.h")) + with open(memory_path, "w") as fh: + fh.write(output_template) + def __generate_casadi_code(self): """Generates CasADi code""" logging.info("Defining CasADi functions and generating C code") @@ -268,6 +279,8 @@ def __generate_casadi_code(self): shutil.move(constraints_penalty_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_PNLT_CONSTRAINTS_FNAME)) + self.__generate_memory_code(cost_fun, grad_cost_fun) + def __build_icasadi(self): icasadi_dir = self.__icasadi_target_dir() command = self.__make_build_command() @@ -439,9 +452,9 @@ def build(self): self.__prepare_target_project() # create folders; init cargo project self.__copy_icasadi_to_target() # copy icasadi/ files to target dir self.__generate_cargo_toml() # generate Cargo.toml using template - self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files + self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) self.__generate_build_rs() # generate build.rs file self.__generate_yaml_data_file() diff --git a/open-codegen/opengen/templates/casadi_memory.h.template b/open-codegen/opengen/templates/casadi_memory.h.template new file mode 100644 index 00000000..7ced8c78 --- /dev/null +++ b/open-codegen/opengen/templates/casadi_memory.h.template @@ -0,0 +1,19 @@ +#pragma once + +/* + * Cost sizes + */ +#define COST_SZ_ARG {{ cost.sz_arg() }} +#define COST_SZ_IW {{ cost.sz_iw() }} +#define COST_SZ_W {{ cost.sz_w() }} +#define COST_SZ_RES {{ cost.sz_res() }} + +/* + * Gradient sizes + */ +#define GRAD_SZ_ARG {{ grad.sz_arg() }} +#define GRAD_SZ_IW {{ grad.sz_iw() }} +#define GRAD_SZ_W {{ grad.sz_w() }} +#define GRAD_SZ_RES {{ grad.sz_res() }} + + diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 24a3f30b..a07e26e0 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -39,24 +39,6 @@ extern "C" { // ----------------------------------------------------------- - /// Gradient of the cost function, Df(u, p), generated by CasADi - /// - /// - /// ## Arguments - /// - /// - `arg`: function arguemnts (u and p) - /// - `casadi_results`: - /// - `iw`: integer workspace (here: empty) - /// - `w`: workspace (here: empty) - /// - `mem`: memory (here, 0) - fn {{build_config.grad_function_name or 'grad_phi'}}( - arg: *const *const c_double, - casadi_results: *mut *mut c_double, - iw: *mut c_longlong, - w: *mut c_double, - mem: *mut c_void, - ) -> c_int; - /// Penalty-related mapping, F1(u, p), generated by CasADi /// /// @@ -99,9 +81,6 @@ extern "C" { // Workspace Length External (C) functions // ----------------------------------------------------------- - /// Workspace lengths of cost function - fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() -> *mut c_longlong; - fn allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() -> *mut c_double; fn allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() -> *mut c_longlong; fn allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() -> *mut c_double; fn allocated_{{build_config.constraint_penalty_function_name}}_iwork() -> *mut c_longlong; @@ -115,6 +94,11 @@ extern "C" { casadi_results: *mut *mut c_double ) -> c_int; + fn grad_cost_function( + arg: *const *const c_double, + casadi_results: *mut *mut c_double + ) -> c_int; + } // END of extern C @@ -205,12 +189,9 @@ pub fn grad(u: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 let grad = &mut [cost_jacobian.as_mut_ptr()]; unsafe { - {{build_config.grad_function_name or 'grad_phi'}}( + grad_cost_function( arguments.as_ptr(), - grad.as_mut_ptr(), - allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork(), - allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork(), - 0 as *mut c_void, + grad.as_mut_ptr() ) as i32 } } diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index dfb13102..6f39f35d 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -26,7 +26,7 @@ * */ #include -#include +#include "casadi_memory.h" #define TRUE 1 #define FALSE 0 @@ -34,15 +34,6 @@ #define NU {{ problem.dim_decision_variables() }} #define NP {{ problem.dim_parameters() + problem.dim_constraints_penalty() }} -/* - * TODO: This is a temporary solution; we need to get the exact value - * from Python using cs.Function.sz_arg() ! - */ -#define MAX_ARG {{ 1 + problem.dim_decision_variables() - + problem.dim_parameters() - + problem.dim_constraints_aug_lagrangian() - + problem.dim_constraints_penalty() }} - #ifndef casadi_real #define casadi_real double #endif @@ -53,13 +44,7 @@ /* * CasADi interface for the cost function - * and its workspace sizes */ -extern int {{build_config.cost_function_name or 'phi'}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); extern int {{build_config.cost_function_name or 'phi'}}( const casadi_real** arg, casadi_real** res, @@ -69,13 +54,7 @@ extern int {{build_config.cost_function_name or 'phi'}}( /* * CasADi interface for the gradient of the cost - * and its workspace sizes */ -extern int {{build_config.grad_function_name or 'grad_phi'}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); extern int {{build_config.grad_function_name or 'grad_phi'}}( const casadi_real** arg, casadi_real** res, @@ -107,27 +86,61 @@ extern int {{build_config.constraint_penalty_function_name}}_work( /* Whether memory has been previously allocated */ static char is_allocated = FALSE; -/* - * ALLOCATED MEMORY + +/* + * Integer workspaces + */ +#if COST_SZ_IW > 0 +static casadi_int allocated_i_workspace_cost[COST_SZ_IW]; /* cost (int ) */ +#else +static casadi_int *allocated_i_workspace_cost = NULL; +#endif + +#if GRAD_SZ_IW > 0 +static casadi_int allocated_i_workspace_grad[GRAD_SZ_IW]; /* grad (int ) */ +#else +static casadi_int *allocated_i_workspace_grad = NULL; +#endif + +static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ +static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ + +/* + * Real workspaces */ +#if COST_SZ_W > 0 +static casadi_real allocated_r_workspace_cost[COST_SZ_W]; /* cost (real) */ +#else +static casadi_real *allocated_r_workspace_cost = NULL; +#endif + -/* Integer workspaces */ -static casadi_int *allocated_i_workspace_cost = NULL; /* cost (int ) */ -static casadi_int *allocated_i_workspace_grad = NULL; /* grad (int ) */ -static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ -static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ +#if GRAD_SZ_W > 0 +static casadi_int allocated_r_workspace_grad[GRAD_SZ_W]; /* grad (real ) */ +#else +static casadi_int *allocated_r_workspace_grad = NULL; +#endif + +static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ +static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ + +/* + * Result workspaces + */ +#if COST_SZ_RES > 0 +static casadi_real *result_space_cost[COST_SZ_RES ]; /* cost (real) */ +#else +static casadi_real **result_space_cost = NULL; +#endif -/* Real workspaces */ -static casadi_real *allocated_r_workspace_cost = NULL; /* cost (real) */ -static casadi_real *allocated_r_workspace_grad = NULL; /* grad (real) */ -static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ -static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ +#if GRAD_SZ_RES > 0 +static casadi_real *result_space_grad[GRAD_SZ_RES]; /* grad (real) */ +#else +static casadi_real **result_space_grad = NULL; +#endif -/* Result workspaces */ -static casadi_real **result_space_cost = NULL; /* cost (real) */ -static casadi_real **result_space_grad = NULL; /* grad (real) */ -static casadi_real **result_space_f1 = NULL; /* F1 (real) */ -static casadi_real **result_space_f2 = NULL; /* F2 (real) */ +static casadi_real **result_space_f1 = NULL; /* F1 (real) */ +static casadi_real **result_space_f2 = NULL; /* F2 (real) */ /** * Allocates memory only the first time it is called @@ -135,18 +148,6 @@ static casadi_real **result_space_f2 = NULL; /* F2 (real) */ */ static int allocate_if_not_yet() { - /* Sizes for cost function */ - casadi_int sz_arg_cost = 0; - casadi_int sz_res_cost = 0; - casadi_int sz_iw_cost = 0; - casadi_int sz_w_cost = 0; - - /* Sizes for gradient */ - casadi_int sz_arg_grad = 0; - casadi_int sz_res_grad = 0; - casadi_int sz_iw_grad = 0; - casadi_int sz_w_grad = 0; - /* Sizes for F1 */ casadi_int sz_arg_f1 = 0; casadi_int sz_res_f1 = 0; @@ -161,17 +162,6 @@ static int allocate_if_not_yet() { if (is_allocated) return 0; - /* Obtain sizes */ - {{build_config.cost_function_name or 'phi'}}_work( - &sz_arg_cost, - &sz_res_cost, - &sz_iw_cost, - &sz_w_cost); - {{build_config.grad_function_name or 'grad_phi'}}_work( - &sz_arg_grad, - &sz_res_grad, - &sz_iw_grad, - &sz_w_grad); {{build_config.alm_mapping_f1_function_name}}_work( &sz_arg_f1, &sz_res_f1, @@ -188,17 +178,6 @@ static int allocate_if_not_yet() { * Allocate memory (if not allocated previously) */ - /* Allocate memory for cost function */ - allocated_i_workspace_cost = (casadi_int*)malloc(sz_iw_cost*sizeof(casadi_int)); /* int work */ - if (sz_iw_cost > 0 && allocated_i_workspace_cost == NULL) goto fail_1; /* int work fail */ - allocated_r_workspace_cost = (casadi_real*)malloc(sz_w_cost*sizeof(casadi_real)); /* work */ - if (sz_w_cost > 0 && allocated_r_workspace_cost == NULL) goto fail_2; /* work fail */ - - /* Allocate memory for gradient */ - allocated_i_workspace_grad = (casadi_int*)malloc(sz_iw_grad*sizeof(casadi_int)); /* int work */ - if (sz_iw_grad > 0 && allocated_i_workspace_grad == NULL) goto fail_3; /* int work fail */ - allocated_r_workspace_grad = (casadi_real*)malloc(sz_w_grad*sizeof(casadi_real)); /* work */ - if (sz_w_grad > 0 && allocated_r_workspace_grad == NULL) goto fail_4; /* work fail */ /* Allocate memory for F1 */ allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); /* int work */ @@ -212,22 +191,12 @@ static int allocate_if_not_yet() { allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); /* work */ if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; /* work fail */ - /* Allocate memory for result spaces */ - result_space_cost = (casadi_real**)malloc((sz_res_cost)*sizeof(casadi_real*)); /* res space: cost */ - if (sz_res_cost > 0 && result_space_cost == NULL) goto fail_9; /* res space fail */ - result_space_grad = (casadi_real**)malloc((sz_res_grad)*sizeof(casadi_real*)); /* res space: grad */ - if (sz_res_grad > 0 && result_space_grad == NULL) goto fail_10; /* res space fail */ - /* All memory has been allocated; it shouldn't be re-allocated */ is_allocated = TRUE; return 0; /* Free memory that has been previously allocated (failure!) */ - fail_10: - free(result_space_cost); - fail_9: - free(allocated_r_workspace_f2); fail_8: free(allocated_i_workspace_f2); fail_7: @@ -235,14 +204,6 @@ static int allocate_if_not_yet() { fail_6: free(allocated_i_workspace_f1); fail_5: - free(allocated_r_workspace_grad); - fail_4: - free(allocated_i_workspace_grad); - fail_3: - free(allocated_r_workspace_cost); - fail_2: - free(allocated_i_workspace_cost); - fail_1: return 1; } @@ -262,17 +223,6 @@ int init_{{build_config.id or '0'}}() { * Destroy all allocated memory */ int destroy_{{build_config.id or '0'}}() { - /* If no memory has been allocated, return 0 (success) */ - if (!is_allocated) return 0; - if (allocated_i_workspace_cost != NULL) free(allocated_i_workspace_cost); - if (allocated_r_workspace_cost != NULL) free(allocated_r_workspace_cost); - if (allocated_i_workspace_grad != NULL) free(allocated_i_workspace_grad); - if (allocated_r_workspace_grad != NULL) free(allocated_r_workspace_grad); - if (allocated_i_workspace_f1 != NULL) free(allocated_i_workspace_f1); - if (allocated_r_workspace_f1 != NULL) free(allocated_r_workspace_f1); - if (allocated_i_workspace_f2 != NULL) free(allocated_i_workspace_f2); - if (allocated_r_workspace_f2 != NULL) free(allocated_r_workspace_f2); - return 0; } @@ -293,11 +243,10 @@ static void copy_args_into_up_space(const casadi_real** arg) { /* Cost function main interface */ int cost_function(const casadi_real** arg, casadi_real** res) { - const casadi_real* args__[MAX_ARG] = {up_space, up_space + NU}; + const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; copy_args_into_up_space(arg); allocate_if_not_yet(); - const casadi_real* arguments_[100] = {arg[0], arg[1]}; result_space_cost[0] = res[0]; return {{build_config.cost_function_name or 'phi'}}( args__, @@ -312,11 +261,12 @@ int cost_function(const casadi_real** arg, casadi_real** res) { /* Cost function main interface */ int grad_cost_function(const casadi_real** arg, casadi_real** res) { + const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; + copy_args_into_up_space(arg); allocate_if_not_yet(); - const casadi_real* arguments_[100] = {arg[0], arg[1]}; result_space_cost[0] = res[0]; return {{build_config.grad_function_name or 'grad_phi'}}( - arguments_, + args__, result_space_cost, allocated_i_workspace_cost, allocated_r_workspace_cost, From bbb9b5daed4fb1fd193eec46d2e1b4348b666eff Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 16:41:46 +0100 Subject: [PATCH 22/42] close #98 no dynamic memory allocation in C --- .../opengen/builder/optimizer_builder.py | 4 +- .../templates/casadi_memory.h.template | 14 ++ .../opengen/templates/icasadi_lib.rs.template | 113 +-------- .../opengen/templates/interface.c.template | 230 ++++++------------ .../templates/optimizer_cargo.toml.template | 2 - .../templates/tcp_server_cargo.toml.template | 2 - 6 files changed, 100 insertions(+), 265 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index df4a2c1c..d24f6927 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -195,7 +195,7 @@ def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) template = env.get_template('casadi_memory.h.template') - output_template = template.render(cost=cost, grad=grad) + output_template = template.render(cost=cost, grad=grad, f1=f1, f2=f2) memory_path = os.path.abspath( os.path.join(self.__icasadi_target_dir(), "extern/casadi_memory.h")) with open(memory_path, "w") as fh: @@ -279,7 +279,7 @@ def __generate_casadi_code(self): shutil.move(constraints_penalty_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_PNLT_CONSTRAINTS_FNAME)) - self.__generate_memory_code(cost_fun, grad_cost_fun) + self.__generate_memory_code(cost_fun, grad_cost_fun, alm_mapping_f1_fun, constraint_penalty_fun) def __build_icasadi(self): icasadi_dir = self.__icasadi_target_dir() diff --git a/open-codegen/opengen/templates/casadi_memory.h.template b/open-codegen/opengen/templates/casadi_memory.h.template index 7ced8c78..bb412c1c 100644 --- a/open-codegen/opengen/templates/casadi_memory.h.template +++ b/open-codegen/opengen/templates/casadi_memory.h.template @@ -16,4 +16,18 @@ #define GRAD_SZ_W {{ grad.sz_w() }} #define GRAD_SZ_RES {{ grad.sz_res() }} +/* + * F1 sizes + */ +#define F1_SZ_ARG {{ f1.sz_arg() }} +#define F1_SZ_IW {{ f1.sz_iw() }} +#define F1_SZ_W {{ f1.sz_w() }} +#define F1_SZ_RES {{ f1.sz_res() }} +/* + * F2 sizes + */ +#define F2_SZ_ARG {{ f2.sz_arg() }} +#define F2_SZ_IW {{ f2.sz_iw() }} +#define F2_SZ_W {{ f2.sz_w() }} +#define F2_SZ_RES {{ f2.sz_res() }} \ No newline at end of file diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index a07e26e0..12b6ebc9 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -18,109 +18,28 @@ #![no_std] /// Number of static parameters (this also includes penalty constraints) -pub const NUM_STATIC_PARAMETERS: usize = {{problem.dim_parameters() + problem.dim_constraints_penalty() or 0}}; +pub const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() + problem.dim_constraints_penalty() or 0 }}; /// Number of decision variables -pub const NUM_DECISION_VARIABLES: usize = {{problem.dim_decision_variables()}}; +pub const NUM_DECISION_VARIABLES: usize = {{ problem.dim_decision_variables() }}; /// Number of ALM-type constraints (dimension of F1, i.e., n1) -pub const NUM_CONSTRAINTS_TYPE_ALM: usize = {{problem.dim_constraints_aug_lagrangian() or 0}}; +pub const NUM_CONSTRAINTS_TYPE_ALM: usize = {{ problem.dim_constraints_aug_lagrangian() or 0 }}; /// Number of penalty constraints (dimension of F2, i.e., n2) -pub const NUM_CONSTAINTS_TYPE_PENALTY: usize = {{problem.dim_constraints_penalty() or 0}}; +pub const NUM_CONSTAINTS_TYPE_PENALTY: usize = {{ problem.dim_constraints_penalty() or 0 }}; -use libc::{c_double, c_int, c_longlong, c_void}; +use libc::{c_double, c_int}; // might need to include: c_longlong, c_void /// C interface (Function API exactly as provided by CasADi) extern "C" { - - // ----------------------------------------------------------- - // Main External (C) functions - // ----------------------------------------------------------- - - - /// Penalty-related mapping, F1(u, p), generated by CasADi - /// - /// - /// ## Arguments - /// - /// - `arg`: function arguemnts (u and p) - /// - `casadi_results`: - /// - `iw`: integer workspace (here: empty) - /// - `w`: workspace (here: empty) - /// - `mem`: memory (here, 0) - fn {{build_config.alm_mapping_f1_function_name or 'mapping_f1'}}( - arg: *const *const c_double, - casadi_results: *mut *mut c_double, - iw: *mut c_longlong, - w: *mut c_double, - mem: *mut c_void, - ) -> c_int; - - /// Penalty-related mapping, F2(u, p), generated by CasADi - /// - /// - /// ## Arguments - /// - /// - `arg`: function arguemnts (u and p) - /// - `casadi_results`: - /// - `iw`: integer workspace (here: empty) - /// - `w`: workspace (here: empty) - /// - `mem`: memory (here, 0) - fn {{build_config.constraint_penalty_function_name or 'mapping_f2'}}( - arg: *const *const c_double, - casadi_results: *mut *mut c_double, - iw: *mut c_longlong, - w: *mut c_double, - mem: *mut c_void, - ) -> c_int; - - - - // ----------------------------------------------------------- - // Workspace Length External (C) functions - // ----------------------------------------------------------- - - fn allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() -> *mut c_longlong; - fn allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() -> *mut c_double; - fn allocated_{{build_config.constraint_penalty_function_name}}_iwork() -> *mut c_longlong; - fn allocated_{{build_config.constraint_penalty_function_name}}_rwork() -> *mut c_double; - - fn init_{{build_config.id or '0'}}() -> c_int; - fn destroy_{{build_config.id or '0'}}() -> c_int; - - fn cost_function( - arg: *const *const c_double, - casadi_results: *mut *mut c_double - ) -> c_int; - - fn grad_cost_function( - arg: *const *const c_double, - casadi_results: *mut *mut c_double - ) -> c_int; - - - + fn cost_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; + fn grad_cost_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; + fn mapping_f1_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; + fn mapping_f2_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; } // END of extern C - - -// Initialisation -pub fn init() -> i32 { - unsafe { - return init_{{build_config.id or '0'}}(); - } -} - - -// Destruction -pub fn destroy() -> i32 { - unsafe { - return destroy_{{build_config.id or '0'}}(); - } -} - // ----------------------------------------------------------- // *MAIN* API Functions in Rust // ----------------------------------------------------------- @@ -225,12 +144,9 @@ pub fn mapping_f1( let constraints = &mut [f1.as_mut_ptr()]; unsafe { - {{build_config.alm_mapping_f1_function_name or 'mapping_f1'}}( + mapping_f1_function( arguments.as_ptr(), - constraints.as_mut_ptr(), - allocated_{{build_config.alm_mapping_f1_function_name}}_iwork(), - allocated_{{build_config.alm_mapping_f1_function_name}}_rwork(), - 0 as *mut c_void, + constraints.as_mut_ptr() ) as i32 } } @@ -262,12 +178,9 @@ pub fn mapping_f2( let constraints = &mut [f2.as_mut_ptr()]; unsafe { - {{build_config.constraint_penalty_function_name or 'constraints_penalty'}}( + mapping_f2_function( arguments.as_ptr(), - constraints.as_mut_ptr(), - allocated_{{build_config.constraint_penalty_function_name}}_iwork(), - allocated_{{build_config.constraint_penalty_function_name}}_rwork(), - 0 as *mut c_void, + constraints.as_mut_ptr() ) as i32 } } diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index 6f39f35d..96dd86f6 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -64,27 +64,23 @@ extern int {{build_config.grad_function_name or 'grad_phi'}}( /* * CasADi interface for the gradient of mapping F1 - * and its workspace sizes */ -extern int {{build_config.alm_mapping_f1_function_name}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); +extern int {{build_config.alm_mapping_f1_function_name}}( + const casadi_real** arg, + casadi_real** res, + casadi_int* iw, + casadi_real* w, + void* mem); /* * CasADi interface for the gradient of mapping F2 - * and its workspace sizes */ -extern int {{build_config.constraint_penalty_function_name}}_work( - casadi_int *sz_arg, - casadi_int *sz_res, - casadi_int *sz_iw, - casadi_int *sz_w); - - -/* Whether memory has been previously allocated */ -static char is_allocated = FALSE; +extern int {{build_config.constraint_penalty_function_name}}( + const casadi_real** arg, + casadi_real** res, + casadi_int* iw, + casadi_real* w, + void* mem); /* @@ -102,8 +98,18 @@ static casadi_int allocated_i_workspace_grad[GRAD_SZ_IW]; /* grad (int ) */ static casadi_int *allocated_i_workspace_grad = NULL; #endif -static casadi_int *allocated_i_workspace_f1 = NULL; /* F1 (int ) */ -static casadi_int *allocated_i_workspace_f2 = NULL; /* F2 (int ) */ +#if F1_SZ_IW > 0 +static casadi_int allocated_i_workspace_f1[F1_SZ_IW]; +#else +static casadi_int *allocated_i_workspace_f1 = NULL; +#endif + +#if F2_SZ_IW > 0 +static casadi_int allocated_i_workspace_f2[F2_SZ_IW]; +#else +static casadi_int *allocated_i_workspace_f2 = NULL; +#endif + /* * Real workspaces @@ -116,13 +122,22 @@ static casadi_real *allocated_r_workspace_cost = NULL; #if GRAD_SZ_W > 0 -static casadi_int allocated_r_workspace_grad[GRAD_SZ_W]; /* grad (real ) */ +static casadi_real allocated_r_workspace_grad[GRAD_SZ_W]; /* grad (real ) */ +#else +static casadi_real *allocated_r_workspace_grad = NULL; +#endif + +#if F1_SZ_W > 0 +static casadi_real allocated_r_workspace_f1[F1_SZ_W]; #else -static casadi_int *allocated_r_workspace_grad = NULL; +static casadi_real *allocated_r_workspace_f1 = NULL; #endif -static casadi_real *allocated_r_workspace_f1 = NULL; /* F1 (real) */ -static casadi_real *allocated_r_workspace_f2 = NULL; /* F2 (real) */ +#if F2_SZ_W > 0 +static casadi_real allocated_r_workspace_f2[F2_SZ_W]; +#else +static casadi_real *allocated_r_workspace_f2 = NULL; +#endif /* * Result workspaces @@ -139,92 +154,19 @@ static casadi_real *result_space_grad[GRAD_SZ_RES]; /* grad (real) */ static casadi_real **result_space_grad = NULL; #endif -static casadi_real **result_space_f1 = NULL; /* F1 (real) */ -static casadi_real **result_space_f2 = NULL; /* F2 (real) */ -/** - * Allocates memory only the first time it is called - * Returns 0 if the allocation of memory was successful - */ -static int allocate_if_not_yet() { - - /* Sizes for F1 */ - casadi_int sz_arg_f1 = 0; - casadi_int sz_res_f1 = 0; - casadi_int sz_iw_f1 = 0; - casadi_int sz_w_f1 = 0; - - /* Sizes for F2 */ - casadi_int sz_arg_f2 = 0; - casadi_int sz_res_f2 = 0; - casadi_int sz_iw_f2 = 0; - casadi_int sz_w_f2 = 0; - - if (is_allocated) return 0; - - {{build_config.alm_mapping_f1_function_name}}_work( - &sz_arg_f1, - &sz_res_f1, - &sz_iw_f1, - &sz_w_f1); - {{build_config.constraint_penalty_function_name}}_work( - &sz_arg_f2, - &sz_res_f2, - &sz_iw_f2, - &sz_w_f2); - - - /* - * Allocate memory (if not allocated previously) - */ - - - /* Allocate memory for F1 */ - allocated_i_workspace_f1 = (casadi_int*)malloc(sz_iw_f1*sizeof(casadi_int)); /* int work */ - if (sz_iw_f1 > 0 && allocated_i_workspace_f1 == NULL) goto fail_5; /* int work fail */ - allocated_r_workspace_f1 = (casadi_real*)malloc(sz_w_f1*sizeof(casadi_real)); /* work */ - if (sz_w_f1 > 0 && allocated_r_workspace_f1 == NULL) goto fail_6; /* work fail */ - - /* Allocate memory for F2 */ - allocated_i_workspace_f2 = (casadi_int*)malloc(sz_iw_f2*sizeof(casadi_int)); /* int work */ - if (sz_iw_f2 > 0 && allocated_i_workspace_f2 == NULL) goto fail_7; /* int work fail */ - allocated_r_workspace_f2 = (casadi_real*)malloc(sz_w_f2*sizeof(casadi_real)); /* work */ - if (sz_w_f2 > 0 && allocated_r_workspace_f2 == NULL) goto fail_8; /* work fail */ - - /* All memory has been allocated; it shouldn't be re-allocated */ - is_allocated = TRUE; - - return 0; - - /* Free memory that has been previously allocated (failure!) */ - fail_8: - free(allocated_i_workspace_f2); - fail_7: - free(allocated_r_workspace_f1); - fail_6: - free(allocated_i_workspace_f1); - fail_5: - return 1; -} +#if F1_SZ_RES > 0 +static casadi_real *result_space_f1[F1_SZ_RES]; +#else +static casadi_real **result_space_f1 = NULL; +#endif -/** - * Initialise the memory - */ -int init_{{build_config.id or '0'}}() { - /* The first time we call this method, it will allocate memory */ - if (!is_allocated){ - return allocate_if_not_yet(); - } - /* Otherwise, it will do nothing and will return 0 (success) */ - return 0; -} -/** - * Destroy all allocated memory - */ -int destroy_{{build_config.id or '0'}}() { - return 0; -} +#if F2_SZ_RES > 0 +static casadi_real *result_space_f2[F2_SZ_RES]; +#else +static casadi_real **result_space_f2 = NULL; +#endif @@ -239,14 +181,10 @@ static void copy_args_into_up_space(const casadi_real** arg) { /* ------COST------------------------------------------------------------------- */ - - -/* Cost function main interface */ int cost_function(const casadi_real** arg, casadi_real** res) { const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; copy_args_into_up_space(arg); - allocate_if_not_yet(); result_space_cost[0] = res[0]; return {{build_config.cost_function_name or 'phi'}}( args__, @@ -259,71 +197,45 @@ int cost_function(const casadi_real** arg, casadi_real** res) { /* ------GRADIENT--------------------------------------------------------------- */ -/* Cost function main interface */ int grad_cost_function(const casadi_real** arg, casadi_real** res) { const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; copy_args_into_up_space(arg); - allocate_if_not_yet(); - result_space_cost[0] = res[0]; + result_space_grad[0] = res[0]; return {{build_config.grad_function_name or 'grad_phi'}}( args__, - result_space_cost, - allocated_i_workspace_cost, - allocated_r_workspace_cost, + result_space_grad, + allocated_i_workspace_grad, + allocated_r_workspace_grad, (void*) 0); } -/** - * Integer-type workspace for GRADient - */ -casadi_int * allocated_{{build_config.grad_function_name or 'grad_phi'}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_grad; -} - -/** - * Real-type workspace for GRADient - */ -casadi_real * allocated_{{build_config.grad_function_name or 'grad_phi'}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_grad; -} - - /* ------MAPPING F1------------------------------------------------------------- */ -/** - * Integer-type workspace for F1 - */ -casadi_int * allocated_{{build_config.alm_mapping_f1_function_name}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_f1; -} - -/** - * Real-type workspace for F1 - */ -casadi_real * allocated_{{build_config.alm_mapping_f1_function_name}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_f1; +int mapping_f1_function(const casadi_real** arg, casadi_real** res) { + const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; + copy_args_into_up_space(arg); + result_space_f1[0] = res[0]; + return {{build_config.alm_mapping_f1_function_name}}( + args__, + result_space_f1, + allocated_i_workspace_f1, + allocated_r_workspace_f1, + (void*) 0); } /* ------MAPPING F2------------------------------------------------------------- */ -/** - * Integer-type workspace for F2 - */ -casadi_int * allocated_{{build_config.constraint_penalty_function_name}}_iwork() { - allocate_if_not_yet(); - return allocated_i_workspace_f2; +int mapping_f2_function(const casadi_real** arg, casadi_real** res) { + const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; + copy_args_into_up_space(arg); + result_space_f2[0] = res[0]; + return {{build_config.constraint_penalty_function_name}}( + args__, + result_space_f2, + allocated_i_workspace_f2, + allocated_r_workspace_f2, + (void*) 0); } -/** - * Real-type workspace for F2 - */ -casadi_real * allocated_{{build_config.constraint_penalty_function_name}}_rwork() { - allocate_if_not_yet(); - return allocated_r_workspace_f2; -} diff --git a/open-codegen/opengen/templates/optimizer_cargo.toml.template b/open-codegen/opengen/templates/optimizer_cargo.toml.template index 18a87ceb..2d65bdb9 100644 --- a/open-codegen/opengen/templates/optimizer_cargo.toml.template +++ b/open-codegen/opengen/templates/optimizer_cargo.toml.template @@ -18,8 +18,6 @@ publish=false [dependencies] -#TODO: In production, change the following line -#optimization_engine = { git = "https://github.com/alphaville/optimization-engine" } optimization_engine = "{{open_version or '0.6.1-alpha.1'}}" icasadi = {path = "./icasadi/"} {% if activate_clib_generation -%} diff --git a/open-codegen/opengen/templates/tcp_server_cargo.toml.template b/open-codegen/opengen/templates/tcp_server_cargo.toml.template index faa72276..49adb41e 100644 --- a/open-codegen/opengen/templates/tcp_server_cargo.toml.template +++ b/open-codegen/opengen/templates/tcp_server_cargo.toml.template @@ -22,8 +22,6 @@ publish=false [dependencies] optimization_engine = "0.6.1-alpha.1" -#DO NOT USE THE FOLLOWING LINE IN PRODUCTION: -#optimization_engine = { path = "../../../../../" } serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" pretty_env_logger = "0.3.0" From 7bbce4c1d4e69cc5f5e71b57cd689b562264981e Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 18:21:31 +0100 Subject: [PATCH 23/42] minor issues with templates --- .../opengen/builder/optimizer_builder.py | 17 ++++--- open-codegen/opengen/config/build_config.py | 1 + .../opengen/icasadi/extern/README.txt | 9 ++++ .../templates/casadi_memory.h.template | 19 ++++++++ .../opengen/templates/icasadi_lib.rs.template | 46 +++++++++++++++---- .../opengen/templates/interface.c.template | 30 ++++++------ 6 files changed, 91 insertions(+), 31 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index d24f6927..3b22ee47 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -195,7 +195,11 @@ def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) template = env.get_template('casadi_memory.h.template') - output_template = template.render(cost=cost, grad=grad, f1=f1, f2=f2) + output_template = template.render(cost=cost, grad=grad, + f1=f1, f2=f2, + build_config=self.__build_config, + meta=self.__meta, + timestamp_created=datetime.datetime.now()) memory_path = os.path.abspath( os.path.join(self.__icasadi_target_dir(), "extern/casadi_memory.h")) with open(memory_path, "w") as fh: @@ -462,8 +466,9 @@ def build(self): if not self.__generate_not_build: logging.info("Building optimizer") self.__build_optimizer() # build overall project - if self.__build_config.tcp_interface_config is not None: - logging.info("Generating TCP/IP server") - self.__generate_code_tcp_interface() - if not self.__generate_not_build: - self.__build_tcp_iface() + + if self.__build_config.tcp_interface_config is not None: + logging.info("Generating TCP/IP server") + self.__generate_code_tcp_interface() + if not self.__generate_not_build: + self.__build_tcp_iface() diff --git a/open-codegen/opengen/config/build_config.py b/open-codegen/opengen/config/build_config.py index 7e06fc5e..c4416d08 100644 --- a/open-codegen/opengen/config/build_config.py +++ b/open-codegen/opengen/config/build_config.py @@ -2,6 +2,7 @@ import random import string + class BuildConfiguration: """Build configuration diff --git a/open-codegen/opengen/icasadi/extern/README.txt b/open-codegen/opengen/icasadi/extern/README.txt index b6886b9a..34c11147 100644 --- a/open-codegen/opengen/icasadi/extern/README.txt +++ b/open-codegen/opengen/icasadi/extern/README.txt @@ -1 +1,10 @@ C files auto-generated using CasADi are stored here + +In particular, in this folder you will find: + +- `auto_casadi_cost.c` +- `auto_casadi_grad.c` +- `auto_casadi_mapping_f1.c` +- `auto_casadi_mapping_f2.c` +- `casadi_memory.h` +- `interface.c` \ No newline at end of file diff --git a/open-codegen/opengen/templates/casadi_memory.h.template b/open-codegen/opengen/templates/casadi_memory.h.template index bb412c1c..6693c156 100644 --- a/open-codegen/opengen/templates/casadi_memory.h.template +++ b/open-codegen/opengen/templates/casadi_memory.h.template @@ -1,3 +1,22 @@ +/* + * Header file with sizes of arrays that need to be allocated + * (statically) once + * + * + * This file is autogenerated by Optimization Engine + * See http://doc.optimization-engine.xyz + * + * + * Metadata: + * + Optimizer + * + name: {{ meta.optimizer_name }} + * + version: {{ meta.version }} + * + licence: {{ meta.licence }} + * + * Generated at: {{timestamp_created}} + * + */ + #pragma once /* diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 12b6ebc9..0926727e 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -3,8 +3,7 @@ //! This is a Rust interface to CasADi C functions. //! //! This is a `no-std` library (however, mind that the CasADi-generated code -//! requires `libm` to call math functions such as `sqrt`, `sin`, etc...) and -//! icallocator.c requires libstd to allocate memory using malloc +//! requires `libm` to call math functions such as `sqrt`, `sin`, etc...) //! //! --- //! @@ -135,10 +134,10 @@ pub fn mapping_f1( static_params: &[f64], f1: &mut [f64], ) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); - assert!(f1.len() == NUM_CONSTAINTS_TYPE_PENALTY || - NUM_CONSTAINTS_TYPE_PENALTY == 0); + assert_eq!(u.len(), NUM_DECISION_VARIABLES, "Incompatible dimension of `u`"); + assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "Incompatible dimension of `p`"); + assert!(f1.len() == NUM_CONSTRAINTS_TYPE_ALM || + NUM_CONSTRAINTS_TYPE_ALM == 0, "Incompatible dimension of `f1` (result)"); let arguments = &[u.as_ptr(), static_params.as_ptr()]; let constraints = &mut [f1.as_mut_ptr()]; @@ -200,10 +199,37 @@ mod tests { } #[test] - fn tst_initialise() { - assert_eq!(0, init()); - assert_eq!(0, init()); - assert_eq!(0, destroy()); + fn tst_call_cost() { + let u = [0.1; NUM_DECISION_VARIABLES]; + let p = [0.1; NUM_STATIC_PARAMETERS]; + let mut cost = 0.0; + assert_eq!(0, super::cost(&u, &p, &mut cost)); + } + + #[test] + fn tst_call_grad() { + let u = [0.1; NUM_DECISION_VARIABLES]; + let p = [0.1; NUM_STATIC_PARAMETERS]; + let mut grad = [0.0; NUM_DECISION_VARIABLES]; + assert_eq!(0, super::grad(&u, &p, &mut grad)); + } + + #[test] + fn tst_f1() { + let u = [0.1; NUM_DECISION_VARIABLES]; + let p = [0.1; NUM_STATIC_PARAMETERS]; + let mut f1up = [0.0; NUM_CONSTRAINTS_TYPE_ALM]; + assert_eq!(0, super::mapping_f1(&u, &p, &mut f1up)); + println!("F1(u, p) = {:#?}", f1up); + } + + #[test] + fn tst_f2() { + let u = [0.1; NUM_DECISION_VARIABLES]; + let p = [0.1; NUM_STATIC_PARAMETERS]; + let mut f2up = [0.0; NUM_CONSTAINTS_TYPE_PENALTY]; + assert_eq!(0, super::mapping_f2(&u, &p, &mut f2up)); + println!("F1(u, p) = {:#?}", f2up); } } diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index 96dd86f6..489042cd 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -184,13 +184,13 @@ static void copy_args_into_up_space(const casadi_real** arg) { int cost_function(const casadi_real** arg, casadi_real** res) { const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; copy_args_into_up_space(arg); - + result_space_cost[0] = res[0]; return {{build_config.cost_function_name or 'phi'}}( - args__, - result_space_cost, - allocated_i_workspace_cost, - allocated_r_workspace_cost, + args__, + result_space_cost, + allocated_i_workspace_cost, + allocated_r_workspace_cost, (void*) 0); } @@ -199,13 +199,13 @@ int cost_function(const casadi_real** arg, casadi_real** res) { int grad_cost_function(const casadi_real** arg, casadi_real** res) { const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; - copy_args_into_up_space(arg); + copy_args_into_up_space(arg); result_space_grad[0] = res[0]; return {{build_config.grad_function_name or 'grad_phi'}}( - args__, - result_space_grad, - allocated_i_workspace_grad, - allocated_r_workspace_grad, + args__, + result_space_grad, + allocated_i_workspace_grad, + allocated_r_workspace_grad, (void*) 0); } @@ -214,13 +214,13 @@ int grad_cost_function(const casadi_real** arg, casadi_real** res) { int mapping_f1_function(const casadi_real** arg, casadi_real** res) { const casadi_real* args__[COST_SZ_ARG] = {up_space, up_space + NU}; - copy_args_into_up_space(arg); + copy_args_into_up_space(arg); result_space_f1[0] = res[0]; return {{build_config.alm_mapping_f1_function_name}}( - args__, - result_space_f1, - allocated_i_workspace_f1, - allocated_r_workspace_f1, + args__, + result_space_f1, + allocated_i_workspace_f1, + allocated_r_workspace_f1, (void*) 0); } From 029ec5b0113f45a4b291126031ccedacd917f9d2 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 18:23:58 +0100 Subject: [PATCH 24/42] branch for Python refactoring Python will generate function psi(u, xi, p) will have three parameters instead of two calls for refactoring in C/Rust interfaces (not Rust solver) From ad30887b5e9f635be6f6a2a22677a4c83e0c69ff Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Wed, 25 Sep 2019 18:30:10 +0100 Subject: [PATCH 25/42] Rust test to demonstrate ALM in loop example of ALM for parametric problem in loop --- src/alm/tests.rs | 67 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/src/alm/tests.rs b/src/alm/tests.rs index 4d0fcbff..44758218 100644 --- a/src/alm/tests.rs +++ b/src/alm/tests.rs @@ -303,6 +303,73 @@ fn t_alm_numeric_test_2() { println!("F2(u*) = {:#?}", &f2u); } +fn mockito_f0(_u: &[f64], _xi: &[f64], _p: &[f64], cost: &mut f64) -> Result<(), SolverError> { + *cost = 0.0; + Ok(()) +} + +fn mockito_jacobian( + _u: &[f64], + _xi: &[f64], + _p: &[f64], + _grad: &mut [f64], +) -> Result<(), SolverError> { + Ok(()) +} + +#[test] +fn t_alm_numeric_test_repeat() { + let tolerance = 1e-8; + let nx = 3; + let n1 = 2; + let n2 = 4; + let lbfgs_mem = 3; + let panoc_cache = PANOCCache::new(nx, tolerance, lbfgs_mem); + let mut alm_cache = AlmCache::new(panoc_cache, n1, n2); + + for i in 1..3 { + let bounds = Ball2::new(None, 10.0); + let set_y = Ball2::new(None, 10000.0); + + let parameter = [1.0, 2.0 * i as f64]; + + let f_ = |u: &[f64], xi: &[f64], cost: &mut f64| -> Result<(), SolverError> { + mockito_f0(u, xi, ¶meter, cost) + }; + + let jf_ = |u: &[f64], xi: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { + mockito_jacobian(u, xi, ¶meter, grad) + }; + + let set_c_b = Ball2::new(None, 1.0); + let alm_problem = AlmProblem::new( + bounds, + Some(set_c_b), + Some(set_y), + f_, + jf_, + Some(mocks::mapping_f1_affine), + Some(mapping_f2), + n1, + n2, + ); + + // ALM *borrows* the cache: lovely! Otherwise we wouldn't be able to + // run this in a loop with the cache allocated outside the loop once + let mut alm_optimizer = AlmOptimizer::new(&mut alm_cache, alm_problem) + .with_delta_tolerance(1e-4) + .with_epsilon_tolerance(1e-5) + .with_initial_inner_tolerance(1e-4); + + let mut u = vec![0.0; nx]; + let solver_result = alm_optimizer.solve(&mut u); + assert!(solver_result.is_ok()); + assert_eq!( + ExitStatus::Converged, + solver_result.unwrap().exit_status()); + } +} + #[test] fn t_alm_numeric_test_out_of_time() { let tolerance = 1e-8; From 2a639f94391b5489c2d46fcc30d1e2a30c9cd548 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 01:25:50 +0100 Subject: [PATCH 26/42] reorganised __generate_casadi_code ready for big refactoring --- .../opengen/builder/optimizer_builder.py | 177 +++++++++++------- .../opengen/templates/icasadi_lib.rs.template | 1 + .../opengen/templates/interface.c.template | 24 +-- 3 files changed, 126 insertions(+), 76 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 3b22ee47..5ab1c4c6 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -205,34 +205,102 @@ def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): with open(memory_path, "w") as fh: fh.write(output_template) + def __construct_function_psi(self) -> cs.Function: + logging.info("Defining function psi(u, xi, p), where xi = (c, y)") + problem = self.__problem + u = problem.decision_variables + p = problem.parameter_variables + n2 = problem.dim_constraints_penalty() + n1 = problem.dim_constraints_aug_lagrangian() + phi = problem.cost_function + alm_set_c = problem.alm_set_c + f1 = problem.penalty_mapping_f1 + f2 = problem.penalty_mapping_f2 + + psi = phi + + if n1 + n2 > 0: + n_xi = n1 + 1 + xi = cs.SX.sym('xi', n_xi, 1) if isinstance(u, cs.SX) \ + else cs.MX.sym('xi', n_xi, 1) + + if n1 > 0: + sq_dist_term = alm_set_c.distance_squared(f1 + xi[1:n1+1]/xi[0]) + psi += xi[0] * sq_dist_term / 2 + + if n2 > 0: + psi += xi[0] * cs.norm_2(f2) / 2 + + psi_fun = cs.Function('psi', [u, xi, p], [psi]) + return psi_fun + + def __construct_mapping_f1_function(self) -> cs.Function: + logging.info("Defining function F1(u, p)") + problem = self.__problem + u = problem.decision_variables + p = problem.parameter_variables + n1 = problem.dim_constraints_aug_lagrangian() + f1 = problem.penalty_mapping_f1 + + if n1 > 0: + mapping_f1 = f1 + else: + mapping_f1 = 0 + + alm_mapping_f1_fun = cs.Function( + self.__build_config.alm_mapping_f1_function_name, + [u, p], [mapping_f1]) + return alm_mapping_f1_fun + + def __construct_mapping_f2_function(self) -> cs.Function: + logging.info("Defining function F2(u, p)") + problem = self.__problem + u = problem.decision_variables + p = problem.parameter_variables + n2 = problem.dim_constraints_penalty() + + if n2 > 0: + penalty_constraints = problem.penalty_mapping_f2 + else: + penalty_constraints = 0 + + alm_mapping_f2_fun = cs.Function( + self.__build_config.constraint_penalty_function_name, + [u, p], [penalty_constraints]) + + return alm_mapping_f2_fun + def __generate_casadi_code(self): - """Generates CasADi code""" + """Generates CasADi C code""" logging.info("Defining CasADi functions and generating C code") - u = self.__problem.decision_variables - p = self.__problem.parameter_variables - ncp = self.__problem.dim_constraints_penalty() - nalm = self.__problem.dim_constraints_aug_lagrangian() - print("number of ALM constraints:", nalm) - phi = self.__problem.cost_function - print("number of PM constraints:", ncp) + problem = self.__problem + bconfig = self.__build_config + u = problem.decision_variables + p = problem.parameter_variables + nalm = problem.dim_constraints_aug_lagrangian() + n2 = problem.dim_constraints_penalty() + phi = problem.cost_function + + psi_fun = self.__construct_function_psi() + print(psi_fun) # If there are penalty-type constraints, we need to define a modified # cost function - if ncp > 0: - penalty_function = self.__problem.penalty_function - mu = cs.SX.sym("mu", self.__problem.dim_constraints_penalty()) \ - if isinstance(p, cs.SX) else cs.MX.sym("mu", self.__problem.dim_constraints_penalty()) + if n2 > 0: + penalty_function = problem.penalty_function + mu = cs.SX.sym("mu", problem.dim_constraints_penalty()) \ + if isinstance(p, cs.SX) else cs.MX.sym("mu", problem.dim_constraints_penalty()) p = cs.vertcat(p, mu) - phi += cs.dot(mu, penalty_function(self.__problem.penalty_mapping_f2)) + phi += cs.dot(mu, penalty_function(problem.penalty_mapping_f2)) # Define cost and its gradient as CasADi functions - cost_fun = cs.Function(self.__build_config.cost_function_name, [u, p], [phi]) - grad_cost_fun = cs.Function(self.__build_config.grad_function_name, + cost_fun = cs.Function(bconfig.cost_function_name, [u, p], [phi]) + grad_cost_fun = cs.Function(bconfig.grad_function_name, [u, p], [cs.jacobian(phi, u)]) # Filenames of cost and gradient (C file names) - cost_file_name = self.__build_config.cost_function_name + ".c" - grad_file_name = self.__build_config.grad_function_name + ".c" + cost_file_name = bconfig.cost_function_name + ".c" + grad_file_name = bconfig.grad_function_name + ".c" # Code generation using CasADi (cost and gradient) cost_fun.generate(cost_file_name) @@ -243,47 +311,28 @@ def __generate_casadi_code(self): shutil.move(cost_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_COST_FNAME)) shutil.move(grad_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_GRAD_FNAME)) - # Next, we generate a CasADi function for the augmented Lagrangian constraints, - # that is, mapping F1(u, p) - if nalm > 0: - mapping_f1 = self.__problem.penalty_mapping_f1 - else: - mapping_f1 = 0 - + # ----------------------------------------------------------------------- + mapping_f1_fun = self.__construct_mapping_f1_function() # Target C file name of mapping F1(u, p) - alm_mapping_f1_file_name = \ - self.__build_config.alm_mapping_f1_function_name + ".c" - # Define CasADi function F1(u, p) - alm_mapping_f1_fun = cs.Function( - self.__build_config.alm_mapping_f1_function_name, - [u, p], [mapping_f1]) + f1_file_name = bconfig.alm_mapping_f1_function_name + ".c" # Generate code for F1(u, p) - alm_mapping_f1_fun.generate(alm_mapping_f1_file_name) + mapping_f1_fun.generate(f1_file_name) # Move auto-generated file to target folder - shutil.move(alm_mapping_f1_file_name, + shutil.move(f1_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_ALM_MAPPING_F1_FNAME)) - # Lastly, we generate code for the penalty constraints; if there aren't - # any, we generate the function c(u; p) = 0 (which will not be used) - if ncp > 0: - penalty_constraints = self.__problem.penalty_mapping_f2 - else: - penalty_constraints = 0 - - # Target C file name - constraints_penalty_file_name = \ - self.__build_config.constraint_penalty_function_name + ".c" - # Define CasADi function for c(u; q) - constraint_penalty_fun = cs.Function( - self.__build_config.constraint_penalty_function_name, - [u, p], [penalty_constraints]) - # Generate code - constraint_penalty_fun.generate(constraints_penalty_file_name) + # ----------------------------------------------------------------------- + mapping_f2_fun = self.__construct_mapping_f2_function() + f2_file_name = bconfig.constraint_penalty_function_name + ".c" + mapping_f2_fun.generate(f2_file_name) # Move auto-generated file to target folder - shutil.move(constraints_penalty_file_name, + shutil.move(f2_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_PNLT_CONSTRAINTS_FNAME)) - self.__generate_memory_code(cost_fun, grad_cost_fun, alm_mapping_f1_fun, constraint_penalty_fun) + self.__generate_memory_code(cost_fun, + grad_cost_fun, + mapping_f1_fun, + mapping_f2_fun) def __build_icasadi(self): icasadi_dir = self.__icasadi_target_dir() @@ -458,17 +507,17 @@ def build(self): self.__generate_cargo_toml() # generate Cargo.toml using template self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files - self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c - self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) - self.__generate_build_rs() # generate build.rs file - self.__generate_yaml_data_file() - - if not self.__generate_not_build: - logging.info("Building optimizer") - self.__build_optimizer() # build overall project - - if self.__build_config.tcp_interface_config is not None: - logging.info("Generating TCP/IP server") - self.__generate_code_tcp_interface() - if not self.__generate_not_build: - self.__build_tcp_iface() + self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c + # self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) + # self.__generate_build_rs() # generate build.rs file + # self.__generate_yaml_data_file() + # + # if not self.__generate_not_build: + # logging.info("Building optimizer") + # self.__build_optimizer() # build overall project + # + # if self.__build_config.tcp_interface_config is not None: + # logging.info("Generating TCP/IP server") + # self.__generate_code_tcp_interface() + # if not self.__generate_not_build: + # self.__build_tcp_iface() diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 0926727e..880aab59 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -233,3 +233,4 @@ mod tests { } } + diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index 489042cd..0ea05b7c 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -87,25 +87,25 @@ extern int {{build_config.constraint_penalty_function_name}}( * Integer workspaces */ #if COST_SZ_IW > 0 -static casadi_int allocated_i_workspace_cost[COST_SZ_IW]; /* cost (int ) */ +static casadi_int allocated_i_workspace_cost[COST_SZ_IW]; /* cost (int ) */ #else static casadi_int *allocated_i_workspace_cost = NULL; #endif #if GRAD_SZ_IW > 0 -static casadi_int allocated_i_workspace_grad[GRAD_SZ_IW]; /* grad (int ) */ +static casadi_int allocated_i_workspace_grad[GRAD_SZ_IW]; /* grad (int ) */ #else static casadi_int *allocated_i_workspace_grad = NULL; #endif #if F1_SZ_IW > 0 -static casadi_int allocated_i_workspace_f1[F1_SZ_IW]; +static casadi_int allocated_i_workspace_f1[F1_SZ_IW]; /* f1 (int ) */ #else static casadi_int *allocated_i_workspace_f1 = NULL; #endif #if F2_SZ_IW > 0 -static casadi_int allocated_i_workspace_f2[F2_SZ_IW]; +static casadi_int allocated_i_workspace_f2[F2_SZ_IW]; /* f2 (int ) */ #else static casadi_int *allocated_i_workspace_f2 = NULL; #endif @@ -115,26 +115,26 @@ static casadi_int *allocated_i_workspace_f2 = NULL; * Real workspaces */ #if COST_SZ_W > 0 -static casadi_real allocated_r_workspace_cost[COST_SZ_W]; /* cost (real) */ +static casadi_real allocated_r_workspace_cost[COST_SZ_W]; /* cost (real) */ #else static casadi_real *allocated_r_workspace_cost = NULL; #endif #if GRAD_SZ_W > 0 -static casadi_real allocated_r_workspace_grad[GRAD_SZ_W]; /* grad (real ) */ +static casadi_real allocated_r_workspace_grad[GRAD_SZ_W]; /* grad (real ) */ #else static casadi_real *allocated_r_workspace_grad = NULL; #endif #if F1_SZ_W > 0 -static casadi_real allocated_r_workspace_f1[F1_SZ_W]; +static casadi_real allocated_r_workspace_f1[F1_SZ_W]; /* f1 (real ) */ #else static casadi_real *allocated_r_workspace_f1 = NULL; #endif #if F2_SZ_W > 0 -static casadi_real allocated_r_workspace_f2[F2_SZ_W]; +static casadi_real allocated_r_workspace_f2[F2_SZ_W]; /* f2 (real ) */ #else static casadi_real *allocated_r_workspace_f2 = NULL; #endif @@ -143,27 +143,27 @@ static casadi_real *allocated_r_workspace_f2 = NULL; * Result workspaces */ #if COST_SZ_RES > 0 -static casadi_real *result_space_cost[COST_SZ_RES ]; /* cost (real) */ +static casadi_real *result_space_cost[COST_SZ_RES ]; /* cost (res ) */ #else static casadi_real **result_space_cost = NULL; #endif #if GRAD_SZ_RES > 0 -static casadi_real *result_space_grad[GRAD_SZ_RES]; /* grad (real) */ +static casadi_real *result_space_grad[GRAD_SZ_RES]; /* grad (res ) */ #else static casadi_real **result_space_grad = NULL; #endif #if F1_SZ_RES > 0 -static casadi_real *result_space_f1[F1_SZ_RES]; +static casadi_real *result_space_f1[F1_SZ_RES]; /* f1 (res ) */ #else static casadi_real **result_space_f1 = NULL; #endif #if F2_SZ_RES > 0 -static casadi_real *result_space_f2[F2_SZ_RES]; +static casadi_real *result_space_f2[F2_SZ_RES]; /* f2 (res ) */ #else static casadi_real **result_space_f2 = NULL; #endif From 47901c5f2aa8a71d0d94377d5141ba22b7b33855 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 15:04:00 +0100 Subject: [PATCH 27/42] new 3-arg functions updated template for casadi C interface new icasadi lib.rs calling 3-arg functions --- .../opengen/builder/optimizer_builder.py | 126 +++++++++--------- .../opengen/templates/icasadi_lib.rs.template | 14 +- .../opengen/templates/interface.c.template | 71 +++++++--- 3 files changed, 130 insertions(+), 81 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 5ab1c4c6..b15dbdf1 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -55,11 +55,9 @@ def __init__(self, def with_verbosity_level(self, verbosity_level): """Specify the verbosity level - Args: - verbosity_level: level of verbosity (0,1,2,3) + :param verbosity_level: level of verbosity (0,1,2,3) - Returns: - Current builder object + :returns: Current builder object """ self.__verbosity_level = verbosity_level return self @@ -67,11 +65,9 @@ def with_verbosity_level(self, verbosity_level): def with_problem(self, problem): """Specify problem - Args: - problem: optimization problem data + :param problem: optimization problem data - Returns: - Current builder object + :returns: Current builder object """ self.__problem = problem return self @@ -82,30 +78,40 @@ def with_generate_not_build_flag(self, flag): If set to true, the code will be generated, but it will not be build (mainly for debugging purposes) - Args: - flag: generate and not build + :param flag: generate and not build - Returns: - Current builder object + :returns: Current builder object """ self.__generate_not_build = flag return self def __make_build_command(self): + """ + Cargo build command (possibly, with --release) + + """ command = ['cargo', 'build'] if self.__build_config.build_mode.lower() == 'release': command.append('--release') return command def __target_dir(self): - """target directory""" + """ + + Target directory + + """ return os.path.abspath( os.path.join( self.__build_config.build_dir, self.__meta.optimizer_name)) def __icasadi_target_dir(self): - """icasadi target directory""" + """ + + Returns icasadi target directory (instance of os.path) + + """ return os.path.abspath( os.path.join( self.__build_config.build_dir, @@ -114,7 +120,7 @@ def __icasadi_target_dir(self): def __prepare_target_project(self): """Creates folder structure - Creates necessary folders (at build/{project_name}) + Creates necessary folders Runs `cargo init` in that folder """ @@ -130,7 +136,11 @@ def __prepare_target_project(self): make_dir_if_not_exists(target_dir) def __copy_icasadi_to_target(self): - """Copy 'icasadi' into target directory""" + """ + + Copy 'icasadi' folder and its contents into target directory + + """ logging.info("Copying icasadi interface to target directory") origin_icasadi_dir = og_dfn.original_icasadi_dir() target_icasadi_dir = self.__icasadi_target_dir() @@ -143,6 +153,10 @@ def __copy_icasadi_to_target(self): '*.lock', 'ci*', 'target', 'auto*')) def __generate_icasadi_c_interface(self): + """ + Generates the C interface file interface.c + + """ logging.info("Generating intercafe.c (C interface)") file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) @@ -157,7 +171,8 @@ def __generate_icasadi_c_interface(self): fh.write(output_template) def __generate_icasadi_lib(self): - """Generates the Rust library file of icasadi + """ + Generates the Rust library file of icasadi Generates src/lib.rs """ @@ -175,7 +190,10 @@ def __generate_icasadi_lib(self): fh.write(output_template) def __generate_cargo_toml(self): - """Generates Cargo.toml for generated project""" + """ + Generates Cargo.toml for generated project + + """ logging.info("Generating Cargo.toml for target optimizer") target_dir = self.__target_dir() file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) @@ -191,6 +209,14 @@ def __generate_cargo_toml(self): fh.write(output_template) def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): + """ + Creates file casadi_memory.h with memory sizes + + :param cost: cost function (cs.Function) + :param grad: grad function (cs.Function) + :param f1: mapping F1 (cs.Function) + :param f2: mapping F2 (cs.Function) + """ logging.info("Generating memory") file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) @@ -205,9 +231,15 @@ def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): with open(memory_path, "w") as fh: fh.write(output_template) - def __construct_function_psi(self) -> cs.Function: - logging.info("Defining function psi(u, xi, p), where xi = (c, y)") + def __construct_function_psi(self): + """ + Construct function psi and its gradient + + :return: cs.Function objects: psi_fun, grad_psi_fun + """ + logging.info("Defining function psi(u, xi, p) and its gradient") problem = self.__problem + bconfig = self.__build_config u = problem.decision_variables p = problem.parameter_variables n2 = problem.dim_constraints_penalty() @@ -231,8 +263,11 @@ def __construct_function_psi(self) -> cs.Function: if n2 > 0: psi += xi[0] * cs.norm_2(f2) / 2 - psi_fun = cs.Function('psi', [u, xi, p], [psi]) - return psi_fun + jac_psi = cs.jacobian(psi, u) + + psi_fun = cs.Function(bconfig.cost_function_name, [u, xi, p], [psi]) + grad_psi_fun = cs.Function(bconfig.grad_function_name, [u, xi, p], [jac_psi]) + return psi_fun, grad_psi_fun def __construct_mapping_f1_function(self) -> cs.Function: logging.info("Defining function F1(u, p)") @@ -273,49 +308,21 @@ def __construct_mapping_f2_function(self) -> cs.Function: def __generate_casadi_code(self): """Generates CasADi C code""" logging.info("Defining CasADi functions and generating C code") - problem = self.__problem bconfig = self.__build_config - u = problem.decision_variables - p = problem.parameter_variables - nalm = problem.dim_constraints_aug_lagrangian() - n2 = problem.dim_constraints_penalty() - phi = problem.cost_function - psi_fun = self.__construct_function_psi() - print(psi_fun) - - # If there are penalty-type constraints, we need to define a modified - # cost function - if n2 > 0: - penalty_function = problem.penalty_function - mu = cs.SX.sym("mu", problem.dim_constraints_penalty()) \ - if isinstance(p, cs.SX) else cs.MX.sym("mu", problem.dim_constraints_penalty()) - p = cs.vertcat(p, mu) - phi += cs.dot(mu, penalty_function(problem.penalty_mapping_f2)) - - # Define cost and its gradient as CasADi functions - cost_fun = cs.Function(bconfig.cost_function_name, [u, p], [phi]) - grad_cost_fun = cs.Function(bconfig.grad_function_name, - [u, p], [cs.jacobian(phi, u)]) - - # Filenames of cost and gradient (C file names) + # ----------------------------------------------------------------------- + psi_fun, grad_psi_fun = self.__construct_function_psi() cost_file_name = bconfig.cost_function_name + ".c" grad_file_name = bconfig.grad_function_name + ".c" - - # Code generation using CasADi (cost and gradient) - cost_fun.generate(cost_file_name) - grad_cost_fun.generate(grad_file_name) - - # Move generated files to target folder + psi_fun.generate(cost_file_name) + grad_psi_fun.generate(grad_file_name) icasadi_extern_dir = os.path.join(self.__icasadi_target_dir(), "extern") shutil.move(cost_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_COST_FNAME)) shutil.move(grad_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_GRAD_FNAME)) # ----------------------------------------------------------------------- mapping_f1_fun = self.__construct_mapping_f1_function() - # Target C file name of mapping F1(u, p) f1_file_name = bconfig.alm_mapping_f1_function_name + ".c" - # Generate code for F1(u, p) mapping_f1_fun.generate(f1_file_name) # Move auto-generated file to target folder shutil.move(f1_file_name, @@ -329,11 +336,10 @@ def __generate_casadi_code(self): shutil.move(f2_file_name, os.path.join(icasadi_extern_dir, _AUTOGEN_PNLT_CONSTRAINTS_FNAME)) - self.__generate_memory_code(cost_fun, - grad_cost_fun, - mapping_f1_fun, - mapping_f2_fun) + self.__generate_memory_code(psi_fun, grad_psi_fun, + mapping_f1_fun, mapping_f2_fun) + # TODO: it seems that the following method is never used def __build_icasadi(self): icasadi_dir = self.__icasadi_target_dir() command = self.__make_build_command() @@ -508,8 +514,8 @@ def build(self): self.__generate_icasadi_lib() # generate icasadi lib.rs self.__generate_casadi_code() # generate all necessary CasADi C files self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c - # self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) - # self.__generate_build_rs() # generate build.rs file + self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) + self.__generate_build_rs() # generate build.rs file # self.__generate_yaml_data_file() # # if not self.__generate_not_build: diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 880aab59..d96808c9 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -63,11 +63,11 @@ extern "C" { /// - `u.len() == NUM_DECISION_VARIABLES` /// - `static_params.len() == NUM_STATIC_PARAMETERS` /// -pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { +pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES); assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); - let arguments = &[u.as_ptr(), static_params.as_ptr()]; + let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let cost = &mut [cost_value as *mut c_double]; unsafe { @@ -98,12 +98,12 @@ pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { /// - `static_params.len() == icasadi::num_static_parameters()` /// - `cost_jacobian.len() == icasadi::num_decision_variables()` /// -pub fn grad(u: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 { +pub fn grad(u: &[f64], xi: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES); assert_eq!(cost_jacobian.len(), NUM_DECISION_VARIABLES); assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); - let arguments = &[u.as_ptr(), static_params.as_ptr()]; + let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let grad = &mut [cost_jacobian.as_mut_ptr()]; unsafe { @@ -202,16 +202,18 @@ mod tests { fn tst_call_cost() { let u = [0.1; NUM_DECISION_VARIABLES]; let p = [0.1; NUM_STATIC_PARAMETERS]; + let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut cost = 0.0; - assert_eq!(0, super::cost(&u, &p, &mut cost)); + assert_eq!(0, super::cost(&u, &p, &xi, &mut cost)); } #[test] fn tst_call_grad() { let u = [0.1; NUM_DECISION_VARIABLES]; let p = [0.1; NUM_STATIC_PARAMETERS]; + let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut grad = [0.0; NUM_DECISION_VARIABLES]; - assert_eq!(0, super::grad(&u, &p, &mut grad)); + assert_eq!(0, super::grad(&u, &p, &xi, &mut grad)); } #[test] diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index 0ea05b7c..cc623523 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -31,9 +31,21 @@ #define TRUE 1 #define FALSE 0 +/* Number of input variables */ #define NU {{ problem.dim_decision_variables() }} + +/* Number of static parameters */ #define NP {{ problem.dim_parameters() + problem.dim_constraints_penalty() }} +/* Dimension of F1 (number of ALM constraints) */ +#define N1 {{ problem.dim_constraints_aug_lagrangian() }} + +/* Dimension of F2 (number of PM constraints) */ +#define N2 {{ problem.dim_constraints_penalty() }} + +/* Dimension of xi = (c, y) */ +#define NXI {% if problem.dim_constraints_aug_lagrangian() + problem.dim_constraints_penalty() > 0 %}{{ 1 + problem.dim_constraints_aug_lagrangian() }}{% endif %} + #ifndef casadi_real #define casadi_real double #endif @@ -42,6 +54,9 @@ #define casadi_int long long int #endif + +/* ------EXTERNAL FUNCTIONS (DEFINED IN C FILES)-------------------------------- */ + /* * CasADi interface for the cost function */ @@ -83,6 +98,8 @@ extern int {{build_config.constraint_penalty_function_name}}( void* mem); +/* ------WORKSPACES------------------------------------------------------------- */ + /* * Integer workspaces */ @@ -170,20 +187,44 @@ static casadi_real **result_space_f2 = NULL; -/* ------U, P------------------------------------------------------------------- */ -static casadi_real up_space[NU+NP]; +/* ------U, XI, P--------------------------------------------------------------- */ + +/* + * Space for storing (u, xi, p) + * that is, uxip_space = [u, xi, p] + * + * 0 NU-1 NU NU+NXI-1 NU+NX NU+NXI+NP-1 + * |----u-----| |-----xi--------| |------p-----------| + */ +static casadi_real uxip_space[NU+NXI+NP]; +/** + * Copy (u, xi, p) into uxip_space + * + * Input arguments: + * - `arg = {u, xi, p}`, where `u`, `xi` and `p` are pointer-to-double + */ +static void copy_args_into_uxip_space(const casadi_real** arg) { + int i; + for (i=0; i Date: Thu, 26 Sep 2019 15:47:07 +0100 Subject: [PATCH 28/42] cover cases F1/F2=[]; fixed bugs in interface.c --- .../opengen/builder/optimizer_builder.py | 22 +++++++--- .../opengen/templates/icasadi_lib.rs.template | 44 ++++++++++--------- .../opengen/templates/interface.c.template | 10 +++-- 3 files changed, 48 insertions(+), 28 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index b15dbdf1..89b52ef7 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -217,7 +217,7 @@ def __generate_memory_code(self, cost=None, grad=None, f1=None, f2=None): :param f1: mapping F1 (cs.Function) :param f2: mapping F2 (cs.Function) """ - logging.info("Generating memory") + logging.info("Generating casadi_memory.h") file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) env = jinja2.Environment(loader=file_loader) template = env.get_template('casadi_memory.h.template') @@ -253,8 +253,11 @@ def __construct_function_psi(self): if n1 + n2 > 0: n_xi = n1 + 1 - xi = cs.SX.sym('xi', n_xi, 1) if isinstance(u, cs.SX) \ - else cs.MX.sym('xi', n_xi, 1) + else: + n_xi = 0 + + xi = cs.SX.sym('xi', n_xi, 1) if isinstance(u, cs.SX) \ + else cs.MX.sym('xi', n_xi, 1) if n1 > 0: sq_dist_term = alm_set_c.distance_squared(f1 + xi[1:n1+1]/xi[0]) @@ -314,6 +317,7 @@ def __generate_casadi_code(self): psi_fun, grad_psi_fun = self.__construct_function_psi() cost_file_name = bconfig.cost_function_name + ".c" grad_file_name = bconfig.grad_function_name + ".c" + logging.info("Function psi and its gradient (C code)") psi_fun.generate(cost_file_name) grad_psi_fun.generate(grad_file_name) icasadi_extern_dir = os.path.join(self.__icasadi_target_dir(), "extern") @@ -323,6 +327,7 @@ def __generate_casadi_code(self): # ----------------------------------------------------------------------- mapping_f1_fun = self.__construct_mapping_f1_function() f1_file_name = bconfig.alm_mapping_f1_function_name + ".c" + logging.info("Mapping F1 (C code)") mapping_f1_fun.generate(f1_file_name) # Move auto-generated file to target folder shutil.move(f1_file_name, @@ -331,6 +336,7 @@ def __generate_casadi_code(self): # ----------------------------------------------------------------------- mapping_f2_fun = self.__construct_mapping_f2_function() f2_file_name = bconfig.constraint_penalty_function_name + ".c" + logging.info("Mapping F2 (C code)") mapping_f2_fun.generate(f2_file_name) # Move auto-generated file to target folder shutil.move(f2_file_name, @@ -512,8 +518,13 @@ def build(self): self.__copy_icasadi_to_target() # copy icasadi/ files to target dir self.__generate_cargo_toml() # generate Cargo.toml using template self.__generate_icasadi_lib() # generate icasadi lib.rs - self.__generate_casadi_code() # generate all necessary CasADi C files - self.__generate_icasadi_c_interface() # generate icasadi/extern/icallocator.c + self.__generate_casadi_code() # generate all necessary CasADi C files: + # # - auto_casadi_cost.c + # # - auto_casadi_grad.c + # # - auto_casadi_mapping_f1.c + # # - auto_casadi_mapping_f2.c + # # - casadi_memory.h + self.__generate_icasadi_c_interface() # generate icasadi/extern/interface.c self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) self.__generate_build_rs() # generate build.rs file # self.__generate_yaml_data_file() @@ -527,3 +538,4 @@ def build(self): # self.__generate_code_tcp_interface() # if not self.__generate_not_build: # self.__build_tcp_iface() + diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index d96808c9..08bafb5c 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -14,7 +14,7 @@ //! Generated at: {{timestamp_created}} //! -#![no_std] +// #![no_std] /// Number of static parameters (this also includes penalty constraints) pub const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() + problem.dim_constraints_penalty() or 0 }}; @@ -45,15 +45,16 @@ extern "C" { /// -/// Consume the cost function written in C +/// Consume the cost function psi(u, xi, p) written in C /// /// # Example -/// ``` +/// ```ignore /// 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 u = [1.0, 2.0, 3.0, -5.0, 6.0]; /// let p = [1.0, -1.0]; +/// let xi = [100.0, 0.0, 1.5., 3.0]; /// let mut cost_value = 0.0; -/// icasadi::cost(&u, &p, &mut cost_value); +/// icasadi::cost(&u, &xi, &p, &mut cost_value); /// } /// ``` /// @@ -64,8 +65,8 @@ extern "C" { /// - `static_params.len() == NUM_STATIC_PARAMETERS` /// pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); + assert_eq!(u.len(), NUM_DECISION_VARIABLES, "wrong length of `u`"); + assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "wrong length of `p`"); let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let cost = &mut [cost_value as *mut c_double]; @@ -82,12 +83,13 @@ pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) /// Consume the Jacobian function written in C /// /// # Example -/// ``` +/// ```ignore /// 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 u = [1.0, 2.0, 3.0, -5.0, 6.0]; /// let p = [1.0, -1.0]; +/// let xi = [100.0, 0.0, 1.5., 3.0]; /// let mut jac = [0.0; 10]; -/// icasadi::grad(&u, &p, &mut jac); +/// icasadi::grad(&u, &xi, &p, &mut jac); /// } /// ``` /// @@ -99,9 +101,9 @@ pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) /// - `cost_jacobian.len() == icasadi::num_decision_variables()` /// pub fn grad(u: &[f64], xi: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES); - assert_eq!(cost_jacobian.len(), NUM_DECISION_VARIABLES); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); + assert_eq!(u.len(), NUM_DECISION_VARIABLES, "wrong length of `u`"); + assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "wrong length of `u`"); + assert_eq!(cost_jacobian.len(), NUM_DECISION_VARIABLES, "wrong length of `cost_jacobian`"); let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let grad = &mut [cost_jacobian.as_mut_ptr()]; @@ -168,10 +170,10 @@ pub fn mapping_f2( static_params: &[f64], f2: &mut [f64], ) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS); + assert_eq!(u.len(), NUM_DECISION_VARIABLES, "Incompatible dimension of `u`"); + assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "Incompatible dimension of `p`"); assert!(f2.len() == NUM_CONSTAINTS_TYPE_PENALTY || - NUM_CONSTAINTS_TYPE_PENALTY == 0); + NUM_CONSTAINTS_TYPE_PENALTY == 0, "Incompatible dimension of `f2` (result)"); let arguments = &[u.as_ptr(), static_params.as_ptr()]; let constraints = &mut [f2.as_mut_ptr()]; @@ -204,16 +206,18 @@ mod tests { let p = [0.1; NUM_STATIC_PARAMETERS]; let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut cost = 0.0; - assert_eq!(0, super::cost(&u, &p, &xi, &mut cost)); + assert_eq!(0, super::cost(&u, &xi, &p, &mut cost)); + println!("psi(u, xi, p) = {}", cost); } #[test] fn tst_call_grad() { let u = [0.1; NUM_DECISION_VARIABLES]; let p = [0.1; NUM_STATIC_PARAMETERS]; - let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; + let xi = [10.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut grad = [0.0; NUM_DECISION_VARIABLES]; - assert_eq!(0, super::grad(&u, &p, &xi, &mut grad)); + assert_eq!(0, super::grad(&u, &xi, &p, &mut grad)); + println!("D_u psi(u, xi, p) = {:#?}", grad); } #[test] @@ -231,7 +235,7 @@ mod tests { let p = [0.1; NUM_STATIC_PARAMETERS]; let mut f2up = [0.0; NUM_CONSTAINTS_TYPE_PENALTY]; assert_eq!(0, super::mapping_f2(&u, &p, &mut f2up)); - println!("F1(u, p) = {:#?}", f2up); + println!("F2(u, p) = {:#?}", f2up); } } diff --git a/open-codegen/opengen/templates/interface.c.template b/open-codegen/opengen/templates/interface.c.template index cc623523..0353f049 100644 --- a/open-codegen/opengen/templates/interface.c.template +++ b/open-codegen/opengen/templates/interface.c.template @@ -44,7 +44,7 @@ #define N2 {{ problem.dim_constraints_penalty() }} /* Dimension of xi = (c, y) */ -#define NXI {% if problem.dim_constraints_aug_lagrangian() + problem.dim_constraints_penalty() > 0 %}{{ 1 + problem.dim_constraints_aug_lagrangian() }}{% endif %} +#define NXI {% if problem.dim_constraints_aug_lagrangian() + problem.dim_constraints_penalty() > 0 %}{{ 1 + problem.dim_constraints_aug_lagrangian() }}{% else %}0{% endif %} #ifndef casadi_real #define casadi_real double @@ -223,7 +223,9 @@ static void copy_args_into_up_space(const casadi_real** arg) { /* ------COST------------------------------------------------------------------- */ int cost_function(const casadi_real** arg, casadi_real** res) { - const casadi_real* args__[COST_SZ_ARG] = {uxip_space, uxip_space + NU}; + const casadi_real* args__[COST_SZ_ARG] = {uxip_space, /* :u */ + uxip_space + NU, /* :xi */ + uxip_space + NU + NXI}; /* :p */ copy_args_into_uxip_space(arg); result_space_cost[0] = res[0]; @@ -239,7 +241,9 @@ int cost_function(const casadi_real** arg, casadi_real** res) { /* ------GRADIENT--------------------------------------------------------------- */ int grad_cost_function(const casadi_real** arg, casadi_real** res) { - const casadi_real* args__[COST_SZ_ARG] = {uxip_space, uxip_space + NU}; + const casadi_real* args__[COST_SZ_ARG] = {uxip_space, /* :u */ + uxip_space + NU, /* :xi */ + uxip_space + NU + NXI}; /* :p */ copy_args_into_uxip_space(arg); result_space_grad[0] = res[0]; return {{build_config.grad_function_name or 'grad_phi'}}( From 40e0af089d45100608e78bc95fd3b53a0cfd6137 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 16:05:33 +0100 Subject: [PATCH 29/42] fixed number of params (when 0) --- .../opengen/templates/icasadi_lib.rs.template | 75 ++++++++++++++----- 1 file changed, 57 insertions(+), 18 deletions(-) diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 08bafb5c..3dca1210 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -17,7 +17,7 @@ // #![no_std] /// Number of static parameters (this also includes penalty constraints) -pub const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() + problem.dim_constraints_penalty() or 0 }}; +pub const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() or 0 }}; /// Number of decision variables pub const NUM_DECISION_VARIABLES: usize = {{ problem.dim_decision_variables() }}; @@ -32,10 +32,17 @@ use libc::{c_double, c_int}; // might need to include: c_longlong, c_void /// C interface (Function API exactly as provided by CasADi) extern "C" { - fn cost_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; - fn grad_cost_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; - fn mapping_f1_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; - fn mapping_f2_function(arg: *const *const c_double,casadi_results: *mut *mut c_double) -> c_int; + fn cost_function(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int; + fn grad_cost_function(arg: *const *const c_double, casadi_results: *mut *mut c_double) + -> c_int; + fn mapping_f1_function( + arg: *const *const c_double, + casadi_results: *mut *mut c_double, + ) -> c_int; + fn mapping_f2_function( + arg: *const *const c_double, + casadi_results: *mut *mut c_double, + ) -> c_int; } // END of extern C @@ -66,7 +73,11 @@ extern "C" { /// pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES, "wrong length of `u`"); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "wrong length of `p`"); + assert_eq!( + static_params.len(), + NUM_STATIC_PARAMETERS, + "wrong length of `p`" + ); let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let cost = &mut [cost_value as *mut c_double]; @@ -102,8 +113,16 @@ pub fn cost(u: &[f64], xi: &[f64], static_params: &[f64], cost_value: &mut f64) /// pub fn grad(u: &[f64], xi: &[f64], static_params: &[f64], cost_jacobian: &mut [f64]) -> i32 { assert_eq!(u.len(), NUM_DECISION_VARIABLES, "wrong length of `u`"); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "wrong length of `u`"); - assert_eq!(cost_jacobian.len(), NUM_DECISION_VARIABLES, "wrong length of `cost_jacobian`"); + assert_eq!( + static_params.len(), + NUM_STATIC_PARAMETERS, + "wrong length of `u`" + ); + assert_eq!( + cost_jacobian.len(), + NUM_DECISION_VARIABLES, + "wrong length of `cost_jacobian`" + ); let arguments = &[u.as_ptr(), xi.as_ptr(), static_params.as_ptr()]; let grad = &mut [cost_jacobian.as_mut_ptr()]; @@ -136,10 +155,20 @@ pub fn mapping_f1( static_params: &[f64], f1: &mut [f64], ) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES, "Incompatible dimension of `u`"); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "Incompatible dimension of `p`"); - assert!(f1.len() == NUM_CONSTRAINTS_TYPE_ALM || - NUM_CONSTRAINTS_TYPE_ALM == 0, "Incompatible dimension of `f1` (result)"); + assert_eq!( + u.len(), + NUM_DECISION_VARIABLES, + "Incompatible dimension of `u`" + ); + assert_eq!( + static_params.len(), + NUM_STATIC_PARAMETERS, + "Incompatible dimension of `p`" + ); + assert!( + f1.len() == NUM_CONSTRAINTS_TYPE_ALM || NUM_CONSTRAINTS_TYPE_ALM == 0, + "Incompatible dimension of `f1` (result)" + ); let arguments = &[u.as_ptr(), static_params.as_ptr()]; let constraints = &mut [f1.as_mut_ptr()]; @@ -170,10 +199,20 @@ pub fn mapping_f2( static_params: &[f64], f2: &mut [f64], ) -> i32 { - assert_eq!(u.len(), NUM_DECISION_VARIABLES, "Incompatible dimension of `u`"); - assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS, "Incompatible dimension of `p`"); - assert!(f2.len() == NUM_CONSTAINTS_TYPE_PENALTY || - NUM_CONSTAINTS_TYPE_PENALTY == 0, "Incompatible dimension of `f2` (result)"); + assert_eq!( + u.len(), + NUM_DECISION_VARIABLES, + "Incompatible dimension of `u`" + ); + assert_eq!( + static_params.len(), + NUM_STATIC_PARAMETERS, + "Incompatible dimension of `p`" + ); + assert!( + f2.len() == NUM_CONSTAINTS_TYPE_PENALTY || NUM_CONSTAINTS_TYPE_PENALTY == 0, + "Incompatible dimension of `f2` (result)" + ); let arguments = &[u.as_ptr(), static_params.as_ptr()]; let constraints = &mut [f2.as_mut_ptr()]; @@ -204,7 +243,7 @@ mod tests { fn tst_call_cost() { let u = [0.1; NUM_DECISION_VARIABLES]; let p = [0.1; NUM_STATIC_PARAMETERS]; - let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; + let xi = [2.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut cost = 0.0; assert_eq!(0, super::cost(&u, &xi, &p, &mut cost)); println!("psi(u, xi, p) = {}", cost); @@ -214,7 +253,7 @@ mod tests { fn tst_call_grad() { let u = [0.1; NUM_DECISION_VARIABLES]; let p = [0.1; NUM_STATIC_PARAMETERS]; - let xi = [10.0; NUM_CONSTRAINTS_TYPE_ALM+1]; + let xi = [10.0; NUM_CONSTRAINTS_TYPE_ALM+1]; let mut grad = [0.0; NUM_DECISION_VARIABLES]; assert_eq!(0, super::grad(&u, &xi, &p, &mut grad)); println!("D_u psi(u, xi, p) = {:#?}", grad); From ecc39f1ce221c44698e0dc795f1a55792a4c384d Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 18:04:52 +0100 Subject: [PATCH 30/42] working on codegen (Python to Rust) From 4f04200b7c03af3448e599a01461c2274e265a5e Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 19:09:42 +0100 Subject: [PATCH 31/42] first dummy implementation of auto-gen solver compiles but not tested yet --- .../opengen/builder/optimizer_builder.py | 10 +- .../opengen/templates/optimizer.rs.template | 309 ++++-------------- .../optimizer_cinterface.rs.template | 120 +++++++ 3 files changed, 194 insertions(+), 245 deletions(-) create mode 100644 open-codegen/opengen/templates/optimizer_cinterface.rs.template diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 89b52ef7..ad0bbea2 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -527,11 +527,11 @@ def build(self): self.__generate_icasadi_c_interface() # generate icasadi/extern/interface.c self.__generate_main_project_code() # generate main part of code (at build/{name}/src/main.rs) self.__generate_build_rs() # generate build.rs file - # self.__generate_yaml_data_file() - # - # if not self.__generate_not_build: - # logging.info("Building optimizer") - # self.__build_optimizer() # build overall project + self.__generate_yaml_data_file() + + if not self.__generate_not_build: + logging.info("Building optimizer") + self.__build_optimizer() # build overall project # # if self.__build_config.tcp_interface_config is not None: # logging.info("Generating TCP/IP server") diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index f17f0ae5..ce8eed64 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -10,295 +10,124 @@ use icasadi; use libc::{c_double, c_ulong, c_ulonglong}; use optimization_engine::continuation::HomotopyCache; {% endif %} -use optimization_engine::{constraints::*, panoc::*, *}; +use optimization_engine::{constraints::*, panoc::*, alm::*, *}; + +// ---Private Constants---------------------------------------------------------------------------------- /// Tolerance of inner solver -const TOLERANCE_INNER_SOLVER: f64 = {{solver_config.tolerance or 0.0001}}; +const EPSILON_TOLERANCE: f64 = {{solver_config.tolerance or 0.0001}}; + +/// Delta tolerance +const DELTA_TOLERANCE: f64 = {{solver_config.constraints_tolerance or 0.0001}}; /// LBFGS memory const LBFGS_MEMORY: usize = {{solver_config.lbfgs_memory or 10}}; /// Maximum number of iterations of the inner solver -const MAX_INNER_ITERS: usize = {{solver_config.max_inner_iterations or 2000}}; - -/// Number of decision variables -pub const {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES: usize = {{problem.dim_decision_variables()}}; - -/// Number of parameters -pub const {{meta.optimizer_name|upper}}_NUM_PARAMETERS: usize = {{problem.dim_parameters()}}; - -/// Number of penalty constraints -const {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY: usize = {{problem.dim_constraints_penalty() or 0}}; - -{% if problem.dim_constraints_aug_lagrangian() > 0 %} -// Number of parameters associated with augmented Lagrangian -const NCAL: usize = {{problem.dim_constraints_aug_lagrangian()}}; -{% endif %} - -// Required tolerance for c(u; p) -const CONSTRAINTS_TOLERANCE: f64 = {{solver_config.constraints_tolerance or 0.0001}}; +const MAX_INNER_ITERATIONS: usize = {{solver_config.max_inner_iterations or 10000}}; -// Maximum number of outer iterations +/// Maximum number of outer iterations const MAX_OUTER_ITERATIONS: usize = {{solver_config.max_outer_iterations or 10}}; -{% if problem.dim_constraints_penalty() > 0 %} -// Update factor for the penalty method -const PENALTY_WEIGHT_UPDATE_FACTOR: f64 = {{solver_config.penalty_weight_update_factor or 5.0}}; +/// Maximum execution duration in microseconds +const MAX_DURATION_MICROS: u64 = {{solver_config.max_duration_micros}}; -// Initial values of the weights -{% if solver_config.initial_penalty_weights | length == 1 -%} -const INITIAL_PENALTY_WEIGHTS: &[f64] = &[{{solver_config.initial_penalty_weights[0]}}; {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY]; -{% else -%} -const INITIAL_PENALTY_WEIGHTS: &[f64] = &[{{solver_config.initial_penalty_weights|join(', ')}}]; -{% endif %} -{% endif %} -// Parameters of the constraints -{% if 'Ball2' == problem.constraints.__class__.__name__ and problem.constraints.center is not none -%} -const XC: [f64; {{problem.dim_decision_variables()}}] = [{{problem.constraints.center | join(', ')}}]; -{%- elif 'Rectangle' == problem.constraints.__class__.__name__ -%} -{%- if problem.constraints.xmin is not none -%} -const XMIN :Option<&[f64]> = Some(&[{{problem.constraints.xmin|join(', ')}}]); -{%- else -%} -const XMIN :Option<&[f64]> = None; -{%- endif -%} -{% if problem.constraints.xmax is not none %} -const XMAX :Option<&[f64]> = Some(&[{{problem.constraints.xmax|join(', ')}}]); -{%- else %} -const XMAX :Option<&[f64]> = None; -{% endif %} -{%- endif %} -{% if solver_config.max_duration_micros > 0 %} -// Maximum execution duration in microseconds -const MAX_DURATION_MICROS: u64 = {{solver_config.max_duration_micros}}; -{% else %} -# --- {{ solver_config.max_duration_micros }} -{% endif %} +// ---Public Constants----------------------------------------------------------------------------------- -{% if activate_clib_generation -%} -/// Solver cache -#[allow(non_camel_case_types)] -pub struct {{meta.optimizer_name}}Cache { - cache: HomotopyCache, -} - -impl {{meta.optimizer_name}}Cache { - pub fn new(cache: HomotopyCache) -> Self { - {{meta.optimizer_name}}Cache { cache } - } -} +/// Number of decision variables +pub const {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES: usize = {{problem.dim_decision_variables()}}; -/// {{meta.optimizer_name}} version of ExitStatus -#[allow(non_camel_case_types)] -#[repr(C)] -pub enum {{meta.optimizer_name}}ExitStatus { - /// The algorithm has converged - /// - /// All termination criteria are satisfied and the algorithm - /// converged within the available time and number of iterations - {{meta.optimizer_name}}Converged, - /// Failed to converge because the maximum number of iterations was reached - {{meta.optimizer_name}}NotConvergedIterations, - /// Failed to converge because the maximum execution time was reached - {{meta.optimizer_name}}NotConvergedOutOfTime, - /// If the gradient or cost function cannot be evaluated internally - {{meta.optimizer_name}}NotConvergedCost, - /// Computation failed and NaN/Infinite value was obtained - {{meta.optimizer_name}}NotConvergedNotFiniteComputation, -} +/// Number of parameters +pub const {{meta.optimizer_name|upper}}_NUM_PARAMETERS: usize = {{problem.dim_parameters()}}; -/// {{meta.optimizer_name}} version of HomotopySolverStatus -#[repr(C)] -pub struct {{meta.optimizer_name}}SolverStatus { - /// Exit status - exit_status: {{meta.optimizer_name}}ExitStatus, - /// Number of outer iterations - num_outer_iterations: c_ulong, - /// Total number of inner iterations - /// - /// This is the sum of the numbers of iterations of - /// inner solvers - num_inner_iterations: c_ulong, - /// Norm of the fixed-point residual of the the problem - last_problem_norm_fpr: c_double, - /// Maximum constraint violation - max_constraint_violation: c_double, - /// Total solve time - solve_time_ns: c_ulonglong, -} +/// Number of parameters associated with augmented Lagrangian +pub const {{meta.optimizer_name|upper}}_N1: usize = {{problem.dim_constraints_aug_lagrangian()}}; -/// Allocate memory and setup the solver -#[no_mangle] -pub extern "C" fn {{meta.optimizer_name|lower}}_new() -> *mut {{meta.optimizer_name}}Cache { - Box::into_raw(Box::new({{meta.optimizer_name}}Cache::new(initialize_solver()))) -} +/// Number of penalty constraints +pub const {{meta.optimizer_name|upper}}_N2: usize = {{problem.dim_constraints_penalty() or 0}}; -/// Run the solver on the input and parameters -#[no_mangle] -pub extern "C" fn {{meta.optimizer_name|lower}}_solve( - instance: *mut {{meta.optimizer_name}}Cache, - u: *mut c_double, - params: *const c_double, -) -> {{meta.optimizer_name}}SolverStatus { - // Convert all pointers into the required data structures - let instance: &mut {{meta.optimizer_name}}Cache = unsafe { - assert!(!instance.is_null()); - &mut *instance - }; +{#- THIS WAY WE CAN INCLUDE ANOTHER TEMPLATE -#} +{% include "optimizer_cinterface.rs.template" %} - let u = unsafe { - assert!(!u.is_null()); - std::slice::from_raw_parts_mut(u as *mut f64, {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES) - }; - let params = unsafe { - assert!(!params.is_null()); - std::slice::from_raw_parts(params as *const f64, {{meta.optimizer_name|upper}}_NUM_PARAMETERS) - }; +// ---Internal private helper functions------------------------------------------------------------------ - let status = solve(params,&mut instance.cache, u); - - match status { - Ok(status) => {{meta.optimizer_name}}SolverStatus { - exit_status: match status.exit_status() { - core::ExitStatus::Converged => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}Converged, - core::ExitStatus::NotConvergedIterations => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedIterations, - core::ExitStatus::NotConvergedOutOfTime => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedOutOfTime, - }, - num_outer_iterations: status.num_outer_iterations() as c_ulong, - num_inner_iterations: status.num_inner_iterations() as c_ulong, - last_problem_norm_fpr: status.last_problem_norm_fpr(), - max_constraint_violation: status.max_constraint_violation(), - solve_time_ns: status.solve_time().as_nanos() as c_ulonglong, - }, - Err(e) => {{meta.optimizer_name}}SolverStatus { - exit_status: match e { - SolverError::Cost => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedCost, - SolverError::NotFiniteComputation => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedNotFiniteComputation, - }, - num_outer_iterations: std::u64::MAX as c_ulong, - num_inner_iterations: std::u64::MAX as c_ulong, - last_problem_norm_fpr: std::f64::INFINITY, - max_constraint_violation: std::f64::INFINITY, - solve_time_ns: std::u64::MAX as c_ulonglong, - }, - } +fn make_constraints() -> impl Constraint { + let bounds = Ball2::new(None, 10.0); + bounds } -/// Deallocate the solver's memory -#[no_mangle] -pub extern "C" fn {{meta.optimizer_name|lower}}_free(instance: *mut {{meta.optimizer_name}}Cache) { - // Add impl - unsafe { - assert!(!instance.is_null()); - Box::from_raw(instance); - } -} -{% endif %} +// ---Main public API functions-------------------------------------------------------------------------- + /// Initialisation of the solver -pub fn initialize_solver() -> continuation::HomotopyCache { - let panoc_cache = PANOCCache::new({{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES, TOLERANCE_INNER_SOLVER, LBFGS_MEMORY); +pub fn initialize_solver() -> AlmCache { + let panoc_cache = PANOCCache::new({{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES, EPSILON_TOLERANCE, LBFGS_MEMORY); {% if solver_config.cbfgs_alpha is not none and solver_config.cbfgs_epsilon is not none -%} let panoc_cache = panoc_cache.with_cbfgs_parameters({{solver_config.cbfgs_alpha}}, {{solver_config.cbfgs_epsilon}}, {{solver_config.cbfgs_sy_epsilon}}); - {% endif %} - - - let {% if problem.dim_constraints_penalty() > 0 %}mut{% endif %} homotopy_cache = continuation::HomotopyCache::new(panoc_cache); + {% endif -%} + let alm_cache = AlmCache::new(panoc_cache, {{meta.optimizer_name|upper}}_N1, {{meta.optimizer_name|upper}}_N2); -{% if problem.dim_constraints_penalty() > 0 %} - // Define the initial weights, the update rule and the update factor - let idx_y: Vec = ({{meta.optimizer_name|upper}}_NUM_PARAMETERS..{{meta.optimizer_name|upper}}_NUM_PARAMETERS + - {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY).collect(); - homotopy_cache.add_continuations( - &idx_y[..], - INITIAL_PENALTY_WEIGHTS, - &[0.; {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY], - &[continuation::ContinuationMode::Geometric(PENALTY_WEIGHT_UPDATE_FACTOR); - {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY], - ); -{% endif %} - - homotopy_cache + alm_cache } pub fn solve( p: &[f64], - cache: &mut continuation::HomotopyCache, + alm_cache: &mut AlmCache, u: &mut [f64], -) -> Result { +) -> Result { assert_eq!(p.len(), {{meta.optimizer_name|upper}}_NUM_PARAMETERS, "Wrong number of parameters (p)"); assert_eq!(u.len(), {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES, "Wrong number of decision variables (u)"); - let mut q_augmented_params = [0.0; {{meta.optimizer_name|upper}}_NUM_PARAMETERS + {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY]; - q_augmented_params[0..{{meta.optimizer_name|upper}}_NUM_PARAMETERS].copy_from_slice(p); - - - // cost function, f(u; q) - let cost_function = |u: &[f64], q: &[f64], cost: &mut f64| -> Result<(), SolverError> { - icasadi::cost(u, q, cost); + let psi = |u: &[f64], xi: &[f64], cost: &mut f64| -> Result<(), SolverError> { + icasadi::cost(&u, &xi, &p, cost); Ok(()) }; - // parametric gradient, df(u, q) - let gradient_function = |u: &[f64], q: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { - if icasadi::grad(u, q, grad) == 0 { - Ok(()) - } else { - Err(SolverError::Cost) - } + let grad_psi = |u: &[f64], xi: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { + icasadi::grad(&u, &xi, &p, grad); + Ok(()) + }; + let f1 = |u: &[f64], res: &mut [f64]| -> Result<(), SolverError> { + icasadi::mapping_f1(&u, &p, res); + Ok(()) }; - /* penalty-type constraints: c(u; p) */ - let penalty_constr_function = - |u: &[f64], q: &[f64], constraints: &mut [f64]| -> Result<(), SolverError> { - if icasadi::mapping_f2(u, q, constraints) == 0 { - Ok(()) - } else { - Err(SolverError::Cost) - } + let f2 = |u: &[f64], res: &mut [f64]| -> Result<(), SolverError> { + icasadi::mapping_f2(&u, &p, res); + Ok(()) }; - // Constraints... -{%- if problem.constraints is none %} - let bounds = NoConstraints::new(); -{%- elif 'Ball2' == problem.constraints.__class__.__name__ %} -{%- if problem.constraints.center is none %} - let bounds = Ball2::new(None, {{problem.constraints.radius}}); -{%- else %} - let bounds = Ball2::new(Some(&XC), {{problem.constraints.radius}}); -{%- endif %} -{%- elif 'Rectangle' == problem.constraints.__class__.__name__ %} - let bounds = Rectangle::new(XMIN, XMAX); -{%- endif %} - - - // Define homotopy problem - let homotopy_problem = continuation::HomotopyProblem::new( + let bounds = make_constraints(); + let set_y = Ball2::new(None, 10000.0); + let set_c = Ball2::new(None, 1.0); + + let alm_problem = AlmProblem::new( bounds, - gradient_function, - cost_function, - penalty_constr_function, - {{meta.optimizer_name|upper}}_NUM_CONSTAINTS_TYPE_PENALTY + Some(set_c), + Some(set_y), + psi, + grad_psi, + Some(f1), + Some(f2), + {{meta.optimizer_name|upper}}_N1, + {{meta.optimizer_name|upper}}_N2, ); - // construct a homotopy optimizer - let homotopy_optimizer = continuation::HomotopyOptimizer::new(homotopy_problem, cache) - .with_constraint_tolerance(CONSTRAINTS_TOLERANCE) + let mut alm_optimizer = AlmOptimizer::new(alm_cache, alm_problem) + .with_delta_tolerance(DELTA_TOLERANCE) + .with_epsilon_tolerance(EPSILON_TOLERANCE) + .with_initial_inner_tolerance(1e-2) + .with_max_duration(std::time::Duration::from_micros(MAX_DURATION_MICROS)) .with_max_outer_iterations(MAX_OUTER_ITERATIONS) - .with_max_inner_iterations(MAX_INNER_ITERS); - -{% if solver_config.max_duration_micros > 0 %} - // set the maximum execution duration - let mut homotopy_optimizer = homotopy_optimizer - .with_max_duration(std::time::Duration::from_micros(MAX_DURATION_MICROS)); -{% endif %} + .with_max_inner_iterations(MAX_INNER_ITERATIONS); - // solve the problem and return its status - // parameter `u` is updated with the solution - homotopy_optimizer.solve(u, &q_augmented_params) + // solve the problem using `u` an the initial condition + // return the problem status (instance of `AlmOptimizerStatus`) + alm_optimizer.solve(u) } diff --git a/open-codegen/opengen/templates/optimizer_cinterface.rs.template b/open-codegen/opengen/templates/optimizer_cinterface.rs.template new file mode 100644 index 00000000..96bf0c6e --- /dev/null +++ b/open-codegen/opengen/templates/optimizer_cinterface.rs.template @@ -0,0 +1,120 @@ +{% if activate_clib_generation -%} +/// Solver cache +#[allow(non_camel_case_types)] +pub struct {{meta.optimizer_name}}Cache { + cache: HomotopyCache, +} + +impl {{meta.optimizer_name}}Cache { + pub fn new(cache: HomotopyCache) -> Self { + {{meta.optimizer_name}}Cache { cache } + } +} + +/// {{meta.optimizer_name}} version of ExitStatus +#[allow(non_camel_case_types)] +#[repr(C)] +pub enum {{meta.optimizer_name}}ExitStatus { + /// The algorithm has converged + /// + /// All termination criteria are satisfied and the algorithm + /// converged within the available time and number of iterations + {{meta.optimizer_name}}Converged, + /// Failed to converge because the maximum number of iterations was reached + {{meta.optimizer_name}}NotConvergedIterations, + /// Failed to converge because the maximum execution time was reached + {{meta.optimizer_name}}NotConvergedOutOfTime, + /// If the gradient or cost function cannot be evaluated internally + {{meta.optimizer_name}}NotConvergedCost, + /// Computation failed and NaN/Infinite value was obtained + {{meta.optimizer_name}}NotConvergedNotFiniteComputation, +} + +/// {{meta.optimizer_name}} version of HomotopySolverStatus +#[repr(C)] +pub struct {{meta.optimizer_name}}SolverStatus { + /// Exit status + exit_status: {{meta.optimizer_name}}ExitStatus, + /// Number of outer iterations + num_outer_iterations: c_ulong, + /// Total number of inner iterations + /// + /// This is the sum of the numbers of iterations of + /// inner solvers + num_inner_iterations: c_ulong, + /// Norm of the fixed-point residual of the the problem + last_problem_norm_fpr: c_double, + /// Maximum constraint violation + max_constraint_violation: c_double, + /// Total solve time + solve_time_ns: c_ulonglong, +} + +/// Allocate memory and setup the solver +#[no_mangle] +pub extern "C" fn {{meta.optimizer_name|lower}}_new() -> *mut {{meta.optimizer_name}}Cache { + Box::into_raw(Box::new({{meta.optimizer_name}}Cache::new(initialize_solver()))) +} + +/// Run the solver on the input and parameters +#[no_mangle] +pub extern "C" fn {{meta.optimizer_name|lower}}_solve( + instance: *mut {{meta.optimizer_name}}Cache, + u: *mut c_double, + params: *const c_double, +) -> {{meta.optimizer_name}}SolverStatus { + // Convert all pointers into the required data structures + let instance: &mut {{meta.optimizer_name}}Cache = unsafe { + assert!(!instance.is_null()); + &mut *instance + }; + + let u = unsafe { + assert!(!u.is_null()); + std::slice::from_raw_parts_mut(u as *mut f64, {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES) + }; + + let params = unsafe { + assert!(!params.is_null()); + std::slice::from_raw_parts(params as *const f64, {{meta.optimizer_name|upper}}_NUM_PARAMETERS) + }; + + let status = solve(params,&mut instance.cache, u); + + match status { + Ok(status) => {{meta.optimizer_name}}SolverStatus { + exit_status: match status.exit_status() { + core::ExitStatus::Converged => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}Converged, + core::ExitStatus::NotConvergedIterations => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedIterations, + core::ExitStatus::NotConvergedOutOfTime => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedOutOfTime, + }, + num_outer_iterations: status.num_outer_iterations() as c_ulong, + num_inner_iterations: status.num_inner_iterations() as c_ulong, + last_problem_norm_fpr: status.last_problem_norm_fpr(), + max_constraint_violation: status.max_constraint_violation(), + solve_time_ns: status.solve_time().as_nanos() as c_ulonglong, + }, + Err(e) => {{meta.optimizer_name}}SolverStatus { + exit_status: match e { + SolverError::Cost => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedCost, + SolverError::NotFiniteComputation => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedNotFiniteComputation, + }, + num_outer_iterations: std::u64::MAX as c_ulong, + num_inner_iterations: std::u64::MAX as c_ulong, + last_problem_norm_fpr: std::f64::INFINITY, + max_constraint_violation: std::f64::INFINITY, + solve_time_ns: std::u64::MAX as c_ulonglong, + }, + } +} + +/// Deallocate the solver's memory +#[no_mangle] +pub extern "C" fn {{meta.optimizer_name|lower}}_free(instance: *mut {{meta.optimizer_name}}Cache) { + // Add impl + unsafe { + assert!(!instance.is_null()); + Box::from_raw(instance); + } +} +{% endif %} \ No newline at end of file From 6a4fa49c8d037a89b5c7ed050f8025c14feb22f5 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 19:35:36 +0100 Subject: [PATCH 32/42] dummy implementation of TCP server --- .../opengen/builder/optimizer_builder.py | 22 +++++++++---------- .../opengen/templates/icasadi_lib.rs.template | 18 ++++----------- .../opengen/templates/tcp_server.rs.template | 8 +++---- 3 files changed, 18 insertions(+), 30 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index ad0bbea2..9d39f453 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -463,13 +463,15 @@ def __generate_yaml_data_file(self): 'version': metadata.version, 'authors': metadata.authors, 'licence': metadata.licence} - build_details = {'open_version': build_config.open_version, + build_details = {'id': build_config.id, + 'open_version': build_config.open_version, 'build_dir': build_config.build_dir, 'build_mode': build_config.build_mode, 'target_system': build_config.target_system, 'cost_function_name': build_config.cost_function_name, 'grad_function_name': build_config.grad_function_name, - 'constraint_penalty_function_name': build_config.constraint_penalty_function_name + 'mapping_f2': build_config.constraint_penalty_function_name, + 'mapping_f1': build_config.alm_mapping_f1_function_name } solver_details = {'initial_penalty_weights': solver_config.initial_penalty_weights, 'lbfgs_memory': solver_config.lbfgs_memory, @@ -480,10 +482,6 @@ def __generate_yaml_data_file(self): 'max_inner_iterations': solver_config.max_inner_iterations, 'max_duration_micros': solver_config.max_duration_micros } - casadi_functions = {'cost_function_name': solver_config.initial_penalty_weights, - 'cost_gradient_name': solver_config.lbfgs_memory, - 'constraint_penalty_function_name': solver_config.tolerance - } details = {'meta': metadata_details, 'tcp': tcp_details, 'build': build_details, 'solver': solver_details} with open(target_yaml_file_path, 'w') as outfile: @@ -532,10 +530,10 @@ def build(self): if not self.__generate_not_build: logging.info("Building optimizer") self.__build_optimizer() # build overall project - # - # if self.__build_config.tcp_interface_config is not None: - # logging.info("Generating TCP/IP server") - # self.__generate_code_tcp_interface() - # if not self.__generate_not_build: - # self.__build_tcp_iface() + + if self.__build_config.tcp_interface_config is not None: + logging.info("Generating TCP/IP server") + self.__generate_code_tcp_interface() + if not self.__generate_not_build: + self.__build_tcp_iface() diff --git a/open-codegen/opengen/templates/icasadi_lib.rs.template b/open-codegen/opengen/templates/icasadi_lib.rs.template index 3dca1210..8e8a14ec 100644 --- a/open-codegen/opengen/templates/icasadi_lib.rs.template +++ b/open-codegen/opengen/templates/icasadi_lib.rs.template @@ -17,16 +17,16 @@ // #![no_std] /// Number of static parameters (this also includes penalty constraints) -pub const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() or 0 }}; +const NUM_STATIC_PARAMETERS: usize = {{ problem.dim_parameters() or 0 }}; /// Number of decision variables -pub const NUM_DECISION_VARIABLES: usize = {{ problem.dim_decision_variables() }}; +const NUM_DECISION_VARIABLES: usize = {{ problem.dim_decision_variables() }}; /// Number of ALM-type constraints (dimension of F1, i.e., n1) -pub const NUM_CONSTRAINTS_TYPE_ALM: usize = {{ problem.dim_constraints_aug_lagrangian() or 0 }}; +const NUM_CONSTRAINTS_TYPE_ALM: usize = {{ problem.dim_constraints_aug_lagrangian() or 0 }}; /// Number of penalty constraints (dimension of F2, i.e., n2) -pub const NUM_CONSTAINTS_TYPE_PENALTY: usize = {{ problem.dim_constraints_penalty() or 0 }}; +const NUM_CONSTAINTS_TYPE_PENALTY: usize = {{ problem.dim_constraints_penalty() or 0 }}; use libc::{c_double, c_int}; // might need to include: c_longlong, c_void @@ -229,16 +229,6 @@ pub fn mapping_f2( 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; - } - #[test] fn tst_call_cost() { let u = [0.1; NUM_DECISION_VARIABLES]; diff --git a/open-codegen/opengen/templates/tcp_server.rs.template b/open-codegen/opengen/templates/tcp_server.rs.template index da8dc5a0..c5dc1a99 100644 --- a/open-codegen/opengen/templates/tcp_server.rs.template +++ b/open-codegen/opengen/templates/tcp_server.rs.template @@ -2,7 +2,7 @@ /// Auto-generated TCP server for optimizer: {{ meta.optimizer_name }} /// Generated at: {{timestamp_created}} /// -use optimization_engine::continuation::{HomotopyCache, HomotopySolverStatus}; +use optimization_engine::alm::*; use serde::{Deserialize, Serialize}; use std::{ @@ -81,7 +81,7 @@ fn write_error_message(stream: &mut std::net::TcpStream, code: i32, error_msg: & /// Serializes the solution and solution status and returns it /// to the client fn return_solution_to_client( - status: HomotopySolverStatus, + status: AlmOptimizerStatus, solution: &[f64], stream: &mut std::net::TcpStream, ) { @@ -90,7 +90,7 @@ fn return_solution_to_client( num_outer_iterations: status.num_outer_iterations(), num_inner_iterations: status.num_inner_iterations(), last_problem_norm_fpr: status.last_problem_norm_fpr(), - max_constraint_violation: status.max_constraint_violation(), + max_constraint_violation: -1.0, solve_time_ms: (status.solve_time().as_nanos() as f64) / 1e6, solution: solution, }; @@ -102,7 +102,7 @@ fn return_solution_to_client( /// Handles an execution request fn execution_handler( - cache: &mut HomotopyCache, + cache: &mut AlmCache, execution_parameter: &ExecutionParameter, u: &mut [f64], p: &mut [f64], From 230d93b22606ac0f680888df454f29de41db4c0f Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Thu, 26 Sep 2019 19:55:38 +0100 Subject: [PATCH 33/42] fixed bug with returned Lagrange multipliers --- src/alm/alm_optimizer_status.rs | 2 +- src/alm/tests.rs | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/alm/alm_optimizer_status.rs b/src/alm/alm_optimizer_status.rs index fa45f993..bff1303c 100644 --- a/src/alm/alm_optimizer_status.rs +++ b/src/alm/alm_optimizer_status.rs @@ -120,7 +120,7 @@ impl AlmOptimizerStatus { /// Lagrange multipliers of correct length /// pub(crate) fn with_lagrange_multipliers(mut self, lagrange_multipliers: &[f64]) -> Self { - self.lagrange_multipliers = Some(vec![0.0]); + self.lagrange_multipliers = Some(vec![]); if let Some(y) = &mut self.lagrange_multipliers { y.extend_from_slice(&lagrange_multipliers); } diff --git a/src/alm/tests.rs b/src/alm/tests.rs index 44758218..e2aa6886 100644 --- a/src/alm/tests.rs +++ b/src/alm/tests.rs @@ -301,6 +301,8 @@ fn t_alm_numeric_test_2() { assert!(mapping_f2(&u, &mut f2u).is_ok()); assert!(crate::matrix_operations::norm2(&f2u) < 1e-4); println!("F2(u*) = {:#?}", &f2u); + + println!("y = {:#?}", r.lagrange_multipliers()); } fn mockito_f0(_u: &[f64], _xi: &[f64], _p: &[f64], cost: &mut f64) -> Result<(), SolverError> { @@ -364,9 +366,7 @@ fn t_alm_numeric_test_repeat() { let mut u = vec![0.0; nx]; let solver_result = alm_optimizer.solve(&mut u); assert!(solver_result.is_ok()); - assert_eq!( - ExitStatus::Converged, - solver_result.unwrap().exit_status()); + assert_eq!(ExitStatus::Converged, solver_result.unwrap().exit_status()); } } From e39563fe75eddd074b74de4615062d264e99e595 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 11:42:41 +0100 Subject: [PATCH 34/42] passing parameters of constraints (Ball2) --- .../opengen/templates/optimizer.rs.template | 37 ++++++++++++------- .../opengen/templates/tcp_server.rs.template | 6 +++ 2 files changed, 30 insertions(+), 13 deletions(-) diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index ce8eed64..6ef43815 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -52,10 +52,21 @@ pub const {{meta.optimizer_name|upper}}_N2: usize = {{problem.dim_constraints_pe {% include "optimizer_cinterface.rs.template" %} +// ---Parameters of the constraints---------------------------------------------------------------------- + +{% if 'Ball2' == problem.constraints.__class__.__name__ -%} +/// Constraints: Centre of Euclidean Ball +const CONSTRAINTS_BALL2_XC: Option<&[f64]> = {% if problem.constraints.center is not none %}Some(&[{{problem.constraints.center | join(', ')}}]){% else %}None{% endif %}; + +/// Constraints: Radius of Euclidean Ball +const CONSTRAINTS_BALL2_RADIUS : f64 = {{problem.constraints.radius}}; +{%- endif %} + + // ---Internal private helper functions------------------------------------------------------------------ fn make_constraints() -> impl Constraint { - let bounds = Ball2::new(None, 10.0); + let bounds = Ball2::new(CONSTRAINTS_BALL2_XC, CONSTRAINTS_BALL2_RADIUS); bounds } @@ -87,34 +98,34 @@ pub fn solve( icasadi::cost(&u, &xi, &p, cost); Ok(()) }; - let grad_psi = |u: &[f64], xi: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { icasadi::grad(&u, &xi, &p, grad); Ok(()) }; - + {% if problem.dim_constraints_aug_lagrangian() > 0 %} let f1 = |u: &[f64], res: &mut [f64]| -> Result<(), SolverError> { icasadi::mapping_f1(&u, &p, res); Ok(()) - }; - - let f2 = |u: &[f64], res: &mut [f64]| -> Result<(), SolverError> { + };{% endif %} + {% if problem.dim_constraints_penalty() %}let f2 = |u: &[f64], res: &mut [f64]| -> Result<(), SolverError> { icasadi::mapping_f2(&u, &p, res); Ok(()) - }; - + };{% endif -%} let bounds = make_constraints(); + + {% if problem.dim_constraints_aug_lagrangian() > 0 -%} let set_y = Ball2::new(None, 10000.0); let set_c = Ball2::new(None, 1.0); + {% endif -%} let alm_problem = AlmProblem::new( bounds, - Some(set_c), - Some(set_y), + {% if problem.dim_constraints_aug_lagrangian() > 0 %}Some(set_c){% else %}NO_SET{% endif %}, + {% if problem.dim_constraints_aug_lagrangian() > 0 %}Some(set_y){% else %}NO_SET{% endif %}, psi, grad_psi, - Some(f1), - Some(f2), + {% if problem.dim_constraints_aug_lagrangian() > 0 %}Some(f1){% else %}NO_MAPPING{% endif %}, + {% if problem.dim_constraints_penalty() %}Some(f2){% else %}NO_MAPPING{% endif %}, {{meta.optimizer_name|upper}}_N1, {{meta.optimizer_name|upper}}_N2, ); @@ -122,7 +133,7 @@ pub fn solve( let mut alm_optimizer = AlmOptimizer::new(alm_cache, alm_problem) .with_delta_tolerance(DELTA_TOLERANCE) .with_epsilon_tolerance(EPSILON_TOLERANCE) - .with_initial_inner_tolerance(1e-2) + .with_initial_inner_tolerance(EPSILON_TOLERANCE) .with_max_duration(std::time::Duration::from_micros(MAX_DURATION_MICROS)) .with_max_outer_iterations(MAX_OUTER_ITERATIONS) .with_max_inner_iterations(MAX_INNER_ITERATIONS); diff --git a/open-codegen/opengen/templates/tcp_server.rs.template b/open-codegen/opengen/templates/tcp_server.rs.template index c5dc1a99..3ea392b3 100644 --- a/open-codegen/opengen/templates/tcp_server.rs.template +++ b/open-codegen/opengen/templates/tcp_server.rs.template @@ -52,7 +52,9 @@ struct OptimizerSolution<'a> { last_problem_norm_fpr: f64, max_constraint_violation: f64, solve_time_ms: f64, + penalty: f64, solution: &'a [f64], + lagrange_multipliers: &'a [f64], } fn pong(stream: &mut std::net::TcpStream, code: i32) { @@ -85,14 +87,18 @@ fn return_solution_to_client( solution: &[f64], stream: &mut std::net::TcpStream, ) { + let empty_vec = [0.0]; let solution: OptimizerSolution = OptimizerSolution { exit_status: format!("{:?}", status.exit_status()), num_outer_iterations: status.num_outer_iterations(), num_inner_iterations: status.num_inner_iterations(), last_problem_norm_fpr: status.last_problem_norm_fpr(), + penalty: status.penalty(), + lagrange_multipliers: if let Some(y) = &status.lagrange_multipliers() { y } else { &empty_vec }, max_constraint_violation: -1.0, solve_time_ms: (status.solve_time().as_nanos() as f64) / 1e6, solution: solution, + }; let solution_json = serde_json::to_string_pretty(&solution).unwrap(); stream From 5a3601d596eec16c60f7cf30e9bfaf846a2c58ad Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 11:59:20 +0100 Subject: [PATCH 35/42] Sets U and C are passed on to template --- .../opengen/templates/optimizer.rs.template | 54 ++++++++++++++++--- 1 file changed, 48 insertions(+), 6 deletions(-) diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index 6ef43815..ffffff05 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -54,22 +54,63 @@ pub const {{meta.optimizer_name|upper}}_N2: usize = {{problem.dim_constraints_pe // ---Parameters of the constraints---------------------------------------------------------------------- -{% if 'Ball2' == problem.constraints.__class__.__name__ -%} +{% if 'Ball2' == problem.constraints.__class__.__name__ or 'BallInf' == problem.constraints.__class__.__name__ -%} /// Constraints: Centre of Euclidean Ball -const CONSTRAINTS_BALL2_XC: Option<&[f64]> = {% if problem.constraints.center is not none %}Some(&[{{problem.constraints.center | join(', ')}}]){% else %}None{% endif %}; +const CONSTRAINTS_BALL_XC: Option<&[f64]> = {% if problem.constraints.center is not none %}Some(&[{{problem.constraints.center | join(', ')}}]){% else %}None{% endif %}; +/// Constraints: Radius of Euclidean Ball +const CONSTRAINTS_BALL_RADIUS : f64 = {{problem.constraints.radius}}; +{% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} +const CONSTRAINTS_XMIN :Option<&[f64]> = {% if problem.constraints.xmin is not none %}Some(&[{{problem.constraints.xmin|join(', ')}}]){% else %}None{% endif %}; +const CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.constraints.xmax is not none %}Some(&[{{problem.constraints.xmax|join(', ')}}]){% else %}None{% endif %}; +{% endif %} + + +// ---Parameters of ALM-type constraints (Set C)--------------------------------------------------------- + +{% if problem.alm_set_c is not none %} +{% if 'Ball2' == problem.alm_set_c.__class__.__name__ or 'BallInf' == problem.alm_set_c.__class__.__name__ -%} +/// Constraints: Centre of Euclidean Ball +const SET_C_BALL_XC: Option<&[f64]> = {% if problem.alm_set_c.center is not none %}Some(&[{{problem.alm_set_c.center | join(', ')}}]){% else %}None{% endif %}; /// Constraints: Radius of Euclidean Ball -const CONSTRAINTS_BALL2_RADIUS : f64 = {{problem.constraints.radius}}; -{%- endif %} +const SET_C_CONSTRAINTS_BALL_RADIUS : f64 = {{problem.alm_set_c.radius}}; +{% elif 'Rectangle' == problem.alm_set_c.__class__.__name__ -%} +const SET_C_CONSTRAINTS_XMIN :Option<&[f64]> = {% if problem.alm_set_c.xmin is not none %}Some(&[{{problem.alm_set_c.xmin|join(', ')}}]){% else %}None{% endif %}; +const SET_C_CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.alm_set_c.xmax is not none %}Some(&[{{problem.alm_set_c.xmax|join(', ')}}]){% else %}None{% endif %}; +{% endif %} +{% endif %} // ---Internal private helper functions------------------------------------------------------------------ fn make_constraints() -> impl Constraint { - let bounds = Ball2::new(CONSTRAINTS_BALL2_XC, CONSTRAINTS_BALL2_RADIUS); + {% if 'Ball2' == problem.constraints.__class__.__name__ -%} + let bounds = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); + {% elif 'BallInf' == problem.constraints.__class__.__name__ -%} + let bounds = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); + {% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} + let bounds = Rectangle::new(CONSTRAINTS_XMIN, CONSTRAINTS_XMAX); + {% elif 'NoConstraints' == problem.constraints.__class__.__name__ -%} + let bounds = NoConstraints::new(); + {% endif -%} bounds } +{% if problem.alm_set_c is not none %} +fn make_set_c() -> impl Constraint { + {% if 'Ball2' == problem.constraints.__class__.__name__ -%} + let set_c = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); + {% elif 'BallInf' == problem.constraints.__class__.__name__ -%} + let set_c = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); + {% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} + let set_c = Rectangle::new(CONSTRAINTS_XMIN, CONSTRAINTS_XMAX); + {% elif 'NoConstraints' == problem.constraints.__class__.__name__ -%} + let set_c = NoConstraints::new(); + {% endif -%} + set_c +} +{% endif %} + // ---Main public API functions-------------------------------------------------------------------------- @@ -85,6 +126,7 @@ pub fn initialize_solver() -> AlmCache { } +/// Solver interface pub fn solve( p: &[f64], alm_cache: &mut AlmCache, @@ -115,7 +157,7 @@ pub fn solve( {% if problem.dim_constraints_aug_lagrangian() > 0 -%} let set_y = Ball2::new(None, 10000.0); - let set_c = Ball2::new(None, 1.0); + let set_c = make_set_c(); {% endif -%} let alm_problem = AlmProblem::new( From 909dd16fdaf1058ae75c939fe80bbf417db62110 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 12:30:18 +0100 Subject: [PATCH 36/42] Support for set Y (ALM method) --- .../opengen/templates/optimizer.rs.template | 55 ++++++++++++++----- 1 file changed, 42 insertions(+), 13 deletions(-) diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index ffffff05..936233f4 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -65,7 +65,6 @@ const CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.constraints.xmax is not n {% endif %} - // ---Parameters of ALM-type constraints (Set C)--------------------------------------------------------- {% if problem.alm_set_c is not none %} @@ -73,10 +72,25 @@ const CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.constraints.xmax is not n /// Constraints: Centre of Euclidean Ball const SET_C_BALL_XC: Option<&[f64]> = {% if problem.alm_set_c.center is not none %}Some(&[{{problem.alm_set_c.center | join(', ')}}]){% else %}None{% endif %}; /// Constraints: Radius of Euclidean Ball -const SET_C_CONSTRAINTS_BALL_RADIUS : f64 = {{problem.alm_set_c.radius}}; +const SET_C_BALL_RADIUS : f64 = {{problem.alm_set_c.radius}}; {% elif 'Rectangle' == problem.alm_set_c.__class__.__name__ -%} -const SET_C_CONSTRAINTS_XMIN :Option<&[f64]> = {% if problem.alm_set_c.xmin is not none %}Some(&[{{problem.alm_set_c.xmin|join(', ')}}]){% else %}None{% endif %}; -const SET_C_CONSTRAINTS_XMAX :Option<&[f64]> = {% if problem.alm_set_c.xmax is not none %}Some(&[{{problem.alm_set_c.xmax|join(', ')}}]){% else %}None{% endif %}; +const SET_C_XMIN :Option<&[f64]> = {% if problem.alm_set_c.xmin is not none %}Some(&[{{problem.alm_set_c.xmin|join(', ')}}]){% else %}None{% endif %}; +const SET_C_XMAX :Option<&[f64]> = {% if problem.alm_set_c.xmax is not none %}Some(&[{{problem.alm_set_c.xmax|join(', ')}}]){% else %}None{% endif %}; +{% endif %} +{% endif %} + + +// ---Parameters of ALM-type constraints (Set Y)--------------------------------------------------------- + +{% if problem.alm_set_y is not none %} +{% if 'Ball2' == problem.alm_set_y.__class__.__name__ or 'BallInf' == problem.alm_set_y.__class__.__name__ -%} +/// Constraints: Centre of Euclidean Ball +const SET_Y_BALL_XC: Option<&[f64]> = {% if problem.alm_set_y.center is not none %}Some(&[{{problem.alm_set_y.center | join(', ')}}]){% else %}None{% endif %}; +/// Constraints: Radius of Euclidean Ball +const SET_Y_BALL_RADIUS : f64 = {{problem.alm_set_y.radius}}; +{% elif 'Rectangle' == problem.alm_set_y.__class__.__name__ -%} +const SET_Y_XMIN :Option<&[f64]> = {% if problem.alm_set_y.xmin is not none %}Some(&[{{problem.alm_set_y.xmin|join(', ')}}]){% else %}None{% endif %}; +const SET_Y_XMAX :Option<&[f64]> = {% if problem.alm_set_y.xmax is not none %}Some(&[{{problem.alm_set_y.xmax|join(', ')}}]){% else %}None{% endif %}; {% endif %} {% endif %} @@ -87,7 +101,7 @@ fn make_constraints() -> impl Constraint { {% if 'Ball2' == problem.constraints.__class__.__name__ -%} let bounds = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); {% elif 'BallInf' == problem.constraints.__class__.__name__ -%} - let bounds = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); + // not implemented yet {% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} let bounds = Rectangle::new(CONSTRAINTS_XMIN, CONSTRAINTS_XMAX); {% elif 'NoConstraints' == problem.constraints.__class__.__name__ -%} @@ -98,19 +112,34 @@ fn make_constraints() -> impl Constraint { {% if problem.alm_set_c is not none %} fn make_set_c() -> impl Constraint { - {% if 'Ball2' == problem.constraints.__class__.__name__ -%} - let set_c = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); - {% elif 'BallInf' == problem.constraints.__class__.__name__ -%} - let set_c = Ball2::new(CONSTRAINTS_BALL_XC, CONSTRAINTS_BALL_RADIUS); - {% elif 'Rectangle' == problem.constraints.__class__.__name__ -%} - let set_c = Rectangle::new(CONSTRAINTS_XMIN, CONSTRAINTS_XMAX); - {% elif 'NoConstraints' == problem.constraints.__class__.__name__ -%} + {% if 'Ball2' == problem.alm_set_c.__class__.__name__ -%} + let set_c = Ball2::new(SET_C_BALL_XC, SET_C_BALL_RADIUS); + {% elif 'BallInf' == problem.alm_set_c.__class__.__name__ -%} + // not implemented yet + {% elif 'Rectangle' == problem.alm_set_c.__class__.__name__ -%} + let set_c = Rectangle::new(SET_C_XMIN, SET_C_XMAX); + {% elif 'NoConstraints' == problem.alm_set_c.__class__.__name__ -%} let set_c = NoConstraints::new(); {% endif -%} set_c } {% endif %} +{% if problem.alm_set_y is not none %} +fn make_set_y() -> impl Constraint { + {% if 'Ball2' == problem.alm_set_y.__class__.__name__ -%} + let set_y = Ball2::new(SET_Y_BALL_XC, SET_Y_BALL_RADIUS); + {% elif 'BallInf' == problem.alm_set_y.__class__.__name__ -%} + /* not implemented yet */ + {% elif 'Rectangle' == problem.alm_set_y.__class__.__name__ -%} + let set_y = Rectangle::new(SET_Y_XMIN, SET_Y_XMAX); + {% elif 'NoConstraints' == problem.alm_set_y.__class__.__name__ -%} + let set_y = NoConstraints::new(); + {% endif -%} + set_y +} +{% endif %} + // ---Main public API functions-------------------------------------------------------------------------- @@ -156,7 +185,7 @@ pub fn solve( let bounds = make_constraints(); {% if problem.dim_constraints_aug_lagrangian() > 0 -%} - let set_y = Ball2::new(None, 10000.0); + let set_y = make_set_y(); let set_c = make_set_c(); {% endif -%} From c5c6cf2664e7d6971b97d89bc50e89c14ee2ea5d Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 14:59:18 +0100 Subject: [PATCH 37/42] passing all solver options from Python to Rust --- .../opengen/builder/optimizer_builder.py | 14 +---- open-codegen/opengen/config/solver_config.py | 61 +++++++++++++------ .../opengen/templates/optimizer.rs.template | 18 +++++- .../templates/optimizer_cargo.toml.template | 3 +- .../opengen/templates/tcp_server.rs.template | 2 - .../templates/tcp_server_cargo.toml.template | 3 +- 6 files changed, 63 insertions(+), 38 deletions(-) diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 9d39f453..a91c16f4 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -401,20 +401,9 @@ def __build_tcp_iface(self): def __initialize(self): logging.info("Initialising builder") - sc = self.__solver_config - pr = self.__problem - ncp = pr.dim_constraints_penalty() - if ncp > 0 and sc.initial_penalty_weights is None: - # set default initial values - self.__solver_config.with_initial_penalty_weights([1] * int(ncp)) def __check_user_provided_parameters(self): logging.info("Checking user parameters") - sc = self.__solver_config - pr = self.__problem - ncp = pr.dim_constraints_penalty() - if 1 != len(sc.initial_penalty_weights) != ncp > 0: - raise Exception("Initial penalty weights have incompatible dimensions with c(u, p)") def __generate_code_tcp_interface(self): logging.info("Generating code for TCP/IP interface (tcp_iface/src/main.rs)") @@ -473,8 +462,7 @@ def __generate_yaml_data_file(self): 'mapping_f2': build_config.constraint_penalty_function_name, 'mapping_f1': build_config.alm_mapping_f1_function_name } - solver_details = {'initial_penalty_weights': solver_config.initial_penalty_weights, - 'lbfgs_memory': solver_config.lbfgs_memory, + solver_details = {'lbfgs_memory': solver_config.lbfgs_memory, 'tolerance': solver_config.tolerance, 'constraints_tolerance': solver_config.constraints_tolerance, 'penalty_weight_update_factor': solver_config.penalty_weight_update_factor, diff --git a/open-codegen/opengen/config/solver_config.py b/open-codegen/opengen/config/solver_config.py index 4591cc25..ba081d1e 100644 --- a/open-codegen/opengen/config/solver_config.py +++ b/open-codegen/opengen/config/solver_config.py @@ -11,18 +11,26 @@ def __init__(self): """ self.__tolerance = 1e-4 + self.__initial_tolerance = 1e-4 self.__lbfgs_memory = 10 self.__max_inner_iterations = 500 self.__max_outer_iterations = 10 self.__constraints_tolerance = 1e-4 + self.__initial_penalty = 1.0 self.__penalty_weight_update_factor = 5.0 - self.__initial_weights = 10.0 self.__max_duration_micros = 5000000 + self.__inner_tolerance_update_factor = 0.1 self.__cbfgs_alpha = None self.__cbfgs_epsilon = None self.__cbfgs_sy_epsilon = None # --------- GETTERS ----------------------------- + + @property + def initial_penalty(self): + """Initial penalty""" + return self.__initial_penalty + @property def cbfgs_alpha(self): return self.__cbfgs_alpha @@ -40,6 +48,16 @@ def tolerance(self): """Tolerance of inner solver""" return self.__tolerance + @property + def initial_tolerance(self): + """Initial tolerance of inner solver""" + return self.__initial_tolerance + + @property + def inner_tolerance_update_factor(self): + """"Update factor for inner tolerance""" + return self.__inner_tolerance_update_factor + @property def lbfgs_memory(self): """LBFGS memory for the inner solver""" @@ -65,14 +83,6 @@ def penalty_weight_update_factor(self): """Multiplicative factor for the update of the penalty weights""" return self.__penalty_weight_update_factor - @property - def initial_penalty_weights(self): - """Initial penalty weights""" - if isinstance(self.__initial_weights, list): - return self.__initial_weights - else: - return [self.__initial_weights] - @property def max_duration_micros(self): """Maximum execution time in microseconds @@ -83,6 +93,15 @@ def max_duration_micros(self): return self.__max_duration_micros # --------- SETTERS ----------------------------- + + def with_initial_penalty(self, initial_penalty): + """Initial penalty""" + if initial_penalty <= 0: + raise Exception("Initial penalty must be >0") + + self.__initial_penalty = float(initial_penalty) + return self + def with_tolerance(self, tolerance): """Specify tolerance @@ -97,6 +116,19 @@ def with_tolerance(self, tolerance): self.__tolerance = float(tolerance) return self + def with_initial_tolerance(self, initial_tolerance): + if initial_tolerance <= 0: + raise Exception("The initial tolerance must be >0") + self.__initial_tolerance = float(initial_tolerance) + return self + + def with_inner_tolerance_update_factor(self, inner_tol_update_factor): + if inner_tol_update_factor <= 0 or inner_tol_update_factor > 1: + raise Exception("The tolerance update factor must be in [0, 1)") + + self.__inner_tolerance_update_factor = float(inner_tol_update_factor) + return self + def with_lfbgs_memory(self, lbfgs_memory): """Specify L-BFGS memory @@ -123,7 +155,7 @@ def with_max_inner_iterations(self, max_iters): self.__max_inner_iterations = int(max_iters) return self - def with_constraints_tolerance(self, constraints_tolerance): + def with_delta_tolerance(self, constraints_tolerance): """Tolerance on constraint violation Returns: @@ -165,15 +197,6 @@ def with_penalty_weight_update_factor(self, penalty_weight_update_factor): self.__penalty_weight_update_factor = float(penalty_weight_update_factor) return self - def with_initial_penalty_weights(self, initial_weights): - """Specify the initial penalty weights - - Returns: - The current object - """ - self.__initial_weights = initial_weights - return self - def with_max_duration_micros(self, max_duration_micros): """Specify the maximum duration in microseconds (must be an integer) diff --git a/open-codegen/opengen/templates/optimizer.rs.template b/open-codegen/opengen/templates/optimizer.rs.template index 936233f4..c51494de 100644 --- a/open-codegen/opengen/templates/optimizer.rs.template +++ b/open-codegen/opengen/templates/optimizer.rs.template @@ -17,6 +17,12 @@ use optimization_engine::{constraints::*, panoc::*, alm::*, *}; /// Tolerance of inner solver const EPSILON_TOLERANCE: f64 = {{solver_config.tolerance or 0.0001}}; +/// Initial tolerance +const INITIAL_EPSILON_TOLERANCE: f64 = {{solver_config.initial_tolerance or 0.0001}}; + +/// Update factor for inner tolerance +const EPSILON_TOLERANCE_UPDATE_FACTOR: f64 = {{solver_config.inner_tolerance_update_factor or 0.1}}; + /// Delta tolerance const DELTA_TOLERANCE: f64 = {{solver_config.constraints_tolerance or 0.0001}}; @@ -32,6 +38,11 @@ const MAX_OUTER_ITERATIONS: usize = {{solver_config.max_outer_iterations or 10}} /// Maximum execution duration in microseconds const MAX_DURATION_MICROS: u64 = {{solver_config.max_duration_micros}}; +/// Penalty update factor +const PENALTY_UPDATE_FACTOR: f64 = {{solver_config.penalty_weight_update_factor or 10.0}}; + +/// Initial penalty +const INITIAL_PENALTY_PARAMETER: f64 = {{solver_config.initial_penalty or 1.0}}; // ---Public Constants----------------------------------------------------------------------------------- @@ -204,10 +215,13 @@ pub fn solve( let mut alm_optimizer = AlmOptimizer::new(alm_cache, alm_problem) .with_delta_tolerance(DELTA_TOLERANCE) .with_epsilon_tolerance(EPSILON_TOLERANCE) - .with_initial_inner_tolerance(EPSILON_TOLERANCE) + .with_initial_inner_tolerance(INITIAL_EPSILON_TOLERANCE) + .with_inner_tolerance_update_factor(EPSILON_TOLERANCE_UPDATE_FACTOR) .with_max_duration(std::time::Duration::from_micros(MAX_DURATION_MICROS)) .with_max_outer_iterations(MAX_OUTER_ITERATIONS) - .with_max_inner_iterations(MAX_INNER_ITERATIONS); + .with_max_inner_iterations(MAX_INNER_ITERATIONS) + .with_initial_penalty(INITIAL_PENALTY_PARAMETER) + .with_penalty_update_factor(PENALTY_UPDATE_FACTOR); // solve the problem using `u` an the initial condition // return the problem status (instance of `AlmOptimizerStatus`) diff --git a/open-codegen/opengen/templates/optimizer_cargo.toml.template b/open-codegen/opengen/templates/optimizer_cargo.toml.template index 2d65bdb9..013c9e0d 100644 --- a/open-codegen/opengen/templates/optimizer_cargo.toml.template +++ b/open-codegen/opengen/templates/optimizer_cargo.toml.template @@ -18,7 +18,8 @@ publish=false [dependencies] -optimization_engine = "{{open_version or '0.6.1-alpha.1'}}" +## optimization_engine = "{{open_version or '0.6.1-alpha.1'}}" +optimization_engine = {path = "../../../../"} icasadi = {path = "./icasadi/"} {% if activate_clib_generation -%} libc = "0.2.0" diff --git a/open-codegen/opengen/templates/tcp_server.rs.template b/open-codegen/opengen/templates/tcp_server.rs.template index 3ea392b3..7aab7534 100644 --- a/open-codegen/opengen/templates/tcp_server.rs.template +++ b/open-codegen/opengen/templates/tcp_server.rs.template @@ -50,7 +50,6 @@ struct OptimizerSolution<'a> { num_outer_iterations: usize, num_inner_iterations: usize, last_problem_norm_fpr: f64, - max_constraint_violation: f64, solve_time_ms: f64, penalty: f64, solution: &'a [f64], @@ -95,7 +94,6 @@ fn return_solution_to_client( last_problem_norm_fpr: status.last_problem_norm_fpr(), penalty: status.penalty(), lagrange_multipliers: if let Some(y) = &status.lagrange_multipliers() { y } else { &empty_vec }, - max_constraint_violation: -1.0, solve_time_ms: (status.solve_time().as_nanos() as f64) / 1e6, solution: solution, diff --git a/open-codegen/opengen/templates/tcp_server_cargo.toml.template b/open-codegen/opengen/templates/tcp_server_cargo.toml.template index 49adb41e..61d1a43e 100644 --- a/open-codegen/opengen/templates/tcp_server_cargo.toml.template +++ b/open-codegen/opengen/templates/tcp_server_cargo.toml.template @@ -21,7 +21,8 @@ publish=false [dependencies] -optimization_engine = "0.6.1-alpha.1" +# optimization_engine = "0.6.1-alpha.1" +optimization_engine = {path = "../../../../../"} serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" pretty_env_logger = "0.3.0" From 59075d3c35d0e4dd8b3d30f57db36951359da785 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 15:00:15 +0100 Subject: [PATCH 38/42] updated Python test renamed function to .with_delta_tolerance() --- open-codegen/opengen/test/test.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/open-codegen/opengen/test/test.py b/open-codegen/opengen/test/test.py index 20269938..58fd3e71 100644 --- a/open-codegen/opengen/test/test.py +++ b/open-codegen/opengen/test/test.py @@ -46,7 +46,7 @@ def test_solver_config_wrong_inner_iterations(self): def test_solver_config_wrong_constraints_tolerance(self): with self.assertRaises(Exception) as __context: - og.config.SolverConfiguration().with_constraints_tolerance(0) + og.config.SolverConfiguration().with_delta_tolerance(0) def test_solver_config_wrong_inner_tolerance(self): with self.assertRaises(Exception) as __context: @@ -151,7 +151,7 @@ def test_fully_featured_release_mode(self): .with_lfbgs_memory(15) \ .with_tolerance(1e-5) \ .with_max_inner_iterations(155) \ - .with_constraints_tolerance(1e-4) \ + .with_delta_tolerance(1e-4) \ .with_max_outer_iterations(15) \ .with_penalty_weight_update_factor(8.0) \ .with_initial_penalty_weights([20.0, 5.0]).with_cbfgs_parameters(1.0, 1e-8, 1e-9) @@ -192,7 +192,7 @@ def test_link_2_c_libs(self): .with_lfbgs_memory(15) \ .with_tolerance(1e-5) \ .with_max_inner_iterations(155) \ - .with_constraints_tolerance(1e-4) \ + .with_delta_tolerance(1e-4) \ .with_max_outer_iterations(15) \ .with_penalty_weight_update_factor(8.0) \ .with_initial_penalty_weights([20.0, 5.0]) From 765ad3c2b30b630a96a91243572a877a892e0584 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 15:01:01 +0100 Subject: [PATCH 39/42] ALM rust implementation: fixed termination issue --- src/alm/alm_factory.rs | 4 ++-- src/alm/alm_optimizer.rs | 17 ++++++++------- src/alm/alm_problem.rs | 46 ++++++++++++++++++++++++++++------------ src/alm/mod.rs | 12 +++++------ 4 files changed, 50 insertions(+), 29 deletions(-) diff --git a/src/alm/alm_factory.rs b/src/alm/alm_factory.rs index 82ad1ad3..bebb51a7 100644 --- a/src/alm/alm_factory.rs +++ b/src/alm/alm_factory.rs @@ -7,8 +7,8 @@ use crate::{constraints::Constraint, matrix_operations, SolverError}; -/// Prepares function `psi` and its gradient given the problem data: `f`, `df`, -/// and optionally `F1`, `JF1`, `C` and `F2` +/// Prepares function $\psi$ and its gradient given the problem data: $f$, $\nabla{}f$, +/// and optionally $F_1$, $JF_1$, $C$ and $F_2$ /// /// ## Types /// diff --git a/src/alm/alm_optimizer.rs b/src/alm/alm_optimizer.rs index 86bd29ed..6236c694 100644 --- a/src/alm/alm_optimizer.rs +++ b/src/alm/alm_optimizer.rs @@ -35,9 +35,8 @@ const SMALL_EPSILON: f64 = std::f64::EPSILON; /// function, $U$ is a (not necessarily convex) closed subset of $\mathbb{R}^{n_u}$ /// on which we can easily compute projections (e.g., a rectangle, a ball, a second-order cone, /// a finite set, etc), $F_1:\mathbb{R}^{n_u}\to\mathbb{R}^{n_1}$ and $F_2:\mathbb{R}^{n_u} -/// \to\mathbb{R}^{n_2}$ are mappings with smooth partial derivatives, and -/// $C\subseteq\mathbb{R}^{n_1}$ is a convex closed set on which we can easily compute -/// projections. +/// \to\mathbb{R}^{n_2}$ are mappings with smooth partial derivatives, and $C\subseteq\mathbb{R}^{n_1}$ +/// is a convex closed set on which we can easily compute projections. /// /// /// ## Theoretical solution guarantees @@ -46,11 +45,11 @@ const SMALL_EPSILON: f64 = std::f64::EPSILON; /// /// $$ /// \begin{aligned} -/// v {}\in{}& \partial_u L(u^\star, y^\star), \text{ with } \|v\| \leq \epsilon, +/// v {}\in{}& \partial_u L(u^\star, y^\star), \text{ with } \Vert v \Vert \leq \epsilon, /// \\\\ -/// w {}\in{}& \partial_y [-L](u^\star, y^\star), \text{ with } \|w\| \leq \delta, +/// w {}\in{}& \partial_y [-L](u^\star, y^\star), \text{ with } \Vert w \Vert \leq \delta, /// \\\\ -/// \|F_2(u^\star)\| {}\leq{}& \delta +/// \Vert F_2(u^\star) \Vert {}\leq{}& \delta /// \end{aligned} /// $$ /// @@ -695,7 +694,8 @@ where let criterion_1 = problem.n1 == 0 || if let Some(xi) = &cache.xi { let c = xi[0]; - cache.delta_y_norm_plus <= c * self.delta_tolerance + SMALL_EPSILON + cache.iteration > 0 + && cache.delta_y_norm_plus <= c * self.delta_tolerance + SMALL_EPSILON } else { true }; @@ -1279,12 +1279,13 @@ mod tests { let (tolerance, nx, n1, n2, lbfgs_mem) = (1e-6, 5, 2, 2, 3); let panoc_cache = PANOCCache::new(nx, tolerance, lbfgs_mem); let mut alm_cache = AlmCache::new(panoc_cache, n1, n2); + alm_cache.iteration = 2; let alm_problem = make_dummy_alm_problem(n1, n2); let alm_optimizer = AlmOptimizer::new(&mut alm_cache, alm_problem).with_delta_tolerance(1e-3); // should not exit yet... - assert!(!alm_optimizer.is_exit_criterion_satisfied()); + assert!(!alm_optimizer.is_exit_criterion_satisfied(), "exists right away"); let mut alm_optimizer = alm_optimizer .with_initial_inner_tolerance(1e-3) diff --git a/src/alm/alm_problem.rs b/src/alm/alm_problem.rs index e3a62a1f..8399f020 100644 --- a/src/alm/alm_problem.rs +++ b/src/alm/alm_problem.rs @@ -1,6 +1,25 @@ use crate::{constraints::Constraint, SolverError}; -/// Definition of optimization problem to be solved with `AlmOptimizer` +/// Definition of optimization problem to be solved with `AlmOptimizer`. The optimization +/// problem has the general form +/// +/// $$\begin{aligned} +/// \mathrm{Minimize}\ f(u) +/// \\\\ +/// u \in U +/// \\\\ +/// F_1(u) \in C +/// \\\\ +/// F_2(u) = 0 +/// \end{aligned}$$ +/// +/// where $u\in\mathbb{R}^{n_u}$, $f:\mathbb{R}^n\to\mathbb{R}$ is a $C^{1,1}$-smooth cost +/// function, $U$ is a (not necessarily convex) closed subset of $\mathbb{R}^{n_u}$ +/// on which we can easily compute projections (e.g., a rectangle, a ball, a second-order cone, +/// a finite set, etc), $F_1:\mathbb{R}^{n_u}\to\mathbb{R}^{n_1}$ and $F_2:\mathbb{R}^{n_u} +/// \to\mathbb{R}^{n_2}$ are mappings with smooth partial derivatives, and $C\subseteq\mathbb{R}^{n_1}$ +/// is a convex closed set on which we can easily compute projections. +/// pub struct AlmProblem< MappingAlm, MappingPm, @@ -77,15 +96,16 @@ where /// /// ## Arguments /// - /// - `constraints`: hard constraints - /// - `alm_set_c`: Set `C` of ALM-specific constraints - /// - `alm_set_y`: Compact, convex set `Y` of Lagrange multipliers - /// - `parametric_cost`: Parametric cost function, `f(x; p)` - /// - `parametric_gradient`: Gradient of cost function wrt `x`, that is `df(x; p)` - /// - `mapping_f1`: Mapping `F1` of ALM-specific constraints (`F1(x; p) in C`) - /// - `mapping_f2`: Mapping `F2` of PM-specific constraints (`F2(x; p) = 0`) - /// - `n1`: range dimension of `mapping_f1` - /// - `n2`: range dimension of `mapping_f2` + /// - `constraints`: hard constraints, set $U$ + /// - `alm_set_c`: Set $C$ of ALM-specific constraints (convex, closed) + /// - `alm_set_y`: Compact, convex set $Y$ of Lagrange multipliers, which needs to be a + /// compact subset of $C^*$ (the convex conjugate of the convex set $C{}\subseteq{}\mathbb{R}^{n_1}$) + /// - `parametric_cost`: Parametric cost function, $\psi(u, \xi)$, where $\xi = (c, y)$ + /// - `parametric_gradient`: Gradient of cost function wrt $u$, that is $\nabla_x \psi(u, \xi)$ + /// - `mapping_f1`: Mapping `F1` of ALM-specific constraints ($F1(u) \in C$) + /// - `mapping_f2`: Mapping `F2` of PM-specific constraints ($F2(u) = 0$) + /// - `n1`: range dimension of $F_1(u)$ (that is, `mapping_f1`) + /// - `n2`: range dimension of $F_2(u)$ (that is, `mapping_f2`) /// /// /// ## Returns @@ -98,13 +118,13 @@ where /// ```rust /// use optimization_engine::{SolverError, alm::*, constraints::Ball2}; /// - /// let f = |_u: &[f64], _p: &[f64], _cost: &mut f64| -> Result<(), SolverError> { Ok(()) }; - /// let df = |_u: &[f64], _p: &[f64], _grad: &mut [f64]| -> Result<(), SolverError> { Ok(()) }; + /// let psi = |_u: &[f64], _p: &[f64], _cost: &mut f64| -> Result<(), SolverError> { Ok(()) }; + /// let dpsi = |_u: &[f64], _p: &[f64], _grad: &mut [f64]| -> Result<(), SolverError> { Ok(()) }; /// let n1 = 0; /// let n2 = 0; /// let bounds = Ball2::new(None, 10.0); /// let _alm_problem = AlmProblem::new( - /// bounds, NO_SET, NO_SET, f, df, NO_MAPPING, NO_MAPPING, n1, n2, + /// bounds, NO_SET, NO_SET, psi, dpsi, NO_MAPPING, NO_MAPPING, n1, n2, /// ); /// ``` /// diff --git a/src/alm/mod.rs b/src/alm/mod.rs index 075f3463..7896c8bb 100644 --- a/src/alm/mod.rs +++ b/src/alm/mod.rs @@ -13,25 +13,25 @@ pub use alm_optimizer::AlmOptimizer; pub use alm_optimizer_status::AlmOptimizerStatus; pub use alm_problem::AlmProblem; -/// Type of mappings `F1` and `F2` +/// Type of mappings $F_1(u)$ and $F_2(u)$ /// -/// Mappings `F1` and `F2` are computed by functions with signature +/// Mappings $F_1$ and $F_2$ are computed by functions with signature /// /// ```ignore /// fn mapping_f(&[f64], &mut [f64]) -> Result<(), crate::SolverError> /// ``` pub type MappingType = fn(&[f64], &mut [f64]) -> Result<(), crate::SolverError>; -/// Type of the Jacobian of mappings `F1` and `F2` +/// Type of the Jacobian of mappings $F_1$ and $F_2$ /// /// These are mappings $(u, d) \mapsto JF_1(u)^\top d$, for given vectors $u\in\mathbb{R}$ -/// and $d\in\mathbb{R}^{n_1}$ (similarly for `F2`) +/// and $d\in\mathbb{R}^{n_1}$ (similarly for $F_2$) pub type JacobianMappingType = fn(&[f64], &[f64], &mut [f64]) -> Result<(), crate::SolverError>; -/// No mapping `F1` or `F2` is specified +/// No mapping $F_1(u)$ or $F_2(u)$ is specified pub const NO_MAPPING: Option = None::; -/// No Jacobian mapping is specified for `F1` and `F2` +/// No Jacobian mapping is specified for $F_1$ and $F_2$ pub const NO_JACOBIAN_MAPPING: Option = None::; /// No set is specified (when specifying a set is optional) From c1eec7a2d12b5500c048e582bac3862ca28b70ae Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 15:11:51 +0100 Subject: [PATCH 40/42] Preparing version 0.6.1-alpha.2 --- .gitignore | 1 + Cargo.toml | 2 +- open-codegen/opengen/icasadi/extern/Makefile | 18 ------------------ 3 files changed, 2 insertions(+), 19 deletions(-) delete mode 100644 open-codegen/opengen/icasadi/extern/Makefile diff --git a/.gitignore b/.gitignore index eab7e0eb..d762d8e0 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,7 @@ target/ *.odp# *.egg-info *.pyc +open-codegen/opengen/icasadi/extern/Makefile # Python tests create this folder: open-codegen/opengen/.python_test_build/ diff --git a/Cargo.toml b/Cargo.toml index 79ed3208..ae8d2e61 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -42,7 +42,7 @@ homepage = "https://alphaville.github.io/optimization-engine/" repository = "https://github.com/alphaville/optimization-engine" # Version of this crate (SemVer) -version = "0.6.1-alpha.1" +version = "0.6.1-alpha.2" edition = "2018" diff --git a/open-codegen/opengen/icasadi/extern/Makefile b/open-codegen/opengen/icasadi/extern/Makefile deleted file mode 100644 index 1a308308..00000000 --- a/open-codegen/opengen/icasadi/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 From 55b6f8df81d455c86e72ddfd26600e63be3c05b1 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 15:14:56 +0100 Subject: [PATCH 41/42] using OpEn v0.6.1-alpha.2 in codegen --- open-codegen/opengen/templates/optimizer_cargo.toml.template | 3 +-- open-codegen/opengen/templates/tcp_server_cargo.toml.template | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/open-codegen/opengen/templates/optimizer_cargo.toml.template b/open-codegen/opengen/templates/optimizer_cargo.toml.template index 013c9e0d..7d0f5c35 100644 --- a/open-codegen/opengen/templates/optimizer_cargo.toml.template +++ b/open-codegen/opengen/templates/optimizer_cargo.toml.template @@ -18,8 +18,7 @@ publish=false [dependencies] -## optimization_engine = "{{open_version or '0.6.1-alpha.1'}}" -optimization_engine = {path = "../../../../"} +optimization_engine = "{{open_version or '0.6.1-alpha.2'}}" icasadi = {path = "./icasadi/"} {% if activate_clib_generation -%} libc = "0.2.0" diff --git a/open-codegen/opengen/templates/tcp_server_cargo.toml.template b/open-codegen/opengen/templates/tcp_server_cargo.toml.template index 61d1a43e..777ae078 100644 --- a/open-codegen/opengen/templates/tcp_server_cargo.toml.template +++ b/open-codegen/opengen/templates/tcp_server_cargo.toml.template @@ -21,8 +21,7 @@ publish=false [dependencies] -# optimization_engine = "0.6.1-alpha.1" -optimization_engine = {path = "../../../../../"} +optimization_engine = "0.6.1-alpha.2" serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" pretty_env_logger = "0.3.0" From aa71813bd7408fafedc5be3d080289989d471bbb Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Sep 2019 15:25:00 +0100 Subject: [PATCH 42/42] update changelog [ci skip] --- CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ecddb351..36b74f6b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] +## [v0.6.1-alpha.2] - 2019-09-7 + ### Fixed * TCP server: Malformed error JSON is now fixed @@ -19,6 +21,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ### Added +* Implementation of joint ALM/PM algorithm * New AKKT-compliant termination criterion * Tolerance relaxation in penalty method * Finite sets supported in Rust @@ -28,6 +31,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ### Removed * Support for Python <3.6 (deprecated) +* Module `continuation` is going to become obsolete -[Unreleased]: https://github.com/alphaville/optimization-engine/compare/v0.5.0...master +[v0.6.1-alpha.2]: https://github.com/alphaville/optimization-engine/compare/v0.5.0...v0.6.1-alpha.2 [v0.5.0]: https://github.com/alphaville/optimization-engine/compare/v0.4.0...v0.5.0 [v0.4.0]: https://github.com/alphaville/optimization-engine/compare/v0.3.1...v0.4.0 [v0.3.1]: https://github.com/alphaville/optimization-engine/compare/v0.3.0...v0.3.1