[RTR] contact force at the end of interval for takeoff

[EveCharbie]

All Submissions:

• Have you followed the guidelines in our Contributing document [docs/contribution.md]?
• Have you checked to ensure there aren't other open [Pull Requests] for the same update/change?
• Have you opened/linked the issue related to your pull request?
• Have you used the tag [WIP] for on-going changes, and removed it when the pull request was ready?
• When ready to merge, have you sent a comment pinging @pariterre in it?

New Feature Submissions:

1. Does your submission pass the tests (if not please explain why this is intended)?
2. Did you write a proper documentation (docstrings and ReadMe)
3. Have you linted your code locally prior to submission (using the command: black . -l120 --exclude "external/*")?

Changes to Core Features:

• Have you added an explanation of what your changes do and why you'd like us to include them?
• Have you written new examples for your core changes, as applicable?
• Have you written new tests for your core changes, as applicable?

---

This change is 

[EveCharbie]

@Ipuch could you review the logic please?
@pariterre could you please review this textually, so that @AnaisFarr can use it quickly?

[Ipuch]

Otherwise looks good to me

Reviewed 5 of 5 files at r1, all commit messages.
Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @EveCharbie)

---

bioptim/limits/penalty.py line 839 at r1 (raw file):

                cx = horzcat(*([controller.states.cx_start] + controller.states.cx_intermediates_list))

                states_integrated = controller.integrate(

I think end_interval_statesis more accurate

---

bioptim/limits/penalty.py line 847 at r1 (raw file):

                    d=controller.numerical_timeseries.cx,
                )["xf"]
                penalty.integrate = True

I'm not sure what it is for.

Code quote:

penalty.integrate = True

---

tests/shard4/test_penalty.py line 1172 at r1 (raw file):

            contact_index=0,
            node=Node.PENULTIMATE,
        )

Just to say something:

if penalty_origin == ObjectiveFcn.Mayer:
        penalty_type = ObjectiveFcn.Mayer.MINIMIZE_CONTACT_FORCES_END_OF_INTERVAL
        penalty_object = Objective
    else:
        penalty_type = ConstraintFcn.TRACK_CONTACT_FORCES_END_OF_INTERVAL
        penalty = Constraint

penalty = penalty_object(
            penalty_type,
            contact_index=0,
            node=Node.PENULTIMATE,
        )

Code quote:

    if penalty_origin == ObjectiveFcn.Mayer:
        penalty_type = ObjectiveFcn.Mayer.MINIMIZE_CONTACT_FORCES_END_OF_INTERVAL
        penalty = Objective(
            penalty_type,
            contact_index=0,
            node=Node.PENULTIMATE,
        )
    else:
        penalty_type = ConstraintFcn.TRACK_CONTACT_FORCES_END_OF_INTERVAL
        penalty = Constraint(
            penalty_type,
            contact_index=0,
            node=Node.PENULTIMATE,
        )

[EveCharbie]

Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @Ipuch and @pariterre)

---

bioptim/limits/penalty.py line 839 at r1 (raw file):

Previously, Ipuch (Pierre Puchaud) wrote…

I think end_interval_statesis more accurate

Done.

---

bioptim/limits/penalty.py line 847 at r1 (raw file):

Previously, Ipuch (Pierre Puchaud) wrote…

I'm not sure what it is for.

I removed it since it is a Mayer term, @pariterre need confirmation.

---

tests/shard4/test_penalty.py line 1172 at r1 (raw file):

Previously, Ipuch (Pierre Puchaud) wrote…

Just to say something:

if penalty_origin == ObjectiveFcn.Mayer:
        penalty_type = ObjectiveFcn.Mayer.MINIMIZE_CONTACT_FORCES_END_OF_INTERVAL
        penalty_object = Objective
    else:
        penalty_type = ConstraintFcn.TRACK_CONTACT_FORCES_END_OF_INTERVAL
        penalty = Constraint

penalty = penalty_object(
            penalty_type,
            contact_index=0,
            node=Node.PENULTIMATE,
        )

Done.

