From e59729132845d08bcc2b137b7abc68aedc4fb40c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 23 Jan 2025 12:22:20 -0500 Subject: [PATCH 01/31] feat: alternative partition recommendation --- .../models/biorbd/holonomic_biorbd_model.py | 89 ++++++++++++++++++- 1 file changed, 87 insertions(+), 2 deletions(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 9a24425ed..03733eeaa 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -1,10 +1,12 @@ from typing import Callable import biorbd_casadi as biorbd +import numpy as np +import scipy.linalg as la from biorbd_casadi import ( GeneralizedCoordinates, ) -from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv +from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv, nlpsol from .biorbd_model import BiorbdModel from ..holonomic_constraints import HolonomicConstraintsList @@ -111,11 +113,94 @@ def check_dependant_jacobian(self): partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] shape = partitioned_constraints_jacobian_v.shape if shape[0] != shape[1]: + output = self.partition_coordinates() raise ValueError( f"The shape of the dependent joint Jacobian should be square. Got: {shape}." - f"Please consider checking the dimension of the holonomic constraints Jacobian." + f"Please consider checking the dimension of the holonomic constraints Jacobian.\n" + f"Here is a recommended partitioning: " + f" - independent_joint_index: {output[1]}," + f" - dependent_joint_index: {output[0]}." ) + def partition_coordinates(self): + q = MX.sym("q", self.nb_q, 1) + s = nlpsol("sol", "ipopt", {"x": q, "g": self.holonomic_constraints(q)}) + q_star = np.array( + s( + x0=np.zeros(self.nb_q), + lbg=np.zeros(self.nb_holonomic_constraints), + ubg=np.zeros(self.nb_holonomic_constraints), + )["x"] + )[:, 0] + return self.jacobian_coordinate_partitioning(self.holonomic_constraints_jacobian(q_star).toarray()) + + @staticmethod + def jacobian_coordinate_partitioning(J, tol=None): + """ + Determine a coordinate partitioning q = {q_u, q_v} from a Jacobian J(q) of size (m x n), + where m is the number of constraints and + n is the total number of generalized coordinates. + + We want to find an invertible submatrix J_v of size (m x m) by reordering + the columns of J according to the largest pivots. Those columns in J_v + correspond to the 'dependent' coordinates q_v, while the remaining columns + correspond to the 'independent' coordinates q_u. + + Parameters + ---------- + J : array_like, shape (m, n) + The constraint Jacobian evaluated at the current configuration q. + tol : float, optional + Tolerance for rank detection. If None, a default based on the machine + precision and the size of J is used. + + Returns + ------- + qv_indices : ndarray of shape (r,) + The indices of the columns in J chosen as dependent coordinates. + Typically, we expect r = m if J has full row rank (i.e. no redundant constraints). + qu_indices : ndarray of shape (n - r,) + The indices of the columns chosen as independent coordinates. + rankJ : int + The detected rank of J. If rankJ < m, it means some constraints are redundant. + + Notes + ----- + - If rankJ < m, then there are redundant or degenerate constraints in J. + The 'extra' constraints can be ignored in subsequent computations. + - If rankJ = m, then J has full row rank and the submatrix J_v is invertible. + - After obtaining qv_indices and qu_indices, one typically reorders q + so that q = [q_u, q_v], and likewise reorders the columns of J so that + J = [J_u, J_v]. + """ + + # J is (m, n): number of constraints = m, number of coords = n. + J = np.asarray(J, dtype=float) + m, n = J.shape + + # Perform a pivoted QR factorization: J = Q * R[:, pivot] + # pivot is a permutation of [0, 1, ..., n-1], + # reordering the columns from "largest pivot" to "smallest pivot" in R. + Q, R, pivot = la.qr(J, pivoting=True) + + # If no tolerance is specified, pick a default related to the matrix norms and eps + if tol is None: + # A common heuristic: tol ~ max(m, n) * machine_eps * largest_abs_entry_in_R + # The largest absolute entry is often approximated by abs(R[0, 0]) if the matrix + # is well-ordered by pivot. However, you can also do np.linalg.norm(R, ord=np.inf). + tol = max(m, n) * np.finfo(J.dtype).eps * abs(R[0, 0]) + + # Rank detection from the diagonal of R + diagR = np.abs(np.diag(R)) + rankJ = np.sum(diagR > tol) + + # The 'best' columns (by largest pivots) are pivot[:rankJ]. + # If J is full row rank and not degenerate, we expect rankJ == m. + qv_indices = pivot[:rankJ] # Dependent variables + qu_indices = pivot[rankJ:] # Independent variables + + return qv_indices, qu_indices, rankJ + @property def nb_independent_joints(self): return len(self._independent_joint_index) From 9b8fef04e97a5c85b37291c010ce8de964b954ad Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 24 Jan 2025 16:59:39 -0500 Subject: [PATCH 02/31] refactor: making partionned_forward_dynamics more flexible for collocations --- .../models/biorbd/holonomic_biorbd_model.py | 70 +++++++++++++++---- 1 file changed, 56 insertions(+), 14 deletions(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 03733eeaa..d08cb210e 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -342,49 +342,91 @@ def partitioned_forward_dynamics(self) -> Function: ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ + q = self.compute_q()(self.q_u, self.q_v_init) + qddot_u = self.partitioned_forward_dynamics_full()(q, self.qdot_u, self.tau) + + casadi_fun = Function( + "partitioned_forward_dynamics", + [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [qddot_u], + ["q_u", "qdot_u", "q_v_init", "tau"], + ["qddot_u"], + ) + return casadi_fun + + def partitioned_forward_dynamics_with_qv(self) -> Function: + """ + Sources + ------- + Docquier, N., Poncelet, A., and Fisette, P.: + ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, + https://doi.org/10.5194/ms-4-199-2013, 2013. + """ + q = self.state_from_partition(self.q_u, self.q_v) + qddot_u = self.partitioned_forward_dynamics_full()(q, self.qdot_u, self.tau) + + casadi_fun = Function( + "partitioned_forward_dynamics", + [self.q_u, self.q_v, self.qdot_u, self.tau], + [qddot_u], + ["q_u", "q_v", "qdot_u", "tau"], + ["qddot_u"], + ) + + return casadi_fun + + def partitioned_forward_dynamics_full(self) -> Function: + """ + Sources + ------- + Docquier, N., Poncelet, A., and Fisette, P.: + ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, + https://doi.org/10.5194/ms-4-199-2013, 2013. + """ # compute q and qdot - q = self.compute_q()(self.q_u, self.q_v_init) + q = self.q qdot = self.compute_qdot()(q, self.qdot_u) + tau = self.tau partitioned_mass_matrix = self.partitioned_mass_matrix(q) m_uu = partitioned_mass_matrix[: self.nb_independent_joints, : self.nb_independent_joints] - m_uv = partitioned_mass_matrix[: self.nb_independent_joints, self.nb_independent_joints :] - m_vu = partitioned_mass_matrix[self.nb_independent_joints :, : self.nb_independent_joints] - m_vv = partitioned_mass_matrix[self.nb_independent_joints :, self.nb_independent_joints :] + m_uv = partitioned_mass_matrix[: self.nb_independent_joints, self.nb_independent_joints:] + m_vu = partitioned_mass_matrix[self.nb_independent_joints:, : self.nb_independent_joints] + m_vv = partitioned_mass_matrix[self.nb_independent_joints:, self.nb_independent_joints:] coupling_matrix_vu = self.coupling_matrix(q) modified_mass_matrix = ( - m_uu - + m_uv @ coupling_matrix_vu - + coupling_matrix_vu.T @ m_vu - + coupling_matrix_vu.T @ m_vv @ coupling_matrix_vu + m_uu + + m_uv @ coupling_matrix_vu + + coupling_matrix_vu.T @ m_vu + + coupling_matrix_vu.T @ m_vv @ coupling_matrix_vu ) second_term = m_uv + coupling_matrix_vu.T @ m_vv # compute the non-linear effect non_linear_effect = self.partitioned_non_linear_effect(q, qdot) non_linear_effect_u = non_linear_effect[: self.nb_independent_joints] - non_linear_effect_v = non_linear_effect[self.nb_independent_joints :] + non_linear_effect_v = non_linear_effect[self.nb_independent_joints:] modified_non_linear_effect = non_linear_effect_u + coupling_matrix_vu.T @ non_linear_effect_v # compute the tau - partitioned_tau = self.partitioned_tau(self.tau) + partitioned_tau = self.partitioned_tau(tau) tau_u = partitioned_tau[: self.nb_independent_joints] - tau_v = partitioned_tau[self.nb_independent_joints :] + tau_v = partitioned_tau[self.nb_independent_joints:] modified_generalized_forces = tau_u + coupling_matrix_vu.T @ tau_v qddot_u = inv(modified_mass_matrix) @ ( - modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect + modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect ) casadi_fun = Function( "partitioned_forward_dynamics", - [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [self.q, self.qdot_u, self.tau], [qddot_u], - ["q_u", "qdot_u", "q_v_init", "tau"], + ["q", "qdot_u", "tau"], ["qddot_u"], ) From 2187a56cad17ba6c3f8ae750efee08cbf8af90bd Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 25 Jan 2025 19:27:47 -0500 Subject: [PATCH 03/31] lint: holonomic model --- .../models/biorbd/holonomic_biorbd_model.py | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index d08cb210e..f5eb806da 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -391,35 +391,35 @@ def partitioned_forward_dynamics_full(self) -> Function: partitioned_mass_matrix = self.partitioned_mass_matrix(q) m_uu = partitioned_mass_matrix[: self.nb_independent_joints, : self.nb_independent_joints] - m_uv = partitioned_mass_matrix[: self.nb_independent_joints, self.nb_independent_joints:] - m_vu = partitioned_mass_matrix[self.nb_independent_joints:, : self.nb_independent_joints] - m_vv = partitioned_mass_matrix[self.nb_independent_joints:, self.nb_independent_joints:] + m_uv = partitioned_mass_matrix[: self.nb_independent_joints, self.nb_independent_joints :] + m_vu = partitioned_mass_matrix[self.nb_independent_joints :, : self.nb_independent_joints] + m_vv = partitioned_mass_matrix[self.nb_independent_joints :, self.nb_independent_joints :] coupling_matrix_vu = self.coupling_matrix(q) modified_mass_matrix = ( - m_uu - + m_uv @ coupling_matrix_vu - + coupling_matrix_vu.T @ m_vu - + coupling_matrix_vu.T @ m_vv @ coupling_matrix_vu + m_uu + + m_uv @ coupling_matrix_vu + + coupling_matrix_vu.T @ m_vu + + coupling_matrix_vu.T @ m_vv @ coupling_matrix_vu ) second_term = m_uv + coupling_matrix_vu.T @ m_vv # compute the non-linear effect non_linear_effect = self.partitioned_non_linear_effect(q, qdot) non_linear_effect_u = non_linear_effect[: self.nb_independent_joints] - non_linear_effect_v = non_linear_effect[self.nb_independent_joints:] + non_linear_effect_v = non_linear_effect[self.nb_independent_joints :] modified_non_linear_effect = non_linear_effect_u + coupling_matrix_vu.T @ non_linear_effect_v # compute the tau partitioned_tau = self.partitioned_tau(tau) tau_u = partitioned_tau[: self.nb_independent_joints] - tau_v = partitioned_tau[self.nb_independent_joints:] + tau_v = partitioned_tau[self.nb_independent_joints :] modified_generalized_forces = tau_u + coupling_matrix_vu.T @ tau_v qddot_u = inv(modified_mass_matrix) @ ( - modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect + modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect ) casadi_fun = Function( From 4da2bbaf2b12cf513cbb5b49bfc1b124443d2f12 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 25 Jan 2025 19:28:28 -0500 Subject: [PATCH 04/31] feat: making sure algebraic states are states. Rolling. --- bioptim/dynamics/configure_new_variable.py | 8 +++++--- bioptim/dynamics/integrator.py | 14 +++++++++++--- bioptim/dynamics/ode_solver.py | 9 ++++++++- bioptim/interfaces/interface_utils.py | 4 ++++ 4 files changed, 28 insertions(+), 7 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index 4a7368f57..ece7f2a2d 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -443,7 +443,7 @@ def _declare_cx_and_plot(self): for node_index in range( self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1 ): - n_cx = 2 + n_cx = self.nlp.ode_solver.n_required_cx + 2 cx_scaled = ( self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[node_index][self.name].original_cx if self.copy_algebraic_states @@ -463,14 +463,16 @@ def _declare_cx_and_plot(self): node_index, ) if not self.skip_plot: - all_variables_in_one_subplot = True if self.name in ["m", "cov", "k"] else False + all_variables_in_one_subplot = ( + True if self.name in ["m", "cov", "k"] else False + ) # To Eve: This should not be there. self.nlp.plot[f"{self.name}_algebraic"] = CustomPlot( lambda t0, phases_dt, node_idx, x, u, p, a, d: ( a[self.nlp.algebraic_states.key_index(self.name), :] if a.any() else np.ndarray((cx[0][0].shape[0], 1)) * np.nan ), - plot_type=PlotType.STEP, + plot_type=PlotType.INTEGRATED, axes_idx=self.axes_idx, legend=self.legend, combine_to=self.combine_name, diff --git a/bioptim/dynamics/integrator.py b/bioptim/dynamics/integrator.py index 4def33e58..43cbb205e 100644 --- a/bioptim/dynamics/integrator.py +++ b/bioptim/dynamics/integrator.py @@ -87,7 +87,7 @@ def __init__(self, ode: dict, ode_opt: dict): self._x_sym_modified, self.u_sym, self.param_sym, - self.a_sym, + self._a_sym_modified, self.numerical_timeseries_sym, ], self.dxdt( @@ -130,6 +130,10 @@ def _time_xall_from_dt_func(self) -> Function: def _x_sym_modified(self): return self.x_sym + @property + def _a_sym_modified(self): + return self.a_sym + @property def _input_names(self): return ["t_span", "x0", "u", "p", "a", "d"] @@ -585,6 +589,10 @@ def _initialize(self, ode: dict, ode_opt: dict): def _x_sym_modified(self): return horzcat(*self.x_sym) if self.duplicate_starting_point else horzcat(*self.x_sym[1:]) + @property + def _a_sym_modified(self): + return horzcat(*self.a_sym) if self.duplicate_starting_point else horzcat(*self.a_sym[1:]) + @property def _output_names(self): return ["xf", "xall", "defects"] @@ -664,7 +672,7 @@ def dxdt( states[j + 1], self.get_u(controls, self._integration_time[j]), params, - algebraic_states, + algebraic_states[j], numerical_timeseries, )[:, self.ode_idx] defects.append(xp_j - f_j * self.h) @@ -676,7 +684,7 @@ def dxdt( states[j + 1], self.get_u(controls, self._integration_time[j]), params, - algebraic_states, + algebraic_states[j], numerical_timeseries, xp_j / self.h, ) diff --git a/bioptim/dynamics/ode_solver.py b/bioptim/dynamics/ode_solver.py index d0901f102..0eb509ae4 100644 --- a/bioptim/dynamics/ode_solver.py +++ b/bioptim/dynamics/ode_solver.py @@ -491,8 +491,15 @@ def x_ode(self, nlp): def p_ode(self, nlp): return nlp.controls.scaled.cx_start + # def a_ode(self, nlp): + # return nlp.algebraic_states.scaled.cx_start + def a_ode(self, nlp): - return nlp.algebraic_states.scaled.cx_start + out = [nlp.algebraic_states.scaled.cx_start] + if not self.duplicate_starting_point: + out += [nlp.algebraic_states.scaled.cx_start] + out += nlp.algebraic_states.scaled.cx_intermediates_list + return out def d_ode(self, nlp): return nlp.numerical_timeseries.cx_start diff --git a/bioptim/interfaces/interface_utils.py b/bioptim/interfaces/interface_utils.py index 83962aa9f..221aa7c15 100644 --- a/bioptim/interfaces/interface_utils.py +++ b/bioptim/interfaces/interface_utils.py @@ -332,6 +332,10 @@ def generic_get_all_penalties(interface, nlp: NonLinearProgram, penalties, scale tp[: u_tp.shape[0], :] = u_tp u_tp = tp u = horzcat(u, u_tp) + if idx != 0 and a_tp.shape[0] != a.shape[0]: + tp = ocp.cx.nan(a.shape[0], 1) + tp[: a_tp.shape[0], :] = a_tp + a_tp = tp a = horzcat(a, a_tp) d = horzcat(d, d_tp) if d is not None else d_tp weight = np.concatenate((weight, [[float(weight_tp)]]), axis=1) From 3fbf174140ff38dd1f56e133a3e1385748e68eb7 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 27 Jan 2025 05:39:27 -0500 Subject: [PATCH 05/31] fix test: restoring tf dependant dynamics tests --- tests/shard6/test_dt_dependent.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/tests/shard6/test_dt_dependent.py b/tests/shard6/test_dt_dependent.py index a9b635b4c..d3b49081e 100644 --- a/tests/shard6/test_dt_dependent.py +++ b/tests/shard6/test_dt_dependent.py @@ -7,10 +7,11 @@ """ import os -import pytest -from casadi import MX, SX, vertcat, sin, Function, DM + import numpy as np import numpy.testing as npt +import pytest +from casadi import MX, SX, vertcat, sin, Function, DM from bioptim import ( BiorbdModel, @@ -53,12 +54,10 @@ def dynamics( q = DynamicsFunctions.get(nlp.states["q"], states) qdot = DynamicsFunctions.get(nlp.states["qdot"], states) - tau = DynamicsFunctions.get(nlp.controls["tau"], controls) * ( - sin(nlp.tf_mx - time) * time.ones(nlp.model.nb_tau) * 10 - ) + tau = DynamicsFunctions.get(nlp.controls["tau"], controls) * (sin(nlp.tf - time) * time.ones(nlp.model.nb_tau) * 10) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(q, qdot, tau) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], parameters) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) @@ -216,8 +215,8 @@ def test_dt_dependent_problem(minimize_time, use_sx): tau_dyn = controls_sym * (sin(tf_sym - time_sym) * MX.ones(ocp.nlp[0].model.nb_tau) * 10) out_dyn = vertcat( states_sym[ocp.nlp[0].model.nb_q :], - ocp.nlp[0].model.forward_dynamics( - states_sym[: ocp.nlp[0].model.nb_q], states_sym[ocp.nlp[0].model.nb_q :], tau_dyn + ocp.nlp[0].model.forward_dynamics()( + states_sym[: ocp.nlp[0].model.nb_q], states_sym[ocp.nlp[0].model.nb_q :], tau_dyn, [], [] ), ) From cc502133ae58be5ab6217176584eb8f173a4071c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 27 Jan 2025 06:00:56 -0500 Subject: [PATCH 06/31] docs : stochastic --- bioptim/models/protocols/stochastic_biomodel.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/bioptim/models/protocols/stochastic_biomodel.py b/bioptim/models/protocols/stochastic_biomodel.py index 82bebb4c3..fc3599281 100644 --- a/bioptim/models/protocols/stochastic_biomodel.py +++ b/bioptim/models/protocols/stochastic_biomodel.py @@ -38,6 +38,10 @@ def sensory_reference(self, time, states, controls, parameters, algebraic_states def reshape_to_matrix(var, shape): """ Restore the matrix form of the variables + + See Also + -------- + reshape_to_vector """ if var.shape[0] != shape[0] * shape[1]: @@ -83,6 +87,10 @@ def reshape_to_cholesky_matrix(var, shape): def reshape_to_vector(matrix): """ Restore the vector form of the matrix + + See Also + -------- + reshape_to_matrix """ shape_0, shape_1 = matrix.shape[0], matrix.shape[1] if isinstance(matrix, np.ndarray): From 2bdf6deb29b0ac48c1b7e561f0c530bbe84b4cd4 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 30 Jan 2025 07:06:27 -0500 Subject: [PATCH 07/31] refactor: splitting ode_solver script I had enough to scroll down to the bottom of the script. --- bioptim/__init__.py | 4 +- bioptim/dynamics/configure_problem.py | 2 +- bioptim/dynamics/ode_solver_base.py | 270 ++++++++++++++ .../{ode_solver.py => ode_solvers.py} | 331 +----------------- bioptim/dynamics/rk_base.py | 67 ++++ bioptim/gui/plot.py | 2 +- bioptim/gui/serializable_class.py | 4 +- bioptim/limits/penalty_controller.py | 2 +- bioptim/optimization/non_linear_program.py | 2 +- .../optimization/optimal_control_program.py | 2 +- bioptim/optimization/solution/solution.py | 2 +- .../stochastic_optimal_control_program.py | 2 +- 12 files changed, 351 insertions(+), 339 deletions(-) create mode 100644 bioptim/dynamics/ode_solver_base.py rename bioptim/dynamics/{ode_solver.py => ode_solvers.py} (53%) create mode 100644 bioptim/dynamics/rk_base.py diff --git a/bioptim/__init__.py b/bioptim/__init__.py index 24da06bec..4f9ce4136 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -178,8 +178,8 @@ from .dynamics.fatigue.michaud_fatigue import MichaudFatigue, MichaudTauFatigue from .dynamics.fatigue.xia_fatigue import XiaFatigue, XiaTauFatigue, XiaFatigueStabilized from .dynamics.fatigue.xia_fatigue import XiaFatigue, XiaTauFatigue, XiaFatigueStabilized -from .dynamics.ode_solver import OdeSolver, OdeSolverBase -from .dynamics.ode_solver import OdeSolver, OdeSolverBase +from .dynamics.ode_solvers import OdeSolver, OdeSolverBase +from .dynamics.ode_solvers import OdeSolver, OdeSolverBase from .gui.online_callback_server import PlottingServer from .gui.online_callback_server import PlottingServer from .gui.plot import CustomPlot diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 10231287e..5949423c4 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -6,7 +6,7 @@ from .configure_new_variable import NewVariableConfiguration from .dynamics_functions import DynamicsFunctions from .fatigue.fatigue_dynamics import FatigueList -from .ode_solver import OdeSolver +from .ode_solvers import OdeSolver from ..gui.plot import CustomPlot from ..limits.constraints import ImplicitConstraintFcn from ..misc.enums import ( diff --git a/bioptim/dynamics/ode_solver_base.py b/bioptim/dynamics/ode_solver_base.py new file mode 100644 index 000000000..d783a4f06 --- /dev/null +++ b/bioptim/dynamics/ode_solver_base.py @@ -0,0 +1,270 @@ +from typing import Callable + +from casadi import MX, SX, integrator as casadi_integrator, horzcat, Function, vertcat + +from . import integrator +from ..misc.enums import ControlType, DefectType, PhaseDynamics + + +class OdeSolverBase: + """ + The base class for the ODE solvers + + Methods + ------- + integrator(self, ocp, nlp, node_index) -> list + The interface of the OdeSolver to the corresponding integrator + prepare_dynamic_integrator(ocp, nlp) + Properly set the integration in an nlp + """ + + def __init__(self, duplicate_starting_point: bool = False): + """ + Parameters + ---------- + duplicate_starting_point: bool + If the starting point should be duplicated in the integrator's casadi function, mostly used in Stochastic OCPs + """ + + self.duplicate_starting_point = duplicate_starting_point + + @property + def integrator(self): + """ + The corresponding integrator class + + Returns + ------- + The integrator class + """ + raise RuntimeError("This method should be implemented in the child class") + + @property + def is_direct_collocation(self) -> bool: + """ + indicating if the ode solver is direct collocation method + + Returns + ------- + True if the ode solver is direct collocation method + """ + raise RuntimeError("This method should be implemented in the child class") + + @property + def is_direct_shooting(self) -> bool: + """ + indicating if the ode solver is direct shooting method + + Returns + ------- + True if the ode solver is direct shooting method + """ + raise RuntimeError("This method should be implemented in the child class") + + @property + def n_required_cx(self) -> int: + """ + The required number of column required in the casadi CX matrix for the state variables + + Returns + ------- + The number of required casadi functions + """ + raise RuntimeError("This method should be implemented in the child class") + + @property + def defects_type(self) -> DefectType: + """ + The type of defect + + Returns + ------- + The type of defect + """ + raise RuntimeError("This method should be implemented in the child class") + + def t_ode(self, nlp) -> list: + """ + The time span of the integration + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The time span of the integration + """ + return vertcat(nlp.time_cx, nlp.dt) + + def x_ode(self, nlp) -> MX: + """ + The symbolic state variables + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The symbolic state variables + """ + raise RuntimeError("This method should be implemented in the child class") + + def p_ode(self, nlp) -> MX: + """ + The symbolic controls. The nomenclature is p_ode (instead of the intuitive u_ode) to be consistent with + the scipy integrator + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The symbolic controls + """ + raise RuntimeError("This method should be implemented in the child class") + + def a_ode(self, nlp) -> MX: + """ + The symbolic numerical timeseries + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The symbolic algebraic variables + """ + raise RuntimeError("This method should be implemented in the child class") + + def param_ode(self, nlp) -> MX: + """ + The symbolic parameters + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The symbolic parameters + """ + return nlp.parameters.scaled.cx_start + + def d_ode(self, nlp) -> MX: + """ + The symbolic algebraic states variables + + Parameters + ---------- + nlp + The NonLinearProgram handler + + Returns + ------- + The symbolic numerical timeseries + """ + raise RuntimeError("This method should be implemented in the child class") + + def initialize_integrator( + self, ocp, nlp, dynamics_index: int, node_index: int, is_extra_dynamics: bool = False, **extra_opt + ) -> Callable: + """ + Initialize the integrator + + Parameters + ---------- + ocp + The Optimal control program handler + nlp + The NonLinearProgram handler + node_index + The index of the node currently initialized + dynamics_index + The current extra dynamics to resolve (that can be referred to nlp.extra_dynamics_func[index]) + is_extra_dynamics + If the dynamics is an extra dynamics + extra_opt + Any extra options to pass to the integrator + + Returns + ------- + The initialized integrator function + """ + + if dynamics_index > 0 and not is_extra_dynamics: + raise RuntimeError("dynamics_index should be 0 if is_extra_dynamics is False") + + nlp.states.node_index = node_index + nlp.states_dot.node_index = node_index + nlp.controls.node_index = node_index + nlp.algebraic_states.node_index = node_index + dynamics_func = nlp.dynamics_func if not is_extra_dynamics else nlp.extra_dynamics_func[dynamics_index] + ode_opt = { + "model": nlp.model, + "cx": nlp.cx, + "control_type": nlp.control_type, + "defects_type": self.defects_type, + "ode_index": node_index if dynamics_func.size2_out("xdot") > 1 else 0, + "duplicate_starting_point": self.duplicate_starting_point, + **extra_opt, + } + + ode = { + "t": self.t_ode(nlp), + "x": self.x_ode(nlp), + "u": self.p_ode(nlp), + "a": self.a_ode(nlp), + "d": self.d_ode(nlp), + "param": self.param_ode(nlp), + "ode": dynamics_func, + "implicit_ode": nlp.implicit_dynamics_func, + } + + return nlp.ode_solver.integrator(ode, ode_opt) + + def prepare_dynamic_integrator(self, ocp, nlp): + """ + Properly set the integration in a nlp + + Parameters + ---------- + ocp: OptimalControlProgram + A reference to the main program + nlp: NonLinearProgram + A reference to the current phase of the ocp + """ + + # Primary dynamics + dynamics = [nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=0, node_index=0)] + if nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE: + dynamics = dynamics * nlp.ns + else: + for node_index in range(1, nlp.ns): + dynamics.append(nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=0, node_index=node_index)) + nlp.dynamics = dynamics + + # Extra dynamics + extra_dynamics = [] + for i in range(len(nlp.extra_dynamics_func)): + extra_dynamics += [ + nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=i, node_index=0, is_extra_dynamics=True) + ] + if nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE: + extra_dynamics = extra_dynamics * nlp.ns + else: + for node_index in range(1, nlp.ns): + extra_dynamics += [ + nlp.ode_solver.initialize_integrator( + ocp, nlp, dynamics_index=i, node_index=node_index, is_extra_dynamics=True + ) + ] + nlp.extra_dynamics.append(extra_dynamics) diff --git a/bioptim/dynamics/ode_solver.py b/bioptim/dynamics/ode_solvers.py similarity index 53% rename from bioptim/dynamics/ode_solver.py rename to bioptim/dynamics/ode_solvers.py index 0eb509ae4..784a315d7 100644 --- a/bioptim/dynamics/ode_solver.py +++ b/bioptim/dynamics/ode_solvers.py @@ -3,336 +3,11 @@ from casadi import MX, SX, integrator as casadi_integrator, horzcat, Function, vertcat from . import integrator +from .ode_solver_base import OdeSolverBase +from .rk_base import RK from ..misc.enums import ControlType, DefectType, PhaseDynamics -class OdeSolverBase: - """ - The base class for the ODE solvers - - Methods - ------- - integrator(self, ocp, nlp, node_index) -> list - The interface of the OdeSolver to the corresponding integrator - prepare_dynamic_integrator(ocp, nlp) - Properly set the integration in an nlp - """ - - def __init__(self, duplicate_starting_point: bool = False): - """ - Parameters - ---------- - duplicate_starting_point: bool - If the starting point should be duplicated in the integrator's casadi function - """ - - self.duplicate_starting_point = duplicate_starting_point - - @property - def integrator(self): - """ - The corresponding integrator class - - Returns - ------- - The integrator class - """ - raise RuntimeError("This method should be implemented in the child class") - - @property - def is_direct_collocation(self) -> bool: - """ - indicating if the ode solver is direct collocation method - - Returns - ------- - True if the ode solver is direct collocation method - """ - raise RuntimeError("This method should be implemented in the child class") - - @property - def is_direct_shooting(self) -> bool: - """ - indicating if the ode solver is direct shooting method - - Returns - ------- - True if the ode solver is direct shooting method - """ - raise RuntimeError("This method should be implemented in the child class") - - @property - def n_required_cx(self) -> int: - """ - The required number of column required in the casadi CX matrix for the state variables - - Returns - ------- - The number of required casadi functions - """ - raise RuntimeError("This method should be implemented in the child class") - - @property - def defects_type(self) -> DefectType: - """ - The type of defect - - Returns - ------- - The type of defect - """ - raise RuntimeError("This method should be implemented in the child class") - - def t_ode(self, nlp) -> list: - """ - The time span of the integration - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The time span of the integration - """ - return vertcat(nlp.time_cx, nlp.dt) - - def x_ode(self, nlp) -> MX: - """ - The symbolic state variables - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The symbolic state variables - """ - raise RuntimeError("This method should be implemented in the child class") - - def p_ode(self, nlp) -> MX: - """ - The symbolic controls. The nomenclature is p_ode (instead of the intuitive u_ode) to be consistent with - the scipy integrator - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The symbolic controls - """ - raise RuntimeError("This method should be implemented in the child class") - - def a_ode(self, nlp) -> MX: - """ - The symbolic numerical timeseries - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The symbolic algebraic variables - """ - raise RuntimeError("This method should be implemented in the child class") - - def param_ode(self, nlp) -> MX: - """ - The symbolic parameters - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The symbolic parameters - """ - return nlp.parameters.scaled.cx_start - - def d_ode(self, nlp) -> MX: - """ - The symbolic algebraic states variables - - Parameters - ---------- - nlp - The NonLinearProgram handler - - Returns - ------- - The symbolic numerical timeseries - """ - raise RuntimeError("This method should be implemented in the child class") - - def initialize_integrator( - self, ocp, nlp, dynamics_index: int, node_index: int, is_extra_dynamics: bool = False, **extra_opt - ) -> Callable: - """ - Initialize the integrator - - Parameters - ---------- - ocp - The Optimal control program handler - nlp - The NonLinearProgram handler - node_index - The index of the node currently initialized - dynamics_index - The current extra dynamics to resolve (that can be referred to nlp.extra_dynamics_func[index]) - is_extra_dynamics - If the dynamics is an extra dynamics - extra_opt - Any extra options to pass to the integrator - - Returns - ------- - The initialized integrator function - """ - - if dynamics_index > 0 and not is_extra_dynamics: - raise RuntimeError("dynamics_index should be 0 if is_extra_dynamics is False") - - nlp.states.node_index = node_index - nlp.states_dot.node_index = node_index - nlp.controls.node_index = node_index - nlp.algebraic_states.node_index = node_index - dynamics_func = nlp.dynamics_func if not is_extra_dynamics else nlp.extra_dynamics_func[dynamics_index] - ode_opt = { - "model": nlp.model, - "cx": nlp.cx, - "control_type": nlp.control_type, - "defects_type": self.defects_type, - "ode_index": node_index if dynamics_func.size2_out("xdot") > 1 else 0, - "duplicate_starting_point": self.duplicate_starting_point, - **extra_opt, - } - - ode = { - "t": self.t_ode(nlp), - "x": self.x_ode(nlp), - "u": self.p_ode(nlp), - "a": self.a_ode(nlp), - "d": self.d_ode(nlp), - "param": self.param_ode(nlp), - "ode": dynamics_func, - "implicit_ode": nlp.implicit_dynamics_func, - } - - return nlp.ode_solver.integrator(ode, ode_opt) - - def prepare_dynamic_integrator(self, ocp, nlp): - """ - Properly set the integration in a nlp - - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the main program - nlp: NonLinearProgram - A reference to the current phase of the ocp - """ - - # Primary dynamics - dynamics = [nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=0, node_index=0)] - if nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE: - dynamics = dynamics * nlp.ns - else: - for node_index in range(1, nlp.ns): - dynamics.append(nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=0, node_index=node_index)) - nlp.dynamics = dynamics - - # Extra dynamics - extra_dynamics = [] - for i in range(len(nlp.extra_dynamics_func)): - extra_dynamics += [ - nlp.ode_solver.initialize_integrator(ocp, nlp, dynamics_index=i, node_index=0, is_extra_dynamics=True) - ] - if nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE: - extra_dynamics = extra_dynamics * nlp.ns - else: - for node_index in range(1, nlp.ns): - extra_dynamics += [ - nlp.ode_solver.initialize_integrator( - ocp, nlp, dynamics_index=i, node_index=node_index, is_extra_dynamics=True - ) - ] - nlp.extra_dynamics.append(extra_dynamics) - - -class RK(OdeSolverBase): - """ - The base class for Runge-Kutta - """ - - def __init__(self, n_integration_steps: int = 5, **kwargs): - """ - Parameters - ---------- - n_integration_steps: int - The number of steps for the integration - """ - - super(RK, self).__init__(**kwargs) - self.n_integration_steps = n_integration_steps - - @property - def is_direct_collocation(self) -> bool: - return False - - @property - def is_direct_shooting(self) -> bool: - return True - - @property - def n_required_cx(self) -> int: - return 1 - - @property - def defects_type(self) -> DefectType: - return DefectType.NOT_APPLICABLE - - def initialize_integrator(self, *args, **kwargs): - return super(RK, self).initialize_integrator( - *args, **kwargs, number_of_finite_elements=self.n_integration_steps - ) - - def x_ode(self, nlp): - return nlp.states.scaled.cx_start - - def p_ode(self, nlp): - if nlp.control_type in ( - ControlType.CONSTANT, - ControlType.CONSTANT_WITH_LAST_NODE, - ): - return nlp.controls.scaled.cx_start - else: - return horzcat(nlp.controls.scaled.cx_start, nlp.controls.scaled.cx_end) - - def a_ode(self, nlp): - return nlp.algebraic_states.scaled.cx_start - - def d_ode(self, nlp): - return nlp.numerical_timeseries.cx_start - - def __str__(self): - ode_solver_string = f"{self.integrator.__name__} {self.n_integration_steps} step" - if self.n_integration_steps > 1: - ode_solver_string += "s" - - return ode_solver_string - - class OdeSolver: """ The public interface to the different OdeSolvers @@ -436,7 +111,7 @@ class COLLOCATION(OdeSolverBase): The degree of the implicit RK method : str The method of interpolation ("legendre" or "radau") - defects_type: DefectType + _defects_type: DefectType The type of defect to use (DefectType.EXPLICIT or DefectType.IMPLICIT) duplicate_starting_point: bool Whether an additional collocation point should be added at the shooting node (this is typically used in SOCPs) diff --git a/bioptim/dynamics/rk_base.py b/bioptim/dynamics/rk_base.py new file mode 100644 index 000000000..22918b4aa --- /dev/null +++ b/bioptim/dynamics/rk_base.py @@ -0,0 +1,67 @@ +from casadi import horzcat + +from .ode_solver_base import OdeSolverBase +from ..misc.enums import ControlType, DefectType + + +class RK(OdeSolverBase): + """ + The base class for Runge-Kutta + """ + + def __init__(self, n_integration_steps: int = 5, **kwargs): + """ + Parameters + ---------- + n_integration_steps: int + The number of steps for the integration + """ + + super(RK, self).__init__(**kwargs) + self.n_integration_steps = n_integration_steps + + @property + def is_direct_collocation(self) -> bool: + return False + + @property + def is_direct_shooting(self) -> bool: + return True + + @property + def n_required_cx(self) -> int: + return 1 + + @property + def defects_type(self) -> DefectType: + return DefectType.NOT_APPLICABLE + + def initialize_integrator(self, *args, **kwargs): + return super(RK, self).initialize_integrator( + *args, **kwargs, number_of_finite_elements=self.n_integration_steps + ) + + def x_ode(self, nlp): + return nlp.states.scaled.cx_start + + def p_ode(self, nlp): + if nlp.control_type in ( + ControlType.CONSTANT, + ControlType.CONSTANT_WITH_LAST_NODE, + ): + return nlp.controls.scaled.cx_start + else: + return horzcat(nlp.controls.scaled.cx_start, nlp.controls.scaled.cx_end) + + def a_ode(self, nlp): + return nlp.algebraic_states.scaled.cx_start + + def d_ode(self, nlp): + return nlp.numerical_timeseries.cx_start + + def __str__(self): + ode_solver_string = f"{self.integrator.__name__} {self.n_integration_steps} step" + if self.n_integration_steps > 1: + ode_solver_string += "s" + + return ode_solver_string diff --git a/bioptim/gui/plot.py b/bioptim/gui/plot.py index a967b511c..946bd9254 100644 --- a/bioptim/gui/plot.py +++ b/bioptim/gui/plot.py @@ -7,7 +7,7 @@ from matplotlib.ticker import FuncFormatter from .serializable_class import OcpSerializable -from ..dynamics.ode_solver import OdeSolver +from ..dynamics.ode_solvers import OdeSolver from ..limits.path_conditions import Bounds from ..limits.penalty_helpers import PenaltyHelpers from ..misc.enums import PlotType, Shooting, SolutionIntegrator, QuadratureRule, InterpolationType diff --git a/bioptim/gui/serializable_class.py b/bioptim/gui/serializable_class.py index e5f780355..80e9ec756 100644 --- a/bioptim/gui/serializable_class.py +++ b/bioptim/gui/serializable_class.py @@ -3,7 +3,7 @@ from casadi import Function import numpy as np -from ..dynamics.ode_solver import OdeSolver +from ..dynamics.ode_solvers import OdeSolver from ..limits.penalty_option import PenaltyOption from ..limits.path_conditions import Bounds from ..misc.mapping import BiMapping @@ -354,7 +354,7 @@ def __init__(self, polynomial_degree: int | None, n_integration_steps: int | Non @classmethod def from_ode_solver(cls, ode_solver): - from ..dynamics.ode_solver import OdeSolver + from ..dynamics.ode_solvers import OdeSolver ode_solver: OdeSolver = ode_solver diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 98eecd16c..320ca08e1 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -2,7 +2,7 @@ from casadi import MX, SX, DM, vertcat -from ..dynamics.ode_solver import OdeSolver +from ..dynamics.ode_solvers import OdeSolver from ..misc.enums import ControlType, PhaseDynamics from ..misc.mapping import BiMapping from ..optimization.non_linear_program import NonLinearProgram diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 093b3804d..a1fb74ffc 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -6,7 +6,7 @@ from .optimization_variable import OptimizationVariableContainer from ..dynamics.dynamics_evaluation import DynamicsEvaluation from ..dynamics.dynamics_functions import DynamicsFunctions -from ..dynamics.ode_solver import OdeSolver +from ..dynamics.ode_solvers import OdeSolver from ..limits.path_conditions import InitialGuessList, BoundsList from ..misc.enums import ControlType, PhaseDynamics from ..misc.mapping import NodeMapping diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 7aab54aff..7e91070bd 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -10,7 +10,7 @@ from .non_linear_program import NonLinearProgram as NLP from .optimization_vector import OptimizationVectorHelper from ..dynamics.configure_problem import DynamicsList, Dynamics, ConfigureProblem -from ..dynamics.ode_solver import OdeSolver, OdeSolverBase +from ..dynamics.ode_solvers import OdeSolver, OdeSolverBase from ..gui.check_conditioning import check_conditioning from ..gui.graph import OcpToConsole, OcpToGraph from ..gui.ipopt_output_plot import SaveIterationsInfo diff --git a/bioptim/optimization/solution/solution.py b/bioptim/optimization/solution/solution.py index 45485efdd..d2a6b0dbe 100644 --- a/bioptim/optimization/solution/solution.py +++ b/bioptim/optimization/solution/solution.py @@ -7,7 +7,7 @@ from .solution_data import SolutionData, SolutionMerge, TimeAlignment, TimeResolution from ..optimization_vector import OptimizationVectorHelper -from ...dynamics.ode_solver import OdeSolver +from ...dynamics.ode_solvers import OdeSolver from ...interfaces.solve_ivp_interface import solve_ivp_interface from ...limits.path_conditions import InitialGuess, InitialGuessList from ...limits.penalty_helpers import PenaltyHelpers diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index e4e3d0519..4f88ce83a 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -6,7 +6,7 @@ from .non_linear_program import NonLinearProgram as NLP from .optimization_vector import OptimizationVectorHelper from ..dynamics.configure_problem import DynamicsList, Dynamics -from ..dynamics.ode_solver import OdeSolver +from ..dynamics.ode_solvers import OdeSolver from ..limits.constraints import ( ConstraintFcn, ConstraintList, From bb2cedc44a20be447e0ec5de7d4f41b6fc6d6760 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 30 Jan 2025 08:01:29 -0500 Subject: [PATCH 08/31] feat/ fix: I found all the places with inconsistent n_cols for algebraic states --- bioptim/optimization/optimization_vector.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/bioptim/optimization/optimization_vector.py b/bioptim/optimization/optimization_vector.py index cd2a1a7e7..dd4607746 100644 --- a/bioptim/optimization/optimization_vector.py +++ b/bioptim/optimization/optimization_vector.py @@ -91,7 +91,8 @@ def declare_ocp_shooting_points(ocp): u_scaled[nlp.phase_idx] = u_scaled[nlp.use_controls_from_phase_idx] u[nlp.phase_idx] = u[nlp.use_controls_from_phase_idx] - n_col = nlp.n_algebraic_states_decision_steps(k) + # n_col = nlp.n_algebraic_states_decision_steps(k) + n_col = nlp.n_states_decision_steps(k) a_scaled[nlp.phase_idx].append( nlp.cx.sym(f"A_scaled_{nlp.phase_idx}_{k}", nlp.algebraic_states.scaled.shape, n_col) ) @@ -234,7 +235,8 @@ def bounds_vectors(ocp) -> tuple[np.ndarray, np.ndarray]: nlp.algebraic_states, nlp.a_bounds, nlp.a_scaling, - lambda n: nlp.n_algebraic_states_decision_steps(n), + # lambda n: nlp.n_algebraic_states_decision_steps(n), + lambda n: nlp.n_states_decision_steps(n), ) v_bounds_min = np.concatenate((v_bounds_min, min_bounds)) @@ -318,7 +320,12 @@ def init_vector(ocp): nlp = ocp.nlp[current_nlp.use_states_from_phase_idx] init = _dispatch_state_initial_guess( - nlp, nlp.algebraic_states, nlp.a_init, nlp.a_scaling, lambda n: nlp.n_algebraic_states_decision_steps(n) + # nlp, nlp.algebraic_states, nlp.a_init, nlp.a_scaling, lambda n: nlp.n_algebraic_states_decision_steps(n) + nlp, + nlp.algebraic_states, + nlp.a_init, + nlp.a_scaling, + lambda n: nlp.n_states_decision_steps(n), ) v_init = np.concatenate((v_init, init)) @@ -455,7 +462,8 @@ def to_dictionaries(ocp, data: np.ndarray | DM) -> tuple: continue for node in range(nlp.n_states_nodes): nlp.algebraic_states.node_index = node - n_cols = nlp.n_algebraic_states_decision_steps(node) + # n_cols = nlp.n_algebraic_states_decision_steps(node) + n_cols = nlp.n_states_decision_steps(node) if na == 0: a_array = np.ndarray((0, 1)) From 68832100537396750c83c018c1c9e6c547bb86cd Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 30 Jan 2025 09:53:58 -0500 Subject: [PATCH 09/31] feat: handling 2d dim with partitionned states so that we can send timeseries to state_partition --- bioptim/models/biorbd/holonomic_biorbd_model.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index f5eb806da..508024c58 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -480,9 +480,11 @@ def state_from_partition(self, state_u: MX, state_v: MX) -> MX: q = MX() if isinstance(state_u, MX) else DM() for i in range(self.nb_q): if i in self._independent_joint_index: - q = vertcat(q, state_u[self._independent_joint_index.index(i)]) + slicing = slice(self._independent_joint_index.index(i), self._independent_joint_index.index(i) + 1) + q = vertcat(q, state_u[slicing, :]) else: - q = vertcat(q, state_v[self._dependent_joint_index.index(i)]) + slicing = slice(self._dependent_joint_index.index(i), self._dependent_joint_index.index(i) + 1) + q = vertcat(q, state_v[slicing, :]) return q From 42fe4117d7233a026d46a12687e8976c76d4879c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 30 Jan 2025 12:37:48 -0500 Subject: [PATCH 10/31] feat: algebraic states as proper entry of OCP --- bioptim/optimization/optimal_control_program.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 7e91070bd..fb17cdb1d 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -144,8 +144,10 @@ def __init__( phase_time: int | float | list | tuple, x_bounds: BoundsList = None, u_bounds: BoundsList = None, + a_bounds: BoundsList = None, x_init: InitialGuessList | None = None, u_init: InitialGuessList | None = None, + a_init: InitialGuessList | None = None, objective_functions: Objective | ObjectiveList = None, constraints: Constraint | ConstraintList = None, parameters: ParameterList = None, @@ -165,6 +167,7 @@ def __init__( x_scaling: VariableScalingList = None, xdot_scaling: VariableScalingList = None, u_scaling: VariableScalingList = None, + a_scaling: VariableScalingList = None, n_threads: int = 1, use_sx: bool = False, integrated_value_functions: dict[str, Callable] = None, @@ -184,16 +187,22 @@ def __init__( The initial guesses for the states u_init: InitialGuess | InitialGuessList The initial guesses for the controls + a_init: InitialGuess | InitialGuessList + The initial guesses for the algebraic states x_bounds: Bounds | BoundsList The bounds for the states u_bounds: Bounds | BoundsList The bounds for the controls + a_bounds: Bounds | BoundsList + The bounds for the algebraic states x_scaling: VariableScalingList The scaling for the states at each phase, if only one is sent, then the scaling is copied over the phases xdot_scaling: VariableScalingList The scaling for the states derivative, if only one is sent, then the scaling is copied over the phases u_scaling: VariableScalingList The scaling for the controls, if only one is sent, then the scaling is copied over the phases + a_scaling: VariableScalingList + The scaling for the algebraic states, if only one is sent, then the scaling is copied over the phases objective_functions: Objective | ObjectiveList All the objective function of the program constraints: Constraint | ConstraintList @@ -233,9 +242,6 @@ def __init__( bio_model = self._initialize_model(bio_model) - # Placed here because of MHE - a_init, a_bounds, a_scaling = self._set_internal_algebraic_states() - self._check_and_set_threads(n_threads) self._check_and_set_shooting_points(n_shooting) self._check_and_set_phase_time(phase_time) From 52e57dea08718901dbf0189a8ea0e5d37929becb Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 09:54:11 -0500 Subject: [PATCH 11/31] example: holonomic refactor plot --- .../holonomic_constraints/custom_dynamics.py | 34 +++ .../holonomic_constraints/two_pendulums.py | 3 +- .../two_pendulums_algebraic.py | 233 ++++++++++++++++++ 3 files changed, 269 insertions(+), 1 deletion(-) create mode 100644 bioptim/examples/holonomic_constraints/custom_dynamics.py create mode 100644 bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py diff --git a/bioptim/examples/holonomic_constraints/custom_dynamics.py b/bioptim/examples/holonomic_constraints/custom_dynamics.py new file mode 100644 index 000000000..9fe1e54be --- /dev/null +++ b/bioptim/examples/holonomic_constraints/custom_dynamics.py @@ -0,0 +1,34 @@ +from bioptim import PenaltyController +from casadi import vertcat + + +def constraint_holonomic( + controllers: PenaltyController, +): + """ + Minimize the distance between two markers + By default this function is quadratic, meaning that it minimizes distance between them. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + """ + + q_u = controllers.states["q_u"] + q_u_complete = q_u.mapping.to_second.map(q_u.cx) + + q_v = controllers.algebraic_states["q_v"] + q_v_complete = q_v.mapping.to_second.map(q_v.cx) + + q = controllers.model.state_from_partition(q_u_complete, q_v_complete) + + holonomic_constraints = controllers.model.holonomic_constraints(q) + + for q_u_cx, q_v_cx in zip(q_u.cx_intermediates_list, q_v.cx_intermediates_list): + q_u_complete = q_u.mapping.to_second.map(q_u_cx) + q_v_complete = q_v.mapping.to_second.map(q_v_cx) + q = controllers.model.state_from_partition(q_u_complete, q_v_complete) + holonomic_constraints = vertcat(holonomic_constraints, controllers.model.holonomic_constraints(q)) + + return holonomic_constraints diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index 421e026ec..e66e46093 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -200,7 +200,6 @@ def main(): sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) # --- Show results --- # - sol.graphs() q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) viewer = "pyorerun" @@ -219,6 +218,8 @@ def main(): viz.rerun("double_pendulum") + sol.graphs() + time = sol.decision_time(to_merge=SolutionMerge.NODES) plt.title("Lagrange multipliers of the holonomic constraint") plt.plot(time, lambdas[0, :], label="y") diff --git a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py new file mode 100644 index 000000000..421e026ec --- /dev/null +++ b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py @@ -0,0 +1,233 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import platform + +import matplotlib.pyplot as plt +import numpy as np +from casadi import DM + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsFcn, + DynamicsList, + HolonomicBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OptimalControlProgram, + Solver, + SolutionMerge, +) + + +def compute_all_states(sol, bio_model: HolonomicBiorbdModel): + """ + Compute all the states from the solution of the optimal control program + + Parameters + ---------- + bio_model: HolonomicBiorbdModel + The biorbd model + sol: + The solution of the optimal control program + + Returns + ------- + + """ + + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + + n = states["q_u"].shape[1] + + q = np.zeros((bio_model.nb_q, n)) + qdot = np.zeros((bio_model.nb_q, n)) + qddot = np.zeros((bio_model.nb_q, n)) + lambdas = np.zeros((bio_model.nb_dependent_joints, n)) + tau = np.zeros((bio_model.nb_tau, n)) + + for i, independent_joint_index in enumerate(bio_model.independent_joint_index): + tau[independent_joint_index, :-1] = controls["tau"][i, :] + for i, dependent_joint_index in enumerate(bio_model.dependent_joint_index): + tau[dependent_joint_index, :-1] = controls["tau"][i, :] + + q_v_init = DM.zeros(bio_model.nb_dependent_joints) + for i in range(n): + q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init).toarray() + q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze() + qdot[:, i] = bio_model.compute_qdot()(q[:, i], states["qdot_u"][:, i]).toarray().squeeze() + qddot_u_i = ( + bio_model.partitioned_forward_dynamics()(states["q_u"][:, i], states["qdot_u"][:, i], q_v_init, tau[:, i]) + .toarray() + .squeeze() + ) + qddot[:, i] = bio_model.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() + lambdas[:, i] = ( + bio_model.compute_the_lagrangian_multipliers()( + states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i] + ) + .toarray() + .squeeze() + ) + + return q, qdot, qddot, lambdas + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + bio_model = HolonomicBiorbdModel(biorbd_model_path) + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + biorbd_model=bio_model, + marker_1="marker_1", + marker_2="marker_3", + index=slice(1, 3), + local_frame_index=0, + ) + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model.set_holonomic_configuration( + constraints_list=holonomic_constraints, independent_joint_index=[0, 3], dependent_joint_index=[1, 2] + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsList() + dynamics.add(DynamicsFcn.HOLONOMIC_TORQUE_DRIVEN, expand_dynamics=expand_dynamics) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, None, None, 1], to_first=[0, 3]) + variable_bimapping.add("qdot", to_second=[0, None, None, 1], to_first=[0, 3]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [1.54, 1.54]) + x_init.add("qdot_u", [0, 0]) + x_bounds["q_u"][:, 0] = [1.54, 1.54] + x_bounds["qdot_u"][:, 0] = [0, 0] + x_bounds["q_u"][0, -1] = -1.54 + x_bounds["q_u"][1, -1] = 0 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, None, None, 1], to_first=[0, 3]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min] * 2, max_bound=[tau_max] * 2) + u_init = InitialGuessList() + u_init.add("tau", [tau_init] * 2) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = "models/two_pendulums.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + + # --- Show results --- # + sol.graphs() + q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + time = sol.decision_time(to_merge=SolutionMerge.NODES) + plt.title("Lagrange multipliers of the holonomic constraint") + plt.plot(time, lambdas[0, :], label="y") + plt.plot(time, lambdas[1, :], label="z") + plt.xlabel("Time (s)") + plt.ylabel("Lagrange multipliers (N)") + plt.legend() + plt.show() + + +if __name__ == "__main__": + main() From cfa1cedf37175c7a07ba9c0b4bf3b7edcfeabafd Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 09:56:07 -0500 Subject: [PATCH 12/31] example: holonomic algebraic --- .../holonomic_constraints/custom_dynamics.py | 149 +++++++++++++++++- .../two_pendulums_algebraic.py | 107 +++++++++---- 2 files changed, 225 insertions(+), 31 deletions(-) diff --git a/bioptim/examples/holonomic_constraints/custom_dynamics.py b/bioptim/examples/holonomic_constraints/custom_dynamics.py index 9fe1e54be..786c67b21 100644 --- a/bioptim/examples/holonomic_constraints/custom_dynamics.py +++ b/bioptim/examples/holonomic_constraints/custom_dynamics.py @@ -1,6 +1,38 @@ -from bioptim import PenaltyController from casadi import vertcat +from bioptim import ( + PenaltyController, + DynamicsEvaluation, + DynamicsFunctions, + NonLinearProgram, + OptimalControlProgram, + ConfigureProblem, +) + + +def constraint_holonomic_end( + controllers: PenaltyController, +): + """ + Minimize the distance between two markers + By default this function is quadratic, meaning that it minimizes distance between them. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + """ + + q_u = controllers.states["q_u"] + q_u_complete = q_u.mapping.to_second.map(q_u.cx) + + q_v = controllers.algebraic_states["q_v"] + q_v_complete = q_v.mapping.to_second.map(q_v.cx) + + q = controllers.model.state_from_partition(q_u_complete, q_v_complete) + + return controllers.model.holonomic_constraints(q) + def constraint_holonomic( controllers: PenaltyController, @@ -32,3 +64,118 @@ def constraint_holonomic( holonomic_constraints = vertcat(holonomic_constraints, controllers.model.holonomic_constraints(q)) return holonomic_constraints + + +def holonomic_torque_driven_with_qv( + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + nlp, +) -> DynamicsEvaluation: + """ + The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, a, d) + + Parameters + ---------- + time: MX.sym | SX.sym + The time of the system + states: MX.sym | SX.sym + The state of the system + controls: MX.sym | SX.sym + The controls of the system + parameters: MX.sym | SX.sym + The parameters acting on the system + algebraic_states: MX.sym | SX.sym + The algebraic states of the system + numerical_timeseries: MX.sym | SX.sym + The numerical timeseries of the system + nlp: NonLinearProgram + A reference to the phase + + Returns + ------- + The derivative of the states in the tuple[MX | SX] format + """ + + q_u = DynamicsFunctions.get(nlp.states["q_u"], states) + qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) + tau = DynamicsFunctions.get(nlp.controls["tau"], controls) + q_v = DynamicsFunctions.get(nlp.algebraic_states["q_v"], algebraic_states) + qddot_u = nlp.model.partitioned_forward_dynamics_with_qv()(q_u, q_v, qdot_u, tau) + + return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) + + +def configure_holonomic_torque_driven( + ocp: OptimalControlProgram, + nlp: NonLinearProgram, + numerical_data_timeseries=None, +): + """ + Tell the program which variables are states and controls. + The user is expected to use the ConfigureProblem.configure_xxx functions. + + Parameters + ---------- + ocp: OptimalControlProgram + A reference to the ocp + nlp: NonLinearProgram + A reference to the phase + """ + + name = "q_u" + names_u = [nlp.model.name_dof[i] for i in nlp.model.independent_joint_index] + ConfigureProblem.configure_new_variable( + name, + names_u, + ocp, + nlp, + True, + False, + False, + ) + + name = "q_v" + names_v = [nlp.model.name_dof[i] for i in nlp.model.dependent_joint_index] + ConfigureProblem.configure_new_variable( + name, + names_v, + ocp, + nlp, + False, + False, + False, + as_algebraic_states=True, + ) + + name = "qdot_u" + names_qdot = ConfigureProblem._get_kinematics_based_names(nlp, "qdot") + names_udot = [names_qdot[i] for i in nlp.model.independent_joint_index] + ConfigureProblem.configure_new_variable( + name, + names_udot, + ocp, + nlp, + True, + False, + False, + # NOTE: not ready for phase mapping yet as it is based on dofnames of the class BioModel + # see _set_kinematic_phase_mapping method + # axes_idx=ConfigureProblem._apply_phase_mapping(ocp, nlp, name), + ) + + ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True) + + # extra plots + ConfigureProblem.configure_qv(ocp, nlp, nlp.model.compute_q_v) + ConfigureProblem.configure_qdotv(ocp, nlp, nlp.model._compute_qdot_v) + ConfigureProblem.configure_lagrange_multipliers_function(ocp, nlp, nlp.model.compute_the_lagrangian_multipliers) + + ConfigureProblem.configure_dynamics_function( + ocp, + nlp, + holonomic_torque_driven_with_qv, + ) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py index 421e026ec..d82db68f1 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py @@ -4,9 +4,6 @@ pendulum simulation. """ -import platform - -import matplotlib.pyplot as plt import numpy as np from casadi import DM @@ -14,7 +11,6 @@ BiMappingList, BoundsList, ConstraintList, - DynamicsFcn, DynamicsList, HolonomicBiorbdModel, HolonomicConstraintsFcn, @@ -25,6 +21,16 @@ OptimalControlProgram, Solver, SolutionMerge, + Node, + CostType, + OdeSolver, + TimeAlignment, +) +from custom_dynamics import ( + holonomic_torque_driven_with_qv, + configure_holonomic_torque_driven, + constraint_holonomic, + constraint_holonomic_end, ) @@ -87,6 +93,7 @@ def prepare_ocp( n_shooting: int = 30, final_time: float = 1, expand_dynamics: bool = False, + ode_solver: OdeSolver = OdeSolver.COLLOCATION(polynomial_degree=5), ) -> (HolonomicBiorbdModel, OptimalControlProgram): """ Prepare the program @@ -133,22 +140,32 @@ def prepare_ocp( # Dynamics dynamics = DynamicsList() - dynamics.add(DynamicsFcn.HOLONOMIC_TORQUE_DRIVEN, expand_dynamics=expand_dynamics) - - # Path Constraints - constraints = ConstraintList() + # dynamics.add(DynamicsFcn.HOLONOMIC_TORQUE_DRIVEN, expand_dynamics=expand_dynamics) + dynamics.add( + configure_holonomic_torque_driven, + dynamic_function=holonomic_torque_driven_with_qv, + expand_dynamics=expand_dynamics, + ) # Boundaries - variable_bimapping = BiMappingList() + u_variable_bimapping = BiMappingList() # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints # The dynamics of the dependent joints will be computed from the holonomic constraint - variable_bimapping.add("q", to_second=[0, None, None, 1], to_first=[0, 3]) - variable_bimapping.add("qdot", to_second=[0, None, None, 1], to_first=[0, 3]) + u_variable_bimapping.add("q", to_second=[0, None, None, 1], to_first=[0, 3]) + u_variable_bimapping.add("qdot", to_second=[0, None, None, 1], to_first=[0, 3]) + + v_variable_bimapping = BiMappingList() + v_variable_bimapping.add("q", to_second=[None, 0, 1, None], to_first=[1, 2]) + x_bounds = BoundsList() # q_u and qdot_u are the states of the independent joints - x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) - x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=u_variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=u_variable_bimapping) + + # q_v is the state of the dependent joints + a_bounds = BoundsList() + a_bounds.add("q_v", bio_model.bounds_from_ranges("q", mapping=v_variable_bimapping)) # Initial guess x_init = InitialGuessList() @@ -159,16 +176,33 @@ def prepare_ocp( x_bounds["q_u"][0, -1] = -1.54 x_bounds["q_u"][1, -1] = 0 + a_init = InitialGuessList() + a_init.add("q_v", [0, 2]) + # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 # Only the rotations are controlled - variable_bimapping.add("tau", to_second=[0, None, None, 1], to_first=[0, 3]) + tau_variable_bimapping = BiMappingList() + tau_variable_bimapping.add("tau", to_second=[0, None, None, 1], to_first=[0, 3]) u_bounds = BoundsList() u_bounds.add("tau", min_bound=[tau_min] * 2, max_bound=[tau_max] * 2) u_init = InitialGuessList() u_init.add("tau", [tau_init] * 2) - # ------------- # + # Path Constraints + constraints = ConstraintList() + constraints.add( + constraint_holonomic, + phase=0, + node=Node.ALL_SHOOTING, + # penalty_type=PenaltyType.INTERNAL, + ) + constraints.add( + constraint_holonomic_end, + phase=0, + node=Node.END, + # penalty_type=PenaltyType.INTERNAL, + ) return ( OptimalControlProgram( @@ -195,13 +229,22 @@ def main(): model_path = "models/two_pendulums.bioMod" ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + ocp.add_plot_penalty(CostType.ALL) # --- Solve the program --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + sol = ocp.solve( + Solver.IPOPT( + # show_online_optim=platform.system() == "Linux" + ) + ) + print(sol.real_time_to_optimize) - # --- Show results --- # - sol.graphs() - q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + # _, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + + stepwise_time = sol.stepwise_time(to_merge=SolutionMerge.NODES, time_alignment=TimeAlignment.STATES) + stepwise_q_u = sol.stepwise_states(to_merge=SolutionMerge.NODES)["q_u"] + stepwise_q_v = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES)["q_v"] + q = ocp.nlp[0].model.state_from_partition(stepwise_q_u, stepwise_q_v).toarray() viewer = "pyorerun" if viewer == "bioviz": @@ -212,21 +255,25 @@ def main(): viz.exec() if viewer == "pyorerun": - import pyorerun + from pyorerun import BiorbdModel as PyorerunBiorbdModel, PhaseRerun - viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) - viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + pyomodel = PyorerunBiorbdModel(model_path) + viz = PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyomodel, q=q) viz.rerun("double_pendulum") - time = sol.decision_time(to_merge=SolutionMerge.NODES) - plt.title("Lagrange multipliers of the holonomic constraint") - plt.plot(time, lambdas[0, :], label="y") - plt.plot(time, lambdas[1, :], label="z") - plt.xlabel("Time (s)") - plt.ylabel("Lagrange multipliers (N)") - plt.legend() - plt.show() + # --- Show results --- # + sol.graphs() + # + # time = sol.decision_time(to_merge=SolutionMerge.NODES) + # plt.title("Lagrange multipliers of the holonomic constraint") + # plt.plot(time, lambdas[0, :], label="y") + # plt.plot(time, lambdas[1, :], label="z") + # plt.xlabel("Time (s)") + # plt.ylabel("Lagrange multipliers (N)") + # plt.legend() + # plt.show() if __name__ == "__main__": From c2b8774d302f3aaee8a45a3c39077a0995975439 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 09:58:35 -0500 Subject: [PATCH 13/31] fix: integrator algebraic states offset idx to be squash --- bioptim/dynamics/integrator.py | 4 ++-- .../examples/holonomic_constraints/two_pendulums.py | 12 ++++++++++-- .../holonomic_constraints/two_pendulums_algebraic.py | 5 ++++- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/bioptim/dynamics/integrator.py b/bioptim/dynamics/integrator.py index 43cbb205e..9ac86fdfd 100644 --- a/bioptim/dynamics/integrator.py +++ b/bioptim/dynamics/integrator.py @@ -672,7 +672,7 @@ def dxdt( states[j + 1], self.get_u(controls, self._integration_time[j]), params, - algebraic_states[j], + algebraic_states[j + 1], numerical_timeseries, )[:, self.ode_idx] defects.append(xp_j - f_j * self.h) @@ -684,7 +684,7 @@ def dxdt( states[j + 1], self.get_u(controls, self._integration_time[j]), params, - algebraic_states[j], + algebraic_states[j + 1], numerical_timeseries, xp_j / self.h, ) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index e66e46093..8ea084dca 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -25,6 +25,7 @@ OptimalControlProgram, Solver, SolutionMerge, + OdeSolver, ) @@ -48,12 +49,13 @@ def compute_all_states(sol, bio_model: HolonomicBiorbdModel): controls = sol.decision_controls(to_merge=SolutionMerge.NODES) n = states["q_u"].shape[1] + n_tau = controls["tau"].shape[1] q = np.zeros((bio_model.nb_q, n)) qdot = np.zeros((bio_model.nb_q, n)) qddot = np.zeros((bio_model.nb_q, n)) lambdas = np.zeros((bio_model.nb_dependent_joints, n)) - tau = np.zeros((bio_model.nb_tau, n)) + tau = np.zeros((bio_model.nb_tau, n_tau)) for i, independent_joint_index in enumerate(bio_model.independent_joint_index): tau[independent_joint_index, :-1] = controls["tau"][i, :] @@ -182,6 +184,7 @@ def prepare_ocp( u_init=u_init, objective_functions=objective_functions, variable_mappings=variable_bimapping, + ode_solver=OdeSolver.COLLOCATION(polynomial_degree=5), constraints=constraints, ), bio_model, @@ -197,7 +200,12 @@ def main(): ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) # --- Solve the program --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + sol = ocp.solve( + Solver.IPOPT( + # show_online_optim=platform.system() == "Linux" + ) + ) + print(sol.real_time_to_optimize) # --- Show results --- # q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py index d82db68f1..e35f710db 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py @@ -212,10 +212,13 @@ def prepare_ocp( final_time, x_bounds=x_bounds, u_bounds=u_bounds, + a_bounds=a_bounds, x_init=x_init, u_init=u_init, + a_init=a_init, objective_functions=objective_functions, - variable_mappings=variable_bimapping, + variable_mappings=tau_variable_bimapping, + ode_solver=ode_solver, constraints=constraints, ), bio_model, From 20905183748d2550010cd5c0a271afe8262c8839 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 12:20:41 -0500 Subject: [PATCH 14/31] refactor: standardizing the way variable size is written --- bioptim/dynamics/configure_new_variable.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index ece7f2a2d..4330b9a7b 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -1,5 +1,5 @@ -from casadi import MX, SX, vertcat import numpy as np +from casadi import MX, SX, vertcat from .fatigue.fatigue_dynamics import FatigueList, MultiFatigueInterface from ..gui.plot import CustomPlot @@ -380,10 +380,11 @@ def _declare_cx_and_plot(self): for node_index in range( self.nlp.n_controls_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1 ): + n_cx = 3 cx_scaled = ( self.ocp.nlp[self.nlp.use_controls_from_phase_idx].controls[node_index][self.name].original_cx if self.copy_controls - else self.define_cx_scaled(n_col=3, n_shooting=0, initial_node=node_index) + else self.define_cx_scaled(n_col=n_cx, n_shooting=0, initial_node=node_index) ) cx = ( self.ocp.nlp[self.nlp.use_controls_from_phase_idx].controls[node_index][self.name].original_cx From b146b05bb2e437de952585e3afe80bd46f61270a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 12:22:03 -0500 Subject: [PATCH 15/31] refactor: giving the right size to OptimizationVariableList even if no decision variable in it worthy for algebraic states --- bioptim/limits/penalty_controller.py | 5 +++++ bioptim/optimization/non_linear_program.py | 5 +++++ bioptim/optimization/optimization_variable.py | 22 +++++++++++++++---- 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 320ca08e1..f63625ed7 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -128,6 +128,7 @@ def t_span(self) -> OptimizationVariable: cx = vertcat(self.time.cx, self.dt.cx) tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) + tp._cx_intermediates = [self._nlp.cx([])] n_val = cx.shape[0] tp.append("t_span", cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val))) return tp["t_span"] @@ -140,6 +141,7 @@ def phases_dt(self) -> OptimizationVariable: tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) n_val = self.ocp.dt_parameter.cx.shape[0] + tp._cx_intermediates = [self._nlp.cx([])] tp.append( "phases_dt", cx=[self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx], @@ -155,6 +157,7 @@ def dt(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) + tp._cx_intermediates = [self._nlp.cx([])] n_val = self._nlp.dt.shape[0] tp.append( "dt", @@ -170,6 +173,7 @@ def time(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) + tp._cx_intermediates = [self._nlp.cx([])] n_val = self._nlp.time_cx.shape[0] tp.append( "time", @@ -185,6 +189,7 @@ def tf(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) + tp._cx_intermediates = [self._nlp.cx([])] n_val = self._nlp.tf.shape[0] tp.append( "tf", diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index a1fb74ffc..25404d825 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -212,6 +212,11 @@ def initialize(self, cx: MX | SX | Callable = None): self.algebraic_states.initialize_from_shooting(n_shooting=self.ns + 1, cx=self.cx) self.integrated_values.initialize_from_shooting(n_shooting=self.ns + 1, cx=self.cx) + self.states.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=self.ode_solver.n_required_cx) + self.states_dot.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=self.ode_solver.n_required_cx) + self.controls.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=1) + self.algebraic_states.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=self.ode_solver.n_required_cx) + @property def n_states_nodes(self) -> int: """ diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 352728dd2..e6706731c 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -293,10 +293,7 @@ def append(self, name: str, cx: list, bimapping: BiMapping): self._cx_end = vertcat(self._cx_end, cx[-1]) for i, c in enumerate(cx[1:-1]): - if i >= len(self._cx_intermediates): - self._cx_intermediates.append(c) - else: - self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) + self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) self.elements.append(OptimizationVariable(name, cx, index, bimapping, parent_list=self)) @@ -504,6 +501,23 @@ def initialize_from_shooting(self, n_shooting: int, cx: Callable): self._scaled.append(OptimizationVariableList(cx, self.phase_dynamics)) self._unscaled.append(OptimizationVariableList(cx, self.phase_dynamics)) + def initialize_intermediate_cx(self, n_shooting: int, n_cx: int): + """ + Initialize the containers so the dimensions are up to the required intermediate points, + especially important in collocations + + Parameters + ---------- + n_shooting: int + The number of shooting points + n_cx: int + The number of intermediate points + """ + + for node_index in range(n_shooting): + self._scaled[node_index]._cx_intermediates = [self.cx_constructor([]) for _ in range(n_cx)] + self._unscaled[node_index]._cx_intermediates = [self.cx_constructor([]) for _ in range(n_cx)] + def __getitem__(self, item: int | str): if isinstance(item, int): raise ValueError("To get a specific node, please set the node_index property then call the desired method.") From 437c72c4d6ea9068efc5d6eb4e68d4e2ca40d6dd Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 12:58:30 -0500 Subject: [PATCH 16/31] fix: for tests to pass --- .../holonomic_constraints/two_pendulums.py | 12 ------------ .../two_pendulums_algebraic.py | 15 +-------------- .../muscle_excitations_tracker.py | 5 +++++ bioptim/optimization/optimal_control_program.py | 1 + tests/shard4/test_penalty.py | 1 + 5 files changed, 8 insertions(+), 26 deletions(-) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index 8ea084dca..1a13f4edb 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -4,9 +4,6 @@ pendulum simulation. """ -import platform - -import matplotlib.pyplot as plt import numpy as np from casadi import DM @@ -228,15 +225,6 @@ def main(): sol.graphs() - time = sol.decision_time(to_merge=SolutionMerge.NODES) - plt.title("Lagrange multipliers of the holonomic constraint") - plt.plot(time, lambdas[0, :], label="y") - plt.plot(time, lambdas[1, :], label="z") - plt.xlabel("Time (s)") - plt.ylabel("Lagrange multipliers (N)") - plt.legend() - plt.show() - if __name__ == "__main__": main() diff --git a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py index e35f710db..423a17650 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py @@ -1,7 +1,7 @@ """ This example presents how to implement a holonomic constraint in bioptim. The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double -pendulum simulation. +pendulum simulation. But this time, the dynamics are computed with the algebraic states, namely q_v the dependent joints """ import numpy as np @@ -24,7 +24,6 @@ Node, CostType, OdeSolver, - TimeAlignment, ) from custom_dynamics import ( holonomic_torque_driven_with_qv, @@ -242,9 +241,6 @@ def main(): ) print(sol.real_time_to_optimize) - # _, qdot, qddot, lambdas = compute_all_states(sol, bio_model) - - stepwise_time = sol.stepwise_time(to_merge=SolutionMerge.NODES, time_alignment=TimeAlignment.STATES) stepwise_q_u = sol.stepwise_states(to_merge=SolutionMerge.NODES)["q_u"] stepwise_q_v = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES)["q_v"] q = ocp.nlp[0].model.state_from_partition(stepwise_q_u, stepwise_q_v).toarray() @@ -268,15 +264,6 @@ def main(): # --- Show results --- # sol.graphs() - # - # time = sol.decision_time(to_merge=SolutionMerge.NODES) - # plt.title("Lagrange multipliers of the holonomic constraint") - # plt.plot(time, lambdas[0, :], label="y") - # plt.plot(time, lambdas[1, :], label="z") - # plt.xlabel("Time (s)") - # plt.ylabel("Lagrange multipliers (N)") - # plt.legend() - # plt.show() if __name__ == "__main__": diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index cdeb7f5a9..c83afebcb 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -109,6 +109,11 @@ def generate_data( nlp.controls.initialize_from_shooting(n_shooting, MX) nlp.algebraic_states.initialize_from_shooting(n_shooting, MX) + nlp.states.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.states_dot.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.controls.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.algebraic_states.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + for node_index in range(n_shooting): nlp.states.append( name="q", diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index fb17cdb1d..e51106d47 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1693,6 +1693,7 @@ def _define_numerical_timeseries(self, dynamics): numerical_timeseries = [] for i_phase, nlp in enumerate(self.nlp): numerical_timeseries += [OptimizationVariableList(self.cx, dynamics[i_phase].phase_dynamics)] + numerical_timeseries[-1]._cx_intermediates = [self.cx([])] if dynamics[i_phase].numerical_data_timeseries is not None: for key in dynamics[i_phase].numerical_data_timeseries.keys(): variable_shape = dynamics[i_phase].numerical_data_timeseries[key].shape diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 7df38ab8c..302dc3f45 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -1321,6 +1321,7 @@ def test_PenaltyFunctionAbstract_get_node(node, ns, phase_dynamics): nlp.A = np.linspace(0, 0, ns + 1) nlp.A_scaled = nlp.A tp = OptimizationVariableList(MX, phase_dynamics=phase_dynamics) + tp._cx_intermediates = [MX()] tp.append(name="param", cx=[MX(), MX(), MX()], bimapping=BiMapping([], [])) nlp.parameters = tp["param"] From 000d74068cd92c83f2164ad81a9a1c51aa11be3c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 13:07:32 -0500 Subject: [PATCH 17/31] tests: fixed --- .../examples/muscle_driven_ocp/muscle_activations_tracker.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index 9d3d2a03a..54132e248 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -106,6 +106,11 @@ def generate_data( nlp.controls.initialize_from_shooting(n_shooting, MX) nlp.algebraic_states.initialize_from_shooting(n_shooting, MX) + nlp.states.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.states_dot.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.controls.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + nlp.algebraic_states.initialize_intermediate_cx(n_shooting=n_shooting, n_cx=1) + for node_index in range(n_shooting): nlp.states.append( name="q", From 5aef6665873b3b29c46ff2a4f24bc82136efd74e Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 13:07:41 -0500 Subject: [PATCH 18/31] fix: IRK case --- bioptim/dynamics/integrator.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/bioptim/dynamics/integrator.py b/bioptim/dynamics/integrator.py index 9ac86fdfd..cdbba4e16 100644 --- a/bioptim/dynamics/integrator.py +++ b/bioptim/dynamics/integrator.py @@ -749,6 +749,9 @@ def dxdt( # Root-finding function, implicitly defines x_collocation_points as a function of x0 and p collocation_states = vertcat(*states[1:]) if self.duplicate_starting_point else vertcat(*states[2:]) + algebraic_states = ( + vertcat(*algebraic_states[1:]) if self.duplicate_starting_point else vertcat(*algebraic_states[2:]) + ) vfcn = Function( "vfcn", [collocation_states, self.t_span_sym, states[0], controls, params, algebraic_states, numerical_timeseries], From 6f86c9334e1bfc5ee27d2e1eab6e9a6045b35126 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 13:29:37 -0500 Subject: [PATCH 19/31] some comment to help me debug --- tests/shard5/test_global_stochastic_collocation.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/shard5/test_global_stochastic_collocation.py b/tests/shard5/test_global_stochastic_collocation.py index 9ed6fcf73..e1fac126c 100644 --- a/tests/shard5/test_global_stochastic_collocation.py +++ b/tests/shard5/test_global_stochastic_collocation.py @@ -130,6 +130,8 @@ def test_arm_reaching_torque_driven_collocations(use_sx: bool): controls_sol = sol_socp.decision_controls(to_merge=SolutionMerge.ALL) algebraic_states = sol_socp.decision_algebraic_states(to_merge=SolutionMerge.NODES) + # TODO #907 @IPUCH: the algebraic states should be retrieve the same way as the states, ask @Eve + k_sol, ref_sol, m_sol, cov_sol = ( algebraic_states["k"], algebraic_states["ref"], @@ -173,9 +175,14 @@ def test_arm_reaching_torque_driven_collocations(use_sx: bool): penalty, 0, lambda p_idx, n_idx, sn_idx: algebraic_sol[:, n_idx], + # TODO #907 @IPUCH + # Shoudnt we have + # lambda p_idx, n_idx, sn_idx: algebraic_sol[:, sn_idx, n_idx], ) shoulder_pos_initial = 0.349065850398866 elbow_pos_initial = 2.245867726451909 + # TODO #907 @IPUCH: the size of algebraic_states doesnt fit the penalty function anymore. + constraint_value = penalty.function[0]( duration, dt, From e9f490157c4a4addf7ff48a7b005f1c18e2f39fb Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 13:30:04 -0500 Subject: [PATCH 20/31] tests: fix the numerical time series utils to fit the one in the OCP class --- tests/utils.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/utils.py b/tests/utils.py index 6d1ccca92..e63d1e39a 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -212,6 +212,7 @@ def assert_equal( @staticmethod def initialize_numerical_timeseries(nlp, dynamics): numerical_timeseries = OptimizationVariableList(nlp.cx, dynamics.phase_dynamics) + numerical_timeseries._cx_intermediates = [nlp.cx([])] if dynamics.numerical_data_timeseries is not None: for key in dynamics.numerical_data_timeseries.keys(): variable_shape = dynamics.numerical_data_timeseries[key].shape From 1dc116c3d4419d507b95033fb05ab892241001b8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Jan 2025 13:43:02 -0500 Subject: [PATCH 21/31] fix: integrated values --- bioptim/optimization/non_linear_program.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 25404d825..a9b1b51f5 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -216,6 +216,9 @@ def initialize(self, cx: MX | SX | Callable = None): self.states_dot.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=self.ode_solver.n_required_cx) self.controls.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=1) self.algebraic_states.initialize_intermediate_cx(n_shooting=self.ns + 1, n_cx=self.ode_solver.n_required_cx) + self.integrated_values.initialize_intermediate_cx( + n_shooting=self.ns + 1, n_cx=1 + ) # TODO #907 to confirm with Eve @property def n_states_nodes(self) -> int: From 7b19efbe176ba1a7ac97987ae7782ced8ccba54a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 3 Feb 2025 06:01:38 -0500 Subject: [PATCH 22/31] todos --- .../obstacle_avoidance_direct_collocation.py | 2 +- bioptim/limits/constraints.py | 7 +++++-- tests/shard5/test_global_stochastic_collocation.py | 6 +++--- tests/shard6/test_global_stochastic_except_collocation.py | 7 +++---- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/bioptim/examples/stochastic_optimal_control/obstacle_avoidance_direct_collocation.py b/bioptim/examples/stochastic_optimal_control/obstacle_avoidance_direct_collocation.py index c6f042c40..e164b351d 100644 --- a/bioptim/examples/stochastic_optimal_control/obstacle_avoidance_direct_collocation.py +++ b/bioptim/examples/stochastic_optimal_control/obstacle_avoidance_direct_collocation.py @@ -623,7 +623,7 @@ def main(): # Solver parameters solver = Solver.IPOPT(show_online_optim=False, show_options=dict(show_bounds=True)) - solver.set_linear_solver("ma57") + # solver.set_linear_solver("ma57") sol_socp = socp.solve(solver) time = sol_socp.decision_time(to_merge=SolutionMerge.NODES) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 978771a57..5241a018c 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -906,7 +906,9 @@ def collocation_jacobians(penalty, controller): horzcat(controller.states_scaled.cx, horzcat(*controller.states_scaled.cx_intermediates_list)), controller.controls_scaled.cx, controller.parameters_scaled.cx, - controller.algebraic_states_scaled.cx, + # controller.algebraic_states_scaled.cx, + # todo #907 : manual duplicate of the algebraic states along 7 + horzcat(controller.algebraic_states.cx, horzcat(*controller.algebraic_states.cx_intermediates_list)), controller.numerical_timeseries.cx, ) @@ -927,7 +929,8 @@ def collocation_jacobians(penalty, controller): horzcat(*controller.states_scaled.cx_intermediates_list), controller.controls_scaled.cx_start, controller.parameters_scaled.cx, - controller.algebraic_states_scaled.cx_start, + horzcat(*controller.algebraic_states_scaled.cx_intermediates_list), + # controller.algebraic_states_scaled.cx_start, controller.numerical_timeseries.cx, ], [Fdz.T - Gdz.T @ m_matrix.T], diff --git a/tests/shard5/test_global_stochastic_collocation.py b/tests/shard5/test_global_stochastic_collocation.py index e1fac126c..6e25cfe58 100644 --- a/tests/shard5/test_global_stochastic_collocation.py +++ b/tests/shard5/test_global_stochastic_collocation.py @@ -1,11 +1,11 @@ import platform -from bioptim import Solver, SocpType, SolutionMerge, PenaltyHelpers, SolutionIntegrator -from casadi import DM, vertcat import numpy as np import numpy.testing as npt import pytest +from casadi import DM, vertcat +from bioptim import Solver, SocpType, SolutionMerge, PenaltyHelpers, SolutionIntegrator from ..utils import TestUtils @@ -439,7 +439,7 @@ def test_obstacle_avoidance_direct_collocation(use_sx: bool): np.random.seed(42) TestUtils.compare_ocp_to_solve( ocp, - v=np.ones([1107, 1]), # Random values here returns nan for g + v=np.ones([1107, 1]), # Random values here returns nan for g # todo : CHANGE THE SIZE ACCORDING TO A expected_v_f_g=[1107.0, 10.01, -170696.19805582374], decimal=6, ) diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 30eb8841c..7861a8d51 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -1,12 +1,11 @@ import platform -from bioptim import Solver, SolutionMerge, SolutionIntegrator -from bioptim.examples.stochastic_optimal_control.arm_reaching_torque_driven_implicit import ExampleType -from casadi import DM, vertcat import numpy as np import numpy.testing as npt import pytest +from casadi import DM, vertcat +from bioptim import Solver, SolutionMerge, SolutionIntegrator from ..utils import TestUtils @@ -443,7 +442,7 @@ def test_arm_reaching_torque_driven_implicit(with_scaling, use_sx): TestUtils.compare_ocp_to_solve( ocp, - v=np.random.rand(457, 1), + v=np.random.rand(457, 1), # change the size accrding to A expected_v_f_g=expected, decimal=6, ) From a200e032630620dcbeb954bbc4d7db34f16d4b98 Mon Sep 17 00:00:00 2001 From: ipuch Date: Tue, 4 Feb 2025 16:49:30 -0500 Subject: [PATCH 23/31] fix: on algebraic states --- bioptim/optimization/optimal_control_program.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index e51106d47..0ec79b0f9 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1118,7 +1118,8 @@ def update_bounds( if a_bounds is not None: if not isinstance(a_bounds, BoundsList): raise RuntimeError("a_bounds should be built from a BoundsList") - for key in a_bounds.keys(): + origin_phase = 0 if len(a_bounds) == 1 else i + for key in a_bounds[origin_phase].keys(): if key not in self.nlp[i].algebraic_states.keys() + ["None"]: raise ValueError( f"{key} is not an algebraic variable, please check for typos in the declaration of a_bounds" From 5c87647edc81e6ca696b4b9035ec0706ea994b36 Mon Sep 17 00:00:00 2001 From: ipuch Date: Tue, 4 Feb 2025 16:56:32 -0500 Subject: [PATCH 24/31] feat: allow for multiple ODE Solvers for multiphases problems. e.g. collocation and then RK4 --- bioptim/optimization/optimal_control_program.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 0ec79b0f9..c181b5302 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -515,8 +515,11 @@ def _check_arguments_and_build_nlp( if ode_solver is None: ode_solver = self._set_default_ode_solver() - elif not isinstance(ode_solver, OdeSolverBase): - raise RuntimeError("ode_solver should be built an instance of OdeSolver") + + is_ode_solver = isinstance(ode_solver, OdeSolverBase) + is_list_ode_solver = all([isinstance(ode, OdeSolverBase) for ode in ode_solver]) + if not is_ode_solver and not is_list_ode_solver: + raise RuntimeError("ode_solver should be built an instance of OdeSolver or a list of OdeSolver") if not isinstance(use_sx, bool): raise RuntimeError("use_sx should be a bool") From 21e60304648499e73fe860ddf9884002f91f1053 Mon Sep 17 00:00:00 2001 From: ipuch Date: Tue, 4 Feb 2025 17:19:28 -0500 Subject: [PATCH 25/31] hacky: disgusting solution, need to make sure i can skip any intermediate phases --- bioptim/optimization/optimal_control_program.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index c181b5302..5786eb6b9 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1122,6 +1122,8 @@ def update_bounds( if not isinstance(a_bounds, BoundsList): raise RuntimeError("a_bounds should be built from a BoundsList") origin_phase = 0 if len(a_bounds) == 1 else i + if origin_phase + 1 > len(a_bounds): + continue # Trying to skip the phases if it doesn't have any algebraic states for key in a_bounds[origin_phase].keys(): if key not in self.nlp[i].algebraic_states.keys() + ["None"]: raise ValueError( @@ -1210,6 +1212,8 @@ def update_initial_guess( if not isinstance(a_init, InitialGuessList): raise RuntimeError("a_init should be built from a InitialGuessList") origin_phase = 0 if len(a_init) == 1 else i + if origin_phase + 1 > len(a_init): + continue # Trying to skip the phases if it doesn't have any algebraic states for key in a_init[origin_phase].keys(): if key not in self.nlp[i].algebraic_states.keys() + ["None"]: raise ValueError( From cf04062cf2fd7f3dd25239a45f371d0b4c50024d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 7 Feb 2025 10:57:00 -0500 Subject: [PATCH 26/31] cherry pick ode solver test --- bioptim/optimization/optimal_control_program.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 5786eb6b9..50501b93a 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -517,7 +517,11 @@ def _check_arguments_and_build_nlp( ode_solver = self._set_default_ode_solver() is_ode_solver = isinstance(ode_solver, OdeSolverBase) - is_list_ode_solver = all([isinstance(ode, OdeSolverBase) for ode in ode_solver]) + is_list_ode_solver = ( + all([isinstance(ode, OdeSolverBase) for ode in ode_solver]) + if isinstance(ode_solver, list) or isinstance(ode_solver, tuple) + else False + ) if not is_ode_solver and not is_list_ode_solver: raise RuntimeError("ode_solver should be built an instance of OdeSolver or a list of OdeSolver") From f312c24e3a7c9edc4721fc5aa6c70015c8f60061 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 7 Feb 2025 10:57:32 -0500 Subject: [PATCH 27/31] very poor success @restoring stochastic... --- bioptim/limits/constraints.py | 11 ++++++----- tests/shard5/test_global_stochastic_collocation.py | 6 ++++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 5241a018c..983e06cc3 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -901,14 +901,16 @@ def collocation_jacobians(penalty, controller): controller.algebraic_states_scaled["m"].cx_start, controller.model.matrix_shape_m ) + # Algebraic states are pre-crunshed for each interval meaning the algebraic state cx_start in enough + n_rep = controller.integrate_extra_dynamics(0).function.sparsity_in(4).shape[1] + duplicated_algebraic_states = horzcat(*[controller.algebraic_states_scaled.cx_start for _ in range(n_rep)]) + xf, _, defects = controller.integrate_extra_dynamics(0).function( vertcat(controller.t_span.cx), horzcat(controller.states_scaled.cx, horzcat(*controller.states_scaled.cx_intermediates_list)), controller.controls_scaled.cx, controller.parameters_scaled.cx, - # controller.algebraic_states_scaled.cx, - # todo #907 : manual duplicate of the algebraic states along 7 - horzcat(controller.algebraic_states.cx, horzcat(*controller.algebraic_states.cx_intermediates_list)), + duplicated_algebraic_states, controller.numerical_timeseries.cx, ) @@ -929,8 +931,7 @@ def collocation_jacobians(penalty, controller): horzcat(*controller.states_scaled.cx_intermediates_list), controller.controls_scaled.cx_start, controller.parameters_scaled.cx, - horzcat(*controller.algebraic_states_scaled.cx_intermediates_list), - # controller.algebraic_states_scaled.cx_start, + controller.algebraic_states_scaled.cx_start, # purposely cx_start because of the pre-crunching controller.numerical_timeseries.cx, ], [Fdz.T - Gdz.T @ m_matrix.T], diff --git a/tests/shard5/test_global_stochastic_collocation.py b/tests/shard5/test_global_stochastic_collocation.py index 6e25cfe58..4fd67e3a0 100644 --- a/tests/shard5/test_global_stochastic_collocation.py +++ b/tests/shard5/test_global_stochastic_collocation.py @@ -436,11 +436,13 @@ def test_obstacle_avoidance_direct_collocation(use_sx: bool): solver.set_maximum_iterations(4) # Check the values which will be sent to the solver + v_size = 4307 + # v_size = 1107 np.random.seed(42) TestUtils.compare_ocp_to_solve( ocp, - v=np.ones([1107, 1]), # Random values here returns nan for g # todo : CHANGE THE SIZE ACCORDING TO A - expected_v_f_g=[1107.0, 10.01, -170696.19805582374], + v=np.ones([v_size, 1]), # Random values here returns nan for g # todo : CHANGE THE SIZE ACCORDING TO A + expected_v_f_g=[v_size, 10.01, -170696.19805582374], decimal=6, ) if platform.system() == "Windows": From fa080a2eb2155c8a954b034746de2fccba66c38f Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 7 Feb 2025 12:44:15 -0500 Subject: [PATCH 28/31] refactor feat delete: astates stuff no longer neeeded black --- .../optimization/optimal_control_program.py | 9 --------- .../stochastic_optimal_control_program.py | 20 +++---------------- 2 files changed, 3 insertions(+), 26 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 50501b93a..0cab1e97a 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1788,15 +1788,6 @@ def _set_default_ode_solver(self): """ return OdeSolver.RK4() - def _set_internal_algebraic_states(self): - """ - Set the algebraic states to their internal values - """ - self._a_init = InitialGuessList() - self._a_bounds = BoundsList() - self._a_scaling = VariableScalingList() - return self._a_init, self._a_bounds, self._a_scaling - def _set_nlp_is_stochastic(self): """ Set the is_stochastic variable to False diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index 4f88ce83a..bec8604c1 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -79,9 +79,6 @@ def __init__( _check_has_no_phase_dynamics_shared_during_the_phase(problem_type, **kwargs) self.problem_type = problem_type - self._a_init = a_init - self._a_bounds = a_bounds - self._a_scaling = a_scaling # Parameters if parameters is None: @@ -138,8 +135,10 @@ def __init__( phase_time=phase_time, x_bounds=x_bounds, u_bounds=u_bounds, + a_bounds=a_bounds, x_init=x_init, u_init=u_init, + a_init=a_init, objective_functions=objective_functions, constraints=constraints, parameters=parameters, @@ -159,6 +158,7 @@ def __init__( x_scaling=x_scaling, xdot_scaling=xdot_scaling, u_scaling=u_scaling, + a_scaling=a_scaling, n_threads=n_threads, use_sx=use_sx, integrated_value_functions=integrated_value_functions, @@ -593,20 +593,6 @@ def _set_default_ode_solver(self): else: raise RuntimeError("Wrong choice of problem_type, you must choose one of the SocpType.") - def _set_internal_algebraic_states(self): - """ - Set the algebraic_states variables to their internal values - - Note - ---- - This method overrides the method in OptimalControlProgram - """ - return ( - self._a_init, - self._a_bounds, - self._a_scaling, - ) # Nothing to do here as they are already set before calling super().__init__ - def _set_nlp_is_stochastic(self): """ Set the is_stochastic variable to True for all the nlp From d89ebf38c2bef6f01f3decc86931fd62e370d99c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Feb 2025 13:58:59 -0500 Subject: [PATCH 29/31] make sure n_cx = 3 is used rolling back on algebraic states --- bioptim/dynamics/configure_new_variable.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index cfafa0514..46779f7b3 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -292,7 +292,7 @@ def _declare_cx_and_plot(self): self.nlp.n_controls_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1 ): n_cx = 3 - cx_scaled = self.define_cx_scaled(n_col=3, node_index=node_index) + cx_scaled = self.define_cx_scaled(n_col=n_cx, node_index=node_index) cx = self.define_cx_unscaled(cx_scaled, self.nlp.u_scaling[self.name].scaling) self.nlp.controls.append( self.name, @@ -339,18 +339,9 @@ def _declare_cx_and_plot(self): for node_index in range( self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1 ): - n_cx = 2 - cx_scaled = ( - self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[node_index][self.name].original_cx - if self.copy_algebraic_states - else self.define_cx_scaled(n_col=n_cx, n_shooting=0, initial_node=node_index) - ) - cx = ( - self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[node_index][self.name].original_cx - if self.copy_algebraic_states - else self.define_cx_unscaled(cx_scaled, self.nlp.a_scaling[self.name].scaling) - ) - + n_cx = self.nlp.ode_solver.n_required_cx + 2 + cx_scaled = self.define_cx_scaled(n_col=n_cx, node_index=node_index) + cx = self.define_cx_unscaled(cx_scaled, self.nlp.a_scaling[self.name].scaling) self.nlp.algebraic_states.append( self.name, cx, From 3d92b82ea1180a93ebd7bc28ed29477b96b28ce8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Feb 2025 14:10:53 -0500 Subject: [PATCH 30/31] tests: restore example --- bioptim/examples/holonomic_constraints/two_pendulums.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index 1a13f4edb..6652ee827 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -175,13 +175,13 @@ def prepare_ocp( dynamics, n_shooting, final_time, + ode_solver=OdeSolver.RK4(), x_bounds=x_bounds, u_bounds=u_bounds, x_init=x_init, u_init=u_init, objective_functions=objective_functions, variable_mappings=variable_bimapping, - ode_solver=OdeSolver.COLLOCATION(polynomial_degree=5), constraints=constraints, ), bio_model, From 21f3516c2cfb7020008e085698dc963b09c05a93 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Feb 2025 14:22:30 -0500 Subject: [PATCH 31/31] tests: algebraic holonomic pb --- .../two_pendulums_algebraic.py | 4 +- tests/shard1/test_biorbd_model_holonomic.py | 165 +++++++++++++++++- 2 files changed, 163 insertions(+), 6 deletions(-) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py index 423a17650..6d693cbfb 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums_algebraic.py @@ -25,7 +25,7 @@ CostType, OdeSolver, ) -from custom_dynamics import ( +from .custom_dynamics import ( holonomic_torque_driven_with_qv, configure_holonomic_torque_driven, constraint_holonomic, @@ -92,7 +92,7 @@ def prepare_ocp( n_shooting: int = 30, final_time: float = 1, expand_dynamics: bool = False, - ode_solver: OdeSolver = OdeSolver.COLLOCATION(polynomial_degree=5), + ode_solver: OdeSolver = OdeSolver.COLLOCATION(polynomial_degree=2), ) -> (HolonomicBiorbdModel, OptimalControlProgram): """ Prepare the program diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 616665cc0..480a1e4e8 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -1,11 +1,9 @@ -import os - -from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge -from casadi import DM, MX import numpy as np import numpy.testing as npt import pytest +from casadi import DM, MX +from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge from ..utils import TestUtils @@ -184,3 +182,162 @@ def test_example_two_pendulums(): ], decimal=6, ) + + +def test_example_two_pendulums_algebraic(): + """Test the holonomic_constraints/two_pendulums_algebraic example""" + from bioptim.examples.holonomic_constraints import two_pendulums_algebraic + + bioptim_folder = TestUtils.module_folder(two_pendulums_algebraic) + + # --- Prepare the ocp --- # + ocp, model = two_pendulums_algebraic.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/two_pendulums.bioMod", + n_shooting=5, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + algebraic_states = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES) + + qu = states["q_u"] + qdot_u = states["qdot_u"] + qv = algebraic_states["q_v"] + + npt.assert_almost_equal( + qu, + np.array( + [ + [ + 1.54, + 1.53147122, + 1.41845956, + 1.34425805, + 1.25630081, + 0.98624955, + 0.87651554, + 0.76604885, + 0.4498159, + 0.32878399, + 0.21276672, + -0.19326641, + -0.37448602, + -0.58527414, + -1.25818395, + -1.54, + ], + [ + 1.54, + 1.54181016, + 1.55158498, + 1.55693049, + 1.55510653, + 1.44104916, + 1.35937733, + 1.25005648, + 0.83921773, + 0.64778373, + 0.41571636, + -0.12210508, + -0.28375032, + -0.347894, + -0.18407068, + 0.0, + ], + ] + ), + decimal=6, + ) + + npt.assert_almost_equal( + qv, + np.array( + [ + [ + 0.99952583, + 0.99922687, + 0.98841918, + 0.97444976, + 0.95095256, + 0.83396227, + 0.76851408, + 0.69329324, + 0.43479975, + 0.32289239, + 0.21116504, + -0.19206551, + -0.36579422, + -0.55242792, + -0.95153339, + -0.99952583, + ], + [ + -0.03079146, + -0.03931497, + -0.15174825, + -0.2246056, + -0.30933677, + -0.55182147, + -0.63983288, + -0.72065559, + -0.90052717, + -0.94643568, + -0.97745042, + -0.98138211, + -0.93069576, + -0.83356067, + -0.30754544, + -0.03079146, + ], + ] + ), + decimal=6, + ) + + npt.assert_almost_equal( + qdot_u, + np.array( + [ + [ + 0.0, + -0.68327871, + -2.57908681, + -3.28363595, + -3.58353342, + -4.21217445, + -4.37247415, + -4.41193342, + -4.71692498, + -4.90073503, + -4.91946837, + -6.80169755, + -8.1608516, + -8.68741596, + -10.73781539, + -11.7122476, + ], + [ + 0.0, + 0.09005906, + 0.1921158, + 0.17676746, + -0.49377154, + -2.79878086, + -3.8156258, + -4.74474516, + -7.11514715, + -7.92128247, + -8.77923903, + -6.74632722, + -4.40017592, + -1.21807955, + 5.94725113, + 8.01054089, + ], + ] + ), + decimal=6, + )