Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[RTM when tests pass] external forces moved out of ocp too #779

Merged
merged 18 commits into from
Oct 20, 2023
Merged

[RTM when tests pass] external forces moved out of ocp too #779

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
385f66f
all_in
Ipuch Oct 16, 2023
cc7cd37
black
Ipuch Oct 16, 2023
b8247b7
on going
Ipuch Oct 16, 2023
f32b61b
remove comment
Ipuch Oct 17, 2023
e22e672
extra check for phase dynamics and external forces
Ipuch Oct 17, 2023
342af0a
scoot doc
Ipuch Oct 17, 2023
b067bb1
missing one input
Ipuch Oct 17, 2023
e0332a2
remove doc
Ipuch Oct 17, 2023
baf9ecb
test dyanmics works again d
Ipuch Oct 17, 2023
5b48c77
black
Ipuch Oct 17, 2023
6f9c840
adjust one test
Ipuch Oct 18, 2023
cea5b3d
readme update :)
Ipuch Oct 18, 2023
1671428
removed from SCOP too
Ipuch Oct 18, 2023
10af3cc
more unit tests
Ipuch Oct 18, 2023
597d30e
black
Ipuch Oct 18, 2023
f7c4058
last review
Ipuch Oct 19, 2023
80cad52
test macproof
Ipuch Oct 20, 2023
f1120f7
black
Ipuch Oct 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
21 changes: 7 additions & 14 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -696,7 +696,6 @@ OptimalControlProgram(
objective_functions: [Objective, ObjectiveList],
constraints: [Constraint, ConstraintList],
parameters: ParameterList,
external_forces: list[list[Any | np.ndarray]],
ode_solver: OdeSolver,
control_type: [ControlType, list],
all_generalized_mapping: BiMapping,
Expand Down Expand Up @@ -724,8 +723,7 @@ In the case of a multiphase optimization, one model per phase should be passed i
`u_scaling` is the scaling applied to the controls variables (see The variable scaling section).
`objective_functions` is the objective function set of the ocp (see The objective functions section).
`constraints` is the constraint set of the ocp (see The constraints section).
`parameters` is the parameter set of the ocp (see The parameters section).
`external_forces` are the external forces acting on the center of mass of the bodies.
`parameters` is the parameter set of the ocp (see The parameters section).
It is a list (one element for each phase) of np.ndarray of shape (6, i, n), where the 6 components are [Mx, My, Mz, Fx, Fy, Fz], for the ith force platform (defined by the externalforceindex) for each node n.
`ode_solver` is the ode solver used to solve the dynamic equations.
`control_type` is the type of discretization of the controls (usually CONSTANT) (see ControlType section).
Expand Down Expand Up @@ -1872,22 +1870,17 @@ The solver must minimize the force to lift the box while reaching the marker in
It is designed to show how to use external forces. An example of external forces that depends on the state (for
example, a spring) can be found at 'examples/torque_driven_ocp/spring_load.py'

Please note that the point of application of the external forces is defined in the `bioMod` file by the
`externalforceindex` tag in segment and is acting at the center of mass of this particular segment. Please note that
this segment must have at least one degree of freedom defined (translations and/or rotations). Otherwise, the
external_force is silently ignored.

`Bioptim` expects `external_forces` to be a list (one element for each phase) of
a list (for each shooting node) of np.ndarray [6 x n], where the six components are [Mx, My, Mz, Fx, Fy, Fz], for the ith force platform
(defined by the `externalforceindex`) for each node n. Let us take a look at the definition of the external forces in
`Bioptim` expects `external_forces` to be a list (for each shooting node) of np.ndarray [6 x n],
where the six components are [Mx, My, Mz, Fx, Fy, Fz], expressed at the origin of the global reference frame for each node n.
Let us take a look at the definition of the external forces in
this example :

```python
external_forces = external_forces = [[np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T for _ in range(n_shooting)]]
external_forces = [[["Seg1", (0, 0, 0, 0, 0, -2)], ["Test", (0, 0, 0, 0, 0, 5)]] for _ in range(n_shooting)]
```

`external_forces` is of len 1 because there is only one phase. The list is 30-element long, and each array are 6x2 since there
is [Mx, My, Mz, Fx, Fy, Fz] for the two `externalforceindex` for each node (in this example, we take 30 shooting nodes).
`external_forces` is 30-element long, and each sub list array are composed of a string, the name of the segment, and a tuple, the external force.
The tuple is [Mx, My, Mz, Fx, Fy, Fz] for each node (in this example, we take 30 shooting nodes).

### The [example_implicit_dynamics.py](./bioptim/examples/getting_started/example_implicit_dynamics.py) file
*#TODO*
Expand Down
148 changes: 103 additions & 45 deletions bioptim/dynamics/configure_problem.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,7 @@ def torque_driven(
rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE,
fatigue: FatigueList = None,
external_forces: list = None,
):
"""
Configure the dynamics for a torque driven program (states are q and qdot, controls are tau)
Expand All @@ -182,26 +183,16 @@ def torque_driven(
which soft contact dynamic should be used
fatigue: FatigueList
A list of fatigue elements

external_forces: list[Any]
A list of external forces
"""
if with_contact and nlp.model.nb_contacts == 0:
raise ValueError("No contact defined in the .bioMod, set with_contact to False")
if nlp.model.nb_soft_contacts != 0:
if (
soft_contacts_dynamics != SoftContactDynamics.CONSTRAINT
and soft_contacts_dynamics != SoftContactDynamics.ODE
):
raise ValueError(
"soft_contacts_dynamics can be used only with SoftContactDynamics.ODE or SoftContactDynamics.CONSTRAINT"
)

if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS:
if soft_contacts_dynamics == SoftContactDynamics.ODE:
raise ValueError(
"Soft contacts dynamics should not be used with SoftContactDynamics.ODE "
"when rigidbody dynamics is not RigidBodyDynamics.ODE . "
"Please set soft_contacts_dynamics=SoftContactDynamics.CONSTRAINT"
)
_check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
_check_soft_contacts_dynamics(
rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx
)
_check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx)
_check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx)

