Merge remote-tracking branch 'pyomeca/master'

bioptim/models/biorbd/holonomic_biorbd_model.py

@@ -2,10 +2,11 @@
 
 import biorbd_casadi as biorbd
 import numpy as np
+import scipy.linalg as la
 from biorbd_casadi import (
     GeneralizedCoordinates,
 )
-from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv
+from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv, nlpsol
 
 from .biorbd_model import BiorbdModel
 from ..holonomic_constraints import HolonomicConstraintsList
@@ -113,11 +114,94 @@ def check_dependant_jacobian(self):
         partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :]
         shape = partitioned_constraints_jacobian_v.shape
         if shape[0] != shape[1]:
+            output = self.partition_coordinates()
             raise ValueError(
                 f"The shape of the dependent joint Jacobian should be square. Got: {shape}."
-                f"Please consider checking the dimension of the holonomic constraints Jacobian."
+                f"Please consider checking the dimension of the holonomic constraints Jacobian.\n"
+                f"Here is a recommended partitioning: "
+                f"      - independent_joint_index: {output[1]},"
+                f"      - dependent_joint_index: {output[0]}."
             )
 
+    def partition_coordinates(self):
+        q = MX.sym("q", self.nb_q, 1)
+        s = nlpsol("sol", "ipopt", {"x": q, "g": self.holonomic_constraints(q)})
+        q_star = np.array(
+            s(
+                x0=np.zeros(self.nb_q),
+                lbg=np.zeros(self.nb_holonomic_constraints),
+                ubg=np.zeros(self.nb_holonomic_constraints),
+            )["x"]
+        )[:, 0]
+        return self.jacobian_coordinate_partitioning(self.holonomic_constraints_jacobian(q_star).toarray())
+
+    @staticmethod
+    def jacobian_coordinate_partitioning(J, tol=None):
+        """
+        Determine a coordinate partitioning q = {q_u, q_v} from a Jacobian J(q) of size (m x n),
+        where m is the number of constraints and
+        n is the total number of generalized coordinates.
+
+        We want to find an invertible submatrix J_v of size (m x m) by reordering
+        the columns of J according to the largest pivots. Those columns in J_v
+        correspond to the 'dependent' coordinates q_v, while the remaining columns
+        correspond to the 'independent' coordinates q_u.
+
+        Parameters
+        ----------
+        J : array_like, shape (m, n)
+            The constraint Jacobian evaluated at the current configuration q.
+        tol : float, optional
+            Tolerance for rank detection. If None, a default based on the machine
+            precision and the size of J is used.
+
+        Returns
+        -------
+        qv_indices : ndarray of shape (r,)
+            The indices of the columns in J chosen as dependent coordinates.
+            Typically, we expect r = m if J has full row rank (i.e. no redundant constraints).
+        qu_indices : ndarray of shape (n - r,)
+            The indices of the columns chosen as independent coordinates.
+        rankJ : int
+            The detected rank of J. If rankJ < m, it means some constraints are redundant.
+
+        Notes
+        -----
+        - If rankJ < m, then there are redundant or degenerate constraints in J.
+          The 'extra' constraints can be ignored in subsequent computations.
+        - If rankJ = m, then J has full row rank and the submatrix J_v is invertible.
+        - After obtaining qv_indices and qu_indices, one typically reorders q
+          so that q = [q_u, q_v], and likewise reorders the columns of J so that
+          J = [J_u, J_v].
+        """
+
+        # J is (m, n): number of constraints = m, number of coords = n.
+        J = np.asarray(J, dtype=float)
+        m, n = J.shape
+
+        # Perform a pivoted QR factorization: J = Q * R[:, pivot]
+        # pivot is a permutation of [0, 1, ..., n-1],
+        # reordering the columns from "largest pivot" to "smallest pivot" in R.
+        Q, R, pivot = la.qr(J, pivoting=True)
+
+        # If no tolerance is specified, pick a default related to the matrix norms and eps
+        if tol is None:
+            # A common heuristic: tol ~ max(m, n) * machine_eps * largest_abs_entry_in_R
+            # The largest absolute entry is often approximated by abs(R[0, 0]) if the matrix
+            # is well-ordered by pivot. However, you can also do np.linalg.norm(R, ord=np.inf).
+            tol = max(m, n) * np.finfo(J.dtype).eps * abs(R[0, 0])
+
+        # Rank detection from the diagonal of R
+        diagR = np.abs(np.diag(R))
+        rankJ = np.sum(diagR > tol)
+
+        # The 'best' columns (by largest pivots) are pivot[:rankJ].
+        # If J is full row rank and not degenerate, we expect rankJ == m.
+        qv_indices = pivot[:rankJ]  # Dependent variables
+        qu_indices = pivot[rankJ:]  # Independent variables
+
+        return qv_indices, qu_indices, rankJ
+
     @property
     def nb_independent_joints(self):
         return len(self._independent_joint_index)
@@ -259,10 +343,52 @@ def partitioned_forward_dynamics(self) -> Function:
         ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219,
         https://doi.org/10.5194/ms-4-199-2013, 2013.
         """
+        q = self.compute_q()(self.q_u, self.q_v_init)
+        qddot_u = self.partitioned_forward_dynamics_full()(q, self.qdot_u, self.tau)
+
+        casadi_fun = Function(
+            "partitioned_forward_dynamics",
+            [self.q_u, self.qdot_u, self.q_v_init, self.tau],
+            [qddot_u],
+            ["q_u", "qdot_u", "q_v_init", "tau"],
+            ["qddot_u"],
+        )
+        return casadi_fun
+
+    def partitioned_forward_dynamics_with_qv(self) -> Function:
+        """
+        Sources
+        -------
+        Docquier, N., Poncelet, A., and Fisette, P.:
+        ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219,
+        https://doi.org/10.5194/ms-4-199-2013, 2013.
+        """
+        q = self.state_from_partition(self.q_u, self.q_v)
+        qddot_u = self.partitioned_forward_dynamics_full()(q, self.qdot_u, self.tau)
+
+        casadi_fun = Function(
+            "partitioned_forward_dynamics",
+            [self.q_u, self.q_v, self.qdot_u, self.tau],
+            [qddot_u],
+            ["q_u", "q_v", "qdot_u", "tau"],
+            ["qddot_u"],
+        )
+
+        return casadi_fun
+
+    def partitioned_forward_dynamics_full(self) -> Function:
+        """
+        Sources
+        -------
+        Docquier, N., Poncelet, A., and Fisette, P.:
+        ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219,
+        https://doi.org/10.5194/ms-4-199-2013, 2013.
+        """
 
         # compute q and qdot
-        q = self.compute_q()(self.q_u, self.q_v_init)
+        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]
@@ -287,7 +413,7 @@ def partitioned_forward_dynamics(self) -> Function:
         modified_non_linear_effect = non_linear_effect_u + coupling_matrix_vu.T @ non_linear_effect_v
 
         # compute the tau
-        partitioned_tau = self.partitioned_tau(self.tau)
+        partitioned_tau = self.partitioned_tau(tau)
         tau_u = partitioned_tau[: self.nb_independent_joints]
         tau_v = partitioned_tau[self.nb_independent_joints :]
 
@@ -299,9 +425,9 @@ def partitioned_forward_dynamics(self) -> Function:
 
         casadi_fun = Function(
             "partitioned_forward_dynamics",
-            [self.q_u, self.qdot_u, self.q_v_init, self.tau],
+            [self.q, self.qdot_u, self.tau],
             [qddot_u],
-            ["q_u", "qdot_u", "q_v_init", "tau"],
+            ["q", "qdot_u", "tau"],
             ["qddot_u"],
         )
 

bioptim/models/protocols/holonomic_biomodel.py

@@ -397,3 +397,55 @@ def compute_the_lagrangian_multipliers(self) -> Function:
         https://doi.org/10.5194/ms-4-199-2013, 2013.
         Equation (17) in the paper.
         """
+
+    def check_state_u_size(self, state_u: MX):
+        """
+        Check if the size of the independent state vector matches the number of independent joints
+
+        Parameters
+        ----------
+        state_u: MX
+            The independent state vector to check
+        """
+
+    def check_state_v_size(self, state_v: MX):
+        """
+        Check if the size of the dependent state vector matches the number of dependent joints
+
+        Parameters
+        ----------
+        state_v: MX
+            The dependent state vector to check
+        """
+
+    def holonomic_forward_dynamics(self) -> Function:
+        """
+        Compute the forward dynamics while respecting holonomic constraints.
+        This combines the regular forward dynamics with constraint forces.
+
+        Returns
+        -------
+        Function
+            The holonomic forward dynamics function
+        """
+
+    def holonomic_inverse_dynamics(self) -> Function:
+        """
+        Compute the inverse dynamics while respecting holonomic constraints.
+        This combines the regular inverse dynamics with constraint forces.
+
+        Returns
+        -------
+        Function
+            The holonomic inverse dynamics function
+        """
+
+    def constraint_forces(self) -> Function:
+        """
+        Compute the forces required to maintain the holonomic constraints.
+
+        Returns
+        -------
+        Function
+            The constraint forces function
+        """

tests/shard1/test_biorbd_model_holonomic.py

@@ -1,11 +1,9 @@
-import os
-
-from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge
-from casadi import DM, MX
 import numpy as np
 import numpy.testing as npt
 import pytest
+from casadi import DM, MX
 
+from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge
 from ..utils import TestUtils
 
 
@@ -96,6 +94,47 @@ def test_model_holonomic():
     q_dot = MX([4, 5, 6])
     q_ddot = MX([7, 8, 9])
     tau = MX([10, 11, 12])
+
+    q_u = MX(TestUtils.to_array(q[model._independent_joint_index]))
+    qdot_u = MX(TestUtils.to_array(q_dot[model._independent_joint_index]))
+    q_v = MX(TestUtils.to_array(q[model._dependent_joint_index]))
+    q_ddot_u = MX(TestUtils.to_array(q_ddot[model._independent_joint_index]))
+
+    # Test partition_coordinates
+    output = model.partition_coordinates()
+    TestUtils.assert_equal(output[0], [0])
+    TestUtils.assert_equal(output[1], [1, 2])
+    TestUtils.assert_equal(output[2], [1])
+
+    # Test partitioned_forward_dynamics_with_qv
+    TestUtils.assert_equal(
+        model.partitioned_forward_dynamics_with_qv()(q_u, q_v[0], qdot_u, tau), [-3.326526], expand=False
+    )
+
+    # Test partitioned_forward_dynamics_full
+    TestUtils.assert_equal(model.partitioned_forward_dynamics_full()(q, qdot_u, tau), [-23.937828], expand=False)
+
+    # Test error message for non-square Jacobian
+    ill_model = HolonomicBiorbdModel(biorbd_model_path)
+    ill_hconstraints = HolonomicConstraintsList()
+    ill_hconstraints.add(
+        "y",
+        HolonomicConstraintsFcn.superimpose_markers,
+        biorbd_model=model,
+        marker_1="marker_1",
+        marker_2="marker_6",
+        index=slice(1, 2),
+    )
+    with pytest.raises(
+        ValueError,
+        match=r"The shape of the dependent joint Jacobian should be square\. Got: \(1, 2\)\."
+        r"Please consider checking the dimension of the holonomic constraints Jacobian\.\n"
+        r"Here is a recommended partitioning: "
+        r"      - independent_joint_index: \[1 2\],"
+        r"      - dependent_joint_index: \[0\]\.",
+    ):
+        ill_model.set_holonomic_configuration(ill_hconstraints, [1, 2], [0])
+
     TestUtils.assert_equal(model.holonomic_constraints(q), [-0.70317549, 0.5104801])
     TestUtils.assert_equal(
         model.holonomic_constraints_jacobian(q),
@@ -123,10 +162,6 @@ def test_model_holonomic():
         [[-0.5104801, 0.02982221, -0.96017029], [-0.70317549, 0.13829549, 0.2794155]],
     )
 
-    q_u = MX(TestUtils.to_array(q[model._independent_joint_index]))
-    qdot_u = MX(TestUtils.to_array(q_dot[model._independent_joint_index]))
-    q_v = MX(TestUtils.to_array(q[model._dependent_joint_index]))
-
     TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -1.101808, expand=False)
     TestUtils.assert_equal(model.coupling_matrix(q), [5.79509793, -0.35166415], expand=False)
     TestUtils.assert_equal(model.biais_vector(q, q_dot), [27.03137348, 23.97095718], expand=False)
@@ -137,6 +172,9 @@ def test_model_holonomic():
     TestUtils.assert_equal(model.compute_qdot_v()(q, qdot_u), [23.18039172, -1.4066566], expand=False)
     TestUtils.assert_equal(model.compute_qdot()(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False)
 
+    TestUtils.assert_equal(model.compute_qddot_v()(q, q_dot, q_ddot_u), [67.597059, 21.509308], expand=False)
+    TestUtils.assert_equal(model.compute_qddot()(q, q_dot, q_ddot_u), [7.0, 67.597059, 21.509308], expand=False)
+
     npt.assert_almost_equal(
         model.compute_q_v()(DM([0.0]), DM([1.0, 1.0])).toarray().squeeze(),
         np.array([2 * np.pi / 3, 2 * np.pi / 3]),