[RTR] Alignment contraint examples and bug fixes

[p-shg]

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 

[codecov]

Codecov Report

❌ Patch coverage is 55.35714% with 100 lines in your changes missing coverage. Please review.
✅ Project coverage is 76.84%. Comparing base (93f93d0) to head (33f98e4).
⚠️ Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
...s/holonomic_constraints/frame_alignment_example.py	37.68%	43 Missing ⚠️
...onomic_constraints/frame_alignment_example_6DOF.py	66.66%	25 Missing ⚠️
bioptim/models/biorbd/holonomic_biorbd_model.py	8.33%	22 Missing ⚠️
bioptim/models/protocols/holonomic_constraints.py	88.09%	5 Missing ⚠️
bioptim/dynamics/configure_variables.py	70.00%	3 Missing ⚠️
bioptim/limits/penalty.py	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1049      +/-   ##
==========================================
- Coverage   76.99%   76.84%   -0.15%     
==========================================
  Files         192      194       +2     
  Lines       21121    21333     +212     
==========================================
+ Hits        16262    16394     +132     
- Misses       4859     4939      +80     

Flag	Coverage Δ
unittests	76.84% <55.35%> (-0.15%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[Ipuch]

@p-shg let me know when you need a review

[Ipuch]

@Ipuch reviewed 12 files and all commit messages, and made 29 comments.
Reviewable status: all files reviewed, 29 unresolved discussions (waiting on p-shg).

---

bioptim/dynamics/configure_variables.py line 1216 at r1 (raw file):

                    if nlp.control_type == ControlType.CONSTANT
                    else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)
                ),

control_cx = nlp.controls.scaled.cx
                    if nlp.control_type is in (ControlType.CONSTANT, ControlType.CONSTANT_WITH_LAST_NODE)
                    else nlp.cx.sym("linear_continuous_compatible_controls", nlp.controls.scaled.cx.shape[0], 2)
                )

Code quote:

                (
                    nlp.controls.scaled.cx
                    if nlp.control_type == ControlType.CONSTANT
                    else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)
                ),

---

bioptim/dynamics/configure_variables.py line 1228 at r1 (raw file):

                        if "q_v" in nlp.algebraic_states.keys()
                        else DM.zeros(nlp.model.nb_dependent_joints, 1)
                    ),

sym_qv = ...
put it out of the function construction.

Code quote:

                    (
                        nlp.algebraic_states["q_v"].cx
                        if "q_v" in nlp.algebraic_states.keys()
                        else DM.zeros(nlp.model.nb_dependent_joints, 1)
                    ),

---

bioptim/dynamics/configure_variables.py line 1281 at r1 (raw file):

                    if nlp.control_type == ControlType.CONSTANT
                    else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)
                ),

idem

Code quote:

                (
                    nlp.controls.scaled.cx
                    if nlp.control_type == ControlType.CONSTANT
                    else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)
                ),

---

bioptim/dynamics/configure_variables.py line 1294 at r1 (raw file):

                        if "q_v" in nlp.algebraic_states.keys()
                        else DM.zeros(nlp.model.nb_dependent_joints, 1)
                    ),

idem

Code quote:

                    (
                        nlp.algebraic_states["q_v"].cx
                        if "q_v" in nlp.algebraic_states.keys()
                        else DM.zeros(nlp.model.nb_dependent_joints, 1)
                    ),

---

bioptim/dynamics/configure_variables.py line 1338 at r1 (raw file):

        if nlp.control_type == ControlType.LINEAR_CONTINUOUS:
            new_control = nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)

remove duplicate :)

Code quote:

        if nlp.control_type == ControlType.LINEAR_CONTINUOUS:
            new_control = nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2)

---

bioptim/dynamics/configure_variables.py line 1342 at r1 (raw file):

        time_span_sym = vertcat(nlp.time_cx, nlp.dt)

        if nlp.control_type == ControlType.LINEAR_CONTINUOUS:

remove duplicate code by deleting the big if case and declare properly the cx of the controls with its if cases.

Code quote:

if nlp.control_type == ControlType.LINEAR_CONTINUOUS:

---

bioptim/dynamics/configure_variables.py line 1363 at r1 (raw file):

                            if "q_v" in nlp.algebraic_states.keys()
                            else DM.zeros(nlp.model.nb_dependent_joints, 1)
                        ),

out !

Code quote:

                        (
                            nlp.algebraic_states["q_v"].cx
                            if "q_v" in nlp.algebraic_states.keys()
                            else DM.zeros(nlp.model.nb_dependent_joints, 1)
                        ),

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example.py line 6 at r1 (raw file):