[Ipuch]

Reviewed 1 of 2 files at r2, all commit messages.
Reviewable status: 4 of 5 files reviewed, 1 unresolved discussion (waiting on @pariterre)

[Ipuch]

Reviewable status: 4 of 5 files reviewed, 1 unresolved discussion (waiting on @pariterre)

---

bioptim/limits/penalty.py line 847 at r1 (raw file):

Previously, EveCharbie (Eve Charbonneau) wrote…

I removed it since it is a Mayer term, @pariterre need confirmation.

Last comment before @pariterre pops. CodeClimate describe a coginitive complexity of 6 (allowed 5)

Here is my proposition:

            if controller.get_nlp.contact_forces_func is None:
                raise RuntimeError("minimize_contact_forces requires a contact dynamics")

            if controller.control_type != ControlType.CONSTANT:
                raise NotImplementedError(
                    f"This constraint is only useful for ControlType.CONSTANT controls. For any other dynamics, you should constraint the contact at the begining of the interval."
                )

            if controller.algebraic_states.cx_start.shape[0] != 0:
                raise NotImplementedError(
                    "This constraint is not implemented for problems with algebraic states as you should provide their dynamics to integrate it as well."
                )

            PenaltyFunctionAbstract.set_axes_rows(penalty, contact_index)
            penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic

            t_span = controller.t_span.cx
            cx = horzcat(*([controller.states.cx_start] + controller.states.cx_intermediates_list)) if controller.get_nlp.ode_solver.is_direct_collocation else controller.states.cx_start

            end_of_interval_states = controller.integrate(
                    t_span=t_span,
                    x0=cx,
                    u=controller.controls.cx_start,
                    p=controller.parameters.cx,
                    a=controller.algebraic_states.cx_start,
                    d=controller.numerical_timeseries.cx,
                )["xf"]

            contact_force = controller.get_nlp.contact_forces_func(
                controller.time.cx,
                end_of_interval_states,
                controller.controls.cx_start,
                controller.parameters.cx,
                controller.algebraic_states.cx_start,
                controller.numerical_timeseries.cx,
            )

            return contact_force

[EveCharbie]

Reviewable status: 4 of 5 files reviewed, 1 unresolved discussion (waiting on @Ipuch and @pariterre)

---

bioptim/limits/penalty.py line 847 at r1 (raw file):

Previously, Ipuch (Pierre Puchaud) wrote…

Last comment before @pariterre pops. CodeClimate describe a coginitive complexity of 6 (allowed 5)

Here is my proposition:

            if controller.get_nlp.contact_forces_func is None:
                raise RuntimeError("minimize_contact_forces requires a contact dynamics")

            if controller.control_type != ControlType.CONSTANT:
                raise NotImplementedError(
                    f"This constraint is only useful for ControlType.CONSTANT controls. For any other dynamics, you should constraint the contact at the begining of the interval."
                )

            if controller.algebraic_states.cx_start.shape[0] != 0:
                raise NotImplementedError(
                    "This constraint is not implemented for problems with algebraic states as you should provide their dynamics to integrate it as well."
                )

            PenaltyFunctionAbstract.set_axes_rows(penalty, contact_index)
            penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic

            t_span = controller.t_span.cx
            cx = horzcat(*([controller.states.cx_start] + controller.states.cx_intermediates_list)) if controller.get_nlp.ode_solver.is_direct_collocation else controller.states.cx_start

            end_of_interval_states = controller.integrate(
                    t_span=t_span,
                    x0=cx,
                    u=controller.controls.cx_start,
                    p=controller.parameters.cx,
                    a=controller.algebraic_states.cx_start,
                    d=controller.numerical_timeseries.cx,
                )["xf"]

            contact_force = controller.get_nlp.contact_forces_func(
                controller.time.cx,
                end_of_interval_states,
                controller.controls.cx_start,
                controller.parameters.cx,
                controller.algebraic_states.cx_start,
                controller.numerical_timeseries.cx,
            )

            return contact_force

Done.

[Ipuch]

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @EveCharbie)