[WIP] Implementing a contraint to keep frames aligned

[p-shg]

Constraint implementation seems sound and first dummy example seems to work TODO make dummy example to verify that torques and forces can be transferred

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?

Constraint formulation is implemented and seems to work ok. @Ipuch please have a look and see if this makes sense
First example implemented, constraint seems to be applied
TODO: Implement an example where forces and torques need to be transferred through the holonomic constraint to verify that it works
TODO add tests for both those examples

---

This change is 

[codecov]

Codecov Report

❌ Patch coverage is 7.50000% with 111 lines in your changes missing coverage. Please review.
✅ Project coverage is 76.76%. Comparing base (e0f5ee5) to head (5e48e06).
⚠️ Report is 13 commits behind head on master.

Files with missing lines	Patch %	Lines
bioptim/models/protocols/holonomic_constraints.py	7.50%	111 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1042      +/-   ##
==========================================
- Coverage   77.15%   76.76%   -0.40%     
==========================================
  Files         193      193              
  Lines       21019    21137     +118     
==========================================
+ Hits        16218    16225       +7     
- Misses       4801     4912     +111     

Flag	Coverage Δ
unittests	76.76% <7.50%> (-0.40%)	⬇️

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.

[pariterre]

@pariterre reviewed 14 files and all commit messages, and made 11 comments.
Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @p-shg).

---

bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_lagrange_2d.py line 266 at r1 (raw file):

if __name__ == "__main__":
    main()

Please add this example to the GUI (bioptim/examples/main.py) and to the test folder

---

bioptim/examples/toy_examples/holonomic_constraints/test_frame_alignment_2constraints.py line 215 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

---

bioptim/examples/toy_examples/holonomic_constraints/test_frame_alignment_lagrange_2d copy.py line 269 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

Remove the "copy" in the file name and if it is just a copy of another file, please remove

---

bioptim/examples/toy_examples/holonomic_constraints/test_translation_laplace_1d.py line 195 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

---

bioptim/examples/toy_examples/holonomic_constraints/test_frame_alignment_constraint.py line 271 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

---

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

        # return -partitioned_constraints_jacobian_v_inv @ self._holonomic_constraints_jacobian_time_derivative_finite_difference(q, qdot) @ qdot
        # return -partitioned_constraints_jacobian_v_inv @ self._holonomic_constraints_jacobian_time_derivative(q, qdot) @ qdot
        return -partitioned_constraints_jacobian_v_inv @ self.holonomic_constraints_double_derivative(q, qdot, MX())

When the right code is confirmed, please remove these comments

---

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

        #     Jdot_rows.append(Jdot_row.T)  # Transposer pour obtenir (1, 5)
        #
        # Jdot = vertcat(*Jdot_rows)  # (2, 5)

When confirmed, please remove these comments

---

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

        """
        Generate the holonomic constraint that forces the orientation of two
        frames to be identical (i.e. the relative rotation between them is the

It seems that "relative_rotation" can be something else than identity as it seems the user are allowed to send values? If so, please update the comments accordingly.

Also, for ease of use, maybe the users could be allowed to send Euler angles? (or one or the other?)

---

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

        # Global homogeneous matrices (4×4) of the two frames
        # T1_glob = model.homogeneous_matrices_in_global(segment_index=frame_1_idx)(q_sym, parameters)  # shape (4,4)
        # T2_glob = model.homogeneous_matrices_in_global(segment_index=frame_2_idx)(q_sym, parameters)  # shape (4,4)

Are these two lines docs or previous codes?

---

bioptim/examples/toy_examples/holonomic_constraints/two_cubes_with_corrected_dofs.py line 214 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

---

bioptim/examples/toy_examples/holonomic_constraints/test_frame_alignment_constraint_asobjective.py line 173 at r1 (raw file):

if __name__ == "__main__":
    main()

Why is this file named "test_..."? If this is a test, please move to the "tests" folder, it can be put an underused shard (5 or 6?). If this is an example, please add to the GUI (bioptim/examples/main.py) and to the test folder

Please and a "_" between "as" and "objective" in the file name