"""
Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames” constraint).

small angles ?
the name of the script: frame_aligment_orientation.py

Code quote:

Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames” constraint).

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example.py line 102 at r1 (raw file):

    holonomic_constraints.add(
        "align_cubes",
        HolonomicConstraintsFcn.align_frames,

align_frame_orientation_small_angles

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_6DOF.py line 6 at r1 (raw file):

"""
Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames_generalized” constraint).

attention docstrings

Code quote:

Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames_generalized” constraint).

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_6DOF.py line 51 at r1 (raw file):

# Combine the two interpolations
interpolated_points = np.vstack((interp1, interp2))

def build_dummy_trajectory_for_the_driving_cube(n_shooting: int):
return interpolated_points

Code quote:

# Define the three points (each is a 4D vector)
point1 = np.array([0]).T
point2 = np.array([1]).T
point3 = np.array([0]).T
# Generate interpolation points (0 to 2)
t = np.linspace(0, 2, n_shooting)

# Interpolate between point1 and point2 (first half)
interp1 = point1 + t[: n_shooting // 2, np.newaxis] * (point2 - point1)

# Interpolate between point2 and point3 (second half)
interp2 = point2 + t[: n_shooting // 2, np.newaxis] * (point3 - point2)

# Combine the two interpolations
interpolated_points = np.vstack((interp1, interp2))

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_6DOF.py line 65 at r1 (raw file):

    holonomic_constraints = HolonomicConstraintsList()
    holonomic_constraints.add(
        "holonomic_constraints",

translation_constraint

Code quote:

holonomic_constraints

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_6DOF.py line 74 at r1 (raw file):

    holonomic_constraints.add(
        "align_cubes",

orientation_constraint

Code quote:

align_cubes

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_6DOF.py line 75 at r1 (raw file):

    holonomic_constraints.add(
        "align_cubes",
        HolonomicConstraintsFcn.align_frames_generalized,

align_frames_orientation

Code quote:

  HolonomicConstraintsFcn.align_frames_generalized,

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_relative_rot.py line 8 at r1 (raw file):

constraint on their orientations (the “align_frames” constraint), maintaining a known relative rotation defined by a rotation matrix.

"""

remove

Code quote:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames” constraint), maintaining a known relative rotation defined by a rotation matrix.

"""

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_example_relative_rot_constraint_as_objective.py line 10 at r1 (raw file):

"""

import os

remove

Code quote:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic
constraint on their orientations (the “align_frames” constraint), maintaining a known relative rotation defined by a rotation matrix.

"""

import os

---

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

            if (
                penalty.integration_rule != QuadratureRule.APPROXIMATE_TRAPEZOIDAL
                and penalty.integration_rule != QuadratureRule.TRAPEZOIDAL

is_target_plotable = penalty.integration_rule != QuadratureRule.APPROXIMATE_TRAPEZOIDAL
and penalty.integration_rule != QuadratureRule.TRAPEZOIDAL

Code quote:

                penalty.integration_rule != QuadratureRule.APPROXIMATE_TRAPEZOIDAL
                and penalty.integration_rule != QuadratureRule.TRAPEZOIDAL

---

bioptim/models/biorbd/holonomic_biorbd_model.py line 891 at r1 (raw file):

        )

        return casadi_fun

remove this method

Code quote:

    @cache_function
    def partitioned_forward_dynamics_full_fast(self) -> Function:
        """
        Sources
        -------
        Function
            CasADi Function with signature:
                Inputs: q, qdot_u, tau
                Output: qddot_u (independent coordinate accelerations)

        Notes
        -----
        - Requires q (full coordinates) and qdot_u (independent velocities only)
        - Dependent velocities are computed internally using coupling_matrix
        - The method assumes J_v is invertible (verified during setup)

        See Also
        --------
        partitioned_forward_dynamics : Wrapper that also computes q from q_u
        coupling_matrix : Computes B_vu = -J_v⁻¹ J_u
        biais_vector : Computes b_v = -J_v⁻¹(J̇q̇)
        constrained_forward_dynamics : Full-coordinate formulation with Lagrange multipliers

        References
        ----------
        .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013).
               ROBOTRAN: a powerful symbolic generator of multibody models.
               Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013
        """

        # compute q and qdot
        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 :]

        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
        )
        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 :]

        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 :]

        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
        # )
        qddot_u_red = (
            modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect
        )

        casadi_fun = Function(
            "partitioned_forward_dynamics",
            [self.q, self.qdot_u, self.tau],
            [modified_mass_matrix, qddot_u_red],
            ["q", "qdot_u", "tau"],
            ["qddot_u"],
        )

        return casadi_fun

---

bioptim/models/protocols/holonomic_constraints.py line 195 at r1 (raw file):

        This formulation:
            - Uses exactly 3 scalar equations (minimal representation)
            - Avoids singularities at θ = 0 through Taylor expansion

of theta/sin(theta) Taylor expansion order of 6 and theta using the first-order expansion of arccos such that arcos(3- trace(R)) ~ sqrt(3- trace(R)).

---

bioptim/models/protocols/holonomic_constraints.py line 213 at r1 (raw file):

            The desired 3×3 relative rotation matrix between the frames.
            Default is identity, meaning frames should be perfectly aligned.
            For non-identity values, the constraint enforces R₁ᵀ R₂ = relative_rotation.

not up to date

Code quote:

        relative_rotation : DM, default=DM.eye(3)
            The desired 3×3 relative rotation matrix between the frames.
            Default is identity, meaning frames should be perfectly aligned.
            For non-identity values, the constraint enforces R₁ᵀ R₂ = relative_rotation.

---

bioptim/models/protocols/holonomic_constraints.py line 251 at r1 (raw file):

        -----
        The Taylor expansion used for θ/sin(θ) provides numerical stability near θ = 0:
            θ/sin(θ) ≈ 1 + θ²/6 + 7θ⁴/360 + 31θ⁶/15120

or here to add the arcos taylor expansion

Code quote:

        The Taylor expansion used for θ/sin(θ) provides numerical stability near θ = 0:
            θ/sin(θ) ≈ 1 + θ²/6 + 7θ⁴/360 + 31θ⁶/15120

---

bioptim/models/protocols/holonomic_constraints.py line 300 at r1 (raw file):

        # (any consistent ordering works, we keep the same order used in the
        #  analytical derivation of the constraint in the OP.)
        # Assume small angle assumption is always valid

si ça n'apporte rien à la doctstring then delete.
keep it minimal.

Code quote:

        # Minimal set of scalar constraints (3 equations)
        # The skew‑symmetric part of a proper rotation is zero when the angle is zero:
        #   S = (R_rel - R_relᵀ) / 2  →  S = 0   ⇔   ω = 0, θ = 0
        # We vectorise the three independent components of S:
        #    S_21, S_31, S_32
        # (any consistent ordering works, we keep the same order used in the
        #  analytical derivation of the constraint in the OP.)
        # Assume small angle assumption is always valid

---

bioptim/models/protocols/holonomic_constraints.py line 309 at r1 (raw file):

        constraint = vertcat(S[2, 1], -S[2, 0], S[1, 0])  # r32 - r23  # r13 - r31  # r21 - r12

        # Jacobian and second derivative (CasADi)

mouais remove

Code quote:

# Jacobian and second derivative (CasADi)

---

bioptim/models/protocols/holonomic_constraints.py line 352 at r1 (raw file):

        it uses a smooth transition and switches to avoid singularities

        In some instances this may be more stable and faster to converge

copy past the docstring and modify where relevant.

Code quote:

        This function creates the same alignment constraint as align_frames
        but it implements both the regular case and the small angle case
        it uses a smooth transition and switches to avoid singularities

        In some instances this may be more stable and faster to converge

---

bioptim/models/protocols/holonomic_constraints.py line 387 at r1 (raw file):

        #    S_21, S_31, S_32
        # (any consistent ordering works, we keep the same order used in the
        #  analytical derivation of the constraint in the OP.)

keep it minimal

Code quote:

        # Minimal set of scalar constraints (3 equations)
        # The skew‑symmetric part of a proper rotation is zero when the angle is zero:
        #   S = (R_rel - R_relᵀ) / 2  →  S = 0   ⇔   ω = 0, θ = 0
        # We vectorise the three independent components of S:
        #    S_21, S_31, S_32
        # (any consistent ordering works, we keep the same order used in the
        #  analytical derivation of the constraint in the OP.)

---

bioptim/models/protocols/holonomic_constraints.py line 413 at r1 (raw file):

            fmax(0, (theta - (smooth_transition - transition_epsilon / 2)) / transition_epsilon), 1
        )  # Shift and scale
        s = t * t * (3 - 2 * t)  # Smoothstep

smoothstep in [0,1]

Code quote:

s = t * t * (3 - 2 * t)  # Smoothstep

---

bioptim/models/protocols/holonomic_constraints.py line 422 at r1 (raw file):

        constraint = vertcat(S[2, 1], -S[2, 0], S[1, 0])  # r32 - r23  # r13 - r31  # r21 - r12

        # Jacobian and second derivative (CasADi)

same here

Code quote:

 # Jacobian and second derivative (CasADi)

---

tests/shard1/test_biorbd_model_holonomic.py line 1296 at r1 (raw file):

    )

    # --- Solve the ocp --- #

q, _, _, _ = frame_aligment_example.compute_all_states(sol, model, ...)

---

tests/shard1/test_biorbd_model_holonomic.py line 1326 at r1 (raw file):

    npt.assert_almost_equal(
        states["q_u"],

test q