better computing jdotqdot

bionc/bionc_casadi/biomechanical_model.py

@@ -251,66 +251,65 @@ def holonomic_constraints_jacobian(self, Q: NaturalCoordinates) -> MX:
 
         return K
 
-    def holonomic_constraints_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX:
+    def holonomic_constraints_acceleration_bias(self, Qdot: NaturalVelocities) -> MX:
         """
-        This function returns the Jacobian matrix the holonomic constraints, denoted Kdot.
-        They are organized as follow, for each segment, the rows of the matrix are:
-        [Phi_k_0, Phi_r_0, Phi_k_1, Phi_r_1, ..., Phi_k_n, Phi_r_n]
+        Compute the holonomic constraints acceleration bias vector.
+
+        For every holonomic constraint phi(Q) = 0, the acceleration-level form is:
+            K(Q) * Qddot + bias(Qdot) = 0
+        where bias_k = Qdot^T H_k Qdot  (quadratic velocity terms).
+
+        This method assembles the full bias vector by calling:
+          - joint.constraint_acceleration_bias(Qdot_parent, Qdot_child) for each joint
+          - NaturalSegment.rigid_body_constraint_acceleration_bias(Qdoti) for each segment
+
+        The rows are ordered identically to holonomic_constraints_jacobian:
+            [joint constraints for segment 0, rigid body constraints for segment 0,
+             joint constraints for segment 1, rigid body constraints for segment 1, ...]
+
+        In the DAE right-hand side the bias enters with a minus sign:
+            bias_rhs = -holonomic_constraints_acceleration_bias(Qdot)
 
         Parameters
         ----------
         Qdot : NaturalVelocities
-            The natural velocities of the segment [12 * nb_segments, 1]
+            The natural velocities [12 * nb_segments, 1]
 
         Returns
         -------
-            Holonomic constraints jacobian derivative [nb_holonomic_constraints, 12 * nb_segments]
+        MX
+            The acceleration bias vector [nb_holonomic_constraints, 1]
         """
-
-        # first we compute the rigid body constraints jacobian
-        rigid_body_constraints_jacobian_dot = self.rigid_body_constraint_jacobian_derivative(Qdot)
-
-        # then we compute the holonomic constraints jacobian
+        bias = MX.zeros((self.nb_holonomic_constraints, 1))
         nb_constraints = 0
-        Kdot = MX.zeros((self.nb_holonomic_constraints, 12 * self.nb_segments))
+
         for i in range(self.nb_segments):
-            # add the joint constraints first
+            # --- joint constraints (for joints whose child is segment i) ---
             joints = self.joints_from_child_index(i, remove_free_joints=True)
             if len(joints) != 0:
                 for j in joints:
                     idx_row = slice(nb_constraints, nb_constraints + j.nb_constraints)
 
-                    idx_col_child = slice(
-                        12 * self.segments[j.child.name].index, 12 * (self.segments[j.child.name].index + 1)
-                    )
-                    idx_col_parent = (
-                        slice(12 * self.segments[j.parent.name].index, 12 * (self.segments[j.parent.name].index + 1))
-                        if j.parent is not None
-                        else None
-                    )
-
                     Qdot_parent = (
                         None if j.parent is None else Qdot.vector(self.segments[j.parent.name].index)
-                    )  # if the joint is a joint with the ground, the parent is None
+                    )
                     Qdot_child = Qdot.vector(self.segments[j.child.name].index)
 
-                    Kdot[idx_row, idx_col_child] = j.child_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
-
-                    if j.parent is not None:  # If the joint is not a ground joint
-                        Kdot[idx_row, idx_col_parent] = j.parent_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
+                    joint_bias = j.constraint_acceleration_bias(Qdot_parent, Qdot_child)
+                    bias[idx_row] = joint_bias
 
                     nb_constraints += j.nb_constraints
 
-            # add the rigid body constraint
+            # --- rigid body constraints for segment i ---
             idx_row = slice(nb_constraints, nb_constraints + 6)
-            idx_rigid_body_constraint = slice(6 * i, 6 * (i + 1))
-            idx_segment = slice(12 * i, 12 * (i + 1))
+            Qdoti = Qdot.vector(i)
 
