From 0a9edee47773328168f1794413fd3327df84a863 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 2 Aug 2023 16:01:55 +0200 Subject: [PATCH 01/70] fixed compilation issue in python3.7 env --- horizon/cpp/CMakeLists.txt | 2 +- horizon/cpp/src/iterate_filter.h | 1 + horizon/cpp/src/profiling.h | 1 + horizon/rhc/tasks/contactTask.py | 2 +- horizon/ros/trajectory_viewer.py | 8 +++++--- 5 files changed, 9 insertions(+), 5 deletions(-) diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index 57c97d3c..49662959 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/horizon/cpp/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(casadi 3.5.5 REQUIRED) find_package(yaml-cpp REQUIRED) set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) -set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD 17) # ilqr library add_library(horizon STATIC diff --git a/horizon/cpp/src/iterate_filter.h b/horizon/cpp/src/iterate_filter.h index 09f94b3b..6f31c561 100644 --- a/horizon/cpp/src/iterate_filter.h +++ b/horizon/cpp/src/iterate_filter.h @@ -4,6 +4,7 @@ #include #include #include +#include class IterateFilter { diff --git a/horizon/cpp/src/profiling.h b/horizon/cpp/src/profiling.h index eab38435..cbf29e9a 100644 --- a/horizon/cpp/src/profiling.h +++ b/horizon/cpp/src/profiling.h @@ -5,6 +5,7 @@ #include #include #include +#include namespace horizon { namespace utils { diff --git a/horizon/rhc/tasks/contactTask.py b/horizon/rhc/tasks/contactTask.py index 8cf92705..de614a15 100644 --- a/horizon/rhc/tasks/contactTask.py +++ b/horizon/rhc/tasks/contactTask.py @@ -15,7 +15,7 @@ def __init__(self, subtask, # todo : default interaction or cartesian task ? # todo : make tasks discoverable by name? subtask: {'interaction': force_contact_1} self.interaction_task: InteractionTask = Task.subtask_by_class(subtask, InteractionTask) - self.cartesian_task: CartesianTask = Task.subtask_by_class(subtask, CartesianTask) # CartesianTask RollingTask + self.cartesian_task: CartesianTask = Task.subtask_by_class(subtask, RollingTask) # CartesianTask RollingTask # initialize data class super().__init__(*args, **kwargs) diff --git a/horizon/ros/trajectory_viewer.py b/horizon/ros/trajectory_viewer.py index 38226092..7bf9af36 100644 --- a/horizon/ros/trajectory_viewer.py +++ b/horizon/ros/trajectory_viewer.py @@ -8,7 +8,6 @@ from sensor_msgs.msg import JointState import subprocess import time -from numpy_ros import to_numpy, to_message class TrajectoryViewer: @@ -94,7 +93,10 @@ def publish_line(self, points): for col in range(points.shape[1]): - point = to_message(Point, points[:3, col]) + point.x = points[0, col] + point.y = points[1, col] + point.z = points[2, col] + marker.points.append(point) self.line_array.markers.append(marker) @@ -126,4 +128,4 @@ def publish_line(self, points): rate.sleep() # # rospy.sleep(0.5) - # rospy.spin() \ No newline at end of file + # rospy.spin() From 46a540ecf1c31fb1b4a6bce35baf06d40e7968ff Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 2 Aug 2023 16:02:19 +0200 Subject: [PATCH 02/70] added specific utility to evaluate torque contraints --- horizon/rhc/taskInterface.py | 32 +++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 691bc367..eae5eacd 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -77,7 +77,6 @@ def bootstrap(self): pass self.solution = self.solver_bs.getSolutionDict() - def rti(self): t = time.time() @@ -89,6 +88,37 @@ def rti(self): return check + def eval_tau_on_sol(self): + + tau = np.zeros([self.model.tau.shape[0], + self.prb.getNNodes() - 1]) + + if self.model.fmap: + + fmap = dict() + + for frame, wrench in self.model.fmap.items(): + + fmap[frame] = self.solution[f'{wrench.getName()}'] + + id = kin_dyn.InverseDynamics(self.model.kd, fmap.keys(), self.model.kd_frame) + + for i in range(tau.shape[1]): + + fmap_i = dict() + + for frame, wrench in fmap.items(): + fmap_i[frame] = wrench[:, i] + + tau_i = id.call(self.solution['q'][:, i], + self.solution['v'][:, i], + self.solution['a'][:, i], + fmap_i) + + tau[:, i] = tau_i.toarray().flatten() + + return tau + def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): if nodes is None: From 32d88389f19ebb49c92725310cf2f9428ae423b8 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Aug 2023 11:04:59 +0200 Subject: [PATCH 03/70] added lagrange mult, opt cost, cost values thorugh iterations and iteration count --- horizon/solvers/nlpsol.py | 76 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/horizon/solvers/nlpsol.py b/horizon/solvers/nlpsol.py index 8d1473cf..c625bd35 100644 --- a/horizon/solvers/nlpsol.py +++ b/horizon/solvers/nlpsol.py @@ -41,9 +41,70 @@ def __init__(self, prb: Problem, opts: Dict, solver_plugin: str) -> None: self.prob_dict = {'f': j, 'x': w, 'g': g, 'p': p} + # callback to get the iteration count + self.iter_counter_callback = self.IterCountCallback('iter_counter_callback', w.shape[0], g.shape[0], p.shape[0], + opts = self.opts) + self.opts['iteration_callback'] = self.iter_counter_callback + # create solver from prob self.solver = cs.nlpsol('solver', solver_plugin, self.prob_dict, self.opts) + class IterCountCallback(cs.Callback): + def __init__(self, name, nx, ng, np, opts={}): + + cs.Callback.__init__(self) + + self.solv_opts = opts + self.constr_viol = self.solv_opts["ipopt.constr_viol_tol"] + + self.nx = nx + self.ng = ng + self.np = np + + self.iter_counter = -1 + self.cost_values = [] + # self.is_feasible = [] + + # Initialize internal objects + self.construct(name, {}) + + def get_n_in(self): return cs.nlpsol_n_out() + def get_n_out(self): return 1 + def get_name_in(self, i): return cs.nlpsol_out(i) + def get_name_out(self, i): return "ret" + + def get_sparsity_in(self, i): + n = cs.nlpsol_out(i) + if n=='f': + return cs.Sparsity. scalar() + elif n in ('x', 'lam_x'): + return cs.Sparsity.dense(self.nx) + elif n in ('g', 'lam_g'): + return cs.Sparsity.dense(self.ng) + else: + return cs.Sparsity(0,0) + + def eval(self, arg): + + # add here any info to be retrieved from the solver + + self.cost_values.append(arg[1]) + self.iter_counter = len(self.cost_values) + + constraint_violations = arg[2] + n_cnstrnts = constraint_violations.shape[0] + + within_feasibility = np.zeros((n_cnstrnts, 1), dtype=bool) + for i in range(0, n_cnstrnts): + # DM types not iterable by default + + within_feasibility[i] = constraint_violations[i] <= self.solv_opts["ipopt.constr_viol_tol"] and \ + constraint_violations[i] >= - self.solv_opts["ipopt.constr_viol_tol"] + + # self.is_feasible.append(np.all(within_feasibility)) + + return [0] + def build(self): """ fill the dictionary "state_var_impl" @@ -95,6 +156,8 @@ def build(self): def solve(self) -> bool: + self.iter_counter_callback.iter_counter = 0 # resetting iteration number at each solve + # update lower/upper bounds of variables lbw = self._getVarList('lb') ubw = self._getVarList('ub') @@ -129,11 +192,21 @@ def solve(self) -> bool: self.cnstr_solution = self._createCnsrtSolDict(sol) + self.lambd_solution = self._createCnsrtLambDict(sol) + #adding lagrange multipliers list of g to dict + # self.cnstr_solution['lam_x'] = np.array(sol['lam_x']) + self.lambd_solution['lam_g'] = np.array(sol['lam_g']) + # retrieve state and input trajector # get solution dict self.var_solution = self._createVarSolDict(sol) + # adding additional info + self.var_solution["opt_cost"] = float(sol['f']) + self.var_solution["n_iter2sol"] = self.iter_counter_callback.iter_counter + self.var_solution["cost_values"] = self.iter_counter_callback.cost_values + # get solution as state/input self._createVarSolAsInOut(sol) self.var_solution['x_opt'] = self.x_opt @@ -150,6 +223,9 @@ def getSolutionDict(self): def getConstraintSolutionDict(self): return self.cnstr_solution + def getCnstrLmbdSolDict(self): + return self.lambd_solution + def getDt(self): return self.dt_solution From 1d3b00bc406738625dc439df19c9500fcf45dca5 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Aug 2023 12:07:51 +0200 Subject: [PATCH 04/70] added callback to get additional solution info Signed-off-by: Andrea Patrizi --- horizon/solvers/ilqr.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 94ed7c1f..81b11e06 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -91,9 +91,14 @@ def __init__(self, # empty solution dict self.solution_dict = dict() + self.current_iteration = 0 + self.iteration_costs = [] + # print iteration statistics self.set_iteration_callback() + self.set_iteration_callback(self._sol_info_callback) + def save(self): data = self.prb.save() data['solver'] = dict() @@ -104,7 +109,6 @@ def save(self): data['dynamics'] = self.dyn.serialize() return data - def set_iteration_callback(self, cb=None): if cb is None: self.ilqr.setIterationCallback(self._iter_callback) @@ -112,12 +116,20 @@ def set_iteration_callback(self, cb=None): print('setting custom iteration callback') self.ilqr.setIterationCallback(cb) + def _sol_info_callback(self, fpres): + + self.current_iteration = self.current_iteration + 1 + + self.iteration_costs.append(fpres.cost) def configure_rti(self) -> bool: self.opts['max_iter'] = 1 def solve(self): + self.iteration_costs = [] # resets costs data + self.current_iteration = 0 # resets iteration counter + # set initial state x0 = self.prb.getInitialState() xinit = self.prb.getState().getInitialGuess() @@ -159,6 +171,10 @@ def solve(self): self.solution_dict['x_opt'] = self.x_opt self.solution_dict['u_opt'] = self.u_opt + self.solution_dict['opt_cost'] = self.iteration_costs[-1] if len(self.iteration_costs) else -1.0 + self.solution_dict['iter_costs'] = np.array(self.iteration_costs) + self.solution_dict['n_iter2sol'] = self.current_iteration + return ret def getSolutionDict(self): From 064c43f9019c98cd6b9888fa5b54f35f560d7a0a Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 4 Aug 2023 14:40:51 +0200 Subject: [PATCH 05/70] added mission getter for lagrange multipliers --- horizon/solvers/solver.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/horizon/solvers/solver.py b/horizon/solvers/solver.py index d6cdef5d..277b0f52 100644 --- a/horizon/solvers/solver.py +++ b/horizon/solvers/solver.py @@ -141,6 +141,22 @@ def _getFunList(self, type): f = cs.vertcat(*fun_list) return f + def _createCnsrtLambDict(self, solution): + + lambd_cnsrt_dict = dict() + pos = 0 + for name, fun in self.prb.function_container.getCnstr().items(): # iterate through each symbolic constraint + + #extracting and reshaping the constraint values associated to name + lam_g_vals = solution['lam_g'][pos:pos + fun.getDim() * len(fun.getNodes())] + lam_g_vals_mat = np.reshape(lam_g_vals, (fun.getDim(), len(fun.getNodes())), order='F') + + lambd_cnsrt_dict[name + "_lambd"] = lam_g_vals_mat + + pos = pos + fun.getDim() * len(fun.getNodes()) + + return lambd_cnsrt_dict + def _createCnsrtSolDict(self, solution): fun_sol_dict = dict() From 6be2e629ac7d623102e63e8e006198f4aeda61f9 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 4 Aug 2023 14:41:29 +0200 Subject: [PATCH 06/70] removed hugely expensive run-time recreation of inverse dynamics object --- horizon/rhc/taskInterface.py | 105 +++++++++++++++++++++++------------ 1 file changed, 68 insertions(+), 37 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index eae5eacd..99b148af 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -56,7 +56,8 @@ def __init__(self, # task list self.task_list = [] - + + self.bootstrap_solved = False def finalize(self, rti=True): """ @@ -64,7 +65,6 @@ def finalize(self, rti=True): """ self.model.setDynamics() self._create_solver(rti) - def bootstrap(self): t = time.time() @@ -77,6 +77,8 @@ def bootstrap(self): pass self.solution = self.solver_bs.getSolutionDict() + self.bootstrap_solved = True + def rti(self): t = time.time() @@ -88,37 +90,66 @@ def rti(self): return check - def eval_tau_on_sol(self): - - tau = np.zeros([self.model.tau.shape[0], - self.prb.getNNodes() - 1]) - - if self.model.fmap: - - fmap = dict() + def init_inv_dyn_for_res(self): + + # we create the inv dynamics for resampling here + # to avoid runtime overhead + if (self.bootstrap_solved): # we need to need the + # force map from the solution, so we wait for the bootstrap, + # since it's solved during the initialization phase and not + # at runtime + + self.fmap = dict() for frame, wrench in self.model.fmap.items(): + self.fmap[frame] = self.solution[f'{wrench.getName()}'] + + self.res_id = kin_dyn.InverseDynamics(self.model.kd, + self.fmap.keys(), + self.model.kd_frame) + + self.tau_eval = np.zeros([self.model.tau.shape[0], + self.prb.getNNodes() - 1]) # evaluated tau on nodes - fmap[frame] = self.solution[f'{wrench.getName()}'] + else: - id = kin_dyn.InverseDynamics(self.model.kd, fmap.keys(), self.model.kd_frame) + raise Exception("The method init_inv_dyn_for_res from " + __class__.__name__ + + " can only be called after bootstrap() has returned!") + + def eval_tau_on_sol(self): + + if self.model.fmap: - for i in range(tau.shape[1]): + for i in range(self.tau_eval.shape[1]): fmap_i = dict() - for frame, wrench in fmap.items(): + for frame, wrench in self.fmap.items(): fmap_i[frame] = wrench[:, i] - tau_i = id.call(self.solution['q'][:, i], + tau_i = self.res_id.call(self.solution['q'][:, i], self.solution['v'][:, i], self.solution['a'][:, i], fmap_i) - tau[:, i] = tau_i.toarray().flatten() + self.tau_eval[:, i] = tau_i.toarray().flatten() + + return self.tau_eval + + def eval_tau_on_first_node(self): + + fmap_0 = dict() - return tau + for frame, wrench in self.fmap.items(): + fmap_0[frame] = wrench[:, 0] + tau_i = self.res_id.call(self.solution['q'][:, 0], + self.solution['v'][:, 0], + self.solution['a'][:, 0], + fmap_0) + + return tau_i.toarray() + def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): if nodes is None: @@ -159,20 +190,11 @@ def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): # new fmap with resampled forces if self.model.fmap: - fmap = dict() - for frame, wrench in self.model.fmap.items(): - fmap[frame] = self.solution[f'{wrench.getName()}'] - - fmap_res = dict() - for frame, wrench in self.model.fmap.items(): - fmap_res[frame] = self.solution[f'{wrench.getName()}_res'] - # get tau resampled if resample_tau: - tau = np.zeros([self.model.tau.shape[0], self.prb.getNNodes() - 1]) - tau_res = np.zeros([self.model.tau.shape[0], u_res.shape[1]]) - id = kin_dyn.InverseDynamics(self.model.kd, fmap_res.keys(), self.model.kd_frame) + tau_res = np.zeros([self.model.tau.shape[0], u_res.shape[1]]) # we create this at runtime + # (can be improved) # id_fn = kin_dyn.InverseDynamics(self.kd, self.fmap.keys(), self.kd_frame) # self.tau = id_fn.call(self.q, self.v, self.a, self.fmap) @@ -180,26 +202,35 @@ def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): # todo: this is horrible. id.call should take matrices, I should not iter over each node - for i in range(tau.shape[1]): + for i in range(self.tau_eval.shape[1]): + fmap_i = dict() - for frame, wrench in fmap.items(): + for frame, wrench in self.fmap.items(): fmap_i[frame] = wrench[:, i] - tau_i = id.call(self.solution['q'][:, i], self.solution['v'][:, i], self.solution['a'][:, i], + + tau_i = self.res_id.call(self.solution['q'][:, i], + self.solution['v'][:, i], + self.solution['a'][:, i], fmap_i) - tau[:, i] = tau_i.toarray().flatten() + + self.tau_eval[:, i] = tau_i.toarray().flatten() for i in range(tau_res.shape[1]): + fmap_res_i = dict() - for frame, wrench in fmap_res.items(): + for frame, wrench in self.fmap.items(): fmap_res_i[frame] = wrench[:, i] - tau_res_i = id.call(self.solution['q_res'][:, i], self.solution['v_res'][:, i], - self.solution['a_res'][:, i], fmap_res_i) + + tau_res_i = self.res_id.call(self.solution['q_res'][:, i], + self.solution['v_res'][:, i], + self.solution['a_res'][:, i], + fmap_res_i) + tau_res[:, i] = tau_res_i.toarray().flatten() - self.solution['tau'] = tau + self.solution['tau'] = self.tau_eval self.solution['tau_res'] = tau_res - def save_solution(self, filename): import copy ms = mat_storer.matStorer(filename) From 877b8e0d1eec2ea7d704282a3c026f8d8ff2a97c Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 16 Aug 2023 16:10:38 +0200 Subject: [PATCH 07/70] made profiling optional and fixed missing update of fmap with last solution --- horizon/rhc/taskInterface.py | 66 ++++++++++++++++++------------------ horizon/solvers/ilqr.py | 8 +++-- horizon/solvers/nlpsol.py | 1 - 3 files changed, 38 insertions(+), 37 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 99b148af..f84f7bfb 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -27,8 +27,15 @@ class TaskInterface: def __init__(self, - prb, - model): + prb, + model, + debug: bool = False, + verbose: bool = False): + + self._debug = debug + self._verbose = verbose + + self.rt_solve_time = -1.0 # get the model self.prb = prb @@ -80,11 +87,20 @@ def bootstrap(self): self.bootstrap_solved = True def rti(self): - - t = time.time() + + if self._debug: + + t = time.time() + check = self.solver_rti.solve() - elapsed = time.time() - t - print(f'rti solved in {elapsed} s') + + if self._debug: + + self.rt_solve_time = time.time() - t + + if self._debug and self._verbose: + + print(f'rti solved in {self.rt_solve_time} s') self.solution = self.solver_rti.getSolutionDict() @@ -96,7 +112,7 @@ def init_inv_dyn_for_res(self): # to avoid runtime overhead if (self.bootstrap_solved): # we need to need the - # force map from the solution, so we wait for the bootstrap, + # force map (in particular the keys) from the solution, so we wait for the bootstrap, # since it's solved during the initialization phase and not # at runtime @@ -110,43 +126,27 @@ def init_inv_dyn_for_res(self): self.tau_eval = np.zeros([self.model.tau.shape[0], self.prb.getNNodes() - 1]) # evaluated tau on nodes + + self.fmap_0 = dict() # we initialize also the force map with the wrenches on the + # first noe else: raise Exception("The method init_inv_dyn_for_res from " + __class__.__name__ + " can only be called after bootstrap() has returned!") - - def eval_tau_on_sol(self): - - if self.model.fmap: - - for i in range(self.tau_eval.shape[1]): - - fmap_i = dict() - - for frame, wrench in self.fmap.items(): - fmap_i[frame] = wrench[:, i] - - tau_i = self.res_id.call(self.solution['q'][:, i], - self.solution['v'][:, i], - self.solution['a'][:, i], - fmap_i) - - self.tau_eval[:, i] = tau_i.toarray().flatten() - - return self.tau_eval - def eval_tau_on_first_node(self): + def eval_efforts_on_first_node(self): - fmap_0 = dict() - - for frame, wrench in self.fmap.items(): - fmap_0[frame] = wrench[:, 0] + for frame, wrench in self.model.fmap.items(): + + # we update the force map from the latest solution + self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, 0] + tau_i = self.res_id.call(self.solution['q'][:, 0], self.solution['v'][:, 0], self.solution['a'][:, 0], - fmap_0) + self.fmap_0) return tau_i.toarray() diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 81b11e06..49637fbb 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -126,7 +126,7 @@ def configure_rti(self) -> bool: self.opts['max_iter'] = 1 def solve(self): - + self.iteration_costs = [] # resets costs data self.current_iteration = 0 # resets iteration counter @@ -134,7 +134,7 @@ def solve(self): x0 = self.prb.getInitialState() xinit = self.prb.getState().getInitialGuess() uinit = self.prb.getInput().getInitialGuess() - + # update initial guess self.ilqr.setStateInitialGuess(xinit) self.ilqr.setInputInitialGuess(uinit) @@ -151,6 +151,7 @@ def solve(self): self._update_nodes() # solve + ret = self.ilqr.solve(self.max_iter) # get solution @@ -174,7 +175,8 @@ def solve(self): self.solution_dict['opt_cost'] = self.iteration_costs[-1] if len(self.iteration_costs) else -1.0 self.solution_dict['iter_costs'] = np.array(self.iteration_costs) self.solution_dict['n_iter2sol'] = self.current_iteration - + + return ret def getSolutionDict(self): diff --git a/horizon/solvers/nlpsol.py b/horizon/solvers/nlpsol.py index c625bd35..7105006f 100644 --- a/horizon/solvers/nlpsol.py +++ b/horizon/solvers/nlpsol.py @@ -6,7 +6,6 @@ import numpy as np import pprint - class NlpsolSolver(Solver): def __init__(self, prb: Problem, opts: Dict, solver_plugin: str) -> None: From 30114a70173245727c15148366e1874617fb2271 Mon Sep 17 00:00:00 2001 From: FrancescoRuscelli Date: Mon, 6 Nov 2023 16:05:35 +0100 Subject: [PATCH 08/70] bug in setweight of task --- horizon/rhc/tasks/task.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/tasks/task.py b/horizon/rhc/tasks/task.py index 81b4f8a8..658af00b 100644 --- a/horizon/rhc/tasks/task.py +++ b/horizon/rhc/tasks/task.py @@ -50,8 +50,19 @@ def __post_init__(self): # self.nodes = list(range(self.prb.getNNodes())) def _createWeightParam(self): - self.weight_param = self.prb.createParameter(f'{self.name}_weight', 1) - self.weight_param.assign(self.weight) + + # weight and dim must be the same dimension + if isinstance(self.weight, (float, int)): + self.weight_param = self.prb.createParameter(f'{self.name}_weight', 1) + self.weight_param.assign(self.weight) + + elif isinstance(self.weight, List): + self.weight_param = [] + for i_dim in range(len(self.weight)): + + temp_par = self.prb.createParameter(f'{self.name}_weight_{i_dim}', 1) + temp_par.assign(self.weight[i_dim]) + self.weight_param.append(temp_par) def setNodes(self, nodes, erasing=True): self.nodes = nodes From 3e0e82ab617b586608da986252f8b8fd973aae2f Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 6 Nov 2023 16:10:26 +0100 Subject: [PATCH 09/70] added missing debug and verbose inputs --- horizon/rhc/taskInterface.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 08c7de84..fd2c143d 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -43,6 +43,8 @@ def __init__(self, self.solver_bs = None self.solver_rti = None + self.bootstrap_solved = False + def finalize(self, rti=True): """ to be called after all variables have been created @@ -88,7 +90,7 @@ def init_inv_dyn_for_res(self): # we create the inv dynamics for resampling here # to avoid runtime overhead - if (self.bootstrap_solved): # we need to need the + if (self.bootstrap_solved): # we need to have the # force map (in particular the keys) from the solution, so we wait for the bootstrap, # since it's solved during the initialization phase and not # at runtime @@ -302,9 +304,11 @@ def _create_solver(self, rti=True): class TaskInterface(ProblemInterface): def __init__(self, prb, - model): + model, + debug = False, + verbose = False): - super().__init__(prb, model) + super().__init__(prb, model, debug, verbose) # here I register the the default tasks # todo: should I do it here? @@ -349,7 +353,6 @@ def setTaskFromYaml(self, yaml_config): self.setTaskFromDict(task_descr) - def setTaskFromDict(self, task_description): # todo if task is dict... ducktyping From b3f3227a3182fb89effe071c40ab7987325be872 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 6 Nov 2023 18:47:30 +0100 Subject: [PATCH 10/70] if in conda env, by default install in it --- horizon/cpp/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index 49662959..279af3ea 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/horizon/cpp/CMakeLists.txt @@ -1,6 +1,12 @@ project(horizon) cmake_minimum_required(VERSION 3.0) +if(DEFINED ENV{CONDA_PREFIX}) + + set(CMAKE_INSTALL_PREFIX $ENV{CONDA_PREFIX}/ CACHE PATH "bindings install prefix" FORCE) + +endif() + # options option(HORIZON_PROFILING OFF "enable profiling features") From 19f9987a5a1419e04c4d5bb7607790f9e77723f4 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 7 Nov 2023 19:44:27 +0100 Subject: [PATCH 11/70] removed a couple of spaces --- horizon/cpp/src/ilqr_forward_pass.cpp | 1 - horizon/solvers/ilqr.py | 1 - 2 files changed, 2 deletions(-) diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index d6b12e24..e3286fcd 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -456,7 +456,6 @@ bool IterativeLQR::should_stop() return false; } - // here we're feasible // exit if merit function directional derivative (normalized) diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index deb39197..a5d17b8a 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -176,7 +176,6 @@ def solve(self): self.solution_dict['iter_costs'] = np.array(self.iteration_costs) self.solution_dict['n_iter2sol'] = self.current_iteration - return ret def getSolutionDict(self): From 2401539f0d71f39a5445d6b5f397f53c20b759ac Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 23 Nov 2023 17:24:25 +0100 Subject: [PATCH 12/70] commented replay trajectory. taskInterface should not be ros-dependent Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 50 ++++++++++++++-------------- horizon/utils/trajectoryGenerator.py | 4 +-- 2 files changed, 26 insertions(+), 28 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index fd2c143d..02bea708 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -20,7 +20,7 @@ from horizon.rhc import task_factory, plugin_handler, solver_interface from horizon.rhc.yaml_handler import YamlParser from horizon.solvers.solver import Solver -from horizon.ros.replay_trajectory import replay_trajectory +# from horizon.ros.replay_trajectory import replay_trajectory import logging import time @@ -247,30 +247,30 @@ def load_initial_guess(self, from_dict=None): self.prb.getInput().setInitialGuess(u_opt) self.prb.setInitialState(x0=x_opt[:, 0]) - def replay_trajectory(self, trajectory_markers=[], trajectory_markers_opts={}): - - # single replay - joint_names = self.model.kd.joint_names() - q_sol = self.solution['q'] - q_sol_minimal = np.zeros([q_sol.shape[0], self.prb.getNNodes()]) - - # if q is not minimal (continuous joints are present) make it minimal - for col in range(q_sol.shape[1]): - q_sol_minimal[:, col] = self.model.kd.getMinimalQ(q_sol[:, col]) - - frame_force_mapping = {cname: self.solution[f.getName()] for cname, f in self.model.fmap.items()} - - repl = replay_trajectory(self.prb.getDt(), - joint_names, - q_sol_minimal, - frame_force_mapping, - self.model.kd_frame, - self.model.kd, - fixed_joint_map=self.model.fixed_joint_map, - trajectory_markers=trajectory_markers, - trajectory_markers_opts=trajectory_markers_opts) - repl.sleep(1.) - repl.replay(is_floating_base=True, base_link='pelvis') + # def replay_trajectory(self, trajectory_markers=[], trajectory_markers_opts={}): + + # # single replay + # joint_names = self.model.kd.joint_names() + # q_sol = self.solution['q'] + # q_sol_minimal = np.zeros([q_sol.shape[0], self.prb.getNNodes()]) + + # # if q is not minimal (continuous joints are present) make it minimal + # for col in range(q_sol.shape[1]): + # q_sol_minimal[:, col] = self.model.kd.getMinimalQ(q_sol[:, col]) + + # frame_force_mapping = {cname: self.solution[f.getName()] for cname, f in self.model.fmap.items()} + + # repl = replay_trajectory(self.prb.getDt(), + # joint_names, + # q_sol_minimal, + # frame_force_mapping, + # self.model.kd_frame, + # self.model.kd, + # fixed_joint_map=self.model.fixed_joint_map, + # trajectory_markers=trajectory_markers, + # trajectory_markers_opts=trajectory_markers_opts) + # repl.sleep(1.) + # repl.replay(is_floating_base=True, base_link='pelvis') def setSolverOptions(self, solver_options): solver_type = solver_options.pop('type') diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index db3a08e9..d4b377c3 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -7,9 +7,8 @@ from numpy import linspace, sin, pi from scipy.interpolate import BPoly, CubicSpline - - class TrajectoryGenerator: + def __init__(self): pass @@ -69,7 +68,6 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None): return y_bpoly - if __name__ == '__main__': tg = TrajectoryGenerator() From a1b81434b9fbbb3ad414c723d9f3dc5923384268 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 30 Nov 2023 19:42:38 +0100 Subject: [PATCH 13/70] fixed wrong nodes used for mpc torque computation and added solver n iter as class argument Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 33 ++++++++++++++++++++++----------- 1 file changed, 22 insertions(+), 11 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 02bea708..a9984c56 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -28,12 +28,15 @@ class ProblemInterface: def __init__(self, prb, model, + max_solver_iter: int = 1, debug: bool = False, verbose: bool = False): self._debug = debug self._verbose = verbose + self.max_solver_iter = max_solver_iter + self.rt_solve_time = -1.0 # get the model @@ -122,8 +125,8 @@ def eval_efforts_on_first_node(self): self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, 0] - tau_i = self.res_id.call(self.solution['q'][:, 0], - self.solution['v'][:, 0], + tau_i = self.res_id.call(self.solution['q'][:, 1], + self.solution['v'][:, 1], self.solution['a'][:, 0], self.fmap_0) @@ -294,21 +297,29 @@ def _create_solver(self, rti=True): if rti: scoped_opts_rti = self.si.opts.copy() - scoped_opts_rti['ilqr.enable_line_search'] = False - scoped_opts_rti['ilqr.max_iter'] = 1 + + scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter + + if self.max_solver_iter == 1: + + # real-time iteration -> no line-search necessary + scoped_opts_rti['ilqr.enable_line_search'] = False + self.solver_rti = Solver.make_solver(self.si.type, self.prb, scoped_opts_rti) return self.solver_bs, self.solver_rti - class TaskInterface(ProblemInterface): def __init__(self, - prb, - model, - debug = False, - verbose = False): - - super().__init__(prb, model, debug, verbose) + prb, + model, + max_solver_iter: int = 1, + debug = False, + verbose = False): + + super().__init__(prb, model, + max_solver_iter, + debug, verbose) # here I register the the default tasks # todo: should I do it here? From ed9e46ec1f3328b9f95592a888beaa5ce533ef7a Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 5 Dec 2023 17:58:21 +0100 Subject: [PATCH 14/70] added debug flag in options, now cost/constraints values dictionary filling optional Signed-off-by: Andrea Patrizi --- horizon/cpp/src/ilqr.cpp | 7 +++- horizon/cpp/src/ilqr.h | 1 + horizon/cpp/src/ilqr_forward_pass.cpp | 53 ++++++++++++++++----------- 3 files changed, 38 insertions(+), 23 deletions(-) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index ad946bcd..c6170e4c 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -84,6 +84,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, { // set options _hxx_reg_base _verbose = value_or(opt, "ilqr.verbose", 0); + _debug = value_or(opt, "ilqr.debug", 0); _log = value_or(opt, "ilqr.log", 0); _rti = value_or(opt, "ilqr.rti", 0); _step_length = value_or(opt, "ilqr.step_length", 1.0); @@ -664,7 +665,11 @@ bool IterativeLQR::solve(int max_iter) } } - std::cout << "max iteration reached \n"; + if (_verbose) { + + std::cout << "max iteration reached \n"; + + } return false; } diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index fcc63d2e..896f278a 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -285,6 +285,7 @@ class IterativeLQR static DecompositionType str_to_decomp_type(const std::string& dt_str); bool _verbose; + bool _debug; bool _log; bool _rti; diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index 93a842d4..83ddeb94 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -190,24 +190,29 @@ double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::Matr double cost = 0.0; - // reset constr value to nan - for(auto& item : _cost_values) - { - item.second.setConstant(std::numeric_limits::quiet_NaN()); - } + if (_debug) { + // reset constr value to nan + for(auto& item : _cost_values) + { + item.second.setConstant(std::numeric_limits::quiet_NaN()); + } + } + // intermediate cost for(int i = 0; i < _N; i++) { cost += _cost[i].evaluate(xtrj.col(i), utrj.col(i), i); - // optionally (TBD) save values of single costs acting on this node - for(auto it : _cost[i].items) - { - // not updating items uninitialized (item vs item_cost) - if (_cost_values[it->getName()].size() != 0) + if (_debug) { + // optionally (TBD) save values of single costs acting on this node + for(auto it : _cost[i].items) { - _cost_values[it->getName()](i) = it->getCostEvaluated(); + // not updating items uninitialized (item vs item_cost) + if (_cost_values[it->getName()].size() != 0) + { + _cost_values[it->getName()](i) = it->getCostEvaluated(); + } } } @@ -245,12 +250,16 @@ double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::Ma double constr = 0.0; - // reset constr value to nan - for(auto& item : _constr_values) - { - item.second.setConstant(std::numeric_limits::quiet_NaN()); - } + if (_debug) { + // reset constr value to nan + for(auto& item : _constr_values) + { + item.second.setConstant(std::numeric_limits::quiet_NaN()); + } + + } + // intermediate constraint violation for(int i = 0; i < _N; i++) { @@ -263,10 +272,12 @@ double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::Ma _fp_res->constraint_values[i] = _constraint[i].h().lpNorm<1>(); constr += _fp_res->constraint_values[i]; - // optionally (TBD) save values of single constraints acting on this node - for(auto it : _constraint[i].items) - { - _constr_values[it->f.function().name()].col(i) = it->h(); + if (_debug) { + // optionally (TBD) save values of single constraints acting on this node + for(auto it : _constraint[i].items) + { + _constr_values[it->f.function().name()].col(i) = it->h(); + } } } @@ -362,8 +373,6 @@ bool IterativeLQR::line_search(int iter) report_result(*_fp_res); } - - // run line search while(alpha >= alpha_min) { From d1c03f389bf30e81aca3f600d89ef712abda8bd9 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 5 Dec 2023 17:58:42 +0100 Subject: [PATCH 15/70] exposed ilqr debug flag to horizon solver interface Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 5c2d9ffb..957a6688 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -297,6 +297,9 @@ def _create_solver(self, rti=True): scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter + scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically + # allows to retrieve costs and constraints values at runtime) + if self.max_solver_iter == 1: # real-time iteration -> no line-search necessary From d4234040351c69a8e13a233a1485c15df294b6ed Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 5 Jan 2024 09:56:52 +0100 Subject: [PATCH 16/70] infeasibility warning now printed only if in debug mode --- horizon/cpp/src/ilqr_backward_pass.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/horizon/cpp/src/ilqr_backward_pass.cpp b/horizon/cpp/src/ilqr_backward_pass.cpp index 1d02e090..5ff37ba7 100644 --- a/horizon/cpp/src/ilqr_backward_pass.cpp +++ b/horizon/cpp/src/ilqr_backward_pass.cpp @@ -61,16 +61,19 @@ void IterativeLQR::backward_pass() // infeasible warning if(residual.lpNorm<1>() > 1e-8) { + if (_debug) { - std::cout << "warn at k = 0: " << _constraint_to_go->dim() << + std::cout << "warn at k = 0: " << _constraint_to_go->dim() << " constraints not satified, residual inf-norm is " << residual.lpNorm() << "\n"; - if(_log) - { - std::cout << "C = \n" << _constraint_to_go->C().format(2) << "\n" << - "h = " << _constraint_to_go->h().transpose().format(2) << "\n"; + if(_log) + { + std::cout << "C = \n" << _constraint_to_go->C().format(2) << "\n" << + "h = " << _constraint_to_go->h().transpose().format(2) << "\n"; + } } + } } From 163a4f91f2acfeab7b00388a49937be218d40cab Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 5 Jan 2024 09:57:20 +0100 Subject: [PATCH 17/70] minor comments and added backup for bootstrap Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 957a6688..0f179b2c 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -17,6 +17,8 @@ from horizon.rhc.yaml_handler import YamlParser from horizon.solvers.solver import Solver +import copy + # from horizon.ros.replay_trajectory import replay_trajectory import time @@ -43,6 +45,9 @@ def __init__(self, self.solver_bs = None self.solver_rti = None + self.solution = {} + self.bootstrap_sol = {} + self.bootstrap_solved = False def finalize(self, rti=True): @@ -53,18 +58,36 @@ def finalize(self, rti=True): self._create_solver(rti) def bootstrap(self): + + # this is called sporadically: we don't really care + # about printing overheads here t = time.time() self.solver_bs.solve() elapsed = time.time() - t print(f'bootstrap solved in {elapsed} s') + try: self.solver_rti.print_timings() + except: pass + self.solution = self.solver_bs.getSolutionDict() + # we backup a copy (needs to be deep to work properly) + # of the bootstrap, which can be used to reset the controller + # if needed + + self.update_bootstrap_from_sol() + self.bootstrap_solved = True + def update_bootstrap_from_sol(self): + + # updates bootstrap backup with latest available solution + + self.bootstrap_sol = copy.deepcopy(self.solution) + def rti(self): if self._debug: @@ -118,10 +141,13 @@ def eval_efforts_on_first_node(self): for frame, wrench in self.model.fmap.items(): - # we update the force map from the latest solution + # we update the force maps from the latest solution - self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, 0] + self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, 0] # it's an input + # we get it from node 0 + # compute torque with inverse dynamics (states from node 1, inputs from + # node 0) tau_i = self.res_id.call(self.solution['q'][:, 1], self.solution['v'][:, 1], self.solution['a'][:, 0], @@ -130,7 +156,7 @@ def eval_efforts_on_first_node(self): return tau_i.toarray() def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): - + if nodes is None: nodes = list(range(self.prb.getNNodes() + 1)) From 2a1945915105109f2667d901977591e9aba80d3a Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 8 Jan 2024 12:01:52 +0100 Subject: [PATCH 18/70] added simple reset method Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 0f179b2c..0a256cad 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -88,6 +88,15 @@ def update_bootstrap_from_sol(self): self.bootstrap_sol = copy.deepcopy(self.solution) + def reset(self): + + # copies latest bootstrap into solution + + self.solution = copy.deepcopy(self.bootstrap) + # resets the controller with the latest solution + + self.load_initial_guess() + def rti(self): if self._debug: From 2519b9908e759d16eaca4fbb94bc6b8b32b71291 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 8 Jan 2024 19:02:47 +0100 Subject: [PATCH 19/70] fix typo --- horizon/rhc/taskInterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 0a256cad..40bd6494 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -92,7 +92,7 @@ def reset(self): # copies latest bootstrap into solution - self.solution = copy.deepcopy(self.bootstrap) + self.solution = copy.deepcopy(self.bootstrap_sol) # resets the controller with the latest solution self.load_initial_guess() From 669baaf61d88282b4309102ed2f28e49a07f7328 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 7 Feb 2024 16:14:21 +0100 Subject: [PATCH 20/70] exposed reset method for hxx regularization --- horizon/cpp/pyilqr.cpp | 1 + horizon/cpp/src/ilqr.cpp | 11 +++++++++++ horizon/cpp/src/ilqr.h | 2 ++ horizon/solvers/ilqr.py | 2 +- 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/horizon/cpp/pyilqr.cpp b/horizon/cpp/pyilqr.cpp index 038bb883..2f15ec9d 100644 --- a/horizon/cpp/pyilqr.cpp +++ b/horizon/cpp/pyilqr.cpp @@ -53,6 +53,7 @@ PYBIND11_MODULE(pyilqr, m) { .def("setInitialState", &IterativeLQR::setInitialState) .def("setInputInitialGuess", &IterativeLQR::setInputInitialGuess) .def("setStateInitialGuess", &IterativeLQR::setStateInitialGuess) + .def("reset", &IterativeLQR::reset) ; } diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index c6170e4c..030df708 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -616,6 +616,11 @@ const std::map &IterativeLQR::getCostsValues() con return _cost_values; } +void IterativeLQR::reset() +{ + _hxx_reg = _hxx_reg_base; +} + bool IterativeLQR::solve(int max_iter) { // set cost value and constraint violation *before* the forward pass @@ -630,6 +635,12 @@ bool IterativeLQR::solve(int max_iter) // reset counters _fp_accepted = 0; + // reset internal states if not rti mode + if(!_rti) + { + reset(); + } + // solve for(int i = 0; i < max_iter; i++) { diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index 896f278a..76c3f0dc 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -114,6 +114,8 @@ class IterativeLQR void setIterationCallback(const CallbackType& cb); + void reset(); + bool solve(int max_iter); const Eigen::MatrixXd& getStateTrajectory() const; diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 969c7e33..17fa0a97 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -32,7 +32,7 @@ def __init__(self, # save max iter if any self.max_iter = self.opts.get('ilqr.max_iter', 100) - + # num shooting interval self.N = prb.getNNodes() - 1 From 0b0397e68ddd2b30d598b6a8de56b2ee2afeeda5 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 7 Feb 2024 16:19:30 +0100 Subject: [PATCH 21/70] exposed ilqr reset method to horizon's solver interface --- horizon/rhc/taskInterface.py | 2 ++ horizon/solvers/ilqr.py | 4 ++++ horizon/solvers/solver.py | 9 +++++++-- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 40bd6494..1ab1c1c4 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -97,6 +97,8 @@ def reset(self): self.load_initial_guess() + self.solver_rti.reset() # resets solver internal state (useful in case of failure) + def rti(self): if self._debug: diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 17fa0a97..68f42a73 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -99,6 +99,10 @@ def __init__(self, self.set_iteration_callback(self._sol_info_callback) + def reset(self): + + self.ilqr.reset() + def save(self): data = self.prb.save() data['solver'] = dict() diff --git a/horizon/solvers/solver.py b/horizon/solvers/solver.py index 277b0f52..159284e4 100644 --- a/horizon/solvers/solver.py +++ b/horizon/solvers/solver.py @@ -56,7 +56,6 @@ def make_solver(cls, ret.type = type return ret - def __init__(self, prb: Problem, opts: Dict = None) -> None: @@ -99,7 +98,6 @@ def __init__(self, self.configure_rti() del self.opts['realtime_iteration'] - def _getVarList(self, type): var_list = list() for var in self.prb.var_container.getVarList(offset=False): @@ -292,6 +290,13 @@ def solve(self) -> bool: bool: success flag """ pass + + def reset(self): + """ + Resets solver (to be overridden by child class) + """ + + pass def getSolutionDict(self): """ From cada63993274a7cf0a10a0d471e76ed8f02ac384 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 7 Feb 2024 16:55:14 +0100 Subject: [PATCH 22/70] added missing rti option passed to ilqr solver --- horizon/rhc/taskInterface.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 1ab1c1c4..ec7e6a29 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -337,6 +337,8 @@ def _create_solver(self, rti=True): scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically # allows to retrieve costs and constraints values at runtime) + scoped_opts_rti['ilqr.rti'] = True + if self.max_solver_iter == 1: # real-time iteration -> no line-search necessary From b96653c0947e53610db8dd35c41f4d95ada709fd Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 7 Feb 2024 19:01:19 +0100 Subject: [PATCH 23/70] exposed residual norm1 getter --- horizon/cpp/pyilqr.cpp | 1 + horizon/cpp/src/ilqr.cpp | 6 ++++++ horizon/cpp/src/ilqr.h | 4 +++- horizon/solvers/ilqr.py | 4 ++++ 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/horizon/cpp/pyilqr.cpp b/horizon/cpp/pyilqr.cpp index 2f15ec9d..c7ba96ad 100644 --- a/horizon/cpp/pyilqr.cpp +++ b/horizon/cpp/pyilqr.cpp @@ -50,6 +50,7 @@ PYBIND11_MODULE(pyilqr, m) { .def("getInputTrajectory", &IterativeLQR::getInputTrajectory) .def("getConstraintsValues", &IterativeLQR::getConstraintsValues) .def("getCostsValues", &IterativeLQR::getCostsValues) + .def("getResidualNorm", &IterativeLQR::getResidualNorm) .def("setInitialState", &IterativeLQR::setInitialState) .def("setInputInitialGuess", &IterativeLQR::setInputInitialGuess) .def("setStateInitialGuess", &IterativeLQR::setStateInitialGuess) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 030df708..48b41d8a 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -616,6 +616,12 @@ const std::map &IterativeLQR::getCostsValues() con return _cost_values; } +const float IterativeLQR::getResidualNorm() const +{ + return (_constraint_to_go->C()*_bp_res[0].dx + + _constraint_to_go->h()).lpNorm<1>(); +} + void IterativeLQR::reset() { _hxx_reg = _hxx_reg_base; diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index 76c3f0dc..a89d8aa6 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -113,7 +113,7 @@ class IterativeLQR void setInputInitialGuess(const Eigen::MatrixXd& u0); void setIterationCallback(const CallbackType& cb); - + void reset(); bool solve(int max_iter); @@ -130,6 +130,8 @@ class IterativeLQR const std::map& getCostsValues() const; + const float getResidualNorm() const; + VecConstRef state(int i) const; VecConstRef input(int i) const; diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 68f42a73..b67dd17b 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -179,6 +179,7 @@ def solve(self): self.solution_dict['opt_cost'] = self.iteration_costs[-1] if len(self.iteration_costs) else -1.0 self.solution_dict['iter_costs'] = np.array(self.iteration_costs) self.solution_dict['n_iter2sol'] = self.current_iteration + self.solution_dict['residual_norm'] = self.getResidualNorm() return ret @@ -208,6 +209,9 @@ def getConstraintsValues(self): def getCostsValues(self): return self.ilqr.getCostsValues() + def getResidualNorm(self): + return self.ilqr.getResidualNorm() + def print_timings(self): prof_info = self.ilqr.getProfilingInfo() From 6fc269d27888374e7406e6e2fcb8395a9ffb2d0e Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 10:51:38 +0100 Subject: [PATCH 24/70] now logging iterations only if flag enabled --- horizon/cpp/src/ilqr.cpp | 1 + horizon/cpp/src/ilqr.h | 1 + horizon/cpp/src/ilqr_forward_pass.cpp | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 48b41d8a..1b7f8108 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -86,6 +86,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _verbose = value_or(opt, "ilqr.verbose", 0); _debug = value_or(opt, "ilqr.debug", 0); _log = value_or(opt, "ilqr.log", 0); + _log_iterations = value_or(opt, "ilqr.log_iterations", 0); _rti = value_or(opt, "ilqr.rti", 0); _step_length = value_or(opt, "ilqr.step_length", 1.0); diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index a89d8aa6..eb08ff22 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -290,6 +290,7 @@ class IterativeLQR bool _verbose; bool _debug; + bool _log_iterations; bool _log; bool _rti; diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index 83ddeb94..1e89a804 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -450,7 +450,9 @@ bool IterativeLQR::line_search(int iter) _utrj = _fp_res->utrj; // save result in history - _fp_res_history.push_back(*_fp_res); + if (_log_iterations) { + _fp_res_history.push_back(*_fp_res); + } // note: we should update the lag mult at the solution // by including the dx part From a1906f649943d7a979178c79fc897dc07a6a94a3 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 10:52:02 +0100 Subject: [PATCH 25/70] disabled ilqr iteration logging to avoid RAM saturation --- horizon/rhc/taskInterface.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index ec7e6a29..a07610ef 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -99,13 +99,14 @@ def reset(self): self.solver_rti.reset() # resets solver internal state (useful in case of failure) - def rti(self): + def rti(self, + idx: int): if self._debug: t = time.time() - check = self.solver_rti.solve() + check = self.solver_rti.solve(idx) if self._debug: @@ -339,6 +340,8 @@ def _create_solver(self, rti=True): scoped_opts_rti['ilqr.rti'] = True + scoped_opts_rti['ilqr.log_iterations'] = False # debugging iLQR logs + if self.max_solver_iter == 1: # real-time iteration -> no line-search necessary From 17ff2ee33941cc250d4b966414f3902e64c2f001 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 11:03:30 +0100 Subject: [PATCH 26/70] removed memory debug changes --- horizon/rhc/taskInterface.py | 5 ++--- horizon/solvers/ilqr.py | 3 ++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index a07610ef..183b7ebb 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -99,14 +99,13 @@ def reset(self): self.solver_rti.reset() # resets solver internal state (useful in case of failure) - def rti(self, - idx: int): + def rti(self): if self._debug: t = time.time() - check = self.solver_rti.solve(idx) + check = self.solver_rti.solve() if self._debug: diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index b67dd17b..d0717e8c 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -99,6 +99,8 @@ def __init__(self, self.set_iteration_callback(self._sol_info_callback) + self._mem_usage = 0.0 + def reset(self): self.ilqr.reset() @@ -155,7 +157,6 @@ def solve(self): self._update_nodes() # solve - ret = self.ilqr.solve(self.max_iter) # get solution From fa833fe8452ff3002181ccaf176f592419c51b4b Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 11:25:13 +0100 Subject: [PATCH 27/70] exposed flag to suppress codegen generation/loading output --- horizon/cpp/src/codegen_function.cpp | 21 ++++++++++++++------- horizon/cpp/src/codegen_function.h | 3 ++- horizon/cpp/src/ilqr.cpp | 19 ++++++++++--------- horizon/cpp/src/ilqr.h | 3 ++- 4 files changed, 28 insertions(+), 18 deletions(-) diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 3abab605..6494577d 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -80,7 +80,8 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio -casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string dir) +casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string dir, + bool verbose) { // save cwd RestoreCwd rcwd = get_current_dir_name(); @@ -102,9 +103,11 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string std::string fname = f.name() + "_generated_" + std::to_string(hash); if(access((fname + ".so").c_str(), F_OK) == 0) - { - std::cout << "exists: loading " << fname << "... \n"; - + { + if (verbose) { + std::cout << "exists: loading " << fname << "... \n"; + } + auto handle = dlopen(("./" + fname + ".so").c_str(), RTLD_NOW); if(!handle) @@ -128,7 +131,9 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string // else, generate and compile f.generate(fname + ".c"); - std::cout << "not found: compiling " << fname << "... \n"; + if (verbose) { + std::cout << "not found: compiling " << fname << "... \n"; + } int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str()); @@ -138,8 +143,10 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string return f; } - std::cout << "loading " << fname << "... \n"; - + if (verbose) { + std::cout << "loading " << fname << "... \n"; + } + auto handle = dlopen(("./" + fname + ".so").c_str(), RTLD_NOW); if(!handle) diff --git a/horizon/cpp/src/codegen_function.h b/horizon/cpp/src/codegen_function.h index 171ede33..84ff9f4d 100644 --- a/horizon/cpp/src/codegen_function.h +++ b/horizon/cpp/src/codegen_function.h @@ -6,7 +6,8 @@ namespace horizon { namespace utils { casadi::Function codegen(const casadi::Function& f, - std::string dir="."); + std::string dir=".", + bool verbose = false); } } diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 1b7f8108..7d0a5a1b 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -87,6 +87,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _debug = value_or(opt, "ilqr.debug", 0); _log = value_or(opt, "ilqr.log", 0); _log_iterations = value_or(opt, "ilqr.log_iterations", 0); + _codegen_verbose = value_or(opt, "ilqr.codegen_verbose", 0); _rti = value_or(opt, "ilqr.rti", 0); _step_length = value_or(opt, "ilqr.step_length", 1.0); @@ -142,8 +143,8 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, // codegen if needed if(_codegen_enabled) { - fdyn = utils::codegen(fdyn, _codegen_workdir); - fdyn_jac = utils::codegen(fdyn_jac, _codegen_workdir); + fdyn = utils::codegen(fdyn, _codegen_workdir, _codegen_verbose); + fdyn_jac = utils::codegen(fdyn_jac, _codegen_workdir, _codegen_verbose); } for(auto& d : _dyn) @@ -255,9 +256,9 @@ void IterativeLQR::setCost(std::vector indices, const casadi::Function& int // codegen if required (we skip it for quadratic costs) if(_codegen_enabled) { - cost = utils::codegen(cost, _codegen_workdir); - grad = utils::codegen(grad, _codegen_workdir); - hess = utils::codegen(hess, _codegen_workdir); + cost = utils::codegen(cost, _codegen_workdir, _codegen_verbose); + grad = utils::codegen(grad, _codegen_workdir, _codegen_verbose); + hess = utils::codegen(hess, _codegen_workdir, _codegen_verbose); } c->setCost(cost, @@ -309,8 +310,8 @@ void IterativeLQR::setResidual(std::vector indices, // codegen if required (we skip it for quadratic costs) if(_codegen_enabled) { - res = utils::codegen(res, _codegen_workdir); - jac = utils::codegen(jac, _codegen_workdir); + res = utils::codegen(res, _codegen_workdir, _codegen_verbose); + jac = utils::codegen(jac, _codegen_workdir, _codegen_verbose); } // local syms to evaluate residual and jacobian @@ -410,8 +411,8 @@ void IterativeLQR::setConstraint(std::vector indices, if(_codegen_enabled) { - ic_fn = utils::codegen(ic_fn, _codegen_workdir); - ic_jac = utils::codegen(ic_jac, _codegen_workdir); + ic_fn = utils::codegen(ic_fn, _codegen_workdir, _codegen_verbose); + ic_jac = utils::codegen(ic_jac, _codegen_workdir, _codegen_verbose); } c->setConstraint(ic_fn, diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index eb08ff22..b8227908 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -293,7 +293,8 @@ class IterativeLQR bool _log_iterations; bool _log; bool _rti; - + bool _codegen_verbose; + const int _nx; const int _nu; const int _N; From 4ed4dd79bf994dadd0cc991624e8a7634d20c41a Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 11:58:01 +0100 Subject: [PATCH 28/70] removed prints in should stop is verbose is false --- horizon/cpp/src/ilqr_forward_pass.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index 1e89a804..de5a4a7e 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -502,14 +502,18 @@ bool IterativeLQR::should_stop() // is too close to zero if(std::fabs(_fp_res->f_der) < merit_der_threshold*(1 + _fp_res->cost)) { - std::cout << "exiting due to small merit derivative \n"; + if (_verbose) { + std::cout << "exiting due to small merit derivative \n"; + } return true; } // exit if step size (normalized) is too short if(_fp_res->step_length < step_length_threshold*(1 + _utrj.norm())) { - std::cout << "exiting due to small control increment \n"; + if (_verbose) { + std::cout << "exiting due to small control increment \n"; + } return true; } From 3f031f6c1679d9199f331738b5315acb77f4755c Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 11:58:31 +0100 Subject: [PATCH 29/70] exposed codegen dir argument --- horizon/rhc/taskInterface.py | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 183b7ebb..178566aa 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -1,3 +1,4 @@ +import code from horizon.utils import kin_dyn, mat_storer, resampler_trajectory from casadi_kin_dyn import pycasadi_kin_dyn @@ -29,10 +30,15 @@ def __init__(self, model, max_solver_iter: int = 1, debug: bool = False, - verbose: bool = False): + verbose: bool = False, + codegen_workdir: str = "/tmp/tyhio", + codegen_verbose: bool = False): self._debug = debug self._verbose = verbose + self._codegen_verbose = codegen_verbose + + self._codegen_workdir = codegen_workdir self.max_solver_iter = max_solver_iter @@ -322,6 +328,13 @@ def _create_solver(self, rti=True): th = Transcriptor.make_method('multiple_shooting', self.prb) # todo if receding is true .... + scoped_opts_bs = self.si.opts.copy() + scoped_opts_bs['ilqr.debug'] = self._debug + scoped_opts_bs['ilqr.verbose'] = self._verbose + scoped_opts_bs['ilqr.codegen_verbose'] = self._codegen_verbose + scoped_opts_bs['ilqr.log_iterations'] = False + scoped_opts_bs['ilqr.codegen_workdir'] = self._codegen_workdir + self.solver_bs = Solver.make_solver(self.si.type, self.prb, self.si.opts) try: @@ -330,19 +343,18 @@ def _create_solver(self, rti=True): pass if rti: + scoped_opts_rti = self.si.opts.copy() scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter - scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically # allows to retrieve costs and constraints values at runtime) - + scoped_opts_rti['ilqr.verbose'] = self._verbose + scoped_opts_rti['ilqr.codegen_verbose'] = self._codegen_verbose scoped_opts_rti['ilqr.rti'] = True - scoped_opts_rti['ilqr.log_iterations'] = False # debugging iLQR logs - + scoped_opts_rti['ilqr.codegen_workdir'] = self._codegen_workdir if self.max_solver_iter == 1: - # real-time iteration -> no line-search necessary scoped_opts_rti['ilqr.enable_line_search'] = False @@ -356,11 +368,15 @@ def __init__(self, model, max_solver_iter: int = 1, debug = False, - verbose = False): + verbose = False, + codegen_workdir: str = "/tmp/tyhio", + codegen_verbose: bool = False): super().__init__(prb, model, max_solver_iter, - debug, verbose) + debug, verbose, + codegen_workdir, + codegen_verbose) # here I register the the default tasks # todo: should I do it here? From 1350f51389621089ff079c3f1f181e24d3cb19cf Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 21 Feb 2024 14:27:40 +0100 Subject: [PATCH 30/70] Signed-off-by: Andrea Patrizi --- horizon/solvers/ilqr.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index d0717e8c..8f9edbcc 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -119,7 +119,7 @@ def set_iteration_callback(self, cb=None): if cb is None: self.ilqr.setIterationCallback(self._iter_callback) else: - print('setting custom iteration callback') + # print('setting custom iteration callback') self.ilqr.setIterationCallback(cb) def _sol_info_callback(self, fpres): From 4fcfa0ba888c35c6b29cf69ac87f6af6b42943bd Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Mon, 26 Feb 2024 13:08:36 +0100 Subject: [PATCH 31/70] fixed unused ilq config for bootstrap --- horizon/rhc/taskInterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 178566aa..4589ef5f 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -335,7 +335,7 @@ def _create_solver(self, rti=True): scoped_opts_bs['ilqr.log_iterations'] = False scoped_opts_bs['ilqr.codegen_workdir'] = self._codegen_workdir - self.solver_bs = Solver.make_solver(self.si.type, self.prb, self.si.opts) + self.solver_bs = Solver.make_solver(self.si.type, self.prb, scoped_opts_bs) try: self.solver_bs.set_iteration_callback() From de8be8d271ab14ef7fa53bf38c996aa600948b74 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 22 Mar 2024 16:51:23 +0100 Subject: [PATCH 32/70] removed unnecessary verbose arg --- horizon/rhc/taskInterface.py | 47 ++++++++++++++++-------------------- 1 file changed, 21 insertions(+), 26 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 4589ef5f..96dc18fe 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -30,13 +30,9 @@ def __init__(self, model, max_solver_iter: int = 1, debug: bool = False, - verbose: bool = False, - codegen_workdir: str = "/tmp/tyhio", - codegen_verbose: bool = False): + codegen_workdir: str = "/tmp/tyhio"): self._debug = debug - self._verbose = verbose - self._codegen_verbose = codegen_verbose self._codegen_workdir = codegen_workdir @@ -108,21 +104,23 @@ def reset(self): def rti(self): if self._debug: + self._rti_db() + else: + self._rti_min() + + def _rti_db(self): - t = time.time() - + t = time.time() check = self.solver_rti.solve() - - if self._debug: - - self.rt_solve_time = time.time() - t - - if self._debug and self._verbose: - - print(f'rti solved in {self.rt_solve_time} s') - + self.rt_solve_time = time.time() - t + print(f'rti solved in {self.rt_solve_time} s') self.solution = self.solver_rti.getSolutionDict() + return check + + def _rti_min(self): + check = self.solver_rti.solve() + self.solution = self.solver_rti.getSolutionDict() return check def init_inv_dyn_for_res(self): @@ -330,8 +328,8 @@ def _create_solver(self, rti=True): # todo if receding is true .... scoped_opts_bs = self.si.opts.copy() scoped_opts_bs['ilqr.debug'] = self._debug - scoped_opts_bs['ilqr.verbose'] = self._verbose - scoped_opts_bs['ilqr.codegen_verbose'] = self._codegen_verbose + scoped_opts_bs['ilqr.verbose'] = self._debug + scoped_opts_bs['ilqr.codegen_verbose'] = self._debug scoped_opts_bs['ilqr.log_iterations'] = False scoped_opts_bs['ilqr.codegen_workdir'] = self._codegen_workdir @@ -349,8 +347,8 @@ def _create_solver(self, rti=True): scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically # allows to retrieve costs and constraints values at runtime) - scoped_opts_rti['ilqr.verbose'] = self._verbose - scoped_opts_rti['ilqr.codegen_verbose'] = self._codegen_verbose + scoped_opts_rti['ilqr.verbose'] = self._debug + scoped_opts_rti['ilqr.codegen_verbose'] = self._debug scoped_opts_rti['ilqr.rti'] = True scoped_opts_rti['ilqr.log_iterations'] = False # debugging iLQR logs scoped_opts_rti['ilqr.codegen_workdir'] = self._codegen_workdir @@ -368,15 +366,12 @@ def __init__(self, model, max_solver_iter: int = 1, debug = False, - verbose = False, - codegen_workdir: str = "/tmp/tyhio", - codegen_verbose: bool = False): + codegen_workdir: str = "/tmp/tyhio"): super().__init__(prb, model, max_solver_iter, - debug, verbose, - codegen_workdir, - codegen_verbose) + debug, + codegen_workdir) # here I register the the default tasks # todo: should I do it here? From 10678f2338c6687db82c163bf2a5b2b3411a6e5d Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 25 Mar 2024 12:05:15 +0100 Subject: [PATCH 33/70] added back verbosity flag --- horizon/rhc/taskInterface.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 96dc18fe..5c8e7d8e 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -30,9 +30,11 @@ def __init__(self, model, max_solver_iter: int = 1, debug: bool = False, + verbose: bool = False, codegen_workdir: str = "/tmp/tyhio"): self._debug = debug + self._verbose = verbose self._codegen_workdir = codegen_workdir @@ -328,8 +330,8 @@ def _create_solver(self, rti=True): # todo if receding is true .... scoped_opts_bs = self.si.opts.copy() scoped_opts_bs['ilqr.debug'] = self._debug - scoped_opts_bs['ilqr.verbose'] = self._debug - scoped_opts_bs['ilqr.codegen_verbose'] = self._debug + scoped_opts_bs['ilqr.verbose'] = self._verbose + scoped_opts_bs['ilqr.codegen_verbose'] = self._verbose scoped_opts_bs['ilqr.log_iterations'] = False scoped_opts_bs['ilqr.codegen_workdir'] = self._codegen_workdir @@ -347,7 +349,7 @@ def _create_solver(self, rti=True): scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically # allows to retrieve costs and constraints values at runtime) - scoped_opts_rti['ilqr.verbose'] = self._debug + scoped_opts_rti['ilqr.verbose'] = self._verbose scoped_opts_rti['ilqr.codegen_verbose'] = self._debug scoped_opts_rti['ilqr.rti'] = True scoped_opts_rti['ilqr.log_iterations'] = False # debugging iLQR logs @@ -366,11 +368,13 @@ def __init__(self, model, max_solver_iter: int = 1, debug = False, + verbose = False, codegen_workdir: str = "/tmp/tyhio"): super().__init__(prb, model, max_solver_iter, debug, + verbose, codegen_workdir) # here I register the the default tasks From 90a1aaeccc6701f958e7deb2b7ccc9e2c70dfca6 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 25 Mar 2024 12:08:24 +0100 Subject: [PATCH 34/70] Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 5c8e7d8e..d28fecca 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -105,13 +105,13 @@ def reset(self): def rti(self): - if self._debug: + if self._verbose: self._rti_db() else: self._rti_min() def _rti_db(self): - + t = time.time() check = self.solver_rti.solve() self.rt_solve_time = time.time() - t From bfc121555a2dd9eb1798bfffa66aebd3085c7eb1 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 25 Mar 2024 12:10:03 +0100 Subject: [PATCH 35/70] removed constr viol db print if not in verbose mode --- horizon/cpp/src/ilqr_backward_pass.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/cpp/src/ilqr_backward_pass.cpp b/horizon/cpp/src/ilqr_backward_pass.cpp index 5ff37ba7..a3403c51 100644 --- a/horizon/cpp/src/ilqr_backward_pass.cpp +++ b/horizon/cpp/src/ilqr_backward_pass.cpp @@ -61,7 +61,7 @@ void IterativeLQR::backward_pass() // infeasible warning if(residual.lpNorm<1>() > 1e-8) { - if (_debug) { + if (_debug && _verbose) { std::cout << "warn at k = 0: " << _constraint_to_go->dim() << " constraints not satified, residual inf-norm is " << From f5f74a602d3791af81e146550c920da8a6590b82 Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Tue, 16 Apr 2024 16:03:48 +0200 Subject: [PATCH 36/70] added vector for holding tot cost on each node --- horizon/cpp/src/ilqr.cpp | 2 ++ horizon/cpp/src/ilqr.h | 1 + horizon/cpp/src/ilqr_forward_pass.cpp | 10 ++++++---- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 7d0a5a1b..cc7f29b8 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -110,6 +110,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _step_length_threshold = value_or(opt, "ilqr.step_length_threshold", 1e-9); _closed_loop_forward_pass = value_or(opt, "ilqr.closed_loop_forward_pass", 1); _codegen_workdir = value_or(opt, "ilqr.codegen_workdir", "/tmp"); + _codegen_enabled = value_or(opt, "ilqr.codegen_enabled", 0); _enable_line_search = value_or(opt, "ilqr.enable_line_search", 1); @@ -1361,6 +1362,7 @@ IterativeLQR::ForwardPassResult::ForwardPassResult(int nx, int nu, int N): utrj.setZero(nu, N); merit = 0.0; step_length = 0.0; + cost_values.setZero(N+1); constraint_values.setZero(N+1); defect_values.setZero(nx, N); diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index b8227908..3a7a7d51 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -162,6 +162,7 @@ class IterativeLQR int iter; bool accepted; + Eigen::VectorXd cost_values; Eigen::VectorXd constraint_values; Eigen::MatrixXd defect_values; diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index de5a4a7e..a5b0053e 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -202,10 +202,11 @@ double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::Matr // intermediate cost for(int i = 0; i < _N; i++) { - cost += _cost[i].evaluate(xtrj.col(i), utrj.col(i), i); - + _fp_res->cost_values[i] = _cost[i].evaluate(xtrj.col(i), utrj.col(i), i); + cost += _fp_res->cost_values[i]; + if (_debug) { - // optionally (TBD) save values of single costs acting on this node + // optionally save values of single costs acting on this node for(auto it : _cost[i].items) { // not updating items uninitialized (item vs item_cost) @@ -221,7 +222,8 @@ double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::Matr // add final cost // note: u not used // todo: enforce this! - cost += _cost[_N].evaluate(xtrj.col(_N), utrj.col(_N-1), _N); + _fp_res->cost_values[_N] = _cost[_N].evaluate(xtrj.col(_N), utrj.col(_N-1), _N); + cost += _fp_res->cost_values[_N]; return cost / _N; } From 35e2be402e432f854c75c106143f016b84b46fc3 Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Tue, 16 Apr 2024 16:18:02 +0200 Subject: [PATCH 37/70] added getters for tot costs and constr viol on nodes --- horizon/cpp/pyilqr.cpp | 2 ++ horizon/cpp/src/ilqr.cpp | 10 ++++++++++ horizon/cpp/src/ilqr.h | 6 ++++-- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/horizon/cpp/pyilqr.cpp b/horizon/cpp/pyilqr.cpp index c7ba96ad..f7eca490 100644 --- a/horizon/cpp/pyilqr.cpp +++ b/horizon/cpp/pyilqr.cpp @@ -51,6 +51,8 @@ PYBIND11_MODULE(pyilqr, m) { .def("getConstraintsValues", &IterativeLQR::getConstraintsValues) .def("getCostsValues", &IterativeLQR::getCostsValues) .def("getResidualNorm", &IterativeLQR::getResidualNorm) + .def("getConstrValOnNodes", &IterativeLQR::getConstrValOnNodes) + .def("getCostValOnNodes", &IterativeLQR::getCostValOnNodes) .def("setInitialState", &IterativeLQR::setInitialState) .def("setInputInitialGuess", &IterativeLQR::setInputInitialGuess) .def("setStateInitialGuess", &IterativeLQR::setStateInitialGuess) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index cc7f29b8..a498f990 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -609,11 +609,21 @@ const std::vector& IterativeLQR::getIterationHi return _fp_res_history; } +const Eigen::VectorXd &IterativeLQR::getConstrValOnNodes() const +{ + return _fp_res->constraint_values; +} + const std::map &IterativeLQR::getConstraintsValues() const { return _constr_values; } +const Eigen::VectorXd &IterativeLQR::getCostValOnNodes() const +{ + return _fp_res->cost_values; +} + const std::map &IterativeLQR::getCostsValues() const { return _cost_values; diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index 3a7a7d51..7d703ed0 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -126,8 +126,12 @@ class IterativeLQR const std::vector& getIterationHistory() const; + const Eigen::VectorXd& getCostValOnNodes() const; + const std::map& getConstraintsValues() const; + const Eigen::VectorXd& getConstrValOnNodes() const; + const std::map& getCostsValues() const; const float getResidualNorm() const; @@ -172,8 +176,6 @@ class IterativeLQR }; - - protected: private: From 336b47b737da9309d0b7216d0eb51e50d129e581 Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Fri, 19 Apr 2024 15:40:56 +0200 Subject: [PATCH 38/70] exposes methods for getting TOT cost and constr viol on nodes --- horizon/solvers/ilqr.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 8f9edbcc..9c4a9158 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -213,6 +213,12 @@ def getCostsValues(self): def getResidualNorm(self): return self.ilqr.getResidualNorm() + def getConstrValOnNodes(self): + return self.ilqr.getConstrValOnNodes() + + def getCostValOnNodes(self): + return self.ilqr.getCostValOnNodes() + def print_timings(self): prof_info = self.ilqr.getProfilingInfo() From 543a8f682780013abc8f60ef9f0f9f88134874e6 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 22 Jul 2024 10:38:45 +0200 Subject: [PATCH 39/70] removed arch spec. compilation flag to allow a bit more of cross compatibility of the generated libs (maybe) --- horizon/cpp/src/codegen_function.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 6494577d..2222c610 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -135,7 +135,9 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string std::cout << "not found: compiling " << fname << "... \n"; } - int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str()); + // int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str()); + // removed -march=native to allow (maybe) more cross compatibility + int ret = system(("clang -fPIC -shared -O3 " + fname + ".c -o " + fname + ".so").c_str()); if(ret != 0) { From 77ffe3fb7010d49dd741a9e7221a6a33ec7ea9d0 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 31 Jul 2024 20:10:33 +0200 Subject: [PATCH 40/70] added soft i state update method --- horizon/problem.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/horizon/problem.py b/horizon/problem.py index 9940e65e..085f4268 100644 --- a/horizon/problem.py +++ b/horizon/problem.py @@ -329,9 +329,20 @@ def getDt(self): raise ValueError('dt not defined, have you called setDt?') return self.dt - def setInitialState(self, x0: Iterable): + def setInitialState(self, x0: np.ndarray): self.getState().setBounds(lb=x0, ub=x0, nodes=0) + def setInitialStateSoft(self, x0_meas: np.ndarray, + x0_internal: np.ndarray): + + # set initial state with a "soft approach", which is useful when running a controller + # in closed loop to avoid issues + + lower_bound_relaxed=np.minimum(x0_meas,x0_internal) + upper_bound_relaxed=np.maximum(x0_meas,x0_internal) + # relax state bound on first node to allow some mismatch + self.getState().setBounds(lb=lower_bound_relaxed, ub=upper_bound_relaxed, nodes=0) + def getInitialState(self) -> np.array: lb, ub = self.getState().getBounds(node=0) if np.any(lb != ub): From 7e65699bb389ce7ee4c61f088792f678c8f59c13 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 5 Aug 2024 11:31:50 +0200 Subject: [PATCH 41/70] added sanity check on supported cartesian task types --- horizon/rhc/tasks/cartesianTask.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/horizon/rhc/tasks/cartesianTask.py b/horizon/rhc/tasks/cartesianTask.py index bf5c77d0..1af79198 100644 --- a/horizon/rhc/tasks/cartesianTask.py +++ b/horizon/rhc/tasks/cartesianTask.py @@ -269,6 +269,9 @@ def __initialize(self): self.ref = self.acc_tgt fun = ee_a[self.indices] - self.acc_tgt + else: + raise ValueError(f'Unsupported cartesian task type {self.cartesian_type}') + self.constr = self.instantiator( f'{frame_name}_cartesian_task', self.weight_param * fun, nodes=self.nodes) From 2d7cabf708308a85ff2e96d83ae87b32f7f2c4e4 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 5 Aug 2024 12:01:32 +0200 Subject: [PATCH 42/70] now always returning list, fixed handling of subtasks for contact task --- horizon/rhc/tasks/contactTask.py | 12 +++++++----- horizon/rhc/tasks/task.py | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/horizon/rhc/tasks/contactTask.py b/horizon/rhc/tasks/contactTask.py index a469ced8..928bb5be 100644 --- a/horizon/rhc/tasks/contactTask.py +++ b/horizon/rhc/tasks/contactTask.py @@ -12,9 +12,9 @@ def __init__(self, subtask, establish/break contact """ - self.dynamics_task: InteractionTask = Task.subtask_by_class(subtask, InteractionTask) + self.dynamics_tasks: InteractionTask = Task.subtask_by_class(subtask, InteractionTask) # allowed tasks are cartesian and rolling - self.kinematics_task: CartesianTask = Task.subtask_by_class(subtask, (CartesianTask, RollingTask)) # CartesianTask RollingTask + self.kinematics_tasks: CartesianTask = Task.subtask_by_class(subtask, (CartesianTask, RollingTask)) # CartesianTask RollingTask # initialize data class super().__init__(*args, **kwargs) @@ -26,6 +26,8 @@ def __initialize(self): self.setNodes(self.nodes) def setNodes(self, nodes, erasing=True): - - self.dynamics_task.setContact(nodes, erasing=erasing) # this is from taskInterface - self.kinematics_task.setNodes(nodes, erasing=erasing) # state + starting from node 1 # this is from taskInterface + + for task in self.dynamics_tasks: + task.setContact(nodes, erasing=erasing) # this is from taskInterface + for task in self.kinematics_tasks: + task.setNodes(nodes, erasing=erasing) # state + starting from node 1 # this is from taskInterface diff --git a/horizon/rhc/tasks/task.py b/horizon/rhc/tasks/task.py index 316d5263..11416615 100644 --- a/horizon/rhc/tasks/task.py +++ b/horizon/rhc/tasks/task.py @@ -42,7 +42,7 @@ def subtask_by_class(cls, subtask: Dict, classname: Union[Type, Tuple[Type]]) -> for _, v in subtask.items(): if isinstance(v, classname): ret.append(v) - return ret[0] if len(ret) == 1 else ret + return ret def __post_init__(self): # todo: this is for simplicity From 2962cd2064f4ca399ccebc228901370d1f89ee2a Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 8 Aug 2024 13:10:29 +0200 Subject: [PATCH 43/70] ilqr now allows to use float instead of double if required (casadi codegen still needs to be fixed) --- horizon/cpp/CMakeLists.txt | 5 + horizon/cpp/pyilqr_helpers.h | 4 +- horizon/cpp/pysqp_helpers.h | 106 +++++++------- horizon/cpp/src/codegen_function.cpp | 12 +- horizon/cpp/src/horizon_parser.cpp | 28 ++-- horizon/cpp/src/horizon_parser.h | 12 +- horizon/cpp/src/ilqr.cpp | 147 ++++++++++--------- horizon/cpp/src/ilqr.h | 174 +++++++++++----------- horizon/cpp/src/ilqr_backward_pass.cpp | 79 +++++----- horizon/cpp/src/ilqr_forward_pass.cpp | 102 ++++++------- horizon/cpp/src/ilqr_impl.h | 191 +++++++++++++------------ horizon/cpp/src/ilqr_test.cpp | 25 ++-- horizon/cpp/src/iterate_filter.cpp | 6 +- horizon/cpp/src/iterate_filter.h | 17 ++- horizon/cpp/src/profiling.cpp | 2 +- horizon/cpp/src/profiling.h | 8 +- horizon/cpp/src/sqp.cpp | 41 +++--- horizon/cpp/src/sqp.h | 96 +++++++------ horizon/cpp/src/sqp_test.cpp | 9 +- horizon/cpp/src/sqp_test2.cpp | 6 +- horizon/cpp/src/typedefs.h | 27 ++++ horizon/cpp/src/wrapped_function.cpp | 39 +++-- horizon/cpp/src/wrapped_function.h | 44 +++--- horizon/cpp/tests/testCasadiUtils.cpp | 43 +++--- horizon/cpp/tests/testIlqr.cpp | 9 +- horizon/cpp/tests/testQr.cpp | 39 ++--- horizon/problem.py | 7 + 27 files changed, 689 insertions(+), 589 deletions(-) create mode 100644 horizon/cpp/src/typedefs.h diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index 279af3ea..a0ee213d 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/horizon/cpp/CMakeLists.txt @@ -9,10 +9,15 @@ endif() # options option(HORIZON_PROFILING OFF "enable profiling features") +option(HORIZON_FLOAT32 OFF "use float32 for data representation") if(${HORIZON_PROFILING}) add_definitions(-DHORIZON_PROFILING) endif() +if(${HORIZON_FLOAT32}) + add_definitions(-DHORIZON_FLOAT32) +endif() + find_package(Eigen3 REQUIRED) find_package(casadi 3.5.5 REQUIRED) diff --git a/horizon/cpp/pyilqr_helpers.h b/horizon/cpp/pyilqr_helpers.h index 84940da6..17aa7c52 100644 --- a/horizon/cpp/pyilqr_helpers.h +++ b/horizon/cpp/pyilqr_helpers.h @@ -8,6 +8,8 @@ #include #include +#include "typedefs.h" + namespace py = pybind11; using namespace horizon; @@ -56,7 +58,7 @@ auto set_final_constraint_wrapper(IterativeLQR& self, py::object pyfn) auto set_inter_constraint_wrapper_single(IterativeLQR& self, std::vector k, py::object f, - std::vector tgt) + std::vector tgt) { self.setConstraint(k, to_cpp(f), tgt); } diff --git a/horizon/cpp/pysqp_helpers.h b/horizon/cpp/pysqp_helpers.h index b7650fdd..c6a6cff7 100644 --- a/horizon/cpp/pysqp_helpers.h +++ b/horizon/cpp/pysqp_helpers.h @@ -9,11 +9,11 @@ #include #include #include +#include "typedefs.h" namespace py = pybind11; using namespace horizon; - py::dict get_qpoases_options_mpc() { py::dict opts; @@ -21,13 +21,13 @@ py::dict get_qpoases_options_mpc() opts["initialStatusBounds"] = "inactive"; opts["numRefinementSteps"] = 0; opts["enableDriftCorrection"] = 0; - opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); + opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); opts["enableFlippingBounds"] = false; opts["enableNZCTests"] = false; opts["enableRamping"] = false; opts["enableRegularisation"] = true; opts["numRegularisationSteps"] = 2; - opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); + opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); return opts; } @@ -38,7 +38,7 @@ py::dict get_qpoases_options_reliable() opts["enableEqualities"] = false; opts["numRefinementSteps"] = 2; opts["enableFullLITest"] = true; - opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); + opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); opts["maxDualJump"] = 10e8; opts["enableCholeskyRefactorisation"] = 1; return opts; @@ -61,15 +61,15 @@ bool setOption(const std::string& key, const std::string& solver_key, py::handle bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict) { // -- sqp options --// - if(setOption (key, "beta", value, dict)) return true; - if(setOption (key, "eps_regularization", value, dict)) return true; - if(setOption (key, "alpha_min", value, dict)) return true; + if(setOption (key, "beta", value, dict)) return true; + if(setOption (key, "eps_regularization", value, dict)) return true; + if(setOption (key, "alpha_min", value, dict)) return true; if(setOption (key, "max_iter", value, dict)) return true; if(setOption (key, "reinitialize_qpsolver", value, dict)) return true; - if(setOption (key, "merit_derivative_tolerance", value, dict)) return true; - if(setOption (key, "merit_eps", value, dict)) return true; - if(setOption (key, "constraint_violation_tolerance", value, dict)) return true; - if(setOption (key, "solution_convergence", value, dict)) return true; + if(setOption (key, "merit_derivative_tolerance", value, dict)) return true; + if(setOption (key, "merit_eps", value, dict)) return true; + if(setOption (key, "constraint_violation_tolerance", value, dict)) return true; + if(setOption (key, "solution_convergence", value, dict)) return true; if(setOption (key, "use_golden_ratio_update", value, dict)) return true; // -- qpoases options --// if(setOption (key, "sparse", value, dict)) return true; @@ -78,7 +78,7 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict) if(setOption (key, "max_schur", value, dict)) return true; if(setOption (key, "linsol_plugin", value, dict)) return true; if(setOption (key, "nWSR", value, dict)) return true; - if(setOption (key, "CPUtime", value, dict)) return true; + if(setOption (key, "CPUtime", value, dict)) return true; if(setOption (key, "printLevel", value, dict)) return true; if(setOption (key, "enableRamping", value, dict)) return true; if(setOption (key, "enableFarBounds", value, dict)) return true; @@ -89,42 +89,42 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict) if(setOption (key, "enableDriftCorrection", value, dict)) return true; if(setOption (key, "enableCholeskyRefactorisation", value, dict)) return true; if(setOption (key, "enableEqualities", value, dict)) return true; - if(setOption (key, "terminationTolerance", value, dict)) return true; - if(setOption (key, "boundTolerance", value, dict)) return true; - if(setOption (key, "boundRelaxation", value, dict)) return true; - if(setOption (key, "epsNum", value, dict)) return true; - if(setOption (key, "epsDen", value, dict)) return true; - if(setOption (key, "maxPrimalJump", value, dict)) return true; - if(setOption (key, "maxDualJump", value, dict)) return true; - if(setOption (key, "initialRamping", value, dict)) return true; - if(setOption (key, "finalRamping", value, dict)) return true; - if(setOption (key, "initialFarBounds", value, dict)) return true; - if(setOption (key, "growFarBounds", value, dict)) return true; + if(setOption (key, "terminationTolerance", value, dict)) return true; + if(setOption (key, "boundTolerance", value, dict)) return true; + if(setOption (key, "boundRelaxation", value, dict)) return true; + if(setOption (key, "epsNum", value, dict)) return true; + if(setOption (key, "epsDen", value, dict)) return true; + if(setOption (key, "maxPrimalJump", value, dict)) return true; + if(setOption (key, "maxDualJump", value, dict)) return true; + if(setOption (key, "initialRamping", value, dict)) return true; + if(setOption (key, "finalRamping", value, dict)) return true; + if(setOption (key, "initialFarBounds", value, dict)) return true; + if(setOption (key, "growFarBounds", value, dict)) return true; if(setOption (key, "initialStatusBounds", value, dict)) return true; - if(setOption (key, "epsFlipping", value, dict)) return true; + if(setOption (key, "epsFlipping", value, dict)) return true; if(setOption (key, "numRegularisationSteps", value, dict)) return true; - if(setOption (key, "epsRegularisation", value, dict)) return true; + if(setOption (key, "epsRegularisation", value, dict)) return true; if(setOption (key, "numRefinementSteps", value, dict)) return true; - if(setOption (key, "epsIterRef", value, dict)) return true; - if(setOption (key, "epsLITests", value, dict)) return true; - if(setOption (key, "epsNZCTests", value, dict)) return true; + if(setOption (key, "epsIterRef", value, dict)) return true; + if(setOption (key, "epsLITests", value, dict)) return true; + if(setOption (key, "epsNZCTests", value, dict)) return true; if(setOption (key, "enableInertiaCorrection", value, dict)) return true; // -- osqp options --// if(setOption (key, "warm_start_primal", value, dict)) return true; if(setOption (key, "warm_start_dual", value, dict)) return true; - if(setOption (key, "osqp.rho", value, dict)) return true; - if(setOption (key, "osqp.sigma", value, dict)) return true; + if(setOption (key, "osqp.rho", value, dict)) return true; + if(setOption (key, "osqp.sigma", value, dict)) return true; if(setOption (key, "osqp.scaling", value, dict)) return true; if(setOption (key, "osqp.adaptive_rho", value, dict)) return true; if(setOption (key, "osqp.adaptive_rho_interval", value, dict)) return true; - if(setOption (key, "osqp.adaptive_rho_tolerance", value, dict)) return true; + if(setOption (key, "osqp.adaptive_rho_tolerance", value, dict)) return true; if(setOption (key, "osqp.max_iter", value, dict)) return true; - if(setOption (key, "osqp.eps_abs", value, dict)) return true; - if(setOption (key, "osqp.eps_rel", value, dict)) return true; - if(setOption (key, "osqp.eps_prim_inf", value, dict)) return true; - if(setOption (key, "osqp.eps_dual_inf", value, dict)) return true; - if(setOption (key, "osqp.alpha", value, dict)) return true; - if(setOption (key, "osqp.delta", value, dict)) return true; + if(setOption (key, "osqp.eps_abs", value, dict)) return true; + if(setOption (key, "osqp.eps_rel", value, dict)) return true; + if(setOption (key, "osqp.eps_prim_inf", value, dict)) return true; + if(setOption (key, "osqp.eps_dual_inf", value, dict)) return true; + if(setOption (key, "osqp.alpha", value, dict)) return true; + if(setOption (key, "osqp.delta", value, dict)) return true; if(setOption (key, "osqp.polish", value, dict)) return true; if(setOption (key, "osqp.polish_refine_iter", value, dict)) return true; if(setOption (key, "osqp.verbose", value, dict)) return true; @@ -189,10 +189,10 @@ bool gSX(SQPGaussNewton& self, py::object g, bool reinitialize_qp_so } auto callMX(SQPGaussNewton& self, - const Eigen::VectorXd& x0, - const Eigen::VectorXd& p, - const Eigen::VectorXd& lbx, const Eigen::VectorXd& ubx, - const Eigen::VectorXd& lbg, const Eigen::VectorXd& ubg) + const VectorXr& x0, + const VectorXr& p, + const VectorXr& lbx, const VectorXr& ubx, + const VectorXr& lbg, const VectorXr& ubg) { casadi::DM _x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_; casadi_utils::toCasadiMatrix(x0, _x0_); @@ -204,22 +204,22 @@ auto callMX(SQPGaussNewton& self, casadi::DMDict tmp = self.solve(_x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_); py::dict solution; - Eigen::VectorXd x; + VectorXr x; casadi_utils::toEigen(tmp.at("x"), x); solution["x"] = x; - solution["f"] = double(tmp.at("f")(0)); - solution["g"] = double(tmp.at("g")(0)); + solution["f"] = Real(tmp.at("f")(0)); + solution["g"] = Real(tmp.at("g")(0)); return solution; } auto callSX(SQPGaussNewton& self, - const Eigen::VectorXd& x0, - const Eigen::VectorXd& p, - const Eigen::VectorXd& lbx, - const Eigen::VectorXd& ubx, - const Eigen::VectorXd& lbg, - const Eigen::VectorXd& ubg) + const VectorXr& x0, + const VectorXr& p, + const VectorXr& lbx, + const VectorXr& ubx, + const VectorXr& lbg, + const VectorXr& ubg) { casadi::DM _x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_; casadi_utils::toCasadiMatrix(x0, _x0_); @@ -231,11 +231,11 @@ auto callSX(SQPGaussNewton& self, casadi::DMDict tmp = self.solve(_x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_); py::dict solution; - Eigen::VectorXd x; + VectorXr x; casadi_utils::toEigen(tmp.at("x"), x); solution["x"] = x; - solution["f"] = double(tmp.at("f")(0)); - solution["g"] = double(tmp.at("g")(0)); + solution["f"] = Real(tmp.at("f")(0)); + solution["g"] = Real(tmp.at("g")(0)); return solution; } diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 2222c610..70217ab4 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -8,10 +8,16 @@ #include #include "wrapped_function.h" +#include "typedefs.h" namespace { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using RInfinity=horizon::RInfinity; + class RestoreCwd { public: @@ -36,14 +42,14 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio casadi_utils::WrappedFunction fw = f; casadi_utils::WrappedFunction gw = g; - std::vector input(f.n_in()); + std::vector input(f.n_in()); for(int iter = 0; iter < 10; iter++) { for(int i = 0; i < f.n_in(); i++) { - Eigen::VectorXd u; + VectorXr u; u.setRandom(f.size1_in(i)); input[i] = 100*u; } @@ -59,7 +65,7 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio for(int i = 0; i < f.n_out(); i++) { - double err = (fw.getOutput(i) - gw.getOutput(i)).lpNorm(); + Real err = (fw.getOutput(i) - gw.getOutput(i)).lpNorm(); if(err > 1e-16) { diff --git a/horizon/cpp/src/horizon_parser.cpp b/horizon/cpp/src/horizon_parser.cpp index 93645e7e..c1613e2c 100644 --- a/horizon/cpp/src/horizon_parser.cpp +++ b/horizon/cpp/src/horizon_parser.cpp @@ -15,13 +15,13 @@ Problem::Variable::Ptr Problem::yaml_to_variable(YAML::Node item) try { - auto lb = var_data["lb"].as>(); - auto ub = var_data["ub"].as>(); - auto ini = var_data["initial_guess"].as>(); + auto lb = var_data["lb"].as>(); + auto ub = var_data["ub"].as>(); + auto ini = var_data["initial_guess"].as>(); - v->lb = Eigen::MatrixXd::Map(lb.data(), size, lb.size()/size); - v->ub = Eigen::MatrixXd::Map(ub.data(), size, ub.size()/size); - v->initial_guess = Eigen::MatrixXd::Map(ini.data(), size, ini.size()/size); + v->lb = MatrixXr::Map(lb.data(), size, lb.size()/size); + v->ub = MatrixXr::Map(ub.data(), size, ub.size()/size); + v->initial_guess = MatrixXr::Map(ini.data(), size, ini.size()/size); } catch(YAML::Exception&) { @@ -30,8 +30,8 @@ Problem::Variable::Ptr Problem::yaml_to_variable(YAML::Node item) try { - auto values = var_data["values"].as>(); - v->value = Eigen::MatrixXd::Map(values.data(), size, values.size()/size); + auto values = var_data["values"].as>(); + v->value = MatrixXr::Map(values.data(), size, values.size()/size); } catch(YAML::Exception&) { @@ -90,11 +90,11 @@ Problem::Function::Ptr Problem::yaml_to_function(std::pair>(); - auto ub = var_data["ub"].as>(); + auto lb = var_data["lb"].as>(); + auto ub = var_data["ub"].as>(); - fun->lb = Eigen::MatrixXd::Map(lb.data(), f.size1_out(0), lb.size()/f.size1_out(0)); - fun->ub = Eigen::MatrixXd::Map(ub.data(), f.size1_out(0), ub.size()/f.size1_out(0)); + fun->lb = MatrixXr::Map(lb.data(), f.size1_out(0), lb.size()/f.size1_out(0)); + fun->ub = MatrixXr::Map(ub.data(), f.size1_out(0), ub.size()/f.size1_out(0)); } catch(YAML::Exception& e) { @@ -113,8 +113,8 @@ void Problem::from_yaml(YAML::Node problem_yaml) // dt try { - dt = problem_yaml["solver"]["dt"].as(); - std::cout << "problem_yaml[solver][dt].as() = " << dt << "\n"; + dt = problem_yaml["solver"]["dt"].as(); + std::cout << "problem_yaml[solver][dt].as() = " << dt << "\n"; } catch(YAML::Exception& e) { diff --git a/horizon/cpp/src/horizon_parser.h b/horizon/cpp/src/horizon_parser.h index ebfdacfa..7d2201b7 100644 --- a/horizon/cpp/src/horizon_parser.h +++ b/horizon/cpp/src/horizon_parser.h @@ -9,12 +9,16 @@ #include #include "wrapped_function.h" +#include "typedefs.h" namespace horizon { class Problem { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; public: @@ -30,7 +34,7 @@ class Problem std::string name; casadi::SX sym; - Eigen::MatrixXd lb, ub, value, initial_guess; + MatrixXr lb, ub, value, initial_guess; int size(); }; @@ -41,7 +45,7 @@ class Problem std::string name; casadi::Function fun; - Eigen::MatrixXd lb, ub; + MatrixXr lb, ub; std::vector nodes; }; @@ -52,14 +56,14 @@ class Problem std::vector state_vec, input_vec; casadi::SX x, u; - Eigen::MatrixXd xlb, xub, ulb, uub, x_ini, u_ini; + MatrixXr xlb, xub, ulb, uub, x_ini, u_ini; casadi::Function dynamics; casadi_utils::WrappedFunction inv_dyn; int N; - double dt; + Real dt; private: diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index a498f990..1b24df6a 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -3,8 +3,7 @@ #include #include - -utils::Timer::TocCallback on_timer_toc = [](const char*, double){}; +utils::Timer::TocCallback on_timer_toc = [](const char*, Real){}; template std::type_info const& var_type(V const& v){ @@ -36,7 +35,7 @@ T value_or(const IterativeLQR::OptionDict& opt, std::string key, T dfl) } } -void set_param_inputs(std::shared_ptr> params, +void set_param_inputs(std::shared_ptr> params, int k, casadi_utils::WrappedFunction& f) @@ -89,33 +88,33 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _log_iterations = value_or(opt, "ilqr.log_iterations", 0); _codegen_verbose = value_or(opt, "ilqr.codegen_verbose", 0); _rti = value_or(opt, "ilqr.rti", 0); - _step_length = value_or(opt, "ilqr.step_length", 1.0); + _step_length = value_or(opt, "ilqr.step_length", 1.0); - _rho_base = value_or(opt, "ilqr.rho_base", 0.0); + _rho_base = value_or(opt, "ilqr.rho_base", 0.0); _rho = _rho_base; - _rho_growth_factor = value_or(opt, "ilqr.rho_growth_factor", 10.0); + _rho_growth_factor = value_or(opt, "ilqr.rho_growth_factor", 10.0); _enable_auglag = value_or(opt, "ilqr.enable_auglag", 0); - _hxx_reg = value_or(opt, "ilqr.hxx_reg", 0.0); - _hxx_reg_base = value_or(opt, "ilqr.hxx_reg_base", 0.0); - _hxx_reg_growth_factor = value_or(opt, "ilqr.hxx_reg_growth_factor", 1e3); - _huu_reg = value_or(opt, "ilqr.huu_reg", 0.0); - _kkt_reg = value_or(opt, "ilqr.kkt_reg", 0.0); - _line_search_accept_ratio = value_or(opt, "ilqr.line_search_accept_ratio", 1e-4); - _alpha_min = value_or(opt, "ilqr.alpha_min", 1e-3); - _svd_threshold = value_or(opt, "ilqr.svd_threshold", 1e-6); - _constraint_violation_threshold = value_or(opt, "ilqr.constraint_violation_threshold", 1e-6); - _defect_norm_threshold = value_or(opt, "ilqr.defect_norm_threshold", 1e-6); - _merit_der_threshold = value_or(opt, "ilqr.merit_der_threshold", 1e-3); - _step_length_threshold = value_or(opt, "ilqr.step_length_threshold", 1e-9); + _hxx_reg = value_or(opt, "ilqr.hxx_reg", 0.0); + _hxx_reg_base = value_or(opt, "ilqr.hxx_reg_base", 0.0); + _hxx_reg_growth_factor = value_or(opt, "ilqr.hxx_reg_growth_factor", 1e3); + _huu_reg = value_or(opt, "ilqr.huu_reg", 0.0); + _kkt_reg = value_or(opt, "ilqr.kkt_reg", 0.0); + _line_search_accept_ratio = value_or(opt, "ilqr.line_search_accept_ratio", 1e-4); + _alpha_min = value_or(opt, "ilqr.alpha_min", 1e-3); + _svd_threshold = value_or(opt, "ilqr.svd_threshold", 1e-6); + _constraint_violation_threshold = value_or(opt, "ilqr.constraint_violation_threshold", 1e-6); + _defect_norm_threshold = value_or(opt, "ilqr.defect_norm_threshold", 1e-6); + _merit_der_threshold = value_or(opt, "ilqr.merit_der_threshold", 1e-3); + _step_length_threshold = value_or(opt, "ilqr.step_length_threshold", 1e-9); _closed_loop_forward_pass = value_or(opt, "ilqr.closed_loop_forward_pass", 1); _codegen_workdir = value_or(opt, "ilqr.codegen_workdir", "/tmp"); _codegen_enabled = value_or(opt, "ilqr.codegen_enabled", 0); _enable_line_search = value_or(opt, "ilqr.enable_line_search", 1); - _it_filt.beta = value_or(opt, "ilqr.filter_beta", 0.99); - _it_filt.gamma = value_or(opt, "ilqr.filter_gamma", 0.20); + _it_filt.beta = value_or(opt, "ilqr.filter_beta", 0.99); + _it_filt.gamma = value_or(opt, "ilqr.filter_gamma", 0.20); _use_it_filter = value_or(opt, "ilqr.use_filter", 0); auto decomp_type_str = value_or(opt, "ilqr.constr_decomp_type", "qr"); @@ -130,7 +129,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _hxx_reg = std::max(_hxx_reg_base, _hxx_reg); // set timer callback - on_timer_toc = [this](const char * name, double usec) + on_timer_toc = [this](const char * name, Real usec) { _prof_info.timings[name].push_back(usec); }; @@ -200,7 +199,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, } } -void IterativeLQR::setStateBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub) +void IterativeLQR::setStateBounds(const MatrixXr& lb, const MatrixXr& ub) { if(_x_lb.rows() != lb.rows() || _x_lb.cols() != lb.cols() || _x_ub.rows() != ub.rows() || _x_ub.cols() != ub.cols() @@ -216,7 +215,7 @@ void IterativeLQR::setStateBounds(const Eigen::MatrixXd& lb, const Eigen::Matrix } -void IterativeLQR::setInputBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub) +void IterativeLQR::setInputBounds(const MatrixXr& lb, const MatrixXr& ub) { if(_u_lb.rows() != lb.rows() || _u_lb.cols() != lb.cols() || _u_ub.rows() != ub.rows() || _u_ub.cols() != ub.cols() @@ -238,7 +237,7 @@ void IterativeLQR::setCost(std::vector indices, const casadi::Function& int auto c = std::make_shared(); // initialize constraint value map - _cost_values[inter_cost.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); + _cost_values[inter_cost.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); // add to map _cost_map[inter_cost.name()] = c; @@ -293,7 +292,7 @@ void IterativeLQR::setResidual(std::vector indices, // create cost entity auto c = std::make_shared(); -// _cost_values[residual.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); +// _cost_values[residual.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); // add to map _cost_map[residual.name()] = c; @@ -360,7 +359,7 @@ void IterativeLQR::setResidual(std::vector indices, c->setCost(cost_fn, grad_fn, hess_fn); - _cost_values[residual.name() + "_cost"].setConstant(_N, std::numeric_limits::quiet_NaN()); + _cost_values[residual.name() + "_cost"].setConstant(_N, std::numeric_limits::quiet_NaN()); if(_verbose) std::cout << "adding residual '" << residual << "' at k = "; @@ -386,7 +385,7 @@ void IterativeLQR::setFinalCost(const casadi::Function &final_cost) void IterativeLQR::setConstraint(std::vector indices, const casadi::Function &inter_constraint, - std::vector target_values) + std::vector target_values) { // add parameters to param_map add_param_to_map(inter_constraint); @@ -399,7 +398,7 @@ void IterativeLQR::setConstraint(std::vector indices, // initialize constraint value map int constr_dim = inter_constraint.size1_out(0); - _constr_values[inter_constraint.name()].setConstant(constr_dim, _N, std::numeric_limits::quiet_NaN()); + _constr_values[inter_constraint.name()].setConstant(constr_dim, _N, std::numeric_limits::quiet_NaN()); // set param map c->param = _param_map; @@ -519,7 +518,7 @@ void IterativeLQR::updateIndices() } } -void IterativeLQR::setParameterValue(const std::string& pname, const Eigen::MatrixXd& value) +void IterativeLQR::setParameterValue(const std::string& pname, const MatrixXr& value) { auto it = _param_map->find(pname); @@ -544,7 +543,7 @@ void IterativeLQR::setParameterValue(const std::string& pname, const Eigen::Matr it->second = value; } -void IterativeLQR::setInitialState(const Eigen::VectorXd &x0) +void IterativeLQR::setInitialState(const VectorXr &x0) { if(x0.size() != _nx) { @@ -554,7 +553,7 @@ void IterativeLQR::setInitialState(const Eigen::VectorXd &x0) _xtrj.col(0) = x0; } -void IterativeLQR::setStateInitialGuess(const Eigen::MatrixXd& x0) +void IterativeLQR::setStateInitialGuess(const MatrixXr& x0) { if(x0.rows() != _xtrj.rows()) { @@ -569,7 +568,7 @@ void IterativeLQR::setStateInitialGuess(const Eigen::MatrixXd& x0) _xtrj = x0; } -void IterativeLQR::setInputInitialGuess(const Eigen::MatrixXd &u0) +void IterativeLQR::setInputInitialGuess(const MatrixXr &u0) { if(u0.rows() != _utrj.rows()) { @@ -589,12 +588,12 @@ void IterativeLQR::setIterationCallback(const CallbackType &cb) _iter_cb = cb; } -const Eigen::MatrixXd &IterativeLQR::getStateTrajectory() const +const MatrixXr &IterativeLQR::getStateTrajectory() const { return _xtrj; } -const Eigen::MatrixXd &IterativeLQR::getInputTrajectory() const +const MatrixXr &IterativeLQR::getInputTrajectory() const { return _utrj; } @@ -609,22 +608,22 @@ const std::vector& IterativeLQR::getIterationHi return _fp_res_history; } -const Eigen::VectorXd &IterativeLQR::getConstrValOnNodes() const +const VectorXr &IterativeLQR::getConstrValOnNodes() const { return _fp_res->constraint_values; } -const std::map &IterativeLQR::getConstraintsValues() const +const std::map &IterativeLQR::getConstraintsValues() const { return _constr_values; } -const Eigen::VectorXd &IterativeLQR::getCostValOnNodes() const +const VectorXr &IterativeLQR::getCostValOnNodes() const { return _fp_res->cost_values; } -const std::map &IterativeLQR::getCostsValues() const +const std::map &IterativeLQR::getCostsValues() const { return _cost_values; } @@ -821,7 +820,7 @@ void IterativeLQR::report_result(const IterativeLQR::ForwardPassResult& fpres) void IterativeLQR::set_default_cost() { - const double dfl_cost_weight = 1e-160; + const Real dfl_cost_weight = 1e-160; auto x = cs::SX::sym("x", _nx); auto u = cs::SX::sym("u", _nu); @@ -936,7 +935,7 @@ void IterativeLQR::add_param_to_map(const casadi::Function& f) // add to map (*_param_map)[f.name_in(i)].setConstant(param_size, _N+1, - std::numeric_limits::quiet_NaN() + std::numeric_limits::quiet_NaN() ); if(_verbose) std::cout << "adding parameter '" << f.name_in(i) << "', " << @@ -944,12 +943,12 @@ void IterativeLQR::add_param_to_map(const casadi::Function& f) } } -const Eigen::MatrixXd &IterativeLQR::Dynamics::A() const +const MatrixXr &IterativeLQR::Dynamics::A() const { return df.getOutput(0); } -const Eigen::MatrixXd &IterativeLQR::Dynamics::B() const +const MatrixXr &IterativeLQR::Dynamics::B() const { return df.getOutput(1); } @@ -959,7 +958,7 @@ IterativeLQR::Dynamics::Dynamics(int nx, int) d.setZero(nx); } -Eigen::Ref IterativeLQR::Dynamics::integrate(VecConstRef x, +Eigen::Ref IterativeLQR::Dynamics::integrate(VecConstRef x, VecConstRef u, int k) { @@ -988,7 +987,7 @@ void IterativeLQR::Dynamics::computeDefect(VecConstRef x, VecConstRef u, VecConstRef xnext, int k, - Eigen::VectorXd& _d) + VectorXr& _d) { TIC(compute_defect_inner) @@ -1021,16 +1020,16 @@ IterativeLQR::BoundAuglagCostEntity::BoundAuglagCostEntity(int N, _ulam.setZero(ulb.size()); } -void IterativeLQR::BoundAuglagCostEntity::setRho(double rho) +void IterativeLQR::BoundAuglagCostEntity::setRho(Real rho) { _rho = rho; } -double IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, +Real IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, VecConstRef u, int k) { - double value = 0.0; + Real value = 0.0; // positive if ub violated, negtive if lb violated _x_violation = (x - _xub).cwiseMax(0) + (x - _xlb).cwiseMin(0); @@ -1052,9 +1051,9 @@ double IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, void IterativeLQR::BoundAuglagCostEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { evaluate(x, u, k); @@ -1115,17 +1114,17 @@ void IterativeLQR::IntermediateCostEntity::setCost(casadi::Function _l, ddl = _ddl; } -Eigen::Ref IterativeLQR::IntermediateCostEntity::q() const +Eigen::Ref IterativeLQR::IntermediateCostEntity::q() const { return dl.getOutput(0).col(0); } -Eigen::Ref IterativeLQR::IntermediateCostEntity::r() const +Eigen::Ref IterativeLQR::IntermediateCostEntity::r() const { return dl.getOutput(1).col(0); } -double IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, VecConstRef u, int k) { @@ -1143,9 +1142,9 @@ double IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, void IterativeLQR::IntermediateCostEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { #ifdef HORIZON_PROFILING horizon::utils::Timer tm("quadratize_" + l.function().name() + "_inner", @@ -1163,7 +1162,7 @@ void IterativeLQR::IntermediateCostEntity::quadratize(VecConstRef x, ddl.setInput(1, u); set_param_inputs(param, k, ddl); - std::vector> out = {Q, R, P}; + std::vector> out = {Q, R, P}; ddl.call_accumulate(out); } @@ -1200,7 +1199,7 @@ void IterativeLQR::IntermediateResidualEntity::setResidual(casadi::Function _res _r.setZero(nu); } -double IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, VecConstRef u, int k) { @@ -1217,9 +1216,9 @@ double IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, void IterativeLQR::IntermediateResidualEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { #ifdef HORIZON_PROFILING horizon::utils::Timer tm("quadratize_" + res.function().name() + "_inner", @@ -1265,17 +1264,17 @@ casadi::Function IterativeLQR::IntermediateResidualEntity::Jacobian(const casadi return f.factory(f.name() + "_jac", f.name_in(), {"jac:res:x", "jac:res:u"}); } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::Q() const +const MatrixXr& IterativeLQR::IntermediateCost::Q() const { return _Q; } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::R() const +const MatrixXr& IterativeLQR::IntermediateCost::R() const { return _R; } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::P() const +const MatrixXr& IterativeLQR::IntermediateCost::P() const { return _P; } @@ -1299,13 +1298,13 @@ IterativeLQR::IntermediateCost::IntermediateCost(int nx, int nu) _r.setZero(nu); } -double IterativeLQR::IntermediateCost::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateCost::evaluate(VecConstRef x, VecConstRef u, int k) { TIC(evaluate_cost_inner); - double cost = 0.0; + Real cost = 0.0; for(auto& it : items) { @@ -1479,7 +1478,7 @@ int IterativeLQR::ConstraintToGo::dim() const return _dim; } -Eigen::Ref IterativeLQR::ConstraintToGo::C() const +Eigen::Ref IterativeLQR::ConstraintToGo::C() const { return _C.topRows(_dim); } @@ -1489,22 +1488,22 @@ MatConstRef IterativeLQR::ConstraintToGo::D() const return _D.topRows(_dim); } -Eigen::Ref IterativeLQR::ConstraintToGo::h() const +Eigen::Ref IterativeLQR::ConstraintToGo::h() const { return _h.head(_dim); } -const Eigen::MatrixXd &IterativeLQR::ConstraintEntity::C() const +const MatrixXr &IterativeLQR::ConstraintEntity::C() const { return df.getOutput(0); } -const Eigen::MatrixXd &IterativeLQR::ConstraintEntity::D() const +const MatrixXr &IterativeLQR::ConstraintEntity::D() const { return df.getOutput(1); } -Eigen::Ref IterativeLQR::ConstraintEntity::h() const +Eigen::Ref IterativeLQR::ConstraintEntity::h() const { return _hvalue; } @@ -1571,7 +1570,7 @@ void IterativeLQR::ConstraintEntity::setConstraint(casadi::Function h, casadi::F _hdes.setZero(f.function().size1_out(0)); } -void IterativeLQR::ConstraintEntity::setTargetValue(const Eigen::VectorXd &hdes) +void IterativeLQR::ConstraintEntity::setTargetValue(const VectorXr &hdes) { if(hdes.size() != _hdes.size()) { @@ -1586,12 +1585,12 @@ casadi::Function IterativeLQR::ConstraintEntity::Jacobian(const casadi::Function return h.factory(h.name() + "_jac", h.name_in(), {"jac:h:x", "jac:h:u"}); } -const Eigen::MatrixXd &IterativeLQR::Constraint::C() const +const MatrixXr &IterativeLQR::Constraint::C() const { return _C; } -const Eigen::MatrixXd &IterativeLQR::Constraint::D() const +const MatrixXr &IterativeLQR::Constraint::D() const { return _D; } diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index 7d703ed0..b266c8a1 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -12,13 +12,13 @@ #include "profiling.h" #include "iterate_filter.h" +#include "typedefs.h" namespace horizon { -typedef Eigen::Ref VecConstRef; -typedef Eigen::Ref MatConstRef; - +typedef Eigen::Ref VecConstRef; +typedef Eigen::Ref MatConstRef; /** * @brief IterativeLQR implements a multiple-shooting variant of the @@ -48,7 +48,7 @@ class IterativeLQR */ typedef std::function CallbackType; - typedef std::variant OptionTypes; + typedef std::variant OptionTypes; typedef std::map OptionDict; @@ -63,9 +63,9 @@ class IterativeLQR OptionDict opt = OptionDict()); - void setStateBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub); + void setStateBounds(const MatrixXr& lb, const MatrixXr& ub); - void setInputBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub); + void setInputBounds(const MatrixXr& lb, const MatrixXr& ub); /** * @brief set an intermediate cost term for the k-th intermediate state, @@ -95,7 +95,7 @@ class IterativeLQR */ void setConstraint(std::vector indices, const casadi::Function& inter_constraint, - std::vector target_values = std::vector()); + std::vector target_values = std::vector()); void setFinalConstraint(const casadi::Function& final_constraint); @@ -104,13 +104,13 @@ class IterativeLQR void updateIndices(); - void setParameterValue(const std::string& pname, const Eigen::MatrixXd& value); + void setParameterValue(const std::string& pname, const MatrixXr& value); - void setInitialState(const Eigen::VectorXd& x0); + void setInitialState(const VectorXr& x0); - void setStateInitialGuess(const Eigen::MatrixXd& x0); + void setStateInitialGuess(const MatrixXr& x0); - void setInputInitialGuess(const Eigen::MatrixXd& u0); + void setInputInitialGuess(const MatrixXr& u0); void setIterationCallback(const CallbackType& cb); @@ -118,21 +118,21 @@ class IterativeLQR bool solve(int max_iter); - const Eigen::MatrixXd& getStateTrajectory() const; + const MatrixXr& getStateTrajectory() const; - const Eigen::MatrixXd& getInputTrajectory() const; + const MatrixXr& getInputTrajectory() const; const utils::ProfilingInfo& getProfilingInfo() const; const std::vector& getIterationHistory() const; - const Eigen::VectorXd& getCostValOnNodes() const; + const VectorXr& getCostValOnNodes() const; - const std::map& getConstraintsValues() const; + const std::map& getConstraintsValues() const; - const Eigen::VectorXd& getConstrValOnNodes() const; + const VectorXr& getConstrValOnNodes() const; - const std::map& getCostsValues() const; + const std::map& getCostsValues() const; const float getResidualNorm() const; @@ -146,29 +146,29 @@ class IterativeLQR struct ForwardPassResult { - Eigen::MatrixXd xtrj; - Eigen::MatrixXd utrj; - double hxx_reg; - double rho; - double alpha; - double cost; - double bound_violation; - double merit; - double armijo_merit; - double mu_f; - double mu_c; - double mu_b; - double f_der; - double merit_der; - double step_length; - double constraint_violation; - double defect_norm; + MatrixXr xtrj; + MatrixXr utrj; + Real hxx_reg; + Real rho; + Real alpha; + Real cost; + Real bound_violation; + Real merit; + Real armijo_merit; + Real mu_f; + Real mu_c; + Real mu_b; + Real f_der; + Real merit_der; + Real step_length; + Real constraint_violation; + Real defect_norm; int iter; bool accepted; - Eigen::VectorXd cost_values; - Eigen::VectorXd constraint_values; - Eigen::MatrixXd defect_values; + VectorXr cost_values; + VectorXr constraint_values; + MatrixXr defect_values; ForwardPassResult(int nx, int nu, int N); @@ -180,7 +180,7 @@ class IterativeLQR private: - static constexpr double inf = std::numeric_limits::infinity(); + static constexpr Real inf = std::numeric_limits::infinity(); struct ConstrainedDynamics; struct ConstrainedCost; @@ -201,7 +201,7 @@ class IterativeLQR typedef std::tuple HandleConstraintsRetType; - typedef std::shared_ptr> + typedef std::shared_ptr> ParameterMapPtr; typedef std::map> @@ -240,37 +240,37 @@ class IterativeLQR bool auglag_update(); - double compute_merit_value(double mu_f, - double mu_c, - double cost, - double defect_norm, - double constr_viol); + Real compute_merit_value(Real mu_f, + Real mu_c, + Real cost, + Real defect_norm, + Real constr_viol); - double compute_merit_slope(double cost_slope, - double mu_f, - double mu_c, - double defect_norm, - double constr_viol); + Real compute_merit_slope(Real cost_slope, + Real mu_f, + Real mu_c, + Real defect_norm, + Real constr_viol); - double compute_cost_slope(); + Real compute_cost_slope(); - std::pair compute_merit_weights(double cost_der, double defect_norm, double constr_viol); + std::pair compute_merit_weights(Real cost_der, Real defect_norm, Real constr_viol); - double compute_cost(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_cost(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_bound_penalty(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_bound_penalty(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_constr(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_constr(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_defect(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_defect(const MatrixXr& xtrj, + const MatrixXr& utrj); - bool forward_pass(double alpha); + bool forward_pass(Real alpha); - void forward_pass_iter(int i, double alpha); + void forward_pass_iter(int i, Real alpha); bool line_search(int iter); @@ -302,22 +302,22 @@ class IterativeLQR const int _nu; const int _N; - double _step_length; - double _rho_base; - double _rho; - double _rho_growth_factor; - double _hxx_reg; - double _hxx_reg_base; - double _hxx_reg_growth_factor; - double _huu_reg; - double _kkt_reg; - double _line_search_accept_ratio; - double _alpha_min; - double _svd_threshold; - double _constraint_violation_threshold; - double _defect_norm_threshold; - double _merit_der_threshold; - double _step_length_threshold; + Real _step_length; + Real _rho_base; + Real _rho; + Real _rho_growth_factor; + Real _hxx_reg; + Real _hxx_reg_base; + Real _hxx_reg_growth_factor; + Real _huu_reg; + Real _kkt_reg; + Real _line_search_accept_ratio; + Real _alpha_min; + Real _svd_threshold; + Real _constraint_violation_threshold; + Real _defect_norm_threshold; + Real _merit_der_threshold; + Real _step_length_threshold; bool _enable_line_search; bool _enable_auglag; @@ -334,8 +334,8 @@ class IterativeLQR std::vector> _auglag_cost; std::vector _cost; std::vector _constraint; - Eigen::MatrixXd _x_lb, _x_ub; - Eigen::MatrixXd _u_lb, _u_ub; + MatrixXr _x_lb, _x_ub; + MatrixXr _u_lb, _u_ub; std::vector _value; std::vector _dyn; @@ -347,13 +347,13 @@ class IterativeLQR IterateFilter _it_filt; bool _use_it_filter; - Eigen::MatrixXd _xtrj; - Eigen::MatrixXd _utrj; - std::vector _lam_g; - Eigen::MatrixXd _lam_x; + MatrixXr _xtrj; + MatrixXr _utrj; + std::vector _lam_g; + MatrixXr _lam_x; - Eigen::MatrixXd _lam_bound_x; - Eigen::MatrixXd _lam_bound_u; + MatrixXr _lam_bound_x; + MatrixXr _lam_bound_u; std::vector _tmp; @@ -371,8 +371,8 @@ class IterativeLQR std::vector _fp_res_history; - std::map _constr_values; - std::map _cost_values; + std::map _constr_values; + std::map _cost_values; }; diff --git a/horizon/cpp/src/ilqr_backward_pass.cpp b/horizon/cpp/src/ilqr_backward_pass.cpp index a3403c51..5e9d8cfc 100644 --- a/horizon/cpp/src/ilqr_backward_pass.cpp +++ b/horizon/cpp/src/ilqr_backward_pass.cpp @@ -1,4 +1,5 @@ #include "ilqr_impl.h" +#include "typedefs.h" struct HessianIndefinite : std::runtime_error { @@ -54,7 +55,7 @@ void IterativeLQR::backward_pass() // some of them could be infeasible unless the initial // satisfies them already, let's check the residual // from the computed dx[0] - Eigen::VectorXd residual; + VectorXr residual; residual = _constraint_to_go->C()*_bp_res[0].dx + _constraint_to_go->h(); @@ -140,31 +141,31 @@ void IterativeLQR::backward_pass_iter(int i) // print if(_log) { - Eigen::MatrixXd H(_nu+_nx, _nu+_nx); + MatrixXr H(_nu+_nx, _nu+_nx); H << tmp.Hxx, tmp.Hux.transpose(), tmp.Hux, tmp.Huu; - Eigen::VectorXd eigH = H.eigenvalues().real(); + VectorXr eigH = H.eigenvalues().real(); std::cout << "eig(H[" << i << "]) in [" << eigH.minCoeff() << ", " << eigH.maxCoeff() << "] \n"; std::cout << "H symmetry error = " << (H - H.transpose()).lpNorm() << "\n"; - Eigen::MatrixXd V(_nu+_nx, _nu+_nx); + MatrixXr V(_nu+_nx, _nu+_nx); V << Q, P.transpose(), P, R; - Eigen::VectorXd eigV = V.eigenvalues().real(); + VectorXr eigV = V.eigenvalues().real(); std::cout << "eig(V[" << i << "]) in [" << eigV.minCoeff() << ", " << eigV.maxCoeff() << "] \n"; std::cout << "V symmetry error = " << (V - V.transpose()).lpNorm() << "\n"; - Eigen::VectorXd eigS = Snext.eigenvalues().real(); + VectorXr eigS = Snext.eigenvalues().real(); std::cout << "eig(S[" << i+1 << "]) in [" << eigS.minCoeff() << ", " << eigS.maxCoeff() << "] \n"; - Eigen::VectorXd eigHuu = tmp.Huu.eigenvalues().real(); + VectorXr eigHuu = tmp.Huu.eigenvalues().real(); std::cout << "eig(Huu[" << i << "]) in [" << eigHuu.minCoeff() << ", " << eigHuu.maxCoeff() << "] \n"; } @@ -210,22 +211,22 @@ void IterativeLQR::backward_pass_iter(int i) case ReducedHessian: // auto R11 = tmp.cqr.matrixR().topLeftCorner(nc, nc).triangularView(); // auto R12 = tmp.cqr.matrixR().topRightCorner(nc, _nu - nc); -// Eigen::VectorXd R11inv_h = R11.solve(constr_feas.h); -// Eigen::MatrixXd R11inv_C = R11.solve(constr_feas.C); -// Eigen::MatrixXd M = -R11.solve(R12); -// Eigen::MatrixXd Hzz = tmp.codP.transpose()*tmp.Huu*tmp.codP; +// VectorXr R11inv_h = R11.solve(constr_feas.h); +// MatrixXr R11inv_C = R11.solve(constr_feas.C); +// MatrixXr M = -R11.solve(R12); +// MatrixXr Hzz = tmp.codP.transpose()*tmp.Huu*tmp.codP; // auto& P = tmp.codP; // auto H11 = Hzz.topLeftCorner(nc, nc); // auto H12 = Hzz.topRightCorner(nc, _nu - nc); // auto H22 = Hzz.bottomLeftCorner(_nu - nc,_nu - nc); -// Eigen::MatrixXd Hred = H22 + M.transpose()*H11*M + +// MatrixXr Hred = H22 + M.transpose()*H11*M + // M*H12 + H12.transpose()*M.transpose(); -// Eigen::LLT llt; +// Eigen::LLT llt; // llt.compute(Hred); -// Eigen::MatrixXd I_MT; // [I M^T] -// Eigen::VectorXd red_grad_0 = I_MT * (P.transpose()*tmp.hu) - +// MatrixXr I_MT; // [I M^T] +// VectorXr red_grad_0 = I_MT * (P.transpose()*tmp.hu) - // (H12 + H11*M).transpose()*R11inv_h; -// Eigen::MatrixXd red_grad_x = I_MT * (P.transpose()*tmp.Hux) - +// MatrixXr red_grad_x = I_MT * (P.transpose()*tmp.Hux) - // (H12 + H11*M).transpose()*R11inv_C; // llt.solveInPlace(red_grad_0); // llt.solveInPlace(red_grad_x); @@ -302,8 +303,8 @@ void IterativeLQR::backward_pass_iter(int i) for(int j = 0; j < tmp.hinf.size(); j++) { // i-th infeasible constraint is in the form 0x = 0 - double hnorm = std::fabs(tmp.hinf[j]); - double Cnorm = tmp.Cinf.row(j).lpNorm(); + Real hnorm = std::fabs(tmp.hinf[j]); + Real Cnorm = tmp.Cinf.row(j).lpNorm(); if(hnorm < 1e-16 && Cnorm < 1e-16) { if(_verbose) @@ -324,8 +325,8 @@ void IterativeLQR::backward_pass_iter(int i) void IterativeLQR::optimize_initial_state() { - Eigen::VectorXd& dx = _bp_res[0].dx; - Eigen::VectorXd& lam = _bp_res[0].dx_lam; + VectorXr& dx = _bp_res[0].dx; + VectorXr& lam = _bp_res[0].dx_lam; // typical case: initial state is fixed if(fixed_initial_state()) @@ -348,14 +349,14 @@ void IterativeLQR::optimize_initial_state() auto Csvd = C.jacobiSvd(); Csvd.setThreshold(_svd_threshold); - Eigen::VectorXd svC = Csvd.singularValues(); + VectorXr svC = Csvd.singularValues(); std::cout << "sv(C[" << 0 << "]) in [" << svC.minCoeff() << ", " << svC.maxCoeff() << "], rank = " << Csvd.rank() << "\n"; } // construct kkt matrix TIC(construct_state_kkt); - Eigen::MatrixXd& K = _tmp[0].x_kkt; + MatrixXr& K = _tmp[0].x_kkt; K.resize(s.size() + h.size(), s.size() + h.size()); K.topLeftCorner(S.rows(), S.cols()) = S; K.topRightCorner(C.cols(), C.rows()) = C.transpose(); @@ -364,7 +365,7 @@ void IterativeLQR::optimize_initial_state() TOC(construct_state_kkt); // residual vector - Eigen::VectorXd k = _tmp[0].x_k0; + VectorXr k = _tmp[0].x_k0; k.resize(s.size() + h.size()); k << -s, -h; @@ -377,7 +378,7 @@ void IterativeLQR::optimize_initial_state() auto& lu = _tmp[0].x_lu; auto& qr = _tmp[0].x_qr; auto& ldlt = _tmp[0].x_ldlt; - Eigen::VectorXd& dx_lam = _tmp[0].dx_lam; + VectorXr& dx_lam = _tmp[0].dx_lam; switch(_kkt_decomp_type) { @@ -408,7 +409,7 @@ void IterativeLQR::optimize_initial_state() if(_log) { - Eigen::VectorXd eigS = S.eigenvalues().real(); + VectorXr eigS = S.eigenvalues().real(); std::cout << "eig(S[" << 0 << "]) in [" << eigS.minCoeff() << ", " << eigS.maxCoeff() << "] \n"; @@ -421,8 +422,8 @@ void IterativeLQR::optimize_initial_state() lam = dx_lam.tail(h.size()); // check constraints - Eigen::MatrixXd Cinf = C; - Eigen::VectorXd hinf = h; + MatrixXr Cinf = C; + VectorXr hinf = h; _constraint_to_go->clear(); @@ -444,7 +445,7 @@ void IterativeLQR::optimize_initial_state() void IterativeLQR::add_bound_constraint(int k) { - Eigen::RowVectorXd x_ei, u_ei; + RowVectorXr x_ei, u_ei; // state bounds u_ei.setZero(_nu); @@ -462,7 +463,7 @@ void IterativeLQR::add_bound_constraint(int k) { x_ei = x_ei.Unit(_nx, i); - Eigen::Matrix hd; + Eigen::Matrix hd; hd(0) = _xtrj(i, k) - _x_lb(i, k); _constraint_to_go->add(x_ei, u_ei, hd); @@ -490,7 +491,7 @@ void IterativeLQR::add_bound_constraint(int k) { u_ei = u_ei.Unit(_nu, i); - Eigen::Matrix hd; + Eigen::Matrix hd; hd(0) = _utrj(i, k) - _u_lb(i, k); _constraint_to_go->add(x_ei, u_ei, hd); @@ -637,9 +638,9 @@ IterativeLQR::FeasibleConstraint IterativeLQR::handle_constraints(int i) } // decompose constraint into a feasible and infeasible components - Eigen::MatrixXd Ctmp = _constraint_to_go->C(); - Eigen::MatrixXd Dtmp = _constraint_to_go->D(); - Eigen::VectorXd htmp = _constraint_to_go->h(); + MatrixXr Ctmp = _constraint_to_go->C(); + MatrixXr Dtmp = _constraint_to_go->D(); + VectorXr htmp = _constraint_to_go->h(); TOC(constraint_prepare_inner); THROW_NAN(Ctmp); THROW_NAN(Dtmp); @@ -652,14 +653,14 @@ IterativeLQR::FeasibleConstraint IterativeLQR::handle_constraints(int i) // it is rather common for D to contain exact zero rows, // we can directly consider them as unsatisfied constr _constraint_to_go->clear(); - Eigen::MatrixXd C(Ctmp.rows(), Ctmp.cols()); - Eigen::MatrixXd D(Dtmp.rows(), Dtmp.cols()); - Eigen::VectorXd h(htmp.size()); + MatrixXr C(Ctmp.rows(), Ctmp.cols()); + MatrixXr D(Dtmp.rows(), Dtmp.cols()); + VectorXr h(htmp.size()); int pruned_idx = 0; for(int j = 0; j < h.size(); j++) { - double Dnorm = Dtmp.row(j).lpNorm(); + Real Dnorm = Dtmp.row(j).lpNorm(); if(Dnorm == 0) { _constraint_to_go->add(Ctmp.row(j), @@ -772,8 +773,8 @@ IterativeLQR::FeasibleConstraint IterativeLQR::handle_constraints(int i) // // regularize feasible part // for(int j = 0; j < rank; j++) // { -// double rjj = qr.matrixR()(j, j); -// double r_mult = _svd_threshold*(1 + qr.maxPivot())/std::fabs(rjj); +// Real rjj = qr.matrixR()(j, j); +// Real r_mult = _svd_threshold*(1 + qr.maxPivot())/std::fabs(rjj); // if(r_mult <= 1.0) // { diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index a5b0053e..e61acf77 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -1,7 +1,7 @@ #include "ilqr_impl.h" +#include "typedefs.h" - -bool IterativeLQR::forward_pass(double alpha) +bool IterativeLQR::forward_pass(Real alpha) { TIC(forward_pass); @@ -29,7 +29,7 @@ bool IterativeLQR::forward_pass(double alpha) return true; } -void IterativeLQR::forward_pass_iter(int i, double alpha) +void IterativeLQR::forward_pass_iter(int i, Real alpha) { TIC(forward_pass_inner) @@ -88,11 +88,11 @@ void IterativeLQR::forward_pass_iter(int i, double alpha) } -double IterativeLQR::compute_merit_slope(double cost_slope, - double mu_f, - double mu_c, - double defect_norm, - double constr_viol) +Real IterativeLQR::compute_merit_slope(Real cost_slope, + Real mu_f, + Real mu_c, + Real defect_norm, + Real constr_viol) { // see Nocedal and Wright, Theorem 18.2, pg. 541 // available online http://www.apmath.spbu.ru/cnsa/pdf/monograf/Numerical_Optimization2006.pdf @@ -101,13 +101,13 @@ double IterativeLQR::compute_merit_slope(double cost_slope, } -double IterativeLQR::compute_cost_slope() +Real IterativeLQR::compute_cost_slope() { TIC(compute_cost_slope); - double der = 0.; + Real der = 0.; - Eigen::VectorXd dx, du; + VectorXr dx, du; dx = _bp_res[0].dx; for(int i = 0; i < _N; i++) @@ -120,11 +120,11 @@ double IterativeLQR::compute_cost_slope() return der; } -double IterativeLQR::compute_merit_value(double mu_f, - double mu_c, - double cost, - double defect_norm, - double constr_viol) +Real IterativeLQR::compute_merit_value(Real mu_f, + Real mu_c, + Real cost, + Real defect_norm, + Real constr_viol) { // we define a merit function as follows // m(alpha) = J + mu_f * |D| + mu_c * |G| @@ -141,18 +141,18 @@ double IterativeLQR::compute_merit_value(double mu_f, } -std::pair IterativeLQR::compute_merit_weights( - double cost_der, - double defect_norm, - double constr_viol) +std::pair IterativeLQR::compute_merit_weights( + Real cost_der, + Real defect_norm, + Real constr_viol) { TIC(compute_merit_weights); // note: we here assume dx = 0, since this function runs before // the forward pass - double lam_x_max = 0.0; - double lam_g_max = 0.0; + Real lam_x_max = 0.0; + Real lam_g_max = 0.0; for(int i = 0; i < _N; i++) { @@ -171,30 +171,30 @@ std::pair IterativeLQR::compute_merit_weights( } } - const double merit_safety_factor = 2.0; - double mu_f = lam_x_max * merit_safety_factor; - double mu_c = std::max(lam_g_max * merit_safety_factor, 0.0); + const Real merit_safety_factor = 2.0; + Real mu_f = lam_x_max * merit_safety_factor; + Real mu_c = std::max(lam_g_max * merit_safety_factor, static_cast(0.0)); -// double g = defect_norm + mu_c/mu_f*constr_viol; -// double rho = 0.5; -// double mu = 2.0 * cost_der / ((1 - rho)*g); +// Real g = defect_norm + mu_c/mu_f*constr_viol; +// Real rho = 0.5; +// Real mu = 2.0 * cost_der / ((1 - rho)*g); // mu = std::max(mu, 0.0); return {mu_f, mu_c}; } -double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_cost(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_cost); - double cost = 0.0; + Real cost = 0.0; if (_debug) { // reset constr value to nan for(auto& item : _cost_values) { - item.second.setConstant(std::numeric_limits::quiet_NaN()); + item.second.setConstant(std::numeric_limits::quiet_NaN()); } } @@ -228,12 +228,12 @@ double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::Matr return cost / _N; } -double IterativeLQR::compute_bound_penalty(const Eigen::MatrixXd &xtrj, - const Eigen::MatrixXd &utrj) +Real IterativeLQR::compute_bound_penalty(const MatrixXr &xtrj, + const MatrixXr &utrj) { TIC(compute_bound_penalty); - double res = 0.0; + Real res = 0.0; auto xineq = _x_lb.array() < _x_ub.array(); auto uineq = _u_lb.array() < _u_ub.array(); @@ -246,18 +246,18 @@ double IterativeLQR::compute_bound_penalty(const Eigen::MatrixXd &xtrj, return res / _N; } -double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_constr(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_constr); - double constr = 0.0; + Real constr = 0.0; if (_debug) { // reset constr value to nan for(auto& item : _constr_values) { - item.second.setConstant(std::numeric_limits::quiet_NaN()); + item.second.setConstant(std::numeric_limits::quiet_NaN()); } } @@ -304,11 +304,11 @@ double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::Ma return constr / _N; } -double IterativeLQR::compute_defect(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_defect(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_defect); - double defect = 0.0; + Real defect = 0.0; // compute defects on given trajectory for(int i = 0; i < _N; i++) @@ -331,14 +331,14 @@ bool IterativeLQR::line_search(int iter) { TIC(line_search); - const double step_reduction_factor = 0.5; - const double alpha_min = _alpha_min; - double alpha = _step_length; - const double eta = _line_search_accept_ratio; + const Real step_reduction_factor = 0.5; + const Real alpha_min = _alpha_min; + Real alpha = _step_length; + const Real eta = _line_search_accept_ratio; // compute merit function weights - double cost_der = compute_cost_slope(); + Real cost_der = compute_cost_slope(); auto [mu_f, mu_c] = compute_merit_weights( cost_der, _fp_res->defect_norm, @@ -349,14 +349,14 @@ bool IterativeLQR::line_search(int iter) _fp_res->rho = _rho; // compute merit function initial value - double merit = compute_merit_value(mu_f, mu_c, + Real merit = compute_merit_value(mu_f, mu_c, _fp_res->cost, _fp_res->defect_norm, _fp_res->constraint_violation); // compute merit function directional derivative - double merit_der = compute_merit_slope(cost_der, + Real merit_der = compute_merit_slope(cost_der, mu_f, mu_c, _fp_res->defect_norm, _fp_res->constraint_violation); @@ -466,7 +466,7 @@ void IterativeLQR::reset_iterate_filter() { _it_filt.clear(); IterateFilter::Pair test_pair; - test_pair.f = std::numeric_limits::lowest(); + test_pair.f = std::numeric_limits::lowest(); test_pair.h = _fp_res->defect_norm + _fp_res->constraint_violation; test_pair.h = std::max(1e2*test_pair.h, 1e3); } @@ -474,10 +474,10 @@ void IterativeLQR::reset_iterate_filter() bool IterativeLQR::should_stop() { - const double constraint_violation_threshold = _constraint_violation_threshold; - const double defect_norm_threshold = _defect_norm_threshold; - const double merit_der_threshold = _merit_der_threshold; - const double step_length_threshold = _step_length_threshold; + const Real constraint_violation_threshold = _constraint_violation_threshold; + const Real defect_norm_threshold = _defect_norm_threshold; + const Real merit_der_threshold = _merit_der_threshold; + const Real step_length_threshold = _step_length_threshold; TIC(should_stop); diff --git a/horizon/cpp/src/ilqr_impl.h b/horizon/cpp/src/ilqr_impl.h index 4ff960d5..df06bf69 100644 --- a/horizon/cpp/src/ilqr_impl.h +++ b/horizon/cpp/src/ilqr_impl.h @@ -3,17 +3,21 @@ #include "ilqr.h" #include "wrapped_function.h" +#include "typedefs.h" using namespace horizon; using namespace casadi_utils; +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + namespace cs = casadi; extern utils::Timer::TocCallback on_timer_toc; struct IterativeLQR::Dynamics { - public: // dynamics function @@ -26,13 +30,13 @@ struct IterativeLQR::Dynamics ParameterMapPtr param; // df/dx - const Eigen::MatrixXd& A() const; + const MatrixXr& A() const; // df/du - const Eigen::MatrixXd& B() const; + const MatrixXr& B() const; // defect (or gap) - Eigen::VectorXd d; + VectorXr d; Dynamics(int nx, int nu); @@ -48,7 +52,7 @@ struct IterativeLQR::Dynamics VecConstRef u, VecConstRef xnext, int k, - Eigen::VectorXd& d); + VectorXr& d); void setDynamics(casadi::Function f); @@ -58,6 +62,7 @@ struct IterativeLQR::Dynamics struct IterativeLQR::ConstraintEntity { + typedef std::shared_ptr Ptr; // constraint function @@ -73,10 +78,10 @@ struct IterativeLQR::ConstraintEntity std::vector indices; // dh/dx - const Eigen::MatrixXd& C() const; + const MatrixXr& C() const; // dh/du - const Eigen::MatrixXd& D() const; + const MatrixXr& D() const; // constraint violation h(x, u) - hdes VecConstRef h() const; @@ -94,27 +99,28 @@ struct IterativeLQR::ConstraintEntity void setConstraint(casadi::Function h, casadi::Function dh); - void setTargetValue(const Eigen::VectorXd& hdes); + void setTargetValue(const VectorXr& hdes); static casadi::Function Jacobian(const casadi::Function& h); private: // desired value - Eigen::VectorXd _hdes; + VectorXr _hdes; // computed value - Eigen::VectorXd _hvalue; + VectorXr _hvalue; }; struct IterativeLQR::Constraint { + // dh/dx - const Eigen::MatrixXd& C() const; + const MatrixXr& C() const; // dh/du - const Eigen::MatrixXd& D() const; + const MatrixXr& D() const; // constraint violation f(x, u) VecConstRef h() const; @@ -139,14 +145,15 @@ struct IterativeLQR::Constraint private: - Eigen::MatrixXd _C; - Eigen::MatrixXd _D; - Eigen::VectorXd _h; + MatrixXr _C; + MatrixXr _D; + VectorXr _h; }; struct IterativeLQR::CostEntityBase { + typedef std::shared_ptr Ptr; // parameters @@ -160,20 +167,20 @@ struct IterativeLQR::CostEntityBase virtual VecConstRef r() const { return _r; } - virtual double evaluate(VecConstRef x, + virtual Real evaluate(VecConstRef x, VecConstRef u, int k) = 0; virtual void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) = 0; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) = 0; virtual std::string getName() = 0; - virtual double getCostEvaluated() const { return _cost_eval; } + virtual Real getCostEvaluated() const { return _cost_eval; } virtual ~CostEntityBase() = default; @@ -181,8 +188,8 @@ struct IterativeLQR::CostEntityBase protected: - Eigen::VectorXd _q, _r; - double _cost_eval; + VectorXr _q, _r; + Real _cost_eval; }; struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase @@ -193,16 +200,16 @@ struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase VecConstRef xlb, VecConstRef xub, VecConstRef ulb, VecConstRef uub); - void setRho(double rho); + void setRho(Real rho); - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -217,11 +224,11 @@ struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase VecConstRef _xlb, _xub; VecConstRef _ulb, _uub; - Eigen::VectorXd _x_violation; - Eigen::VectorXd _u_violation; + VectorXr _x_violation; + VectorXr _u_violation; - Eigen::VectorXd _xlam, _ulam; - double _rho; + VectorXr _xlam, _ulam; + Real _rho; const int _N; }; @@ -231,8 +238,6 @@ struct IterativeLQR::IntermediateCostEntity : CostEntityBase { typedef std::shared_ptr Ptr; - - // set cost void setCost(casadi::Function l, casadi::Function dl, @@ -241,14 +246,14 @@ struct IterativeLQR::IntermediateCostEntity : CostEntityBase VecConstRef q() const override; VecConstRef r() const override; - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -274,14 +279,14 @@ struct IterativeLQR::IntermediateResidualEntity : CostEntityBase void setResidual(casadi::Function res, casadi::Function dres); - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -302,17 +307,17 @@ struct IterativeLQR::IntermediateCost { /* Quadratized cost */ - const Eigen::MatrixXd& Q() const; + const MatrixXr& Q() const; VecConstRef q() const; - const Eigen::MatrixXd& R() const; + const MatrixXr& R() const; VecConstRef r() const; - const Eigen::MatrixXd& P() const; + const MatrixXr& P() const; IntermediateCost(int nx, int nu); void addCost(CostEntityBase::Ptr cost); - double evaluate(VecConstRef x, VecConstRef u, int k); + Real evaluate(VecConstRef x, VecConstRef u, int k); void quadratize(VecConstRef x, VecConstRef u, int k); void clear(); @@ -321,8 +326,8 @@ struct IterativeLQR::IntermediateCost private: - Eigen::MatrixXd _Q, _R, _P; - Eigen::VectorXd _q, _r; + MatrixXr _Q, _R, _P; + VectorXr _q, _r; }; struct IterativeLQR::Temporaries @@ -330,59 +335,59 @@ struct IterativeLQR::Temporaries /* Backward pass */ // temporary for s + S*d - Eigen::MatrixXd s_plus_S_d; + MatrixXr s_plus_S_d; // temporary for S*A - Eigen::MatrixXd S_A; + MatrixXr S_A; // feasible constraint - Eigen::MatrixXd Cf, Df; - Eigen::VectorXd hf; + MatrixXr Cf, Df; + VectorXr hf; // cod of constraint - Eigen::CompleteOrthogonalDecomposition ccod; - Eigen::ColPivHouseholderQR cqr; - Eigen::BDCSVD csvd; - Eigen::MatrixXd codQ; + Eigen::CompleteOrthogonalDecomposition ccod; + Eigen::ColPivHouseholderQR cqr; + Eigen::BDCSVD csvd; + MatrixXr codQ; Eigen::PermutationMatrix codP; // quadratized value function - Eigen::MatrixXd Huu; - Eigen::MatrixXd Hux; - Eigen::MatrixXd Hxx; - Eigen::VectorXd hx; - Eigen::VectorXd hu; + MatrixXr Huu; + MatrixXr Hux; + MatrixXr Hxx; + VectorXr hx; + VectorXr hu; // temporary for kkt rhs - Eigen::MatrixXd kkt; - Eigen::MatrixXd kx0; + MatrixXr kkt; + MatrixXr kx0; // lu for kkt matrix - Eigen::PartialPivLU lu; - Eigen::ColPivHouseholderQR qr; - Eigen::LDLT ldlt; + Eigen::PartialPivLU lu; + Eigen::ColPivHouseholderQR qr; + Eigen::LDLT ldlt; // kkt solution - Eigen::MatrixXd u_lam; + MatrixXr u_lam; // infeasible component of constraint - Eigen::MatrixXd Cinf; - Eigen::MatrixXd Dinf; - Eigen::VectorXd hinf; + MatrixXr Cinf; + MatrixXr Dinf; + VectorXr hinf; // optimal state computation // (note: only for initial state x[0]) - Eigen::FullPivLU x_lu; - Eigen::ColPivHouseholderQR x_qr; - Eigen::LDLT x_ldlt; - Eigen::MatrixXd x_kkt; - Eigen::VectorXd x_k0; - Eigen::VectorXd dx_lam; + Eigen::FullPivLU x_lu; + Eigen::ColPivHouseholderQR x_qr; + Eigen::LDLT x_ldlt; + MatrixXr x_kkt; + VectorXr x_k0; + VectorXr dx_lam; /* Forward pass */ - Eigen::VectorXd dx; - Eigen::VectorXd du; - Eigen::VectorXd defect; + VectorXr dx; + VectorXr du; + VectorXr defect; }; @@ -415,16 +420,16 @@ struct IterativeLQR::ConstraintToGo private: - Eigen::Matrix _C; - Eigen::Matrix _D; - Eigen::VectorXd _h; + Eigen::Matrix _C; + Eigen::Matrix _D; + VectorXr _h; int _dim; }; struct IterativeLQR::ValueFunction { - Eigen::MatrixXd S; - Eigen::VectorXd s; + MatrixXr S; + VectorXr s; ValueFunction(int nx); }; @@ -433,26 +438,26 @@ struct IterativeLQR::BackwardPassResult { // real input as function of state // (u = Lu*x + lu) - Eigen::MatrixXd Lu; - Eigen::VectorXd lu; + MatrixXr Lu; + VectorXr lu; // auxiliary input as function of state // (z = Lz*x + lz, where u = lc + Lc*x + Bz*z) - Eigen::MatrixXd Lz; - Eigen::VectorXd lz; + MatrixXr Lz; + VectorXr lz; // constraint-to-go size int nc; // lagrange multipliers - Eigen::MatrixXd Gu; - Eigen::MatrixXd Gx; - Eigen::VectorXd glam; + MatrixXr Gu; + MatrixXr Gx; + VectorXr glam; // optimal state // (this is only filled at i = 0) - Eigen::VectorXd dx; - Eigen::VectorXd dx_lam; + VectorXr dx; + VectorXr dx_lam; BackwardPassResult(int nx, int nu); }; @@ -464,7 +469,7 @@ struct IterativeLQR::FeasibleConstraint VecConstRef h; }; -static void set_param_inputs(std::shared_ptr> params, int k, +static void set_param_inputs(std::shared_ptr> params, int k, casadi_utils::WrappedFunction& f); #define THROW_NAN(mat) \ diff --git a/horizon/cpp/src/ilqr_test.cpp b/horizon/cpp/src/ilqr_test.cpp index b0161e4a..533529df 100644 --- a/horizon/cpp/src/ilqr_test.cpp +++ b/horizon/cpp/src/ilqr_test.cpp @@ -1,9 +1,14 @@ #include "ilqr.h" #include #include "wrapped_function.h" +#include "typedefs.h" int main() { + using Real=horizon::Real; + using MatrixXr=horizon::MatrixXr; + using VectorXr=horizon::VectorXr; + bool stop = false; auto f = casadi::external("zero_velocity_l_foot_l_sole_vel_task_jac", @@ -18,9 +23,9 @@ int main() while(!stop) { - auto x = Eigen::VectorXd::Random(f.size1_in(0)).eval(); - auto u = Eigen::VectorXd::Random(f.size1_in(1), 1).eval(); - auto tgt = Eigen::VectorXd::Random(f.size1_in(2), 1).eval(); + auto x = VectorXr::Random(f.size1_in(0)).eval(); + auto u = VectorXr::Random(f.size1_in(1), 1).eval(); + auto tgt = VectorXr::Random(f.size1_in(2), 1).eval(); fw.setInput(0, x); fw.setInput(1, u); @@ -61,6 +66,10 @@ int main() int not_a_main() { + using Real=horizon::Real; + using MatrixXr=horizon::MatrixXr; + using VectorXr=horizon::VectorXr; + auto x = casadi::SX::sym("x", 1); auto u = casadi::SX::sym("u", 1); auto p = casadi::SX::sym("p", 1); @@ -73,21 +82,21 @@ int not_a_main() int N = 3; horizon::IterativeLQR ilqr(f, N); - Eigen::MatrixXd xlb, xub; + MatrixXr xlb, xub; xlb.setConstant(1, N+1, -INFINITY); xub.setConstant(1, N+1, INFINITY); xlb(N) = 1.0; xub(N) = 1.0; ilqr.setStateBounds(xlb, xub); - Eigen::MatrixXd ulb, uub; + MatrixXr ulb, uub; ulb.setConstant(1, N, -INFINITY); uub.setConstant(1, N, INFINITY); ulb(0) = 11.0; uub(0) = 11.0; ilqr.setInputBounds(ulb, uub); - Eigen::VectorXd x0(1); + VectorXr x0(1); x0 << 0.0; ilqr.setInitialState(x0); xlb(0) = x0(0); @@ -96,11 +105,11 @@ int not_a_main() ilqr.setCost({0, 1}, l); ilqr.setConstraint({2}, cf); - Eigen::MatrixXd myparam_values; + MatrixXr myparam_values; myparam_values.setConstant(1, N+1, -1.0); ilqr.setParameterValue("myparam", myparam_values); - Eigen::MatrixXd dt_values; + MatrixXr dt_values; dt_values.setConstant(1, N+1, 0.1); ilqr.setParameterValue("dt", dt_values); diff --git a/horizon/cpp/src/iterate_filter.cpp b/horizon/cpp/src/iterate_filter.cpp index 499e7c04..fbb2dc8d 100644 --- a/horizon/cpp/src/iterate_filter.cpp +++ b/horizon/cpp/src/iterate_filter.cpp @@ -2,11 +2,11 @@ IterateFilter::Pair::Pair(): - f(std::numeric_limits::max()), - h(std::numeric_limits::max()) + f(std::numeric_limits::max()), + h(std::numeric_limits::max()) {} -bool IterateFilter::Pair::dominates(const IterateFilter::Pair &other, double beta, double gamma) const +bool IterateFilter::Pair::dominates(const IterateFilter::Pair &other, Real beta, Real gamma) const { return f < other.f + gamma*other.h && beta*h < other.h; } diff --git a/horizon/cpp/src/iterate_filter.h b/horizon/cpp/src/iterate_filter.h index 6f31c561..8f864183 100644 --- a/horizon/cpp/src/iterate_filter.h +++ b/horizon/cpp/src/iterate_filter.h @@ -5,22 +5,25 @@ #include #include #include +#include "typedefs.h" class IterateFilter { +using Real=horizon::Real; + public: struct Pair { - double f; - double h; + Real f; + Real h; Pair(); bool dominates(const Pair& other, - double beta = 1.0, - double gamma = 0.0) const; + Real beta = 1.0, + Real gamma = 0.0) const; }; IterateFilter() = default; @@ -33,9 +36,9 @@ class IterateFilter void print(); - double beta = 1.0; - double gamma = 0.0; - double constr_tol = 1e-6; + Real beta = 1.0; + Real gamma = 0.0; + Real constr_tol = 1e-6; private: diff --git a/horizon/cpp/src/profiling.cpp b/horizon/cpp/src/profiling.cpp index d7b68507..7f34a5db 100644 --- a/horizon/cpp/src/profiling.cpp +++ b/horizon/cpp/src/profiling.cpp @@ -23,7 +23,7 @@ void Timer::toc() { if(_done) return; - double usec = (hrc::now() - _t0).count() * 1e-3; + Real usec = (hrc::now() - _t0).count() * 1e-3; _on_toc(_name, usec); _done = true; } diff --git a/horizon/cpp/src/profiling.h b/horizon/cpp/src/profiling.h index cbf29e9a..b8ea5076 100644 --- a/horizon/cpp/src/profiling.h +++ b/horizon/cpp/src/profiling.h @@ -7,14 +7,18 @@ #include #include +#include "typedefs.h" + namespace horizon { namespace utils { struct Timer { + using Real=horizon::Real; + typedef std::chrono::high_resolution_clock hrc; - typedef std::function TocCallback; + typedef std::function TocCallback; Timer(const char* name, TocCallback& cb); @@ -36,7 +40,7 @@ struct Timer struct ProfilingInfo { - std::map> timings; + std::map> timings; }; diff --git a/horizon/cpp/src/sqp.cpp b/horizon/cpp/src/sqp.cpp index 992062d7..feb72c26 100644 --- a/horizon/cpp/src/sqp.cpp +++ b/horizon/cpp/src/sqp.cpp @@ -2,7 +2,6 @@ using namespace horizon; - template const casadi::DMDict& SQPGaussNewton::solve( const casadi::DM& initial_guess_x, @@ -24,8 +23,8 @@ const casadi::DMDict& SQPGaussNewton::solve( _iteration_to_solve = 0; // set parameters as second input of f and df - _f.setInput(1, Eigen::VectorXd::Map(p->data(), p.size1())); - _df.setInput(1, Eigen::VectorXd::Map(p->data(), p.size1())); + _f.setInput(1, VectorXr::Map(p->data(), p.size1())); + _df.setInput(1, VectorXr::Map(p->data(), p.size1())); // do the same on g and A (i.e., dg) _g_dict.input[_g.name_in(1)] = p; @@ -66,7 +65,7 @@ const casadi::DMDict& SQPGaussNewton::solve( casadi_utils::toCasadiMatrix(_grad, grad_); if(!H_.is_init()) - H_ = casadi_utils::WrappedSparseMatrix(_H); + H_ = casadi_utils::WrappedSparseMatrix(_H); else H_.update_values(_H); @@ -147,7 +146,7 @@ const casadi::DMDict& SQPGaussNewton::solve( _solution["x"] = x0_; - double norm_f = _f.getOutput(0).norm(); + Real norm_f = _f.getOutput(0).norm(); _solution["f"] = 0.5*norm_f*norm_f; _solution["g"] = casadi::norm_2(_g_dict.output[_g.name_out(0)].get_elements()); @@ -156,10 +155,10 @@ const casadi::DMDict& SQPGaussNewton::solve( template bool SQPGaussNewton::lineSearch( - Eigen::VectorXd &x, - const Eigen::VectorXd &dx, - const Eigen::VectorXd &lam_x, - const Eigen::VectorXd &lam_a, + VectorXr &x, + const VectorXr &dx, + const VectorXr &lam_x, + const VectorXr &lam_a, const casadi::DM &lbg, const casadi::DM &ubg, const casadi::DM &lbx, @@ -173,20 +172,20 @@ bool SQPGaussNewton::lineSearch( _x0_ = x; - const double merit_safety_factor = 2.0; - double norminf_lam_x = lam_x.lpNorm(); - double norminf_lam_a = lam_a.lpNorm(); - double norminf_lam = merit_safety_factor*std::max(norminf_lam_x, norminf_lam_a); + const Real merit_safety_factor = 2.0; + Real norminf_lam_x = lam_x.lpNorm(); + Real norminf_lam_a = lam_a.lpNorm(); + Real norminf_lam = merit_safety_factor*std::max(norminf_lam_x, norminf_lam_a); - double initial_cost = computeCost(_f); + Real initial_cost = computeCost(_f); - double cost_derr = computeCostDerivative(dx, _grad); + Real cost_derr = computeCostDerivative(dx, _grad); casadi_utils::toEigen(_g_dict.output[_g.name_out(0)], _g_); - double constraint_violation = computeConstraintViolation(_g_, _x0_, _lbg_, _ubg_, _lbx_, _ubx_); + Real constraint_violation = computeConstraintViolation(_g_, _x0_, _lbg_, _ubg_, _lbx_, _ubx_); - double merit_der = cost_derr - norminf_lam * constraint_violation; - double initial_merit = initial_cost + norminf_lam*constraint_violation; + Real merit_der = cost_derr - norminf_lam * constraint_violation; + Real initial_merit = initial_cost + norminf_lam*constraint_violation; // report initial value (only if iter == 0) if(iter == 0) @@ -215,15 +214,15 @@ bool SQPGaussNewton::lineSearch( { x = _x0_ + _alpha*dx; eval(_f, 0, x, false); - double candidate_cost = computeCost(_f); + Real candidate_cost = computeCost(_f); casadi_utils::toCasadiMatrix(x, _x_); _g_dict.input[_g.name_in(0)] = _x_; eval(_g, _g_dict); casadi_utils::toEigen(_g_dict.output[_g.name_out(0)], _g_); - double candidate_constraint_violation = computeConstraintViolation(_g_, x, _lbg_, _ubg_, _lbx_, _ubx_); + Real candidate_constraint_violation = computeConstraintViolation(_g_, x, _lbg_, _ubg_, _lbx_, _ubx_); - double candidate_merit = candidate_cost + norminf_lam*candidate_constraint_violation; + Real candidate_merit = candidate_cost + norminf_lam*candidate_constraint_violation; // evaluate Armijo's condition accepted = candidate_merit < (initial_merit + _beta*_alpha*merit_der); diff --git a/horizon/cpp/src/sqp.h b/horizon/cpp/src/sqp.h index 162c5292..a1159ad9 100644 --- a/horizon/cpp/src/sqp.h +++ b/horizon/cpp/src/sqp.h @@ -11,18 +11,24 @@ #include "profiling.h" #include "ilqr.h" +#include "typedefs.h" #define GR 1.61803398875 namespace horizon{ -typedef Eigen::Ref VecConstRef; -typedef Eigen::Ref MatConstRef; +typedef Eigen::Ref VecConstRef; +typedef Eigen::Ref MatConstRef; template ///casadi::SX or casadi::MX class SQPGaussNewton { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using RInfinity=horizon::RInfinity; + public: static void setQPOasesOptionsMPC(casadi::Dict& opts) @@ -31,13 +37,13 @@ class SQPGaussNewton opts["initialStatusBounds"] = "inactive"; opts["numRefinementSteps"] = 0; opts["enableDriftCorrection"] = 0; - opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); + opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); opts["enableFlippingBounds"] = false; opts["enableNZCTests"] = false; opts["enableRamping"] = false; opts["enableRegularisation"] = true; opts["numRegularisationSteps"] = 2; - opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); + opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); } static void setQPOasesOptionsReliable(casadi::Dict& opts) @@ -45,7 +51,7 @@ class SQPGaussNewton opts["enableEqualities"] = false; opts["numRefinementSteps"] = 2; opts["enableFullLITest"] = true; - opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); + opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); opts["maxDualJump"] = 10e8; opts["enableCholeskyRefactorisation"] = 1; } @@ -223,12 +229,12 @@ class SQPGaussNewton * @brief setAlphaMin set the minumi allowed alpha during linesearch * @param alpha min in Newton's method step */ - void setAlphaMin(const double alpha_min) + void setAlphaMin(const Real alpha_min) { _alpha_min = alpha_min; } - const double& getAlpha() const + const Real& getAlpha() const { return _alpha; } @@ -248,26 +254,26 @@ class SQPGaussNewton const casadi::DM& lbx, const casadi::DM& ubx, const casadi::DM& lbg, const casadi::DM& ubg); - double computeCost(const casadi_utils::WrappedFunction& f) + Real computeCost(const casadi_utils::WrappedFunction& f) { return f.getOutput(0).squaredNorm(); } - double computeCostDerivative(const Eigen::VectorXd& dx, const Eigen::VectorXd& grad) + Real computeCostDerivative(const VectorXr& dx, const VectorXr& grad) { return dx.dot(grad); } - double computeConstraintViolation(const Eigen::VectorXd& g, const Eigen::VectorXd& x, - const Eigen::VectorXd& lbg, const Eigen::VectorXd& ubg, - const Eigen::VectorXd& lbx, const Eigen::VectorXd& ubx) + Real computeConstraintViolation(const VectorXr& g, const VectorXr& x, + const VectorXr& lbg, const VectorXr& ubg, + const VectorXr& lbx, const VectorXr& ubx) { return (lbg-g).cwiseMax(0.).lpNorm<1>() + (ubg-g).cwiseMin(0.).lpNorm<1>() + (lbx-x).cwiseMax(0.).lpNorm<1>() + (ubx-x).cwiseMin(0.).lpNorm<1>(); } - bool lineSearch(Eigen::VectorXd& x, const Eigen::VectorXd& dx, const Eigen::VectorXd& lam_x, const Eigen::VectorXd& lam_a, + bool lineSearch(VectorXr& x, const VectorXr& dx, const VectorXr& lam_x, const VectorXr& lam_a, const casadi::DM& lbg, const casadi::DM& ubg, const casadi::DM& lbx, const casadi::DM& ubx, int iter); @@ -339,9 +345,9 @@ class SQPGaussNewton * @brief getObjectiveIterations * @return 0.5*norm2 of objective (one per iteration) */ - const std::vector& getObjectiveIterations() + const std::vector& getObjectiveIterations() { - Eigen::VectorXd tmp; + VectorXr tmp; _objective.clear(); _objective.reserve(_iteration_to_solve); for(unsigned int k = 0; k < _iteration_to_solve; ++k) @@ -349,7 +355,7 @@ class SQPGaussNewton casadi_utils::toEigen(_variable_trj[k], tmp); _f.setInput(0, tmp); // cost function _f.call(); - double norm = _f.getOutput(0).norm(); + Real norm = _f.getOutput(0).norm(); _objective.push_back(0.5*norm*norm); } return _objective; @@ -359,7 +365,7 @@ class SQPGaussNewton * @brief getConstraintNormIterations * @return norm2 of the constraint vector (one per iteration) */ - const std::vector& getConstraintNormIterations() + const std::vector& getConstraintNormIterations() { _constraints_norm.clear(); _constraints_norm.reserve(_iteration_to_solve); @@ -376,7 +382,7 @@ class SQPGaussNewton * @brief getHessianComputationTime * @return vector of times needed to compute hessian (one value per iteration) */ - const std::vector& getHessianComputationTime() const + const std::vector& getHessianComputationTime() const { return _hessian_computation_time; } @@ -385,22 +391,22 @@ class SQPGaussNewton * @brief getQPComputationTime * @return vector of times needed to solve qp (one value per iteration) */ - const std::vector& getQPComputationTime() const + const std::vector& getQPComputationTime() const { return _qp_computation_time; } - const std::vector& getLineSearchComputationTime() const + const std::vector& getLineSearchComputationTime() const { return _line_search_time; } - void setBeta(const double beta) + void setBeta(const Real beta) { _beta = beta; } - double getBeta() + Real getBeta() { return _beta; } @@ -422,7 +428,7 @@ class SQPGaussNewton * @param x point * @param sparse if result will be sparse */ - void eval(casadi_utils::WrappedFunction& wf, const int i, const Eigen::VectorXd& x, const bool sparse) + void eval(casadi_utils::WrappedFunction& wf, const int i, const VectorXr& x, const bool sparse) { wf.setInput(i, x); // cost function wf.call(sparse); @@ -440,7 +446,7 @@ class SQPGaussNewton } - bool checkIsStationary(const Eigen::VectorXd& grad, const double tol) + bool checkIsStationary(const VectorXr& grad, const Real tol) { for(unsigned int i = 0; i < grad.size(); ++i) { @@ -478,27 +484,27 @@ class SQPGaussNewton casadi::Dict _qp_opts; casadi::DMVector _variable_trj; - std::vector _objective, _constraints_norm; + std::vector _objective, _constraints_norm; - Eigen::SparseMatrix _J; - Eigen::SparseMatrix _H; - Eigen::SparseMatrix _I; - Eigen::VectorXd _grad; + Eigen::SparseMatrix _J; + Eigen::SparseMatrix _H; + Eigen::SparseMatrix _I; + VectorXr _grad; casadi::DM grad_; casadi::DM g_; casadi::DM A_; - casadi_utils::WrappedSparseMatrix H_; + casadi_utils::WrappedSparseMatrix H_; casadi::DM x0_; - Eigen::VectorXd _sol, _dx, _lam_a, _lam_x; + VectorXr _sol, _dx, _lam_a, _lam_x; IODMDict _g_dict; IODMDict _A_dict; - double _alpha, _alpha_min; + Real _alpha, _alpha_min; - std::vector _hessian_computation_time; - std::vector _qp_computation_time; - std::vector _line_search_time; + std::vector _hessian_computation_time; + std::vector _qp_computation_time; + std::vector _line_search_time; unsigned int _iteration_to_solve; @@ -506,23 +512,23 @@ class SQPGaussNewton IterativeLQR::ForwardPassResult _fpr; CallbackType _iter_cb; - double _beta; + Real _beta; - double _solution_convergence; - double _constraint_violation_tolerance; - double _merit_derivative_tolerance; + Real _solution_convergence; + Real _constraint_violation_tolerance; + Real _merit_derivative_tolerance; - double _eps_regularization = 0.0; - double _eps_regularization_base = 0.0; + Real _eps_regularization = 0.0; + Real _eps_regularization_base = 0.0; bool _use_gr; //line search - Eigen::VectorXd _lbg_, _ubg_, _lbx_, _ubx_; - Eigen::VectorXd _x0_; + VectorXr _lbg_, _ubg_, _lbx_, _ubx_; + VectorXr _x0_; casadi::DM _x_; - Eigen::VectorXd _g_; - double _merit_eps; + VectorXr _g_; + Real _merit_eps; }; diff --git a/horizon/cpp/src/sqp_test.cpp b/horizon/cpp/src/sqp_test.cpp index 021d0f02..5d8449d9 100644 --- a/horizon/cpp/src/sqp_test.cpp +++ b/horizon/cpp/src/sqp_test.cpp @@ -1,5 +1,6 @@ #include "sqp.h" #include +#include "typedefs.h" int main() { @@ -38,8 +39,8 @@ int main() // std::cout<<"solution: "< objs = sqp.getObjectiveIterations(); -// std::vector cons = sqp.getConstraintNormIterations(); +// std::vector objs = sqp.getObjectiveIterations(); +// std::vector cons = sqp.getConstraintNormIterations(); // for(unsigned int i = 0; i < sqp.getNumberOfIterations(); ++i) // std::cout<<"iter "< sol: "< objs2 = sqp2.getObjectiveIterations(); -// std::vector cons2 = sqp2.getConstraintNormIterations(); +// std::vector objs2 = sqp2.getObjectiveIterations(); +// std::vector cons2 = sqp2.getConstraintNormIterations(); // for(unsigned int i = 0; i < sqp2.getNumberOfIterations(); ++i) // std::cout<<"iter "< sol2: "< + +namespace horizon +{ + +#ifdef HORIZON_FLOAT32 +typedef float Real; +#else +typedef double Real; +#endif + +typedef Eigen::Matrix VectorXr; +typedef Eigen::Matrix MatrixXr; + +typedef Eigen::Matrix Matrix2r; +typedef Eigen::Matrix Vector2r; + +typedef Eigen::Matrix RowVectorXr; + +const Real RInfinity = std::numeric_limits::infinity(); + +} + +#endif // TYPDEFS_H \ No newline at end of file diff --git a/horizon/cpp/src/wrapped_function.cpp b/horizon/cpp/src/wrapped_function.cpp index e5c10040..7e0c2ef8 100644 --- a/horizon/cpp/src/wrapped_function.cpp +++ b/horizon/cpp/src/wrapped_function.cpp @@ -5,7 +5,6 @@ using namespace casadi_utils; extern horizon::utils::Timer::TocCallback on_timer_toc; - WrappedFunction::WrappedFunction(casadi::Function f) { *this = f; @@ -39,10 +38,10 @@ WrappedFunction &WrappedFunction::operator=(casadi::Function f) _out_buf.push_back(_out_data.back().data()); // allocate a zero dense matrix to store the output - _out_matrix.emplace_back(Eigen::MatrixXd::Zero(sp.size1(), sp.size2())); + _out_matrix.emplace_back(MatrixXr::Zero(sp.size1(), sp.size2())); //allocate a zero sparse matrix to store the output - _out_matrix_sparse.emplace_back(Eigen::SparseMatrix(sp.size1(), sp.size2())); + _out_matrix_sparse.emplace_back(Eigen::SparseMatrix(sp.size1(), sp.size2())); // save sparsity pattern for i-th output std::vector rows, cols; @@ -64,7 +63,7 @@ WrappedFunction::WrappedFunction(const WrappedFunction & other) *this = other._f; } -void WrappedFunction::setInput(int i, Eigen::Ref xi) +void WrappedFunction::setInput(int i, Eigen::Ref xi) { if(xi.size() != _f.size1_in(i)) { @@ -127,7 +126,7 @@ void WrappedFunction::call(bool sparse) oss << _out_matrix[i].format(3) << "\n"; for(int j = 0; j < _f.n_in(); j++) { - auto u = Eigen::VectorXd::Map(_in_buf[j], + auto u = VectorXr::Map(_in_buf[j], _f.size1_in(j)); oss << _f.name() << " input " << j << " = " << u.transpose().format(3) << "\n"; @@ -142,7 +141,7 @@ void WrappedFunction::call(bool sparse) _f.release(mem); } -void WrappedFunction::call_accumulate(std::vector> &out) +void WrappedFunction::call_accumulate(std::vector> &out) { // call function (allocation-free) casadi_int mem = _f.checkout(); @@ -175,7 +174,7 @@ void WrappedFunction::call_accumulate(std::vector> & _f.release(mem); } -const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const +const MatrixXr& WrappedFunction::getOutput(int i) const { if(_out_matrix[i].hasNaN() || !_out_matrix[i].allFinite()) { @@ -183,7 +182,7 @@ const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const std::cout << "output #" << i << ": \n" << _out_matrix[i].format(3) << "\n"; for(int j = 0; j < _f.n_in(); j++) { - auto in = Eigen::VectorXd::Map(_in_buf[j], _f.size1_in(j)); + auto in = VectorXr::Map(_in_buf[j], _f.size1_in(j)); std::cout << "input #" << j << ": \n" << in.transpose().format(3) << "\n"; } } @@ -191,12 +190,12 @@ const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const return _out_matrix[i]; } -const Eigen::SparseMatrix& WrappedFunction::getSparseOutput(int i) const +const Eigen::SparseMatrix& WrappedFunction::getSparseOutput(int i) const { return _out_matrix_sparse[i]; } -Eigen::MatrixXd& WrappedFunction::out(int i) +MatrixXr& WrappedFunction::out(int i) { return _out_matrix[i]; } @@ -219,13 +218,13 @@ bool WrappedFunction::is_valid() const void WrappedFunction::csc_to_sparse_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::SparseMatrix& matrix) + const std::vector& data, + Eigen::SparseMatrix& matrix) { - std::vector> triplet_list; + std::vector> triplet_list; triplet_list.reserve(data.size()); for(unsigned int i = 0; i < data.size(); ++i) - triplet_list.push_back(Eigen::Triplet(sp_rows[i], sp_cols[i], data[i])); + triplet_list.push_back(Eigen::Triplet(sp_rows[i], sp_cols[i], data[i])); matrix.setFromTriplets(triplet_list.begin(), triplet_list.end()); } @@ -233,14 +232,14 @@ void WrappedFunction::csc_to_sparse_matrix(const casadi::Sparsity& sp, void WrappedFunction::csc_to_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::MatrixXd& matrix) + const std::vector& data, + MatrixXr& matrix) { // if dense output, do copy assignment which should be // faster if(sp.is_dense()) { - matrix = Eigen::MatrixXd::Map(data.data(), + matrix = MatrixXr::Map(data.data(), matrix.rows(), matrix.cols()); @@ -262,14 +261,14 @@ void WrappedFunction::csc_to_matrix(const casadi::Sparsity& sp, void WrappedFunction::csc_to_matrix_accu(const casadi::Sparsity &sp, const std::vector &sp_rows, const std::vector &sp_cols, - const std::vector &data, - Eigen::Ref matrix) + const std::vector &data, + Eigen::Ref matrix) { // if dense output, do copy assignment which should be // faster if(sp.is_dense()) { - matrix += Eigen::MatrixXd::Map(data.data(), + matrix += MatrixXr::Map(data.data(), matrix.rows(), matrix.cols()); diff --git a/horizon/cpp/src/wrapped_function.h b/horizon/cpp/src/wrapped_function.h index 071ebc7e..ca7b2519 100644 --- a/horizon/cpp/src/wrapped_function.h +++ b/horizon/cpp/src/wrapped_function.h @@ -5,9 +5,15 @@ #include #include +#include "typedefs.h" + namespace casadi_utils { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + template void toCasadiMatrix(const Eigen::Matrix& E, casadi::Matrix& C) { @@ -35,7 +41,7 @@ class WrappedSparseMatrix to_triplets(E, _r, _c); _s = casadi::Sparsity::triplet(E.rows(), E.cols(), _r, _c); _values = casadi::DM::zeros(E.nonZeros()); - std::memcpy(_values.ptr(), E.valuePtr(), sizeof(double)*E.nonZeros()); + std::memcpy(_values.ptr(), E.valuePtr(), sizeof(Real)*E.nonZeros()); _C = casadi::Matrix(_s, _values); } @@ -106,14 +112,14 @@ class WrappedFunction WrappedFunction(const WrappedFunction&); WrappedFunction& operator=(const WrappedFunction&); - void setInput(int i, Eigen::Ref xi); + void setInput(int i, Eigen::Ref xi); void call(bool sparse = false); - void call_accumulate(std::vector>& out); - const Eigen::MatrixXd& getOutput(int i) const; - const Eigen::SparseMatrix& getSparseOutput(int i) const; + void call_accumulate(std::vector>& out); + const MatrixXr& getOutput(int i) const; + const Eigen::SparseMatrix& getSparseOutput(int i) const; casadi::Function& functionRef(); const casadi::Function& function() const; - Eigen::MatrixXd& out(int i); + MatrixXr& out(int i); bool is_valid() const; @@ -122,28 +128,28 @@ class WrappedFunction void csc_to_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::MatrixXd& matrix); + const std::vector& data, + MatrixXr& matrix); void csc_to_matrix_accu(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::Ref matrix); + const std::vector& data, + Eigen::Ref matrix); void csc_to_sparse_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::SparseMatrix& matrix); - - std::vector _in_buf; - std::vector> _out_data; - std::vector _out_matrix; - std::vector > _out_matrix_sparse; - std::vector _out_buf; + const std::vector& data, + Eigen::SparseMatrix& matrix); + + std::vector _in_buf; + std::vector> _out_data; + std::vector _out_matrix; + std::vector > _out_matrix_sparse; + std::vector _out_buf; std::vector _iw; - std::vector _dw; + std::vector _dw; std::vector> _rows; std::vector> _cols; diff --git a/horizon/cpp/tests/testCasadiUtils.cpp b/horizon/cpp/tests/testCasadiUtils.cpp index cf2d0403..7accf42f 100644 --- a/horizon/cpp/tests/testCasadiUtils.cpp +++ b/horizon/cpp/tests/testCasadiUtils.cpp @@ -1,8 +1,13 @@ #include #include "../src/wrapped_function.h" +#include "typedefs.h" namespace{ +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + class testCasadiUtils: public ::testing::Test { protected: @@ -27,14 +32,14 @@ class testCasadiUtils: public ::testing::Test TEST_F(testCasadiUtils, testSparseHessian) { // // This compile - Eigen::MatrixXd A, B; + MatrixXr A, B; A.resize(8,8); A.setRandom(8,8); B.resize(8, 8); B.triangularView() = A.transpose()*A; //This does not compile - Eigen::SparseMatrix J, H; + Eigen::SparseMatrix J, H; J.resize(8, 8); J.setIdentity(); H.resize(8, 8); @@ -46,19 +51,19 @@ TEST_F(testCasadiUtils, testSparseHessian) -void EXPECT_EQUAL(const Eigen::SparseMatrix& E, const casadi::DM& C) +void EXPECT_EQUAL(const Eigen::SparseMatrix& E, const casadi::DM& C) { EXPECT_EQ(E.rows(), C.rows()); EXPECT_EQ(E.cols(), C.columns()); EXPECT_EQ(E.nonZeros(), C.nnz()); - std::vector e; + std::vector e; e.assign(E.valuePtr(), E.valuePtr() + E.nonZeros()); - std::vector c; + std::vector c; c.assign(C->data(), C->data() + C.nnz()); for(unsigned int i = 0; i < e.size(); ++i) - EXPECT_DOUBLE_EQ(e[i], c[i]); + EXPECT_Real_EQ(e[i], c[i]); } @@ -66,28 +71,28 @@ TEST_F(testCasadiUtils, toCasadiSparse) { std::default_random_engine gen; - std::uniform_real_distribution dist(0.0,1.0); + std::uniform_real_distribution dist(0.0,1.0); int rows=100; int cols=100; - std::vector > tripletList; + std::vector > tripletList; for(int i=0;i(i,j,v_ij)); //if larger than treshold, insert it + tripletList.push_back(Eigen::Triplet(i,j,v_ij)); //if larger than treshold, insert it } } - Eigen::SparseMatrix E(rows,cols); + Eigen::SparseMatrix E(rows,cols); E.setFromTriplets(tripletList.begin(), tripletList.end()); //std::cout<<"E: "< C(E); + casadi_utils::WrappedSparseMatrix C(E); auto toc = std::chrono::high_resolution_clock::now(); std::cout<<"Constructor: "<<(toc-tic).count()*1E-9<<" [s]"< #include "../src/ilqr.h" - +#include "typedefs.h" class testIlqr : public ::testing::Test { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using Matrix2r=horizon::Matrix2r; +using Vector2r=horizon::Vector2r; protected: testIlqr(){ @@ -70,7 +75,7 @@ TEST_F(testIlqr, checkResiduals) { ilqr->setConstraint({10}, fc); - Eigen::Vector2d x0(1, 1); + Vector2r x0(1, 1); ilqr->setInitialState(x0); diff --git a/horizon/cpp/tests/testQr.cpp b/horizon/cpp/tests/testQr.cpp index f6533653..70d5e822 100644 --- a/horizon/cpp/tests/testQr.cpp +++ b/horizon/cpp/tests/testQr.cpp @@ -4,22 +4,29 @@ #include #include +#include "typedefs.h" + +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using Matrix2r=horizon::Matrix2r; +using Vector2r=horizon::Vector2r; TEST(testQr, rank) { - Eigen::MatrixXd A, R; - Eigen::HouseholderQR qr; + MatrixXr A, R; + Eigen::HouseholderQR qr; for(int i = 0; i < 100000; i++) { - A = Eigen::MatrixXd::Random(10, 3)*Eigen::MatrixXd::Random(3, 5); + A = MatrixXr::Random(10, 3)*MatrixXr::Random(3, 5); qr.compute(A); R = qr.matrixQR().triangularView(); EXPECT_LT(std::fabs(R(3, 3)), 1e-9); EXPECT_LT(std::fabs(R(4, 4)), 1e-9); - A = Eigen::MatrixXd::Random(5, 3)*Eigen::MatrixXd::Random(3, 10); + A = MatrixXr::Random(5, 3)*MatrixXr::Random(3, 10); qr.compute(A); R = qr.matrixQR().triangularView(); @@ -33,7 +40,7 @@ TEST(testDecomp, compare) int nu = 18 + 12; int nc = 6 + 12; - Eigen::MatrixXd K, Huu, D; + MatrixXr K, Huu, D; K.setZero(nu+nc, nu+nc); Huu.setIdentity(nu, nu); @@ -42,14 +49,14 @@ TEST(testDecomp, compare) K.bottomLeftCorner(nc, nu) = D; K.topRightCorner(nu, nc) = D.transpose(); - Eigen::ColPivHouseholderQR qr(K); - Eigen::BDCSVD bdcsvd(K); - Eigen::JacobiSVD jsvd(K); - Eigen::CompleteOrthogonalDecomposition cod(K); - Eigen::FullPivLU lu(K); + Eigen::ColPivHouseholderQR qr(K); + Eigen::BDCSVD bdcsvd(K); + Eigen::JacobiSVD jsvd(K); + Eigen::CompleteOrthogonalDecomposition cod(K); + Eigen::FullPivLU lu(K); int n_trials = 1000; - std::map> times; + std::map> times; using hrc = std::chrono::high_resolution_clock; hrc::time_point tic, toc; for(int i = 0; i < n_trials; i++) @@ -65,7 +72,7 @@ TEST(testDecomp, compare) tic = hrc::now(); qr.compute(K); - Eigen::MatrixXd q = qr.householderQ(); + MatrixXr q = qr.householderQ(); toc = hrc::now(); times["qr"].push_back((toc - tic).count()*1e-3); @@ -102,21 +109,21 @@ TEST(testDecomp, compare) TEST(testLdlt, basic) { #if false - Eigen::MatrixXd L; + MatrixXr L; L.setZero(3, 3); L.triangularView() = L.Random(3, 3); L.diagonal().setConstant(1); - Eigen::VectorXd d = d.Random(3); + VectorXr d = d.Random(3); - Eigen::MatrixXd K = L*d.asDiagonal()*L.transpose(); + MatrixXr K = L*d.asDiagonal()*L.transpose(); std::cout << "L=\n" << L << std::endl; std::cout << "d = " << d.transpose() << std::endl; int n = K.rows(); - Eigen::VectorXd AP = AP.Zero(n*(n+1)/2); + VectorXr AP = AP.Zero(n*(n+1)/2); Eigen::VectorXi ipiv(n); diff --git a/horizon/problem.py b/horizon/problem.py index 085f4268..566d4f48 100644 --- a/horizon/problem.py +++ b/horizon/problem.py @@ -340,6 +340,13 @@ def setInitialStateSoft(self, x0_meas: np.ndarray, lower_bound_relaxed=np.minimum(x0_meas,x0_internal) upper_bound_relaxed=np.maximum(x0_meas,x0_internal) + + delta=0.01 + # lower_bound_relaxed=x0_internal-delta + # upper_bound_relaxed=x0_internal+delta + # lower_bound_relaxed=x0_meas-delta + # upper_bound_relaxed=x0_meas+delta + # relax state bound on first node to allow some mismatch self.getState().setBounds(lb=lower_bound_relaxed, ub=upper_bound_relaxed, nodes=0) From 2345547b9ed5f376ecea2fb0fb2124e6c07f6eef Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 8 Aug 2024 15:05:26 +0200 Subject: [PATCH 44/70] fixed some typos, float still not working due to casadi::function --- horizon/cpp/pyilqr_helpers.h | 2 +- horizon/cpp/pysqp_helpers.h | 2 +- horizon/cpp/src/codegen_function.cpp | 8 ++++++-- horizon/cpp/src/sqp.h | 1 - horizon/cpp/src/typedefs.h | 8 ++++++++ horizon/cpp/src/wrapped_function.cpp | 3 +++ horizon/cpp/tests/testCasadiUtils.cpp | 12 ++++++------ horizon/cpp/tests/testIlqr.cpp | 8 +++++--- horizon/cpp/tests/testQr.cpp | 2 +- 9 files changed, 31 insertions(+), 15 deletions(-) diff --git a/horizon/cpp/pyilqr_helpers.h b/horizon/cpp/pyilqr_helpers.h index 17aa7c52..f1d8c621 100644 --- a/horizon/cpp/pyilqr_helpers.h +++ b/horizon/cpp/pyilqr_helpers.h @@ -8,7 +8,7 @@ #include #include -#include "typedefs.h" +#include "src/typedefs.h" namespace py = pybind11; using namespace horizon; diff --git a/horizon/cpp/pysqp_helpers.h b/horizon/cpp/pysqp_helpers.h index c6a6cff7..9d75b365 100644 --- a/horizon/cpp/pysqp_helpers.h +++ b/horizon/cpp/pysqp_helpers.h @@ -9,7 +9,7 @@ #include #include #include -#include "typedefs.h" +#include "src/typedefs.h" namespace py = pybind11; using namespace horizon; diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 70217ab4..5c01b8c1 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -16,7 +16,6 @@ namespace using Real=horizon::Real; using MatrixXr=horizon::MatrixXr; using VectorXr=horizon::VectorXr; -using RInfinity=horizon::RInfinity; class RestoreCwd { @@ -135,7 +134,12 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string } // else, generate and compile - f.generate(fname + ".c"); + #ifdef HORIZON_FLOAT32 + casadi::Dict opts={{"casadi_real", "float"}}; + #else + casadi::Dict opts={{"casadi_real", "double"}}; + #endif + f.generate(fname + ".c",opts); if (verbose) { std::cout << "not found: compiling " << fname << "... \n"; diff --git a/horizon/cpp/src/sqp.h b/horizon/cpp/src/sqp.h index a1159ad9..9bd0ece5 100644 --- a/horizon/cpp/src/sqp.h +++ b/horizon/cpp/src/sqp.h @@ -27,7 +27,6 @@ class SQPGaussNewton using Real=horizon::Real; using MatrixXr=horizon::MatrixXr; using VectorXr=horizon::VectorXr; -using RInfinity=horizon::RInfinity; public: diff --git a/horizon/cpp/src/typedefs.h b/horizon/cpp/src/typedefs.h index 2c0093c2..885fee86 100644 --- a/horizon/cpp/src/typedefs.h +++ b/horizon/cpp/src/typedefs.h @@ -8,8 +8,10 @@ namespace horizon #ifdef HORIZON_FLOAT32 typedef float Real; +#define casadi_real float #else typedef double Real; +#define casadi_real double #endif typedef Eigen::Matrix VectorXr; @@ -22,6 +24,12 @@ typedef Eigen::Matrix RowVectorXr; const Real RInfinity = std::numeric_limits::infinity(); +#ifdef HORIZON_FLOAT32 +#define EXPECT_Real_EQ EXPECT_FLOAT_EQ +#else +#define EXPECT_Real_EQ EXPECT_DOUBLE_EQ +#endif + } #endif // TYPDEFS_H \ No newline at end of file diff --git a/horizon/cpp/src/wrapped_function.cpp b/horizon/cpp/src/wrapped_function.cpp index 7e0c2ef8..934fc6bc 100644 --- a/horizon/cpp/src/wrapped_function.cpp +++ b/horizon/cpp/src/wrapped_function.cpp @@ -18,6 +18,9 @@ WrappedFunction &WrappedFunction::operator=(casadi::Function f) } _f = f; + // const std::string dtype =""; + // const GenericType = ; + // _f.change_option(); // resize work vectors _iw.assign(_f.sz_iw(), 0); diff --git a/horizon/cpp/tests/testCasadiUtils.cpp b/horizon/cpp/tests/testCasadiUtils.cpp index 7accf42f..fc696d65 100644 --- a/horizon/cpp/tests/testCasadiUtils.cpp +++ b/horizon/cpp/tests/testCasadiUtils.cpp @@ -1,6 +1,6 @@ #include #include "../src/wrapped_function.h" -#include "typedefs.h" +#include "../src/typedefs.h" namespace{ @@ -63,7 +63,7 @@ void EXPECT_EQUAL(const Eigen::SparseMatrix& E, const casadi::DM& C) c.assign(C->data(), C->data() + C.nnz()); for(unsigned int i = 0; i < e.size(); ++i) - EXPECT_Real_EQ(e[i], c[i]); + EXPECT_EQ(e[i], c[i]); } @@ -130,7 +130,7 @@ TEST_F(testCasadiUtils, testToCasadiMatrix) for(unsigned int i = 0; i < E.rows(); ++i) { for(unsigned int j = 0; j < E.cols(); ++j) - EXPECT_Real_EQ(E(i,j), Real(C(i,j))); + EXPECT_EQ(E(i,j), Real(C(i,j))); } MatrixXr EE; @@ -142,7 +142,7 @@ TEST_F(testCasadiUtils, testToCasadiMatrix) for(unsigned int i = 0; i < EE.rows(); ++i) { for(unsigned int j = 0; j < EE.cols(); ++j) - EXPECT_Real_EQ(EE(i,j), Real(C(i,j))); + EXPECT_EQ(EE(i,j), Real(C(i,j))); } std::cout<<"E: \n"< #include "../src/ilqr.h" -#include "typedefs.h" +#include "../src/typedefs.h" -class testIlqr : public ::testing::Test -{ using Real=horizon::Real; using MatrixXr=horizon::MatrixXr; using VectorXr=horizon::VectorXr; using Matrix2r=horizon::Matrix2r; using Vector2r=horizon::Vector2r; + +class testIlqr : public ::testing::Test +{ + protected: testIlqr(){ diff --git a/horizon/cpp/tests/testQr.cpp b/horizon/cpp/tests/testQr.cpp index 70d5e822..59b62a43 100644 --- a/horizon/cpp/tests/testQr.cpp +++ b/horizon/cpp/tests/testQr.cpp @@ -4,7 +4,7 @@ #include #include -#include "typedefs.h" +#include "../src/typedefs.h" using Real=horizon::Real; using MatrixXr=horizon::MatrixXr; From 9ec62c450860531f13934ab5c0b11b86a9f4f758 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 9 Aug 2024 15:50:18 +0200 Subject: [PATCH 45/70] getNodes now returning a list of both active and feasible nodes --- horizon/functions.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/horizon/functions.py b/horizon/functions.py index abbc7883..39780d84 100644 --- a/horizon/functions.py +++ b/horizon/functions.py @@ -100,7 +100,10 @@ def getNodes(self) -> list: a list of the nodes where the function is active """ - return misc.getNodesFromBinary(self._active_nodes_array) + # we only want to get node indexes which are both feasible and active + active_and_feasible=np.logical_and(self._active_nodes_array,self._feas_nodes_array) + + return misc.getNodesFromBinary(active_and_feasible) def setNodes(self, nodes, erasing=True): """ From 769a8e6c4fd80372c884dbd38321af841aeacd36 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 13 Aug 2024 11:13:36 +0200 Subject: [PATCH 46/70] added method to get derivative of polynomial traj --- horizon/utils/trajectoryGenerator.py | 62 +++++++++++++++++++++++++--- 1 file changed, 56 insertions(+), 6 deletions(-) diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index d4b377c3..38d98894 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -68,16 +68,66 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None): return y_bpoly + def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None): + if derivatives is None: + derivatives = [None] * len(p_start) + + cxi = [0, 0.5, 1] + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, val in enumerate(derivatives): + yder.append([cyi[i], val] if val is not None else [cyi[i]]) + + bpoly = BPoly.from_derivatives(cxi, yder) + + # Compute the derivative of the BPoly object + bpoly_derivative = bpoly.derivative() + + # Evaluate the derivative at the given points + y_bpoly_derivative = bpoly_derivative(xcurve) + + return y_bpoly_derivative + if __name__ == '__main__': tg = TrajectoryGenerator() - # # - # # + n_samples = 100 - # z_trj = tg.compute_polynomial_trajectory(0, range(n_samples), n_samples - 1, [0, 0, 0], [0, 0, 0], 1, dim=2) - z_trj = tg.from_derivatives(n_samples, -1, -1, 1, derivatives=[None, 0, None]) + derivatives=[None, 0, 0] + start_pos = -1 + end_pos = -1 + height = 1 + # Original trajectory + z_trj = tg.from_derivatives(n_samples, start_pos, end_pos, height, derivatives=derivatives) + + # Derivative of the trajectory + z_trj_derivative = tg.derivative_of_trajectory(n_samples, start_pos, end_pos, height, derivatives=derivatives) + axis = np.linspace(0, 1, num=z_trj.shape[0]) - print(z_trj) - plt.plot(axis, z_trj) + + # Plotting the trajectory + plt.figure(figsize=(12, 6)) + + plt.subplot(1, 2, 1) + plt.plot(axis, z_trj, label="Trajectory") + plt.title("Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Position") + plt.grid() + plt.legend() + + # Plotting the derivative of the trajectory + plt.subplot(1, 2, 2) + plt.plot(axis, z_trj_derivative, label="Trajectory Derivative", linestyle="--") + plt.title("Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Velocity") plt.grid() + plt.legend() + + plt.tight_layout() plt.show() + From 1a516068b88e3f26e0ed2102556024270a188dca Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 13 Aug 2024 11:24:09 +0200 Subject: [PATCH 47/70] added options to also specify second derivate --- horizon/utils/trajectoryGenerator.py | 124 +++++++++++++++++++++------ 1 file changed, 100 insertions(+), 24 deletions(-) diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index 38d98894..014d78a5 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -48,29 +48,38 @@ def compute_polynomial_trajectory(self, k_start, nodes, nodes_duration, p_start, return np.array(traj_array) - def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None): - + def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): if derivatives is None: derivatives = [None] * len(p_start) - + + if second_der is None: + second_der = [None] * len(p_start) cxi = [0, 0.5, 1] - cyi = [p_start, p_goal + clearance, p_goal] + cyi = [p_start, p_goal + clearance, p_goal] - xcurve = linspace(0, 1, nodes) + xcurve = np.linspace(0, 1, nodes) yder = [] - for i, val in enumerate(derivatives): - yder.append([cyi[i], val] if val is not None else [cyi[i]]) + for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) y_bpoly = bpoly(xcurve) return y_bpoly - def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None): + def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): if derivatives is None: derivatives = [None] * len(p_start) + + if second_der is None: + second_der = [None] * len(p_start) cxi = [0, 0.5, 1] cyi = [p_start, p_goal + clearance, p_goal] @@ -78,12 +87,17 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative xcurve = np.linspace(0, 1, nodes) yder = [] - for i, val in enumerate(derivatives): - yder.append([cyi[i], val] if val is not None else [cyi[i]]) + for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) - # Compute the derivative of the BPoly object + # Compute the first derivative of the BPoly object bpoly_derivative = bpoly.derivative() # Evaluate the derivative at the given points @@ -91,27 +105,82 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative return y_bpoly_derivative + def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): + if derivatives is None: + derivatives = [None] * len(p_start) + + if second_der is None: + second_der = [None] * len(p_start) + + cxi = [0, 0.5, 1] + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + yder.append(constraints) + + bpoly = BPoly.from_derivatives(cxi, yder) + + # Compute the second derivative of the BPoly object + bpoly_second_derivative = bpoly.derivative().derivative() + + # Evaluate the second derivative at the given points + y_bpoly_second_derivative = bpoly_second_derivative(xcurve) + + return y_bpoly_second_derivative + if __name__ == '__main__': tg = TrajectoryGenerator() n_samples = 100 - derivatives=[None, 0, 0] - start_pos = -1 - end_pos = -1 - height = 1 - # Original trajectory - z_trj = tg.from_derivatives(n_samples, start_pos, end_pos, height, derivatives=derivatives) - # Derivative of the trajectory - z_trj_derivative = tg.derivative_of_trajectory(n_samples, start_pos, end_pos, height, derivatives=derivatives) + der= [None, 0, 0] + second_der=[None, 0, 0] + + # Original trajectory with first and second derivative constraints + z_trj = tg.from_derivatives( + n_samples, + -1, + -1, + 1, + derivatives=der, + second_der=second_der + ) + + # First derivative of the trajectory with first and second derivative constraints + z_trj_derivative = tg.derivative_of_trajectory( + n_samples, + -1, + -1, + 1, + derivatives=der, + second_der=second_der + ) + + # Second derivative of the trajectory with first and second derivative constraints + z_trj_second_derivative = tg.second_derivative_of_trajectory( + n_samples, + -1, + -1, + 1, + derivatives=der, + second_der=second_der + ) axis = np.linspace(0, 1, num=z_trj.shape[0]) - # Plotting the trajectory - plt.figure(figsize=(12, 6)) + # Plotting the trajectory, its first derivative, and its second derivative + plt.figure(figsize=(18, 6)) - plt.subplot(1, 2, 1) + plt.subplot(1, 3, 1) plt.plot(axis, z_trj, label="Trajectory") plt.title("Trajectory") plt.xlabel("Time (normalized)") @@ -119,8 +188,7 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative plt.grid() plt.legend() - # Plotting the derivative of the trajectory - plt.subplot(1, 2, 2) + plt.subplot(1, 3, 2) plt.plot(axis, z_trj_derivative, label="Trajectory Derivative", linestyle="--") plt.title("Derivative of Trajectory") plt.xlabel("Time (normalized)") @@ -128,6 +196,14 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative plt.grid() plt.legend() + plt.subplot(1, 3, 3) + plt.plot(axis, z_trj_second_derivative, label="Trajectory Second Derivative", linestyle="-.") + plt.title("Second Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Acceleration") + plt.grid() + plt.legend() + plt.tight_layout() plt.show() From 345d43c3da70b93d143496fab4d8cf01e37c8aab Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 13 Aug 2024 13:15:15 +0200 Subject: [PATCH 48/70] fixed 7 being always reported when getting CartesianTask dimension (can be less depending on task size) --- horizon/rhc/tasks/cartesianTask.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/tasks/cartesianTask.py b/horizon/rhc/tasks/cartesianTask.py index 1af79198..b50ba615 100644 --- a/horizon/rhc/tasks/cartesianTask.py +++ b/horizon/rhc/tasks/cartesianTask.py @@ -255,6 +255,7 @@ def __initialize(self): self.vel_tgt = self.prb.createParameter( f'{frame_name}_tgt', self.indices.size) self.ref = self.vel_tgt + # exit() fun = ee_rel[self.indices] - self.vel_tgt elif self.cartesian_type == 'acceleration': @@ -339,8 +340,7 @@ def addReference(self): # , ref_traj): return True def getDim(self): - # todo: if its position is seven, if its velocity is 6 (now it's one because BUGS) - return 7 + return self.indices.size def getValues(self): # necessary method for using this task as an item + reference in phaseManager From fda525886fb4e3c4d480aa48222e211d5342939f Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 23 Aug 2024 18:18:36 +0200 Subject: [PATCH 49/70] solved bug: now reporting 7 in case of position task, otherwise indices dim --- horizon/rhc/tasks/cartesianTask.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/horizon/rhc/tasks/cartesianTask.py b/horizon/rhc/tasks/cartesianTask.py index b50ba615..09db6454 100644 --- a/horizon/rhc/tasks/cartesianTask.py +++ b/horizon/rhc/tasks/cartesianTask.py @@ -340,7 +340,10 @@ def addReference(self): # , ref_traj): return True def getDim(self): - return self.indices.size + if self.cartesian_type == 'position': + return 7 + else: + return self.indices.size def getValues(self): # necessary method for using this task as an item + reference in phaseManager From 4d710a2ba84cd56555e66d00c92c53903a160b44 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Tue, 27 Aug 2024 13:48:54 +0200 Subject: [PATCH 50/70] renamed flag for compilation with float instead of double --- horizon/cpp/CMakeLists.txt | 7 +++---- horizon/cpp/src/codegen_function.cpp | 2 +- horizon/cpp/src/typedefs.h | 4 ++-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index a0ee213d..26b4faae 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/horizon/cpp/CMakeLists.txt @@ -9,13 +9,13 @@ endif() # options option(HORIZON_PROFILING OFF "enable profiling features") -option(HORIZON_FLOAT32 OFF "use float32 for data representation") +option(ILQR_FLOAT OFF "use float for data representation instead of double") if(${HORIZON_PROFILING}) add_definitions(-DHORIZON_PROFILING) endif() -if(${HORIZON_FLOAT32}) - add_definitions(-DHORIZON_FLOAT32) +if(${ILQR_FLOAT}) + add_definitions(-DILQR_FLOAT) endif() @@ -49,7 +49,6 @@ add_library(sqp STATIC src/sqp.cpp ) - target_link_libraries(ilqr Eigen3::Eigen casadi pthread) target_link_libraries(sqp Eigen3::Eigen casadi ilqr) diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 5c01b8c1..3ba61382 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -134,7 +134,7 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string } // else, generate and compile - #ifdef HORIZON_FLOAT32 + #ifdef ILQR_FLOAT casadi::Dict opts={{"casadi_real", "float"}}; #else casadi::Dict opts={{"casadi_real", "double"}}; diff --git a/horizon/cpp/src/typedefs.h b/horizon/cpp/src/typedefs.h index 885fee86..104a9fd6 100644 --- a/horizon/cpp/src/typedefs.h +++ b/horizon/cpp/src/typedefs.h @@ -6,7 +6,7 @@ namespace horizon { -#ifdef HORIZON_FLOAT32 +#ifdef ILQR_FLOAT typedef float Real; #define casadi_real float #else @@ -24,7 +24,7 @@ typedef Eigen::Matrix RowVectorXr; const Real RInfinity = std::numeric_limits::infinity(); -#ifdef HORIZON_FLOAT32 +#ifdef ILQR_FLOAT #define EXPECT_Real_EQ EXPECT_FLOAT_EQ #else #define EXPECT_Real_EQ EXPECT_DOUBLE_EQ From 7fc30ec50dcd8dfd93168a590fb8373b13e514cb Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Fri, 30 Aug 2024 17:43:18 +0200 Subject: [PATCH 51/70] temporarily removed setup.py to avoid running builds --- pyproject.toml | 11 +++++++ setup.cfg => setup.cfg.old | 0 setup.py => setup.py.old | 64 +++++++++++++++++++++++--------------- 3 files changed, 50 insertions(+), 25 deletions(-) create mode 100644 pyproject.toml rename setup.cfg => setup.cfg.old (100%) rename setup.py => setup.py.old (50%) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..ecdf79bc --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,11 @@ +[build-system] +requires = ["flit_core >=2,<4"] +build-backend = "flit_core.buildapi" + +[project] +name = "horizon" +version = "0.4.5" +description = "" +authors = [{name = "Francesco Ruscelli", email = "francesco.ruscelli@iit.it"}] +readme = "README.md" +license = {file = "LICENSE.md"} \ No newline at end of file diff --git a/setup.cfg b/setup.cfg.old similarity index 100% rename from setup.cfg rename to setup.cfg.old diff --git a/setup.py b/setup.py.old similarity index 50% rename from setup.py rename to setup.py.old index f71fe954..ce529f39 100644 --- a/setup.py +++ b/setup.py.old @@ -1,8 +1,6 @@ import setuptools - from setuptools.command.develop import develop from setuptools.command.build_py import build_py - import os import codecs import subprocess @@ -21,40 +19,56 @@ def get_version(rel_path): raise RuntimeError("Unable to find version string.") def _pre_build(dirname): - # create a build dir and run 'make generate_python_package' current_dir = os.getcwd() print('building from dir: ', current_dir) os.makedirs(dirname, exist_ok=True) build_dir = current_dir + '/' + dirname try: - p = subprocess.run(["cmake", "-DCMAKE_BUILD_TYPE=Release", "../horizon/cpp"], cwd=build_dir) - except subprocess.CalledProcessError: - raise + subprocess.run(["cmake", "-DCMAKE_BUILD_TYPE=Release", "../horizon/cpp"], cwd=build_dir, check=True) + subprocess.run(["make", "-j8"], cwd=build_dir, check=True) + subprocess.run(["make", "generate_python_package", "-j8"], cwd=build_dir, check=True) + except subprocess.CalledProcessError as e: + raise RuntimeError("Build failed") from e - try: - p = subprocess.run(["make", "-j8"], cwd=build_dir) - except subprocess.CalledProcessError: - raise +class CustomBuild(build_py): + user_options = build_py.user_options + [ + ('skip-build', None, 'Skip custom build steps') + ] - try: - p = subprocess.run(["make", "generate_python_package", "-j8"], cwd=build_dir) - except subprocess.CalledProcessError: - raise + def initialize_options(self): + build_py.initialize_options(self) + self.skip_build = False + + def finalize_options(self): + build_py.finalize_options(self) -class CustomBuild(build_py): - # called by pip install and by python setup.py build and python setup.py install - # build_py is not called by pip install -e def run(self): - dir_name = 'temp_build' - _pre_build(dir_name) + if not self.skip_build: + dir_name = 'temp_build' + _pre_build(dir_name) + else: + print("Skipping custom build steps") build_py.run(self) class CustomDevelop(develop): - # called by pip install -e + user_options = develop.user_options + [ + ('skip-build', None, 'Skip custom build steps') + ] + + def initialize_options(self): + develop.initialize_options(self) + self.skip_build = False + + def finalize_options(self): + develop.finalize_options(self) + def run(self): - dir_name = 'temp_build' - _pre_build(dir_name) + if not self.skip_build: + dir_name = 'temp_build' + _pre_build(dir_name) + else: + print("Skipping custom build steps") develop.run(self) setuptools.setup( @@ -65,11 +79,10 @@ def run(self): description="Library for Trajectory Optimization based on CasADi", long_description_content_type="text/markdown", url="https://github.com/ADVRHumanoids/horizon", - packages=['horizon', 'horizon.utils', 'horizon.solvers', 'horizon.transcriptions', 'horizon.examples', 'horizon.ros', 'horizon.rhc', 'horizon.rhc.tasks'], + packages=['horizon.', 'horizon.horizon.utils', 'horizon.horizon.solvers', 'horizon.horizon.transcriptions', 'horizon.horizon.examples', 'horizon.horizon.ros', 'horizon.horizon.rhc', 'horizon.horizon.rhc.tasks'], install_requires=['numpy', 'matplotlib', 'scipy', 'casadi-kin-dyn', 'rospkg'], python_requires=">=3.6", - cmdclass={'build_py': CustomBuild, - 'develop': CustomDevelop}, + cmdclass={'build_py': CustomBuild, 'develop': CustomDevelop}, ext_modules=[ setuptools.Extension( name="ilqrext", sources=[] @@ -79,3 +92,4 @@ def run(self): ) ] ) + From 6df865a1167c71b8e4bcf4f667f3f645982bd62e Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Fri, 13 Sep 2024 15:09:30 +0200 Subject: [PATCH 52/70] allowing evaluation of effort on any node now --- horizon/rhc/taskInterface.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index d28fecca..085fb07d 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -154,20 +154,20 @@ def init_inv_dyn_for_res(self): raise Exception("The method init_inv_dyn_for_res from " + __class__.__name__ + " can only be called after bootstrap() has returned!") - def eval_efforts_on_first_node(self): + def eval_efforts_on_node(self, node_idx: int =0): for frame, wrench in self.model.fmap.items(): # we update the force maps from the latest solution - self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, 0] # it's an input + self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, node_idx] # it's an input # we get it from node 0 # compute torque with inverse dynamics (states from node 1, inputs from # node 0) - tau_i = self.res_id.call(self.solution['q'][:, 1], - self.solution['v'][:, 1], - self.solution['a'][:, 0], + tau_i = self.res_id.call(self.solution['q'][:, node_idx+1], + self.solution['v'][:, node_idx+1], + self.solution['a'][:, node_idx], self.fmap_0) return tau_i.toarray() From f143cb67aa2e175b161c908f2eaca1fd6cdd3f04 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Oct 2024 13:59:09 +0200 Subject: [PATCH 53/70] fixed efforts evaluation --- horizon/rhc/taskInterface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 085fb07d..a9467312 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -165,8 +165,8 @@ def eval_efforts_on_node(self, node_idx: int =0): # compute torque with inverse dynamics (states from node 1, inputs from # node 0) - tau_i = self.res_id.call(self.solution['q'][:, node_idx+1], - self.solution['v'][:, node_idx+1], + tau_i = self.res_id.call(self.solution['q'][:, node_idx], + self.solution['v'][:, node_idx], self.solution['a'][:, node_idx], self.fmap_0) From 70ff0e696ba939a72169c98822bd25999b028dfe Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Oct 2024 17:48:02 +0200 Subject: [PATCH 54/70] cleaned imports: in particular, removed matplotlib import from ilqr solver (100MB of lib!!!) --- horizon/rhc/RecedingHorizon.py | 4 ++-- horizon/solvers/ilqr.py | 3 ++- horizon/utils/trajectoryGenerator.py | 9 ++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/horizon/rhc/RecedingHorizon.py b/horizon/rhc/RecedingHorizon.py index 30160d46..b7871a07 100644 --- a/horizon/rhc/RecedingHorizon.py +++ b/horizon/rhc/RecedingHorizon.py @@ -6,8 +6,6 @@ from horizon.rhc.action_manager.ActionManager import ActionManager import numpy as np import horizon.utils.kin_dyn as kd -import matplotlib.pyplot as plt -from horizon.ros import replay_trajectory from typing import Union from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint @@ -299,6 +297,8 @@ def __init_replayer(self): raise Exception("ROS required for replayer.") contact_list_repl = list(self.model.cmap.keys()) + from horizon.ros import replay_trajectory + self.repl = replay_trajectory.replay_trajectory(self.dt, self.model.kd.joint_names(), np.array([]), {k: None for k in self.model.fmap.keys()}, self.model.kd_frame, self.model.kd, diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 9c4a9158..b96cb138 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -12,7 +12,6 @@ from horizon.transcriptions import integrators import casadi as cs import numpy as np -from matplotlib import pyplot as plt class SolverILQR(Solver): @@ -83,6 +82,7 @@ def __init__(self, # set a default iteration callback self.plot_iter = False + self.xax = None self.uax = None self.dax = None @@ -371,6 +371,7 @@ def _iter_callback(self, fpres): # print(f'il male รจ {fpres.constraint_values[0]} + {np.linalg.norm(fpres.defect_values[:, 0])}') if self.plot_iter: + from matplotlib import pyplot as plt if self.dax is None: _, (self.dax, self.hax) = plt.subplots(2) diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index 014d78a5..5e29bb71 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -1,10 +1,8 @@ import numpy as np -import matplotlib.pyplot as plt -from scipy.optimize import curve_fit -from scipy.interpolate import splprep +# from scipy.optimize import curve_fit +# from scipy.interpolate import splprep - -from numpy import linspace, sin, pi +# from numpy import linspace, sin, pi from scipy.interpolate import BPoly, CubicSpline class TrajectoryGenerator: @@ -137,6 +135,7 @@ def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, der return y_bpoly_second_derivative if __name__ == '__main__': + import matplotlib.pyplot as plt tg = TrajectoryGenerator() From 23a71da64b90e92783258ac0d01509def6773049 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Oct 2024 17:48:13 +0200 Subject: [PATCH 55/70] minor changes Signed-off-by: Andrea Patrizi --- horizon/problem.py | 19 +++++++------------ horizon/rhc/taskInterface.py | 6 ++---- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/horizon/problem.py b/horizon/problem.py index 566d4f48..2bb34db6 100644 --- a/horizon/problem.py +++ b/horizon/problem.py @@ -337,18 +337,13 @@ def setInitialStateSoft(self, x0_meas: np.ndarray, # set initial state with a "soft approach", which is useful when running a controller # in closed loop to avoid issues - - lower_bound_relaxed=np.minimum(x0_meas,x0_internal) - upper_bound_relaxed=np.maximum(x0_meas,x0_internal) - - delta=0.01 - # lower_bound_relaxed=x0_internal-delta - # upper_bound_relaxed=x0_internal+delta - # lower_bound_relaxed=x0_meas-delta - # upper_bound_relaxed=x0_meas+delta - - # relax state bound on first node to allow some mismatch - self.getState().setBounds(lb=lower_bound_relaxed, ub=upper_bound_relaxed, nodes=0) + x_tilde=(x0_internal+x0_meas)/2 + x_sigma=np.absolute((x0_internal-x0_meas)/2) + + # relax state bound on first node depending on the mismatch between the measured + # and internal MPC state. Works also if x0_meas already contains some data + # from MPC + self.getState().setBounds(lb=x_tilde-x_sigma, ub=x_tilde+x_sigma, nodes=0) def getInitialState(self) -> np.array: lb, ub = self.getState().getBounds(node=0) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index a9467312..b129c3f9 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -1,7 +1,5 @@ -import code from horizon.utils import kin_dyn, mat_storer, resampler_trajectory -from casadi_kin_dyn import pycasadi_kin_dyn from horizon.rhc.tasks.cartesianTask import CartesianTask from horizon.rhc.tasks.contactTask import ContactTask from horizon.rhc.tasks.interactionTask import InteractionTask, SurfaceContact, VertexContact @@ -96,13 +94,13 @@ def reset(self): # copies latest bootstrap into solution + self.solver_rti.reset() # resets solver internal state (useful in case of failure) + self.solution = copy.deepcopy(self.bootstrap_sol) # resets the controller with the latest solution self.load_initial_guess() - self.solver_rti.reset() # resets solver internal state (useful in case of failure) - def rti(self): if self._verbose: From 4970cfd4636ad84177a2a3e36ee310f1ecaf162f Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 23 Oct 2024 20:38:11 +0200 Subject: [PATCH 56/70] added vertex frames to attributes --- horizon/rhc/tasks/interactionTask.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/horizon/rhc/tasks/interactionTask.py b/horizon/rhc/tasks/interactionTask.py index 11595b53..208a25ba 100644 --- a/horizon/rhc/tasks/interactionTask.py +++ b/horizon/rhc/tasks/interactionTask.py @@ -181,6 +181,8 @@ def __init__(self, frame, vertex_frames, *args, **kwargs): # init base super().__init__(frame, *args, **kwargs) + self.vertex_frames=vertex_frames + # ask model to create vertex forces self.forces = self.model.setContactFrame(frame, 'vertex', From b64295081f1b7e1bc6363413716cbf3f2e56c361 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 28 Oct 2024 12:28:22 +0100 Subject: [PATCH 57/70] added reset of fp_res data --- horizon/cpp/src/ilqr.cpp | 6 ++++++ horizon/solvers/ilqr.py | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 1b24df6a..6d6f0c5d 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -637,6 +637,12 @@ const float IterativeLQR::getResidualNorm() const void IterativeLQR::reset() { _hxx_reg = _hxx_reg_base; + + _fp_res->cost_values.setZero(); + _fp_res->constraint_values.setZero(); + _fp_res->defect_norm.setZero(); + _fp_res->bound_violation.setZero(); + } bool IterativeLQR::solve(int max_iter) diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index b96cb138..d216894e 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -71,7 +71,8 @@ def __init__(self, # create ilqr solver self.ilqr = IterativeLQR(self.prb.getIntegrator(), self.N, self.opts) - + self.ilqr.reset() + # should we use GN approx for residuals? self.use_gn = self.opts.get('ilqr.enable_gn', False) From 17dab1a792c932eabfad05473df7e0ce4eaf7620 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 28 Oct 2024 14:57:17 +0100 Subject: [PATCH 58/70] fixed eigen method being called on a scalar --- horizon/cpp/src/ilqr.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index 6d6f0c5d..2a4db719 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -640,8 +640,8 @@ void IterativeLQR::reset() _fp_res->cost_values.setZero(); _fp_res->constraint_values.setZero(); - _fp_res->defect_norm.setZero(); - _fp_res->bound_violation.setZero(); + _fp_res->defect_norm=0; + _fp_res->bound_violation=0; } From 61b668564b5e1472ae7bb1403c3b73a827b2399e Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 30 Oct 2024 11:00:56 +0100 Subject: [PATCH 59/70] bug fix --- horizon/utils/trajectoryGenerator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index 5e29bb71..cae60806 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -74,10 +74,10 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): if derivatives is None: - derivatives = [None] * len(p_start) + derivatives = [None] * len(nodes) if second_der is None: - second_der = [None] * len(p_start) + second_der = [None] * len(nodes) cxi = [0, 0.5, 1] cyi = [p_start, p_goal + clearance, p_goal] @@ -105,10 +105,10 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): if derivatives is None: - derivatives = [None] * len(p_start) + derivatives = [None] * len(nodes) if second_der is None: - second_der = [None] * len(p_start) + second_der = [None] * len(nodes) cxi = [0, 0.5, 1] cyi = [p_start, p_goal + clearance, p_goal] From 6e66da137b728f9ba809937c6675321b96932c11 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Sun, 3 Nov 2024 20:50:27 +0100 Subject: [PATCH 60/70] added indeces for regularization task --- horizon/rhc/tasks/regularizationTask.py | 40 ++++++++++++++++++++----- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/horizon/rhc/tasks/regularizationTask.py b/horizon/rhc/tasks/regularizationTask.py index 0f3af113..826d473e 100644 --- a/horizon/rhc/tasks/regularizationTask.py +++ b/horizon/rhc/tasks/regularizationTask.py @@ -22,16 +22,42 @@ def __init__(self, opt_variable_names, *args, **kwargs): super().__init__(*args, **kwargs) self._createWeightParam() + self.opt_varariable_dim = [] + self.opt_variable_list = [] if not isinstance(opt_variable_names, list): - self.opt_variable_list = [opt_variable_names] + var=self.prb.getVariables(opt_variable_names) + if var is not None: + self.opt_variable_list.append(var) + self.opt_varariable_dim.append(var.getDim()) + else: + self.opt_varariable_dim.append(None) else: - self.opt_variable_list = [self.prb.getVariables(name) for name in opt_variable_names] - - # todo: what to do with this one? - self.opt_reference_list = [self.prb.createParameter(f'{name}_ref', self.prb.getVariables(name).getDim()) for name in opt_variable_names] - + for name in opt_variable_names: + var=self.prb.getVariables(name) + var_dim=None + if var is not None: + var_dim=var.getDim() + self.opt_variable_list.append(var) + self.opt_varariable_dim.append(var_dim) if None in self.opt_variable_list: raise ValueError(f'variable inserted is not in the problem.') + + if None in self.opt_variable_list: + raise ValueError(f'variable inserted is not in the problem.') + + var_dim_match=all(x == self.opt_varariable_dim[0] for x in self.opt_varariable_dim) + if not var_dim_match: + incorrect_dims=list(map(str, self.opt_varariable_dim)) + raise ValueError(f'Dimensions of variables do not match! -> {", ".join(incorrect_dims)}') + + self.indices = np.array(list(range(self.opt_varariable_dim[0]))).astype(int) if self.indices is None else np.array(self.indices).astype(int) + + indices_within_bounds=all(x Date: Tue, 12 Nov 2024 15:13:33 +0100 Subject: [PATCH 61/70] removed initial state setting in task interface --- horizon/rhc/taskInterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index b129c3f9..19524dd7 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -286,7 +286,7 @@ def load_initial_guess(self, from_dict=None): self.prb.getState().setInitialGuess(x_opt) self.prb.getInput().setInitialGuess(u_opt) - self.prb.setInitialState(x0=x_opt[:, 0]) + # self.prb.setInitialState(x0=x_opt[:, 0]) # def replay_trajectory(self, trajectory_markers=[], trajectory_markers_opts={}): From e88973a2ecbbef1906630fa541f20740df8bc086 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 18 Nov 2024 12:36:23 +0100 Subject: [PATCH 62/70] removing unil barrier if thresh is low "enough" --- horizon/rhc/tasks/interactionTask.py | 19 ++++++++++++------- horizon/utils/trajectoryGenerator.py | 27 +++++++++++++++------------ 2 files changed, 27 insertions(+), 19 deletions(-) diff --git a/horizon/rhc/tasks/interactionTask.py b/horizon/rhc/tasks/interactionTask.py index 208a25ba..4fc18a76 100644 --- a/horizon/rhc/tasks/interactionTask.py +++ b/horizon/rhc/tasks/interactionTask.py @@ -208,13 +208,17 @@ def __initialize(self): def make_fn_barrier(self): - fn_barrier_cost = [] - for f in self.forces: - fn_barrier_cost.append(barrier_fun(f[2] - self.fn_min)) - fn_barrier_cost = cs.vertcat(*fn_barrier_cost) - fn_barrier = self.prb.createResidual(f'{self.frame}_unil_barrier', 1e1 * fn_barrier_cost, self.all_nodes) - return fn_barrier + if not self.fn_min < -1e3: + fn_barrier_cost = [] + for f in self.forces: + fn_barrier_cost.append(barrier_fun(f[2] - self.fn_min)) + fn_barrier_cost = cs.vertcat(*fn_barrier_cost) + fn_barrier = self.prb.createResidual(f'{self.frame}_unil_barrier', 1e1 * fn_barrier_cost, self.all_nodes) + return fn_barrier + else: + return None + def make_friction_cone(self): fcost = [] for f in self.forces: @@ -251,7 +255,8 @@ def setContact(self, nodes, erasing=True): # start_time3 = time.time() # add normal force constraint - self.fn_barrier.setNodes(good_nodes, erasing=erasing) + if self.fn_barrier is not None: + self.fn_barrier.setNodes(good_nodes, erasing=erasing) # end_time3 = time.time() - start_time3 # start_time4 = time.time() diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index cae60806..5b5ed55a 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -141,15 +141,18 @@ def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, der n_samples = 100 - der= [None, 0, 0] - second_der=[None, 0, 0] - + der= [0, 0, 0] + second_der=[0, 0, 0] + + start=0.0 + end=0.0 + dh=0.1 # Original trajectory with first and second derivative constraints z_trj = tg.from_derivatives( n_samples, - -1, - -1, - 1, + start, + end, + dh, derivatives=der, second_der=second_der ) @@ -157,9 +160,9 @@ def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, der # First derivative of the trajectory with first and second derivative constraints z_trj_derivative = tg.derivative_of_trajectory( n_samples, - -1, - -1, - 1, + start, + end, + dh, derivatives=der, second_der=second_der ) @@ -167,9 +170,9 @@ def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, der # Second derivative of the trajectory with first and second derivative constraints z_trj_second_derivative = tg.second_derivative_of_trajectory( n_samples, - -1, - -1, - 1, + start, + end, + dh, derivatives=der, second_der=second_der ) From 5b736e9c6c6c06bdcfce40e8c826f249323ff5df Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 18 Nov 2024 18:51:49 +0100 Subject: [PATCH 63/70] added safety clip when evaluating efforts --- horizon/rhc/taskInterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 19524dd7..2afd1014 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -168,7 +168,7 @@ def eval_efforts_on_node(self, node_idx: int =0): self.solution['a'][:, node_idx], self.fmap_0) - return tau_i.toarray() + return np.clip(tau_i.toarray(), a_min=-300, a_max=300) def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): From 546508bc38668535329e31284026f60b9d8f6b8d Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 18 Nov 2024 19:15:54 +0100 Subject: [PATCH 64/70] properly hadnling nonfinite values when evaluating torques --- horizon/rhc/taskInterface.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 2afd1014..01ec63b1 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -167,8 +167,13 @@ def eval_efforts_on_node(self, node_idx: int =0): self.solution['v'][:, node_idx], self.solution['a'][:, node_idx], self.fmap_0) - - return np.clip(tau_i.toarray(), a_min=-300, a_max=300) + + tau_array=tau_i.toarray() + np.nan_to_num(tau_array, copy=False, nan=300.0, + posinf=300, neginf=-300) # handle not finite vals + np.clip(tau_array, out=tau_array, a_min=-300, a_max=300) # clip + + return tau_array def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): From bad10e37bf53567ced5a7266d8e00eb312fb288b Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 18 Nov 2024 19:24:07 +0100 Subject: [PATCH 65/70] Signed-off-by: Andrea Patrizi --- horizon/rhc/taskInterface.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 01ec63b1..2670ecac 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -169,11 +169,9 @@ def eval_efforts_on_node(self, node_idx: int =0): self.fmap_0) tau_array=tau_i.toarray() - np.nan_to_num(tau_array, copy=False, nan=300.0, + tau_array[:, :]=np.nan_to_num(tau_array, copy=True, nan=300.0, posinf=300, neginf=-300) # handle not finite vals - np.clip(tau_array, out=tau_array, a_min=-300, a_max=300) # clip - - return tau_array + return np.clip(tau_array, a_min=-300, a_max=300) def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): From 73e2738934358e113c79c3bc12885952f73c0e3d Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Wed, 27 Nov 2024 11:09:36 +0100 Subject: [PATCH 66/70] moved global np setting --- horizon/rhc/model_description.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/rhc/model_description.py b/horizon/rhc/model_description.py index 8b52b636..73a86c83 100644 --- a/horizon/rhc/model_description.py +++ b/horizon/rhc/model_description.py @@ -9,8 +9,6 @@ import urdf_parser_py.urdf as upp from collections import OrderedDict -np.set_printoptions(precision=3, suppress=True) - class FullModelInverseDynamics: def __init__(self, problem, kd, q_init, base_init=None, floating_base=True, fixed_joint_map=None, sys_order_degree=2, **kwargs): @@ -630,6 +628,8 @@ def getContacts(self): if __name__ == '__main__': + np.set_printoptions(precision=3, suppress=True) + import rospkg import casadi_kin_dyn.py3casadi_kin_dyn as casadi_kin_dyn From cd47a05ee5157a1bf60b286e38a4bbe9b6628d89 Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Mon, 16 Dec 2024 14:52:45 +0100 Subject: [PATCH 67/70] added third derivative specification --- horizon/utils/trajectoryGenerator.py | 236 +++++++++++++++++++++------ 1 file changed, 185 insertions(+), 51 deletions(-) diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index 5811b1e3..56719bdc 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -1,9 +1,6 @@ +from scipy.interpolate import BPoly, CubicSpline import numpy as np -# from scipy.optimize import curve_fit -# from scipy.interpolate import splprep -# from numpy import linspace, sin, pi -from scipy.interpolate import BPoly, CubicSpline class TrajectoryGenerator: @@ -17,22 +14,18 @@ def sin_trj(self, tau): return np.sin(tau * np.pi) def bezier_trj(self, tau): - P0 = 1 P1 = 3 P2 = 2 fun = (1 - tau) ** 2 * P0 + 2 * (1 - tau) * tau * P1 + tau * 2 * P2 - return fun + def compute_polynomial_trajectory(self, k_start, nodes, nodes_duration, p_start, p_goal, clearance, dim=None): if dim is None: dim = [0, 1, 2] - # todo check dimension of parameter before assigning it - traj_array = np.zeros(len(nodes)) - start = p_start[dim] goal = p_goal[dim] @@ -46,11 +39,13 @@ def compute_polynomial_trajectory(self, k_start, nodes, nodes_duration, p_start, return np.array(traj_array) - def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): - + def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): if derivatives is None: derivatives = [None] * nodes - + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes cxi = [0, 0.5, 1] @@ -62,12 +57,14 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, xcurve = np.linspace(0, 1, nodes) yder = [] - for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): constraints = [cyi[i]] if d1 is not None: constraints.append(d1) if d2 is not None: constraints.append(d2) + if d3 is not None: + constraints.append(d3) yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) @@ -75,14 +72,16 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, return y_bpoly - def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): + def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): if derivatives is None: derivatives = [None] * nodes - if second_der is None: second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes cxi = [0, 0.5, 1] + if p_start >= p_goal: cyi = [p_start, p_start + clearance, p_goal] else: @@ -91,12 +90,14 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative xcurve = np.linspace(0, 1, nodes) yder = [] - for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): constraints = [cyi[i]] if d1 is not None: constraints.append(d1) if d2 is not None: constraints.append(d2) + if d3 is not None: + constraints.append(d3) yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) @@ -107,7 +108,116 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative y_bpoly_derivative = bpoly_derivative(xcurve) return y_bpoly_derivative - + + def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): + """ + Compute the second derivative of the trajectory. + + Parameters: + - nodes: int, number of points in the trajectory + - p_start: float, start position + - p_goal: float, goal position + - clearance: float, distance above or below the straight line + - derivatives: list of first derivative constraints or `None` + - second_der: list of second derivative constraints or `None` + - third_der: list of third derivative constraints or `None` + + Returns: + - y_bpoly_second_derivative: np.ndarray, second derivative values + """ + if derivatives is None: + derivatives = [None] * nodes + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes + + cxi = [0, 0.5, 1] + + # Adjust clearance for the middle point + if p_start >= p_goal: + cyi = [p_start, p_start + clearance, p_goal] + else: + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + if d3 is not None: + constraints.append(d3) + yder.append(constraints) + + # Create the BPoly object from derivatives + bpoly = BPoly.from_derivatives(cxi, yder) + # Compute the second derivative of the BPoly object + bpoly_second_derivative = bpoly.derivative().derivative() + + # Evaluate the second derivative at the given points + y_bpoly_second_derivative = bpoly_second_derivative(xcurve) + + return y_bpoly_second_derivative + + def third_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): + """ + Compute the third derivative of the trajectory. + + Parameters: + - nodes: int, number of points in the trajectory + - p_start: float, start position + - p_goal: float, goal position + - clearance: float, distance above or below the straight line + - derivatives: list of first derivative constraints or `None` + - second_der: list of second derivative constraints or `None` + - third_der: list of third derivative constraints or `None` + + Returns: + - y_bpoly_third_derivative: np.ndarray, third derivative values + """ + if derivatives is None: + derivatives = [None] * nodes + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes + + cxi = [0, 0.5, 1] + + # Adjust clearance for the middle point + if p_start >= p_goal: + cyi = [p_start, p_start + clearance, p_goal] + else: + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + if d3 is not None: + constraints.append(d3) + yder.append(constraints) + + # Create the BPoly object from derivatives + bpoly = BPoly.from_derivatives(cxi, yder) + # Compute the third derivative of the BPoly object + bpoly_third_derivative = bpoly.derivative().derivative().derivative() + + # Evaluate the third derivative at the given points + y_bpoly_third_derivative = bpoly_third_derivative(xcurve) + + return y_bpoly_third_derivative + + if __name__ == '__main__': import matplotlib.pyplot as plt @@ -115,45 +225,61 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative n_samples = 100 - der = [None, 0, 0] - second_der = [None, 0, 0] + # Specify derivatives for testing + der = [None, 0, 0] # First derivatives at key points + second_der = [None, None, 0] # Second derivatives at key points + third_der = [None, None, 0] # Third derivatives at key points - # Original trajectory with first and second derivative constraints + # Compute the trajectory z_trj = tg.from_derivatives( n_samples, - -1, - -1, - 1, + 0, + 0, + 0.12, derivatives=der, - second_der=second_der + second_der=second_der, + third_der=third_der ) - # First derivative of the trajectory with first and second derivative constraints + # First derivative of the trajectory z_trj_derivative = tg.derivative_of_trajectory( n_samples, - -1, - -1, - 1, + 0, + 0, + 0.12, derivatives=der, - second_der=second_der + second_der=second_der, + third_der=third_der ) - # # Second derivative of the trajectory with first and second derivative constraints - # z_trj_second_derivative = tg.second_derivative_of_trajectory( - # n_samples, - # -1, - # -1, - # 1, - # derivatives=der, - # second_der=second_der - # ) + # Compute the second derivative of the trajectory + z_trj_second_derivative = tg.second_derivative_of_trajectory( + n_samples, + 0, + 0, + 0.12, + derivatives=der, + second_der=second_der, + third_der=third_der + ) + + # Compute the third derivative of the trajectory + z_trj_third_derivative = tg.third_derivative_of_trajectory( + n_samples, + 0, + 0, + 0.12, + derivatives=der, + second_der=second_der, + third_der=third_der + ) axis = np.linspace(0, 1, num=z_trj.shape[0]) - # Plotting the trajectory, its first derivative, and its second derivative - plt.figure(figsize=(18, 6)) + # Plot the trajectory, its first, second, and third derivatives + plt.figure(figsize=(18, 8)) - plt.subplot(1, 3, 1) + plt.subplot(2, 2, 1) plt.plot(axis, z_trj, label="Trajectory") plt.title("Trajectory") plt.xlabel("Time (normalized)") @@ -161,21 +287,29 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative plt.grid() plt.legend() - plt.subplot(1, 3, 2) - plt.plot(axis, z_trj_derivative, label="Trajectory Derivative", linestyle="--") - plt.title("Derivative of Trajectory") + plt.subplot(2, 2, 2) + plt.plot(axis, z_trj_derivative, label="First Derivative", linestyle="--") + plt.title("First Derivative of Trajectory") plt.xlabel("Time (normalized)") plt.ylabel("Velocity") plt.grid() plt.legend() - # plt.subplot(1, 3, 3) - # plt.plot(axis, z_trj_second_derivative, label="Trajectory Second Derivative", linestyle="-.") - # plt.title("Second Derivative of Trajectory") - # plt.xlabel("Time (normalized)") - # plt.ylabel("Acceleration") - # plt.grid() - # plt.legend() + plt.subplot(2, 2, 3) + plt.plot(axis, z_trj_second_derivative, label="Second Derivative", linestyle="-.") + plt.title("Second Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Acceleration") + plt.grid() + plt.legend() + + plt.subplot(2, 2, 4) + plt.plot(axis, z_trj_third_derivative, label="Third Derivative", linestyle=":") + plt.title("Third Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Jerk") + plt.grid() + plt.legend() plt.tight_layout() plt.show() From 8d207e286ef938b61d3e8050410e3ed4c6227b5a Mon Sep 17 00:00:00 2001 From: andrea patrizi Date: Thu, 26 Dec 2024 22:08:43 +0100 Subject: [PATCH 68/70] added back -march native flag --- horizon/cpp/src/codegen_function.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 3ba61382..74e569fd 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -145,9 +145,9 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string std::cout << "not found: compiling " << fname << "... \n"; } - // int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str()); + int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str()); // removed -march=native to allow (maybe) more cross compatibility - int ret = system(("clang -fPIC -shared -O3 " + fname + ".c -o " + fname + ".so").c_str()); + // int ret = system(("clang -fPIC -shared -O3 " + fname + ".c -o " + fname + ".so").c_str()); if(ret != 0) { From c33029a1186a5162a3281fb831a4dd767a1cc813 Mon Sep 17 00:00:00 2001 From: AndrePatri Date: Mon, 24 Feb 2025 13:35:55 +0100 Subject: [PATCH 69/70] fixed wrong license file --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index ecdf79bc..5c564fa6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -8,4 +8,4 @@ version = "0.4.5" description = "" authors = [{name = "Francesco Ruscelli", email = "francesco.ruscelli@iit.it"}] readme = "README.md" -license = {file = "LICENSE.md"} \ No newline at end of file +license = {file = "LICENSE.txt"} \ No newline at end of file From ea75c81b1ce600eca7d9a065cb6142c5d22f2b6f Mon Sep 17 00:00:00 2001 From: Andrea Patrizi Date: Thu, 3 Apr 2025 15:39:43 +0100 Subject: [PATCH 70/70] raised minimum cmake version to avoid compilation error (cmake 3.5 not supported anymore) --- horizon/cpp/CMakeLists.txt | 3 +-- horizon/cpp/tests/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index 588dc2b6..088cf03d 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/horizon/cpp/CMakeLists.txt @@ -1,6 +1,5 @@ project(horizon) -cmake_minimum_required(VERSION 3.0) - +cmake_minimum_required(VERSION 3.10) if(DEFINED ENV{CONDA_PREFIX}) set(CMAKE_INSTALL_PREFIX $ENV{CONDA_PREFIX}/ CACHE PATH "bindings install prefix" FORCE) diff --git a/horizon/cpp/tests/CMakeLists.txt b/horizon/cpp/tests/CMakeLists.txt index b4ba9b30..c5cd28e2 100644 --- a/horizon/cpp/tests/CMakeLists.txt +++ b/horizon/cpp/tests/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.11) +cmake_minimum_required(VERSION 3.10) include(ExternalProject) set(PROJECTNAME tests)