diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index db94aca16..987e2cbf3 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -134,7 +134,10 @@ def initialize(ocp, nlp): nlp.dynamics_type.type(ocp, nlp, **nlp.dynamics_type.extra_parameters) @staticmethod - def custom(ocp, nlp, **extra_params): + def custom(ocp, + nlp, + dynamics_constants_used_at_each_nodes: dict[list] = {}, + **extra_params): """ Call the user-defined dynamics configuration function @@ -146,7 +149,7 @@ def custom(ocp, nlp, **extra_params): A reference to the phase """ - nlp.dynamics_type.configure(ocp, nlp, **extra_params) + nlp.dynamics_type.configure(ocp, nlp, dynamics_constants_used_at_each_nodes, **extra_params) @staticmethod def torque_driven( @@ -955,6 +958,7 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): nlp.controls.scaled.mx_reduced, nlp.parameters.scaled.mx_reduced, nlp.algebraic_states.scaled.mx_reduced, + nlp.dynamics_constants.mx, nlp, **extra_params, ) diff --git a/bioptim/limits/penalty_helpers.py b/bioptim/limits/penalty_helpers.py index 969ade59f..1aaeafaca 100644 --- a/bioptim/limits/penalty_helpers.py +++ b/bioptim/limits/penalty_helpers.py @@ -135,22 +135,14 @@ def dynamics_constants(penalty, index, get_dynamics_constants: Callable, is_cons d = [] phases, _, subnodes = _get_multinode_indices(penalty, is_constructing_penalty) for phase, sub in zip(phases, subnodes): - d += [_reshape_to_vector(get_dynamics_constants(phase, node, sub))] - return _vertcat(d) + d_tp = get_dynamics_constants(phase, node, sub) + if d_tp.shape != (0, 0): + d += [_reshape_to_vector(get_dynamics_constants(phase, node, sub))] + return _vertcat(d) if len(d) > 0 else DM() d = get_dynamics_constants(penalty.phase, node, 0) # cx_start - try: - if d.shape != (0, 0): - d = _reshape_to_vector(d) - except: - print("ici") - - # if is_constructing_penalty: - # d = _reshape_to_vector(get_dynamics_constants(penalty.phase, node, 0)) # cx_start - # else: - # d0 = _reshape_to_vector(get_dynamics_constants(penalty.phase, node, 0)) - # d1 = _reshape_to_vector(get_dynamics_constants(penalty.phase, node, -1)) - # d = _vertcat([d0, d1]) + if d.shape != (0, 0): + d = _reshape_to_vector(d) return d diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index e41001ea4..d5bb16156 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1620,7 +1620,7 @@ def _define_dynamics_constants(self, dynamics): ) dynamics_constants[-1].append( - name=key, + name=f"{key}_{i_component}", cx=[cx, cx, cx], mx=mx, bimapping=BiMapping( diff --git a/bioptim/optimization/solution/solution.py b/bioptim/optimization/solution/solution.py index 7c8992e70..a6eb2ff81 100644 --- a/bioptim/optimization/solution/solution.py +++ b/bioptim/optimization/solution/solution.py @@ -846,6 +846,7 @@ def _states_for_phase_integration( ------- The states to integrate """ + from ...interfaces.interface_utils import _get_dynamics_constants # In the case of multiple shootings, we don't need to do anything special if shooting_type == Shooting.MULTIPLE: @@ -869,15 +870,21 @@ def _states_for_phase_integration( 0, lambda p, n, sn: decision_controls[p][n][:, sn] if n < len(decision_controls[p]) else np.ndarray((0, 1)), ) - s = PenaltyHelpers.states( + a = PenaltyHelpers.states( penalty, 0, lambda p, n, sn: ( decision_algebraic_states[p][n][:, sn] if n < len(decision_algebraic_states[p]) else np.ndarray((0, 1)) ), ) + d_tp = PenaltyHelpers.dynamics_constants( + penalty, + 0, + lambda p, n, sn: _get_dynamics_constants(self.ocp, p, n, sn) + ) + d = np.array([]) if d_tp.shape == (0, 0) else np.array(d_tp) - dx = penalty.function[-1](t0, dt, x, u, params, s) + dx = penalty.function[-1](t0, dt, x, u, params, a, d) if dx.shape[0] != decision_states[phase_idx][0].shape[0]: raise RuntimeError( f"Phase transition must have the same number of states ({dx.shape[0]}) " @@ -897,6 +904,7 @@ def _integrate_stepwise(self) -> None: dict The integrated data structure similar in structure to the original _decision_states """ + from ...interfaces.interface_utils import _get_dynamics_constants params = self._parameters.to_dict(to_merge=SolutionMerge.KEYS, scaled=True)[0][0] t_spans = self.t_span(time_alignment=TimeAlignment.CONTROLS) @@ -908,6 +916,14 @@ def _integrate_stepwise(self) -> None: unscaled: list = [None] * len(self.ocp.nlp) for p, nlp in enumerate(self.ocp.nlp): + d = [] + for n_idx in range(nlp.ns + 1): + d_tp = _get_dynamics_constants(self.ocp, p, n_idx, 0) + if d_tp.shape == (0, 0): + d += [np.array([])] + else: + d += [np.array(d_tp)] + integrated_sol = solve_ivp_interface( shooting_type=Shooting.MULTIPLE, nlp=nlp, @@ -916,6 +932,7 @@ def _integrate_stepwise(self) -> None: u=u[p], a=a[p], p=params, + d=d, method=SolutionIntegrator.OCP, ) @@ -1114,11 +1131,11 @@ def animate( self._check_models_comes_from_same_super_class() all_bioviz = [] - for i, d in enumerate(data_to_animate): + for i, data in enumerate(data_to_animate): all_bioviz.append( self.ocp.nlp[i].model.animate( self.ocp, - solution=d, + solution=data, show_now=show_now, tracked_markers=tracked_markers, **kwargs, diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 8a728d86e..19b7eaa0a 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -49,13 +49,15 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx nlp.time_mx = MX.sym("time", 1, 1) nlp.dt_mx = MX.sym("dt", 1, 1) nlp.initialize(cx) + # TODO: fake the initialization of dynamics_constants nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) @@ -64,87 +66,49 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - external_forces = ( - [ - [ - [ - "Seg0", - np.array( - [ - 0.374540118847362, - 0.950714306409916, - 0.731993941811405, - 0.598658484197037, - 0.156018640442437, - 0.155994520336203, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.058083612168199, - 0.866176145774935, - 0.601115011743209, - 0.708072577796045, - 0.020584494295802, - 0.969909852161994, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.832442640800422, - 0.212339110678276, - 0.181824967207101, - 0.183404509853434, - 0.304242242959538, - 0.524756431632238, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.431945018642116, - 0.291229140198042, - 0.611852894722379, - 0.139493860652042, - 0.292144648535218, - 0.366361843293692, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.456069984217036, - 0.785175961393014, - 0.19967378215836, - 0.514234438413612, - 0.592414568862042, - 0.046450412719998, - ] - ), - ] - ], - ] - if with_external_force - else None - ) + external_forces = None + if with_external_force: + external_forces = np.zeros((6, 1, nlp.ns + 1)) + external_forces[:, 0, 0] = [ + 0.374540118847362, + 0.950714306409916, + 0.731993941811405, + 0.598658484197037, + 0.156018640442437, + 0.155994520336203, + ] + external_forces[:, 0, 1] = [ + 0.058083612168199, + 0.866176145774935, + 0.601115011743209, + 0.708072577796045, + 0.020584494295802, + 0.969909852161994, + ] + external_forces[:, 0, 2] = [ + 0.832442640800422, + 0.212339110678276, + 0.181824967207101, + 0.183404509853434, + 0.304242242959538, + 0.524756431632238, + ] + external_forces[:, 0, 3] = [ + 0.431945018642116, + 0.291229140198042, + 0.611852894722379, + 0.139493860652042, + 0.292144648535218, + 0.366361843293692, + ] + external_forces[:, 0, 4] = [ + 0.456069984217036, + 0.785175961393014, + 0.19967378215836, + 0.514234438413612, + 0.592414568862042, + 0.046450412719998, + ] ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) nlp.control_type = ControlType.CONSTANT @@ -157,7 +121,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + dynamics_constants_used_at_each_nodes={"external_forces": external_forces} if external_forces is not None else {}, ), False, ) @@ -174,16 +138,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics if with_external_force: np.random.rand(nlp.ns, 6) # just not to change the values of the next random values - # Prepare the dynamics - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) @@ -439,7 +394,8 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -456,28 +412,18 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) nlp.control_type = ControlType.CONSTANT - external_forces = ( - [ - [ - [ - "Seg0", - np.array( - [ - 0.3745401188473625, - 0.9507143064099162, - 0.7319939418114051, - 0.5986584841970366, - 0.15601864044243652, - 0.15599452033620265, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces = None + if with_external_force: + external_forces = np.zeros((6, 1, nlp.ns + 1)) + external_forces[:, 0, 0] = [ + 0.3745401188473625, + 0.9507143064099162, + 0.7319939418114051, + 0.5986584841970366, + 0.15601864044243652, + 0.15599452033620265, + ] + external_forces[:, 0, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -485,14 +431,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0.020584494295802447, 0.9699098521619943, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -500,14 +439,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0.3042422429595377, 0.5247564316322378, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -515,14 +447,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0.29214464853521815, 0.3663618432936917, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -530,13 +455,6 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0.5924145688620425, 0.046450412719997725, ] - ), - ] - ], - ] - if with_external_force - else None - ) NonLinearProgram.add( ocp, @@ -546,7 +464,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + dynamics_constants_used_at_each_nodes={"external_forces": external_forces} if external_forces is not None else {}, ), False, ) @@ -564,16 +482,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d if with_external_force: np.random.rand(nlp.ns, 6) - # Prepare the dynamics - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) @@ -962,7 +871,8 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -977,13 +887,10 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - external_forces = ( - [ - [ - [ - "Seg0", - np.array( - [ + external_forces = None + if with_external_force: + external_forces = np.zeros((6, 1, nlp.ns + 1)) + external_forces[:, 0, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -991,14 +898,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0.15601864044243652, 0.15599452033620265, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -1006,14 +906,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0.020584494295802447, 0.9699098521619943, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -1021,14 +914,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0.3042422429595377, 0.5247564316322378, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -1036,14 +922,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0.29214464853521815, 0.3663618432936917, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -1051,13 +930,6 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0.5924145688620425, 0.046450412719997725, ] - ), - ] - ], - ] - if with_external_force - else None - ) ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) nlp.control_type = ControlType.CONSTANT @@ -1069,7 +941,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + dynamics_constants_used_at_each_nodes={"external_forces": external_forces} if external_forces is not None else {}, ), False, ) @@ -1086,16 +958,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d if with_external_force: np.random.rand(nlp.ns, 6) - # Prepare the dynamics - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) @@ -1166,7 +1029,8 @@ def test_torque_activation_driven_with_residual_torque( # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod" + TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", + segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -1180,13 +1044,10 @@ def test_torque_activation_driven_with_residual_torque( nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - external_forces = ( - [ - [ - [ - "Seg0", - np.array( - [ + external_forces = None + if with_external_force: + external_forces = np.zeros((6, 1, nlp.ns + 1)) + external_forces[:, 0, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -1194,14 +1055,7 @@ def test_torque_activation_driven_with_residual_torque( 0.15601864044243652, 0.15599452033620265, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -1209,14 +1063,7 @@ def test_torque_activation_driven_with_residual_torque( 0.020584494295802447, 0.9699098521619943, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -1224,14 +1071,7 @@ def test_torque_activation_driven_with_residual_torque( 0.3042422429595377, 0.5247564316322378, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -1239,14 +1079,7 @@ def test_torque_activation_driven_with_residual_torque( 0.29214464853521815, 0.3663618432936917, ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ + external_forces[:, 0, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -1254,13 +1087,6 @@ def test_torque_activation_driven_with_residual_torque( 0.5924145688620425, 0.046450412719997725, ] - ), - ] - ], - ] - if with_external_force - else None - ) ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) nlp.control_type = ControlType.CONSTANT @@ -1272,7 +1098,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque=with_residual_torque, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + dynamics_constants_used_at_each_nodes={"external_forces": external_forces} if external_forces is not None else {}, ), False, ) @@ -1289,16 +1115,7 @@ def test_torque_activation_driven_with_residual_torque( if with_external_force: np.random.rand(nlp.ns, 6) - # Prepare the dynamics - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) @@ -1434,7 +1251,8 @@ def test_muscle_driven( ): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) - nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod") + nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", + segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"]) nlp.ns = 5 nlp.cx = cx nlp.time_mx = MX.sym("time", 1, 1) @@ -1449,13 +1267,10 @@ def test_muscle_driven( nlp.a_scaling = VariableScalingList() nlp.phase_idx = 0 - external_forces = ( - [ - [ - [ - "r_ulna_radius_hand_rotation1", - np.array( - [ + external_forces = None + if with_external_force: + external_forces = np.zeros((6, 1, nlp.ns + 1)) + external_forces[:, 0, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -1463,14 +1278,7 @@ def test_muscle_driven( 0.15601864044243652, 0.15599452033620265, ] - ), - ] - ], - [ - [ - "r_ulna_radius_hand_rotation1", - np.array( - [ + external_forces[:, 0, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -1478,14 +1286,7 @@ def test_muscle_driven( 0.020584494295802447, 0.9699098521619943, ] - ), - ] - ], - [ - [ - "r_ulna_radius_hand_rotation1", - np.array( - [ + external_forces[:, 0, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -1493,14 +1294,7 @@ def test_muscle_driven( 0.3042422429595377, 0.5247564316322378, ] - ), - ] - ], - [ - [ - "r_ulna_radius_hand_rotation1", - np.array( - [ + external_forces[:, 0, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -1508,14 +1302,7 @@ def test_muscle_driven( 0.29214464853521815, 0.3663618432936917, ] - ), - ] - ], - [ - [ - "r_ulna_radius_hand_rotation1", - np.array( - [ + external_forces[:, 0, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -1523,13 +1310,6 @@ def test_muscle_driven( 0.5924145688620425, 0.046450412719997725, ] - ), - ] - ], - ] - if with_external_force - else None - ) ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) nlp.control_type = ControlType.CONSTANT @@ -1544,7 +1324,7 @@ def test_muscle_driven( rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + dynamics_constants_used_at_each_nodes={"external_forces": external_forces} if external_forces is not None else {}, ), False, ) @@ -1564,15 +1344,7 @@ def test_muscle_driven( # Prepare the dynamics if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: pass - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns)