diff --git a/bioptim/examples/getting_started/example_multiphase.py b/bioptim/examples/getting_started/example_multiphase.py index a4b912673..cb841718e 100644 --- a/bioptim/examples/getting_started/example_multiphase.py +++ b/bioptim/examples/getting_started/example_multiphase.py @@ -27,6 +27,7 @@ MultinodeObjectiveList, PhaseDynamics, ControlType, + QuadratureRule, ) @@ -42,6 +43,7 @@ def prepare_ocp( phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, expand_dynamics: bool = True, control_type: ControlType = ControlType.CONSTANT, + quadrature_rule: QuadratureRule = QuadratureRule.RECTANGLE_LEFT, ) -> OptimalControlProgram: """ Prepare the ocp @@ -65,6 +67,8 @@ def prepare_ocp( (for instance IRK is not compatible with expanded dynamics) control_type: ControlType The type of the controls + quadrature_rule: QuadratureRule + The quadrature method to use to integrate the objective functions Returns ------- @@ -83,9 +87,27 @@ def prepare_ocp( # Add objective functions objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) + objective_functions.add( + ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, + key="tau", + weight=100, + phase=0, + integration_rule=quadrature_rule, + ) + objective_functions.add( + ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, + key="tau", + weight=100, + phase=1, + integration_rule=quadrature_rule, + ) + objective_functions.add( + ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, + key="tau", + weight=100, + phase=2, + integration_rule=quadrature_rule, + ) multinode_objective = MultinodeObjectiveList() multinode_objective.add( diff --git a/bioptim/limits/penalty_option.py b/bioptim/limits/penalty_option.py index 44124f86f..b96e4dad2 100644 --- a/bioptim/limits/penalty_option.py +++ b/bioptim/limits/penalty_option.py @@ -1,14 +1,14 @@ from typing import Any, Callable -from casadi import vertcat, Function, MX, SX, jacobian, diag import numpy as np +from casadi import vertcat, Function, MX, SX, jacobian, diag from .penalty_controller import PenaltyController +from ..limits.penalty_helpers import PenaltyHelpers from ..misc.enums import Node, PlotType, ControlType, PenaltyType, QuadratureRule, PhaseDynamics -from ..misc.options import OptionGeneric from ..misc.mapping import BiMapping +from ..misc.options import OptionGeneric from ..models.protocols.stochastic_biomodel import StochasticBioModel -from ..limits.penalty_helpers import PenaltyHelpers class PenaltyOption(OptionGeneric): @@ -51,8 +51,6 @@ class PenaltyOption(OptionGeneric): If the minimization is applied to derivative of the penalty [f(t, t+1)] integration_rule: QuadratureRule The integration rule to use for the penalty - transition: bool - If the penalty is a transition nodes_phase: tuple[int, ...] The index of the phases when penalty is multinodes penalty_type: PenaltyType diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 81ebf8bcd..62da7efa0 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -3,15 +3,15 @@ from casadi import vertcat, MX -from .multinode_penalty import MultinodePenalty, MultinodePenaltyFunctions from .multinode_constraint import MultinodeConstraint -from .path_conditions import Bounds +from .multinode_penalty import MultinodePenalty, MultinodePenaltyFunctions from .objective_functions import ObjectiveFunction +from .path_conditions import Bounds from ..limits.penalty import PenaltyFunctionAbstract, PenaltyController -from ..misc.enums import Node, PenaltyType, InterpolationType +from ..misc.enums import Node, PenaltyType, InterpolationType, ControlType from ..misc.fcn_enum import FcnEnum -from ..misc.options import UniquePerPhaseOptionList from ..misc.mapping import BiMapping +from ..misc.options import UniquePerPhaseOptionList class PhaseTransition(MultinodePenalty): @@ -127,48 +127,6 @@ def print(self): """ raise NotImplementedError("Printing of PhaseTransitionList is not ready yet") - def prepare_phase_transitions(self, ocp) -> list: - """ - Configure all the phase transitions and put them in a list - - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - - Returns - ------- - The list of all the transitions prepared - """ - - # By default it assume Continuous. It can be change later - full_phase_transitions = [ - PhaseTransition( - phase_pre_idx=i, - transition=PhaseTransitionFcn.CONTINUOUS, - weight=ocp.nlp[i].dynamics_type.state_continuity_weight, - ) - for i in range(ocp.n_phases - 1) - ] - - existing_phases = [] - - for pt in self: - idx_phase = pt.nodes_phase[0] - if idx_phase >= ocp.n_phases: - raise RuntimeError("Phase index of the phase transition is higher than the number of phases") - existing_phases.append(idx_phase) - - if pt.weight: - pt.base = ObjectiveFunction.MayerFunction - - if idx_phase % ocp.n_phases == ocp.n_phases - 1: - # Add a cyclic constraint or objective - full_phase_transitions.append(pt) - else: - full_phase_transitions[idx_phase] = pt - return full_phase_transitions - class PhaseTransitionFunctions(PenaltyFunctionAbstract): """ @@ -211,6 +169,40 @@ def continuous( transition, controllers, "all", states_mapping=states_mapping ) + @staticmethod + def continuous_controls( + transition, + controllers: list[PenaltyController, PenaltyController], + controls_mapping: list[BiMapping, ...] = None, + ): + """ + This continuity function is only relevant for ControlType.LINEAR_CONTINUOUS otherwise don't use it. + + Parameters + ---------- + transition : PhaseTransition + A reference to the phase transition + controllers: list[PenaltyController, PenaltyController] + The penalty node elements + controls_mapping: list + A list of the mapping for the states between nodes. It should provide a mapping between 0 and i, where + the first (0) link the controllers[0].controls to a number of values using to_second. Thereafter, the + to_first is used sequentially for all the controllers (meaning controllers[1] uses the + controls_mapping[0].to_first. Therefore, the dimension of the states_mapping + should be 'len(controllers) - 1' + + Returns + ------- + The difference between the controls after and before + """ + if controls_mapping is not None: + raise NotImplementedError( + "Controls_mapping is not yet implemented " + "for continuous_controls with linear continuous control type." + ) + + return MultinodePenaltyFunctions.Functions.controls_equality(transition, controllers, "all") + @staticmethod def discontinuous(transition, controllers: list[PenaltyController, PenaltyController]): """ @@ -353,6 +345,7 @@ class PhaseTransitionFcn(FcnEnum): """ CONTINUOUS = (PhaseTransitionFunctions.Functions.continuous,) + CONTINUOUS_CONTROLS = (PhaseTransitionFunctions.Functions.continuous_controls,) DISCONTINUOUS = (PhaseTransitionFunctions.Functions.discontinuous,) IMPACT = (PhaseTransitionFunctions.Functions.impact,) CYCLIC = (PhaseTransitionFunctions.Functions.cyclic,) diff --git a/bioptim/limits/phase_transtion_factory.py b/bioptim/limits/phase_transtion_factory.py new file mode 100644 index 000000000..246119b58 --- /dev/null +++ b/bioptim/limits/phase_transtion_factory.py @@ -0,0 +1,100 @@ +from .objective_functions import ObjectiveFunction +from .phase_transition import PhaseTransition, PhaseTransitionFcn, PhaseTransitionList +from ..misc.enums import ControlType + + +class PhaseTransitionFactory: + """ + A class to prepare the phase transitions for the ocp builder + + Methods + ------- + create_default_transitions() + Create the default phase transitions for states continuity between phases. + extend_transitions_for_linear_continuous() + Add phase transitions for linear continuous controls. + update_existing_transitions() + Update the existing phase transitions with Mayer functions and add cyclic transitions + + Attributes + ---------- + ocp: OptimalControlProgram + A reference to the ocp + full_phase_transitions: list[PhaseTransition] + The list of all the transitions prepared + """ + + def __init__(self, ocp): + self.ocp = ocp + self.full_phase_transitions = self.create_default_transitions() + + def create_default_transitions(self) -> list[PhaseTransition]: + """Create the default phase transitions for states continuity between phases.""" + return [ + PhaseTransition( + phase_pre_idx=i, + transition=PhaseTransitionFcn.CONTINUOUS, + weight=self.ocp.nlp[i].dynamics_type.state_continuity_weight, + ) + for i in range(self.ocp.n_phases - 1) + ] + + def extend_transitions_for_linear_continuous_controls(self): + """Add phase transitions for linear continuous controls. + This is a special case where the controls are continuous""" + + for phase, nlp in enumerate(self.ocp.nlp[:-1]): + if nlp.control_type == ControlType.LINEAR_CONTINUOUS: + self.full_phase_transitions.append( + PhaseTransition( + phase_pre_idx=phase, + transition=PhaseTransitionFcn.CONTINUOUS_CONTROLS, + weight=None, # Continuity always enforced by the linear continuous control + ) + ) + + def check_phase_index(self, idx_phase): + """Check if the phase index is valid.""" + if idx_phase >= self.ocp.n_phases: + raise RuntimeError("Phase index of the phase transition is higher than the number of phases") + + def update_transition_base(self, pt): + """Update the transition base with Mayer functions + if the user provided a weight like for an objective function.""" + if pt.weight: + pt.base = ObjectiveFunction.MayerFunction + + def handle_cyclic_transition(self, idx_phase, pt): + """The case of a cyclic transition, the terminal phase is linked (n) to the initial phase (0).""" + if idx_phase % self.ocp.n_phases == self.ocp.n_phases - 1: + self.full_phase_transitions.append(pt) + else: + self.full_phase_transitions[idx_phase] = pt + + def update_existing_transitions(self, phase_transition_list) -> list[PhaseTransition]: + """Update the existing phase transitions with Mayer functions and add cyclic transitions.""" + existing_phases = [] + for pt in phase_transition_list: + idx_phase = pt.nodes_phase[0] + self.check_phase_index(idx_phase) + existing_phases.append(idx_phase) + self.update_transition_base(pt) + self.handle_cyclic_transition(idx_phase, pt) + return self.full_phase_transitions + + def prepare_phase_transitions(self, phase_transition_list: PhaseTransitionList) -> list[PhaseTransition]: + """ + Configure all the phase transitions and put them in a list + + Parameters + ---------- + phase_transition_list: PhaseTransitionList + The phase transitions to prepare added by the user + + Returns + ------- + list[PhaseTransition] + The list of all the transitions prepared + """ + self.extend_transitions_for_linear_continuous_controls() + return self.update_existing_transitions(phase_transition_list) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 815fa296c..def862906 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1,20 +1,19 @@ -from typing import Callable, Any from math import inf +from typing import Callable, Any -import numpy as np import biorbd_casadi as biorbd import casadi -from casadi import MX, SX, sum1, horzcat, vertcat +import numpy as np +from casadi import MX, SX, sum1, horzcat from matplotlib import pyplot as plt -from .optimization_vector import OptimizationVectorHelper 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 ..gui.plot import CustomPlot, PlotOcp +from ..gui.check_conditioning import check_conditioning from ..gui.graph import OcpToConsole, OcpToGraph -from ..models.protocols.biomodel import BioModel -from ..models.biorbd.variational_biorbd_model import VariationalBiorbdModel +from ..gui.plot import CustomPlot, PlotOcp from ..interfaces import Solver from ..interfaces.abstract_options import GenericSolver from ..limits.constraints import ( @@ -25,7 +24,6 @@ ParameterConstraintList, ParameterConstraint, ) -from ..limits.phase_transition import PhaseTransitionList, PhaseTransitionFcn from ..limits.multinode_constraint import MultinodeConstraintList from ..limits.multinode_objective import MultinodeObjectiveList from ..limits.objective_functions import ( @@ -35,11 +33,13 @@ ParameterObjectiveList, ParameterObjective, ) +from ..limits.objective_functions import ObjectiveFunction from ..limits.path_conditions import BoundsList, Bounds from ..limits.path_conditions import InitialGuess, InitialGuessList from ..limits.penalty import PenaltyOption from ..limits.penalty_helpers import PenaltyHelpers -from ..limits.objective_functions import ObjectiveFunction +from ..limits.phase_transition import PhaseTransitionList, PhaseTransitionFcn +from ..limits.phase_transtion_factory import PhaseTransitionFactory from ..misc.__version__ import __version__ from ..misc.enums import ( ControlType, @@ -54,11 +54,12 @@ ) from ..misc.mapping import BiMappingList, Mapping, BiMapping, NodeMappingList from ..misc.options import OptionDict +from ..models.biorbd.variational_biorbd_model import VariationalBiorbdModel +from ..models.protocols.biomodel import BioModel from ..optimization.parameters import ParameterList, Parameter, ParameterContainer from ..optimization.solution.solution import Solution from ..optimization.solution.solution_data import SolutionMerge from ..optimization.variable_scaling import VariableScalingList, VariableScaling -from ..gui.check_conditioning import check_conditioning class OptimalControlProgram: @@ -661,7 +662,7 @@ def _finalize_penalties( # Define continuity constraints # Prepare phase transitions (Reminder, it is important that parameters are declared before, # otherwise they will erase the phase_transitions) - self.phase_transitions = phase_transitions.prepare_phase_transitions(self) + self.phase_transitions = PhaseTransitionFactory(ocp=self).prepare_phase_transitions(phase_transitions) # Skipping creates an OCP without built-in continuity constraints, make sure you declared constraints elsewhere self._declare_continuity() @@ -818,42 +819,50 @@ def _declare_continuity(self) -> None: """ for nlp in self.nlp: # Inner-phase - if nlp.dynamics_type.skip_continuity: - continue + self._declare_inner_phase_continuity(nlp) - if nlp.dynamics_type.state_continuity_weight is None: - # Continuity as constraints - penalty = Constraint( - ConstraintFcn.STATE_CONTINUITY, node=Node.ALL_SHOOTING, penalty_type=PenaltyType.INTERNAL - ) - penalty.add_or_replace_to_penalty_pool(self, nlp) - if nlp.ode_solver.is_direct_collocation and nlp.ode_solver.duplicate_starting_point: - penalty = Constraint( - ConstraintFcn.FIRST_COLLOCATION_HELPER_EQUALS_STATE, - node=Node.ALL_SHOOTING, - penalty_type=PenaltyType.INTERNAL, - ) - penalty.add_or_replace_to_penalty_pool(self, nlp) + for pt in self.phase_transitions: # Inter-phase + self._declare_phase_transition_continuity(pt) - else: - # Continuity as objectives - penalty = Objective( - ObjectiveFcn.Mayer.STATE_CONTINUITY, - weight=nlp.dynamics_type.state_continuity_weight, - quadratic=True, + def _declare_inner_phase_continuity(self, nlp: NLP) -> None: + """Declare the continuity function for the state variables in a phase""" + if nlp.dynamics_type.skip_continuity: + return + + if nlp.dynamics_type.state_continuity_weight is None: + # Continuity as constraints + penalty = Constraint( + ConstraintFcn.STATE_CONTINUITY, node=Node.ALL_SHOOTING, penalty_type=PenaltyType.INTERNAL + ) + penalty.add_or_replace_to_penalty_pool(self, nlp) + if nlp.ode_solver.is_direct_collocation and nlp.ode_solver.duplicate_starting_point: + penalty = Constraint( + ConstraintFcn.FIRST_COLLOCATION_HELPER_EQUALS_STATE, node=Node.ALL_SHOOTING, penalty_type=PenaltyType.INTERNAL, ) penalty.add_or_replace_to_penalty_pool(self, nlp) - for pt in self.phase_transitions: - # Phase transition as constraints - if pt.type == PhaseTransitionFcn.DISCONTINUOUS: - continue - # Dynamics must be respected between phases - pt.name = f"PHASE_TRANSITION ({pt.type.name}) {pt.nodes_phase[0] % self.n_phases}->{pt.nodes_phase[1] % self.n_phases}" - pt.list_index = -1 - pt.add_or_replace_to_penalty_pool(self, self.nlp[pt.nodes_phase[0]]) + else: + # Continuity as objectives + penalty = Objective( + ObjectiveFcn.Mayer.STATE_CONTINUITY, + weight=nlp.dynamics_type.state_continuity_weight, + quadratic=True, + node=Node.ALL_SHOOTING, + penalty_type=PenaltyType.INTERNAL, + ) + penalty.add_or_replace_to_penalty_pool(self, nlp) + + def _declare_phase_transition_continuity(self, pt): + """Declare the continuity function for the variables between phases, mainly for the state variables""" + # Phase transition as constraints + if pt.type == PhaseTransitionFcn.DISCONTINUOUS: + return + # Dynamics must be respected between phases + pt.name = f"PHASE_TRANSITION ({pt.type.name}) {pt.nodes_phase[0] % self.n_phases}->{pt.nodes_phase[1] % self.n_phases}" + pt.list_index = -1 + pt.add_or_replace_to_penalty_pool(self, self.nlp[pt.nodes_phase[0]]) def update_objectives(self, new_objective_function: Objective | ObjectiveList): """ diff --git a/tests/shard1/test_continuity_linear_continuous.py b/tests/shard1/test_continuity_linear_continuous.py new file mode 100644 index 000000000..ad5eaae1c --- /dev/null +++ b/tests/shard1/test_continuity_linear_continuous.py @@ -0,0 +1,58 @@ +import os + +import numpy as np + +from bioptim import PhaseDynamics, ControlType, QuadratureRule, Solver + + +def test_continuity_linear_continuous_global(): + """ + This test combines linear continuous controls and trapezoidal integration method. + This is to make sure we take into account the combined effect of these two features. + """ + from bioptim.examples.getting_started import example_multiphase as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube.bioMod", + long_optim=False, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics=True, + control_type=ControlType.LINEAR_CONTINUOUS, + quadrature_rule=QuadratureRule.TRAPEZOIDAL, + ) + + solution = ocp.solve(Solver.IPOPT(_max_iter=5)) + + actual_costs = solution.detailed_cost + + expected_costs = [ + { + "name": "Lagrange.MINIMIZE_CONTROL", + "cost_value_weighted": 19401.119148753893, + "cost_value": 196.2000000000025, + }, + { + "name": "MultinodeObjectiveFcn.CUSTOM", + "cost_value_weighted": 0.08631534265793345, + "cost_value": -0.03625119905959934, + }, + { + "name": "Lagrange.MINIMIZE_CONTROL", + "cost_value_weighted": 48130.30361835311, + "cost_value": 294.30000000000007, + }, + { + "name": "Lagrange.MINIMIZE_CONTROL", + "cost_value_weighted": 38561.67005961334, + "cost_value": 196.1999999999997, + }, + ] + + # Assert that the expected and actual cost values are the same + for expected, actual in zip(expected_costs, actual_costs): + assert expected["name"] == actual["name"] + + np.testing.assert_almost_equal(expected["cost_value_weighted"], actual["cost_value_weighted"]) + np.testing.assert_almost_equal(expected["cost_value"], actual["cost_value"])