# Declared rigidbody states and controls
ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False)
Expand Down Expand Up @@ -288,11 +279,14 @@ def torque_driven(
with_passive_torque=with_passive_torque,
with_ligament=with_ligament,
with_friction=with_friction,
external_forces=external_forces,
)

# Configure the contact forces
if with_contact:
ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven)
ConfigureProblem.configure_contact_function(
ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces
)
# Configure the soft contact forces
ConfigureProblem.configure_soft_contact_function(ocp, nlp)
# Algebraic constraints of soft contact forces if needed
Expand Down Expand Up @@ -391,6 +385,7 @@ def torque_derivative_driven(
with_ligament: bool = False,
rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE,
external_forces: list = None,
):
"""
Configure the dynamics for a torque driven program (states are q and qdot, controls are tau)
Expand All @@ -411,30 +406,20 @@ def torque_derivative_driven(
which rigidbody dynamics should be used
soft_contacts_dynamics: SoftContactDynamics
which soft contact dynamic should be used
external_forces: list[Any]
A list of external forces

"""
if with_contact and nlp.model.nb_contacts == 0:
raise ValueError("No contact defined in the .bioMod, set with_contact to False")
_check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)

if rigidbody_dynamics not in (RigidBodyDynamics.DAE_INVERSE_DYNAMICS, RigidBodyDynamics.ODE):
raise NotImplementedError("TORQUE_DERIVATIVE_DRIVEN cannot be used with this enum RigidBodyDynamics yet")

if nlp.model.nb_soft_contacts != 0:
if (
soft_contacts_dynamics != SoftContactDynamics.CONSTRAINT
and soft_contacts_dynamics != SoftContactDynamics.ODE
):
raise ValueError(
"soft_contacts_dynamics can be used only with RigidBodyDynamics.ODE or SoftContactDynamics.CONSTRAINT"
)

if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS:
if soft_contacts_dynamics == SoftContactDynamics.ODE:
raise ValueError(
"Soft contacts dynamics should not be used with RigidBodyDynamics.ODE "
"when rigidbody dynamics is not RigidBodyDynamics.ODE . "
"Please set soft_contacts_dynamics=SoftContactDynamics.CONSTRAINT"
)
_check_soft_contacts_dynamics(
rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx
)
_check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx)
_check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx)

ConfigureProblem.configure_q(ocp, nlp, True, False)
ConfigureProblem.configure_qdot(ocp, nlp, True, False)
Expand Down Expand Up @@ -464,10 +449,16 @@ def torque_derivative_driven(
rigidbody_dynamics=rigidbody_dynamics,
with_passive_torque=with_passive_torque,
with_ligament=with_ligament,
external_forces=external_forces,
)

if with_contact:
ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven)
ConfigureProblem.configure_contact_function(
ocp,
nlp,
DynamicsFunctions.forces_from_torque_driven,
external_forces=external_forces,
)

ConfigureProblem.configure_soft_contact_function(ocp, nlp)
if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT:
Expand All @@ -486,6 +477,7 @@ def torque_activations_driven(
with_passive_torque: bool = False,
with_residual_torque: bool = False,
with_ligament: bool = False,
external_forces: list[Any] = None,
):
"""
Configure the dynamics for a torque driven program (states are q and qdot, controls are tau activations).
Expand All @@ -506,11 +498,14 @@ def torque_activations_driven(
If the dynamic with a residual torque should be used
with_ligament: bool
If the dynamic with ligament should be used
external_forces: list[Any]
A list of external forces

"""

if with_contact and nlp.model.nb_contacts == 0:
raise ValueError("No contact defined in the .bioMod, set with_contact to False")
_check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
_check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx)
_check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx)