-            Kdot[idx_row, idx_segment] = rigid_body_constraints_jacobian_dot[idx_rigid_body_constraint, idx_segment]
+            from .natural_segment import NaturalSegment
+            bias[idx_row] = NaturalSegment.rigid_body_constraint_acceleration_bias(Qdoti)
 
             nb_constraints += 6
 
-        return Kdot
+        return bias
 
     def gravity_forces(self) -> MX:
         """
@@ -373,20 +372,19 @@ def forward_dynamics(
         external_forces = self.external_force_set() if external_forces is None else external_forces
         fext = external_forces.to_natural_external_forces(Q)
         # if stabilization is not None:
-        #     biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[
+        #     bias -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[
         #         "beta"
         #     ] * self.rigid_body_constraint_derivative(Qi, Qdoti)
 
         # augmented system
         # [G, K.T] [Qddot]  = [forces]
-        # [K, 0  ] [lambda] = [biais]
+        # [K, 0  ] [lambda] = [bias]
         augmented_mass_matrix = self.augmented_mass_matrix(Q)
 
         forces = self.gravity_forces() + fext
 
-        Kdot = self.holonomic_constraints_jacobian_derivative(Qdot)
-        biais = -Kdot @ Qdot
-        B = vertcat(forces, biais)
+        bias = -self.holonomic_constraints_acceleration_bias(Qdot)
+        B = vertcat(forces, bias)
 
         # solve the linear system Ax = B with casadi symbolic qr
         x = solve(augmented_mass_matrix, B, "symbolicqr")

bionc/bionc_casadi/biomechanical_model_joints.py

@@ -77,37 +77,4 @@ def constraints_jacobian(self, Q: NaturalCoordinates) -> MX:
 
         return K_k
 
-    def constraints_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX:
-        """
-        This function returns the derivative of the Jacobian matrix of the joint constraints denoted K_k_dot
-
-        Parameters
-        ----------
-        Qdot : NaturalVelocities
-            The natural velocities of the segment [12 * nb_segments, 1]
-
-        Returns
-        -------
-        MX
-            The derivative of the Jacobian matrix of the joint constraints [nb_joint_constraints, 12 * nb_segments]
-        """
-
-        K_k_dot = MX.zeros((self.nb_constraints, Qdot.shape[0]))
-
-        for joint_name, joint in self.joints_with_constraints.items():
-            idx_row = self.constraints_index(joint.index)
-
-            idx_col_child = joint.child.coordinates_slice
-            idx_col_parent = joint.parent.coordinates_slice if joint.parent is not None else None
-
-            Qdot_parent = (
-                None if joint.parent is None else Qdot.vector(joint.parent.index)
-            )  # if the joint is a joint with the ground, the parent is None
-            Qdot_child = Qdot.vector(joint.child.index)
-
-            if joint.parent is not None:  # If the joint is not a ground joint
-                K_k_dot[idx_row, idx_col_parent] = joint.parent_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
-
-            K_k_dot[idx_row, idx_col_child] = joint.child_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
 
-        return K_k_dot

bionc/bionc_casadi/biomechanical_model_segments.py

@@ -78,25 +78,4 @@ def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> MX:
 
         return K_r
 
-    def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX:
-        """
-        This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot
-
-        Parameters
-        ----------
-        Qdot : NaturalVelocities
-            The natural velocities of the segment [12 * nb_segments, 1]
-
-        Returns
-        -------
-        MX
-            The derivative of the Jacobian matrix of the rigid body constraints [6 * nb_segments, 12 * nb_segments]
-        """
-
-        Kr_dot = MX.zeros((6 * self.nb_segments, Qdot.shape[0]))
-        for i, segment in enumerate(self.segments_no_ground.values()):
-            idx_row = slice(6 * i, 6 * (i + 1))
-            idx_col = slice(12 * i, 12 * (i + 1))
-            Kr_dot[idx_row, idx_col] = segment.rigid_body_constraint_jacobian_derivative(Qdot.vector(i))
 
-        return Kr_dot