Repository: sympy/sympy

Issue:
KanesMethod solves kinematics explicitly even if not asked
While dynamic equations are left in explicit form `M [u].diff(t) = f(q, u)`, the kinematic equations are solved explicitly [here](https://github.com/sympy/sympy/blob/0e4bab73562d6f90fcdf5fa079731f26a455347c/sympy/physics/mechanics/kane.py#L259):
```
            f_k = k_kqdot.LUsolve(f_k)
            k_ku = k_kqdot.LUsolve(k_ku)
            k_kqdot = eye(len(qdot))
```

The end result is that in `mass_matrix_full` the columns associated with kinematics are just the identity. This works fine most of the time, but can be unnecessarily slow if an explicit form of the equations is not desired, not to mention that it requires simplification of the explicit right hand side.

Would it make sense to avoid this step unless explicitly requested by the initialization function?

Happy to submit a PR if others here suggest how to best control that behavior.


Discussion:
sympy-bot: :white_check_mark:

Hi, I am the [SymPy bot](https://github.com/sympy/sympy-bot) (v167). I'm here to help you write a release notes entry. Please read the [guide on how to write release notes](https://github.com/sympy/sympy/wiki/Writing-Release-Notes).



Your release notes are in good order.

Here is what the release notes will look like:
* physics.mechanics
  * Added option to return kinematics and full mass matrix in implicit form in KanesMethod ([#23580](https://github.com/sympy/sympy/pull/23580) by [@zouhairm](https://github.com/zouhairm))

This will be added to https://github.com/sympy/sympy/wiki/Release-Notes-for-1.11.

<details><summary>Click here to see the pull request description that was parsed.</summary>

    Fixes #22626 

    This adds an option to KanesMethod to expose kinematic matrices in implicit form.

    Default behavior should remain unchanged, so existing tests should be fine. Added one test to exercise the new code.

    Closing PR https://github.com/sympy/sympy/pull/22818 to take a different approach as proposed by @moorepants in https://github.com/sympy/sympy/pull/22818#issuecomment-1086811861 . Doing this in a different PR in case we decide to pursue the other method at some point (and because I have some 3rd party code relying on that branch right now which I'll migrate to this PR once it's approved)

    <!-- BEGIN RELEASE NOTES -->
    * physics.mechanics
      * Added option to return kinematics and full mass matrix in implicit form in KanesMethod

    <!-- END RELEASE NOTES -->


</details><p>

zouhairm: @moorepants - plz let me know if this along the lines of what you're looking for.
github-actions[bot]: Benchmark results from GitHub Actions

Lower numbers are good, higher numbers are bad. A ratio less than 1
means a speed up and greater than 1 means a slowdown. Green lines
beginning with `+` are slowdowns (the PR is slower then master or
master is slower than the previous release). Red lines beginning
with `-` are speedups.

Significantly changed benchmark results (PR vs master)
```diff

```
Significantly changed benchmark results (master vs previous release)
```diff
       before           after         ratio
     [77f1d79c]       [7f72eb9b]
     <sympy-1.10.1^0>                 
+         112±2ms          211±3ms     1.88  sum.TimeSum.time_doit

```
Full benchmark results can be found as artifacts in GitHub Actions
(click on checks at the top of the PR).
moorepants: @TJStienstra would you like to review this? It would be a good practice for your GSoC.
TJStienstra: Yeah fine, thanks for showing. Will have a lot more time this week, so will look into it somewhere in the coming days (probably tomorrow).
zouhairm: thanks for the comments @TJStienstra - will try to address them this week.
zouhairm: @TJStienstra - sorry for the much longer than expected delay as some higher priorities ended up taking over.

I believe I've addressed all of your comments. I'm hoping we can get this merged as it'd be great to get our internal codebase updated to use a 'built wheel' from sympy as opposed to having to build it from scratch from my branch.

Thanks!
TJStienstra: Nice work, like the tests and I agree it would be nice to have this in the code base as soon as possible. Overall there are some small typos, but will be addressing a more general refactoring of `KanesMethod` in #24093.

However just discussed the PR with @moorepants and @brocksam and came with the following proposal to reduce the current polutting (~100 lines of code for these properties and 3 extra external methods and 3 protected methods). By implementing an attribute `explicit`, which is defaultly set to `True` we can drastically reduce it. Here is some example code:

```py
kanes = KanesMethod(...)
kanes.kanes_equations()
kanes.forcing_kin  # Explicit kinematic forcing vector
kanes.mass_matrix_full # Explicit full mass matrix
kanes.explicit = False
kanes.forcing_kin  # Implicit kinematic forcing vector
```

@zouhairm What do you think of this proposal (it is in the end a bit of what you've started with)?

Will be giving the entire suggestion, that you can check and commit, in a few minutes.
zouhairm: The suggestion sounds reasonable to me (In fact that was the initial [implementation](https://github.com/sympy/sympy/pull/22818#discussion_r836788688) but we moved away from it)

--
Thanks for taking the time to type up the suggestions, unfortunately, github is not allowing me to commit them (due to this issue https://github.com/isaacs/github/issues/1755)

I can manually type them up, but if you happen to have this in branch already, can you either submit a PR to my fork, or perhaps post a patch file?

TJStienstra: Almost have a patch file if you'd like
TJStienstra: [explicit_attribute.zip](https://github.com/sympy/sympy/files/9677135/explicit_attribute.zip)

TJStienstra: Using `explicit_kinematics` as attribute is indeed more clear.
moorepants: > The suggestion sounds reasonable to me (In fact that was the initial https://github.com/sympy/sympy/pull/22818#discussion_r836788688 but we moved away from it)

Sorry to run you in circles. That comment was still tied up with the 2 parts, so maybe not as relevant now. I also made a comment further down about the flag belonging in `.kanes_equations()`. Having a flag and no new attributes does seem cleaner at this point.
zouhairm: Not sure why `test/sphinx` is failing - seems to be unrelated to my changes as it's failing at the installation step:
```
Err:38 http://azure.archive.ubuntu.com/ubuntu focal-updates/main amd64 libgs9-common all 9.50~dfsg-5ubuntu4.5
  404  Not Found [IP: 52.252.75.106 80]
Err:42 http://azure.archive.ubuntu.com/ubuntu focal-updates/main amd64 libgs9 amd64 9.50~dfsg-5ubuntu4.5
  404  Not Found [IP: 52.252.75.106 80]
Err:43 http://azure.archive.ubuntu.com/ubuntu focal-updates/main amd64 ghostscript amd64 9.50~dfsg-5ubuntu4.5
  404  Not Found [IP: 52.252.75.106 80]
Fetched 665 MB in 15s (43.5 MB/s)
E: Failed to fetch http://azure.archive.ubuntu.com/ubuntu/pool/main/g/ghostscript/libgs9-common_9.50~dfsg-5ubuntu4.5_all.deb  404  Not Found [IP: 52.252.75.106 80]
E: Failed to fetch http://azure.archive.ubuntu.com/ubuntu/pool/main/g/ghostscript/libgs9_9.50~dfsg-5ubuntu4.5_amd64.deb  404  Not Found [IP: 52.252.75.106 80]
E: Failed to fetch http://azure.archive.ubuntu.com/ubuntu/pool/main/g/ghostscript/ghostscript_9.50~dfsg-5ubuntu4.5_amd64.deb  404  Not Found [IP: 52.252.75.106 80]
E: Unable to fetch some archives, maybe run apt-get update or try with --fix-missing?
Error: Process completed with exit code 100.
```
TJStienstra: The problem should be fixed in the master now

Patch:
diff --git a/sympy/physics/mechanics/kane.py b/sympy/physics/mechanics/kane.py
index 76819b6f18..7d1a6d6d0e 100644
--- a/sympy/physics/mechanics/kane.py
+++ b/sympy/physics/mechanics/kane.py
@@ -14,7 +14,7 @@
 
 
 class KanesMethod(_Methods):
-    """Kane's method object.
+    r"""Kane's method object.
 
     Explanation
     ===========
@@ -35,18 +35,37 @@ class KanesMethod(_Methods):
     loads : iterable
         Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
         describing the forces on the system.
-    auxiliary : Matrix
+    auxiliary_eqs : Matrix
         If applicable, the set of auxiliary Kane's
         equations used to solve for non-contributing
         forces.
     mass_matrix : Matrix
-        The system's mass matrix
+        The system's dynamics mass matrix: [k_d; k_dnh]
     forcing : Matrix
-        The system's forcing vector
+        The system's dynamics forcing vector: -[f_d; f_dnh]
+    mass_matrix_kin : Matrix
+        The "mass matrix" for kinematic differential equations: k_kqdot
+    forcing_kin : Matrix
+        The forcing vector for kinematic differential equations: -(k_ku*u + f_k)
     mass_matrix_full : Matrix
-        The "mass matrix" for the u's and q's
+        The "mass matrix" for the u's and q's with dynamics and kinematics
     forcing_full : Matrix
-        The "forcing vector" for the u's and q's
+        The "forcing vector" for the u's and q's with dynamics and kinematics
+    explicit_kinematics : bool
+        Boolean whether the mass matrices and forcing vectors should use the
+        explicit form (default) or implicit form for kinematics.
+        See the notes for more details.
+
+    Notes
+    =====
+
+    The mass matrices and forcing vectors related to kinematic equations
+    are given in the explicit form by default. In other words, the kinematic
+    mass matrix is $\mathbf{k_{k\dot{q}}} = \mathbf{I}$.
+    In order to get the implicit form of those matrices/vectors, you can set the
+    ``explicit_kinematics`` attribute to ``False``. So $\mathbf{k_{k\dot{q}}}$ is not
+    necessarily an identity matrix. This can provide more compact equations for
+    non-simple kinematics (see #22626).
 
     Examples
     ========
@@ -117,7 +136,7 @@ class KanesMethod(_Methods):
     def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None,
             configuration_constraints=None, u_dependent=None,
             velocity_constraints=None, acceleration_constraints=None,
-            u_auxiliary=None, bodies=None, forcelist=None):
+            u_auxiliary=None, bodies=None, forcelist=None, explicit_kinematics=True):
 
         """Please read the online documentation. """
         if not q_ind:
@@ -134,6 +153,8 @@ def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None,
         self._forcelist = forcelist
         self._bodylist = bodies
 
+        self.explicit_kinematics = explicit_kinematics
+
         self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent,
                 u_auxiliary)
         self._initialize_kindiffeq_matrices(kd_eqs)
@@ -281,6 +302,10 @@ def _initialize_kindiffeq_matrices(self, kdeqs):
                        'coordinates.')
                 raise ValueError(msg.format(nonlin_vars))
 
+            self._f_k_implicit = f_k.xreplace(uaux_zero)
+            self._k_ku_implicit = k_ku.xreplace(uaux_zero)
+            self._k_kqdot_implicit = k_kqdot
+
             # Solve for q'(t) such that the coefficient matrices are now in
             # this form:
             #
@@ -289,19 +314,19 @@ def _initialize_kindiffeq_matrices(self, kdeqs):
             # NOTE : Solving the kinematic differential equations here is not
             # necessary and prevents the equations from being provided in fully
             # implicit form.
-            f_k = k_kqdot.LUsolve(f_k)
-            k_ku = k_kqdot.LUsolve(k_ku)
-            k_kqdot = eye(len(qdot))
-            self._qdot_u_map = dict(zip(qdot, -(k_ku*u + f_k)))
-
-            self._f_k = f_k.xreplace(uaux_zero)
-            self._k_ku = k_ku.xreplace(uaux_zero)
-            self._k_kqdot = k_kqdot
+            f_k_explicit = k_kqdot.LUsolve(f_k)
+            k_ku_explicit = k_kqdot.LUsolve(k_ku)
+            self._qdot_u_map = dict(zip(qdot, -(k_ku_explicit*u + f_k_explicit)))
+
+            self._f_k = f_k_explicit.xreplace(uaux_zero)
+            self._k_ku = k_ku_explicit.xreplace(uaux_zero)
+            self._k_kqdot = eye(len(qdot))
+
         else:
             self._qdot_u_map = None
-            self._f_k = Matrix()
-            self._k_ku = Matrix()
-            self._k_kqdot = Matrix()
+            self._f_k_implicit = self._f_k = Matrix()
+            self._k_ku_implicit = self._k_ku = Matrix()
+            self._k_kqdot_implicit = self._k_kqdot = Matrix()
 
     def _form_fr(self, fl):
         """Form the generalized active force."""
@@ -548,7 +573,7 @@ def kanes_equations(self, bodies=None, loads=None):
         constraints) the length of the returned vectors will be o - m + s in
         length. The first o - m equations will be the constrained Kane's
         equations, then the s auxiliary Kane's equations. These auxiliary
-        equations can be accessed with the auxiliary_eqs().
+        equations can be accessed with the auxiliary_eqs property.
 
         Parameters
         ==========
@@ -581,7 +606,10 @@ def kanes_equations(self, bodies=None, loads=None):
                 km = KanesMethod(self._inertial, self.q, self._uaux,
                         u_auxiliary=self._uaux, u_dependent=self._udep,
                         velocity_constraints=(self._k_nh * self.u +
-                        self._f_nh))
+                        self._f_nh),
+                        acceleration_constraints=(self._k_dnh * self._udot +
+                        self._f_dnh)
+                        )
             km._qdot_u_map = self._qdot_u_map
             self._km = km
             fraux = km._form_fr(loads)
@@ -644,6 +672,19 @@ def auxiliary_eqs(self):
             raise ValueError('No auxiliary speeds have been declared.')
         return self._aux_eq
 
+    @property
+    def mass_matrix_kin(self):
+        r"""The kinematic "mass matrix" $\mathbf{k_{k\dot{q}}}$ of the system."""
+        return self._k_kqdot if self.explicit_kinematics else self._k_kqdot_implicit
+
+    @property
+    def forcing_kin(self):
+        """The kinematic "forcing vector" of the system."""
+        if self.explicit_kinematics:
+            return -(self._k_ku * Matrix(self.u) + self._f_k)
+        else:
+            return -(self._k_ku_implicit * Matrix(self.u) + self._f_k_implicit)
+
     @property
     def mass_matrix(self):
         """The mass matrix of the system."""
@@ -652,31 +693,27 @@ def mass_matrix(self):
         return Matrix([self._k_d, self._k_dnh])
 
     @property
-    def mass_matrix_full(self):
-        """The mass matrix of the system, augmented by the kinematic
-        differential equations."""
+    def forcing(self):
+        """The forcing vector of the system."""
         if not self._fr or not self._frstar:
             raise ValueError('Need to compute Fr, Fr* first.')
-        o = len(self.u)
-        n = len(self.q)
-        return ((self._k_kqdot).row_join(zeros(n, o))).col_join((zeros(o,
-                n)).row_join(self.mass_matrix))
+        return -Matrix([self._f_d, self._f_dnh])
 
     @property
-    def forcing(self):
-        """The forcing vector of the system."""
+    def mass_matrix_full(self):
+        """The mass matrix of the system, augmented by the kinematic
+        differential equations in explicit or implicit form."""
         if not self._fr or not self._frstar:
             raise ValueError('Need to compute Fr, Fr* first.')
-        return -Matrix([self._f_d, self._f_dnh])
+        o, n = len(self.u), len(self.q)
+        return (self.mass_matrix_kin.row_join(zeros(n, o))).col_join(
+            zeros(o, n).row_join(self.mass_matrix))
 
     @property
     def forcing_full(self):
         """The forcing vector of the system, augmented by the kinematic
-        differential equations."""
-        if not self._fr or not self._frstar:
-            raise ValueError('Need to compute Fr, Fr* first.')
-        f1 = self._k_ku * Matrix(self.u) + self._f_k
-        return -Matrix([f1, self._f_d, self._f_dnh])
+        differential equations in explicit or implicit form."""
+        return Matrix([self.forcing_kin, self.forcing])
 
     @property
     def q(self):


Test patch:
diff --git a/sympy/physics/mechanics/tests/test_kane.py b/sympy/physics/mechanics/tests/test_kane.py
index ac4d57ace6..1dc11be14f 100644
--- a/sympy/physics/mechanics/tests/test_kane.py
+++ b/sympy/physics/mechanics/tests/test_kane.py
@@ -1,10 +1,12 @@
+from sympy import solve
 from sympy.core.backend import (cos, expand, Matrix, sin, symbols, tan, sqrt, S,
-                                zeros)
+                                zeros, eye)
 from sympy.simplify.simplify import simplify
 from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
                                      RigidBody, KanesMethod, inertia, Particle,
                                      dot)
 from sympy.testing.pytest import raises
+from sympy.core.backend import USE_SYMENGINE
 
 
 def test_one_dof():
@@ -52,7 +54,9 @@ def test_two_dof():
     P2 = Point('P2')
     P1.set_vel(N, u1 * N.x)
     P2.set_vel(N, (u1 + u2) * N.x)
-    kd = [q1d - u1, q2d - u2]
+    # Note we multiply the kinematic equation by an arbitrary factor
+    # to test the implicit vs explicit kinematics attribute
+    kd = [q1d/2 - u1/2, 2*q2d - 2*u2]
 
     # Now we create the list of forces, then assign properties to each
     # particle, then create a list of all particles.
@@ -74,8 +78,20 @@ def test_two_dof():
     assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
                                     c2 * u2) / m)
 
-    assert simplify(KM.rhs() -
-                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
+    # Check that the explicit form is the default and kinematic mass matrix is identity
+    assert KM.explicit_kinematics
+    assert KM.mass_matrix_kin == eye(2)
+
+    # Check that for the implicit form the mass matrix is not identity
+    KM.explicit_kinematics = False
+    assert KM.mass_matrix_kin == Matrix([[S(1)/2, 0], [0, 2]])
+
+    # Check that whether using implicit or explicit kinematics the RHS
+    # equations are consisten with the matrix form
+    for explicit_kinematics in [False, True]:
+        KM.explicit_kinematics = explicit_kinematics
+        assert simplify(KM.rhs() -
+                        KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
 
     # Make sure an error is raised if nonlinear kinematic differential
     # equations are supplied.
@@ -348,3 +364,155 @@ def test_input_format():
     assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
     assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
                                     c2 * u2) / m)
+
+
+def test_implicit_kinematics():
+    # Test that implicit kinematics can handle complicated
+    # equations that explicit form struggles with
+    # See https://github.com/sympy/sympy/issues/22626
+
+    # Inertial frame
+    NED = ReferenceFrame('NED')
+    NED_o = Point('NED_o')
+    NED_o.set_vel(NED, 0)
+
+    # body frame
+    q_att = dynamicsymbols('lambda_0:4', real=True)
+    B = NED.orientnew('B', 'Quaternion', q_att)
+
+    # Generalized coordinates
+    q_pos = dynamicsymbols('B_x:z')
+    B_cm = NED_o.locatenew('B_cm', q_pos[0]*B.x + q_pos[1]*B.y + q_pos[2]*B.z)
+
+    q_ind = q_att[1:] + q_pos
+    q_dep = [q_att[0]]
+
+    kinematic_eqs = []
+
+    # Generalized velocities
+    B_ang_vel = B.ang_vel_in(NED)
+    P, Q, R = dynamicsymbols('P Q R')
+    B.set_ang_vel(NED, P*B.x + Q*B.y + R*B.z)
+
+    B_ang_vel_kd = (B.ang_vel_in(NED) - B_ang_vel).simplify()
+
+    # Equating the two gives us the kinematic equation
+    kinematic_eqs += [
+        B_ang_vel_kd & B.x,
+        B_ang_vel_kd & B.y,
+        B_ang_vel_kd & B.z
+    ]
+
+    B_cm_vel = B_cm.vel(NED)
+    U, V, W = dynamicsymbols('U V W')
+    B_cm.set_vel(NED, U*B.x + V*B.y + W*B.z)
+
+    # Compute the velocity of the point using the two methods
+    B_ref_vel_kd = (B_cm.vel(NED) - B_cm_vel)
+
+    # taking dot product with unit vectors to get kinematic equations
+    # relating body coordinates and velocities
+
+    # Note, there is a choice to dot with NED.xyz here. That makes
+    # the implicit form have some bigger terms but is still fine, the
+    # explicit form still struggles though
+    kinematic_eqs += [
+                      B_ref_vel_kd & B.x,
+                      B_ref_vel_kd & B.y,
+                      B_ref_vel_kd & B.z,
+                     ]
+
+    u_ind = [U, V, W, P, Q, R]
+
+    # constraints
+    q_att_vec = Matrix(q_att)
+    config_cons = [(q_att_vec.T*q_att_vec)[0] - 1] #unit norm
+    kinematic_eqs = kinematic_eqs + [(q_att_vec.T * q_att_vec.diff())[0]]
+
+    try:
+        KM = KanesMethod(NED, q_ind, u_ind,
+          q_dependent= q_dep,
+          kd_eqs = kinematic_eqs,
+          configuration_constraints = config_cons,
+          velocity_constraints= [],
+          u_dependent= [], #no dependent speeds
+          u_auxiliary = [], # No auxiliary speeds
+          explicit_kinematics = False # implicit kinematics
+        )
+    except Exception as e:
+        # symengine struggles with these kinematic equations
+        if USE_SYMENGINE and 'Matrix is rank deficient' in str(e):
+            return
+        else:
+            raise e
+
+    # mass and inertia dyadic relative to CM
+    M_B = symbols('M_B')
+    J_B = inertia(B, *[S(f'J_B_{ax}')*(1 if ax[0] == ax[1] else -1)
+            for ax in ['xx', 'yy', 'zz', 'xy', 'yz', 'xz']])
+    J_B = J_B.subs({S('J_B_xy'): 0, S('J_B_yz'): 0})
+    RB = RigidBody('RB', B_cm, B, M_B, (J_B, B_cm))
+
+    rigid_bodies = [RB]
+    # Forces
+    force_list = [
+        #gravity pointing down
+        (RB.masscenter, RB.mass*S('g')*NED.z),
+        #generic forces and torques in body frame(inputs)
+        (RB.frame, dynamicsymbols('T_z')*B.z),
+        (RB.masscenter, dynamicsymbols('F_z')*B.z)
+    ]
+
+    KM.kanes_equations(rigid_bodies, force_list)
+
+    # Expecting implicit form to be less than 5% of the flops
+    n_ops_implicit = sum(
+        [x.count_ops() for x in KM.forcing_full] +
+        [x.count_ops() for x in KM.mass_matrix_full]
+    )
+    # Save implicit kinematic matrices to use later
+    mass_matrix_kin_implicit = KM.mass_matrix_kin
+    forcing_kin_implicit = KM.forcing_kin
+
+    KM.explicit_kinematics = True
+    n_ops_explicit = sum(
+        [x.count_ops() for x in KM.forcing_full] +
+        [x.count_ops() for x in KM.mass_matrix_full]
+    )
+    forcing_kin_explicit = KM.forcing_kin
+
+    assert n_ops_implicit / n_ops_explicit < .05
+
+    # Ideally we would check that implicit and explicit equations give the same result as done in test_one_dof
+    # But the whole raison-d'etre of the implicit equations is to deal with problems such
+    # as this one where the explicit form is too complicated to handle, especially the angular part
+    # (i.e. tests would be too slow)
+    # Instead, we check that the kinematic equations are correct using more fundamental tests:
+    #
+    # (1) that we recover the kinematic equations we have provided
+    assert (mass_matrix_kin_implicit * KM.q.diff() - forcing_kin_implicit) == Matrix(kinematic_eqs)
+
+    # (2) that rate of quaternions matches what 'textbook' solutions give
+    # Note that we just use the explicit kinematics for the linear velocities
+    # as they are not as complicated as the angular ones
+    qdot_candidate = forcing_kin_explicit
+
+    quat_dot_textbook = Matrix([
+        [0, -P, -Q, -R],
+        [P,  0,  R, -Q],
+        [Q, -R,  0,  P],
+        [R,  Q, -P,  0],
+    ]) * q_att_vec / 2
+
+    # Again, if we don't use this "textbook" solution
+    # sympy will struggle to deal with the terms related to quaternion rates
+    # due to the number of operations involved
+    qdot_candidate[-1] = quat_dot_textbook[0] # lambda_0, note the [-1] as sympy's Kane puts the dependent coordinate last
+    qdot_candidate[0]  = quat_dot_textbook[1] # lambda_1
+    qdot_candidate[1]  = quat_dot_textbook[2] # lambda_2
+    qdot_candidate[2]  = quat_dot_textbook[3] # lambda_3
+
+    # sub the config constraint in the candidate solution and compare to the implicit rhs
+    lambda_0_sol = solve(config_cons[0], q_att_vec[0])[1]
+    lhs_candidate = simplify(mass_matrix_kin_implicit * qdot_candidate).subs({q_att_vec[0]: lambda_0_sol})
+    assert lhs_candidate == forcing_kin_implicit