ConfigureProblem.configure_q(ocp, nlp, True, False)
ConfigureProblem.configure_qdot(ocp, nlp, True, False)
Expand All @@ -530,11 +525,12 @@ def torque_activations_driven(
with_passive_torque=with_passive_torque,
with_residual_torque=with_residual_torque,
with_ligament=with_ligament,
external_forces=external_forces,
)

if with_contact:
ConfigureProblem.configure_contact_function(
ocp, nlp, DynamicsFunctions.forces_from_torque_activation_driven
ocp, nlp, DynamicsFunctions.forces_from_torque_activation_driven, external_forces=external_forces
)
ConfigureProblem.configure_soft_contact_function(ocp, nlp)

Expand Down Expand Up @@ -599,6 +595,7 @@ def muscle_driven(
with_passive_torque: bool = False,
with_ligament: bool = False,
rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
external_forces: list[Any] = None,
):
"""
Configure the dynamics for a muscle driven program.
Expand Down Expand Up @@ -627,10 +624,12 @@ def muscle_driven(
If the dynamic with ligament should be used
rigidbody_dynamics: RigidBodyDynamics
which rigidbody dynamics should be used

external_forces: list[Any]
A list of external forces
"""
if with_contact and nlp.model.nb_contacts == 0:
raise ValueError("No contact defined in the .bioMod, set with_contact to False")
_check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
_check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx)
_check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx)

if fatigue is not None and "tau" in fatigue and not with_residual_torque:
raise RuntimeError("Residual torques need to be used to apply fatigue on torques")
Expand Down Expand Up @@ -670,10 +669,13 @@ def muscle_driven(
with_passive_torque=with_passive_torque,
with_ligament=with_ligament,
rigidbody_dynamics=rigidbody_dynamics,
external_forces=external_forces,
)

if with_contact:
ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_muscle_driven)
ConfigureProblem.configure_contact_function(
ocp, nlp, DynamicsFunctions.forces_from_muscle_driven, external_forces=external_forces
)
ConfigureProblem.configure_soft_contact_function(ocp, nlp)

@staticmethod
Expand Down Expand Up @@ -1696,3 +1698,59 @@ def print(self):
Print the DynamicsList to the console
"""
raise NotImplementedError("Printing of DynamicsList is not ready yet")


def _check_external_forces_format(external_forces: list[Any], n_shooting: int, phase_idx: int):
"""Check if the external_forces is of the right format"""
if external_forces is not None and len(external_forces) != n_shooting:
raise RuntimeError(
f"Phase {phase_idx} has {n_shooting} shooting points but the external_forces "
f"has {len(external_forces)} shooting points."
f"The external_forces should be of format list[Any] "
f"where the list is the number of shooting points of the phase and the dict is the Any "
f"is the specific way to add external_force for the specific implementation of the biomodel."
)


def _check_external_forces_and_phase_dynamics(
external_forces: list[Any], phase_dynamics: PhaseDynamics, phase_idx: int
):
"""If external_forces is not None, phase_dynamics should be PhaseDynamics.ONE_PER_NODE"""
# Note to the developer: External_forces doesn't necessitate ONE_PER_NODE, we made it anyway
# because at this stage it makes more sens to send full time series of external forces
# but one day someone could be interested in sending a unique value that could be applied to all nodes
if external_forces is not None and phase_dynamics != PhaseDynamics.ONE_PER_NODE:
raise RuntimeError(
f"Phase {phase_idx} has external_forces but the phase_dynamics is {phase_dynamics}."
f"Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE"
)


def _check_soft_contacts_dynamics(
rigidbody_dynamics: RigidBodyDynamics,
soft_contacts_dynamics: SoftContactDynamics,
nb_soft_contacts,
phase_idx: int,
):
if nb_soft_contacts != 0:
if (
soft_contacts_dynamics != SoftContactDynamics.CONSTRAINT
and soft_contacts_dynamics != SoftContactDynamics.ODE
):
raise ValueError(
f"Phase {phase_idx} has soft contacts but the soft_contacts_dynamics is not "
f"SoftContactDynamics.CONSTRAINT or SoftContactDynamics.ODE."
)

if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS:
if soft_contacts_dynamics == SoftContactDynamics.ODE:
raise ValueError(
f"Phase {phase_idx} has soft contacts but the rigidbody_dynamics is "
f"RigidBodyDynamics.DAE_INVERSE_DYNAMICS and soft_contacts_dynamics is SoftContactDynamics.ODE."
f"Please set soft_contacts_dynamics=SoftContactDynamics.CONSTRAINT"
)


def _check_contacts_in_biorbd_model(with_contact: bool, nb_contacts: int, phase_idx: int):
if with_contact and nb_contacts == 0:
raise ValueError(f"No contact defined in the .bioMod of phase {phase_idx}, set with_contact to False.")