Add files using upload-large-folder tool

lib/python3.12/site-packages/sympy/physics/mechanics/functions.py

@@ -0,0 +1,735 @@
+from sympy.utilities import dict_merge
+from sympy.utilities.iterables import iterable
+from sympy.physics.vector import (Dyadic, Vector, ReferenceFrame,
+                                  Point, dynamicsymbols)
+from sympy.physics.vector.printing import (vprint, vsprint, vpprint, vlatex,
+                                           init_vprinting)
+from sympy.physics.mechanics.particle import Particle
+from sympy.physics.mechanics.rigidbody import RigidBody
+from sympy.simplify.simplify import simplify
+from sympy import Matrix, Mul, Derivative, sin, cos, tan, S
+from sympy.core.function import AppliedUndef
+from sympy.physics.mechanics.inertia import (inertia as _inertia,
+    inertia_of_point_mass as _inertia_of_point_mass)
+from sympy.utilities.exceptions import sympy_deprecation_warning
+
+__all__ = ['linear_momentum',
+           'angular_momentum',
+           'kinetic_energy',
+           'potential_energy',
+           'Lagrangian',
+           'mechanics_printing',
+           'mprint',
+           'msprint',
+           'mpprint',
+           'mlatex',
+           'msubs',
+           'find_dynamicsymbols']
+
+# These are functions that we've moved and renamed during extracting the
+# basic vector calculus code from the mechanics packages.
+
+mprint = vprint
+msprint = vsprint
+mpprint = vpprint
+mlatex = vlatex
+
+
+def mechanics_printing(**kwargs):
+    """
+    Initializes time derivative printing for all SymPy objects in
+    mechanics module.
+    """
+
+    init_vprinting(**kwargs)
+
+mechanics_printing.__doc__ = init_vprinting.__doc__
+
+
+def inertia(frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0):
+    sympy_deprecation_warning(
+        """
+        The inertia function has been moved.
+        Import it from "sympy.physics.mechanics".
+        """,
+        deprecated_since_version="1.13",
+        active_deprecations_target="moved-mechanics-functions"
+    )
+    return _inertia(frame, ixx, iyy, izz, ixy, iyz, izx)
+
+
+def inertia_of_point_mass(mass, pos_vec, frame):
+    sympy_deprecation_warning(
+        """
+        The inertia_of_point_mass function has been moved.
+        Import it from "sympy.physics.mechanics".
+        """,
+        deprecated_since_version="1.13",
+        active_deprecations_target="moved-mechanics-functions"
+    )
+    return _inertia_of_point_mass(mass, pos_vec, frame)
+
+
+def linear_momentum(frame, *body):
+    """Linear momentum of the system.
+
+    Explanation
+    ===========
+
+    This function returns the linear momentum of a system of Particle's and/or
+    RigidBody's. The linear momentum of a system is equal to the vector sum of
+    the linear momentum of its constituents. Consider a system, S, comprised of
+    a rigid body, A, and a particle, P. The linear momentum of the system, L,
+    is equal to the vector sum of the linear momentum of the particle, L1, and
+    the linear momentum of the rigid body, L2, i.e.
+
+    L = L1 + L2
+
+    Parameters
+    ==========
+
+    frame : ReferenceFrame
+        The frame in which linear momentum is desired.
+    body1, body2, body3... : Particle and/or RigidBody
+        The body (or bodies) whose linear momentum is required.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
+    >>> from sympy.physics.mechanics import RigidBody, outer, linear_momentum
+    >>> N = ReferenceFrame('N')
+    >>> P = Point('P')
+    >>> P.set_vel(N, 10 * N.x)
+    >>> Pa = Particle('Pa', P, 1)
+    >>> Ac = Point('Ac')
+    >>> Ac.set_vel(N, 25 * N.y)
+    >>> I = outer(N.x, N.x)
+    >>> A = RigidBody('A', Ac, N, 20, (I, Ac))
+    >>> linear_momentum(N, A, Pa)
+    10*N.x + 500*N.y
+
+    """
+
+    if not isinstance(frame, ReferenceFrame):
+        raise TypeError('Please specify a valid ReferenceFrame')
+    else:
+        linear_momentum_sys = Vector(0)
+        for e in body:
+            if isinstance(e, (RigidBody, Particle)):
+                linear_momentum_sys += e.linear_momentum(frame)
+            else:
+                raise TypeError('*body must have only Particle or RigidBody')
+    return linear_momentum_sys
+
+
+def angular_momentum(point, frame, *body):
+    """Angular momentum of a system.
+
+    Explanation
+    ===========
+
+    This function returns the angular momentum of a system of Particle's and/or
+    RigidBody's. The angular momentum of such a system is equal to the vector
+    sum of the angular momentum of its constituents. Consider a system, S,
+    comprised of a rigid body, A, and a particle, P. The angular momentum of
+    the system, H, is equal to the vector sum of the angular momentum of the
+    particle, H1, and the angular momentum of the rigid body, H2, i.e.
+
+    H = H1 + H2
+
+    Parameters
+    ==========
+
+    point : Point
+        The point about which angular momentum of the system is desired.
+    frame : ReferenceFrame
+        The frame in which angular momentum is desired.
+    body1, body2, body3... : Particle and/or RigidBody
+        The body (or bodies) whose angular momentum is required.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
+    >>> from sympy.physics.mechanics import RigidBody, outer, angular_momentum
+    >>> N = ReferenceFrame('N')
+    >>> O = Point('O')
+    >>> O.set_vel(N, 0 * N.x)
+    >>> P = O.locatenew('P', 1 * N.x)
+    >>> P.set_vel(N, 10 * N.x)
+    >>> Pa = Particle('Pa', P, 1)
+    >>> Ac = O.locatenew('Ac', 2 * N.y)
+    >>> Ac.set_vel(N, 5 * N.y)
+    >>> a = ReferenceFrame('a')
+    >>> a.set_ang_vel(N, 10 * N.z)
+    >>> I = outer(N.z, N.z)
+    >>> A = RigidBody('A', Ac, a, 20, (I, Ac))
+    >>> angular_momentum(O, N, Pa, A)
+    10*N.z
+
+    """
+
+    if not isinstance(frame, ReferenceFrame):
+        raise TypeError('Please enter a valid ReferenceFrame')
+    if not isinstance(point, Point):
+        raise TypeError('Please specify a valid Point')
+    else:
+        angular_momentum_sys = Vector(0)
+        for e in body:
+            if isinstance(e, (RigidBody, Particle)):
+                angular_momentum_sys += e.angular_momentum(point, frame)
+            else:
+                raise TypeError('*body must have only Particle or RigidBody')
+    return angular_momentum_sys
+
+
+def kinetic_energy(frame, *body):
+    """Kinetic energy of a multibody system.
+
+    Explanation
+    ===========
+
+    This function returns the kinetic energy of a system of Particle's and/or
+    RigidBody's. The kinetic energy of such a system is equal to the sum of
+    the kinetic energies of its constituents. Consider a system, S, comprising
+    a rigid body, A, and a particle, P. The kinetic energy of the system, T,
+    is equal to the vector sum of the kinetic energy of the particle, T1, and
+    the kinetic energy of the rigid body, T2, i.e.
+
+    T = T1 + T2
+
+    Kinetic energy is a scalar.
+
+    Parameters
+    ==========
+
+    frame : ReferenceFrame
+        The frame in which the velocity or angular velocity of the body is
+        defined.
+    body1, body2, body3... : Particle and/or RigidBody
+        The body (or bodies) whose kinetic energy is required.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
+    >>> from sympy.physics.mechanics import RigidBody, outer, kinetic_energy
+    >>> N = ReferenceFrame('N')
+    >>> O = Point('O')
+    >>> O.set_vel(N, 0 * N.x)
+    >>> P = O.locatenew('P', 1 * N.x)
+    >>> P.set_vel(N, 10 * N.x)
+    >>> Pa = Particle('Pa', P, 1)
+    >>> Ac = O.locatenew('Ac', 2 * N.y)
+    >>> Ac.set_vel(N, 5 * N.y)
+    >>> a = ReferenceFrame('a')
+    >>> a.set_ang_vel(N, 10 * N.z)
+    >>> I = outer(N.z, N.z)
+    >>> A = RigidBody('A', Ac, a, 20, (I, Ac))
+    >>> kinetic_energy(N, Pa, A)
+    350
+
+    """
+
+    if not isinstance(frame, ReferenceFrame):
+        raise TypeError('Please enter a valid ReferenceFrame')
+    ke_sys = S.Zero
+    for e in body:
+        if isinstance(e, (RigidBody, Particle)):
+            ke_sys += e.kinetic_energy(frame)
+        else:
+            raise TypeError('*body must have only Particle or RigidBody')
+    return ke_sys
+
+
+def potential_energy(*body):
+    """Potential energy of a multibody system.
+
+    Explanation
+    ===========
+
+    This function returns the potential energy of a system of Particle's and/or
+    RigidBody's. The potential energy of such a system is equal to the sum of
+    the potential energy of its constituents. Consider a system, S, comprising
+    a rigid body, A, and a particle, P. The potential energy of the system, V,
+    is equal to the vector sum of the potential energy of the particle, V1, and
+    the potential energy of the rigid body, V2, i.e.
+
+    V = V1 + V2
+
+    Potential energy is a scalar.
+
+    Parameters
+    ==========
+
+    body1, body2, body3... : Particle and/or RigidBody
+        The body (or bodies) whose potential energy is required.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
+    >>> from sympy.physics.mechanics import RigidBody, outer, potential_energy
+    >>> from sympy import symbols
+    >>> M, m, g, h = symbols('M m g h')
+    >>> N = ReferenceFrame('N')
+    >>> O = Point('O')
+    >>> O.set_vel(N, 0 * N.x)
+    >>> P = O.locatenew('P', 1 * N.x)
+    >>> Pa = Particle('Pa', P, m)
+    >>> Ac = O.locatenew('Ac', 2 * N.y)
+    >>> a = ReferenceFrame('a')
+    >>> I = outer(N.z, N.z)
+    >>> A = RigidBody('A', Ac, a, M, (I, Ac))
+    >>> Pa.potential_energy = m * g * h
+    >>> A.potential_energy = M * g * h
+    >>> potential_energy(Pa, A)
+    M*g*h + g*h*m
+
+    """
+
+    pe_sys = S.Zero
+    for e in body:
+        if isinstance(e, (RigidBody, Particle)):
+            pe_sys += e.potential_energy
+        else:
+            raise TypeError('*body must have only Particle or RigidBody')
+    return pe_sys
+
+
+def gravity(acceleration, *bodies):
+    from sympy.physics.mechanics.loads import gravity as _gravity
+    sympy_deprecation_warning(
+        """
+        The gravity function has been moved.
+        Import it from "sympy.physics.mechanics.loads".
+        """,
+        deprecated_since_version="1.13",
+        active_deprecations_target="moved-mechanics-functions"
+    )
+    return _gravity(acceleration, *bodies)
+
+
+def center_of_mass(point, *bodies):
+    """
+    Returns the position vector from the given point to the center of mass
+    of the given bodies(particles or rigidbodies).
+
+    Example
+    =======
+
+    >>> from sympy import symbols, S
+    >>> from sympy.physics.vector import Point
+    >>> from sympy.physics.mechanics import Particle, ReferenceFrame, RigidBody, outer
+    >>> from sympy.physics.mechanics.functions import center_of_mass
+    >>> a = ReferenceFrame('a')
+    >>> m = symbols('m', real=True)
+    >>> p1 = Particle('p1', Point('p1_pt'), S(1))
+    >>> p2 = Particle('p2', Point('p2_pt'), S(2))
+    >>> p3 = Particle('p3', Point('p3_pt'), S(3))
+    >>> p4 = Particle('p4', Point('p4_pt'), m)
+    >>> b_f = ReferenceFrame('b_f')
+    >>> b_cm = Point('b_cm')
+    >>> mb = symbols('mb')
+    >>> b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
+    >>> p2.point.set_pos(p1.point, a.x)
+    >>> p3.point.set_pos(p1.point, a.x + a.y)
+    >>> p4.point.set_pos(p1.point, a.y)
+    >>> b.masscenter.set_pos(p1.point, a.y + a.z)
+    >>> point_o=Point('o')
+    >>> point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
+    >>> expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
+    >>> point_o.pos_from(p1.point)
+    5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
+
+    """
+    if not bodies:
+        raise TypeError("No bodies(instances of Particle or Rigidbody) were passed.")
+
+    total_mass = 0
+    vec = Vector(0)
+    for i in bodies:
+        total_mass += i.mass
+
+        masscenter = getattr(i, 'masscenter', None)
+        if masscenter is None:
+            masscenter = i.point
+        vec += i.mass*masscenter.pos_from(point)
+
+    return vec/total_mass
+
+
+def Lagrangian(frame, *body):
+    """Lagrangian of a multibody system.
+
+    Explanation
+    ===========
+
+    This function returns the Lagrangian of a system of Particle's and/or
+    RigidBody's. The Lagrangian of such a system is equal to the difference
+    between the kinetic energies and potential energies of its constituents. If
+    T and V are the kinetic and potential energies of a system then it's
+    Lagrangian, L, is defined as
+
+    L = T - V
+
+    The Lagrangian is a scalar.
+
+    Parameters
+    ==========
+
+    frame : ReferenceFrame
+        The frame in which the velocity or angular velocity of the body is
+        defined to determine the kinetic energy.
+
+    body1, body2, body3... : Particle and/or RigidBody
+        The body (or bodies) whose Lagrangian is required.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
+    >>> from sympy.physics.mechanics import RigidBody, outer, Lagrangian
+    >>> from sympy import symbols
+    >>> M, m, g, h = symbols('M m g h')
+    >>> N = ReferenceFrame('N')
+    >>> O = Point('O')
+    >>> O.set_vel(N, 0 * N.x)
+    >>> P = O.locatenew('P', 1 * N.x)
+    >>> P.set_vel(N, 10 * N.x)
+    >>> Pa = Particle('Pa', P, 1)
+    >>> Ac = O.locatenew('Ac', 2 * N.y)
+    >>> Ac.set_vel(N, 5 * N.y)
+    >>> a = ReferenceFrame('a')
+    >>> a.set_ang_vel(N, 10 * N.z)
+    >>> I = outer(N.z, N.z)
+    >>> A = RigidBody('A', Ac, a, 20, (I, Ac))
+    >>> Pa.potential_energy = m * g * h
+    >>> A.potential_energy = M * g * h
+    >>> Lagrangian(N, Pa, A)
+    -M*g*h - g*h*m + 350
+
+    """
+
+    if not isinstance(frame, ReferenceFrame):
+        raise TypeError('Please supply a valid ReferenceFrame')
+    for e in body:
+        if not isinstance(e, (RigidBody, Particle)):
+            raise TypeError('*body must have only Particle or RigidBody')
+    return kinetic_energy(frame, *body) - potential_energy(*body)
+
+
+def find_dynamicsymbols(expression, exclude=None, reference_frame=None):
+    """Find all dynamicsymbols in expression.
+
+    Explanation
+    ===========
+
+    If the optional ``exclude`` kwarg is used, only dynamicsymbols
+    not in the iterable ``exclude`` are returned.
+    If we intend to apply this function on a vector, the optional
+    ``reference_frame`` is also used to inform about the corresponding frame
+    with respect to which the dynamic symbols of the given vector is to be
+    determined.
+
+    Parameters
+    ==========
+
+    expression : SymPy expression
+
+    exclude : iterable of dynamicsymbols, optional
+
+    reference_frame : ReferenceFrame, optional
+        The frame with respect to which the dynamic symbols of the
+        given vector is to be determined.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import dynamicsymbols, find_dynamicsymbols
+    >>> from sympy.physics.mechanics import ReferenceFrame
+    >>> x, y = dynamicsymbols('x, y')
+    >>> expr = x + x.diff()*y
+    >>> find_dynamicsymbols(expr)
+    {x(t), y(t), Derivative(x(t), t)}
+    >>> find_dynamicsymbols(expr, exclude=[x, y])
+    {Derivative(x(t), t)}
+    >>> a, b, c = dynamicsymbols('a, b, c')
+    >>> A = ReferenceFrame('A')
+    >>> v = a * A.x + b * A.y + c * A.z
+    >>> find_dynamicsymbols(v, reference_frame=A)
+    {a(t), b(t), c(t)}
+
+    """
+    t_set = {dynamicsymbols._t}
+    if exclude:
+        if iterable(exclude):
+            exclude_set = set(exclude)
+        else:
+            raise TypeError("exclude kwarg must be iterable")
+    else:
+        exclude_set = set()
+    if isinstance(expression, Vector):
+        if reference_frame is None:
+            raise ValueError("You must provide reference_frame when passing a "
+                             "vector expression, got %s." % reference_frame)
+        else:
+            expression = expression.to_matrix(reference_frame)
+    return {i for i in expression.atoms(AppliedUndef, Derivative) if
+            i.free_symbols == t_set} - exclude_set
+
+
+def msubs(expr, *sub_dicts, smart=False, **kwargs):
+    """A custom subs for use on expressions derived in physics.mechanics.
+
+    Traverses the expression tree once, performing the subs found in sub_dicts.
+    Terms inside ``Derivative`` expressions are ignored:
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import dynamicsymbols, msubs
+    >>> x = dynamicsymbols('x')
+    >>> msubs(x.diff() + x, {x: 1})
+    Derivative(x(t), t) + 1
+
+    Note that sub_dicts can be a single dictionary, or several dictionaries:
+
+    >>> x, y, z = dynamicsymbols('x, y, z')
+    >>> sub1 = {x: 1, y: 2}
+    >>> sub2 = {z: 3, x.diff(): 4}
+    >>> msubs(x.diff() + x + y + z, sub1, sub2)
+    10
+
+    If smart=True (default False), also checks for conditions that may result
+    in ``nan``, but if simplified would yield a valid expression. For example:
+
+    >>> from sympy import sin, tan
+    >>> (sin(x)/tan(x)).subs(x, 0)
+    nan
+    >>> msubs(sin(x)/tan(x), {x: 0}, smart=True)
+    1
+
+    It does this by first replacing all ``tan`` with ``sin/cos``. Then each
+    node is traversed. If the node is a fraction, subs is first evaluated on
+    the denominator. If this results in 0, simplification of the entire
+    fraction is attempted. Using this selective simplification, only
+    subexpressions that result in 1/0 are targeted, resulting in faster
+    performance.
+
+    """
+
+    sub_dict = dict_merge(*sub_dicts)
+    if smart:
+        func = _smart_subs
+    elif hasattr(expr, 'msubs'):
+        return expr.msubs(sub_dict)
+    else:
+        func = lambda expr, sub_dict: _crawl(expr, _sub_func, sub_dict)
+    if isinstance(expr, (Matrix, Vector, Dyadic)):
+        return expr.applyfunc(lambda x: func(x, sub_dict))
+    else:
+        return func(expr, sub_dict)
+
+
+def _crawl(expr, func, *args, **kwargs):
+    """Crawl the expression tree, and apply func to every node."""
+    val = func(expr, *args, **kwargs)
+    if val is not None:
+        return val
+    new_args = (_crawl(arg, func, *args, **kwargs) for arg in expr.args)
+    return expr.func(*new_args)
+
+
+def _sub_func(expr, sub_dict):
+    """Perform direct matching substitution, ignoring derivatives."""
+    if expr in sub_dict:
+        return sub_dict[expr]
+    elif not expr.args or expr.is_Derivative:
+        return expr
+
+
+def _tan_repl_func(expr):
+    """Replace tan with sin/cos."""
+    if isinstance(expr, tan):
+        return sin(*expr.args) / cos(*expr.args)
+    elif not expr.args or expr.is_Derivative:
+        return expr
+
+
+def _smart_subs(expr, sub_dict):
+    """Performs subs, checking for conditions that may result in `nan` or
+    `oo`, and attempts to simplify them out.
+
+    The expression tree is traversed twice, and the following steps are
+    performed on each expression node:
+    - First traverse:
+        Replace all `tan` with `sin/cos`.
+    - Second traverse:
+        If node is a fraction, check if the denominator evaluates to 0.
+        If so, attempt to simplify it out. Then if node is in sub_dict,
+        sub in the corresponding value.
+
+    """
+    expr = _crawl(expr, _tan_repl_func)
+
+    def _recurser(expr, sub_dict):
+        # Decompose the expression into num, den
+        num, den = _fraction_decomp(expr)
+        if den != 1:
+            # If there is a non trivial denominator, we need to handle it
+            denom_subbed = _recurser(den, sub_dict)
+            if denom_subbed.evalf() == 0:
+                # If denom is 0 after this, attempt to simplify the bad expr
+                expr = simplify(expr)
+            else:
+                # Expression won't result in nan, find numerator
+                num_subbed = _recurser(num, sub_dict)
+                return num_subbed / denom_subbed
+        # We have to crawl the tree manually, because `expr` may have been
+        # modified in the simplify step. First, perform subs as normal:
+        val = _sub_func(expr, sub_dict)
+        if val is not None:
+            return val
+        new_args = (_recurser(arg, sub_dict) for arg in expr.args)
+        return expr.func(*new_args)
+    return _recurser(expr, sub_dict)
+
+
+def _fraction_decomp(expr):
+    """Return num, den such that expr = num/den."""
+    if not isinstance(expr, Mul):
+        return expr, 1
+    num = []
+    den = []
+    for a in expr.args:
+        if a.is_Pow and a.args[1] < 0:
+            den.append(1 / a)
+        else:
+            num.append(a)
+    if not den:
+        return expr, 1
+    num = Mul(*num)
+    den = Mul(*den)
+    return num, den
+
+
+def _f_list_parser(fl, ref_frame):
+    """Parses the provided forcelist composed of items
+    of the form (obj, force).
+    Returns a tuple containing:
+        vel_list: The velocity (ang_vel for Frames, vel for Points) in
+                the provided reference frame.
+        f_list: The forces.
+
+    Used internally in the KanesMethod and LagrangesMethod classes.
+
+    """
+    def flist_iter():
+        for pair in fl:
+            obj, force = pair
+            if isinstance(obj, ReferenceFrame):
+                yield obj.ang_vel_in(ref_frame), force
+            elif isinstance(obj, Point):
+                yield obj.vel(ref_frame), force
+            else:
+                raise TypeError('First entry in each forcelist pair must '
+                                'be a point or frame.')
+
+    if not fl:
+        vel_list, f_list = (), ()
+    else:
+        unzip = lambda l: list(zip(*l)) if l[0] else [(), ()]
+        vel_list, f_list = unzip(list(flist_iter()))
+    return vel_list, f_list
+
+
+def _validate_coordinates(coordinates=None, speeds=None, check_duplicates=True,
+                          is_dynamicsymbols=True, u_auxiliary=None):
+    """Validate the generalized coordinates and generalized speeds.
+
+    Parameters
+    ==========
+    coordinates : iterable, optional
+        Generalized coordinates to be validated.
+    speeds : iterable, optional
+        Generalized speeds to be validated.
+    check_duplicates : bool, optional
+        Checks if there are duplicates in the generalized coordinates and
+        generalized speeds. If so it will raise a ValueError. The default is
+        True.
+    is_dynamicsymbols : iterable, optional
+        Checks if all the generalized coordinates and generalized speeds are
+        dynamicsymbols. If any is not a dynamicsymbol, a ValueError will be
+        raised. The default is True.
+    u_auxiliary : iterable, optional
+        Auxiliary generalized speeds to be validated.
+
+    """
+    t_set = {dynamicsymbols._t}
+    # Convert input to iterables
+    if coordinates is None:
+        coordinates = []
+    elif not iterable(coordinates):
+        coordinates = [coordinates]
+    if speeds is None:
+        speeds = []
+    elif not iterable(speeds):
+        speeds = [speeds]
+    if u_auxiliary is None:
+        u_auxiliary = []
+    elif not iterable(u_auxiliary):
+        u_auxiliary = [u_auxiliary]
+
+    msgs = []
+    if check_duplicates:  # Check for duplicates
+        seen = set()
+        coord_duplicates = {x for x in coordinates if x in seen or seen.add(x)}
+        seen = set()
+        speed_duplicates = {x for x in speeds if x in seen or seen.add(x)}
+        seen = set()
+        aux_duplicates = {x for x in u_auxiliary if x in seen or seen.add(x)}
+        overlap_coords = set(coordinates).intersection(speeds)
+        overlap_aux = set(coordinates).union(speeds).intersection(u_auxiliary)
+        if coord_duplicates:
+            msgs.append(f'The generalized coordinates {coord_duplicates} are '
+                        f'duplicated, all generalized coordinates should be '
+                        f'unique.')
+        if speed_duplicates:
+            msgs.append(f'The generalized speeds {speed_duplicates} are '
+                        f'duplicated, all generalized speeds should be unique.')
+        if aux_duplicates:
+            msgs.append(f'The auxiliary speeds {aux_duplicates} are duplicated,'
+                        f' all auxiliary speeds should be unique.')
+        if overlap_coords:
+            msgs.append(f'{overlap_coords} are defined as both generalized '
+                        f'coordinates and generalized speeds.')
+        if overlap_aux:
+            msgs.append(f'The auxiliary speeds {overlap_aux} are also defined '
+                        f'as generalized coordinates or generalized speeds.')
+    if is_dynamicsymbols:  # Check whether all coordinates are dynamicsymbols
+        for coordinate in coordinates:
+            if not (isinstance(coordinate, (AppliedUndef, Derivative)) and
+                    coordinate.free_symbols == t_set):
+                msgs.append(f'Generalized coordinate "{coordinate}" is not a '
+                            f'dynamicsymbol.')
+        for speed in speeds:
+            if not (isinstance(speed, (AppliedUndef, Derivative)) and
+                    speed.free_symbols == t_set):
+                msgs.append(
+                    f'Generalized speed "{speed}" is not a dynamicsymbol.')
+        for aux in u_auxiliary:
+            if not (isinstance(aux, (AppliedUndef, Derivative)) and
+                    aux.free_symbols == t_set):
+                msgs.append(
+                    f'Auxiliary speed "{aux}" is not a dynamicsymbol.')
+    if msgs:
+        raise ValueError('\n'.join(msgs))
+
+
+def _parse_linear_solver(linear_solver):
+    """Helper function to retrieve a specified linear solver."""
+    if callable(linear_solver):
+        return linear_solver
+    return lambda A, b: Matrix.solve(A, b, method=linear_solver)

lib/python3.12/site-packages/sympy/physics/mechanics/inertia.py

@@ -0,0 +1,197 @@
+from sympy import sympify
+from sympy.physics.vector import Point, Dyadic, ReferenceFrame, outer
+from collections import namedtuple
+
+__all__ = ['inertia', 'inertia_of_point_mass', 'Inertia']
+
+
+def inertia(frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0):
+    """Simple way to create inertia Dyadic object.
+
+    Explanation
+    ===========
+
+    Creates an inertia Dyadic based on the given tensor values and a body-fixed
+    reference frame.
+
+    Parameters
+    ==========
+
+    frame : ReferenceFrame
+        The frame the inertia is defined in.
+    ixx : Sympifyable
+        The xx element in the inertia dyadic.
+    iyy : Sympifyable
+        The yy element in the inertia dyadic.
+    izz : Sympifyable
+        The zz element in the inertia dyadic.
+    ixy : Sympifyable
+        The xy element in the inertia dyadic.
+    iyz : Sympifyable
+        The yz element in the inertia dyadic.
+    izx : Sympifyable
+        The zx element in the inertia dyadic.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import ReferenceFrame, inertia
+    >>> N = ReferenceFrame('N')
+    >>> inertia(N, 1, 2, 3)
+    (N.x|N.x) + 2*(N.y|N.y) + 3*(N.z|N.z)
+
+    """
+
+    if not isinstance(frame, ReferenceFrame):
+        raise TypeError('Need to define the inertia in a frame')
+    ixx, iyy, izz = sympify(ixx), sympify(iyy), sympify(izz)
+    ixy, iyz, izx = sympify(ixy), sympify(iyz), sympify(izx)
+    return (ixx*outer(frame.x, frame.x) + ixy*outer(frame.x, frame.y) +
+            izx*outer(frame.x, frame.z) + ixy*outer(frame.y, frame.x) +
+            iyy*outer(frame.y, frame.y) + iyz*outer(frame.y, frame.z) +
+            izx*outer(frame.z, frame.x) + iyz*outer(frame.z, frame.y) +
+            izz*outer(frame.z, frame.z))
+
+
+def inertia_of_point_mass(mass, pos_vec, frame):
+    """Inertia dyadic of a point mass relative to point O.
+
+    Parameters
+    ==========
+
+    mass : Sympifyable
+        Mass of the point mass
+    pos_vec : Vector
+        Position from point O to point mass
+    frame : ReferenceFrame
+        Reference frame to express the dyadic in
+
+    Examples
+    ========
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import ReferenceFrame, inertia_of_point_mass
+    >>> N = ReferenceFrame('N')
+    >>> r, m = symbols('r m')
+    >>> px = r * N.x
+    >>> inertia_of_point_mass(m, px, N)
+    m*r**2*(N.y|N.y) + m*r**2*(N.z|N.z)
+
+    """
+
+    return mass*(
+        (outer(frame.x, frame.x) +
+         outer(frame.y, frame.y) +
+         outer(frame.z, frame.z)) *
+        (pos_vec.dot(pos_vec)) - outer(pos_vec, pos_vec))
+
+
+class Inertia(namedtuple('Inertia', ['dyadic', 'point'])):
+    """Inertia object consisting of a Dyadic and a Point of reference.
+
+    Explanation
+    ===========
+
+    This is a simple class to store the Point and Dyadic, belonging to an
+    inertia.
+
+    Attributes
+    ==========
+
+    dyadic : Dyadic
+        The dyadic of the inertia.
+    point : Point
+        The reference point of the inertia.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import ReferenceFrame, Point, Inertia
+    >>> N = ReferenceFrame('N')
+    >>> Po = Point('Po')
+    >>> Inertia(N.x.outer(N.x) + N.y.outer(N.y) + N.z.outer(N.z), Po)
+    ((N.x|N.x) + (N.y|N.y) + (N.z|N.z), Po)
+
+    In the example above the Dyadic was created manually, one can however also
+    use the ``inertia`` function for this or the class method ``from_tensor`` as
+    shown below.
+
+    >>> Inertia.from_inertia_scalars(Po, N, 1, 1, 1)
+    ((N.x|N.x) + (N.y|N.y) + (N.z|N.z), Po)
+
+    """
+    def __new__(cls, dyadic, point):
+        # Switch order if given in the wrong order
+        if isinstance(dyadic, Point) and isinstance(point, Dyadic):
+            point, dyadic = dyadic, point
+        if not isinstance(point, Point):
+            raise TypeError('Reference point should be of type Point')
+        if not isinstance(dyadic, Dyadic):
+            raise TypeError('Inertia value should be expressed as a Dyadic')
+        return super().__new__(cls, dyadic, point)
+
+    @classmethod
+    def from_inertia_scalars(cls, point, frame, ixx, iyy, izz, ixy=0, iyz=0,
+                             izx=0):
+        """Simple way to create an Inertia object based on the tensor values.
+
+        Explanation
+        ===========
+
+        This class method uses the :func`~.inertia` to create the Dyadic based
+        on the tensor values.
+
+        Parameters
+        ==========
+
+        point : Point
+            The reference point of the inertia.
+        frame : ReferenceFrame
+            The frame the inertia is defined in.
+        ixx : Sympifyable
+            The xx element in the inertia dyadic.
+        iyy : Sympifyable
+            The yy element in the inertia dyadic.
+        izz : Sympifyable
+            The zz element in the inertia dyadic.
+        ixy : Sympifyable
+            The xy element in the inertia dyadic.
+        iyz : Sympifyable
+            The yz element in the inertia dyadic.
+        izx : Sympifyable
+            The zx element in the inertia dyadic.
+
+        Examples
+        ========
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import ReferenceFrame, Point, Inertia
+        >>> ixx, iyy, izz, ixy, iyz, izx = symbols('ixx iyy izz ixy iyz izx')
+        >>> N = ReferenceFrame('N')
+        >>> P = Point('P')
+        >>> I = Inertia.from_inertia_scalars(P, N, ixx, iyy, izz, ixy, iyz, izx)
+
+        The tensor values can easily be seen when converting the dyadic to a
+        matrix.
+
+        >>> I.dyadic.to_matrix(N)
+        Matrix([
+        [ixx, ixy, izx],
+        [ixy, iyy, iyz],
+        [izx, iyz, izz]])
+
+        """
+        return cls(inertia(frame, ixx, iyy, izz, ixy, iyz, izx), point)
+
+    def __add__(self, other):
+        raise TypeError(f"unsupported operand type(s) for +: "
+                        f"'{self.__class__.__name__}' and "
+                        f"'{other.__class__.__name__}'")
+
+    def __mul__(self, other):
+        raise TypeError(f"unsupported operand type(s) for *: "
+                        f"'{self.__class__.__name__}' and "
+                        f"'{other.__class__.__name__}'")
+
+    __radd__ = __add__
+    __rmul__ = __mul__

lib/python3.12/site-packages/sympy/physics/mechanics/lagrange.py

@@ -0,0 +1,512 @@
+from sympy import diff, zeros, Matrix, eye, sympify
+from sympy.core.sorting import default_sort_key
+from sympy.physics.vector import dynamicsymbols, ReferenceFrame
+from sympy.physics.mechanics.method import _Methods
+from sympy.physics.mechanics.functions import (
+    find_dynamicsymbols, msubs, _f_list_parser, _validate_coordinates)
+from sympy.physics.mechanics.linearize import Linearizer
+from sympy.utilities.iterables import iterable
+
+__all__ = ['LagrangesMethod']
+
+
+class LagrangesMethod(_Methods):
+    """Lagrange's method object.
+
+    Explanation
+    ===========
+
+    This object generates the equations of motion in a two step procedure. The
+    first step involves the initialization of LagrangesMethod by supplying the
+    Lagrangian and the generalized coordinates, at the bare minimum. If there
+    are any constraint equations, they can be supplied as keyword arguments.
+    The Lagrange multipliers are automatically generated and are equal in
+    number to the constraint equations. Similarly any non-conservative forces
+    can be supplied in an iterable (as described below and also shown in the
+    example) along with a ReferenceFrame. This is also discussed further in the
+    __init__ method.
+
+    Attributes
+    ==========
+
+    q, u : Matrix
+        Matrices of the generalized coordinates and speeds
+    loads : iterable
+        Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
+        describing the forces on the system.
+    bodies : iterable
+        Iterable containing the rigid bodies and particles of the system.
+    mass_matrix : Matrix
+        The system's mass matrix
+    forcing : Matrix
+        The system's forcing vector
+    mass_matrix_full : Matrix
+        The "mass matrix" for the qdot's, qdoubledot's, and the
+        lagrange multipliers (lam)
+    forcing_full : Matrix
+        The forcing vector for the qdot's, qdoubledot's and
+        lagrange multipliers (lam)
+
+    Examples
+    ========
+
+    This is a simple example for a one degree of freedom translational
+    spring-mass-damper.
+
+    In this example, we first need to do the kinematics.
+    This involves creating generalized coordinates and their derivatives.
+    Then we create a point and set its velocity in a frame.
+
+        >>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian
+        >>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point
+        >>> from sympy.physics.mechanics import dynamicsymbols
+        >>> from sympy import symbols
+        >>> q = dynamicsymbols('q')
+        >>> qd = dynamicsymbols('q', 1)
+        >>> m, k, b = symbols('m k b')
+        >>> N = ReferenceFrame('N')
+        >>> P = Point('P')
+        >>> P.set_vel(N, qd * N.x)
+
+    We need to then prepare the information as required by LagrangesMethod to
+    generate equations of motion.
+    First we create the Particle, which has a point attached to it.
+    Following this the lagrangian is created from the kinetic and potential
+    energies.
+    Then, an iterable of nonconservative forces/torques must be constructed,
+    where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple,
+    with the Vectors representing the nonconservative forces or torques.
+
+        >>> Pa = Particle('Pa', P, m)
+        >>> Pa.potential_energy = k * q**2 / 2.0
+        >>> L = Lagrangian(N, Pa)
+        >>> fl = [(P, -b * qd * N.x)]
+
+    Finally we can generate the equations of motion.
+    First we create the LagrangesMethod object. To do this one must supply
+    the Lagrangian, and the generalized coordinates. The constraint equations,
+    the forcelist, and the inertial frame may also be provided, if relevant.
+    Next we generate Lagrange's equations of motion, such that:
+    Lagrange's equations of motion = 0.
+    We have the equations of motion at this point.
+
+        >>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N)
+        >>> print(l.form_lagranges_equations())
+        Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), (t, 2))]])
+
+    We can also solve for the states using the 'rhs' method.
+
+        >>> print(l.rhs())
+        Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]])
+
+    Please refer to the docstrings on each method for more details.
+    """
+
+    def __init__(self, Lagrangian, qs, forcelist=None, bodies=None, frame=None,
+                 hol_coneqs=None, nonhol_coneqs=None):
+        """Supply the following for the initialization of LagrangesMethod.
+
+        Lagrangian : Sympifyable
+
+        qs : array_like
+            The generalized coordinates
+
+        hol_coneqs : array_like, optional
+            The holonomic constraint equations
+
+        nonhol_coneqs : array_like, optional
+            The nonholonomic constraint equations
+
+        forcelist : iterable, optional
+            Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
+            tuples which represent the force at a point or torque on a frame.
+            This feature is primarily to account for the nonconservative forces
+            and/or moments.
+
+        bodies : iterable, optional
+            Takes an iterable containing the rigid bodies and particles of the
+            system.
+
+        frame : ReferenceFrame, optional
+            Supply the inertial frame. This is used to determine the
+            generalized forces due to non-conservative forces.
+        """
+
+        self._L = Matrix([sympify(Lagrangian)])
+        self.eom = None
+        self._m_cd = Matrix()           # Mass Matrix of differentiated coneqs
+        self._m_d = Matrix()            # Mass Matrix of dynamic equations
+        self._f_cd = Matrix()           # Forcing part of the diff coneqs
+        self._f_d = Matrix()            # Forcing part of the dynamic equations
+        self.lam_coeffs = Matrix()      # The coeffecients of the multipliers
+
+        forcelist = forcelist if forcelist else []
+        if not iterable(forcelist):
+            raise TypeError('Force pairs must be supplied in an iterable.')
+        self._forcelist = forcelist
+        if frame and not isinstance(frame, ReferenceFrame):
+            raise TypeError('frame must be a valid ReferenceFrame')
+        self._bodies = bodies
+        self.inertial = frame
+
+        self.lam_vec = Matrix()
+
+        self._term1 = Matrix()
+        self._term2 = Matrix()
+        self._term3 = Matrix()
+        self._term4 = Matrix()
+
+        # Creating the qs, qdots and qdoubledots
+        if not iterable(qs):
+            raise TypeError('Generalized coordinates must be an iterable')
+        self._q = Matrix(qs)
+        self._qdots = self.q.diff(dynamicsymbols._t)
+        self._qdoubledots = self._qdots.diff(dynamicsymbols._t)
+        _validate_coordinates(self.q)
+
+        mat_build = lambda x: Matrix(x) if x else Matrix()
+        hol_coneqs = mat_build(hol_coneqs)
+        nonhol_coneqs = mat_build(nonhol_coneqs)
+        self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t),
+                nonhol_coneqs])
+        self._hol_coneqs = hol_coneqs
+
+    def form_lagranges_equations(self):
+        """Method to form Lagrange's equations of motion.
+
+        Returns a vector of equations of motion using Lagrange's equations of
+        the second kind.
+        """
+
+        qds = self._qdots
+        qdd_zero = dict.fromkeys(self._qdoubledots, 0)
+        n = len(self.q)
+
+        # Internally we represent the EOM as four terms:
+        # EOM = term1 - term2 - term3 - term4 = 0
+
+        # First term
+        self._term1 = self._L.jacobian(qds)
+        self._term1 = self._term1.diff(dynamicsymbols._t).T
+
+        # Second term
+        self._term2 = self._L.jacobian(self.q).T
+
+        # Third term
+        if self.coneqs:
+            coneqs = self.coneqs
+            m = len(coneqs)
+            # Creating the multipliers
+            self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
+            self.lam_coeffs = -coneqs.jacobian(qds)
+            self._term3 = self.lam_coeffs.T * self.lam_vec
+            # Extracting the coeffecients of the qdds from the diff coneqs
+            diffconeqs = coneqs.diff(dynamicsymbols._t)
+            self._m_cd = diffconeqs.jacobian(self._qdoubledots)
+            # The remaining terms i.e. the 'forcing' terms in diff coneqs
+            self._f_cd = -diffconeqs.subs(qdd_zero)
+        else:
+            self._term3 = zeros(n, 1)
+
+        # Fourth term
+        if self.forcelist:
+            N = self.inertial
+            self._term4 = zeros(n, 1)
+            for i, qd in enumerate(qds):
+                flist = zip(*_f_list_parser(self.forcelist, N))
+                self._term4[i] = sum(v.diff(qd, N).dot(f) for (v, f) in flist)
+        else:
+            self._term4 = zeros(n, 1)
+
+        # Form the dynamic mass and forcing matrices
+        without_lam = self._term1 - self._term2 - self._term4
+        self._m_d = without_lam.jacobian(self._qdoubledots)
+        self._f_d = -without_lam.subs(qdd_zero)
+
+        # Form the EOM
+        self.eom = without_lam - self._term3
+        return self.eom
+
+    def _form_eoms(self):
+        return self.form_lagranges_equations()
+
+    @property
+    def mass_matrix(self):
+        """Returns the mass matrix, which is augmented by the Lagrange
+        multipliers, if necessary.
+
+        Explanation
+        ===========
+
+        If the system is described by 'n' generalized coordinates and there are
+        no constraint equations then an n X n matrix is returned.
+
+        If there are 'n' generalized coordinates and 'm' constraint equations
+        have been supplied during initialization then an n X (n+m) matrix is
+        returned. The (n + m - 1)th and (n + m)th columns contain the
+        coefficients of the Lagrange multipliers.
+        """
+
+        if self.eom is None:
+            raise ValueError('Need to compute the equations of motion first')
+        if self.coneqs:
+            return (self._m_d).row_join(self.lam_coeffs.T)
+        else:
+            return self._m_d
+
+    @property
+    def mass_matrix_full(self):
+        """Augments the coefficients of qdots to the mass_matrix."""
+
+        if self.eom is None:
+            raise ValueError('Need to compute the equations of motion first')
+        n = len(self.q)
+        m = len(self.coneqs)
+        row1 = eye(n).row_join(zeros(n, n + m))
+        row2 = zeros(n, n).row_join(self.mass_matrix)
+        if self.coneqs:
+            row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m))
+            return row1.col_join(row2).col_join(row3)
+        else:
+            return row1.col_join(row2)
+
+    @property
+    def forcing(self):
+        """Returns the forcing vector from 'lagranges_equations' method."""
+
+        if self.eom is None:
+            raise ValueError('Need to compute the equations of motion first')
+        return self._f_d
+
+    @property
+    def forcing_full(self):
+        """Augments qdots to the forcing vector above."""
+
+        if self.eom is None:
+            raise ValueError('Need to compute the equations of motion first')
+        if self.coneqs:
+            return self._qdots.col_join(self.forcing).col_join(self._f_cd)
+        else:
+            return self._qdots.col_join(self.forcing)
+
+    def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None,
+                      linear_solver='LU'):
+        """Returns an instance of the Linearizer class, initiated from the data
+        in the LagrangesMethod class. This may be more desirable than using the
+        linearize class method, as the Linearizer object will allow more
+        efficient recalculation (i.e. about varying operating points).
+
+        Parameters
+        ==========
+
+        q_ind, qd_ind : array_like, optional
+            The independent generalized coordinates and speeds.
+        q_dep, qd_dep : array_like, optional
+            The dependent generalized coordinates and speeds.
+        linear_solver : str, callable
+            Method used to solve the several symbolic linear systems of the
+            form ``A*x=b`` in the linearization process. If a string is
+            supplied, it should be a valid method that can be used with the
+            :meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
+            supplied, it should have the format ``x = f(A, b)``, where it
+            solves the equations and returns the solution. The default is
+            ``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
+            ``LUsolve()`` is fast to compute but will often result in
+            divide-by-zero and thus ``nan`` results.
+
+        Returns
+        =======
+        Linearizer
+            An instantiated
+            :class:`sympy.physics.mechanics.linearize.Linearizer`.
+
+        """
+
+        # Compose vectors
+        t = dynamicsymbols._t
+        q = self.q
+        u = self._qdots
+        ud = u.diff(t)
+        # Get vector of lagrange multipliers
+        lams = self.lam_vec
+
+        mat_build = lambda x: Matrix(x) if x else Matrix()
+        q_i = mat_build(q_ind)
+        q_d = mat_build(q_dep)
+        u_i = mat_build(qd_ind)
+        u_d = mat_build(qd_dep)
+
+        # Compose general form equations
+        f_c = self._hol_coneqs
+        f_v = self.coneqs
+        f_a = f_v.diff(t)
+        f_0 = u
+        f_1 = -u
+        f_2 = self._term1
+        f_3 = -(self._term2 + self._term4)
+        f_4 = -self._term3
+
+        # Check that there are an appropriate number of independent and
+        # dependent coordinates
+        if len(q_d) != len(f_c) or len(u_d) != len(f_v):
+            raise ValueError(("Must supply {:} dependent coordinates, and " +
+                    "{:} dependent speeds").format(len(f_c), len(f_v)))
+        if set(Matrix([q_i, q_d])) != set(q):
+            raise ValueError("Must partition q into q_ind and q_dep, with " +
+                    "no extra or missing symbols.")
+        if set(Matrix([u_i, u_d])) != set(u):
+            raise ValueError("Must partition qd into qd_ind and qd_dep, " +
+                    "with no extra or missing symbols.")
+
+        # Find all other dynamic symbols, forming the forcing vector r.
+        # Sort r to make it canonical.
+        insyms = set(Matrix([q, u, ud, lams]))
+        r = list(find_dynamicsymbols(f_3, insyms))
+        r.sort(key=default_sort_key)
+        # Check for any derivatives of variables in r that are also found in r.
+        for i in r:
+            if diff(i, dynamicsymbols._t) in r:
+                raise ValueError('Cannot have derivatives of specified \
+                                 quantities when linearizing forcing terms.')
+
+        return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
+                          q_d, u_i, u_d, r, lams, linear_solver=linear_solver)
+
+    def linearize(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None,
+                  linear_solver='LU', **kwargs):
+        """Linearize the equations of motion about a symbolic operating point.
+
+        Parameters
+        ==========
+        linear_solver : str, callable
+            Method used to solve the several symbolic linear systems of the
+            form ``A*x=b`` in the linearization process. If a string is
+            supplied, it should be a valid method that can be used with the
+            :meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
+            supplied, it should have the format ``x = f(A, b)``, where it
+            solves the equations and returns the solution. The default is
+            ``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
+            ``LUsolve()`` is fast to compute but will often result in
+            divide-by-zero and thus ``nan`` results.
+        **kwargs
+            Extra keyword arguments are passed to
+            :meth:`sympy.physics.mechanics.linearize.Linearizer.linearize`.
+
+        Explanation
+        ===========
+
+        If kwarg A_and_B is False (default), returns M, A, B, r for the
+        linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r.
+
+        If kwarg A_and_B is True, returns A, B, r for the linearized form
+        dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is
+        computationally intensive if there are many symbolic parameters. For
+        this reason, it may be more desirable to use the default A_and_B=False,
+        returning M, A, and B. Values may then be substituted in to these
+        matrices, and the state space form found as
+        A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat.
+
+        In both cases, r is found as all dynamicsymbols in the equations of
+        motion that are not part of q, u, q', or u'. They are sorted in
+        canonical form.
+
+        The operating points may be also entered using the ``op_point`` kwarg.
+        This takes a dictionary of {symbol: value}, or a an iterable of such
+        dictionaries. The values may be numeric or symbolic. The more values
+        you can specify beforehand, the faster this computation will run.
+
+        For more documentation, please see the ``Linearizer`` class."""
+
+        linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep,
+                                        linear_solver=linear_solver)
+        result = linearizer.linearize(**kwargs)
+        return result + (linearizer.r,)
+
+    def solve_multipliers(self, op_point=None, sol_type='dict'):
+        """Solves for the values of the lagrange multipliers symbolically at
+        the specified operating point.
+
+        Parameters
+        ==========
+
+        op_point : dict or iterable of dicts, optional
+            Point at which to solve at. The operating point is specified as
+            a dictionary or iterable of dictionaries of {symbol: value}. The
+            value may be numeric or symbolic itself.
+
+        sol_type : str, optional
+            Solution return type. Valid options are:
+            - 'dict': A dict of {symbol : value} (default)
+            - 'Matrix': An ordered column matrix of the solution
+        """
+
+        # Determine number of multipliers
+        k = len(self.lam_vec)
+        if k == 0:
+            raise ValueError("System has no lagrange multipliers to solve for.")
+        # Compose dict of operating conditions
+        if isinstance(op_point, dict):
+            op_point_dict = op_point
+        elif iterable(op_point):
+            op_point_dict = {}
+            for op in op_point:
+                op_point_dict.update(op)
+        elif op_point is None:
+            op_point_dict = {}
+        else:
+            raise TypeError("op_point must be either a dictionary or an "
+                            "iterable of dictionaries.")
+        # Compose the system to be solved
+        mass_matrix = self.mass_matrix.col_join(-self.lam_coeffs.row_join(
+                zeros(k, k)))
+        force_matrix = self.forcing.col_join(self._f_cd)
+        # Sub in the operating point
+        mass_matrix = msubs(mass_matrix, op_point_dict)
+        force_matrix = msubs(force_matrix, op_point_dict)
+        # Solve for the multipliers
+        sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
+        if sol_type == 'dict':
+            return dict(zip(self.lam_vec, sol_list))
+        elif sol_type == 'Matrix':
+            return Matrix(sol_list)
+        else:
+            raise ValueError("Unknown sol_type {:}.".format(sol_type))
+
+    def rhs(self, inv_method=None, **kwargs):
+        """Returns equations that can be solved numerically.
+
+        Parameters
+        ==========
+
+        inv_method : str
+            The specific sympy inverse matrix calculation method to use. For a
+            list of valid methods, see
+            :meth:`~sympy.matrices.matrixbase.MatrixBase.inv`
+        """
+
+        if inv_method is None:
+            self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full)
+        else:
+            self._rhs = (self.mass_matrix_full.inv(inv_method,
+                         try_block_diag=True) * self.forcing_full)
+        return self._rhs
+
+    @property
+    def q(self):
+        return self._q
+
+    @property
+    def u(self):
+        return self._qdots
+
+    @property
+    def bodies(self):
+        return self._bodies
+
+    @property
+    def forcelist(self):
+        return self._forcelist
+
+    @property
+    def loads(self):
+        return self._forcelist

lib/python3.12/site-packages/sympy/physics/mechanics/loads.py

@@ -0,0 +1,177 @@
+from abc import ABC
+from collections import namedtuple
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.physics.vector import Vector, ReferenceFrame, Point
+
+__all__ = ['LoadBase', 'Force', 'Torque']
+
+
+class LoadBase(ABC, namedtuple('LoadBase', ['location', 'vector'])):
+    """Abstract base class for the various loading types."""
+
+    def __add__(self, other):
+        raise TypeError(f"unsupported operand type(s) for +: "
+                        f"'{self.__class__.__name__}' and "
+                        f"'{other.__class__.__name__}'")
+
+    def __mul__(self, other):
+        raise TypeError(f"unsupported operand type(s) for *: "
+                        f"'{self.__class__.__name__}' and "
+                        f"'{other.__class__.__name__}'")
+
+    __radd__ = __add__
+    __rmul__ = __mul__
+
+
+class Force(LoadBase):
+    """Force acting upon a point.
+
+    Explanation
+    ===========
+
+    A force is a vector that is bound to a line of action. This class stores
+    both a point, which lies on the line of action, and the vector. A tuple can
+    also be used, with the location as the first entry and the vector as second
+    entry.
+
+    Examples
+    ========
+
+    A force of magnitude 2 along N.x acting on a point Po can be created as
+    follows:
+
+    >>> from sympy.physics.mechanics import Point, ReferenceFrame, Force
+    >>> N = ReferenceFrame('N')
+    >>> Po = Point('Po')
+    >>> Force(Po, 2 * N.x)
+    (Po, 2*N.x)
+
+    If a body is supplied, then the center of mass of that body is used.
+
+    >>> from sympy.physics.mechanics import Particle
+    >>> P = Particle('P', point=Po)
+    >>> Force(P, 2 * N.x)
+    (Po, 2*N.x)
+
+    """
+
+    def __new__(cls, point, force):
+        if isinstance(point, BodyBase):
+            point = point.masscenter
+        if not isinstance(point, Point):
+            raise TypeError('Force location should be a Point.')
+        if not isinstance(force, Vector):
+            raise TypeError('Force vector should be a Vector.')
+        return super().__new__(cls, point, force)
+
+    def __repr__(self):
+        return (f'{self.__class__.__name__}(point={self.point}, '
+                f'force={self.force})')
+
+    @property
+    def point(self):
+        return self.location
+
+    @property
+    def force(self):
+        return self.vector
+
+
+class Torque(LoadBase):
+    """Torque acting upon a frame.
+
+    Explanation
+    ===========
+
+    A torque is a free vector that is acting on a reference frame, which is
+    associated with a rigid body. This class stores both the frame and the
+    vector. A tuple can also be used, with the location as the first item and
+    the vector as second item.
+
+    Examples
+    ========
+
+    A torque of magnitude 2 about N.x acting on a frame N can be created as
+    follows:
+
+    >>> from sympy.physics.mechanics import ReferenceFrame, Torque
+    >>> N = ReferenceFrame('N')
+    >>> Torque(N, 2 * N.x)
+    (N, 2*N.x)
+
+    If a body is supplied, then the frame fixed to that body is used.
+
+    >>> from sympy.physics.mechanics import RigidBody
+    >>> rb = RigidBody('rb', frame=N)
+    >>> Torque(rb, 2 * N.x)
+    (N, 2*N.x)
+
+    """
+
+    def __new__(cls, frame, torque):
+        if isinstance(frame, BodyBase):
+            frame = frame.frame
+        if not isinstance(frame, ReferenceFrame):
+            raise TypeError('Torque location should be a ReferenceFrame.')
+        if not isinstance(torque, Vector):
+            raise TypeError('Torque vector should be a Vector.')
+        return super().__new__(cls, frame, torque)
+
+    def __repr__(self):
+        return (f'{self.__class__.__name__}(frame={self.frame}, '
+                f'torque={self.torque})')
+
+    @property
+    def frame(self):
+        return self.location
+
+    @property
+    def torque(self):
+        return self.vector
+
+
+def gravity(acceleration, *bodies):
+    """
+    Returns a list of gravity forces given the acceleration
+    due to gravity and any number of particles or rigidbodies.
+
+    Example
+    =======
+
+    >>> from sympy.physics.mechanics import ReferenceFrame, Particle, RigidBody
+    >>> from sympy.physics.mechanics.loads import gravity
+    >>> from sympy import symbols
+    >>> N = ReferenceFrame('N')
+    >>> g = symbols('g')
+    >>> P = Particle('P')
+    >>> B = RigidBody('B')
+    >>> gravity(g*N.y, P, B)
+    [(P_masscenter, P_mass*g*N.y),
+     (B_masscenter, B_mass*g*N.y)]
+
+    """
+
+    gravity_force = []
+    for body in bodies:
+        if not isinstance(body, BodyBase):
+            raise TypeError(f'{type(body)} is not a body type')
+        gravity_force.append(Force(body.masscenter, body.mass * acceleration))
+    return gravity_force
+
+
+def _parse_load(load):
+    """Helper function to parse loads and convert tuples to load objects."""
+    if isinstance(load, LoadBase):
+        return load
+    elif isinstance(load, tuple):
+        if len(load) != 2:
+            raise ValueError(f'Load {load} should have a length of 2.')
+        if isinstance(load[0], Point):
+            return Force(load[0], load[1])
+        elif isinstance(load[0], ReferenceFrame):
+            return Torque(load[0], load[1])
+        else:
+            raise ValueError(f'Load not recognized. The load location {load[0]}'
+                             f' should either be a Point or a ReferenceFrame.')
+    raise TypeError(f'Load type {type(load)} not recognized as a load. It '
+                    f'should be a Force, Torque or tuple.')

lib/python3.12/site-packages/sympy/physics/mechanics/method.py

@@ -0,0 +1,39 @@
+from abc import ABC, abstractmethod
+
+class _Methods(ABC):
+    """Abstract Base Class for all methods."""
+
+    @abstractmethod
+    def q(self):
+        pass
+
+    @abstractmethod
+    def u(self):
+        pass
+
+    @abstractmethod
+    def bodies(self):
+        pass
+
+    @abstractmethod
+    def loads(self):
+        pass
+
+    @abstractmethod
+    def mass_matrix(self):
+        pass
+
+    @abstractmethod
+    def forcing(self):
+        pass
+
+    @abstractmethod
+    def mass_matrix_full(self):
+        pass
+
+    @abstractmethod
+    def forcing_full(self):
+        pass
+
+    def _form_eoms(self):
+        raise NotImplementedError("Subclasses must implement this.")

lib/python3.12/site-packages/sympy/physics/mechanics/pathway.py

@@ -0,0 +1,688 @@
+"""Implementations of pathways for use by actuators."""
+
+from abc import ABC, abstractmethod
+
+from sympy.core.singleton import S
+from sympy.physics.mechanics.loads import Force
+from sympy.physics.mechanics.wrapping_geometry import WrappingGeometryBase
+from sympy.physics.vector import Point, dynamicsymbols
+
+
+__all__ = ['PathwayBase', 'LinearPathway', 'ObstacleSetPathway',
+           'WrappingPathway']
+
+
+class PathwayBase(ABC):
+    """Abstract base class for all pathway classes to inherit from.
+
+    Notes
+    =====
+
+    Instances of this class cannot be directly instantiated by users. However,
+    it can be used to created custom pathway types through subclassing.
+
+    """
+
+    def __init__(self, *attachments):
+        """Initializer for ``PathwayBase``."""
+        self.attachments = attachments
+
+    @property
+    def attachments(self):
+        """The pair of points defining a pathway's ends."""
+        return self._attachments
+
+    @attachments.setter
+    def attachments(self, attachments):
+        if hasattr(self, '_attachments'):
+            msg = (
+                f'Can\'t set attribute `attachments` to {repr(attachments)} '
+                f'as it is immutable.'
+            )
+            raise AttributeError(msg)
+        if len(attachments) != 2:
+            msg = (
+                f'Value {repr(attachments)} passed to `attachments` was an '
+                f'iterable of length {len(attachments)}, must be an iterable '
+                f'of length 2.'
+            )
+            raise ValueError(msg)
+        for i, point in enumerate(attachments):
+            if not isinstance(point, Point):
+                msg = (
+                    f'Value {repr(point)} passed to `attachments` at index '
+                    f'{i} was of type {type(point)}, must be {Point}.'
+                )
+                raise TypeError(msg)
+        self._attachments = tuple(attachments)
+
+    @property
+    @abstractmethod
+    def length(self):
+        """An expression representing the pathway's length."""
+        pass
+
+    @property
+    @abstractmethod
+    def extension_velocity(self):
+        """An expression representing the pathway's extension velocity."""
+        pass
+
+    @abstractmethod
+    def to_loads(self, force):
+        """Loads required by the equations of motion method classes.
+
+        Explanation
+        ===========
+
+        ``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
+        passed to the ``loads`` parameters of its ``kanes_equations`` method
+        when constructing the equations of motion. This method acts as a
+        utility to produce the correctly-structred pairs of points and vectors
+        required so that these can be easily concatenated with other items in
+        the list of loads and passed to ``KanesMethod.kanes_equations``. These
+        loads are also in the correct form to also be passed to the other
+        equations of motion method classes, e.g. ``LagrangesMethod``.
+
+        """
+        pass
+
+    def __repr__(self):
+        """Default representation of a pathway."""
+        attachments = ', '.join(str(a) for a in self.attachments)
+        return f'{self.__class__.__name__}({attachments})'
+
+
+class LinearPathway(PathwayBase):
+    """Linear pathway between a pair of attachment points.
+
+    Explanation
+    ===========
+
+    A linear pathway forms a straight-line segment between two points and is
+    the simplest pathway that can be formed. It will not interact with any
+    other objects in the system, i.e. a ``LinearPathway`` will intersect other
+    objects to ensure that the path between its two ends (its attachments) is
+    the shortest possible.
+
+    A linear pathway is made up of two points that can move relative to each
+    other, and a pair of equal and opposite forces acting on the points. If the
+    positive time-varying Euclidean distance between the two points is defined,
+    then the "extension velocity" is the time derivative of this distance. The
+    extension velocity is positive when the two points are moving away from
+    each other and negative when moving closer to each other. The direction for
+    the force acting on either point is determined by constructing a unit
+    vector directed from the other point to this point. This establishes a sign
+    convention such that a positive force magnitude tends to push the points
+    apart. The following diagram shows the positive force sense and the
+    distance between the points::
+
+       P           Q
+       o<--- F --->o
+       |           |
+       |<--l(t)--->|
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import LinearPathway
+
+    To construct a pathway, two points are required to be passed to the
+    ``attachments`` parameter as a ``tuple``.
+
+    >>> from sympy.physics.mechanics import Point
+    >>> pA, pB = Point('pA'), Point('pB')
+    >>> linear_pathway = LinearPathway(pA, pB)
+    >>> linear_pathway
+    LinearPathway(pA, pB)
+
+    The pathway created above isn't very interesting without the positions and
+    velocities of its attachment points being described. Without this its not
+    possible to describe how the pathway moves, i.e. its length or its
+    extension velocity.
+
+    >>> from sympy.physics.mechanics import ReferenceFrame
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> N = ReferenceFrame('N')
+    >>> q = dynamicsymbols('q')
+    >>> pB.set_pos(pA, q*N.x)
+    >>> pB.pos_from(pA)
+    q(t)*N.x
+
+    A pathway's length can be accessed via its ``length`` attribute.
+
+    >>> linear_pathway.length
+    sqrt(q(t)**2)
+
+    Note how what appears to be an overly-complex expression is returned. This
+    is actually required as it ensures that a pathway's length is always
+    positive.
+
+    A pathway's extension velocity can be accessed similarly via its
+    ``extension_velocity`` attribute.
+
+    >>> linear_pathway.extension_velocity
+    sqrt(q(t)**2)*Derivative(q(t), t)/q(t)
+
+    Parameters
+    ==========
+
+    attachments : tuple[Point, Point]
+        Pair of ``Point`` objects between which the linear pathway spans.
+        Constructor expects two points to be passed, e.g.
+        ``LinearPathway(Point('pA'), Point('pB'))``. More or fewer points will
+        cause an error to be thrown.
+
+    """
+
+    def __init__(self, *attachments):
+        """Initializer for ``LinearPathway``.
+
+        Parameters
+        ==========
+
+        attachments : Point
+            Pair of ``Point`` objects between which the linear pathway spans.
+            Constructor expects two points to be passed, e.g.
+            ``LinearPathway(Point('pA'), Point('pB'))``. More or fewer points
+            will cause an error to be thrown.
+
+        """
+        super().__init__(*attachments)
+
+    @property
+    def length(self):
+        """Exact analytical expression for the pathway's length."""
+        return _point_pair_length(*self.attachments)
+
+    @property
+    def extension_velocity(self):
+        """Exact analytical expression for the pathway's extension velocity."""
+        return _point_pair_extension_velocity(*self.attachments)
+
+    def to_loads(self, force):
+        """Loads required by the equations of motion method classes.
+
+        Explanation
+        ===========
+
+        ``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
+        passed to the ``loads`` parameters of its ``kanes_equations`` method
+        when constructing the equations of motion. This method acts as a
+        utility to produce the correctly-structred pairs of points and vectors
+        required so that these can be easily concatenated with other items in
+        the list of loads and passed to ``KanesMethod.kanes_equations``. These
+        loads are also in the correct form to also be passed to the other
+        equations of motion method classes, e.g. ``LagrangesMethod``.
+
+        Examples
+        ========
+
+        The below example shows how to generate the loads produced in a linear
+        actuator that produces an expansile force ``F``. First, create a linear
+        actuator between two points separated by the coordinate ``q`` in the
+        ``x`` direction of the global frame ``N``.
+
+        >>> from sympy.physics.mechanics import (LinearPathway, Point,
+        ...     ReferenceFrame)
+        >>> from sympy.physics.vector import dynamicsymbols
+        >>> q = dynamicsymbols('q')
+        >>> N = ReferenceFrame('N')
+        >>> pA, pB = Point('pA'), Point('pB')
+        >>> pB.set_pos(pA, q*N.x)
+        >>> linear_pathway = LinearPathway(pA, pB)
+
+        Now create a symbol ``F`` to describe the magnitude of the (expansile)
+        force that will be produced along the pathway. The list of loads that
+        ``KanesMethod`` requires can be produced by calling the pathway's
+        ``to_loads`` method with ``F`` passed as the only argument.
+
+        >>> from sympy import symbols
+        >>> F = symbols('F')
+        >>> linear_pathway.to_loads(F)
+        [(pA, - F*q(t)/sqrt(q(t)**2)*N.x), (pB, F*q(t)/sqrt(q(t)**2)*N.x)]
+
+        Parameters
+        ==========
+
+        force : Expr
+            Magnitude of the force acting along the length of the pathway. As
+            per the sign conventions for the pathway length, pathway extension
+            velocity, and pair of point forces, if this ``Expr`` is positive
+            then the force will act to push the pair of points away from one
+            another (it is expansile).
+
+        """
+        relative_position = _point_pair_relative_position(*self.attachments)
+        loads = [
+            Force(self.attachments[0], -force*relative_position/self.length),
+            Force(self.attachments[-1], force*relative_position/self.length),
+        ]
+        return loads
+
+
+class ObstacleSetPathway(PathwayBase):
+    """Obstacle-set pathway between a set of attachment points.
+
+    Explanation
+    ===========
+
+    An obstacle-set pathway forms a series of straight-line segment between
+    pairs of consecutive points in a set of points. It is similiar to multiple
+    linear pathways joined end-to-end. It will not interact with any other
+    objects in the system, i.e. an ``ObstacleSetPathway`` will intersect other
+    objects to ensure that the path between its pairs of points (its
+    attachments) is the shortest possible.
+
+    Examples
+    ========
+
+    To construct an obstacle-set pathway, three or more points are required to
+    be passed to the ``attachments`` parameter as a ``tuple``.
+
+    >>> from sympy.physics.mechanics import ObstacleSetPathway, Point
+    >>> pA, pB, pC, pD = Point('pA'), Point('pB'), Point('pC'), Point('pD')
+    >>> obstacle_set_pathway = ObstacleSetPathway(pA, pB, pC, pD)
+    >>> obstacle_set_pathway
+    ObstacleSetPathway(pA, pB, pC, pD)
+
+    The pathway created above isn't very interesting without the positions and
+    velocities of its attachment points being described. Without this its not
+    possible to describe how the pathway moves, i.e. its length or its
+    extension velocity.
+
+    >>> from sympy import cos, sin
+    >>> from sympy.physics.mechanics import ReferenceFrame
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> N = ReferenceFrame('N')
+    >>> q = dynamicsymbols('q')
+    >>> pO = Point('pO')
+    >>> pA.set_pos(pO, N.y)
+    >>> pB.set_pos(pO, -N.x)
+    >>> pC.set_pos(pA, cos(q) * N.x - (sin(q) + 1) * N.y)
+    >>> pD.set_pos(pA, sin(q) * N.x + (cos(q) - 1) * N.y)
+    >>> pB.pos_from(pA)
+    - N.x - N.y
+    >>> pC.pos_from(pA)
+    cos(q(t))*N.x + (-sin(q(t)) - 1)*N.y
+    >>> pD.pos_from(pA)
+    sin(q(t))*N.x + (cos(q(t)) - 1)*N.y
+
+    A pathway's length can be accessed via its ``length`` attribute.
+
+    >>> obstacle_set_pathway.length.simplify()
+    sqrt(2)*(sqrt(cos(q(t)) + 1) + 2)
+
+    A pathway's extension velocity can be accessed similarly via its
+    ``extension_velocity`` attribute.
+
+    >>> obstacle_set_pathway.extension_velocity.simplify()
+    -sqrt(2)*sin(q(t))*Derivative(q(t), t)/(2*sqrt(cos(q(t)) + 1))
+
+    Parameters
+    ==========
+
+    attachments : tuple[Point, Point]
+        The set of ``Point`` objects that define the segmented obstacle-set
+        pathway.
+
+    """
+
+    def __init__(self, *attachments):
+        """Initializer for ``ObstacleSetPathway``.
+
+        Parameters
+        ==========
+
+        attachments : tuple[Point, ...]
+            The set of ``Point`` objects that define the segmented obstacle-set
+            pathway.
+
+        """
+        super().__init__(*attachments)
+
+    @property
+    def attachments(self):
+        """The set of points defining a pathway's segmented path."""
+        return self._attachments
+
+    @attachments.setter
+    def attachments(self, attachments):
+        if hasattr(self, '_attachments'):
+            msg = (
+                f'Can\'t set attribute `attachments` to {repr(attachments)} '
+                f'as it is immutable.'
+            )
+            raise AttributeError(msg)
+        if len(attachments) <= 2:
+            msg = (
+                f'Value {repr(attachments)} passed to `attachments` was an '
+                f'iterable of length {len(attachments)}, must be an iterable '
+                f'of length 3 or greater.'
+            )
+            raise ValueError(msg)
+        for i, point in enumerate(attachments):
+            if not isinstance(point, Point):
+                msg = (
+                    f'Value {repr(point)} passed to `attachments` at index '
+                    f'{i} was of type {type(point)}, must be {Point}.'
+                )
+                raise TypeError(msg)
+        self._attachments = tuple(attachments)
+
+    @property
+    def length(self):
+        """Exact analytical expression for the pathway's length."""
+        length = S.Zero
+        attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
+        for attachment_pair in attachment_pairs:
+            length += _point_pair_length(*attachment_pair)
+        return length
+
+    @property
+    def extension_velocity(self):
+        """Exact analytical expression for the pathway's extension velocity."""
+        extension_velocity = S.Zero
+        attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
+        for attachment_pair in attachment_pairs:
+            extension_velocity += _point_pair_extension_velocity(*attachment_pair)
+        return extension_velocity
+
+    def to_loads(self, force):
+        """Loads required by the equations of motion method classes.
+
+        Explanation
+        ===========
+
+        ``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
+        passed to the ``loads`` parameters of its ``kanes_equations`` method
+        when constructing the equations of motion. This method acts as a
+        utility to produce the correctly-structred pairs of points and vectors
+        required so that these can be easily concatenated with other items in
+        the list of loads and passed to ``KanesMethod.kanes_equations``. These
+        loads are also in the correct form to also be passed to the other
+        equations of motion method classes, e.g. ``LagrangesMethod``.
+
+        Examples
+        ========
+
+        The below example shows how to generate the loads produced in an
+        actuator that follows an obstacle-set pathway between four points and
+        produces an expansile force ``F``. First, create a pair of reference
+        frames, ``A`` and ``B``, in which the four points ``pA``, ``pB``,
+        ``pC``, and ``pD`` will be located. The first two points in frame ``A``
+        and the second two in frame ``B``. Frame ``B`` will also be oriented
+        such that it relates to ``A`` via a rotation of ``q`` about an axis
+        ``N.z`` in a global frame (``N.z``, ``A.z``, and ``B.z`` are parallel).
+
+        >>> from sympy.physics.mechanics import (ObstacleSetPathway, Point,
+        ...     ReferenceFrame)
+        >>> from sympy.physics.vector import dynamicsymbols
+        >>> q = dynamicsymbols('q')
+        >>> N = ReferenceFrame('N')
+        >>> N = ReferenceFrame('N')
+        >>> A = N.orientnew('A', 'axis', (0, N.x))
+        >>> B = A.orientnew('B', 'axis', (q, N.z))
+        >>> pO = Point('pO')
+        >>> pA, pB, pC, pD = Point('pA'), Point('pB'), Point('pC'), Point('pD')
+        >>> pA.set_pos(pO, A.x)
+        >>> pB.set_pos(pO, -A.y)
+        >>> pC.set_pos(pO, B.y)
+        >>> pD.set_pos(pO, B.x)
+        >>> obstacle_set_pathway = ObstacleSetPathway(pA, pB, pC, pD)
+
+        Now create a symbol ``F`` to describe the magnitude of the (expansile)
+        force that will be produced along the pathway. The list of loads that
+        ``KanesMethod`` requires can be produced by calling the pathway's
+        ``to_loads`` method with ``F`` passed as the only argument.
+
+        >>> from sympy import Symbol
+        >>> F = Symbol('F')
+        >>> obstacle_set_pathway.to_loads(F)
+        [(pA, sqrt(2)*F/2*A.x + sqrt(2)*F/2*A.y),
+         (pB, - sqrt(2)*F/2*A.x - sqrt(2)*F/2*A.y),
+         (pB, - F/sqrt(2*cos(q(t)) + 2)*A.y - F/sqrt(2*cos(q(t)) + 2)*B.y),
+         (pC, F/sqrt(2*cos(q(t)) + 2)*A.y + F/sqrt(2*cos(q(t)) + 2)*B.y),
+         (pC, - sqrt(2)*F/2*B.x + sqrt(2)*F/2*B.y),
+         (pD, sqrt(2)*F/2*B.x - sqrt(2)*F/2*B.y)]
+
+        Parameters
+        ==========
+
+        force : Expr
+            The force acting along the length of the pathway. It is assumed
+            that this ``Expr`` represents an expansile force.
+
+        """
+        loads = []
+        attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
+        for attachment_pair in attachment_pairs:
+            relative_position = _point_pair_relative_position(*attachment_pair)
+            length = _point_pair_length(*attachment_pair)
+            loads.extend([
+                Force(attachment_pair[0], -force*relative_position/length),
+                Force(attachment_pair[1], force*relative_position/length),
+            ])
+        return loads
+
+
+class WrappingPathway(PathwayBase):
+    """Pathway that wraps a geometry object.
+
+    Explanation
+    ===========
+
+    A wrapping pathway interacts with a geometry object and forms a path that
+    wraps smoothly along its surface. The wrapping pathway along the geometry
+    object will be the geodesic that the geometry object defines based on the
+    two points. It will not interact with any other objects in the system, i.e.
+    a ``WrappingPathway`` will intersect other objects to ensure that the path
+    between its two ends (its attachments) is the shortest possible.
+
+    To explain the sign conventions used for pathway length, extension
+    velocity, and direction of applied forces, we can ignore the geometry with
+    which the wrapping pathway interacts. A wrapping pathway is made up of two
+    points that can move relative to each other, and a pair of equal and
+    opposite forces acting on the points. If the positive time-varying
+    Euclidean distance between the two points is defined, then the "extension
+    velocity" is the time derivative of this distance. The extension velocity
+    is positive when the two points are moving away from each other and
+    negative when moving closer to each other. The direction for the force
+    acting on either point is determined by constructing a unit vector directed
+    from the other point to this point. This establishes a sign convention such
+    that a positive force magnitude tends to push the points apart. The
+    following diagram shows the positive force sense and the distance between
+    the points::
+
+       P           Q
+       o<--- F --->o
+       |           |
+       |<--l(t)--->|
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import WrappingPathway
+
+    To construct a wrapping pathway, like other pathways, a pair of points must
+    be passed, followed by an instance of a wrapping geometry class as a
+    keyword argument. We'll use a cylinder with radius ``r`` and its axis
+    parallel to ``N.x`` passing through a point ``pO``.
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import Point, ReferenceFrame, WrappingCylinder
+    >>> r = symbols('r')
+    >>> N = ReferenceFrame('N')
+    >>> pA, pB, pO = Point('pA'), Point('pB'), Point('pO')
+    >>> cylinder = WrappingCylinder(r, pO, N.x)
+    >>> wrapping_pathway = WrappingPathway(pA, pB, cylinder)
+    >>> wrapping_pathway
+    WrappingPathway(pA, pB, geometry=WrappingCylinder(radius=r, point=pO,
+        axis=N.x))
+
+    Parameters
+    ==========
+
+    attachment_1 : Point
+        First of the pair of ``Point`` objects between which the wrapping
+        pathway spans.
+    attachment_2 : Point
+        Second of the pair of ``Point`` objects between which the wrapping
+        pathway spans.
+    geometry : WrappingGeometryBase
+        Geometry about which the pathway wraps.
+
+    """
+
+    def __init__(self, attachment_1, attachment_2, geometry):
+        """Initializer for ``WrappingPathway``.
+
+        Parameters
+        ==========
+
+        attachment_1 : Point
+            First of the pair of ``Point`` objects between which the wrapping
+            pathway spans.
+        attachment_2 : Point
+            Second of the pair of ``Point`` objects between which the wrapping
+            pathway spans.
+        geometry : WrappingGeometryBase
+            Geometry about which the pathway wraps.
+            The geometry about which the pathway wraps.
+
+        """
+        super().__init__(attachment_1, attachment_2)
+        self.geometry = geometry
+
+    @property
+    def geometry(self):
+        """Geometry around which the pathway wraps."""
+        return self._geometry
+
+    @geometry.setter
+    def geometry(self, geometry):
+        if hasattr(self, '_geometry'):
+            msg = (
+                f'Can\'t set attribute `geometry` to {repr(geometry)} as it '
+                f'is immutable.'
+            )
+            raise AttributeError(msg)
+        if not isinstance(geometry, WrappingGeometryBase):
+            msg = (
+                f'Value {repr(geometry)} passed to `geometry` was of type '
+                f'{type(geometry)}, must be {WrappingGeometryBase}.'
+            )
+            raise TypeError(msg)
+        self._geometry = geometry
+
+    @property
+    def length(self):
+        """Exact analytical expression for the pathway's length."""
+        return self.geometry.geodesic_length(*self.attachments)
+
+    @property
+    def extension_velocity(self):
+        """Exact analytical expression for the pathway's extension velocity."""
+        return self.length.diff(dynamicsymbols._t)
+
+    def to_loads(self, force):
+        """Loads required by the equations of motion method classes.
+
+        Explanation
+        ===========
+
+        ``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
+        passed to the ``loads`` parameters of its ``kanes_equations`` method
+        when constructing the equations of motion. This method acts as a
+        utility to produce the correctly-structred pairs of points and vectors
+        required so that these can be easily concatenated with other items in
+        the list of loads and passed to ``KanesMethod.kanes_equations``. These
+        loads are also in the correct form to also be passed to the other
+        equations of motion method classes, e.g. ``LagrangesMethod``.
+
+        Examples
+        ========
+
+        The below example shows how to generate the loads produced in an
+        actuator that produces an expansile force ``F`` while wrapping around a
+        cylinder. First, create a cylinder with radius ``r`` and an axis
+        parallel to the ``N.z`` direction of the global frame ``N`` that also
+        passes through a point ``pO``.
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import (Point, ReferenceFrame,
+        ...     WrappingCylinder)
+        >>> N = ReferenceFrame('N')
+        >>> r = symbols('r', positive=True)
+        >>> pO = Point('pO')
+        >>> cylinder = WrappingCylinder(r, pO, N.z)
+
+        Create the pathway of the actuator using the ``WrappingPathway`` class,
+        defined to span between two points ``pA`` and ``pB``. Both points lie
+        on the surface of the cylinder and the location of ``pB`` is defined
+        relative to ``pA`` by the dynamics symbol ``q``.
+
+        >>> from sympy import cos, sin
+        >>> from sympy.physics.mechanics import WrappingPathway, dynamicsymbols
+        >>> q = dynamicsymbols('q')
+        >>> pA = Point('pA')
+        >>> pB = Point('pB')
+        >>> pA.set_pos(pO, r*N.x)
+        >>> pB.set_pos(pO, r*(cos(q)*N.x + sin(q)*N.y))
+        >>> pB.pos_from(pA)
+        (r*cos(q(t)) - r)*N.x + r*sin(q(t))*N.y
+        >>> pathway = WrappingPathway(pA, pB, cylinder)
+
+        Now create a symbol ``F`` to describe the magnitude of the (expansile)
+        force that will be produced along the pathway. The list of loads that
+        ``KanesMethod`` requires can be produced by calling the pathway's
+        ``to_loads`` method with ``F`` passed as the only argument.
+
+        >>> F = symbols('F')
+        >>> loads = pathway.to_loads(F)
+        >>> [load.__class__(load.location, load.vector.simplify()) for load in loads]
+        [(pA, F*N.y), (pB, F*sin(q(t))*N.x - F*cos(q(t))*N.y),
+         (pO, - F*sin(q(t))*N.x + F*(cos(q(t)) - 1)*N.y)]
+
+        Parameters
+        ==========
+
+        force : Expr
+            Magnitude of the force acting along the length of the pathway. It
+            is assumed that this ``Expr`` represents an expansile force.
+
+        """
+        pA, pB = self.attachments
+        pO = self.geometry.point
+        pA_force, pB_force = self.geometry.geodesic_end_vectors(pA, pB)
+        pO_force = -(pA_force + pB_force)
+
+        loads = [
+            Force(pA, force * pA_force),
+            Force(pB, force * pB_force),
+            Force(pO, force * pO_force),
+        ]
+        return loads
+
+    def __repr__(self):
+        """Representation of a ``WrappingPathway``."""
+        attachments = ', '.join(str(a) for a in self.attachments)
+        return (
+            f'{self.__class__.__name__}({attachments}, '
+            f'geometry={self.geometry})'
+        )
+
+
+def _point_pair_relative_position(point_1, point_2):
+    """The relative position between a pair of points."""
+    return point_2.pos_from(point_1)
+
+
+def _point_pair_length(point_1, point_2):
+    """The length of the direct linear path between two points."""
+    return _point_pair_relative_position(point_1, point_2).magnitude()
+
+
+def _point_pair_extension_velocity(point_1, point_2):
+    """The extension velocity of the direct linear path between two points."""
+    return _point_pair_length(point_1, point_2).diff(dynamicsymbols._t)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_actuator.py

@@ -0,0 +1,851 @@
+"""Tests for the ``sympy.physics.mechanics.actuator.py`` module."""
+
+import pytest
+
+from sympy import (
+    S,
+    Matrix,
+    Symbol,
+    SympifyError,
+    sqrt,
+    Abs
+)
+from sympy.physics.mechanics import (
+    ActuatorBase,
+    Force,
+    ForceActuator,
+    KanesMethod,
+    LinearDamper,
+    LinearPathway,
+    LinearSpring,
+    Particle,
+    PinJoint,
+    Point,
+    ReferenceFrame,
+    RigidBody,
+    TorqueActuator,
+    Vector,
+    dynamicsymbols,
+    DuffingSpring,
+)
+
+from sympy.core.expr import Expr as ExprType
+
+target = RigidBody('target')
+reaction = RigidBody('reaction')
+
+
+class TestForceActuator:
+
+    @pytest.fixture(autouse=True)
+    def _linear_pathway_fixture(self):
+        self.force = Symbol('F')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.pathway = LinearPathway(self.pA, self.pB)
+        self.q1 = dynamicsymbols('q1')
+        self.q2 = dynamicsymbols('q2')
+        self.q3 = dynamicsymbols('q3')
+        self.q1d = dynamicsymbols('q1', 1)
+        self.q2d = dynamicsymbols('q2', 1)
+        self.q3d = dynamicsymbols('q3', 1)
+        self.N = ReferenceFrame('N')
+
+    def test_is_actuator_base_subclass(self):
+        assert issubclass(ForceActuator, ActuatorBase)
+
+    @pytest.mark.parametrize(
+        'force, expected_force',
+        [
+            (1, S.One),
+            (S.One, S.One),
+            (Symbol('F'), Symbol('F')),
+            (dynamicsymbols('F'), dynamicsymbols('F')),
+            (Symbol('F')**2 + Symbol('F'), Symbol('F')**2 + Symbol('F')),
+        ]
+    )
+    def test_valid_constructor_force(self, force, expected_force):
+        instance = ForceActuator(force, self.pathway)
+        assert isinstance(instance, ForceActuator)
+        assert hasattr(instance, 'force')
+        assert isinstance(instance.force, ExprType)
+        assert instance.force == expected_force
+
+    @pytest.mark.parametrize('force', [None, 'F'])
+    def test_invalid_constructor_force_not_sympifyable(self, force):
+        with pytest.raises(SympifyError):
+            _ = ForceActuator(force, self.pathway)
+
+    @pytest.mark.parametrize(
+        'pathway',
+        [
+            LinearPathway(Point('pA'), Point('pB')),
+        ]
+    )
+    def test_valid_constructor_pathway(self, pathway):
+        instance = ForceActuator(self.force, pathway)
+        assert isinstance(instance, ForceActuator)
+        assert hasattr(instance, 'pathway')
+        assert isinstance(instance.pathway, LinearPathway)
+        assert instance.pathway == pathway
+
+    def test_invalid_constructor_pathway_not_pathway_base(self):
+        with pytest.raises(TypeError):
+            _ = ForceActuator(self.force, None)
+
+    @pytest.mark.parametrize(
+        'property_name, fixture_attr_name',
+        [
+            ('force', 'force'),
+            ('pathway', 'pathway'),
+        ]
+    )
+    def test_properties_are_immutable(self, property_name, fixture_attr_name):
+        instance = ForceActuator(self.force, self.pathway)
+        value = getattr(self, fixture_attr_name)
+        with pytest.raises(AttributeError):
+            setattr(instance, property_name, value)
+
+    def test_repr(self):
+        actuator = ForceActuator(self.force, self.pathway)
+        expected = "ForceActuator(F, LinearPathway(pA, pB))"
+        assert repr(actuator) == expected
+
+    def test_to_loads_static_pathway(self):
+        self.pB.set_pos(self.pA, 2*self.N.x)
+        actuator = ForceActuator(self.force, self.pathway)
+        expected = [
+            (self.pA, - self.force*self.N.x),
+            (self.pB, self.force*self.N.x),
+        ]
+        assert actuator.to_loads() == expected
+
+    def test_to_loads_2D_pathway(self):
+        self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
+        actuator = ForceActuator(self.force, self.pathway)
+        expected = [
+            (self.pA, - self.force*(self.q1/sqrt(self.q1**2))*self.N.x),
+            (self.pB, self.force*(self.q1/sqrt(self.q1**2))*self.N.x),
+        ]
+        assert actuator.to_loads() == expected
+
+    def test_to_loads_3D_pathway(self):
+        self.pB.set_pos(
+            self.pA,
+            self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
+        )
+        actuator = ForceActuator(self.force, self.pathway)
+        length = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
+        pO_force = (
+            - self.force*self.q1*self.N.x/length
+            + self.force*self.q2*self.N.y/length
+            - 2*self.force*self.q3*self.N.z/length
+        )
+        pI_force = (
+            self.force*self.q1*self.N.x/length
+            - self.force*self.q2*self.N.y/length
+            + 2*self.force*self.q3*self.N.z/length
+        )
+        expected = [
+            (self.pA, pO_force),
+            (self.pB, pI_force),
+        ]
+        assert actuator.to_loads() == expected
+
+
+class TestLinearSpring:
+
+    @pytest.fixture(autouse=True)
+    def _linear_spring_fixture(self):
+        self.stiffness = Symbol('k')
+        self.l = Symbol('l')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.pathway = LinearPathway(self.pA, self.pB)
+        self.q = dynamicsymbols('q')
+        self.N = ReferenceFrame('N')
+
+    def test_is_force_actuator_subclass(self):
+        assert issubclass(LinearSpring, ForceActuator)
+
+    def test_is_actuator_base_subclass(self):
+        assert issubclass(LinearSpring, ActuatorBase)
+
+    @pytest.mark.parametrize(
+        (
+            'stiffness, '
+            'expected_stiffness, '
+            'equilibrium_length, '
+            'expected_equilibrium_length, '
+            'force'
+        ),
+        [
+            (
+                1,
+                S.One,
+                0,
+                S.Zero,
+                -sqrt(dynamicsymbols('q')**2),
+            ),
+            (
+                Symbol('k'),
+                Symbol('k'),
+                0,
+                S.Zero,
+                -Symbol('k')*sqrt(dynamicsymbols('q')**2),
+            ),
+            (
+                Symbol('k'),
+                Symbol('k'),
+                S.Zero,
+                S.Zero,
+                -Symbol('k')*sqrt(dynamicsymbols('q')**2),
+            ),
+            (
+                Symbol('k'),
+                Symbol('k'),
+                Symbol('l'),
+                Symbol('l'),
+                -Symbol('k')*(sqrt(dynamicsymbols('q')**2) - Symbol('l')),
+            ),
+        ]
+    )
+    def test_valid_constructor(
+        self,
+        stiffness,
+        expected_stiffness,
+        equilibrium_length,
+        expected_equilibrium_length,
+        force,
+    ):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        spring = LinearSpring(stiffness, self.pathway, equilibrium_length)
+
+        assert isinstance(spring, LinearSpring)
+
+        assert hasattr(spring, 'stiffness')
+        assert isinstance(spring.stiffness, ExprType)
+        assert spring.stiffness == expected_stiffness
+
+        assert hasattr(spring, 'pathway')
+        assert isinstance(spring.pathway, LinearPathway)
+        assert spring.pathway == self.pathway
+
+        assert hasattr(spring, 'equilibrium_length')
+        assert isinstance(spring.equilibrium_length, ExprType)
+        assert spring.equilibrium_length == expected_equilibrium_length
+
+        assert hasattr(spring, 'force')
+        assert isinstance(spring.force, ExprType)
+        assert spring.force == force
+
+    @pytest.mark.parametrize('stiffness', [None, 'k'])
+    def test_invalid_constructor_stiffness_not_sympifyable(self, stiffness):
+        with pytest.raises(SympifyError):
+            _ = LinearSpring(stiffness, self.pathway, self.l)
+
+    def test_invalid_constructor_pathway_not_pathway_base(self):
+        with pytest.raises(TypeError):
+            _ = LinearSpring(self.stiffness, None, self.l)
+
+    @pytest.mark.parametrize('equilibrium_length', [None, 'l'])
+    def test_invalid_constructor_equilibrium_length_not_sympifyable(
+        self,
+        equilibrium_length,
+    ):
+        with pytest.raises(SympifyError):
+            _ = LinearSpring(self.stiffness, self.pathway, equilibrium_length)
+
+    @pytest.mark.parametrize(
+        'property_name, fixture_attr_name',
+        [
+            ('stiffness', 'stiffness'),
+            ('pathway', 'pathway'),
+            ('equilibrium_length', 'l'),
+        ]
+    )
+    def test_properties_are_immutable(self, property_name, fixture_attr_name):
+        spring = LinearSpring(self.stiffness, self.pathway, self.l)
+        value = getattr(self, fixture_attr_name)
+        with pytest.raises(AttributeError):
+            setattr(spring, property_name, value)
+
+    @pytest.mark.parametrize(
+        'equilibrium_length, expected',
+        [
+            (S.Zero, 'LinearSpring(k, LinearPathway(pA, pB))'),
+            (
+                Symbol('l'),
+                'LinearSpring(k, LinearPathway(pA, pB), equilibrium_length=l)',
+            ),
+        ]
+    )
+    def test_repr(self, equilibrium_length, expected):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        spring = LinearSpring(self.stiffness, self.pathway, equilibrium_length)
+        assert repr(spring) == expected
+
+    def test_to_loads(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        spring = LinearSpring(self.stiffness, self.pathway, self.l)
+        normal = self.q/sqrt(self.q**2)*self.N.x
+        pA_force = self.stiffness*(sqrt(self.q**2) - self.l)*normal
+        pB_force = -self.stiffness*(sqrt(self.q**2) - self.l)*normal
+        expected = [Force(self.pA, pA_force), Force(self.pB, pB_force)]
+        loads = spring.to_loads()
+
+        for load, (point, vector) in zip(loads, expected):
+            assert isinstance(load, Force)
+            assert load.point == point
+            assert (load.vector - vector).simplify() == 0
+
+
+class TestLinearDamper:
+
+    @pytest.fixture(autouse=True)
+    def _linear_damper_fixture(self):
+        self.damping = Symbol('c')
+        self.l = Symbol('l')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.pathway = LinearPathway(self.pA, self.pB)
+        self.q = dynamicsymbols('q')
+        self.dq = dynamicsymbols('q', 1)
+        self.u = dynamicsymbols('u')
+        self.N = ReferenceFrame('N')
+
+    def test_is_force_actuator_subclass(self):
+        assert issubclass(LinearDamper, ForceActuator)
+
+    def test_is_actuator_base_subclass(self):
+        assert issubclass(LinearDamper, ActuatorBase)
+
+    def test_valid_constructor(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        damper = LinearDamper(self.damping, self.pathway)
+
+        assert isinstance(damper, LinearDamper)
+
+        assert hasattr(damper, 'damping')
+        assert isinstance(damper.damping, ExprType)
+        assert damper.damping == self.damping
+
+        assert hasattr(damper, 'pathway')
+        assert isinstance(damper.pathway, LinearPathway)
+        assert damper.pathway == self.pathway
+
+    def test_valid_constructor_force(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        damper = LinearDamper(self.damping, self.pathway)
+
+        expected_force = -self.damping*sqrt(self.q**2)*self.dq/self.q
+        assert hasattr(damper, 'force')
+        assert isinstance(damper.force, ExprType)
+        assert damper.force == expected_force
+
+    @pytest.mark.parametrize('damping', [None, 'c'])
+    def test_invalid_constructor_damping_not_sympifyable(self, damping):
+        with pytest.raises(SympifyError):
+            _ = LinearDamper(damping, self.pathway)
+
+    def test_invalid_constructor_pathway_not_pathway_base(self):
+        with pytest.raises(TypeError):
+            _ = LinearDamper(self.damping, None)
+
+    @pytest.mark.parametrize(
+        'property_name, fixture_attr_name',
+        [
+            ('damping', 'damping'),
+            ('pathway', 'pathway'),
+        ]
+    )
+    def test_properties_are_immutable(self, property_name, fixture_attr_name):
+        damper = LinearDamper(self.damping, self.pathway)
+        value = getattr(self, fixture_attr_name)
+        with pytest.raises(AttributeError):
+            setattr(damper, property_name, value)
+
+    def test_repr(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        damper = LinearDamper(self.damping, self.pathway)
+        expected = 'LinearDamper(c, LinearPathway(pA, pB))'
+        assert repr(damper) == expected
+
+    def test_to_loads(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        damper = LinearDamper(self.damping, self.pathway)
+        direction = self.q**2/self.q**2*self.N.x
+        pA_force = self.damping*self.dq*direction
+        pB_force = -self.damping*self.dq*direction
+        expected = [Force(self.pA, pA_force), Force(self.pB, pB_force)]
+        assert damper.to_loads() == expected
+
+
+class TestForcedMassSpringDamperModel():
+    r"""A single degree of freedom translational forced mass-spring-damper.
+
+    Notes
+    =====
+
+    This system is well known to have the governing equation:
+
+    .. math::
+        m \ddot{x} = F - k x - c \dot{x}
+
+    where $F$ is an externally applied force, $m$ is the mass of the particle
+    to which the spring and damper are attached, $k$ is the spring's stiffness,
+    $c$ is the dampers damping coefficient, and $x$ is the generalized
+    coordinate representing the system's single (translational) degree of
+    freedom.
+
+    """
+
+    @pytest.fixture(autouse=True)
+    def _force_mass_spring_damper_model_fixture(self):
+        self.m = Symbol('m')
+        self.k = Symbol('k')
+        self.c = Symbol('c')
+        self.F = Symbol('F')
+
+        self.q = dynamicsymbols('q')
+        self.dq = dynamicsymbols('q', 1)
+        self.u = dynamicsymbols('u')
+
+        self.frame = ReferenceFrame('N')
+        self.origin = Point('pO')
+        self.origin.set_vel(self.frame, 0)
+
+        self.attachment = Point('pA')
+        self.attachment.set_pos(self.origin, self.q*self.frame.x)
+
+        self.mass = Particle('mass', self.attachment, self.m)
+        self.pathway = LinearPathway(self.origin, self.attachment)
+
+        self.kanes_method = KanesMethod(
+            self.frame,
+            q_ind=[self.q],
+            u_ind=[self.u],
+            kd_eqs=[self.dq - self.u],
+        )
+        self.bodies = [self.mass]
+
+        self.mass_matrix = Matrix([[self.m]])
+        self.forcing = Matrix([[self.F - self.c*self.u - self.k*self.q]])
+
+    def test_force_acuator(self):
+        stiffness = -self.k*self.pathway.length
+        spring = ForceActuator(stiffness, self.pathway)
+        damping = -self.c*self.pathway.extension_velocity
+        damper = ForceActuator(damping, self.pathway)
+
+        loads = [
+            (self.attachment, self.F*self.frame.x),
+            *spring.to_loads(),
+            *damper.to_loads(),
+        ]
+        self.kanes_method.kanes_equations(self.bodies, loads)
+
+        assert self.kanes_method.mass_matrix == self.mass_matrix
+        assert self.kanes_method.forcing == self.forcing
+
+    def test_linear_spring_linear_damper(self):
+        spring = LinearSpring(self.k, self.pathway)
+        damper = LinearDamper(self.c, self.pathway)
+
+        loads = [
+            (self.attachment, self.F*self.frame.x),
+            *spring.to_loads(),
+            *damper.to_loads(),
+        ]
+        self.kanes_method.kanes_equations(self.bodies, loads)
+
+        assert self.kanes_method.mass_matrix == self.mass_matrix
+        assert self.kanes_method.forcing == self.forcing
+
+
+class TestTorqueActuator:
+
+    @pytest.fixture(autouse=True)
+    def _torque_actuator_fixture(self):
+        self.torque = Symbol('T')
+        self.N = ReferenceFrame('N')
+        self.A = ReferenceFrame('A')
+        self.axis = self.N.z
+        self.target = RigidBody('target', frame=self.N)
+        self.reaction = RigidBody('reaction', frame=self.A)
+
+    def test_is_actuator_base_subclass(self):
+        assert issubclass(TorqueActuator, ActuatorBase)
+
+    @pytest.mark.parametrize(
+        'torque',
+        [
+            Symbol('T'),
+            dynamicsymbols('T'),
+            Symbol('T')**2 + Symbol('T'),
+        ]
+    )
+    @pytest.mark.parametrize(
+        'target_frame, reaction_frame',
+        [
+            (target.frame, reaction.frame),
+            (target, reaction.frame),
+            (target.frame, reaction),
+            (target, reaction),
+        ]
+    )
+    def test_valid_constructor_with_reaction(
+        self,
+        torque,
+        target_frame,
+        reaction_frame,
+    ):
+        instance = TorqueActuator(
+            torque,
+            self.axis,
+            target_frame,
+            reaction_frame,
+        )
+        assert isinstance(instance, TorqueActuator)
+
+        assert hasattr(instance, 'torque')
+        assert isinstance(instance.torque, ExprType)
+        assert instance.torque == torque
+
+        assert hasattr(instance, 'axis')
+        assert isinstance(instance.axis, Vector)
+        assert instance.axis == self.axis
+
+        assert hasattr(instance, 'target_frame')
+        assert isinstance(instance.target_frame, ReferenceFrame)
+        assert instance.target_frame == target.frame
+
+        assert hasattr(instance, 'reaction_frame')
+        assert isinstance(instance.reaction_frame, ReferenceFrame)
+        assert instance.reaction_frame == reaction.frame
+
+    @pytest.mark.parametrize(
+        'torque',
+        [
+            Symbol('T'),
+            dynamicsymbols('T'),
+            Symbol('T')**2 + Symbol('T'),
+        ]
+    )
+    @pytest.mark.parametrize('target_frame', [target.frame, target])
+    def test_valid_constructor_without_reaction(self, torque, target_frame):
+        instance = TorqueActuator(torque, self.axis, target_frame)
+        assert isinstance(instance, TorqueActuator)
+
+        assert hasattr(instance, 'torque')
+        assert isinstance(instance.torque, ExprType)
+        assert instance.torque == torque
+
+        assert hasattr(instance, 'axis')
+        assert isinstance(instance.axis, Vector)
+        assert instance.axis == self.axis
+
+        assert hasattr(instance, 'target_frame')
+        assert isinstance(instance.target_frame, ReferenceFrame)
+        assert instance.target_frame == target.frame
+
+        assert hasattr(instance, 'reaction_frame')
+        assert instance.reaction_frame is None
+
+    @pytest.mark.parametrize('torque', [None, 'T'])
+    def test_invalid_constructor_torque_not_sympifyable(self, torque):
+        with pytest.raises(SympifyError):
+            _ = TorqueActuator(torque, self.axis, self.target)
+
+    @pytest.mark.parametrize('axis', [Symbol('a'), dynamicsymbols('a')])
+    def test_invalid_constructor_axis_not_vector(self, axis):
+        with pytest.raises(TypeError):
+            _ = TorqueActuator(self.torque, axis, self.target, self.reaction)
+
+    @pytest.mark.parametrize(
+        'frames',
+        [
+            (None, ReferenceFrame('child')),
+            (ReferenceFrame('parent'), True),
+            (None, RigidBody('child')),
+            (RigidBody('parent'), True),
+        ]
+    )
+    def test_invalid_constructor_frames_not_frame(self, frames):
+        with pytest.raises(TypeError):
+            _ = TorqueActuator(self.torque, self.axis, *frames)
+
+    @pytest.mark.parametrize(
+        'property_name, fixture_attr_name',
+        [
+            ('torque', 'torque'),
+            ('axis', 'axis'),
+            ('target_frame', 'target'),
+            ('reaction_frame', 'reaction'),
+        ]
+    )
+    def test_properties_are_immutable(self, property_name, fixture_attr_name):
+        actuator = TorqueActuator(
+            self.torque,
+            self.axis,
+            self.target,
+            self.reaction,
+        )
+        value = getattr(self, fixture_attr_name)
+        with pytest.raises(AttributeError):
+            setattr(actuator, property_name, value)
+
+    def test_repr_without_reaction(self):
+        actuator = TorqueActuator(self.torque, self.axis, self.target)
+        expected = 'TorqueActuator(T, axis=N.z, target_frame=N)'
+        assert repr(actuator) == expected
+
+    def test_repr_with_reaction(self):
+        actuator = TorqueActuator(
+            self.torque,
+            self.axis,
+            self.target,
+            self.reaction,
+        )
+        expected = 'TorqueActuator(T, axis=N.z, target_frame=N, reaction_frame=A)'
+        assert repr(actuator) == expected
+
+    def test_at_pin_joint_constructor(self):
+        pin_joint = PinJoint(
+            'pin',
+            self.target,
+            self.reaction,
+            coordinates=dynamicsymbols('q'),
+            speeds=dynamicsymbols('u'),
+            parent_interframe=self.N,
+            joint_axis=self.axis,
+        )
+        instance = TorqueActuator.at_pin_joint(self.torque, pin_joint)
+        assert isinstance(instance, TorqueActuator)
+
+        assert hasattr(instance, 'torque')
+        assert isinstance(instance.torque, ExprType)
+        assert instance.torque == self.torque
+
+        assert hasattr(instance, 'axis')
+        assert isinstance(instance.axis, Vector)
+        assert instance.axis == self.axis
+
+        assert hasattr(instance, 'target_frame')
+        assert isinstance(instance.target_frame, ReferenceFrame)
+        assert instance.target_frame == self.A
+
+        assert hasattr(instance, 'reaction_frame')
+        assert isinstance(instance.reaction_frame, ReferenceFrame)
+        assert instance.reaction_frame == self.N
+
+    def test_at_pin_joint_pin_joint_not_pin_joint_invalid(self):
+        with pytest.raises(TypeError):
+            _ = TorqueActuator.at_pin_joint(self.torque, Symbol('pin'))
+
+    def test_to_loads_without_reaction(self):
+        actuator = TorqueActuator(self.torque, self.axis, self.target)
+        expected = [
+            (self.N, self.torque*self.axis),
+        ]
+        assert actuator.to_loads() == expected
+
+    def test_to_loads_with_reaction(self):
+        actuator = TorqueActuator(
+            self.torque,
+            self.axis,
+            self.target,
+            self.reaction,
+        )
+        expected = [
+            (self.N, self.torque*self.axis),
+            (self.A, - self.torque*self.axis),
+        ]
+        assert actuator.to_loads() == expected
+
+
+class NonSympifyable:
+    pass
+
+
+class TestDuffingSpring:
+    @pytest.fixture(autouse=True)
+    # Set up common vairables that will be used in multiple tests
+    def _duffing_spring_fixture(self):
+        self.linear_stiffness = Symbol('beta')
+        self.nonlinear_stiffness = Symbol('alpha')
+        self.equilibrium_length = Symbol('l')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.pathway = LinearPathway(self.pA, self.pB)
+        self.q = dynamicsymbols('q')
+        self.N = ReferenceFrame('N')
+
+    # Simples tests to check that DuffingSpring is a subclass of ForceActuator and ActuatorBase
+    def test_is_force_actuator_subclass(self):
+        assert issubclass(DuffingSpring, ForceActuator)
+
+    def test_is_actuator_base_subclass(self):
+        assert issubclass(DuffingSpring, ActuatorBase)
+
+    @pytest.mark.parametrize(
+    # Create parametrized tests that allows running the same test function multiple times with different sets of arguments
+    (
+        'linear_stiffness,  '
+        'expected_linear_stiffness,  '
+        'nonlinear_stiffness,   '
+        'expected_nonlinear_stiffness,  '
+        'equilibrium_length,    '
+        'expected_equilibrium_length,   '
+        'force'
+    ),
+    [
+            (
+                1,
+                S.One,
+                1,
+                S.One,
+                0,
+                S.Zero,
+                -sqrt(dynamicsymbols('q')**2)-(sqrt(dynamicsymbols('q')**2))**3,
+            ),
+            (
+                Symbol('beta'),
+                Symbol('beta'),
+                Symbol('alpha'),
+                Symbol('alpha'),
+                0,
+                S.Zero,
+                -Symbol('beta')*sqrt(dynamicsymbols('q')**2)-Symbol('alpha')*(sqrt(dynamicsymbols('q')**2))**3,
+            ),
+            (
+                Symbol('beta'),
+                Symbol('beta'),
+                Symbol('alpha'),
+                Symbol('alpha'),
+                S.Zero,
+                S.Zero,
+                -Symbol('beta')*sqrt(dynamicsymbols('q')**2)-Symbol('alpha')*(sqrt(dynamicsymbols('q')**2))**3,
+            ),
+            (
+                Symbol('beta'),
+                Symbol('beta'),
+                Symbol('alpha'),
+                Symbol('alpha'),
+                Symbol('l'),
+                Symbol('l'),
+                -Symbol('beta') * (sqrt(dynamicsymbols('q')**2) - Symbol('l')) - Symbol('alpha') * (sqrt(dynamicsymbols('q')**2) - Symbol('l'))**3,
+            ),
+        ]
+    )
+
+    # Check if DuffingSpring correctly inializes its attributes
+    # It tests various combinations of linear & nonlinear stiffness, equilibriun length, and the resulting force expression
+    def test_valid_constructor(
+        self,
+        linear_stiffness,
+        expected_linear_stiffness,
+        nonlinear_stiffness,
+        expected_nonlinear_stiffness,
+        equilibrium_length,
+        expected_equilibrium_length,
+        force,
+    ):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        spring = DuffingSpring(linear_stiffness, nonlinear_stiffness, self.pathway, equilibrium_length)
+
+        assert isinstance(spring, DuffingSpring)
+
+        assert hasattr(spring, 'linear_stiffness')
+        assert isinstance(spring.linear_stiffness, ExprType)
+        assert spring.linear_stiffness == expected_linear_stiffness
+
+        assert hasattr(spring, 'nonlinear_stiffness')
+        assert isinstance(spring.nonlinear_stiffness, ExprType)
+        assert spring.nonlinear_stiffness == expected_nonlinear_stiffness
+
+        assert hasattr(spring, 'pathway')
+        assert isinstance(spring.pathway, LinearPathway)
+        assert spring.pathway == self.pathway
+
+        assert hasattr(spring, 'equilibrium_length')
+        assert isinstance(spring.equilibrium_length, ExprType)
+        assert spring.equilibrium_length == expected_equilibrium_length
+
+        assert hasattr(spring, 'force')
+        assert isinstance(spring.force, ExprType)
+        assert spring.force == force
+
+    @pytest.mark.parametrize('linear_stiffness', [None, NonSympifyable()])
+    def test_invalid_constructor_linear_stiffness_not_sympifyable(self, linear_stiffness):
+        with pytest.raises(SympifyError):
+            _ = DuffingSpring(linear_stiffness, self.nonlinear_stiffness, self.pathway, self.equilibrium_length)
+
+    @pytest.mark.parametrize('nonlinear_stiffness', [None, NonSympifyable()])
+    def test_invalid_constructor_nonlinear_stiffness_not_sympifyable(self, nonlinear_stiffness):
+        with pytest.raises(SympifyError):
+            _ = DuffingSpring(self.linear_stiffness, nonlinear_stiffness, self.pathway, self.equilibrium_length)
+
+    def test_invalid_constructor_pathway_not_pathway_base(self):
+        with pytest.raises(TypeError):
+            _ = DuffingSpring(self.linear_stiffness, self.nonlinear_stiffness, NonSympifyable(), self.equilibrium_length)
+
+    @pytest.mark.parametrize('equilibrium_length', [None, NonSympifyable()])
+    def test_invalid_constructor_equilibrium_length_not_sympifyable(self, equilibrium_length):
+        with pytest.raises(SympifyError):
+            _ = DuffingSpring(self.linear_stiffness, self.nonlinear_stiffness, self.pathway, equilibrium_length)
+
+    @pytest.mark.parametrize(
+        'property_name, fixture_attr_name',
+        [
+            ('linear_stiffness', 'linear_stiffness'),
+            ('nonlinear_stiffness', 'nonlinear_stiffness'),
+            ('pathway', 'pathway'),
+            ('equilibrium_length', 'equilibrium_length')
+        ]
+    )
+    # Check if certain properties of DuffingSpring object are immutable after initialization
+    # Ensure that once DuffingSpring is created, its key properties cannot be changed
+    def test_properties_are_immutable(self, property_name, fixture_attr_name):
+        spring = DuffingSpring(self.linear_stiffness, self.nonlinear_stiffness, self.pathway, self.equilibrium_length)
+        with pytest.raises(AttributeError):
+            setattr(spring, property_name, getattr(self, fixture_attr_name))
+
+    @pytest.mark.parametrize(
+        'equilibrium_length, expected',
+        [
+            (0, 'DuffingSpring(beta, alpha, LinearPathway(pA, pB), equilibrium_length=0)'),
+            (Symbol('l'), 'DuffingSpring(beta, alpha, LinearPathway(pA, pB), equilibrium_length=l)'),
+        ]
+    )
+    # Check the __repr__ method of DuffingSpring class
+    # Check if the actual string representation of DuffingSpring instance matches the expected string for each provided parameter values
+    def test_repr(self, equilibrium_length, expected):
+        spring = DuffingSpring(self.linear_stiffness, self.nonlinear_stiffness, self.pathway, equilibrium_length)
+        assert repr(spring) == expected
+
+    def test_to_loads(self):
+        self.pB.set_pos(self.pA, self.q*self.N.x)
+        spring = DuffingSpring(self.linear_stiffness, self.nonlinear_stiffness, self.pathway, self.equilibrium_length)
+
+        # Calculate the displacement from the equilibrium length
+        displacement = self.q - self.equilibrium_length
+
+        # Make sure this matches the computation in DuffingSpring class
+        force = -self.linear_stiffness * displacement - self.nonlinear_stiffness * displacement**3
+
+        # The expected loads on pA and pB due to the spring
+        expected_loads = [Force(self.pA, force * self.N.x), Force(self.pB, -force * self.N.x)]
+
+        # Compare expected loads to what is returned from DuffingSpring.to_loads()
+        calculated_loads = spring.to_loads()
+        for calculated, expected in zip(calculated_loads, expected_loads):
+            assert calculated.point == expected.point
+            for dim in self.N:  # Assuming self.N is the reference frame
+                calculated_component = calculated.vector.dot(dim)
+                expected_component = expected.vector.dot(dim)
+                # Substitute all symbols with numeric values
+                substitutions = {self.q: 1, Symbol('l'): 1, Symbol('alpha'): 1, Symbol('beta'): 1}  # Add other necessary symbols as needed
+                diff = (calculated_component - expected_component).subs(substitutions).evalf()
+                # Check if the absolute value of the difference is below a threshold
+                assert Abs(diff) < 1e-9, f"The forces do not match. Difference: {diff}"

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_body.py

@@ -0,0 +1,340 @@
+from sympy import (Symbol, symbols, sin, cos, Matrix, zeros,
+                                simplify)
+from sympy.physics.vector import Point, ReferenceFrame, dynamicsymbols, Dyadic
+from sympy.physics.mechanics import inertia, Body
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+
+
+def test_default():
+    with warns_deprecated_sympy():
+        body = Body('body')
+    assert body.name == 'body'
+    assert body.loads == []
+    point = Point('body_masscenter')
+    point.set_vel(body.frame, 0)
+    com = body.masscenter
+    frame = body.frame
+    assert com.vel(frame) == point.vel(frame)
+    assert body.mass == Symbol('body_mass')
+    ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
+    ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
+    assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
+                            body.masscenter)
+
+
+def test_custom_rigid_body():
+    # Body with RigidBody.
+    rigidbody_masscenter = Point('rigidbody_masscenter')
+    rigidbody_mass = Symbol('rigidbody_mass')
+    rigidbody_frame = ReferenceFrame('rigidbody_frame')
+    body_inertia = inertia(rigidbody_frame, 1, 0, 0)
+    with warns_deprecated_sympy():
+        rigid_body = Body('rigidbody_body', rigidbody_masscenter,
+                          rigidbody_mass, rigidbody_frame, body_inertia)
+    com = rigid_body.masscenter
+    frame = rigid_body.frame
+    rigidbody_masscenter.set_vel(rigidbody_frame, 0)
+    assert com.vel(frame) == rigidbody_masscenter.vel(frame)
+    assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)
+
+    assert rigid_body.mass == rigidbody_mass
+    assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)
+
+    assert rigid_body.is_rigidbody
+
+    assert hasattr(rigid_body, 'masscenter')
+    assert hasattr(rigid_body, 'mass')
+    assert hasattr(rigid_body, 'frame')
+    assert hasattr(rigid_body, 'inertia')
+
+
+def test_particle_body():
+    #  Body with Particle
+    particle_masscenter = Point('particle_masscenter')
+    particle_mass = Symbol('particle_mass')
+    particle_frame = ReferenceFrame('particle_frame')
+    with warns_deprecated_sympy():
+        particle_body = Body('particle_body', particle_masscenter,
+                             particle_mass, particle_frame)
+    com = particle_body.masscenter
+    frame = particle_body.frame
+    particle_masscenter.set_vel(particle_frame, 0)
+    assert com.vel(frame) == particle_masscenter.vel(frame)
+    assert com.pos_from(com) == particle_masscenter.pos_from(com)
+
+    assert particle_body.mass == particle_mass
+    assert not hasattr(particle_body, "_inertia")
+    assert hasattr(particle_body, 'frame')
+    assert hasattr(particle_body, 'masscenter')
+    assert hasattr(particle_body, 'mass')
+    assert particle_body.inertia == (Dyadic(0), particle_body.masscenter)
+    assert particle_body.central_inertia == Dyadic(0)
+    assert not particle_body.is_rigidbody
+
+    particle_body.central_inertia = inertia(particle_frame, 1, 1, 1)
+    assert particle_body.central_inertia == inertia(particle_frame, 1, 1, 1)
+    assert particle_body.is_rigidbody
+
+    with warns_deprecated_sympy():
+        particle_body = Body('particle_body', mass=particle_mass)
+    assert not particle_body.is_rigidbody
+    point = particle_body.masscenter.locatenew('point', particle_body.x)
+    point_inertia = particle_mass * inertia(particle_body.frame, 0, 1, 1)
+    particle_body.inertia = (point_inertia, point)
+    assert particle_body.inertia == (point_inertia, point)
+    assert particle_body.central_inertia == Dyadic(0)
+    assert particle_body.is_rigidbody
+
+
+def test_particle_body_add_force():
+    #  Body with Particle
+    particle_masscenter = Point('particle_masscenter')
+    particle_mass = Symbol('particle_mass')
+    particle_frame = ReferenceFrame('particle_frame')
+    with warns_deprecated_sympy():
+        particle_body = Body('particle_body', particle_masscenter,
+                             particle_mass, particle_frame)
+
+    a = Symbol('a')
+    force_vector = a * particle_body.frame.x
+    particle_body.apply_force(force_vector, particle_body.masscenter)
+    assert len(particle_body.loads) == 1
+    point = particle_body.masscenter.locatenew(
+        particle_body._name + '_point0', 0)
+    point.set_vel(particle_body.frame, 0)
+    force_point = particle_body.loads[0][0]
+
+    frame = particle_body.frame
+    assert force_point.vel(frame) == point.vel(frame)
+    assert force_point.pos_from(force_point) == point.pos_from(force_point)
+
+    assert particle_body.loads[0][1] == force_vector
+
+
+def test_body_add_force():
+    # Body with RigidBody.
+    rigidbody_masscenter = Point('rigidbody_masscenter')
+    rigidbody_mass = Symbol('rigidbody_mass')
+    rigidbody_frame = ReferenceFrame('rigidbody_frame')
+    body_inertia = inertia(rigidbody_frame, 1, 0, 0)
+    with warns_deprecated_sympy():
+        rigid_body = Body('rigidbody_body', rigidbody_masscenter,
+                          rigidbody_mass, rigidbody_frame, body_inertia)
+
+    l = Symbol('l')
+    Fa = Symbol('Fa')
+    point = rigid_body.masscenter.locatenew(
+        'rigidbody_body_point0',
+        l * rigid_body.frame.x)
+    point.set_vel(rigid_body.frame, 0)
+    force_vector = Fa * rigid_body.frame.z
+    # apply_force with point
+    rigid_body.apply_force(force_vector, point)
+    assert len(rigid_body.loads) == 1
+    force_point = rigid_body.loads[0][0]
+    frame = rigid_body.frame
+    assert force_point.vel(frame) == point.vel(frame)
+    assert force_point.pos_from(force_point) == point.pos_from(force_point)
+    assert rigid_body.loads[0][1] == force_vector
+    # apply_force without point
+    rigid_body.apply_force(force_vector)
+    assert len(rigid_body.loads) == 2
+    assert rigid_body.loads[1][1] == force_vector
+    # passing something else than point
+    raises(TypeError, lambda: rigid_body.apply_force(force_vector,  0))
+    raises(TypeError, lambda: rigid_body.apply_force(0))
+
+def test_body_add_torque():
+    with warns_deprecated_sympy():
+        body = Body('body')
+    torque_vector = body.frame.x
+    body.apply_torque(torque_vector)
+
+    assert len(body.loads) == 1
+    assert body.loads[0] == (body.frame, torque_vector)
+    raises(TypeError, lambda: body.apply_torque(0))
+
+def test_body_masscenter_vel():
+    with warns_deprecated_sympy():
+        A = Body('A')
+    N = ReferenceFrame('N')
+    with warns_deprecated_sympy():
+        B = Body('B', frame=N)
+    A.masscenter.set_vel(N, N.z)
+    assert A.masscenter_vel(B) == N.z
+    assert A.masscenter_vel(N) == N.z
+
+def test_body_ang_vel():
+    with warns_deprecated_sympy():
+        A = Body('A')
+    N = ReferenceFrame('N')
+    with warns_deprecated_sympy():
+        B = Body('B', frame=N)
+    A.frame.set_ang_vel(N, N.y)
+    assert A.ang_vel_in(B) == N.y
+    assert B.ang_vel_in(A) == -N.y
+    assert A.ang_vel_in(N) == N.y
+
+def test_body_dcm():
+    with warns_deprecated_sympy():
+        A = Body('A')
+        B = Body('B')
+    A.frame.orient_axis(B.frame, B.frame.z, 10)
+    assert A.dcm(B) == Matrix([[cos(10), sin(10), 0], [-sin(10), cos(10), 0], [0, 0, 1]])
+    assert A.dcm(B.frame) == Matrix([[cos(10), sin(10), 0], [-sin(10), cos(10), 0], [0, 0, 1]])
+
+def test_body_axis():
+    N = ReferenceFrame('N')
+    with warns_deprecated_sympy():
+        B = Body('B', frame=N)
+    assert B.x == N.x
+    assert B.y == N.y
+    assert B.z == N.z
+
+def test_apply_force_multiple_one_point():
+    a, b = symbols('a b')
+    P = Point('P')
+    with warns_deprecated_sympy():
+        B = Body('B')
+    f1 = a*B.x
+    f2 = b*B.y
+    B.apply_force(f1, P)
+    assert B.loads == [(P, f1)]
+    B.apply_force(f2, P)
+    assert B.loads == [(P, f1+f2)]
+
+def test_apply_force():
+    f, g = symbols('f g')
+    q, x, v1, v2 = dynamicsymbols('q x v1 v2')
+    P1 = Point('P1')
+    P2 = Point('P2')
+    with warns_deprecated_sympy():
+        B1 = Body('B1')
+        B2 = Body('B2')
+    N = ReferenceFrame('N')
+
+    P1.set_vel(B1.frame, v1*B1.x)
+    P2.set_vel(B2.frame, v2*B2.x)
+    force = f*q*N.z # time varying force
+
+    B1.apply_force(force, P1, B2, P2) #applying equal and opposite force on moving points
+    assert B1.loads == [(P1, force)]
+    assert B2.loads == [(P2, -force)]
+
+    g1 = B1.mass*g*N.y
+    g2 = B2.mass*g*N.y
+
+    B1.apply_force(g1) #applying gravity on B1 masscenter
+    B2.apply_force(g2) #applying gravity on B2 masscenter
+
+    assert B1.loads == [(P1,force), (B1.masscenter, g1)]
+    assert B2.loads == [(P2, -force), (B2.masscenter, g2)]
+
+    force2 = x*N.x
+
+    B1.apply_force(force2, reaction_body=B2) #Applying time varying force on masscenter
+
+    assert B1.loads == [(P1, force), (B1.masscenter, force2+g1)]
+    assert B2.loads == [(P2, -force), (B2.masscenter, -force2+g2)]
+
+def test_apply_torque():
+    t = symbols('t')
+    q = dynamicsymbols('q')
+    with warns_deprecated_sympy():
+        B1 = Body('B1')
+        B2 = Body('B2')
+    N = ReferenceFrame('N')
+    torque = t*q*N.x
+
+    B1.apply_torque(torque, B2) #Applying equal and opposite torque
+    assert B1.loads == [(B1.frame, torque)]
+    assert B2.loads == [(B2.frame, -torque)]
+
+    torque2 = t*N.y
+    B1.apply_torque(torque2)
+    assert B1.loads == [(B1.frame, torque+torque2)]
+
+def test_clear_load():
+    a = symbols('a')
+    P = Point('P')
+    with warns_deprecated_sympy():
+        B = Body('B')
+    force = a*B.z
+    B.apply_force(force, P)
+    assert B.loads == [(P, force)]
+    B.clear_loads()
+    assert B.loads == []
+
+def test_remove_load():
+    P1 = Point('P1')
+    P2 = Point('P2')
+    with warns_deprecated_sympy():
+        B = Body('B')
+    f1 = B.x
+    f2 = B.y
+    B.apply_force(f1, P1)
+    B.apply_force(f2, P2)
+    assert B.loads == [(P1, f1), (P2, f2)]
+    B.remove_load(P2)
+    assert B.loads == [(P1, f1)]
+    B.apply_torque(f1.cross(f2))
+    assert B.loads == [(P1, f1), (B.frame, f1.cross(f2))]
+    B.remove_load()
+    assert B.loads == [(P1, f1)]
+
+def test_apply_loads_on_multi_degree_freedom_holonomic_system():
+    """Example based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.html"""
+    with warns_deprecated_sympy():
+        W = Body('W') #Wall
+        B = Body('B') #Block
+        P = Body('P') #Pendulum
+        b = Body('b') #bob
+    q1, q2 = dynamicsymbols('q1 q2') #generalized coordinates
+    k, c, g, kT = symbols('k c g kT') #constants
+    F, T = dynamicsymbols('F T') #Specified forces
+
+    #Applying forces
+    B.apply_force(F*W.x)
+    W.apply_force(k*q1*W.x, reaction_body=B) #Spring force
+    W.apply_force(c*q1.diff()*W.x, reaction_body=B) #dampner
+    P.apply_force(P.mass*g*W.y)
+    b.apply_force(b.mass*g*W.y)
+
+    #Applying torques
+    P.apply_torque(kT*q2*W.z, reaction_body=b)
+    P.apply_torque(T*W.z)
+
+    assert B.loads == [(B.masscenter, (F - k*q1 - c*q1.diff())*W.x)]
+    assert P.loads == [(P.masscenter, P.mass*g*W.y), (P.frame, (T + kT*q2)*W.z)]
+    assert b.loads == [(b.masscenter, b.mass*g*W.y), (b.frame, -kT*q2*W.z)]
+    assert W.loads == [(W.masscenter, (c*q1.diff() + k*q1)*W.x)]
+
+
+def test_parallel_axis():
+    N = ReferenceFrame('N')
+    m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
+    Io = inertia(N, Ix, Iy, Iz)
+    # Test RigidBody
+    o = Point('o')
+    p = o.locatenew('p', a * N.x + b * N.y)
+    with warns_deprecated_sympy():
+        R = Body('R', masscenter=o, frame=N, mass=m, central_inertia=Io)
+    Ip = R.parallel_axis(p)
+    Ip_expected = inertia(N, Ix + m * b**2, Iy + m * a**2,
+                          Iz + m * (a**2 + b**2), ixy=-m * a * b)
+    assert Ip == Ip_expected
+    # Reference frame from which the parallel axis is viewed should not matter
+    A = ReferenceFrame('A')
+    A.orient_axis(N, N.z, 1)
+    assert simplify(
+        (R.parallel_axis(p, A) - Ip_expected).to_matrix(A)) == zeros(3, 3)
+    # Test Particle
+    o = Point('o')
+    p = o.locatenew('p', a * N.x + b * N.y)
+    with warns_deprecated_sympy():
+        P = Body('P', masscenter=o, mass=m, frame=N)
+    Ip = P.parallel_axis(p, N)
+    Ip_expected = inertia(N, m * b ** 2, m * a ** 2, m * (a ** 2 + b ** 2),
+                          ixy=-m * a * b)
+    assert not P.is_rigidbody
+    assert Ip == Ip_expected

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_functions.py

@@ -0,0 +1,262 @@
+from sympy import sin, cos, tan, pi, symbols, Matrix, S, Function
+from sympy.physics.mechanics import (Particle, Point, ReferenceFrame,
+                                     RigidBody)
+from sympy.physics.mechanics import (angular_momentum, dynamicsymbols,
+                                     kinetic_energy, linear_momentum,
+                                     outer, potential_energy, msubs,
+                                     find_dynamicsymbols, Lagrangian)
+
+from sympy.physics.mechanics.functions import (
+    center_of_mass, _validate_coordinates, _parse_linear_solver)
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+
+
+q1, q2, q3, q4, q5 = symbols('q1 q2 q3 q4 q5')
+N = ReferenceFrame('N')
+A = N.orientnew('A', 'Axis', [q1, N.z])
+B = A.orientnew('B', 'Axis', [q2, A.x])
+C = B.orientnew('C', 'Axis', [q3, B.y])
+
+
+def test_linear_momentum():
+    N = ReferenceFrame('N')
+    Ac = Point('Ac')
+    Ac.set_vel(N, 25 * N.y)
+    I = outer(N.x, N.x)
+    A = RigidBody('A', Ac, N, 20, (I, Ac))
+    P = Point('P')
+    Pa = Particle('Pa', P, 1)
+    Pa.point.set_vel(N, 10 * N.x)
+    raises(TypeError, lambda: linear_momentum(A, A, Pa))
+    raises(TypeError, lambda: linear_momentum(N, N, Pa))
+    assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
+
+
+def test_angular_momentum_and_linear_momentum():
+    """A rod with length 2l, centroidal inertia I, and mass M along with a
+    particle of mass m fixed to the end of the rod rotate with an angular rate
+    of omega about point O which is fixed to the non-particle end of the rod.
+    The rod's reference frame is A and the inertial frame is N."""
+    m, M, l, I = symbols('m, M, l, I')
+    omega = dynamicsymbols('omega')
+    N = ReferenceFrame('N')
+    a = ReferenceFrame('a')
+    O = Point('O')
+    Ac = O.locatenew('Ac', l * N.x)
+    P = Ac.locatenew('P', l * N.x)
+    O.set_vel(N, 0 * N.x)
+    a.set_ang_vel(N, omega * N.z)
+    Ac.v2pt_theory(O, N, a)
+    P.v2pt_theory(O, N, a)
+    Pa = Particle('Pa', P, m)
+    A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
+    expected = 2 * m * omega * l * N.y + M * l * omega * N.y
+    assert linear_momentum(N, A, Pa) == expected
+    raises(TypeError, lambda: angular_momentum(N, N, A, Pa))
+    raises(TypeError, lambda: angular_momentum(O, O, A, Pa))
+    raises(TypeError, lambda: angular_momentum(O, N, O, Pa))
+    expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
+    assert angular_momentum(O, N, A, Pa) == expected
+
+
+def test_kinetic_energy():
+    m, M, l1 = symbols('m M l1')
+    omega = dynamicsymbols('omega')
+    N = ReferenceFrame('N')
+    O = Point('O')
+    O.set_vel(N, 0 * N.x)
+    Ac = O.locatenew('Ac', l1 * N.x)
+    P = Ac.locatenew('P', l1 * N.x)
+    a = ReferenceFrame('a')
+    a.set_ang_vel(N, omega * N.z)
+    Ac.v2pt_theory(O, N, a)
+    P.v2pt_theory(O, N, a)
+    Pa = Particle('Pa', P, m)
+    I = outer(N.z, N.z)
+    A = RigidBody('A', Ac, a, M, (I, Ac))
+    raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
+    raises(TypeError, lambda: kinetic_energy(N, N, A))
+    assert 0 == (kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
+            + 2*l1**2*m*omega**2 + omega**2/2)).expand()
+
+
+def test_potential_energy():
+    m, M, l1, g, h, H = symbols('m M l1 g h H')
+    omega = dynamicsymbols('omega')
+    N = ReferenceFrame('N')
+    O = Point('O')
+    O.set_vel(N, 0 * N.x)
+    Ac = O.locatenew('Ac', l1 * N.x)
+    P = Ac.locatenew('P', l1 * N.x)
+    a = ReferenceFrame('a')
+    a.set_ang_vel(N, omega * N.z)
+    Ac.v2pt_theory(O, N, a)
+    P.v2pt_theory(O, N, a)
+    Pa = Particle('Pa', P, m)
+    I = outer(N.z, N.z)
+    A = RigidBody('A', Ac, a, M, (I, Ac))
+    Pa.potential_energy = m * g * h
+    A.potential_energy = M * g * H
+    assert potential_energy(A, Pa) == m * g * h + M * g * H
+
+
+def test_Lagrangian():
+    M, m, g, h = symbols('M m g h')
+    N = ReferenceFrame('N')
+    O = Point('O')
+    O.set_vel(N, 0 * N.x)
+    P = O.locatenew('P', 1 * N.x)
+    P.set_vel(N, 10 * N.x)
+    Pa = Particle('Pa', P, 1)
+    Ac = O.locatenew('Ac', 2 * N.y)
+    Ac.set_vel(N, 5 * N.y)
+    a = ReferenceFrame('a')
+    a.set_ang_vel(N, 10 * N.z)
+    I = outer(N.z, N.z)
+    A = RigidBody('A', Ac, a, 20, (I, Ac))
+    Pa.potential_energy = m * g * h
+    A.potential_energy = M * g * h
+    raises(TypeError, lambda: Lagrangian(A, A, Pa))
+    raises(TypeError, lambda: Lagrangian(N, N, Pa))
+
+
+def test_msubs():
+    a, b = symbols('a, b')
+    x, y, z = dynamicsymbols('x, y, z')
+    # Test simple substitution
+    expr = Matrix([[a*x + b, x*y.diff() + y],
+                   [x.diff().diff(), z + sin(z.diff())]])
+    sol = Matrix([[a + b, y],
+                  [x.diff().diff(), 1]])
+    sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
+    assert msubs(expr, sd) == sol
+    # Test smart substitution
+    expr = cos(x + y)*tan(x + y) + b*x.diff()
+    sd = {x: 0, y: pi/2, x.diff(): 1}
+    assert msubs(expr, sd, smart=True) == b + 1
+    N = ReferenceFrame('N')
+    v = x*N.x + y*N.y
+    d = x*(N.x|N.x) + y*(N.y|N.y)
+    v_sol = 1*N.y
+    d_sol = 1*(N.y|N.y)
+    sd = {x: 0, y: 1}
+    assert msubs(v, sd) == v_sol
+    assert msubs(d, sd) == d_sol
+
+
+def test_find_dynamicsymbols():
+    a, b = symbols('a, b')
+    x, y, z = dynamicsymbols('x, y, z')
+    expr = Matrix([[a*x + b, x*y.diff() + y],
+                   [x.diff().diff(), z + sin(z.diff())]])
+    # Test finding all dynamicsymbols
+    sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
+    assert find_dynamicsymbols(expr) == sol
+    # Test finding all but those in sym_list
+    exclude_list = [x, y, z]
+    sol = {y.diff(), x.diff().diff(), z.diff()}
+    assert find_dynamicsymbols(expr, exclude=exclude_list) == sol
+    # Test finding all dynamicsymbols in a vector with a given reference frame
+    d, e, f = dynamicsymbols('d, e, f')
+    A = ReferenceFrame('A')
+    v = d * A.x + e * A.y + f * A.z
+    sol = {d, e, f}
+    assert find_dynamicsymbols(v, reference_frame=A) == sol
+    # Test if a ValueError is raised on supplying only a vector as input
+    raises(ValueError, lambda: find_dynamicsymbols(v))
+
+
+# This function tests the center_of_mass() function
+# that was added in PR #14758 to compute the center of
+# mass of a system of bodies.
+def test_center_of_mass():
+    a = ReferenceFrame('a')
+    m = symbols('m', real=True)
+    p1 = Particle('p1', Point('p1_pt'), S.One)
+    p2 = Particle('p2', Point('p2_pt'), S(2))
+    p3 = Particle('p3', Point('p3_pt'), S(3))
+    p4 = Particle('p4', Point('p4_pt'), m)
+    b_f = ReferenceFrame('b_f')
+    b_cm = Point('b_cm')
+    mb = symbols('mb')
+    b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
+    p2.point.set_pos(p1.point, a.x)
+    p3.point.set_pos(p1.point, a.x + a.y)
+    p4.point.set_pos(p1.point, a.y)
+    b.masscenter.set_pos(p1.point, a.y + a.z)
+    point_o=Point('o')
+    point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
+    expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
+    assert point_o.pos_from(p1.point)-expr == 0
+
+
+def test_validate_coordinates():
+    q1, q2, q3, u1, u2, u3, ua1, ua2, ua3 = dynamicsymbols('q1:4 u1:4 ua1:4')
+    s1, s2, s3 = symbols('s1:4')
+    # Test normal
+    _validate_coordinates([q1, q2, q3], [u1, u2, u3],
+                          u_auxiliary=[ua1, ua2, ua3])
+    # Test not equal number of coordinates and speeds
+    _validate_coordinates([q1, q2])
+    _validate_coordinates([q1, q2], [u1])
+    _validate_coordinates(speeds=[u1, u2])
+    # Test duplicate
+    _validate_coordinates([q1, q2, q2], [u1, u2, u3], check_duplicates=False)
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q2], [u1, u2, u3]))
+    _validate_coordinates([q1, q2, q3], [u1, u2, u2], check_duplicates=False)
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q3], [u1, u2, u2], check_duplicates=True))
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q3], [q1, u2, u3], check_duplicates=True))
+    _validate_coordinates([q1, q2, q3], [u1, u2, u3], check_duplicates=False,
+                          u_auxiliary=[u1, ua2, ua2])
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q3], [u1, u2, u3], u_auxiliary=[u1, ua2, ua3]))
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q3], [u1, u2, u3], u_auxiliary=[q1, ua2, ua3]))
+    raises(ValueError, lambda: _validate_coordinates(
+        [q1, q2, q3], [u1, u2, u3], u_auxiliary=[ua1, ua2, ua2]))
+    # Test is_dynamicsymbols
+    _validate_coordinates([q1 + q2, q3], is_dynamicsymbols=False)
+    raises(ValueError, lambda: _validate_coordinates([q1 + q2, q3]))
+    _validate_coordinates([s1, q1, q2], [0, u1, u2], is_dynamicsymbols=False)
+    raises(ValueError, lambda: _validate_coordinates(
+        [s1, q1, q2], [0, u1, u2], is_dynamicsymbols=True))
+    _validate_coordinates([s1 + s2 + s3, q1], [0, u1], is_dynamicsymbols=False)
+    raises(ValueError, lambda: _validate_coordinates(
+        [s1 + s2 + s3, q1], [0, u1], is_dynamicsymbols=True))
+    _validate_coordinates(u_auxiliary=[s1, ua1], is_dynamicsymbols=False)
+    raises(ValueError, lambda: _validate_coordinates(u_auxiliary=[s1, ua1]))
+    # Test normal function
+    t = dynamicsymbols._t
+    a = symbols('a')
+    f1, f2 = symbols('f1:3', cls=Function)
+    _validate_coordinates([f1(a), f2(a)], is_dynamicsymbols=False)
+    raises(ValueError, lambda: _validate_coordinates([f1(a), f2(a)]))
+    raises(ValueError, lambda: _validate_coordinates(speeds=[f1(a), f2(a)]))
+    dynamicsymbols._t = a
+    _validate_coordinates([f1(a), f2(a)])
+    raises(ValueError, lambda: _validate_coordinates([f1(t), f2(t)]))
+    dynamicsymbols._t = t
+
+
+def test_parse_linear_solver():
+    A, b = Matrix(3, 3, symbols('a:9')), Matrix(3, 2, symbols('b:6'))
+    assert _parse_linear_solver(Matrix.LUsolve) == Matrix.LUsolve  # Test callable
+    assert _parse_linear_solver('LU')(A, b) == Matrix.LUsolve(A, b)
+
+
+def test_deprecated_moved_functions():
+    from sympy.physics.mechanics.functions import (
+        inertia, inertia_of_point_mass, gravity)
+    N = ReferenceFrame('N')
+    with warns_deprecated_sympy():
+        assert inertia(N, 0, 1, 0, 1) == (N.x | N.y) + (N.y | N.x) + (N.y | N.y)
+    with warns_deprecated_sympy():
+        assert inertia_of_point_mass(1, N.x + N.y, N) == (
+            (N.x | N.x) + (N.y | N.y) + 2 * (N.z | N.z) -
+            (N.x | N.y) - (N.y | N.x))
+    p = Particle('P')
+    with warns_deprecated_sympy():
+        assert gravity(-2 * N.z, p) == [(p.masscenter, -2 * p.mass * N.z)]

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_inertia.py

@@ -0,0 +1,71 @@
+from sympy import symbols
+from sympy.testing.pytest import raises
+from sympy.physics.mechanics import (inertia, inertia_of_point_mass,
+                                     Inertia, ReferenceFrame, Point)
+
+
+def test_inertia_dyadic():
+    N = ReferenceFrame('N')
+    ixx, iyy, izz = symbols('ixx iyy izz')
+    ixy, iyz, izx = symbols('ixy iyz izx')
+    assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
+            (N.y | N.y) + izz * (N.z | N.z))
+    assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
+    raises(TypeError, lambda: inertia(0, 0, 0, 0))
+    assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
+            ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
+        (N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
+            N.y) + izz * (N.z | N.z))
+
+
+def test_inertia_of_point_mass():
+    r, s, t, m = symbols('r s t m')
+    N = ReferenceFrame('N')
+
+    px = r * N.x
+    I = inertia_of_point_mass(m, px, N)
+    assert I == m * r**2 * (N.y | N.y) + m * r**2 * (N.z | N.z)
+
+    py = s * N.y
+    I = inertia_of_point_mass(m, py, N)
+    assert I == m * s**2 * (N.x | N.x) + m * s**2 * (N.z | N.z)
+
+    pz = t * N.z
+    I = inertia_of_point_mass(m, pz, N)
+    assert I == m * t**2 * (N.x | N.x) + m * t**2 * (N.y | N.y)
+
+    p = px + py + pz
+    I = inertia_of_point_mass(m, p, N)
+    assert I == (m * (s**2 + t**2) * (N.x | N.x) -
+                 m * r * s * (N.x | N.y) -
+                 m * r * t * (N.x | N.z) -
+                 m * r * s * (N.y | N.x) +
+                 m * (r**2 + t**2) * (N.y | N.y) -
+                 m * s * t * (N.y | N.z) -
+                 m * r * t * (N.z | N.x) -
+                 m * s * t * (N.z | N.y) +
+                 m * (r**2 + s**2) * (N.z | N.z))
+
+
+def test_inertia_object():
+    N = ReferenceFrame('N')
+    O = Point('O')
+    ixx, iyy, izz = symbols('ixx iyy izz')
+    I_dyadic = ixx * (N.x | N.x) + iyy * (N.y | N.y) + izz * (N.z | N.z)
+    I = Inertia(inertia(N, ixx, iyy, izz), O)
+    assert isinstance(I, tuple)
+    assert I.__repr__() == ('Inertia(dyadic=ixx*(N.x|N.x) + iyy*(N.y|N.y) + '
+                            'izz*(N.z|N.z), point=O)')
+    assert I.dyadic == I_dyadic
+    assert I.point == O
+    assert I[0] == I_dyadic
+    assert I[1] == O
+    assert I == (I_dyadic, O)  # Test tuple equal
+    raises(TypeError, lambda: I != (O, I_dyadic))  # Incorrect tuple order
+    assert I == Inertia(O, I_dyadic)  # Parse changed argument order
+    assert I == Inertia.from_inertia_scalars(O, N, ixx, iyy, izz)
+    # Test invalid tuple operations
+    raises(TypeError, lambda: I + (1, 2))
+    raises(TypeError, lambda: (1, 2) + I)
+    raises(TypeError, lambda: I * 2)
+    raises(TypeError, lambda: 2 * I)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_joint.py

@@ -0,0 +1,1240 @@
+from sympy.core.function import expand_mul
+from sympy.core.numbers import pi
+from sympy.core.singleton import S
+from sympy.functions.elementary.miscellaneous import sqrt
+from sympy.functions.elementary.trigonometric import (cos, sin)
+from sympy import Matrix, simplify, eye, zeros
+from sympy.core.symbol import symbols
+from sympy.physics.mechanics import (
+    dynamicsymbols, RigidBody, Particle, JointsMethod, PinJoint, PrismaticJoint,
+    CylindricalJoint, PlanarJoint, SphericalJoint, WeldJoint, Body)
+from sympy.physics.mechanics.joint import Joint
+from sympy.physics.vector import Vector, ReferenceFrame, Point
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+
+
+t = dynamicsymbols._t # type: ignore
+
+
+def _generate_body(interframe=False):
+    N = ReferenceFrame('N')
+    A = ReferenceFrame('A')
+    P = RigidBody('P', frame=N)
+    C = RigidBody('C', frame=A)
+    if interframe:
+        Pint, Cint = ReferenceFrame('P_int'), ReferenceFrame('C_int')
+        Pint.orient_axis(N, N.x, pi)
+        Cint.orient_axis(A, A.y, -pi / 2)
+        return N, A, P, C, Pint, Cint
+    return N, A, P, C
+
+
+def test_Joint():
+    parent = RigidBody('parent')
+    child = RigidBody('child')
+    raises(TypeError, lambda: Joint('J', parent, child))
+
+
+def test_coordinate_generation():
+    q, u, qj, uj = dynamicsymbols('q u q_J u_J')
+    q0j, q1j, q2j, q3j, u0j, u1j, u2j, u3j = dynamicsymbols('q0:4_J u0:4_J')
+    q0, q1, q2, q3, u0, u1, u2, u3 = dynamicsymbols('q0:4 u0:4')
+    _, _, P, C = _generate_body()
+    # Using PinJoint to access Joint's coordinate generation method
+    J = PinJoint('J', P, C)
+    # Test single given
+    assert J._fill_coordinate_list(q, 1) == Matrix([q])
+    assert J._fill_coordinate_list([u], 1) == Matrix([u])
+    assert J._fill_coordinate_list([u], 1, offset=2) == Matrix([u])
+    # Test None
+    assert J._fill_coordinate_list(None, 1) == Matrix([qj])
+    assert J._fill_coordinate_list([None], 1) == Matrix([qj])
+    assert J._fill_coordinate_list([q0, None, None], 3) == Matrix(
+        [q0, q1j, q2j])
+    # Test autofill
+    assert J._fill_coordinate_list(None, 3) == Matrix([q0j, q1j, q2j])
+    assert J._fill_coordinate_list([], 3) == Matrix([q0j, q1j, q2j])
+    # Test offset
+    assert J._fill_coordinate_list([], 3, offset=1) == Matrix([q1j, q2j, q3j])
+    assert J._fill_coordinate_list([q1, None, q3], 3, offset=1) == Matrix(
+        [q1, q2j, q3])
+    assert J._fill_coordinate_list(None, 2, offset=2) == Matrix([q2j, q3j])
+    # Test label
+    assert J._fill_coordinate_list(None, 1, 'u') == Matrix([uj])
+    assert J._fill_coordinate_list([], 3, 'u') == Matrix([u0j, u1j, u2j])
+    # Test single numbering
+    assert J._fill_coordinate_list(None, 1, number_single=True) == Matrix([q0j])
+    assert J._fill_coordinate_list([], 1, 'u', 2, True) == Matrix([u2j])
+    assert J._fill_coordinate_list([], 3, 'q') == Matrix([q0j, q1j, q2j])
+    # Test invalid number of coordinates supplied
+    raises(ValueError, lambda: J._fill_coordinate_list([q0, q1], 1))
+    raises(ValueError, lambda: J._fill_coordinate_list([u0, u1, None], 2, 'u'))
+    raises(ValueError, lambda: J._fill_coordinate_list([q0, q1], 3))
+    # Test incorrect coordinate type
+    raises(TypeError, lambda: J._fill_coordinate_list([q0, symbols('q1')], 2))
+    raises(TypeError, lambda: J._fill_coordinate_list([q0 + q1, q1], 2))
+    # Test if derivative as generalized speed is allowed
+    _, _, P, C = _generate_body()
+    PinJoint('J', P, C, q1, q1.diff(t))
+    # Test duplicate coordinates
+    _, _, P, C = _generate_body()
+    raises(ValueError, lambda: SphericalJoint('J', P, C, [q1j, None, None]))
+    raises(ValueError, lambda: SphericalJoint('J', P, C, speeds=[u0, u0, u1]))
+
+
+def test_pin_joint():
+    P = RigidBody('P')
+    C = RigidBody('C')
+    l, m = symbols('l m')
+    q, u = dynamicsymbols('q_J, u_J')
+    Pj = PinJoint('J', P, C)
+    assert Pj.name == 'J'
+    assert Pj.parent == P
+    assert Pj.child == C
+    assert Pj.coordinates == Matrix([q])
+    assert Pj.speeds == Matrix([u])
+    assert Pj.kdes == Matrix([u - q.diff(t)])
+    assert Pj.joint_axis == P.frame.x
+    assert Pj.child_point.pos_from(C.masscenter) == Vector(0)
+    assert Pj.parent_point.pos_from(P.masscenter) == Vector(0)
+    assert Pj.parent_point.pos_from(Pj._child_point) == Vector(0)
+    assert C.masscenter.pos_from(P.masscenter) == Vector(0)
+    assert Pj.parent_interframe == P.frame
+    assert Pj.child_interframe == C.frame
+    assert Pj.__str__() == 'PinJoint: J  parent: P  child: C'
+
+    P1 = RigidBody('P1')
+    C1 = RigidBody('C1')
+    Pint = ReferenceFrame('P_int')
+    Pint.orient_axis(P1.frame, P1.y, pi / 2)
+    J1 = PinJoint('J1', P1, C1, parent_point=l*P1.frame.x,
+                  child_point=m*C1.frame.y, joint_axis=P1.frame.z,
+                  parent_interframe=Pint)
+    assert J1._joint_axis == P1.frame.z
+    assert J1._child_point.pos_from(C1.masscenter) == m * C1.frame.y
+    assert J1._parent_point.pos_from(P1.masscenter) == l * P1.frame.x
+    assert J1._parent_point.pos_from(J1._child_point) == Vector(0)
+    assert (P1.masscenter.pos_from(C1.masscenter) ==
+            -l*P1.frame.x + m*C1.frame.y)
+    assert J1.parent_interframe == Pint
+    assert J1.child_interframe == C1.frame
+
+    q, u = dynamicsymbols('q, u')
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    parent_point = P.masscenter.locatenew('parent_point', N.x + N.y)
+    child_point = C.masscenter.locatenew('child_point', C.y + C.z)
+    J = PinJoint('J', P, C, q, u, parent_point=parent_point,
+                 child_point=child_point, parent_interframe=Pint,
+                 child_interframe=Cint, joint_axis=N.z)
+    assert J.joint_axis == N.z
+    assert J.parent_point.vel(N) == 0
+    assert J.parent_point == parent_point
+    assert J.child_point == child_point
+    assert J.child_point.pos_from(P.masscenter) == N.x + N.y
+    assert J.parent_point.pos_from(C.masscenter) == C.y + C.z
+    assert C.masscenter.pos_from(P.masscenter) == N.x + N.y - C.y - C.z
+    assert C.masscenter.vel(N).express(N) == (u * sin(q) - u * cos(q)) * N.x + (
+            -u * sin(q) - u * cos(q)) * N.y
+    assert J.parent_interframe == Pint
+    assert J.child_interframe == Cint
+
+
+def test_particle_compatibility():
+    m, l = symbols('m l')
+    C_frame = ReferenceFrame('C')
+    P = Particle('P')
+    C = Particle('C', mass=m)
+    q, u = dynamicsymbols('q, u')
+    J = PinJoint('J', P, C, q, u, child_interframe=C_frame,
+                 child_point=l * C_frame.y)
+    assert J.child_interframe == C_frame
+    assert J.parent_interframe.name == 'J_P_frame'
+    assert C.masscenter.pos_from(P.masscenter) == -l * C_frame.y
+    assert C_frame.dcm(J.parent_interframe) == Matrix([[1, 0, 0],
+                                                       [0, cos(q), sin(q)],
+                                                       [0, -sin(q), cos(q)]])
+    assert C.masscenter.vel(J.parent_interframe) == -l * u * C_frame.z
+    # Test with specified joint axis
+    P_frame = ReferenceFrame('P')
+    C_frame = ReferenceFrame('C')
+    P = Particle('P')
+    C = Particle('C', mass=m)
+    q, u = dynamicsymbols('q, u')
+    J = PinJoint('J', P, C, q, u, parent_interframe=P_frame,
+                 child_interframe=C_frame, child_point=l * C_frame.y,
+                 joint_axis=P_frame.z)
+    assert J.joint_axis == J.parent_interframe.z
+    assert C_frame.dcm(J.parent_interframe) == Matrix([[cos(q), sin(q), 0],
+                                                       [-sin(q), cos(q), 0],
+                                                       [0, 0, 1]])
+    assert P.masscenter.vel(J.parent_interframe) == 0
+    assert C.masscenter.vel(J.parent_interframe) == l * u * C_frame.x
+    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4 u1:4')
+    qdot_to_u = {qi.diff(t): ui for qi, ui in ((q1, u1), (q2, u2), (q3, u3))}
+    # Test compatibility for prismatic joint
+    P, C = Particle('P'), Particle('C')
+    J = PrismaticJoint('J', P, C, q, u)
+    assert J.parent_interframe.dcm(J.child_interframe) == eye(3)
+    assert C.masscenter.pos_from(P.masscenter) == q * J.parent_interframe.x
+    assert P.masscenter.vel(J.parent_interframe) == 0
+    assert C.masscenter.vel(J.parent_interframe) == u * J.parent_interframe.x
+    # Test compatibility for cylindrical joint
+    P, C = Particle('P'), Particle('C')
+    P_frame = ReferenceFrame('P_frame')
+    J = CylindricalJoint('J', P, C, q1, q2, u1, u2, parent_interframe=P_frame,
+                         parent_point=l * P_frame.x, joint_axis=P_frame.y)
+    assert J.parent_interframe.dcm(J.child_interframe) == Matrix([
+        [cos(q1), 0, sin(q1)], [0, 1, 0], [-sin(q1), 0, cos(q1)]])
+    assert C.masscenter.pos_from(P.masscenter) == l * P_frame.x + q2 * P_frame.y
+    assert C.masscenter.vel(J.parent_interframe) == u2 * P_frame.y
+    assert P.masscenter.vel(J.child_interframe).xreplace(qdot_to_u) == (
+        -u2 * P_frame.y - l * u1 * P_frame.z)
+    # Test compatibility for planar joint
+    P, C = Particle('P'), Particle('C')
+    C_frame = ReferenceFrame('C_frame')
+    J = PlanarJoint('J', P, C, q1, [q2, q3], u1, [u2, u3],
+                    child_interframe=C_frame, child_point=l * C_frame.z)
+    P_frame = J.parent_interframe
+    assert J.parent_interframe.dcm(J.child_interframe) == Matrix([
+        [1, 0, 0], [0, cos(q1), -sin(q1)], [0, sin(q1), cos(q1)]])
+    assert C.masscenter.pos_from(P.masscenter) == (
+        -l * C_frame.z + q2 * P_frame.y + q3 * P_frame.z)
+    assert C.masscenter.vel(J.parent_interframe) == (
+        l * u1 * C_frame.y + u2 * P_frame.y + u3 * P_frame.z)
+    # Test compatibility for weld joint
+    P, C = Particle('P'), Particle('C')
+    C_frame, P_frame = ReferenceFrame('C_frame'), ReferenceFrame('P_frame')
+    J = WeldJoint('J', P, C, parent_interframe=P_frame,
+                  child_interframe=C_frame, parent_point=l * P_frame.x,
+                  child_point=l * C_frame.y)
+    assert P_frame.dcm(C_frame) == eye(3)
+    assert C.masscenter.pos_from(P.masscenter) == l * P_frame.x - l * C_frame.y
+    assert C.masscenter.vel(J.parent_interframe) == 0
+
+
+def test_body_compatibility():
+    m, l = symbols('m l')
+    C_frame = ReferenceFrame('C')
+    with warns_deprecated_sympy():
+        P = Body('P')
+        C = Body('C', mass=m, frame=C_frame)
+    q, u = dynamicsymbols('q, u')
+    PinJoint('J', P, C, q, u, child_point=l * C_frame.y)
+    assert C.frame == C_frame
+    assert P.frame.name == 'P_frame'
+    assert C.masscenter.pos_from(P.masscenter) == -l * C.y
+    assert C.frame.dcm(P.frame) == Matrix([[1, 0, 0],
+                                           [0, cos(q), sin(q)],
+                                           [0, -sin(q), cos(q)]])
+    assert C.masscenter.vel(P.frame) == -l * u * C.z
+
+
+def test_pin_joint_double_pendulum():
+    q1, q2 = dynamicsymbols('q1 q2')
+    u1, u2 = dynamicsymbols('u1 u2')
+    m, l = symbols('m l')
+    N = ReferenceFrame('N')
+    A = ReferenceFrame('A')
+    B = ReferenceFrame('B')
+    C = RigidBody('C', frame=N)  # ceiling
+    PartP = RigidBody('P', frame=A, mass=m)
+    PartR = RigidBody('R', frame=B, mass=m)
+
+    J1 = PinJoint('J1', C, PartP, speeds=u1, coordinates=q1,
+                  child_point=-l*A.x, joint_axis=C.frame.z)
+    J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2,
+                  child_point=-l*B.x, joint_axis=PartP.frame.z)
+
+    # Check orientation
+    assert N.dcm(A) == Matrix([[cos(q1), -sin(q1), 0],
+                               [sin(q1), cos(q1), 0], [0, 0, 1]])
+    assert A.dcm(B) == Matrix([[cos(q2), -sin(q2), 0],
+                               [sin(q2), cos(q2), 0], [0, 0, 1]])
+    assert simplify(N.dcm(B)) == Matrix([[cos(q1 + q2), -sin(q1 + q2), 0],
+                                                 [sin(q1 + q2), cos(q1 + q2), 0],
+                                                 [0, 0, 1]])
+
+    # Check Angular Velocity
+    assert A.ang_vel_in(N) == u1 * N.z
+    assert B.ang_vel_in(A) == u2 * A.z
+    assert B.ang_vel_in(N) == u1 * N.z + u2 * A.z
+
+    # Check kde
+    assert J1.kdes == Matrix([u1 - q1.diff(t)])
+    assert J2.kdes == Matrix([u2 - q2.diff(t)])
+
+    # Check Linear Velocity
+    assert PartP.masscenter.vel(N) == l*u1*A.y
+    assert PartR.masscenter.vel(A) == l*u2*B.y
+    assert PartR.masscenter.vel(N) == l*u1*A.y + l*(u1 + u2)*B.y
+
+
+def test_pin_joint_chaos_pendulum():
+    mA, mB, lA, lB, h = symbols('mA, mB, lA, lB, h')
+    theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha')
+    N = ReferenceFrame('N')
+    A = ReferenceFrame('A')
+    B = ReferenceFrame('B')
+    lA = (lB - h / 2) / 2
+    lC = (lB/2 + h/4)
+    rod = RigidBody('rod', frame=A, mass=mA)
+    plate = RigidBody('plate', mass=mB, frame=B)
+    C = RigidBody('C', frame=N)
+    J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega,
+                  child_point=lA*A.z, joint_axis=N.y)
+    J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha,
+                  parent_point=lC*A.z, joint_axis=A.z)
+
+    # Check orientation
+    assert A.dcm(N) == Matrix([[cos(theta), 0, -sin(theta)],
+                               [0, 1, 0],
+                               [sin(theta), 0, cos(theta)]])
+    assert A.dcm(B) == Matrix([[cos(phi), -sin(phi), 0],
+                               [sin(phi), cos(phi), 0],
+                               [0, 0, 1]])
+    assert B.dcm(N) == Matrix([
+        [cos(phi)*cos(theta), sin(phi), -sin(theta)*cos(phi)],
+        [-sin(phi)*cos(theta), cos(phi), sin(phi)*sin(theta)],
+        [sin(theta), 0, cos(theta)]])
+
+    # Check Angular Velocity
+    assert A.ang_vel_in(N) == omega*N.y
+    assert A.ang_vel_in(B) == -alpha*A.z
+    assert N.ang_vel_in(B) == -omega*N.y - alpha*A.z
+
+    # Check kde
+    assert J1.kdes == Matrix([omega - theta.diff(t)])
+    assert J2.kdes == Matrix([alpha - phi.diff(t)])
+
+    # Check pos of masscenters
+    assert C.masscenter.pos_from(rod.masscenter) == lA*A.z
+    assert rod.masscenter.pos_from(plate.masscenter) == - lC * A.z
+
+    # Check Linear Velocities
+    assert rod.masscenter.vel(N) == (h/4 - lB/2)*omega*A.x
+    assert plate.masscenter.vel(N) == ((h/4 - lB/2)*omega +
+                                       (h/4 + lB/2)*omega)*A.x
+
+
+def test_pin_joint_interframe():
+    q, u = dynamicsymbols('q, u')
+    # Check not connected
+    N, A, P, C = _generate_body()
+    Pint, Cint = ReferenceFrame('Pint'), ReferenceFrame('Cint')
+    raises(ValueError, lambda: PinJoint('J', P, C, parent_interframe=Pint))
+    raises(ValueError, lambda: PinJoint('J', P, C, child_interframe=Cint))
+    # Check not fixed interframe
+    Pint.orient_axis(N, N.z, q)
+    Cint.orient_axis(A, A.z, q)
+    raises(ValueError, lambda: PinJoint('J', P, C, parent_interframe=Pint))
+    raises(ValueError, lambda: PinJoint('J', P, C, child_interframe=Cint))
+    # Check only parent_interframe
+    N, A, P, C = _generate_body()
+    Pint = ReferenceFrame('Pint')
+    Pint.orient_body_fixed(N, (pi / 4, pi, pi / 3), 'xyz')
+    PinJoint('J', P, C, q, u, parent_point=N.x, child_point=-C.y,
+             parent_interframe=Pint, joint_axis=Pint.x)
+    assert simplify(N.dcm(A)) - Matrix([
+        [-1 / 2, sqrt(3) * cos(q) / 2, -sqrt(3) * sin(q) / 2],
+        [sqrt(6) / 4, sqrt(2) * (2 * sin(q) + cos(q)) / 4,
+         sqrt(2) * (-sin(q) + 2 * cos(q)) / 4],
+        [sqrt(6) / 4, sqrt(2) * (-2 * sin(q) + cos(q)) / 4,
+         -sqrt(2) * (sin(q) + 2 * cos(q)) / 4]]) == zeros(3)
+    assert A.ang_vel_in(N) == u * Pint.x
+    assert C.masscenter.pos_from(P.masscenter) == N.x + A.y
+    assert C.masscenter.vel(N) == u * A.z
+    assert P.masscenter.vel(Pint) == Vector(0)
+    assert C.masscenter.vel(Pint) == u * A.z
+    # Check only child_interframe
+    N, A, P, C = _generate_body()
+    Cint = ReferenceFrame('Cint')
+    Cint.orient_body_fixed(A, (2 * pi / 3, -pi, pi / 2), 'xyz')
+    PinJoint('J', P, C, q, u, parent_point=-N.z, child_point=C.x,
+             child_interframe=Cint, joint_axis=P.x + P.z)
+    assert simplify(N.dcm(A)) == Matrix([
+        [-sqrt(2) * sin(q) / 2,
+         -sqrt(3) * (cos(q) - 1) / 4 - cos(q) / 4 - S(1) / 4,
+         sqrt(3) * (cos(q) + 1) / 4 - cos(q) / 4 + S(1) / 4],
+        [cos(q), (sqrt(2) + sqrt(6)) * -sin(q) / 4,
+         (-sqrt(2) + sqrt(6)) * sin(q) / 4],
+        [sqrt(2) * sin(q) / 2,
+         sqrt(3) * (cos(q) + 1) / 4 + cos(q) / 4 - S(1) / 4,
+         sqrt(3) * (1 - cos(q)) / 4 + cos(q) / 4 + S(1) / 4]])
+    assert A.ang_vel_in(N) == sqrt(2) * u / 2 * N.x + sqrt(2) * u / 2 * N.z
+    assert C.masscenter.pos_from(P.masscenter) == - N.z - A.x
+    assert C.masscenter.vel(N).simplify() == (
+        -sqrt(6) - sqrt(2)) * u / 4 * A.y + (
+               -sqrt(2) + sqrt(6)) * u / 4 * A.z
+    assert C.masscenter.vel(Cint) == Vector(0)
+    # Check combination
+    N, A, P, C = _generate_body()
+    Pint, Cint = ReferenceFrame('Pint'), ReferenceFrame('Cint')
+    Pint.orient_body_fixed(N, (-pi / 2, pi, pi / 2), 'xyz')
+    Cint.orient_body_fixed(A, (2 * pi / 3, -pi, pi / 2), 'xyz')
+    PinJoint('J', P, C, q, u, parent_point=N.x - N.y, child_point=-C.z,
+             parent_interframe=Pint, child_interframe=Cint,
+             joint_axis=Pint.x + Pint.z)
+    assert simplify(N.dcm(A)) == Matrix([
+        [cos(q), (sqrt(2) + sqrt(6)) * -sin(q) / 4,
+         (-sqrt(2) + sqrt(6)) * sin(q) / 4],
+        [-sqrt(2) * sin(q) / 2,
+         -sqrt(3) * (cos(q) + 1) / 4 - cos(q) / 4 + S(1) / 4,
+         sqrt(3) * (cos(q) - 1) / 4 - cos(q) / 4 - S(1) / 4],
+        [sqrt(2) * sin(q) / 2,
+         sqrt(3) * (cos(q) - 1) / 4 + cos(q) / 4 + S(1) / 4,
+         -sqrt(3) * (cos(q) + 1) / 4 + cos(q) / 4 - S(1) / 4]])
+    assert A.ang_vel_in(N) == sqrt(2) * u / 2 * Pint.x + sqrt(
+        2) * u / 2 * Pint.z
+    assert C.masscenter.pos_from(P.masscenter) == N.x - N.y + A.z
+    N_v_C = (-sqrt(2) + sqrt(6)) * u / 4 * A.x
+    assert C.masscenter.vel(N).simplify() == N_v_C
+    assert C.masscenter.vel(Pint).simplify() == N_v_C
+    assert C.masscenter.vel(Cint) == Vector(0)
+
+
+def test_pin_joint_joint_axis():
+    q, u = dynamicsymbols('q, u')
+    # Check parent as reference
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    pin = PinJoint('J', P, C, q, u, parent_interframe=Pint,
+                   child_interframe=Cint, joint_axis=P.y)
+    assert pin.joint_axis == P.y
+    assert N.dcm(A) == Matrix([[sin(q), 0, cos(q)], [0, -1, 0],
+                               [cos(q), 0, -sin(q)]])
+    # Check parent_interframe as reference
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    pin = PinJoint('J', P, C, q, u, parent_interframe=Pint,
+                   child_interframe=Cint, joint_axis=Pint.y)
+    assert pin.joint_axis == Pint.y
+    assert N.dcm(A) == Matrix([[-sin(q), 0, cos(q)], [0, -1, 0],
+                               [cos(q), 0, sin(q)]])
+    # Check combination of joint_axis with interframes supplied as vectors (2x)
+    N, A, P, C = _generate_body()
+    pin = PinJoint('J', P, C, q, u, parent_interframe=N.z,
+                   child_interframe=-C.z, joint_axis=N.z)
+    assert pin.joint_axis == N.z
+    assert N.dcm(A) == Matrix([[-cos(q), -sin(q), 0], [-sin(q), cos(q), 0],
+                               [0, 0, -1]])
+    N, A, P, C = _generate_body()
+    pin = PinJoint('J', P, C, q, u, parent_interframe=N.z,
+                   child_interframe=-C.z, joint_axis=N.x)
+    assert pin.joint_axis == N.x
+    assert N.dcm(A) == Matrix([[-1, 0, 0], [0, cos(q), sin(q)],
+                               [0, sin(q), -cos(q)]])
+    # Check time varying axis
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    raises(ValueError, lambda: PinJoint('J', P, C,
+                                        joint_axis=cos(q) * N.x + sin(q) * N.y))
+    # Check joint_axis provided in child frame
+    raises(ValueError, lambda: PinJoint('J', P, C, joint_axis=C.x))
+    # Check some invalid combinations
+    raises(ValueError, lambda: PinJoint('J', P, C, joint_axis=P.x + C.y))
+    raises(ValueError, lambda: PinJoint(
+        'J', P, C, parent_interframe=Pint, child_interframe=Cint,
+        joint_axis=Pint.x + C.y))
+    raises(ValueError, lambda: PinJoint(
+        'J', P, C, parent_interframe=Pint, child_interframe=Cint,
+        joint_axis=P.x + Cint.y))
+    # Check valid special combination
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    PinJoint('J', P, C, parent_interframe=Pint, child_interframe=Cint,
+             joint_axis=Pint.x + P.y)
+    # Check invalid zero vector
+    raises(Exception, lambda: PinJoint(
+        'J', P, C, parent_interframe=Pint, child_interframe=Cint,
+        joint_axis=Vector(0)))
+    raises(Exception, lambda: PinJoint(
+        'J', P, C, parent_interframe=Pint, child_interframe=Cint,
+        joint_axis=P.y + Pint.y))
+
+
+def test_pin_joint_arbitrary_axis():
+    q, u = dynamicsymbols('q_J, u_J')
+
+    # When the bodies are attached though masscenters but axes are opposite.
+    N, A, P, C = _generate_body()
+    PinJoint('J', P, C, child_interframe=-A.x)
+
+    assert (-A.x).angle_between(N.x) == 0
+    assert -A.x.express(N) == N.x
+    assert A.dcm(N) == Matrix([[-1, 0, 0],
+                               [0, -cos(q), -sin(q)],
+                               [0, -sin(q), cos(q)]])
+    assert A.ang_vel_in(N) == u*N.x
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    assert C.masscenter.pos_from(P.masscenter) == 0
+    assert C.masscenter.pos_from(P.masscenter).express(N).simplify() == 0
+    assert C.masscenter.vel(N) == 0
+
+    # When axes are different and parent joint is at masscenter but child joint
+    # is at a unit vector from child masscenter.
+    N, A, P, C = _generate_body()
+    PinJoint('J', P, C, child_interframe=A.y, child_point=A.x)
+
+    assert A.y.angle_between(N.x) == 0  # Axis are aligned
+    assert A.y.express(N) == N.x
+    assert A.dcm(N) == Matrix([[0, -cos(q), -sin(q)],
+                               [1, 0, 0],
+                               [0, -sin(q), cos(q)]])
+    assert A.ang_vel_in(N) == u*N.x
+    assert A.ang_vel_in(N).express(A) == u * A.y
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    assert A.ang_vel_in(N).cross(A.y) == 0
+    assert C.masscenter.vel(N) == u*A.z
+    assert C.masscenter.pos_from(P.masscenter) == -A.x
+    assert (C.masscenter.pos_from(P.masscenter).express(N).simplify() ==
+            cos(q)*N.y + sin(q)*N.z)
+    assert C.masscenter.vel(N).angle_between(A.x) == pi/2
+
+    # Similar to previous case but wrt parent body
+    N, A, P, C = _generate_body()
+    PinJoint('J', P, C, parent_interframe=N.y, parent_point=N.x)
+
+    assert N.y.angle_between(A.x) == 0  # Axis are aligned
+    assert N.y.express(A) == A.x
+    assert A.dcm(N) == Matrix([[0, 1, 0],
+                               [-cos(q), 0, sin(q)],
+                               [sin(q), 0, cos(q)]])
+    assert A.ang_vel_in(N) == u*N.y
+    assert A.ang_vel_in(N).express(A) == u*A.x
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    angle = A.ang_vel_in(N).angle_between(A.x)
+    assert angle.xreplace({u: 1}) == 0
+    assert C.masscenter.vel(N) == 0
+    assert C.masscenter.pos_from(P.masscenter) == N.x
+
+    # Both joint pos id defined but different axes
+    N, A, P, C = _generate_body()
+    PinJoint('J', P, C, parent_point=N.x, child_point=A.x,
+             child_interframe=A.x + A.y)
+    assert expand_mul(N.x.angle_between(A.x + A.y)) == 0  # Axis are aligned
+    assert (A.x + A.y).express(N).simplify() == sqrt(2)*N.x
+    assert simplify(A.dcm(N)) == Matrix([
+        [sqrt(2)/2, -sqrt(2)*cos(q)/2, -sqrt(2)*sin(q)/2],
+        [sqrt(2)/2, sqrt(2)*cos(q)/2, sqrt(2)*sin(q)/2],
+        [0, -sin(q), cos(q)]])
+    assert A.ang_vel_in(N) == u*N.x
+    assert (A.ang_vel_in(N).express(A).simplify() ==
+            (u*A.x + u*A.y)/sqrt(2))
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    angle = A.ang_vel_in(N).angle_between(A.x + A.y)
+    assert angle.xreplace({u: 1}) == 0
+    assert C.masscenter.vel(N).simplify() == (u * A.z)/sqrt(2)
+    assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
+    assert (C.masscenter.pos_from(P.masscenter).express(N).simplify() ==
+            (1 - sqrt(2)/2)*N.x + sqrt(2)*cos(q)/2*N.y +
+            sqrt(2)*sin(q)/2*N.z)
+    assert (C.masscenter.vel(N).express(N).simplify() ==
+            -sqrt(2)*u*sin(q)/2*N.y + sqrt(2)*u*cos(q)/2*N.z)
+    assert C.masscenter.vel(N).angle_between(A.x) == pi/2
+
+    N, A, P, C = _generate_body()
+    PinJoint('J', P, C, parent_point=N.x, child_point=A.x,
+             child_interframe=A.x + A.y - A.z)
+    assert expand_mul(N.x.angle_between(A.x + A.y - A.z)) == 0  # Axis aligned
+    assert (A.x + A.y - A.z).express(N).simplify() == sqrt(3)*N.x
+    assert simplify(A.dcm(N)) == Matrix([
+        [sqrt(3)/3, -sqrt(6)*sin(q + pi/4)/3,
+         sqrt(6)*cos(q + pi/4)/3],
+        [sqrt(3)/3, sqrt(6)*cos(q + pi/12)/3,
+         sqrt(6)*sin(q + pi/12)/3],
+        [-sqrt(3)/3, sqrt(6)*cos(q + 5*pi/12)/3,
+         sqrt(6)*sin(q + 5*pi/12)/3]])
+    assert A.ang_vel_in(N) == u*N.x
+    assert A.ang_vel_in(N).express(A).simplify() == (u*A.x + u*A.y -
+                                                     u*A.z)/sqrt(3)
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    angle = A.ang_vel_in(N).angle_between(A.x + A.y-A.z)
+    assert angle.xreplace({u: 1}).simplify() == 0
+    assert C.masscenter.vel(N).simplify() == (u*A.y + u*A.z)/sqrt(3)
+    assert C.masscenter.pos_from(P.masscenter) == N.x - A.x
+    assert (C.masscenter.pos_from(P.masscenter).express(N).simplify() ==
+            (1 - sqrt(3)/3)*N.x + sqrt(6)*sin(q + pi/4)/3*N.y -
+            sqrt(6)*cos(q + pi/4)/3*N.z)
+    assert (C.masscenter.vel(N).express(N).simplify() ==
+            sqrt(6)*u*cos(q + pi/4)/3*N.y +
+            sqrt(6)*u*sin(q + pi/4)/3*N.z)
+    assert C.masscenter.vel(N).angle_between(A.x) == pi/2
+
+    N, A, P, C = _generate_body()
+    m, n = symbols('m n')
+    PinJoint('J', P, C, parent_point=m * N.x, child_point=n * A.x,
+             child_interframe=A.x + A.y - A.z,
+             parent_interframe=N.x - N.y + N.z)
+    angle = (N.x - N.y + N.z).angle_between(A.x + A.y - A.z)
+    assert expand_mul(angle) == 0  # Axis are aligned
+    assert ((A.x-A.y+A.z).express(N).simplify() ==
+            (-4*cos(q)/3 - S(1)/3)*N.x + (S(1)/3 - 4*sin(q + pi/6)/3)*N.y +
+            (4*cos(q + pi/3)/3 - S(1)/3)*N.z)
+    assert simplify(A.dcm(N)) == Matrix([
+        [S(1)/3 - 2*cos(q)/3, -2*sin(q + pi/6)/3 - S(1)/3,
+         2*cos(q + pi/3)/3 + S(1)/3],
+        [2*cos(q + pi/3)/3 + S(1)/3, 2*cos(q)/3 - S(1)/3,
+         2*sin(q + pi/6)/3 + S(1)/3],
+        [-2*sin(q + pi/6)/3 - S(1)/3, 2*cos(q + pi/3)/3 + S(1)/3,
+         2*cos(q)/3 - S(1)/3]])
+    assert (A.ang_vel_in(N) - (u*N.x - u*N.y + u*N.z)/sqrt(3)).simplify()
+    assert A.ang_vel_in(N).express(A).simplify() == (u*A.x + u*A.y -
+                                                     u*A.z)/sqrt(3)
+    assert A.ang_vel_in(N).magnitude() == sqrt(u**2)
+    angle = A.ang_vel_in(N).angle_between(A.x+A.y-A.z)
+    assert angle.xreplace({u: 1}).simplify() == 0
+    assert (C.masscenter.vel(N).simplify() ==
+            sqrt(3)*n*u/3*A.y + sqrt(3)*n*u/3*A.z)
+    assert C.masscenter.pos_from(P.masscenter) == m*N.x - n*A.x
+    assert (C.masscenter.pos_from(P.masscenter).express(N).simplify() ==
+            (m + n*(2*cos(q) - 1)/3)*N.x + n*(2*sin(q + pi/6) +
+            1)/3*N.y - n*(2*cos(q + pi/3) + 1)/3*N.z)
+    assert (C.masscenter.vel(N).express(N).simplify() ==
+            - 2*n*u*sin(q)/3*N.x + 2*n*u*cos(q + pi/6)/3*N.y +
+            2*n*u*sin(q + pi/3)/3*N.z)
+    assert C.masscenter.vel(N).dot(N.x - N.y + N.z).simplify() == 0
+
+
+def test_create_aligned_frame_pi():
+    N, A, P, C = _generate_body()
+    f = Joint._create_aligned_interframe(P, -P.x, P.x)
+    assert f.z == P.z
+    f = Joint._create_aligned_interframe(P, -P.y, P.y)
+    assert f.x == P.x
+    f = Joint._create_aligned_interframe(P, -P.z, P.z)
+    assert f.y == P.y
+    f = Joint._create_aligned_interframe(P, -P.x - P.y, P.x + P.y)
+    assert f.z == P.z
+    f = Joint._create_aligned_interframe(P, -P.y - P.z, P.y + P.z)
+    assert f.x == P.x
+    f = Joint._create_aligned_interframe(P, -P.x - P.z, P.x + P.z)
+    assert f.y == P.y
+    f = Joint._create_aligned_interframe(P, -P.x - P.y - P.z, P.x + P.y + P.z)
+    assert f.y - f.z == P.y - P.z
+
+
+def test_pin_joint_axis():
+    q, u = dynamicsymbols('q u')
+    # Test default joint axis
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    J = PinJoint('J', P, C, q, u, parent_interframe=Pint, child_interframe=Cint)
+    assert J.joint_axis == Pint.x
+    # Test for the same joint axis expressed in different frames
+    N_R_A = Matrix([[0, sin(q), cos(q)],
+                    [0, -cos(q), sin(q)],
+                    [1, 0, 0]])
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    PinJoint('J', P, C, q, u, parent_interframe=Pint, child_interframe=Cint,
+             joint_axis=N.z)
+    assert N.dcm(A) == N_R_A
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    PinJoint('J', P, C, q, u, parent_interframe=Pint, child_interframe=Cint,
+             joint_axis=-Pint.z)
+    assert N.dcm(A) == N_R_A
+    # Test time varying joint axis
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    raises(ValueError, lambda: PinJoint('J', P, C, joint_axis=q * N.z))
+
+
+def test_locate_joint_pos():
+    # Test Vector and default
+    N, A, P, C = _generate_body()
+    joint = PinJoint('J', P, C, parent_point=N.y + N.z)
+    assert joint.parent_point.name == 'J_P_joint'
+    assert joint.parent_point.pos_from(P.masscenter) == N.y + N.z
+    assert joint.child_point == C.masscenter
+    # Test Point objects
+    N, A, P, C = _generate_body()
+    parent_point = P.masscenter.locatenew('p', N.y + N.z)
+    joint = PinJoint('J', P, C, parent_point=parent_point,
+                     child_point=C.masscenter)
+    assert joint.parent_point == parent_point
+    assert joint.child_point == C.masscenter
+    # Check invalid type
+    N, A, P, C = _generate_body()
+    raises(TypeError,
+           lambda: PinJoint('J', P, C, parent_point=N.x.to_matrix(N)))
+    # Test time varying positions
+    q = dynamicsymbols('q')
+    N, A, P, C = _generate_body()
+    raises(ValueError, lambda: PinJoint('J', P, C, parent_point=q * N.x))
+    N, A, P, C = _generate_body()
+    child_point = C.masscenter.locatenew('p', q * A.y)
+    raises(ValueError, lambda: PinJoint('J', P, C, child_point=child_point))
+    # Test undefined position
+    child_point = Point('p')
+    raises(ValueError, lambda: PinJoint('J', P, C, child_point=child_point))
+
+
+def test_locate_joint_frame():
+    # Test rotated frame and default
+    N, A, P, C = _generate_body()
+    parent_interframe = ReferenceFrame('int_frame')
+    parent_interframe.orient_axis(N, N.z, 1)
+    joint = PinJoint('J', P, C, parent_interframe=parent_interframe)
+    assert joint.parent_interframe == parent_interframe
+    assert joint.parent_interframe.ang_vel_in(N) == 0
+    assert joint.child_interframe == A
+    # Test time varying orientations
+    q = dynamicsymbols('q')
+    N, A, P, C = _generate_body()
+    parent_interframe = ReferenceFrame('int_frame')
+    parent_interframe.orient_axis(N, N.z, q)
+    raises(ValueError,
+           lambda: PinJoint('J', P, C, parent_interframe=parent_interframe))
+    # Test undefined frame
+    N, A, P, C = _generate_body()
+    child_interframe = ReferenceFrame('int_frame')
+    child_interframe.orient_axis(N, N.z, 1)  # Defined with respect to parent
+    raises(ValueError,
+           lambda: PinJoint('J', P, C, child_interframe=child_interframe))
+
+
+def test_prismatic_joint():
+    _, _, P, C = _generate_body()
+    q, u = dynamicsymbols('q_S, u_S')
+    S = PrismaticJoint('S', P, C)
+    assert S.name == 'S'
+    assert S.parent == P
+    assert S.child == C
+    assert S.coordinates == Matrix([q])
+    assert S.speeds == Matrix([u])
+    assert S.kdes == Matrix([u - q.diff(t)])
+    assert S.joint_axis == P.frame.x
+    assert S.child_point.pos_from(C.masscenter) == Vector(0)
+    assert S.parent_point.pos_from(P.masscenter) == Vector(0)
+    assert S.parent_point.pos_from(S.child_point) == - q * P.frame.x
+    assert P.masscenter.pos_from(C.masscenter) == - q * P.frame.x
+    assert C.masscenter.vel(P.frame) == u * P.frame.x
+    assert P.frame.ang_vel_in(C.frame) == 0
+    assert C.frame.ang_vel_in(P.frame) == 0
+    assert S.__str__() == 'PrismaticJoint: S  parent: P  child: C'
+
+    N, A, P, C = _generate_body()
+    l, m = symbols('l m')
+    Pint = ReferenceFrame('P_int')
+    Pint.orient_axis(P.frame, P.y, pi / 2)
+    S = PrismaticJoint('S', P, C, parent_point=l * P.frame.x,
+                       child_point=m * C.frame.y, joint_axis=P.frame.z,
+                       parent_interframe=Pint)
+
+    assert S.joint_axis == P.frame.z
+    assert S.child_point.pos_from(C.masscenter) == m * C.frame.y
+    assert S.parent_point.pos_from(P.masscenter) == l * P.frame.x
+    assert S.parent_point.pos_from(S.child_point) == - q * P.frame.z
+    assert P.masscenter.pos_from(C.masscenter) == - l * N.x - q * N.z + m * A.y
+    assert C.masscenter.vel(P.frame) == u * P.frame.z
+    assert P.masscenter.vel(Pint) == Vector(0)
+    assert C.frame.ang_vel_in(P.frame) == 0
+    assert P.frame.ang_vel_in(C.frame) == 0
+
+    _, _, P, C = _generate_body()
+    Pint = ReferenceFrame('P_int')
+    Pint.orient_axis(P.frame, P.y, pi / 2)
+    S = PrismaticJoint('S', P, C, parent_point=l * P.frame.z,
+                       child_point=m * C.frame.x, joint_axis=P.frame.z,
+                       parent_interframe=Pint)
+    assert S.joint_axis == P.frame.z
+    assert S.child_point.pos_from(C.masscenter) == m * C.frame.x
+    assert S.parent_point.pos_from(P.masscenter) == l * P.frame.z
+    assert S.parent_point.pos_from(S.child_point) == - q * P.frame.z
+    assert P.masscenter.pos_from(C.masscenter) == (-l - q)*P.frame.z + m*C.frame.x
+    assert C.masscenter.vel(P.frame) == u * P.frame.z
+    assert C.frame.ang_vel_in(P.frame) == 0
+    assert P.frame.ang_vel_in(C.frame) == 0
+
+
+def test_prismatic_joint_arbitrary_axis():
+    q, u = dynamicsymbols('q_S, u_S')
+
+    N, A, P, C = _generate_body()
+    PrismaticJoint('S', P, C, child_interframe=-A.x)
+
+    assert (-A.x).angle_between(N.x) == 0
+    assert -A.x.express(N) == N.x
+    assert A.dcm(N) == Matrix([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
+    assert C.masscenter.pos_from(P.masscenter) == q * N.x
+    assert C.masscenter.pos_from(P.masscenter).express(A).simplify() == -q * A.x
+    assert C.masscenter.vel(N) == u * N.x
+    assert C.masscenter.vel(N).express(A) == -u * A.x
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+    #When axes are different and parent joint is at masscenter but child joint is at a unit vector from
+    #child masscenter.
+    N, A, P, C = _generate_body()
+    PrismaticJoint('S', P, C, child_interframe=A.y, child_point=A.x)
+
+    assert A.y.angle_between(N.x) == 0 #Axis are aligned
+    assert A.y.express(N) == N.x
+    assert A.dcm(N) == Matrix([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
+    assert C.masscenter.vel(N) == u * N.x
+    assert C.masscenter.vel(N).express(A) == u * A.y
+    assert C.masscenter.pos_from(P.masscenter) == q*N.x - A.x
+    assert C.masscenter.pos_from(P.masscenter).express(N).simplify() == q*N.x + N.y
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+    #Similar to previous case but wrt parent body
+    N, A, P, C = _generate_body()
+    PrismaticJoint('S', P, C, parent_interframe=N.y, parent_point=N.x)
+
+    assert N.y.angle_between(A.x) == 0 #Axis are aligned
+    assert N.y.express(A) ==  A.x
+    assert A.dcm(N) == Matrix([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+    assert C.masscenter.vel(N) == u * N.y
+    assert C.masscenter.vel(N).express(A) == u * A.x
+    assert C.masscenter.pos_from(P.masscenter) == N.x + q*N.y
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+    #Both joint pos is defined but different axes
+    N, A, P, C = _generate_body()
+    PrismaticJoint('S', P, C, parent_point=N.x, child_point=A.x,
+                   child_interframe=A.x + A.y)
+    assert N.x.angle_between(A.x + A.y) == 0 #Axis are aligned
+    assert (A.x + A.y).express(N) == sqrt(2)*N.x
+    assert A.dcm(N) == Matrix([[sqrt(2)/2, -sqrt(2)/2, 0], [sqrt(2)/2, sqrt(2)/2, 0], [0, 0, 1]])
+    assert C.masscenter.pos_from(P.masscenter) == (q + 1)*N.x - A.x
+    assert C.masscenter.pos_from(P.masscenter).express(N) == (q - sqrt(2)/2 + 1)*N.x + sqrt(2)/2*N.y
+    assert C.masscenter.vel(N).express(A) == u * (A.x + A.y)/sqrt(2)
+    assert C.masscenter.vel(N) == u*N.x
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+    N, A, P, C = _generate_body()
+    PrismaticJoint('S', P, C, parent_point=N.x, child_point=A.x,
+                   child_interframe=A.x + A.y - A.z)
+    assert N.x.angle_between(A.x + A.y - A.z).simplify() == 0 #Axis are aligned
+    assert ((A.x + A.y - A.z).express(N) - sqrt(3)*N.x).simplify() == 0
+    assert simplify(A.dcm(N)) == Matrix([[sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3],
+                                                 [sqrt(3)/3, sqrt(3)/6 + S(1)/2, S(1)/2 - sqrt(3)/6],
+                                                 [-sqrt(3)/3, S(1)/2 - sqrt(3)/6, sqrt(3)/6 + S(1)/2]])
+    assert C.masscenter.pos_from(P.masscenter) == (q + 1)*N.x - A.x
+    assert (C.masscenter.pos_from(P.masscenter).express(N) -
+        ((q - sqrt(3)/3 + 1)*N.x + sqrt(3)/3*N.y - sqrt(3)/3*N.z)).simplify() == 0
+    assert C.masscenter.vel(N) == u*N.x
+    assert (C.masscenter.vel(N).express(A) - (
+        sqrt(3)*u/3*A.x + sqrt(3)*u/3*A.y - sqrt(3)*u/3*A.z)).simplify()
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+    N, A, P, C = _generate_body()
+    m, n = symbols('m n')
+    PrismaticJoint('S', P, C, parent_point=m*N.x, child_point=n*A.x,
+                   child_interframe=A.x + A.y - A.z,
+                   parent_interframe=N.x - N.y + N.z)
+    # 0 angle means that the axis are aligned
+    assert (N.x-N.y+N.z).angle_between(A.x+A.y-A.z).simplify() == 0
+    assert ((A.x+A.y-A.z).express(N) - (N.x - N.y + N.z)).simplify() == 0
+    assert simplify(A.dcm(N)) == Matrix([[-S(1)/3, -S(2)/3, S(2)/3],
+                                                 [S(2)/3, S(1)/3, S(2)/3],
+                                                 [-S(2)/3, S(2)/3, S(1)/3]])
+    assert (C.masscenter.pos_from(P.masscenter) - (
+        (m + sqrt(3)*q/3)*N.x - sqrt(3)*q/3*N.y + sqrt(3)*q/3*N.z - n*A.x)
+            ).express(N).simplify() == 0
+    assert (C.masscenter.pos_from(P.masscenter).express(N) - (
+        (m + n/3 + sqrt(3)*q/3)*N.x + (2*n/3 - sqrt(3)*q/3)*N.y +
+        (-2*n/3 + sqrt(3)*q/3)*N.z)).simplify() == 0
+    assert (C.masscenter.vel(N).express(N) - (
+        sqrt(3)*u/3*N.x - sqrt(3)*u/3*N.y + sqrt(3)*u/3*N.z)).simplify() == 0
+    assert (C.masscenter.vel(N).express(A) -
+            (sqrt(3)*u/3*A.x + sqrt(3)*u/3*A.y - sqrt(3)*u/3*A.z)).simplify() == 0
+    assert A.ang_vel_in(N) == 0
+    assert N.ang_vel_in(A) == 0
+
+
+def test_cylindrical_joint():
+    N, A, P, C = _generate_body()
+    q0_def, q1_def, u0_def, u1_def = dynamicsymbols('q0:2_J, u0:2_J')
+    Cj = CylindricalJoint('J', P, C)
+    assert Cj.name == 'J'
+    assert Cj.parent == P
+    assert Cj.child == C
+    assert Cj.coordinates == Matrix([q0_def, q1_def])
+    assert Cj.speeds == Matrix([u0_def, u1_def])
+    assert Cj.rotation_coordinate == q0_def
+    assert Cj.translation_coordinate == q1_def
+    assert Cj.rotation_speed == u0_def
+    assert Cj.translation_speed == u1_def
+    assert Cj.kdes == Matrix([u0_def - q0_def.diff(t), u1_def - q1_def.diff(t)])
+    assert Cj.joint_axis == N.x
+    assert Cj.child_point.pos_from(C.masscenter) == Vector(0)
+    assert Cj.parent_point.pos_from(P.masscenter) == Vector(0)
+    assert Cj.parent_point.pos_from(Cj._child_point) == -q1_def * N.x
+    assert C.masscenter.pos_from(P.masscenter) == q1_def * N.x
+    assert Cj.child_point.vel(N) == u1_def * N.x
+    assert A.ang_vel_in(N) == u0_def * N.x
+    assert Cj.parent_interframe == N
+    assert Cj.child_interframe == A
+    assert Cj.__str__() == 'CylindricalJoint: J  parent: P  child: C'
+
+    q0, q1, u0, u1 = dynamicsymbols('q0:2, u0:2')
+    l, m = symbols('l, m')
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    Cj = CylindricalJoint('J', P, C, rotation_coordinate=q0, rotation_speed=u0,
+                          translation_speed=u1, parent_point=m * N.x,
+                          child_point=l * A.y, parent_interframe=Pint,
+                          child_interframe=Cint, joint_axis=2 * N.z)
+    assert Cj.coordinates == Matrix([q0, q1_def])
+    assert Cj.speeds == Matrix([u0, u1])
+    assert Cj.rotation_coordinate == q0
+    assert Cj.translation_coordinate == q1_def
+    assert Cj.rotation_speed == u0
+    assert Cj.translation_speed == u1
+    assert Cj.kdes == Matrix([u0 - q0.diff(t), u1 - q1_def.diff(t)])
+    assert Cj.joint_axis == 2 * N.z
+    assert Cj.child_point.pos_from(C.masscenter) == l * A.y
+    assert Cj.parent_point.pos_from(P.masscenter) == m * N.x
+    assert Cj.parent_point.pos_from(Cj._child_point) == -q1_def * N.z
+    assert C.masscenter.pos_from(
+        P.masscenter) == m * N.x + q1_def * N.z - l * A.y
+    assert C.masscenter.vel(N) == u1 * N.z - u0 * l * A.z
+    assert A.ang_vel_in(N) == u0 * N.z
+
+
+def test_planar_joint():
+    N, A, P, C = _generate_body()
+    q0_def, q1_def, q2_def = dynamicsymbols('q0:3_J')
+    u0_def, u1_def, u2_def = dynamicsymbols('u0:3_J')
+    Cj = PlanarJoint('J', P, C)
+    assert Cj.name == 'J'
+    assert Cj.parent == P
+    assert Cj.child == C
+    assert Cj.coordinates == Matrix([q0_def, q1_def, q2_def])
+    assert Cj.speeds == Matrix([u0_def, u1_def, u2_def])
+    assert Cj.rotation_coordinate == q0_def
+    assert Cj.planar_coordinates == Matrix([q1_def, q2_def])
+    assert Cj.rotation_speed == u0_def
+    assert Cj.planar_speeds == Matrix([u1_def, u2_def])
+    assert Cj.kdes == Matrix([u0_def - q0_def.diff(t), u1_def - q1_def.diff(t),
+                              u2_def - q2_def.diff(t)])
+    assert Cj.rotation_axis == N.x
+    assert Cj.planar_vectors == [N.y, N.z]
+    assert Cj.child_point.pos_from(C.masscenter) == Vector(0)
+    assert Cj.parent_point.pos_from(P.masscenter) == Vector(0)
+    r_P_C = q1_def * N.y + q2_def * N.z
+    assert Cj.parent_point.pos_from(Cj.child_point) == -r_P_C
+    assert C.masscenter.pos_from(P.masscenter) == r_P_C
+    assert Cj.child_point.vel(N) == u1_def * N.y + u2_def * N.z
+    assert A.ang_vel_in(N) == u0_def * N.x
+    assert Cj.parent_interframe == N
+    assert Cj.child_interframe == A
+    assert Cj.__str__() == 'PlanarJoint: J  parent: P  child: C'
+
+    q0, q1, q2, u0, u1, u2 = dynamicsymbols('q0:3, u0:3')
+    l, m = symbols('l, m')
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    Cj = PlanarJoint('J', P, C, rotation_coordinate=q0,
+                     planar_coordinates=[q1, q2], planar_speeds=[u1, u2],
+                     parent_point=m * N.x, child_point=l * A.y,
+                     parent_interframe=Pint, child_interframe=Cint)
+    assert Cj.coordinates == Matrix([q0, q1, q2])
+    assert Cj.speeds == Matrix([u0_def, u1, u2])
+    assert Cj.rotation_coordinate == q0
+    assert Cj.planar_coordinates == Matrix([q1, q2])
+    assert Cj.rotation_speed == u0_def
+    assert Cj.planar_speeds == Matrix([u1, u2])
+    assert Cj.kdes == Matrix([u0_def - q0.diff(t), u1 - q1.diff(t),
+                              u2 - q2.diff(t)])
+    assert Cj.rotation_axis == Pint.x
+    assert Cj.planar_vectors == [Pint.y, Pint.z]
+    assert Cj.child_point.pos_from(C.masscenter) == l * A.y
+    assert Cj.parent_point.pos_from(P.masscenter) == m * N.x
+    assert Cj.parent_point.pos_from(Cj.child_point) == q1 * N.y + q2 * N.z
+    assert C.masscenter.pos_from(
+        P.masscenter) == m * N.x - q1 * N.y - q2 * N.z - l * A.y
+    assert C.masscenter.vel(N) == -u1 * N.y - u2 * N.z + u0_def * l * A.x
+    assert A.ang_vel_in(N) == u0_def * N.x
+
+
+def test_planar_joint_advanced():
+    # Tests whether someone is able to just specify two normals, which will form
+    # the rotation axis seen from the parent and child body.
+    # This specific example is a block on a slope, which has that same slope of
+    # 30 degrees, so in the zero configuration the frames of the parent and
+    # child are actually aligned.
+    q0, q1, q2, u0, u1, u2 = dynamicsymbols('q0:3, u0:3')
+    l1, l2 = symbols('l1:3')
+    N, A, P, C = _generate_body()
+    J = PlanarJoint('J', P, C, q0, [q1, q2], u0, [u1, u2],
+                    parent_point=l1 * N.z,
+                    child_point=-l2 * C.z,
+                    parent_interframe=N.z + N.y / sqrt(3),
+                    child_interframe=A.z + A.y / sqrt(3))
+    assert J.rotation_axis.express(N) == (N.z + N.y / sqrt(3)).normalize()
+    assert J.rotation_axis.express(A) == (A.z + A.y / sqrt(3)).normalize()
+    assert J.rotation_axis.angle_between(N.z) == pi / 6
+    assert N.dcm(A).xreplace({q0: 0, q1: 0, q2: 0}) == eye(3)
+    N_R_A = Matrix([
+        [cos(q0), -sqrt(3) * sin(q0) / 2, sin(q0) / 2],
+        [sqrt(3) * sin(q0) / 2, 3 * cos(q0) / 4 + 1 / 4,
+         sqrt(3) * (1 - cos(q0)) / 4],
+        [-sin(q0) / 2, sqrt(3) * (1 - cos(q0)) / 4, cos(q0) / 4 + 3 / 4]])
+    # N.dcm(A) == N_R_A did not work
+    assert simplify(N.dcm(A) - N_R_A) == zeros(3)
+
+
+def test_spherical_joint():
+    N, A, P, C = _generate_body()
+    q0, q1, q2, u0, u1, u2 = dynamicsymbols('q0:3_S, u0:3_S')
+    S = SphericalJoint('S', P, C)
+    assert S.name == 'S'
+    assert S.parent == P
+    assert S.child == C
+    assert S.coordinates == Matrix([q0, q1, q2])
+    assert S.speeds == Matrix([u0, u1, u2])
+    assert S.kdes == Matrix([u0 - q0.diff(t), u1 - q1.diff(t), u2 - q2.diff(t)])
+    assert S.child_point.pos_from(C.masscenter) == Vector(0)
+    assert S.parent_point.pos_from(P.masscenter) == Vector(0)
+    assert S.parent_point.pos_from(S.child_point) == Vector(0)
+    assert P.masscenter.pos_from(C.masscenter) == Vector(0)
+    assert C.masscenter.vel(N) == Vector(0)
+    assert N.ang_vel_in(A) == (-u0 * cos(q1) * cos(q2) - u1 * sin(q2)) * A.x + (
+            u0 * sin(q2) * cos(q1) - u1 * cos(q2)) * A.y + (
+                   -u0 * sin(q1) - u2) * A.z
+    assert A.ang_vel_in(N) == (u0 * cos(q1) * cos(q2) + u1 * sin(q2)) * A.x + (
+            -u0 * sin(q2) * cos(q1) + u1 * cos(q2)) * A.y + (
+                   u0 * sin(q1) + u2) * A.z
+    assert S.__str__() == 'SphericalJoint: S  parent: P  child: C'
+    assert S._rot_type == 'BODY'
+    assert S._rot_order == 123
+    assert S._amounts is None
+
+
+def test_spherical_joint_speeds_as_derivative_terms():
+    # This tests checks whether the system remains valid if the user chooses to
+    # pass the derivative of the generalized coordinates as generalized speeds
+    q0, q1, q2 = dynamicsymbols('q0:3')
+    u0, u1, u2 = dynamicsymbols('q0:3', 1)
+    N, A, P, C = _generate_body()
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2])
+    assert S.coordinates == Matrix([q0, q1, q2])
+    assert S.speeds == Matrix([u0, u1, u2])
+    assert S.kdes == Matrix([0, 0, 0])
+    assert N.ang_vel_in(A) == (-u0 * cos(q1) * cos(q2) - u1 * sin(q2)) * A.x + (
+        u0 * sin(q2) * cos(q1) - u1 * cos(q2)) * A.y + (
+               -u0 * sin(q1) - u2) * A.z
+
+
+def test_spherical_joint_coords():
+    q0s, q1s, q2s, u0s, u1s, u2s = dynamicsymbols('q0:3_S, u0:3_S')
+    q0, q1, q2, q3, u0, u1, u2, u4 = dynamicsymbols('q0:4, u0:4')
+    # Test coordinates as list
+    N, A, P, C = _generate_body()
+    S = SphericalJoint('S', P, C, [q0, q1, q2], [u0, u1, u2])
+    assert S.coordinates == Matrix([q0, q1, q2])
+    assert S.speeds == Matrix([u0, u1, u2])
+    # Test coordinates as Matrix
+    N, A, P, C = _generate_body()
+    S = SphericalJoint('S', P, C, Matrix([q0, q1, q2]),
+                       Matrix([u0, u1, u2]))
+    assert S.coordinates == Matrix([q0, q1, q2])
+    assert S.speeds == Matrix([u0, u1, u2])
+    # Test too few generalized coordinates
+    N, A, P, C = _generate_body()
+    raises(ValueError,
+           lambda: SphericalJoint('S', P, C, Matrix([q0, q1]), Matrix([u0])))
+    # Test too many generalized coordinates
+    raises(ValueError, lambda: SphericalJoint(
+        'S', P, C, Matrix([q0, q1, q2, q3]), Matrix([u0, u1, u2])))
+    raises(ValueError, lambda: SphericalJoint(
+        'S', P, C, Matrix([q0, q1, q2]), Matrix([u0, u1, u2, u4])))
+
+
+def test_spherical_joint_orient_body():
+    q0, q1, q2, u0, u1, u2 = dynamicsymbols('q0:3, u0:3')
+    N_R_A = Matrix([
+        [-sin(q1), -sin(q2) * cos(q1), cos(q1) * cos(q2)],
+        [-sin(q0) * cos(q1), sin(q0) * sin(q1) * sin(q2) - cos(q0) * cos(q2),
+         -sin(q0) * sin(q1) * cos(q2) - sin(q2) * cos(q0)],
+        [cos(q0) * cos(q1), -sin(q0) * cos(q2) - sin(q1) * sin(q2) * cos(q0),
+         -sin(q0) * sin(q2) + sin(q1) * cos(q0) * cos(q2)]])
+    N_w_A = Matrix([[-u0 * sin(q1) - u2],
+                    [-u0 * sin(q2) * cos(q1) + u1 * cos(q2)],
+                    [u0 * cos(q1) * cos(q2) + u1 * sin(q2)]])
+    N_v_Co = Matrix([
+        [-sqrt(2) * (u0 * cos(q2 + pi / 4) * cos(q1) + u1 * sin(q2 + pi / 4))],
+        [-u0 * sin(q1) - u2], [-u0 * sin(q1) - u2]])
+    # Test default rot_type='BODY', rot_order=123
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.y, child_point=-A.y + A.z,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='body', rot_order=123)
+    assert S._rot_type.upper() == 'BODY'
+    assert S._rot_order == 123
+    assert simplify(N.dcm(A) - N_R_A) == zeros(3)
+    assert simplify(A.ang_vel_in(N).to_matrix(A) - N_w_A) == zeros(3, 1)
+    assert simplify(C.masscenter.vel(N).to_matrix(A)) == N_v_Co
+    # Test change of amounts
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.y, child_point=-A.y + A.z,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='BODY', amounts=(q1, q0, q2), rot_order=123)
+    switch_order = lambda expr: expr.xreplace(
+        {q0: q1, q1: q0, q2: q2, u0: u1, u1: u0, u2: u2})
+    assert S._rot_type.upper() == 'BODY'
+    assert S._rot_order == 123
+    assert simplify(N.dcm(A) - switch_order(N_R_A)) == zeros(3)
+    assert simplify(A.ang_vel_in(N).to_matrix(A) - switch_order(N_w_A)
+                            ) == zeros(3, 1)
+    assert simplify(C.masscenter.vel(N).to_matrix(A)) == switch_order(N_v_Co)
+    # Test different rot_order
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.y, child_point=-A.y + A.z,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='BodY', rot_order='yxz')
+    assert S._rot_type.upper() == 'BODY'
+    assert S._rot_order == 'yxz'
+    assert simplify(N.dcm(A) - Matrix([
+        [-sin(q0) * cos(q1), sin(q0) * sin(q1) * cos(q2) - sin(q2) * cos(q0),
+         sin(q0) * sin(q1) * sin(q2) + cos(q0) * cos(q2)],
+        [-sin(q1), -cos(q1) * cos(q2), -sin(q2) * cos(q1)],
+        [cos(q0) * cos(q1), -sin(q0) * sin(q2) - sin(q1) * cos(q0) * cos(q2),
+         sin(q0) * cos(q2) - sin(q1) * sin(q2) * cos(q0)]])) == zeros(3)
+    assert simplify(A.ang_vel_in(N).to_matrix(A) - Matrix([
+        [u0 * sin(q1) - u2], [u0 * cos(q1) * cos(q2) - u1 * sin(q2)],
+        [u0 * sin(q2) * cos(q1) + u1 * cos(q2)]])) == zeros(3, 1)
+    assert simplify(C.masscenter.vel(N).to_matrix(A)) == Matrix([
+        [-sqrt(2) * (u0 * sin(q2 + pi / 4) * cos(q1) + u1 * cos(q2 + pi / 4))],
+        [u0 * sin(q1) - u2], [u0 * sin(q1) - u2]])
+
+
+def test_spherical_joint_orient_space():
+    q0, q1, q2, u0, u1, u2 = dynamicsymbols('q0:3, u0:3')
+    N_R_A = Matrix([
+        [-sin(q0) * sin(q2) - sin(q1) * cos(q0) * cos(q2),
+         sin(q0) * sin(q1) * cos(q2) - sin(q2) * cos(q0), cos(q1) * cos(q2)],
+        [-sin(q0) * cos(q2) + sin(q1) * sin(q2) * cos(q0),
+         -sin(q0) * sin(q1) * sin(q2) - cos(q0) * cos(q2), -sin(q2) * cos(q1)],
+        [cos(q0) * cos(q1), -sin(q0) * cos(q1), sin(q1)]])
+    N_w_A = Matrix([
+        [u1 * sin(q0) - u2 * cos(q0) * cos(q1)],
+        [u1 * cos(q0) + u2 * sin(q0) * cos(q1)], [u0 - u2 * sin(q1)]])
+    N_v_Co = Matrix([
+        [u0 - u2 * sin(q1)], [u0 - u2 * sin(q1)],
+        [sqrt(2) * (-u1 * sin(q0 + pi / 4) + u2 * cos(q0 + pi / 4) * cos(q1))]])
+    # Test default rot_type='BODY', rot_order=123
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.z, child_point=-A.x + A.y,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='space', rot_order=123)
+    assert S._rot_type.upper() == 'SPACE'
+    assert S._rot_order == 123
+    assert simplify(N.dcm(A) - N_R_A) == zeros(3)
+    assert simplify(A.ang_vel_in(N).to_matrix(A)) == N_w_A
+    assert simplify(C.masscenter.vel(N).to_matrix(A)) == N_v_Co
+    # Test change of amounts
+    switch_order = lambda expr: expr.xreplace(
+        {q0: q1, q1: q0, q2: q2, u0: u1, u1: u0, u2: u2})
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.z, child_point=-A.x + A.y,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='SPACE', amounts=(q1, q0, q2), rot_order=123)
+    assert S._rot_type.upper() == 'SPACE'
+    assert S._rot_order == 123
+    assert simplify(N.dcm(A) - switch_order(N_R_A)) == zeros(3)
+    assert simplify(A.ang_vel_in(N).to_matrix(A)) == switch_order(N_w_A)
+    assert simplify(C.masscenter.vel(N).to_matrix(A)) == switch_order(N_v_Co)
+    # Test different rot_order
+    N, A, P, C, Pint, Cint = _generate_body(True)
+    S = SphericalJoint('S', P, C, coordinates=[q0, q1, q2], speeds=[u0, u1, u2],
+                       parent_point=N.x + N.z, child_point=-A.x + A.y,
+                       parent_interframe=Pint, child_interframe=Cint,
+                       rot_type='SPaCe', rot_order='zxy')
+    assert S._rot_type.upper() == 'SPACE'
+    assert S._rot_order == 'zxy'
+    assert simplify(N.dcm(A) - Matrix([
+        [-sin(q2) * cos(q1), -sin(q0) * cos(q2) + sin(q1) * sin(q2) * cos(q0),
+         sin(q0) * sin(q1) * sin(q2) + cos(q0) * cos(q2)],
+        [-sin(q1), -cos(q0) * cos(q1), -sin(q0) * cos(q1)],
+        [cos(q1) * cos(q2), -sin(q0) * sin(q2) - sin(q1) * cos(q0) * cos(q2),
+         -sin(q0) * sin(q1) * cos(q2) + sin(q2) * cos(q0)]]))
+    assert simplify(A.ang_vel_in(N).to_matrix(A) - Matrix([
+        [-u0 + u2 * sin(q1)], [-u1 * sin(q0) + u2 * cos(q0) * cos(q1)],
+        [u1 * cos(q0) + u2 * sin(q0) * cos(q1)]])) == zeros(3, 1)
+    assert simplify(C.masscenter.vel(N).to_matrix(A) - Matrix([
+        [u1 * cos(q0) + u2 * sin(q0) * cos(q1)],
+        [u1 * cos(q0) + u2 * sin(q0) * cos(q1)],
+        [u0 + u1 * sin(q0) - u2 * sin(q1) -
+         u2 * cos(q0) * cos(q1)]])) == zeros(3, 1)
+
+
+def test_weld_joint():
+    _, _, P, C = _generate_body()
+    W = WeldJoint('W', P, C)
+    assert W.name == 'W'
+    assert W.parent == P
+    assert W.child == C
+    assert W.coordinates == Matrix()
+    assert W.speeds == Matrix()
+    assert W.kdes == Matrix(1, 0, []).T
+    assert P.frame.dcm(C.frame) == eye(3)
+    assert W.child_point.pos_from(C.masscenter) == Vector(0)
+    assert W.parent_point.pos_from(P.masscenter) == Vector(0)
+    assert W.parent_point.pos_from(W.child_point) == Vector(0)
+    assert P.masscenter.pos_from(C.masscenter) == Vector(0)
+    assert C.masscenter.vel(P.frame) == Vector(0)
+    assert P.frame.ang_vel_in(C.frame) == 0
+    assert C.frame.ang_vel_in(P.frame) == 0
+    assert W.__str__() == 'WeldJoint: W  parent: P  child: C'
+
+    N, A, P, C = _generate_body()
+    l, m = symbols('l m')
+    Pint = ReferenceFrame('P_int')
+    Pint.orient_axis(P.frame, P.y, pi / 2)
+    W = WeldJoint('W', P, C, parent_point=l * P.frame.x,
+                  child_point=m * C.frame.y, parent_interframe=Pint)
+
+    assert W.child_point.pos_from(C.masscenter) == m * C.frame.y
+    assert W.parent_point.pos_from(P.masscenter) == l * P.frame.x
+    assert W.parent_point.pos_from(W.child_point) == Vector(0)
+    assert P.masscenter.pos_from(C.masscenter) == - l * N.x + m * A.y
+    assert C.masscenter.vel(P.frame) == Vector(0)
+    assert P.masscenter.vel(Pint) == Vector(0)
+    assert C.frame.ang_vel_in(P.frame) == 0
+    assert P.frame.ang_vel_in(C.frame) == 0
+    assert P.x == A.z
+
+    with warns_deprecated_sympy():
+        JointsMethod(P, W)  # Tests #10770
+
+
+def test_deprecated_parent_child_axis():
+    q, u = dynamicsymbols('q_J, u_J')
+    N, A, P, C = _generate_body()
+    with warns_deprecated_sympy():
+        PinJoint('J', P, C, child_axis=-A.x)
+    assert (-A.x).angle_between(N.x) == 0
+    assert -A.x.express(N) == N.x
+    assert A.dcm(N) == Matrix([[-1, 0, 0],
+                               [0, -cos(q), -sin(q)],
+                               [0, -sin(q), cos(q)]])
+    assert A.ang_vel_in(N) == u * N.x
+    assert A.ang_vel_in(N).magnitude() == sqrt(u ** 2)
+
+    N, A, P, C = _generate_body()
+    with warns_deprecated_sympy():
+        PrismaticJoint('J', P, C, parent_axis=P.x + P.y)
+    assert (A.x).angle_between(N.x + N.y) == 0
+    assert A.x.express(N) == (N.x + N.y) / sqrt(2)
+    assert A.dcm(N) == Matrix([[sqrt(2) / 2, sqrt(2) / 2, 0],
+                               [-sqrt(2) / 2, sqrt(2) / 2, 0], [0, 0, 1]])
+    assert A.ang_vel_in(N) == Vector(0)
+
+
+def test_deprecated_joint_pos():
+    N, A, P, C = _generate_body()
+    with warns_deprecated_sympy():
+        pin = PinJoint('J', P, C, parent_joint_pos=N.x + N.y,
+                       child_joint_pos=C.y - C.z)
+    assert pin.parent_point.pos_from(P.masscenter) == N.x + N.y
+    assert pin.child_point.pos_from(C.masscenter) == C.y - C.z
+
+    N, A, P, C = _generate_body()
+    with warns_deprecated_sympy():
+        slider = PrismaticJoint('J', P, C, parent_joint_pos=N.z + N.y,
+                                child_joint_pos=C.y - C.x)
+    assert slider.parent_point.pos_from(P.masscenter) == N.z + N.y
+    assert slider.child_point.pos_from(C.masscenter) == C.y - C.x

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.py

@@ -0,0 +1,249 @@
+from sympy.core.function import expand
+from sympy.core.symbol import symbols
+from sympy.functions.elementary.trigonometric import (cos, sin)
+from sympy.matrices.dense import Matrix
+from sympy.simplify.trigsimp import trigsimp
+from sympy.physics.mechanics import (
+    PinJoint, JointsMethod, RigidBody, Particle, Body, KanesMethod,
+    PrismaticJoint, LagrangesMethod, inertia)
+from sympy.physics.vector import dynamicsymbols, ReferenceFrame
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+from sympy import zeros
+from sympy.utilities.lambdify import lambdify
+from sympy.solvers.solvers import solve
+
+
+t = dynamicsymbols._t # type: ignore
+
+
+def test_jointsmethod():
+    with warns_deprecated_sympy():
+        P = Body('P')
+        C = Body('C')
+    Pin = PinJoint('P1', P, C)
+    C_ixx, g = symbols('C_ixx g')
+    q, u = dynamicsymbols('q_P1, u_P1')
+    P.apply_force(g*P.y)
+    with warns_deprecated_sympy():
+        method = JointsMethod(P, Pin)
+    assert method.frame == P.frame
+    assert method.bodies == [C, P]
+    assert method.loads == [(P.masscenter, g*P.frame.y)]
+    assert method.q == Matrix([q])
+    assert method.u == Matrix([u])
+    assert method.kdes == Matrix([u - q.diff()])
+    soln = method.form_eoms()
+    assert soln == Matrix([[-C_ixx*u.diff()]])
+    assert method.forcing_full == Matrix([[u], [0]])
+    assert method.mass_matrix_full == Matrix([[1, 0], [0, C_ixx]])
+    assert isinstance(method.method, KanesMethod)
+
+
+def test_rigid_body_particle_compatibility():
+    l, m, g = symbols('l m g')
+    C = RigidBody('C')
+    b = Particle('b', mass=m)
+    b_frame = ReferenceFrame('b_frame')
+    q, u = dynamicsymbols('q u')
+    P = PinJoint('P', C, b, coordinates=q, speeds=u, child_interframe=b_frame,
+                 child_point=-l * b_frame.x, joint_axis=C.z)
+    with warns_deprecated_sympy():
+        method = JointsMethod(C, P)
+    method.loads.append((b.masscenter, m * g * C.x))
+    method.form_eoms()
+    rhs = method.rhs()
+    assert rhs[1] == -g*sin(q)/l
+
+
+def test_jointmethod_duplicate_coordinates_speeds():
+    with warns_deprecated_sympy():
+        P = Body('P')
+        C = Body('C')
+        T = Body('T')
+    q, u = dynamicsymbols('q u')
+    P1 = PinJoint('P1', P, C, q)
+    P2 = PrismaticJoint('P2', C, T, q)
+    with warns_deprecated_sympy():
+        raises(ValueError, lambda: JointsMethod(P, P1, P2))
+
+    P1 = PinJoint('P1', P, C, speeds=u)
+    P2 = PrismaticJoint('P2', C, T, speeds=u)
+    with warns_deprecated_sympy():
+        raises(ValueError, lambda: JointsMethod(P, P1, P2))
+
+    P1 = PinJoint('P1', P, C, q, u)
+    P2 = PrismaticJoint('P2', C, T, q, u)
+    with warns_deprecated_sympy():
+        raises(ValueError, lambda: JointsMethod(P, P1, P2))
+
+def test_complete_simple_double_pendulum():
+    q1, q2 = dynamicsymbols('q1 q2')
+    u1, u2 = dynamicsymbols('u1 u2')
+    m, l, g = symbols('m l g')
+    with warns_deprecated_sympy():
+        C = Body('C')  # ceiling
+        PartP = Body('P', mass=m)
+        PartR = Body('R', mass=m)
+    J1 = PinJoint('J1', C, PartP, speeds=u1, coordinates=q1,
+                  child_point=-l*PartP.x, joint_axis=C.z)
+    J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2,
+                  child_point=-l*PartR.x, joint_axis=PartP.z)
+
+    PartP.apply_force(m*g*C.x)
+    PartR.apply_force(m*g*C.x)
+
+    with warns_deprecated_sympy():
+        method = JointsMethod(C, J1, J2)
+    method.form_eoms()
+
+    assert expand(method.mass_matrix_full) == Matrix([[1, 0, 0, 0],
+                                                      [0, 1, 0, 0],
+                                                      [0, 0, 2*l**2*m*cos(q2) + 3*l**2*m, l**2*m*cos(q2) + l**2*m],
+                                                      [0, 0, l**2*m*cos(q2) + l**2*m, l**2*m]])
+    assert trigsimp(method.forcing_full) == trigsimp(Matrix([[u1], [u2], [-g*l*m*(sin(q1 + q2) + sin(q1)) -
+                                           g*l*m*sin(q1) + l**2*m*(2*u1 + u2)*u2*sin(q2)],
+                                          [-g*l*m*sin(q1 + q2) - l**2*m*u1**2*sin(q2)]]))
+
+def test_two_dof_joints():
+    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
+    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
+    with warns_deprecated_sympy():
+        W = Body('W')
+        B1 = Body('B1', mass=m)
+        B2 = Body('B2', mass=m)
+    J1 = PrismaticJoint('J1', W, B1, coordinates=q1, speeds=u1)
+    J2 = PrismaticJoint('J2', B1, B2, coordinates=q2, speeds=u2)
+    W.apply_force(k1*q1*W.x, reaction_body=B1)
+    W.apply_force(c1*u1*W.x, reaction_body=B1)
+    B1.apply_force(k2*q2*W.x, reaction_body=B2)
+    B1.apply_force(c2*u2*W.x, reaction_body=B2)
+    with warns_deprecated_sympy():
+        method = JointsMethod(W, J1, J2)
+    method.form_eoms()
+    MM = method.mass_matrix
+    forcing = method.forcing
+    rhs = MM.LUsolve(forcing)
+    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_simple_pedulum():
+    l, m, g = symbols('l m g')
+    with warns_deprecated_sympy():
+        C = Body('C')
+        b = Body('b', mass=m)
+    q = dynamicsymbols('q')
+    P = PinJoint('P', C, b, speeds=q.diff(t), coordinates=q,
+                 child_point=-l * b.x, joint_axis=C.z)
+    b.potential_energy = - m * g * l * cos(q)
+    with warns_deprecated_sympy():
+        method = JointsMethod(C, P)
+    method.form_eoms(LagrangesMethod)
+    rhs = method.rhs()
+    assert rhs[1] == -g*sin(q)/l
+
+def test_chaos_pendulum():
+    #https://www.pydy.org/examples/chaos_pendulum.html
+    mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g = symbols('mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g')
+    theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha')
+
+    A = ReferenceFrame('A')
+    B = ReferenceFrame('B')
+
+    with warns_deprecated_sympy():
+        rod = Body('rod', mass=mA, frame=A,
+                   central_inertia=inertia(A, IAxx, IAxx, 0))
+        plate = Body('plate', mass=mB, frame=B,
+                     central_inertia=inertia(B, IBxx, IByy, IBzz))
+        C = Body('C')
+    J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega,
+                  child_point=-lA * rod.z, joint_axis=C.y)
+    J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha,
+                  parent_point=(lB - lA) * rod.z, joint_axis=rod.z)
+
+    rod.apply_force(mA*g*C.z)
+    plate.apply_force(mB*g*C.z)
+
+    with warns_deprecated_sympy():
+        method = JointsMethod(C, J1, J2)
+    method.form_eoms()
+
+    MM = method.mass_matrix
+    forcing = method.forcing
+    rhs = MM.LUsolve(forcing)
+    xd = (-2 * IBxx * alpha * omega * sin(phi) * cos(phi) + 2 * IByy * alpha * omega * sin(phi) *
+            cos(phi) - g * lA * mA * sin(theta) - g * lB * mB * sin(theta)) / (IAxx + IBxx *
+                sin(phi)**2 + IByy * cos(phi)**2 + lA**2 * mA + lB**2 * mB)
+    assert (rhs[0] - xd).simplify() == 0
+    xd = (IBxx - IByy) * omega**2 * sin(phi) * cos(phi) / IBzz
+    assert (rhs[1] - xd).simplify() == 0
+
+def test_four_bar_linkage_with_manual_constraints():
+    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4, u1:4')
+    l1, l2, l3, l4, rho = symbols('l1:5, rho')
+
+    N = ReferenceFrame('N')
+    inertias = [inertia(N, 0, 0, rho * l ** 3 / 12) for l in (l1, l2, l3, l4)]
+    with warns_deprecated_sympy():
+        link1 = Body('Link1', frame=N, mass=rho * l1,
+                     central_inertia=inertias[0])
+        link2 = Body('Link2', mass=rho * l2, central_inertia=inertias[1])
+        link3 = Body('Link3', mass=rho * l3, central_inertia=inertias[2])
+        link4 = Body('Link4', mass=rho * l4, central_inertia=inertias[3])
+
+    joint1 = PinJoint(
+        'J1', link1, link2, coordinates=q1, speeds=u1, joint_axis=link1.z,
+        parent_point=l1 / 2 * link1.x, child_point=-l2 / 2 * link2.x)
+    joint2 = PinJoint(
+        'J2', link2, link3, coordinates=q2, speeds=u2, joint_axis=link2.z,
+        parent_point=l2 / 2 * link2.x, child_point=-l3 / 2 * link3.x)
+    joint3 = PinJoint(
+        'J3', link3, link4, coordinates=q3, speeds=u3, joint_axis=link3.z,
+        parent_point=l3 / 2 * link3.x, child_point=-l4 / 2 * link4.x)
+
+    loop = link4.masscenter.pos_from(link1.masscenter) \
+           + l1 / 2 * link1.x + l4 / 2 * link4.x
+
+    fh = Matrix([loop.dot(link1.x), loop.dot(link1.y)])
+
+    with warns_deprecated_sympy():
+        method = JointsMethod(link1, joint1, joint2, joint3)
+
+    t = dynamicsymbols._t
+    qdots = solve(method.kdes, [q1.diff(t), q2.diff(t), q3.diff(t)])
+    fhd = fh.diff(t).subs(qdots)
+
+    kane = KanesMethod(method.frame, q_ind=[q1], u_ind=[u1],
+                       q_dependent=[q2, q3], u_dependent=[u2, u3],
+                       kd_eqs=method.kdes, configuration_constraints=fh,
+                       velocity_constraints=fhd, forcelist=method.loads,
+                       bodies=method.bodies)
+    fr, frs = kane.kanes_equations()
+    assert fr == zeros(1)
+
+    # Numerically check the mass- and forcing-matrix
+    p = Matrix([l1, l2, l3, l4, rho])
+    q = Matrix([q1, q2, q3])
+    u = Matrix([u1, u2, u3])
+    eval_m = lambdify((q, p), kane.mass_matrix)
+    eval_f = lambdify((q, u, p), kane.forcing)
+    eval_fhd = lambdify((q, u, p), fhd)
+
+    p_vals = [0.13, 0.24, 0.21, 0.34, 997]
+    q_vals = [2.1, 0.6655470375077588, 2.527408138024188]  # Satisfies fh
+    u_vals = [0.2, -0.17963733938852067, 0.1309060540601612]  # Satisfies fhd
+    mass_check = Matrix([[3.452709815256506e+01, 7.003948798374735e+00,
+                          -4.939690970641498e+00],
+                         [-2.203792703880936e-14, 2.071702479957077e-01,
+                          2.842917573033711e-01],
+                         [-1.300000000000123e-01, -8.836934896046506e-03,
+                          1.864891330060847e-01]])
+    forcing_check = Matrix([[-0.031211821321648],
+                            [-0.00066022608181],
+                            [0.001813559741243]])
+    eps = 1e-10
+    assert all(abs(x) < eps for x in eval_fhd(q_vals, u_vals, p_vals))
+    assert all(abs(x) < eps for x in
+               (Matrix(eval_m(q_vals, p_vals)) - mass_check))
+    assert all(abs(x) < eps for x in
+               (Matrix(eval_f(q_vals, u_vals, p_vals)) - forcing_check))

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane.py

@@ -0,0 +1,553 @@
+from sympy import solve
+from sympy import (cos, expand, Matrix, sin, symbols, tan, sqrt, S,
+                                zeros, eye)
+from sympy.simplify.simplify import simplify
+from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
+                                     RigidBody, KanesMethod, inertia, Particle,
+                                     dot, find_dynamicsymbols)
+from sympy.testing.pytest import raises
+
+
+def test_invalid_coordinates():
+    # Simple pendulum, but use symbols instead of dynamicsymbols
+    l, m, g = symbols('l m g')
+    q, u = symbols('q u')  # Generalized coordinate
+    kd = [q.diff(dynamicsymbols._t) - u]
+    N, O = ReferenceFrame('N'), Point('O')
+    O.set_vel(N, 0)
+    P = Particle('P', Point('P'), m)
+    P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
+    F = (P.point, -m * g * N.y)
+    raises(ValueError, lambda: KanesMethod(N, [q], [u], kd, bodies=[P],
+                                           forcelist=[F]))
+
+
+def test_one_dof():
+    # This is for a 1 dof spring-mass-damper case.
+    # It is described in more detail in the KanesMethod docstring.
+    q, u = dynamicsymbols('q u')
+    qd, ud = dynamicsymbols('q u', 1)
+    m, c, k = symbols('m c k')
+    N = ReferenceFrame('N')
+    P = Point('P')
+    P.set_vel(N, u * N.x)
+
+    kd = [qd - u]
+    FL = [(P, (-k * q - c * u) * N.x)]
+    pa = Particle('pa', P, m)
+    BL = [pa]
+
+    KM = KanesMethod(N, [q], [u], kd)
+    KM.kanes_equations(BL, FL)
+
+    assert KM.bodies == BL
+    assert KM.loads == FL
+
+    MM = KM.mass_matrix
+    forcing = KM.forcing
+    rhs = MM.inv() * forcing
+    assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
+
+    assert simplify(KM.rhs() -
+                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
+
+    assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
+
+
+def test_two_dof():
+    # This is for a 2 d.o.f., 2 particle spring-mass-damper.
+    # The first coordinate is the displacement of the first particle, and the
+    # second is the relative displacement between the first and second
+    # particles. Speeds are defined as the time derivatives of the particles.
+    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
+    q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
+    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
+    N = ReferenceFrame('N')
+    P1 = Point('P1')
+    P2 = Point('P2')
+    P1.set_vel(N, u1 * N.x)
+    P2.set_vel(N, (u1 + u2) * N.x)
+    # 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.
+    FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
+        q2 - c2 * u2) * N.x)]
+    pa1 = Particle('pa1', P1, m)
+    pa2 = Particle('pa2', P2, m)
+    BL = [pa1, pa2]
+
+    # Finally we create the KanesMethod object, specify the inertial frame,
+    # pass relevant information, and form Fr & Fr*. Then we calculate the mass
+    # matrix and forcing terms, and finally solve for the udots.
+    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
+    KM.kanes_equations(BL, FL)
+    MM = KM.mass_matrix
+    forcing = KM.forcing
+    rhs = MM.inv() * forcing
+    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)
+
+    # 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 consistent 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.
+    kd = [q1d - u1**2, sin(q2d) - cos(u2)]
+    raises(ValueError, lambda: KanesMethod(N, q_ind=[q1, q2],
+                                           u_ind=[u1, u2], kd_eqs=kd))
+
+def test_pend():
+    q, u = dynamicsymbols('q u')
+    qd, ud = dynamicsymbols('q u', 1)
+    m, l, g = symbols('m l g')
+    N = ReferenceFrame('N')
+    P = Point('P')
+    P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
+    kd = [qd - u]
+
+    FL = [(P, m * g * N.x)]
+    pa = Particle('pa', P, m)
+    BL = [pa]
+
+    KM = KanesMethod(N, [q], [u], kd)
+    KM.kanes_equations(BL, FL)
+    MM = KM.mass_matrix
+    forcing = KM.forcing
+    rhs = MM.inv() * forcing
+    rhs.simplify()
+    assert expand(rhs[0]) == expand(-g / l * sin(q))
+    assert simplify(KM.rhs() -
+                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
+
+
+def test_rolling_disc():
+    # Rolling Disc Example
+    # Here the rolling disc is formed from the contact point up, removing the
+    # need to introduce generalized speeds. Only 3 configuration and three
+    # speed variables are need to describe this system, along with the disc's
+    # mass and radius, and the local gravity (note that mass will drop out).
+    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
+    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
+    r, m, g = symbols('r m g')
+
+    # The kinematics are formed by a series of simple rotations. Each simple
+    # rotation creates a new frame, and the next rotation is defined by the new
+    # frame's basis vectors. This example uses a 3-1-2 series of rotations, or
+    # Z, X, Y series of rotations. Angular velocity for this is defined using
+    # the second frame's basis (the lean frame).
+    N = ReferenceFrame('N')
+    Y = N.orientnew('Y', 'Axis', [q1, N.z])
+    L = Y.orientnew('L', 'Axis', [q2, Y.x])
+    R = L.orientnew('R', 'Axis', [q3, L.y])
+    w_R_N_qd = R.ang_vel_in(N)
+    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
+
+    # This is the translational kinematics. We create a point with no velocity
+    # in N; this is the contact point between the disc and ground. Next we form
+    # the position vector from the contact point to the disc's center of mass.
+    # Finally we form the velocity and acceleration of the disc.
+    C = Point('C')
+    C.set_vel(N, 0)
+    Dmc = C.locatenew('Dmc', r * L.z)
+    Dmc.v2pt_theory(C, N, R)
+
+    # This is a simple way to form the inertia dyadic.
+    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
+
+    # Kinematic differential equations; how the generalized coordinate time
+    # derivatives relate to generalized speeds.
+    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
+
+    # Creation of the force list; it is the gravitational force at the mass
+    # center of the disc. Then we create the disc by assigning a Point to the
+    # center of mass attribute, a ReferenceFrame to the frame attribute, and mass
+    # and inertia. Then we form the body list.
+    ForceList = [(Dmc, - m * g * Y.z)]
+    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
+    BodyList = [BodyD]
+
+    # Finally we form the equations of motion, using the same steps we did
+    # before. Specify inertial frame, supply generalized speeds, supply
+    # kinematic differential equation dictionary, compute Fr from the force
+    # list and Fr* from the body list, compute the mass matrix and forcing
+    # terms, then solve for the u dots (time derivatives of the generalized
+    # speeds).
+    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
+    KM.kanes_equations(BodyList, ForceList)
+    MM = KM.mass_matrix
+    forcing = KM.forcing
+    rhs = MM.inv() * forcing
+    kdd = KM.kindiffdict()
+    rhs = rhs.subs(kdd)
+    rhs.simplify()
+    assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) +
+        4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()
+    assert simplify(KM.rhs() -
+                    KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(6, 1)
+
+    # This code tests our output vs. benchmark values. When r=g=m=1, the
+    # critical speed (where all eigenvalues of the linearized equations are 0)
+    # is 1 / sqrt(3) for the upright case.
+    A = KM.linearize(A_and_B=True)[0]
+    A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0})
+    import sympy
+    assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {S.Zero: 6}
+
+
+def test_aux():
+    # Same as above, except we have 2 auxiliary speeds for the ground contact
+    # point, which is known to be zero. In one case, we go through then
+    # substitute the aux. speeds in at the end (they are zero, as well as their
+    # derivative), in the other case, we use the built-in auxiliary speed part
+    # of KanesMethod. The equations from each should be the same.
+    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
+    q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
+    u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
+    u4d, u5d = dynamicsymbols('u4, u5', 1)
+    r, m, g = symbols('r m g')
+
+    N = ReferenceFrame('N')
+    Y = N.orientnew('Y', 'Axis', [q1, N.z])
+    L = Y.orientnew('L', 'Axis', [q2, Y.x])
+    R = L.orientnew('R', 'Axis', [q3, L.y])
+    w_R_N_qd = R.ang_vel_in(N)
+    R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
+
+    C = Point('C')
+    C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
+    Dmc = C.locatenew('Dmc', r * L.z)
+    Dmc.v2pt_theory(C, N, R)
+    Dmc.a2pt_theory(C, N, R)
+
+    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
+
+    kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
+
+    ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
+    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
+    BodyList = [BodyD]
+
+    KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
+                     kd_eqs=kd)
+    (fr, frstar) = KM.kanes_equations(BodyList, ForceList)
+    fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
+    frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
+
+    KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
+                      u_auxiliary=[u4, u5])
+    (fr2, frstar2) = KM2.kanes_equations(BodyList, ForceList)
+    fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
+    frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
+
+    frstar.simplify()
+    frstar2.simplify()
+
+    assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
+    assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
+
+
+def test_parallel_axis():
+    # This is for a 2 dof inverted pendulum on a cart.
+    # This tests the parallel axis code in KanesMethod. The inertia of the
+    # pendulum is defined about the hinge, not about the center of mass.
+
+    # Defining the constants and knowns of the system
+    gravity = symbols('g')
+    k, ls = symbols('k ls')
+    a, mA, mC = symbols('a mA mC')
+    F = dynamicsymbols('F')
+    Ix, Iy, Iz = symbols('Ix Iy Iz')
+
+    # Declaring the Generalized coordinates and speeds
+    q1, q2 = dynamicsymbols('q1 q2')
+    q1d, q2d = dynamicsymbols('q1 q2', 1)
+    u1, u2 = dynamicsymbols('u1 u2')
+    u1d, u2d = dynamicsymbols('u1 u2', 1)
+
+    # Creating reference frames
+    N = ReferenceFrame('N')
+    A = ReferenceFrame('A')
+
+    A.orient(N, 'Axis', [-q2, N.z])
+    A.set_ang_vel(N, -u2 * N.z)
+
+    # Origin of Newtonian reference frame
+    O = Point('O')
+
+    # Creating and Locating the positions of the cart, C, and the
+    # center of mass of the pendulum, A
+    C = O.locatenew('C', q1 * N.x)
+    Ao = C.locatenew('Ao', a * A.y)
+
+    # Defining velocities of the points
+    O.set_vel(N, 0)
+    C.set_vel(N, u1 * N.x)
+    Ao.v2pt_theory(C, N, A)
+    Cart = Particle('Cart', C, mC)
+    Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
+
+    # kinematical differential equations
+
+    kindiffs = [q1d - u1, q2d - u2]
+
+    bodyList = [Cart, Pendulum]
+
+    forceList = [(Ao, -N.y * gravity * mA),
+                 (C, -N.y * gravity * mC),
+                 (C, -N.x * k * (q1 - ls)),
+                 (C, N.x * F)]
+
+    km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
+    (fr, frstar) = km.kanes_equations(bodyList, forceList)
+    mm = km.mass_matrix_full
+    assert mm[3, 3] == Iz
+
+def test_input_format():
+    # 1 dof problem from test_one_dof
+    q, u = dynamicsymbols('q u')
+    qd, ud = dynamicsymbols('q u', 1)
+    m, c, k = symbols('m c k')
+    N = ReferenceFrame('N')
+    P = Point('P')
+    P.set_vel(N, u * N.x)
+
+    kd = [qd - u]
+    FL = [(P, (-k * q - c * u) * N.x)]
+    pa = Particle('pa', P, m)
+    BL = [pa]
+
+    KM = KanesMethod(N, [q], [u], kd)
+    # test for input format kane.kanes_equations((body1, body2, particle1))
+    assert KM.kanes_equations(BL)[0] == Matrix([0])
+    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
+    assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
+    # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
+    assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
+    # test for input format kane.kanes_equations(bodies=(body1, body 2))
+    assert KM.kanes_equations(BL)[0] == Matrix([0])
+    # test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
+    assert KM.kanes_equations(BL, [])[0] == Matrix([0])
+    # test for error raised when a wrong force list (in this case a string) is provided
+    raises(ValueError, lambda: KM._form_fr('bad input'))
+
+    # 1 dof problem from test_one_dof with FL & BL in instance
+    KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
+    assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])
+
+    # 2 dof problem from test_two_dof
+    q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
+    q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
+    m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
+    N = ReferenceFrame('N')
+    P1 = Point('P1')
+    P2 = Point('P2')
+    P1.set_vel(N, u1 * N.x)
+    P2.set_vel(N, (u1 + u2) * N.x)
+    kd = [q1d - u1, q2d - u2]
+
+    FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
+        q2 - c2 * u2) * N.x))
+    pa1 = Particle('pa1', P1, m)
+    pa2 = Particle('pa2', P2, m)
+    BL = (pa1, pa2)
+
+    KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
+    # test for input format
+    # kane.kanes_equations((body1, body2), (load1, load2))
+    KM.kanes_equations(BL, FL)
+    MM = KM.mass_matrix
+    forcing = KM.forcing
+    rhs = MM.inv() * forcing
+    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:
+        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
+
+def test_issue_24887():
+    # Spherical pendulum
+    g, l, m, c = symbols('g l m c')
+    q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4 u1:4')
+    N = ReferenceFrame('N')
+    A = ReferenceFrame('A')
+    A.orient_body_fixed(N, (q1, q2, q3), 'zxy')
+    N_w_A = A.ang_vel_in(N)
+    # A.set_ang_vel(N, u1 * A.x + u2 * A.y + u3 * A.z)
+    kdes = [N_w_A.dot(A.x) - u1, N_w_A.dot(A.y) - u2, N_w_A.dot(A.z) - u3]
+    O = Point('O')
+    O.set_vel(N, 0)
+    Po = O.locatenew('Po', -l * A.y)
+    Po.set_vel(A, 0)
+    P = Particle('P', Po, m)
+    kane = KanesMethod(N, [q1, q2, q3], [u1, u2, u3], kdes, bodies=[P],
+                       forcelist=[(Po, -m * g * N.y)])
+    kane.kanes_equations()
+    expected_md = m * l ** 2 * Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 1]])
+    expected_fd = Matrix([
+        [l*m*(g*(sin(q1)*sin(q3) - sin(q2)*cos(q1)*cos(q3)) - l*u2*u3)],
+        [0], [l*m*(-g*(sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)) + l*u1*u2)]])
+    assert find_dynamicsymbols(kane.forcing).issubset({q1, q2, q3, u1, u2, u3})
+    assert simplify(kane.mass_matrix - expected_md) == zeros(3, 3)
+    assert simplify(kane.forcing - expected_fd) == zeros(3, 1)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane2.py

@@ -0,0 +1,464 @@
+from sympy import cos, Matrix, sin, zeros, tan, pi, symbols
+from sympy.simplify.simplify import simplify
+from sympy.simplify.trigsimp import trigsimp
+from sympy.solvers.solvers import solve
+from sympy.physics.mechanics import (cross, dot, dynamicsymbols,
+                                     find_dynamicsymbols, KanesMethod, inertia,
+                                     inertia_of_point_mass, Point,
+                                     ReferenceFrame, RigidBody)
+
+
+def test_aux_dep():
+    # This test is about rolling disc dynamics, comparing the results found
+    # with KanesMethod to those found when deriving the equations "manually"
+    # with SymPy.
+    # The terms Fr, Fr*, and Fr*_steady are all compared between the two
+    # methods. Here, Fr*_steady refers to the generalized inertia forces for an
+    # equilibrium configuration.
+    # Note: comparing to the test of test_rolling_disc() in test_kane.py, this
+    # test also tests auxiliary speeds and configuration and motion constraints
+    #, seen in  the generalized dependent coordinates q[3], and depend speeds
+    # u[3], u[4] and u[5].
+
+
+    # First, manual derivation of Fr, Fr_star, Fr_star_steady.
+
+    # Symbols for time and constant parameters.
+    # Symbols for contact forces: Fx, Fy, Fz.
+    t, r, m, g, I, J = symbols('t r m g I J')
+    Fx, Fy, Fz = symbols('Fx Fy Fz')
+
+    # Configuration variables and their time derivatives:
+    # q[0] -- yaw
+    # q[1] -- lean
+    # q[2] -- spin
+    # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
+    #         A.z direction
+    # Generalized speeds and their time derivatives:
+    # u[0] -- disc angular velocity component, disc fixed x direction
+    # u[1] -- disc angular velocity component, disc fixed y direction
+    # u[2] -- disc angular velocity component, disc fixed z direction
+    # u[3] -- disc velocity component, A.x direction
+    # u[4] -- disc velocity component, A.y direction
+    # u[5] -- disc velocity component, A.z direction
+    # Auxiliary generalized speeds:
+    # ua[0] -- contact point auxiliary generalized speed, A.x direction
+    # ua[1] -- contact point auxiliary generalized speed, A.y direction
+    # ua[2] -- contact point auxiliary generalized speed, A.z direction
+    q = dynamicsymbols('q:4')
+    qd = [qi.diff(t) for qi in q]
+    u = dynamicsymbols('u:6')
+    ud = [ui.diff(t) for ui in u]
+    ud_zero = dict(zip(ud, [0.]*len(ud)))
+    ua = dynamicsymbols('ua:3')
+    ua_zero = dict(zip(ua, [0.]*len(ua))) # noqa:F841
+
+    # Reference frames:
+    # Yaw intermediate frame: A.
+    # Lean intermediate frame: B.
+    # Disc fixed frame: C.
+    N = ReferenceFrame('N')
+    A = N.orientnew('A', 'Axis', [q[0], N.z])
+    B = A.orientnew('B', 'Axis', [q[1], A.x])
+    C = B.orientnew('C', 'Axis', [q[2], B.y])
+
+    # Angular velocity and angular acceleration of disc fixed frame
+    # u[0], u[1] and u[2] are generalized independent speeds.
+    C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
+    C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+                   + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
+
+    # Velocity and acceleration of points:
+    # Disc-ground contact point: P.
+    # Center of disc: O, defined from point P with depend coordinate: q[3]
+    # u[3], u[4] and u[5] are generalized dependent speeds.
+    P = Point('P')
+    P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
+    O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
+    O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
+    O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
+
+    # Kinematic differential equations:
+    # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
+    #                 directions of B, for qd0, qd1 and qd2.
+    #                 the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
+    # Then, solve for dq/dt's in terms of u's: qd_kd.
+    w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
+    v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
+    kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
+                      [dot(v_o_n_qd - O.vel(N), A.z)])
+    qd_kd = solve(kindiffs, qd) # noqa:F841
+
+    # Values of generalized speeds during a steady turn for later substitution
+    # into the Fr_star_steady.
+    steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
+    steady_conditions.update({qd[1] : 0, qd[3] : 0})
+
+    # Partial angular velocities and velocities.
+    partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
+    partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
+    partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
+
+    # Configuration constraint: f_c, the projection of radius r in A.z direction
+    #                                is q[3].
+    # Velocity constraints: f_v, for u3, u4 and u5.
+    # Acceleration constraints: f_a.
+    f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
+    f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
+        O.pos_from(P))), ai).expand() for ai in A])
+    v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
+    a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
+    f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # noqa:F841
+
+    # Solve for constraint equations in the form of
+    #                           u_dependent = A_rs * [u_i; u_aux].
+    # First, obtain constraint coefficient matrix:  M_v * [u; ua] = 0;
+    # Second, taking u[0], u[1], u[2] as independent,
+    #         taking u[3], u[4], u[5] as dependent,
+    #         rearranging the matrix of M_v to be A_rs for u_dependent.
+    # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
+    M_v = zeros(3, 9)
+    for i in range(3):
+        for j, ui in enumerate(u + ua):
+            M_v[i, j] = f_v[i].diff(ui)
+
+    M_v_i = M_v[:, :3]
+    M_v_d = M_v[:, 3:6]
+    M_v_aux = M_v[:, 6:]
+    M_v_i_aux = M_v_i.row_join(M_v_aux)
+    A_rs = - M_v_d.inv() * M_v_i_aux
+
+    u_dep = A_rs[:, :3] * Matrix(u[:3])
+    u_dep_dict = dict(zip(u[3:], u_dep))
+
+    # Active forces: F_O acting on point O; F_P acting on point P.
+    # Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
+    F_O = m*g*A.z
+    F_P = Fx * A.x + Fy * A.y + Fz * A.z
+    Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
+            zip(partial_v_O, partial_v_P)])
+
+    # Inertia force: R_star_O.
+    # Inertia of disc: I_C_O, where J is a inertia component about principal axis.
+    # Inertia torque: T_star_C.
+    # Generalized inertia forces (unconstrained): Fr_star_u.
+    R_star_O = -m*O.acc(N)
+    I_C_O = inertia(B, I, J, I)
+    T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+                 + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
+    Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
+                        zip(partial_v_O, partial_w_C)])
+
+    # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
+    # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
+    Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
+    Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+                + A_rs.T * Fr_star_u[3:6, :]
+    Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
+            .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
+
+
+    # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
+
+    # Rigid Bodies: disc, with inertia I_C_O.
+    iner_tuple = (I_C_O, O)
+    disc = RigidBody('disc', O, C, m, iner_tuple)
+    bodyList = [disc]
+
+    # Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
+    F_o = (O, F_O)
+    F_p = (P, F_P)
+    forceList = [F_o,  F_p]
+
+    # KanesMethod.
+    kane = KanesMethod(
+        N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
+        q_dependent=q[3:], configuration_constraints = f_c,
+        u_dependent=u[3:], velocity_constraints= f_v,
+        u_auxiliary=ua
+        )
+
+    # fr, frstar, frstar_steady and kdd(kinematic differential equations).
+    (fr, frstar)= kane.kanes_equations(bodyList, forceList)
+    frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
+                    .subs({q[3]: -r*cos(q[1])}).expand()
+    kdd = kane.kindiffdict()
+
+    assert Matrix(Fr_c).expand() == fr.expand()
+    assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
+    # These Matrices have some Integer(0) and some Float(0). Running under
+    # SymEngine gives different types of zero.
+    assert (simplify(Matrix(Fr_star_steady).expand()).xreplace({0:0.0}) ==
+            simplify(frstar_steady.expand()).xreplace({0:0.0}))
+
+    syms_in_forcing = find_dynamicsymbols(kane.forcing)
+    for qdi in qd:
+        assert qdi not in syms_in_forcing
+
+
+def test_non_central_inertia():
+    # This tests that the calculation of Fr* does not depend the point
+    # about which the inertia of a rigid body is defined. This test solves
+    # exercises 8.12, 8.17 from Kane 1985.
+
+    # Declare symbols
+    q1, q2, q3 = dynamicsymbols('q1:4')
+    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
+    u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
+    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
+    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
+    Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
+    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
+
+    # Reference Frames
+    F = ReferenceFrame('F')
+    P = F.orientnew('P', 'axis', [-theta, F.y])
+    A = P.orientnew('A', 'axis', [q1, P.x])
+    A.set_ang_vel(F, u1*A.x + u3*A.z)
+    # define frames for wheels
+    B = A.orientnew('B', 'axis', [q2, A.z])
+    C = A.orientnew('C', 'axis', [q3, A.z])
+    B.set_ang_vel(A, u4 * A.z)
+    C.set_ang_vel(A, u5 * A.z)
+
+    # define points D, S*, Q on frame A and their velocities
+    pD = Point('D')
+    pD.set_vel(A, 0)
+    # u3 will not change v_D_F since wheels are still assumed to roll without slip.
+    pD.set_vel(F, u2 * A.y)
+
+    pS_star = pD.locatenew('S*', e*A.y)
+    pQ = pD.locatenew('Q', f*A.y - R*A.x)
+    for p in [pS_star, pQ]:
+        p.v2pt_theory(pD, F, A)
+
+    # masscenters of bodies A, B, C
+    pA_star = pD.locatenew('A*', a*A.y)
+    pB_star = pD.locatenew('B*', b*A.z)
+    pC_star = pD.locatenew('C*', -b*A.z)
+    for p in [pA_star, pB_star, pC_star]:
+        p.v2pt_theory(pD, F, A)
+
+    # points of B, C touching the plane P
+    pB_hat = pB_star.locatenew('B^', -R*A.x)
+    pC_hat = pC_star.locatenew('C^', -R*A.x)
+    pB_hat.v2pt_theory(pB_star, F, B)
+    pC_hat.v2pt_theory(pC_star, F, C)
+
+    # the velocities of B^, C^ are zero since B, C are assumed to roll without slip
+    kde = [q1d - u1, q2d - u4, q3d - u5]
+    vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
+
+    # inertias of bodies A, B, C
+    # IA22, IA23, IA33 are not specified in the problem statement, but are
+    # necessary to define an inertia object. Although the values of
+    # IA22, IA23, IA33 are not known in terms of the variables given in the
+    # problem statement, they do not appear in the general inertia terms.
+    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
+    inertia_B = inertia(B, K, K, J)
+    inertia_C = inertia(C, K, K, J)
+
+    # define the rigid bodies A, B, C
+    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
+    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
+    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
+
+    km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde,
+                     u_dependent=[u4, u5], velocity_constraints=vc,
+                     u_auxiliary=[u3])
+
+    forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
+    bodies = [rbA, rbB, rbC]
+    fr, fr_star = km.kanes_equations(bodies, forces)
+    vc_map = solve(vc, [u4, u5])
+
+    # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
+    fr_star_expected = Matrix([
+            -(IA + 2*J*b**2/R**2 + 2*K +
+              mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
+            -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
+            0])
+    t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
+    assert ((fr_star_expected - t).expand() == zeros(3, 1))
+
+    # define inertias of rigid bodies A, B, C about point D
+    # I_S/O = I_S/S* + I_S*/O
+    bodies2 = []
+    for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
+        I = I_star + inertia_of_point_mass(rb.mass,
+                                           rb.masscenter.pos_from(pD),
+                                           rb.frame)
+        bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
+                                 (I, pD)))
+    fr2, fr_star2 = km.kanes_equations(bodies2, forces)
+
+    t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
+    assert (fr_star_expected - t).expand() == zeros(3, 1)
+
+def test_sub_qdot():
+    # This test solves exercises 8.12, 8.17 from Kane 1985 and defines
+    # some velocities in terms of q, qdot.
+
+    ## --- Declare symbols ---
+    q1, q2, q3 = dynamicsymbols('q1:4')
+    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
+    u1, u2, u3 = dynamicsymbols('u1:4')
+    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
+    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
+    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
+    Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
+
+    # --- Reference Frames ---
+    F = ReferenceFrame('F')
+    P = F.orientnew('P', 'axis', [-theta, F.y])
+    A = P.orientnew('A', 'axis', [q1, P.x])
+    A.set_ang_vel(F, u1*A.x + u3*A.z)
+    # define frames for wheels
+    B = A.orientnew('B', 'axis', [q2, A.z])
+    C = A.orientnew('C', 'axis', [q3, A.z])
+
+    ## --- define points D, S*, Q on frame A and their velocities ---
+    pD = Point('D')
+    pD.set_vel(A, 0)
+    # u3 will not change v_D_F since wheels are still assumed to roll w/o slip
+    pD.set_vel(F, u2 * A.y)
+
+    pS_star = pD.locatenew('S*', e*A.y)
+    pQ = pD.locatenew('Q', f*A.y - R*A.x)
+    # masscenters of bodies A, B, C
+    pA_star = pD.locatenew('A*', a*A.y)
+    pB_star = pD.locatenew('B*', b*A.z)
+    pC_star = pD.locatenew('C*', -b*A.z)
+    for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
+        p.v2pt_theory(pD, F, A)
+
+    # points of B, C touching the plane P
+    pB_hat = pB_star.locatenew('B^', -R*A.x)
+    pC_hat = pC_star.locatenew('C^', -R*A.x)
+    pB_hat.v2pt_theory(pB_star, F, B)
+    pC_hat.v2pt_theory(pC_star, F, C)
+
+    # --- relate qdot, u ---
+    # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
+    kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
+    kde += [u1 - q1d]
+    kde_map = solve(kde, [q1d, q2d, q3d])
+    for k, v in list(kde_map.items()):
+        kde_map[k.diff(t)] = v.diff(t)
+
+    # inertias of bodies A, B, C
+    # IA22, IA23, IA33 are not specified in the problem statement, but are
+    # necessary to define an inertia object. Although the values of
+    # IA22, IA23, IA33 are not known in terms of the variables given in the
+    # problem statement, they do not appear in the general inertia terms.
+    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
+    inertia_B = inertia(B, K, K, J)
+    inertia_C = inertia(C, K, K, J)
+
+    # define the rigid bodies A, B, C
+    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
+    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
+    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
+
+    ## --- use kanes method ---
+    km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
+
+    forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
+    bodies = [rbA, rbB, rbC]
+
+    # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
+    # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
+    fr_expected = Matrix([
+            f*Q3 + M*g*e*sin(theta)*cos(q1),
+            Q2 + M*g*sin(theta)*sin(q1),
+            e*M*g*cos(theta) - Q1*f - Q2*R])
+             #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
+    fr_star_expected = Matrix([
+            -(IA + 2*J*b**2/R**2 + 2*K +
+              mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
+            -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
+            0])
+
+    fr, fr_star = km.kanes_equations(bodies, forces)
+    assert (fr.expand() == fr_expected.expand())
+    assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
+
+def test_sub_qdot2():
+    # This test solves exercises 8.3 from Kane 1985 and defines
+    # all velocities in terms of q, qdot. We check that the generalized active
+    # forces are correctly computed if u terms are only defined in the
+    # kinematic differential equations.
+    #
+    # This functionality was added in PR 8948. Without qdot/u substitution, the
+    # KanesMethod constructor will fail during the constraint initialization as
+    # the B matrix will be poorly formed and inversion of the dependent part
+    # will fail.
+
+    g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
+    q = dynamicsymbols('q:5')
+    qd = dynamicsymbols('q:5', level=1)
+    u = dynamicsymbols('u:5')
+
+    ## Define inertial, intermediate, and rigid body reference frames
+    A = ReferenceFrame('A')
+    B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
+    B = B_prime.orientnew('B', 'Axis', [pi/2 - q[1], B_prime.x])
+    C = B.orientnew('C', 'Axis', [q[2], B.z])
+
+    ## Define points of interest and their velocities
+    pO = Point('O')
+    pO.set_vel(A, 0)
+
+    # R is the point in plane H that comes into contact with disk C.
+    pR = pO.locatenew('R', q[3]*A.x + q[4]*A.y)
+    pR.set_vel(A, pR.pos_from(pO).diff(t, A))
+    pR.set_vel(B, 0)
+
+    # C^ is the point in disk C that comes into contact with plane H.
+    pC_hat = pR.locatenew('C^', 0)
+    pC_hat.set_vel(C, 0)
+
+    # C* is the point at the center of disk C.
+    pCs = pC_hat.locatenew('C*', R*B.y)
+    pCs.set_vel(C, 0)
+    pCs.set_vel(B, 0)
+
+    # calculate velocites of points C* and C^ in frame A
+    pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
+    pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
+
+    ## Define forces on each point of the system
+    R_C_hat = Px*A.x + Py*A.y + Pz*A.z
+    R_Cs = -m*g*A.z
+    forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
+
+    ## Define kinematic differential equations
+    # let ui = omega_C_A & bi (i = 1, 2, 3)
+    # u4 = qd4, u5 = qd5
+    u_expr = [C.ang_vel_in(A) & uv for uv in B]
+    u_expr += qd[3:]
+    kde = [ui - e for ui, e in zip(u, u_expr)]
+    km1 = KanesMethod(A, q, u, kde)
+    fr1, _ = km1.kanes_equations([], forces)
+
+    ## Calculate generalized active forces if we impose the condition that the
+    # disk C is rolling without slipping
+    u_indep = u[:3]
+    u_dep = list(set(u) - set(u_indep))
+    vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
+    km2 = KanesMethod(A, q, u_indep, kde,
+                      u_dependent=u_dep, velocity_constraints=vc)
+    fr2, _ = km2.kanes_equations([], forces)
+
+    fr1_expected = Matrix([
+        -R*g*m*sin(q[1]),
+        -R*(Px*cos(q[0]) + Py*sin(q[0]))*tan(q[1]),
+        R*(Px*cos(q[0]) + Py*sin(q[0])),
+        Px,
+        Py])
+    fr2_expected = Matrix([
+        -R*g*m*sin(q[1]),
+        0,
+        0])
+    assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
+    assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane3.py

@@ -0,0 +1,315 @@
+from sympy.core.numbers import pi
+from sympy.core.symbol import symbols
+from sympy.functions.elementary.miscellaneous import sqrt
+from sympy.functions.elementary.trigonometric import acos, sin, cos
+from sympy.matrices.dense import Matrix
+from sympy.physics.mechanics import (ReferenceFrame, dynamicsymbols,
+                                     KanesMethod, inertia, Point, RigidBody,
+                                     dot)
+from sympy.testing.pytest import slow
+
+
+@slow
+def test_bicycle():
+    # Code to get equations of motion for a bicycle modeled as in:
+    # J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
+    # dynamics equations for the balance and steer of a bicycle: a benchmark
+    # and review. Proceedings of The Royal Society (2007) 463, 1955-1982
+    # doi: 10.1098/rspa.2007.1857
+
+    # Note that this code has been crudely ported from Autolev, which is the
+    # reason for some of the unusual naming conventions. It was purposefully as
+    # similar as possible in order to aide debugging.
+
+    # Declare Coordinates & Speeds
+    # Simple definitions for qdots - qd = u
+    # Speeds are:
+    # - u1: yaw frame ang. rate
+    # - u2: roll frame ang. rate
+    # - u3: rear wheel frame ang. rate (spinning motion)
+    # - u4: frame ang. rate (pitching motion)
+    # - u5: steering frame ang. rate
+    # - u6: front wheel ang. rate (spinning motion)
+    # Wheel positions are ignorable coordinates, so they are not introduced.
+    q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
+    q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
+    u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
+    u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
+
+    # Declare System's Parameters
+    WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
+    forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
+    forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
+    Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
+    Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
+    Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
+    mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
+
+    # Set up reference frames for the system
+    # N - inertial
+    # Y - yaw
+    # R - roll
+    # WR - rear wheel, rotation angle is ignorable coordinate so not oriented
+    # Frame - bicycle frame
+    # TempFrame - statically rotated frame for easier reference inertia definition
+    # Fork - bicycle fork
+    # TempFork - statically rotated frame for easier reference inertia definition
+    # WF - front wheel, again posses a ignorable coordinate
+    N = ReferenceFrame('N')
+    Y = N.orientnew('Y', 'Axis', [q1, N.z])
+    R = Y.orientnew('R', 'Axis', [q2, Y.x])
+    Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
+    WR = ReferenceFrame('WR')
+    TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
+    Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
+    TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
+    WF = ReferenceFrame('WF')
+
+    # Kinematics of the Bicycle First block of code is forming the positions of
+    # the relevant points
+    # rear wheel contact -> rear wheel mass center -> frame mass center +
+    # frame/fork connection -> fork mass center + front wheel mass center ->
+    # front wheel contact point
+    WR_cont = Point('WR_cont')
+    WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
+    Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
+    Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x
+                                           + framecg3 * Frame.z)
+    Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x
+                                         + forkcg3 * Fork.z)
+    WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
+    WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y -
+                                                  Y.z).normalize())
+
+    # Set the angular velocity of each frame.
+    # Angular accelerations end up being calculated automatically by
+    # differentiating the angular velocities when first needed.
+    # u1 is yaw rate
+    # u2 is roll rate
+    # u3 is rear wheel rate
+    # u4 is frame pitch rate
+    # u5 is fork steer rate
+    # u6 is front wheel rate
+    Y.set_ang_vel(N, u1 * Y.z)
+    R.set_ang_vel(Y, u2 * R.x)
+    WR.set_ang_vel(Frame, u3 * Frame.y)
+    Frame.set_ang_vel(R, u4 * Frame.y)
+    Fork.set_ang_vel(Frame, u5 * Fork.x)
+    WF.set_ang_vel(Fork, u6 * Fork.y)
+
+    # Form the velocities of the previously defined points, using the 2 - point
+    # theorem (written out by hand here).  Accelerations again are calculated
+    # automatically when first needed.
+    WR_cont.set_vel(N, 0)
+    WR_mc.v2pt_theory(WR_cont, N, WR)
+    Steer.v2pt_theory(WR_mc, N, Frame)
+    Frame_mc.v2pt_theory(WR_mc, N, Frame)
+    Fork_mc.v2pt_theory(Steer, N, Fork)
+    WF_mc.v2pt_theory(Steer, N, Fork)
+    WF_cont.v2pt_theory(WF_mc, N, WF)
+
+    # Sets the inertias of each body. Uses the inertia frame to construct the
+    # inertia dyadics. Wheel inertias are only defined by principle moments of
+    # inertia, and are in fact constant in the frame and fork reference frames;
+    # it is for this reason that the orientations of the wheels does not need
+    # to be defined. The frame and fork inertias are defined in the 'Temp'
+    # frames which are fixed to the appropriate body frames; this is to allow
+    # easier input of the reference values of the benchmark paper. Note that
+    # due to slightly different orientations, the products of inertia need to
+    # have their signs flipped; this is done later when entering the numerical
+    # value.
+
+    Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
+    Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
+    WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
+    WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
+
+    # Declaration of the RigidBody containers. ::
+
+    BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
+    BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
+    BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
+    BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
+
+    # The kinematic differential equations; they are defined quite simply. Each
+    # entry in this list is equal to zero.
+    kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
+
+    # The nonholonomic constraints are the velocity of the front wheel contact
+    # point dotted into the X, Y, and Z directions; the yaw frame is used as it
+    # is "closer" to the front wheel (1 less DCM connecting them). These
+    # constraints force the velocity of the front wheel contact point to be 0
+    # in the inertial frame; the X and Y direction constraints enforce a
+    # "no-slip" condition, and the Z direction constraint forces the front
+    # wheel contact point to not move away from the ground frame, essentially
+    # replicating the holonomic constraint which does not allow the frame pitch
+    # to change in an invalid fashion.
+
+    conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
+
+    # The holonomic constraint is that the position from the rear wheel contact
+    # point to the front wheel contact point when dotted into the
+    # normal-to-ground plane direction must be zero; effectively that the front
+    # and rear wheel contact points are always touching the ground plane. This
+    # is actually not part of the dynamic equations, but instead is necessary
+    # for the lineraization process.
+
+    conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
+
+    # The force list; each body has the appropriate gravitational force applied
+    # at its mass center.
+    FL = [(Frame_mc, -mframe * g * Y.z),
+        (Fork_mc, -mfork * g * Y.z),
+        (WF_mc, -mwf * g * Y.z),
+        (WR_mc, -mwr * g * Y.z)]
+    BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
+
+
+    # The N frame is the inertial frame, coordinates are supplied in the order
+    # of independent, dependent coordinates, as are the speeds. The kinematic
+    # differential equation are also entered here.  Here the dependent speeds
+    # are specified, in the same order they were provided in earlier, along
+    # with the non-holonomic constraints.  The dependent coordinate is also
+    # provided, with the holonomic constraint.  Again, this is only provided
+    # for the linearization process.
+
+    KM = KanesMethod(N, q_ind=[q1, q2, q5],
+            q_dependent=[q4], configuration_constraints=conlist_coord,
+            u_ind=[u2, u3, u5],
+            u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
+            kd_eqs=kd,
+            constraint_solver="CRAMER")
+    (fr, frstar) = KM.kanes_equations(BL, FL)
+
+    # This is the start of entering in the numerical values from the benchmark
+    # paper to validate the eigen values of the linearized equations from this
+    # model to the reference eigen values. Look at the aforementioned paper for
+    # more information. Some of these are intermediate values, used to
+    # transform values from the paper into the coordinate systems used in this
+    # model.
+    PaperRadRear                    =  0.3
+    PaperRadFront                   =  0.35
+    HTA                             =  (pi / 2 - pi / 10).evalf()
+    TrailPaper                      =  0.08
+    rake                            =  (-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA)))).evalf()
+    PaperWb                         =  1.02
+    PaperFrameCgX                   =  0.3
+    PaperFrameCgZ                   =  0.9
+    PaperForkCgX                    =  0.9
+    PaperForkCgZ                    =  0.7
+    FrameLength                     =  (PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA))).evalf()
+    FrameCGNorm                     =  ((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA)).evalf()
+    FrameCGPar                      =  (PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)).evalf()
+    tempa                           =  (PaperForkCgZ - PaperRadFront)
+    tempb                           =  (PaperWb-PaperForkCgX)
+    tempc                           =  (sqrt(tempa**2+tempb**2)).evalf()
+    PaperForkL                      =  (PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)).evalf()
+    ForkCGNorm                      =  (rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc)))).evalf()
+    ForkCGPar                       =  (tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL).evalf()
+
+    # Here is the final assembly of the numerical values. The symbol 'v' is the
+    # forward speed of the bicycle (a concept which only makes sense in the
+    # upright, static equilibrium case?). These are in a dictionary which will
+    # later be substituted in. Again the sign on the *product* of inertia
+    # values is flipped here, due to different orientations of coordinate
+    # systems.
+    v = symbols('v')
+    val_dict = {WFrad: PaperRadFront,
+                WRrad: PaperRadRear,
+                htangle: HTA,
+                forkoffset: rake,
+                forklength: PaperForkL,
+                framelength: FrameLength,
+                forkcg1: ForkCGPar,
+                forkcg3: ForkCGNorm,
+                framecg1: FrameCGNorm,
+                framecg3: FrameCGPar,
+                Iwr11: 0.0603,
+                Iwr22: 0.12,
+                Iwf11: 0.1405,
+                Iwf22: 0.28,
+                Ifork11: 0.05892,
+                Ifork22: 0.06,
+                Ifork33: 0.00708,
+                Ifork31: 0.00756,
+                Iframe11: 9.2,
+                Iframe22: 11,
+                Iframe33: 2.8,
+                Iframe31: -2.4,
+                mfork: 4,
+                mframe: 85,
+                mwf: 3,
+                mwr: 2,
+                g: 9.81,
+                q1: 0,
+                q2: 0,
+                q4: 0,
+                q5: 0,
+                u1: 0,
+                u2: 0,
+                u3: v / PaperRadRear,
+                u4: 0,
+                u5: 0,
+                u6: v / PaperRadFront}
+
+    # Linearizes the forcing vector; the equations are set up as MM udot =
+    # forcing, where MM is the mass matrix, udot is the vector representing the
+    # time derivatives of the generalized speeds, and forcing is a vector which
+    # contains both external forcing terms and internal forcing terms, such as
+    # centripital or coriolis forces.  This actually returns a matrix with as
+    # many rows as *total* coordinates and speeds, but only as many columns as
+    # independent coordinates and speeds.
+
+    A, B, _ = KM.linearize(
+        A_and_B=True,
+        op_point={
+            # Operating points for the accelerations are required for the
+            # linearizer to eliminate u' terms showing up in the coefficient
+            # matrices.
+            u1.diff(): 0,
+            u2.diff(): 0,
+            u3.diff(): 0,
+            u4.diff(): 0,
+            u5.diff(): 0,
+            u6.diff(): 0,
+            u1: 0,
+            u2: 0,
+            u3: v / PaperRadRear,
+            u4: 0,
+            u5: 0,
+            u6: v / PaperRadFront,
+            q1: 0,
+            q2: 0,
+            q4: 0,
+            q5: 0,
+        },
+        linear_solver="CRAMER",
+    )
+    # As mentioned above, the size of the linearized forcing terms is expanded
+    # to include both q's and u's, so the mass matrix must have this done as
+    # well.  This will likely be changed to be part of the linearized process,
+    # for future reference.
+    A_s = A.xreplace(val_dict)
+    B_s = B.xreplace(val_dict)
+
+    A_s = A_s.evalf()
+    B_s = B_s.evalf()
+
+    # Finally, we construct an "A" matrix for the form xdot = A x (x being the
+    # state vector, although in this case, the sizes are a little off). The
+    # following line extracts only the minimum entries required for eigenvalue
+    # analysis, which correspond to rows and columns for lean, steer, lean
+    # rate, and steer rate.
+    A = A_s.extract([1, 2, 3, 5], [1, 2, 3, 5])
+
+    # Precomputed for comparison
+    Res = Matrix([[               0,                                           0,                  1.0,                    0],
+                  [               0,                                           0,                    0,                  1.0],
+                  [9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
+                  [11.7194768719633,   -1.97171508499972*v**2 + 30.9087533932407,   3.67680523332152*v,  -3.08486552743311*v]])
+
+    # Actual eigenvalue comparison
+    eps = 1.e-12
+    for i in range(6):
+        error = Res.subs(v, i) - A.subs(v, i)
+        assert all(abs(x) < eps for x in error)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane4.py

@@ -0,0 +1,115 @@
+from sympy import (cos, sin, Matrix, symbols)
+from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
+                                        KanesMethod, Particle)
+
+def test_replace_qdots_in_force():
+    # Test PR 16700 "Replaces qdots with us in force-list in kanes.py"
+    # The new functionality allows one to specify forces in qdots which will
+    # automatically be replaced with u:s which are defined by the kde supplied
+    # to KanesMethod. The test case is the double pendulum with interacting
+    # forces in the example of chapter 4.7 "CONTRIBUTING INTERACTION FORCES"
+    # in Ref. [1]. Reference list at end test function.
+
+    q1, q2 = dynamicsymbols('q1, q2')
+    qd1, qd2 = dynamicsymbols('q1, q2', level=1)
+    u1, u2 = dynamicsymbols('u1, u2')
+
+    l, m = symbols('l, m')
+
+    N = ReferenceFrame('N') # Inertial frame
+    A = N.orientnew('A', 'Axis', (q1, N.z)) # Rod A frame
+    B = A.orientnew('B', 'Axis', (q2, N.z)) # Rod B frame
+
+    O = Point('O') # Origo
+    O.set_vel(N, 0)
+
+    P = O.locatenew('P', ( l * A.x )) # Point @ end of rod A
+    P.v2pt_theory(O, N, A)
+
+    Q = P.locatenew('Q', ( l * B.x )) # Point @ end of rod B
+    Q.v2pt_theory(P, N, B)
+
+    Ap = Particle('Ap', P, m)
+    Bp = Particle('Bp', Q, m)
+
+    # The forces are specified below. sigma is the torsional spring stiffness
+    # and delta is the viscous damping coefficient acting between the two
+    # bodies. Here, we specify the viscous damper as function of qdots prior
+    # forming the kde. In more complex systems it not might be obvious which
+    # kde is most efficient, why it is convenient to specify viscous forces in
+    # qdots independently of the kde.
+    sig, delta = symbols('sigma, delta')
+    Ta = (sig * q2 + delta * qd2) * N.z
+    forces = [(A, Ta), (B, -Ta)]
+
+    # Try different kdes.
+    kde1 = [u1 - qd1, u2 - qd2]
+    kde2 = [u1 - qd1, u2 - (qd1 + qd2)]
+
+    KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
+    fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
+
+    KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
+    fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
+
+    # Check EOM for KM2:
+    # Mass and force matrix from p.6 in Ref. [2] with added forces from
+    # example of chapter 4.7 in [1] and without gravity.
+    forcing_matrix_expected = Matrix( [ [ m * l**2 * sin(q2) * u2**2 + sig * q2
+                                        + delta * (u2 - u1)],
+                                        [ m * l**2 * sin(q2) * -u1**2 - sig * q2
+                                        - delta * (u2 - u1)] ] )
+    mass_matrix_expected = Matrix( [ [ 2 * m * l**2, m * l**2 * cos(q2) ],
+                                    [ m * l**2 * cos(q2), m * l**2 ] ] )
+
+    assert (KM2.mass_matrix.expand() == mass_matrix_expected.expand())
+    assert (KM2.forcing.expand() == forcing_matrix_expected.expand())
+
+    # Check fr1 with reference fr_expected from [1] with u:s instead of qdots.
+    fr1_expected = Matrix([ 0, -(sig*q2 + delta * u2) ])
+    assert fr1.expand() == fr1_expected.expand()
+
+    # Check fr2
+    fr2_expected = Matrix([sig * q2 + delta * (u2 - u1),
+                            - sig * q2 - delta * (u2 - u1)])
+    assert fr2.expand() == fr2_expected.expand()
+
+    # Specifying forces in u:s should stay the same:
+    Ta = (sig * q2 + delta * u2) * N.z
+    forces = [(A, Ta), (B, -Ta)]
+    KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
+    fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
+
+    assert fr1.expand() == fr1_expected.expand()
+
+    Ta = (sig * q2 + delta * (u2-u1)) * N.z
+    forces = [(A, Ta), (B, -Ta)]
+    KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
+    fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
+
+    assert fr2.expand() == fr2_expected.expand()
+
+    # Test if we have a qubic qdot force:
+    Ta = (sig * q2 + delta * qd2**3) * N.z
+    forces = [(A, Ta), (B, -Ta)]
+
+    KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
+    fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
+
+    fr1_cubic_expected = Matrix([ 0, -(sig*q2 + delta * u2**3) ])
+
+    assert fr1.expand() == fr1_cubic_expected.expand()
+
+    KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
+    fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
+
+    fr2_cubic_expected = Matrix([sig * q2 + delta * (u2 - u1)**3,
+                            - sig * q2 - delta * (u2 - u1)**3])
+
+    assert fr2.expand() == fr2_cubic_expected.expand()
+
+    # References:
+    # [1] T.R. Kane, D. a Levinson, Dynamics Theory and Applications, 2005.
+    # [2] Arun K Banerjee, Flexible Multibody Dynamics:Efficient Formulations
+    #     and Applications, John Wiley and Sons, Ltd, 2016.
+    #     doi:http://dx.doi.org/10.1002/9781119015635.

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane5.py

@@ -0,0 +1,128 @@
+from sympy import (zeros, Matrix, symbols, lambdify, sqrt, pi,
+                                simplify)
+from sympy.physics.mechanics import (dynamicsymbols, cross, inertia, RigidBody,
+                                     ReferenceFrame, KanesMethod)
+
+
+def _create_rolling_disc():
+    # Define symbols and coordinates
+    t = dynamicsymbols._t
+    q1, q2, q3, q4, q5, u1, u2, u3, u4, u5 = dynamicsymbols('q1:6 u1:6')
+    g, r, m = symbols('g r m')
+    # Define bodies and frames
+    ground = RigidBody('ground')
+    disc = RigidBody('disk', mass=m)
+    disc.inertia = (m * r ** 2 / 4 * inertia(disc.frame, 1, 2, 1),
+                    disc.masscenter)
+    ground.masscenter.set_vel(ground.frame, 0)
+    disc.masscenter.set_vel(disc.frame, 0)
+    int_frame = ReferenceFrame('int_frame')
+    # Orient frames
+    int_frame.orient_body_fixed(ground.frame, (q1, q2, 0), 'zxy')
+    disc.frame.orient_axis(int_frame, int_frame.y, q3)
+    g_w_d = disc.frame.ang_vel_in(ground.frame)
+    disc.frame.set_ang_vel(ground.frame,
+                           u1 * disc.x + u2 * disc.y + u3 * disc.z)
+    # Define points
+    cp = ground.masscenter.locatenew('contact_point',
+                                     q4 * ground.x + q5 * ground.y)
+    cp.set_vel(ground.frame, u4 * ground.x + u5 * ground.y)
+    disc.masscenter.set_pos(cp, r * int_frame.z)
+    disc.masscenter.set_vel(ground.frame, cross(
+        disc.frame.ang_vel_in(ground.frame), disc.masscenter.pos_from(cp)))
+    # Define kinematic differential equations
+    kdes = [g_w_d.dot(disc.x) - u1, g_w_d.dot(disc.y) - u2,
+            g_w_d.dot(disc.z) - u3, q4.diff(t) - u4, q5.diff(t) - u5]
+    # Define nonholonomic constraints
+    v0 = cp.vel(ground.frame) + cross(
+        disc.frame.ang_vel_in(int_frame), cp.pos_from(disc.masscenter))
+    fnh = [v0.dot(ground.x), v0.dot(ground.y)]
+    # Define loads
+    loads = [(disc.masscenter, -disc.mass * g * ground.z)]
+    bodies = [disc]
+    return {
+        'frame': ground.frame,
+        'q_ind': [q1, q2, q3, q4, q5],
+        'u_ind': [u1, u2, u3],
+        'u_dep': [u4, u5],
+        'kdes': kdes,
+        'fnh': fnh,
+        'bodies': bodies,
+        'loads': loads
+    }
+
+
+def _verify_rolling_disc_numerically(kane, all_zero=False):
+    q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m')
+    eval_sys = lambdify((q, u, p), (kane.mass_matrix_full, kane.forcing_full),
+                        cse=True)
+    solve_sys = lambda q, u, p: Matrix.LUsolve(
+        *(Matrix(mat) for mat in eval_sys(q, u, p)))
+    solve_u_dep = lambdify((q, u[:3], p), kane._Ars * Matrix(u[:3]), cse=True)
+    eps = 1e-10
+    p_vals = (9.81, 0.26, 3.43)
+    # First numeric test
+    q_vals = (0.3, 0.1, 1.97, -0.35, 2.27)
+    u_vals = [-0.2, 1.3, 0.15]
+    u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0])
+    expected = Matrix([
+        0.126603940595934, 0.215942571601660, 1.28736069604936,
+        0.319764288376543, 0.0989146857254898, -0.925848952664489,
+        -0.0181350656532944, 2.91695398184589, -0.00992793421754526,
+        0.0412861634829171])
+    assert all(abs(x) < eps for x in
+               (solve_sys(q_vals, u_vals, p_vals) - expected))
+    # Second numeric test
+    q_vals = (3.97, -0.28, 8.2, -0.35, 2.27)
+    u_vals = [-0.25, -2.2, 0.62]
+    u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0])
+    expected = Matrix([
+        0.0259159090798597, 0.668041660387416, -2.19283799213811,
+        0.385441810852219, 0.420109283790573, 1.45030568179066,
+        -0.0110924422400793, -8.35617840186040, -0.154098542632173,
+        -0.146102664410010])
+    assert all(abs(x) < eps for x in
+               (solve_sys(q_vals, u_vals, p_vals) - expected))
+    if all_zero:
+        q_vals = (0, 0, 0, 0, 0)
+        u_vals = (0, 0, 0, 0, 0)
+        assert solve_sys(q_vals, u_vals, p_vals) == zeros(10, 1)
+
+
+def test_kane_rolling_disc_lu():
+    props = _create_rolling_disc()
+    kane = KanesMethod(props['frame'], props['q_ind'], props['u_ind'],
+                       props['kdes'], u_dependent=props['u_dep'],
+                       velocity_constraints=props['fnh'],
+                       bodies=props['bodies'], forcelist=props['loads'],
+                       explicit_kinematics=False, constraint_solver='LU')
+    kane.kanes_equations()
+    _verify_rolling_disc_numerically(kane)
+
+
+def test_kane_rolling_disc_kdes_callable():
+    props = _create_rolling_disc()
+    kane = KanesMethod(
+        props['frame'], props['q_ind'], props['u_ind'], props['kdes'],
+        u_dependent=props['u_dep'], velocity_constraints=props['fnh'],
+        bodies=props['bodies'], forcelist=props['loads'],
+        explicit_kinematics=False,
+        kd_eqs_solver=lambda A, b: simplify(A.LUsolve(b)))
+    q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m')
+    qd = dynamicsymbols('q1:6', 1)
+    eval_kdes = lambdify((q, qd, u, p), tuple(kane.kindiffdict().items()))
+    eps = 1e-10
+    # Test with only zeros. If 'LU' would be used this would result in nan.
+    p_vals = (9.81, 0.25, 3.5)
+    zero_vals = (0, 0, 0, 0, 0)
+    assert all(abs(qdi - fui) < eps for qdi, fui in
+               eval_kdes(zero_vals, zero_vals, zero_vals, p_vals))
+    # Test with some arbitrary values
+    q_vals = tuple(map(float, (pi / 6, pi / 3, pi / 2, 0.42, 0.62)))
+    qd_vals = tuple(map(float, (4, 1 / 3, 4 - 2 * sqrt(3),
+                                0.25 * (2 * sqrt(3) - 3),
+                                0.25 * (2 - sqrt(3)))))
+    u_vals = tuple(map(float, (-2, 4, 1 / 3, 0.25 * (-3 + 2 * sqrt(3)),
+                               0.25 * (-sqrt(3) + 2))))
+    assert all(abs(qdi - fui) < eps for qdi, fui in
+               eval_kdes(q_vals, qd_vals, u_vals, p_vals))

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_lagrange.py

@@ -0,0 +1,247 @@
+from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
+                                    RigidBody, LagrangesMethod, Particle,
+                                    inertia, Lagrangian)
+from sympy.core.function import (Derivative, Function)
+from sympy.core.numbers import pi
+from sympy.core.symbol import symbols
+from sympy.functions.elementary.trigonometric import (cos, sin, tan)
+from sympy.matrices.dense import Matrix
+from sympy.simplify.simplify import simplify
+from sympy.testing.pytest import raises
+
+
+def test_invalid_coordinates():
+    # Simple pendulum, but use symbol instead of dynamicsymbol
+    l, m, g = symbols('l m g')
+    q = symbols('q')  # Generalized coordinate
+    N, O = ReferenceFrame('N'), Point('O')
+    O.set_vel(N, 0)
+    P = Particle('P', Point('P'), m)
+    P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
+    P.potential_energy = m * g * P.point.pos_from(O).dot(N.y)
+    L = Lagrangian(N, P)
+    raises(ValueError, lambda: LagrangesMethod(L, [q], bodies=P))
+
+
+def test_disc_on_an_incline_plane():
+    # Disc rolling on an inclined plane
+    # First the generalized coordinates are created. The mass center of the
+    # disc is located from top vertex of the inclined plane by the generalized
+    # coordinate 'y'. The orientation of the disc is defined by the angle
+    # 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
+    # the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
+    # gravitational constant.
+    y, theta = dynamicsymbols('y theta')
+    yd, thetad = dynamicsymbols('y theta', 1)
+    m, g, R, l, alpha = symbols('m g R l alpha')
+
+    # Next, we create the inertial reference frame 'N'. A reference frame 'A'
+    # is attached to the inclined plane. Finally a frame is created which is attached to the disk.
+    N = ReferenceFrame('N')
+    A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z])
+    B = A.orientnew('B', 'Axis', [-theta, A.z])
+
+    # Creating the disc 'D'; we create the point that represents the mass
+    # center of the disc and set its velocity. The inertia dyadic of the disc
+    # is created. Finally, we create the disc.
+    Do = Point('Do')
+    Do.set_vel(N, yd * A.x)
+    I = m * R**2/2 * B.z | B.z
+    D = RigidBody('D', Do, B, m, (I, Do))
+
+    # To construct the Lagrangian, 'L', of the disc, we determine its kinetic
+    # and potential energies, T and U, respectively. L is defined as the
+    # difference between T and U.
+    D.potential_energy = m * g * (l - y) * sin(alpha)
+    L = Lagrangian(N, D)
+
+    # We then create the list of generalized coordinates and constraint
+    # equations. The constraint arises due to the disc rolling without slip on
+    # on the inclined path. We then invoke the 'LagrangesMethod' class and
+    # supply it the necessary arguments and generate the equations of motion.
+    # The'rhs' method solves for the q_double_dots (i.e. the second derivative
+    # with respect to time  of the generalized coordinates and the lagrange
+    # multipliers.
+    q = [y, theta]
+    hol_coneqs = [y - R * theta]
+    m = LagrangesMethod(L, q, hol_coneqs=hol_coneqs)
+    m.form_lagranges_equations()
+    rhs = m.rhs()
+    rhs.simplify()
+    assert rhs[2] == 2*g*sin(alpha)/3
+
+
+def test_simp_pen():
+    # This tests that the equations generated by LagrangesMethod are identical
+    # to those obtained by hand calculations. The system under consideration is
+    # the simple pendulum.
+    # We begin by creating the generalized coordinates as per the requirements
+    # of LagrangesMethod. Also we created the associate symbols
+    # that characterize the system: 'm' is the mass of the bob, l is the length
+    # of the massless rigid rod connecting the bob to a point O fixed in the
+    # inertial frame.
+    q, u = dynamicsymbols('q u')
+    qd, ud = dynamicsymbols('q u ', 1)
+    l, m, g = symbols('l m g')
+
+    # We then create the inertial frame and a frame attached to the massless
+    # string following which we define the inertial angular velocity of the
+    # string.
+    N = ReferenceFrame('N')
+    A = N.orientnew('A', 'Axis', [q, N.z])
+    A.set_ang_vel(N, qd * N.z)
+
+    # Next, we create the point O and fix it in the inertial frame. We then
+    # locate the point P to which the bob is attached. Its corresponding
+    # velocity is then determined by the 'two point formula'.
+    O = Point('O')
+    O.set_vel(N, 0)
+    P = O.locatenew('P', l * A.x)
+    P.v2pt_theory(O, N, A)
+
+    # The 'Particle' which represents the bob is then created and its
+    # Lagrangian generated.
+    Pa = Particle('Pa', P, m)
+    Pa.potential_energy = - m * g * l * cos(q)
+    L = Lagrangian(N, Pa)
+
+    # The 'LagrangesMethod' class is invoked to obtain equations of motion.
+    lm = LagrangesMethod(L, [q])
+    lm.form_lagranges_equations()
+    RHS = lm.rhs()
+    assert RHS[1] == -g*sin(q)/l
+
+
+def test_nonminimal_pendulum():
+    q1, q2 = dynamicsymbols('q1:3')
+    q1d, q2d = dynamicsymbols('q1:3', level=1)
+    L, m, t = symbols('L, m, t')
+    g = 9.8
+    # Compose World Frame
+    N = ReferenceFrame('N')
+    pN = Point('N*')
+    pN.set_vel(N, 0)
+    # Create point P, the pendulum mass
+    P = pN.locatenew('P1', q1*N.x + q2*N.y)
+    P.set_vel(N, P.pos_from(pN).dt(N))
+    pP = Particle('pP', P, m)
+    # Constraint Equations
+    f_c = Matrix([q1**2 + q2**2 - L**2])
+    # Calculate the lagrangian, and form the equations of motion
+    Lag = Lagrangian(N, pP)
+    LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c,
+            forcelist=[(P, m*g*N.x)], frame=N)
+    LM.form_lagranges_equations()
+    # Check solution
+    lam1 = LM.lam_vec[0, 0]
+    eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
+                      [m*Derivative(q2, t, t) + 2*lam1*q2]])
+    assert LM.eom == eom_sol
+    # Check multiplier solution
+    lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
+    assert simplify(LM.solve_multipliers(sol_type='Matrix')) == simplify(lam_sol)
+
+
+def test_dub_pen():
+
+    # The system considered is the double pendulum. Like in the
+    # test of the simple pendulum above, we begin by creating the generalized
+    # coordinates and the simple generalized speeds and accelerations which
+    # will be used later. Following this we create frames and points necessary
+    # for the kinematics. The procedure isn't explicitly explained as this is
+    # similar to the simple  pendulum. Also this is documented on the pydy.org
+    # website.
+    q1, q2 = dynamicsymbols('q1 q2')
+    q1d, q2d = dynamicsymbols('q1 q2', 1)
+    q1dd, q2dd = dynamicsymbols('q1 q2', 2)
+    u1, u2 = dynamicsymbols('u1 u2')
+    u1d, u2d = dynamicsymbols('u1 u2', 1)
+    l, m, g = symbols('l m g')
+
+    N = ReferenceFrame('N')
+    A = N.orientnew('A', 'Axis', [q1, N.z])
+    B = N.orientnew('B', 'Axis', [q2, N.z])
+
+    A.set_ang_vel(N, q1d * A.z)
+    B.set_ang_vel(N, q2d * A.z)
+
+    O = Point('O')
+    P = O.locatenew('P', l * A.x)
+    R = P.locatenew('R', l * B.x)
+
+    O.set_vel(N, 0)
+    P.v2pt_theory(O, N, A)
+    R.v2pt_theory(P, N, B)
+
+    ParP = Particle('ParP', P, m)
+    ParR = Particle('ParR', R, m)
+
+    ParP.potential_energy = - m * g * l * cos(q1)
+    ParR.potential_energy = - m * g * l * cos(q1) - m * g * l * cos(q2)
+    L = Lagrangian(N, ParP, ParR)
+    lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR])
+    lm.form_lagranges_equations()
+
+    assert simplify(l*m*(2*g*sin(q1) + l*sin(q1)*sin(q2)*q2dd
+        + l*sin(q1)*cos(q2)*q2d**2 - l*sin(q2)*cos(q1)*q2d**2
+        + l*cos(q1)*cos(q2)*q2dd + 2*l*q1dd) - lm.eom[0]) == 0
+    assert simplify(l*m*(g*sin(q2) + l*sin(q1)*sin(q2)*q1dd
+        - l*sin(q1)*cos(q2)*q1d**2 + l*sin(q2)*cos(q1)*q1d**2
+        + l*cos(q1)*cos(q2)*q1dd + l*q2dd) - lm.eom[1]) == 0
+    assert lm.bodies == [ParP, ParR]
+
+
+def test_rolling_disc():
+    # Rolling Disc Example
+    # Here the rolling disc is formed from the contact point up, removing the
+    # need to introduce generalized speeds. Only 3 configuration and 3
+    # speed variables are need to describe this system, along with the
+    # disc's mass and radius, and the local gravity.
+    q1, q2, q3 = dynamicsymbols('q1 q2 q3')
+    q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
+    r, m, g = symbols('r m g')
+
+    # The kinematics are formed by a series of simple rotations. Each simple
+    # rotation creates a new frame, and the next rotation is defined by the new
+    # frame's basis vectors. This example uses a 3-1-2 series of rotations, or
+    # Z, X, Y series of rotations. Angular velocity for this is defined using
+    # the second frame's basis (the lean frame).
+    N = ReferenceFrame('N')
+    Y = N.orientnew('Y', 'Axis', [q1, N.z])
+    L = Y.orientnew('L', 'Axis', [q2, Y.x])
+    R = L.orientnew('R', 'Axis', [q3, L.y])
+
+    # This is the translational kinematics. We create a point with no velocity
+    # in N; this is the contact point between the disc and ground. Next we form
+    # the position vector from the contact point to the disc's center of mass.
+    # Finally we form the velocity and acceleration of the disc.
+    C = Point('C')
+    C.set_vel(N, 0)
+    Dmc = C.locatenew('Dmc', r * L.z)
+    Dmc.v2pt_theory(C, N, R)
+
+    # Forming the inertia dyadic.
+    I = inertia(L, m/4 * r**2, m/2 * r**2, m/4 * r**2)
+    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
+
+    # Finally we form the equations of motion, using the same steps we did
+    # before. Supply the Lagrangian, the generalized speeds.
+    BodyD.potential_energy = - m * g * r * cos(q2)
+    Lag = Lagrangian(N, BodyD)
+    q = [q1, q2, q3]
+    q1 = Function('q1')
+    q2 = Function('q2')
+    q3 = Function('q3')
+    l = LagrangesMethod(Lag, q)
+    l.form_lagranges_equations()
+    RHS = l.rhs()
+    RHS.simplify()
+    t = symbols('t')
+
+    assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
+    assert RHS[4].simplify() == (
+        (-8*g*sin(q2(t)) + r*(5*sin(2*q2(t))*Derivative(q1(t), t) +
+        12*cos(q2(t))*Derivative(q3(t), t))*Derivative(q1(t), t))/(10*r))
+    assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
+        )*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
+        )*Derivative(q2(t), t)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_lagrange2.py

@@ -0,0 +1,46 @@
+from sympy import symbols
+from sympy.physics.mechanics import dynamicsymbols
+from sympy.physics.mechanics import ReferenceFrame, Point, Particle
+from sympy.physics.mechanics import LagrangesMethod, Lagrangian
+
+### This test asserts that a system with more than one external forces
+### is acurately formed with Lagrange method (see issue #8626)
+
+def test_lagrange_2forces():
+    ### Equations for two damped springs in serie with two forces
+
+    ### generalized coordinates
+    q1, q2 = dynamicsymbols('q1, q2')
+    ### generalized speeds
+    q1d, q2d = dynamicsymbols('q1, q2', 1)
+
+    ### Mass, spring strength, friction coefficient
+    m, k, nu = symbols('m, k, nu')
+
+    N = ReferenceFrame('N')
+    O = Point('O')
+
+    ### Two points
+    P1 = O.locatenew('P1', q1 * N.x)
+    P1.set_vel(N, q1d * N.x)
+    P2 = O.locatenew('P1', q2 * N.x)
+    P2.set_vel(N, q2d * N.x)
+
+    pP1 = Particle('pP1', P1, m)
+    pP1.potential_energy = k * q1**2 / 2
+
+    pP2 = Particle('pP2', P2, m)
+    pP2.potential_energy = k * (q1 - q2)**2 / 2
+
+    #### Friction forces
+    forcelist = [(P1, - nu * q1d * N.x),
+                 (P2, - nu * q2d * N.x)]
+    lag = Lagrangian(N, pP1, pP2)
+
+    l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N)
+    l_method.form_lagranges_equations()
+
+    eq1 = l_method.eom[0]
+    assert eq1.diff(q1d) == nu
+    eq2 = l_method.eom[1]
+    assert eq2.diff(q2d) == nu

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_linearize.py

@@ -0,0 +1,372 @@
+from sympy import symbols, Matrix, cos, sin, atan, sqrt, Rational
+from sympy.core.sympify import sympify
+from sympy.simplify.simplify import simplify
+from sympy.solvers.solvers import solve
+from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point,\
+    dot, cross, inertia, KanesMethod, Particle, RigidBody, Lagrangian,\
+    LagrangesMethod
+from sympy.testing.pytest import slow
+
+
+@slow
+def test_linearize_rolling_disc_kane():
+    # Symbols for time and constant parameters
+    t, r, m, g, v = symbols('t r m g v')
+
+    # Configuration variables and their time derivatives
+    q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
+    q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
+
+    # Generalized speeds and their time derivatives
+    u = dynamicsymbols('u:6')
+    u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
+    u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
+
+    # Reference frames
+    N = ReferenceFrame('N')                   # Inertial frame
+    NO = Point('NO')                          # Inertial origin
+    A = N.orientnew('A', 'Axis', [q1, N.z])   # Yaw intermediate frame
+    B = A.orientnew('B', 'Axis', [q2, A.x])   # Lean intermediate frame
+    C = B.orientnew('C', 'Axis', [q3, B.y])   # Disc fixed frame
+    CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z)      # Disc center
+
+    # Disc angular velocity in N expressed using time derivatives of coordinates
+    w_c_n_qd = C.ang_vel_in(N)
+    w_b_n_qd = B.ang_vel_in(N)
+
+    # Inertial angular velocity and angular acceleration of disc fixed frame
+    C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
+
+    # Disc center velocity in N expressed using time derivatives of coordinates
+    v_co_n_qd = CO.pos_from(NO).dt(N)
+
+    # Disc center velocity in N expressed using generalized speeds
+    CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
+
+    # Disc Ground Contact Point
+    P = CO.locatenew('P', r*B.z)
+    P.v2pt_theory(CO, N, C)
+
+    # Configuration constraint
+    f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
+
+    # Velocity level constraints
+    f_v = Matrix([dot(P.vel(N), uv) for uv in C])
+
+    # Kinematic differential equations
+    kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
+                        [dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
+    qdots = solve(kindiffs, qd)
+
+    # Set angular velocity of remaining frames
+    B.set_ang_vel(N, w_b_n_qd.subs(qdots))
+    C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
+
+    # Active forces
+    F_CO = m*g*A.z
+
+    # Create inertia dyadic of disc C about point CO
+    I = (m * r**2) / 4
+    J = (m * r**2) / 2
+    I_C_CO = inertia(C, I, J, I)
+
+    Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
+    BL = [Disc]
+    FL = [(CO, F_CO)]
+    KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
+            q_dependent=[q6], configuration_constraints=f_c,
+            u_dependent=[u4, u5, u6], velocity_constraints=f_v)
+    (fr, fr_star) = KM.kanes_equations(BL, FL)
+
+    # Test generalized form equations
+    linearizer = KM.to_linearizer()
+    assert linearizer.f_c == f_c
+    assert linearizer.f_v == f_v
+    assert linearizer.f_a == f_v.diff(t).subs(KM.kindiffdict())
+    sol = solve(linearizer.f_0 + linearizer.f_1, qd)
+    for qi in qdots.keys():
+        assert sol[qi] == qdots[qi]
+    assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
+
+    # Perform the linearization
+    # Precomputed operating point
+    q_op = {q6: -r*cos(q2)}
+    u_op = {u1: 0,
+            u2: sin(q2)*q1d + q3d,
+            u3: cos(q2)*q1d,
+            u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
+            u5: 0,
+            u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
+    qd_op = {q2d: 0,
+             q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
+             q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
+             q6d: 0}
+    ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
+             u2d: 0,
+             u3d: 0,
+             u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
+             u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
+             u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
+
+    A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
+
+    upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
+
+    # Precomputed solution
+    A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
+                    [0, 0, 0, 0, 0, 1, 0, 0],
+                    [0, 0, 0, 0, 0, 0, 1, 0],
+                    [sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
+                    [-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
+                    [0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5],
+                    [0, 0, 0, 0, 0, 0, 0, 0],
+                    [0, 0, 0, 0, 0, -2*q3d, 0, 0]])
+    B_sol = Matrix([])
+
+    # Check that linearization is correct
+    assert A.subs(upright_nominal) == A_sol
+    assert B.subs(upright_nominal) == B_sol
+
+    # Check eigenvalues at critical speed are all zero:
+    assert sympify(A.subs(upright_nominal).subs(q3d, 1/sqrt(3))).eigenvals() == {0: 8}
+
+    # Check whether alternative solvers work
+    # symengine doesn't support method='GJ'
+    linearizer = KM.to_linearizer(linear_solver='GJ')
+    A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
+                                A_and_B=True, simplify=True)
+    assert A.subs(upright_nominal) == A_sol
+    assert B.subs(upright_nominal) == B_sol
+
+def test_linearize_pendulum_kane_minimal():
+    q1 = dynamicsymbols('q1')                     # angle of pendulum
+    u1 = dynamicsymbols('u1')                     # Angular velocity
+    q1d = dynamicsymbols('q1', 1)                 # Angular velocity
+    L, m, t = symbols('L, m, t')
+    g = 9.8
+
+    # Compose world frame
+    N = ReferenceFrame('N')
+    pN = Point('N*')
+    pN.set_vel(N, 0)
+
+    # A.x is along the pendulum
+    A = N.orientnew('A', 'axis', [q1, N.z])
+    A.set_ang_vel(N, u1*N.z)
+
+    # Locate point P relative to the origin N*
+    P = pN.locatenew('P', L*A.x)
+    P.v2pt_theory(pN, N, A)
+    pP = Particle('pP', P, m)
+
+    # Create Kinematic Differential Equations
+    kde = Matrix([q1d - u1])
+
+    # Input the force resultant at P
+    R = m*g*N.x
+
+    # Solve for eom with kanes method
+    KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
+    (fr, frstar) = KM.kanes_equations([pP], [(P, R)])
+
+    # Linearize
+    A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True)
+
+    assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
+    assert B == Matrix([])
+
+def test_linearize_pendulum_kane_nonminimal():
+    # Create generalized coordinates and speeds for this non-minimal realization
+    # q1, q2 = N.x and N.y coordinates of pendulum
+    # u1, u2 = N.x and N.y velocities of pendulum
+    q1, q2 = dynamicsymbols('q1:3')
+    q1d, q2d = dynamicsymbols('q1:3', level=1)
+    u1, u2 = dynamicsymbols('u1:3')
+    u1d, u2d = dynamicsymbols('u1:3', level=1)
+    L, m, t = symbols('L, m, t')
+    g = 9.8
+
+    # Compose world frame
+    N = ReferenceFrame('N')
+    pN = Point('N*')
+    pN.set_vel(N, 0)
+
+    # A.x is along the pendulum
+    theta1 = atan(q2/q1)
+    A = N.orientnew('A', 'axis', [theta1, N.z])
+
+    # Locate the pendulum mass
+    P = pN.locatenew('P1', q1*N.x + q2*N.y)
+    pP = Particle('pP', P, m)
+
+    # Calculate the kinematic differential equations
+    kde = Matrix([q1d - u1,
+                  q2d - u2])
+    dq_dict = solve(kde, [q1d, q2d])
+
+    # Set velocity of point P
+    P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
+
+    # Configuration constraint is length of pendulum
+    f_c = Matrix([P.pos_from(pN).magnitude() - L])
+
+    # Velocity constraint is that the velocity in the A.x direction is
+    # always zero (the pendulum is never getting longer).
+    f_v = Matrix([P.vel(N).express(A).dot(A.x)])
+    f_v.simplify()
+
+    # Acceleration constraints is the time derivative of the velocity constraint
+    f_a = f_v.diff(t)
+    f_a.simplify()
+
+    # Input the force resultant at P
+    R = m*g*N.x
+
+    # Derive the equations of motion using the KanesMethod class.
+    KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
+            u_dependent=[u1], configuration_constraints=f_c,
+            velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
+    (fr, frstar) = KM.kanes_equations([pP], [(P, R)])
+
+    # Set the operating point to be straight down, and non-moving
+    q_op = {q1: L, q2: 0}
+    u_op = {u1: 0, u2: 0}
+    ud_op = {u1d: 0, u2d: 0}
+
+    A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
+                                 simplify=True)
+
+    assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
+    assert B == Matrix([])
+
+
+    # symengine doesn't support method='GJ'
+    A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
+                                simplify=True, linear_solver='GJ')
+
+    assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
+    assert B == Matrix([])
+
+    A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op],
+                                 A_and_B=True,
+                                 simplify=True,
+                                 linear_solver=lambda A, b: A.LUsolve(b))
+
+    assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
+    assert B == Matrix([])
+
+
+def test_linearize_pendulum_lagrange_minimal():
+    q1 = dynamicsymbols('q1')                     # angle of pendulum
+    q1d = dynamicsymbols('q1', 1)                 # Angular velocity
+    L, m, t = symbols('L, m, t')
+    g = 9.8
+
+    # Compose world frame
+    N = ReferenceFrame('N')
+    pN = Point('N*')
+    pN.set_vel(N, 0)
+
+    # A.x is along the pendulum
+    A = N.orientnew('A', 'axis', [q1, N.z])
+    A.set_ang_vel(N, q1d*N.z)
+
+    # Locate point P relative to the origin N*
+    P = pN.locatenew('P', L*A.x)
+    P.v2pt_theory(pN, N, A)
+    pP = Particle('pP', P, m)
+
+    # Solve for eom with Lagranges method
+    Lag = Lagrangian(N, pP)
+    LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
+    LM.form_lagranges_equations()
+
+    # Linearize
+    A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)
+
+    assert simplify(A) == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
+    assert B == Matrix([])
+
+    # Check an alternative solver
+    A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True, linear_solver='GJ')
+
+    assert simplify(A) == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
+    assert B == Matrix([])
+
+
+def test_linearize_pendulum_lagrange_nonminimal():
+    q1, q2 = dynamicsymbols('q1:3')
+    q1d, q2d = dynamicsymbols('q1:3', level=1)
+    L, m, t = symbols('L, m, t')
+    g = 9.8
+    # Compose World Frame
+    N = ReferenceFrame('N')
+    pN = Point('N*')
+    pN.set_vel(N, 0)
+    # A.x is along the pendulum
+    theta1 = atan(q2/q1)
+    A = N.orientnew('A', 'axis', [theta1, N.z])
+    # Create point P, the pendulum mass
+    P = pN.locatenew('P1', q1*N.x + q2*N.y)
+    P.set_vel(N, P.pos_from(pN).dt(N))
+    pP = Particle('pP', P, m)
+    # Constraint Equations
+    f_c = Matrix([q1**2 + q2**2 - L**2])
+    # Calculate the lagrangian, and form the equations of motion
+    Lag = Lagrangian(N, pP)
+    LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m*g*N.x)], frame=N)
+    LM.form_lagranges_equations()
+    # Compose operating point
+    op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
+    # Solve for multiplier operating point
+    lam_op = LM.solve_multipliers(op_point=op_point)
+    op_point.update(lam_op)
+    # Perform the Linearization
+    A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
+            op_point=op_point, A_and_B=True)
+    assert simplify(A) == Matrix([[0, 1], [-9.8/L, 0]])
+    assert B == Matrix([])
+
+    # Check if passing a function to linear_solver works
+    A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d], op_point=op_point,
+                                 A_and_B=True, linear_solver=lambda A, b:
+                                 A.LUsolve(b))
+    assert simplify(A) == Matrix([[0, 1], [-9.8/L, 0]])
+    assert B == Matrix([])
+
+def test_linearize_rolling_disc_lagrange():
+    q1, q2, q3 = q = dynamicsymbols('q1 q2 q3')
+    q1d, q2d, q3d = qd = dynamicsymbols('q1 q2 q3', 1)
+    r, m, g = symbols('r m g')
+
+    N = ReferenceFrame('N')
+    Y = N.orientnew('Y', 'Axis', [q1, N.z])
+    L = Y.orientnew('L', 'Axis', [q2, Y.x])
+    R = L.orientnew('R', 'Axis', [q3, L.y])
+
+    C = Point('C')
+    C.set_vel(N, 0)
+    Dmc = C.locatenew('Dmc', r * L.z)
+    Dmc.v2pt_theory(C, N, R)
+
+    I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
+    BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
+    BodyD.potential_energy = - m * g * r * cos(q2)
+
+    Lag = Lagrangian(N, BodyD)
+    l = LagrangesMethod(Lag, q)
+    l.form_lagranges_equations()
+
+    # Linearize about steady-state upright rolling
+    op_point = {q1: 0, q2: 0, q3: 0,
+                q1d: 0, q2d: 0,
+                q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
+    A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
+    sol = Matrix([[0, 0, 0, 1, 0, 0],
+                  [0, 0, 0, 0, 1, 0],
+                  [0, 0, 0, 0, 0, 1],
+                  [0, 0, 0, 0, -6*q3d, 0],
+                  [0, -4*g/(5*r), 0, 6*q3d/5, 0, 0],
+                  [0, 0, 0, 0, 0, 0]])
+
+    assert A == sol

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_loads.py

@@ -0,0 +1,86 @@
+from pytest import raises
+
+from sympy import symbols
+from sympy.physics.mechanics import (RigidBody, Particle, ReferenceFrame, Point,
+                                     outer, dynamicsymbols, Force, Torque)
+from sympy.physics.mechanics.loads import gravity, _parse_load
+
+
+def test_force_default():
+    N = ReferenceFrame('N')
+    Po = Point('Po')
+    f1 = Force(Po, N.x)
+    assert f1.point == Po
+    assert f1.force == N.x
+    assert f1.__repr__() == 'Force(point=Po, force=N.x)'
+    # Test tuple behaviour
+    assert isinstance(f1, tuple)
+    assert f1[0] == Po
+    assert f1[1] == N.x
+    assert f1 == (Po, N.x)
+    assert f1 != (N.x, Po)
+    assert f1 != (Po, N.x + N.y)
+    assert f1 != (Point('Co'), N.x)
+    # Test body as input
+    P = Particle('P', Po)
+    f2 = Force(P, N.x)
+    assert f1 == f2
+
+
+def test_torque_default():
+    N = ReferenceFrame('N')
+    f1 = Torque(N, N.x)
+    assert f1.frame == N
+    assert f1.torque == N.x
+    assert f1.__repr__() == 'Torque(frame=N, torque=N.x)'
+    # Test tuple behaviour
+    assert isinstance(f1, tuple)
+    assert f1[0] == N
+    assert f1[1] == N.x
+    assert f1 == (N, N.x)
+    assert f1 != (N.x, N)
+    assert f1 != (N, N.x + N.y)
+    assert f1 != (ReferenceFrame('A'), N.x)
+    # Test body as input
+    rb = RigidBody('P', frame=N)
+    f2 = Torque(rb, N.x)
+    assert f1 == f2
+
+
+def test_gravity():
+    N = ReferenceFrame('N')
+    m, M, g = symbols('m M g')
+    F1, F2 = dynamicsymbols('F1 F2')
+    po = Point('po')
+    pa = Particle('pa', po, m)
+    A = ReferenceFrame('A')
+    P = Point('P')
+    I = outer(A.x, A.x)
+    B = RigidBody('B', P, A, M, (I, P))
+    forceList = [(po, F1), (P, F2)]
+    forceList.extend(gravity(g * N.y, pa, B))
+    l = [(po, F1), (P, F2), (po, g * m * N.y), (P, g * M * N.y)]
+
+    for i in range(len(l)):
+        for j in range(len(l[i])):
+            assert forceList[i][j] == l[i][j]
+
+
+def test_parse_loads():
+    N = ReferenceFrame('N')
+    po = Point('po')
+    assert _parse_load(Force(po, N.z)) == (po, N.z)
+    assert _parse_load(Torque(N, N.x)) == (N, N.x)
+    f1 = _parse_load((po, N.x))  # Test whether a force is recognized
+    assert isinstance(f1, Force)
+    assert f1 == Force(po, N.x)
+    t1 = _parse_load((N, N.y))  # Test whether a torque is recognized
+    assert isinstance(t1, Torque)
+    assert t1 == Torque(N, N.y)
+    # Bodies should be undetermined (even in case of a Particle)
+    raises(ValueError, lambda: _parse_load((Particle('pa', po), N.x)))
+    raises(ValueError, lambda: _parse_load((RigidBody('pa', po, N), N.x)))
+    # Invalid tuple length
+    raises(ValueError, lambda: _parse_load((po, N.x, po, N.x)))
+    # Invalid type
+    raises(TypeError, lambda: _parse_load([po, N.x]))

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_method.py

@@ -0,0 +1,5 @@
+from sympy.physics.mechanics.method import _Methods
+from sympy.testing.pytest import raises
+
+def test_method():
+    raises(TypeError, lambda: _Methods())

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_models.py

@@ -0,0 +1,117 @@
+import sympy.physics.mechanics.models as models
+from sympy import (cos, sin, Matrix, symbols, zeros)
+from sympy.simplify.simplify import simplify
+from sympy.physics.mechanics import (dynamicsymbols)
+
+
+def test_multi_mass_spring_damper_inputs():
+
+    c0, k0, m0 = symbols("c0 k0 m0")
+    g = symbols("g")
+    v0, x0, f0 = dynamicsymbols("v0 x0 f0")
+
+    kane1 = models.multi_mass_spring_damper(1)
+    massmatrix1 = Matrix([[m0]])
+    forcing1 = Matrix([[-c0*v0 - k0*x0]])
+    assert simplify(massmatrix1 - kane1.mass_matrix) == Matrix([0])
+    assert simplify(forcing1 - kane1.forcing) == Matrix([0])
+
+    kane2 = models.multi_mass_spring_damper(1, True)
+    massmatrix2 = Matrix([[m0]])
+    forcing2 = Matrix([[-c0*v0 + g*m0 - k0*x0]])
+    assert simplify(massmatrix2 - kane2.mass_matrix) == Matrix([0])
+    assert simplify(forcing2 - kane2.forcing) == Matrix([0])
+
+    kane3 = models.multi_mass_spring_damper(1, True, True)
+    massmatrix3 = Matrix([[m0]])
+    forcing3 = Matrix([[-c0*v0 + g*m0 - k0*x0 + f0]])
+    assert simplify(massmatrix3 - kane3.mass_matrix) == Matrix([0])
+    assert simplify(forcing3 - kane3.forcing) == Matrix([0])
+
+    kane4 = models.multi_mass_spring_damper(1, False, True)
+    massmatrix4 = Matrix([[m0]])
+    forcing4 = Matrix([[-c0*v0 - k0*x0 + f0]])
+    assert simplify(massmatrix4 - kane4.mass_matrix) == Matrix([0])
+    assert simplify(forcing4 - kane4.forcing) == Matrix([0])
+
+
+def test_multi_mass_spring_damper_higher_order():
+    c0, k0, m0 = symbols("c0 k0 m0")
+    c1, k1, m1 = symbols("c1 k1 m1")
+    c2, k2, m2 = symbols("c2 k2 m2")
+    v0, x0 = dynamicsymbols("v0 x0")
+    v1, x1 = dynamicsymbols("v1 x1")
+    v2, x2 = dynamicsymbols("v2 x2")
+
+    kane1 = models.multi_mass_spring_damper(3)
+    massmatrix1 = Matrix([[m0 + m1 + m2, m1 + m2, m2],
+                          [m1 + m2, m1 + m2, m2],
+                          [m2, m2, m2]])
+    forcing1 = Matrix([[-c0*v0 - k0*x0],
+                       [-c1*v1 - k1*x1],
+                       [-c2*v2 - k2*x2]])
+    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
+    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
+
+
+def test_n_link_pendulum_on_cart_inputs():
+    l0, m0 = symbols("l0 m0")
+    m1 = symbols("m1")
+    g = symbols("g")
+    q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
+    u0, u1 = dynamicsymbols("u0 u1")
+
+    kane1 = models.n_link_pendulum_on_cart(1)
+    massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
+                          [-l0*m1*cos(q1), l0**2*m1]])
+    forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
+    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
+    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])
+
+    kane2 = models.n_link_pendulum_on_cart(1, False)
+    massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
+                          [-l0*m1*cos(q1), l0**2*m1]])
+    forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
+    assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
+    assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])
+
+    kane3 = models.n_link_pendulum_on_cart(1, False, True)
+    massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
+                          [-l0*m1*cos(q1), l0**2*m1]])
+    forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
+    assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
+    assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])
+
+    kane4 = models.n_link_pendulum_on_cart(1, True, False)
+    massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
+                          [-l0*m1*cos(q1), l0**2*m1]])
+    forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
+    assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
+    assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])
+
+
+def test_n_link_pendulum_on_cart_higher_order():
+    l0, m0 = symbols("l0 m0")
+    l1, m1 = symbols("l1 m1")
+    m2 = symbols("m2")
+    g = symbols("g")
+    q0, q1, q2 = dynamicsymbols("q0 q1 q2")
+    u0, u1, u2 = dynamicsymbols("u0 u1 u2")
+    F, T1 = dynamicsymbols("F T1")
+
+    kane1 = models.n_link_pendulum_on_cart(2)
+    massmatrix1 = Matrix([[m0 + m1 + m2, -l0*m1*cos(q1) - l0*m2*cos(q1),
+                           -l1*m2*cos(q2)],
+                          [-l0*m1*cos(q1) - l0*m2*cos(q1), l0**2*m1 + l0**2*m2,
+                           l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2))],
+                          [-l1*m2*cos(q2),
+                           l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2)),
+                           l1**2*m2]])
+    forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) - l0*m2*u1**2*sin(q1) -
+                        l1*m2*u2**2*sin(q2) + F],
+                       [g*l0*m1*sin(q1) + g*l0*m2*sin(q1) -
+                        l0*l1*m2*(sin(q1)*cos(q2) - sin(q2)*cos(q1))*u2**2],
+                       [g*l1*m2*sin(q2) - l0*l1*m2*(-sin(q1)*cos(q2) +
+                                                    sin(q2)*cos(q1))*u1**2]])
+    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
+    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_particle.py

@@ -0,0 +1,78 @@
+from sympy import symbols
+from sympy.physics.mechanics import Point, Particle, ReferenceFrame, inertia
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+
+
+def test_particle_default():
+    # Test default
+    p = Particle('P')
+    assert p.name == 'P'
+    assert p.mass == symbols('P_mass')
+    assert p.masscenter.name == 'P_masscenter'
+    assert p.potential_energy == 0
+    assert p.__str__() == 'P'
+    assert p.__repr__() == ("Particle('P', masscenter=P_masscenter, "
+                            "mass=P_mass)")
+    raises(AttributeError, lambda: p.frame)
+
+
+def test_particle():
+    # Test initializing with parameters
+    m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
+    P = Point('P')
+    P2 = Point('P2')
+    p = Particle('pa', P, m)
+    assert isinstance(p, BodyBase)
+    assert p.mass == m
+    assert p.point == P
+    # Test the mass setter
+    p.mass = m2
+    assert p.mass == m2
+    # Test the point setter
+    p.point = P2
+    assert p.point == P2
+    # Test the linear momentum function
+    N = ReferenceFrame('N')
+    O = Point('O')
+    P2.set_pos(O, r * N.y)
+    P2.set_vel(N, v1 * N.x)
+    raises(TypeError, lambda: Particle(P, P, m))
+    raises(TypeError, lambda: Particle('pa', m, m))
+    assert p.linear_momentum(N) == m2 * v1 * N.x
+    assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
+    P2.set_vel(N, v2 * N.y)
+    assert p.linear_momentum(N) == m2 * v2 * N.y
+    assert p.angular_momentum(O, N) == 0
+    P2.set_vel(N, v3 * N.z)
+    assert p.linear_momentum(N) == m2 * v3 * N.z
+    assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
+    P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
+    assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
+    assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
+    p.potential_energy = m * g * h
+    assert p.potential_energy == m * g * h
+    # TODO make the result not be system-dependent
+    assert p.kinetic_energy(
+        N) in [m2 * (v1 ** 2 + v2 ** 2 + v3 ** 2) / 2,
+               m2 * v1 ** 2 / 2 + m2 * v2 ** 2 / 2 + m2 * v3 ** 2 / 2]
+
+
+def test_parallel_axis():
+    N = ReferenceFrame('N')
+    m, a, b = symbols('m, a, b')
+    o = Point('o')
+    p = o.locatenew('p', a * N.x + b * N.y)
+    P = Particle('P', o, m)
+    Ip = P.parallel_axis(p, N)
+    Ip_expected = inertia(N, m * b ** 2, m * a ** 2, m * (a ** 2 + b ** 2),
+                          ixy=-m * a * b)
+    assert Ip == Ip_expected
+
+
+def test_deprecated_set_potential_energy():
+    m, g, h = symbols('m g h')
+    P = Point('P')
+    p = Particle('pa', P, m)
+    with warns_deprecated_sympy():
+        p.set_potential_energy(m * g * h)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_pathway.py

@@ -0,0 +1,691 @@
+"""Tests for the ``sympy.physics.mechanics.pathway.py`` module."""
+
+import pytest
+
+from sympy import (
+    Rational,
+    Symbol,
+    cos,
+    pi,
+    sin,
+    sqrt,
+)
+from sympy.physics.mechanics import (
+    Force,
+    LinearPathway,
+    ObstacleSetPathway,
+    PathwayBase,
+    Point,
+    ReferenceFrame,
+    WrappingCylinder,
+    WrappingGeometryBase,
+    WrappingPathway,
+    WrappingSphere,
+    dynamicsymbols,
+)
+from sympy.simplify.simplify import simplify
+
+
+def _simplify_loads(loads):
+    return [
+        load.__class__(load.location, load.vector.simplify())
+        for load in loads
+    ]
+
+
+class TestLinearPathway:
+
+    def test_is_pathway_base_subclass(self):
+        assert issubclass(LinearPathway, PathwayBase)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'args, kwargs',
+        [
+            ((Point('pA'), Point('pB')), {}),
+        ]
+    )
+    def test_valid_constructor(args, kwargs):
+        pointA, pointB = args
+        instance = LinearPathway(*args, **kwargs)
+        assert isinstance(instance, LinearPathway)
+        assert hasattr(instance, 'attachments')
+        assert len(instance.attachments) == 2
+        assert instance.attachments[0] is pointA
+        assert instance.attachments[1] is pointB
+        assert isinstance(instance.attachments[0], Point)
+        assert instance.attachments[0].name == 'pA'
+        assert isinstance(instance.attachments[1], Point)
+        assert instance.attachments[1].name == 'pB'
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments',
+        [
+            (Point('pA'), ),
+            (Point('pA'), Point('pB'), Point('pZ')),
+        ]
+    )
+    def test_invalid_attachments_incorrect_number(attachments):
+        with pytest.raises(ValueError):
+            _ = LinearPathway(*attachments)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments',
+        [
+            (None, Point('pB')),
+            (Point('pA'), None),
+        ]
+    )
+    def test_invalid_attachments_not_point(attachments):
+        with pytest.raises(TypeError):
+            _ = LinearPathway(*attachments)
+
+    @pytest.fixture(autouse=True)
+    def _linear_pathway_fixture(self):
+        self.N = ReferenceFrame('N')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.pathway = LinearPathway(self.pA, self.pB)
+        self.q1 = dynamicsymbols('q1')
+        self.q2 = dynamicsymbols('q2')
+        self.q3 = dynamicsymbols('q3')
+        self.q1d = dynamicsymbols('q1', 1)
+        self.q2d = dynamicsymbols('q2', 1)
+        self.q3d = dynamicsymbols('q3', 1)
+        self.F = Symbol('F')
+
+    def test_properties_are_immutable(self):
+        instance = LinearPathway(self.pA, self.pB)
+        with pytest.raises(AttributeError):
+            instance.attachments = None
+        with pytest.raises(TypeError):
+            instance.attachments[0] = None
+        with pytest.raises(TypeError):
+            instance.attachments[1] = None
+
+    def test_repr(self):
+        pathway = LinearPathway(self.pA, self.pB)
+        expected = 'LinearPathway(pA, pB)'
+        assert repr(pathway) == expected
+
+    def test_static_pathway_length(self):
+        self.pB.set_pos(self.pA, 2*self.N.x)
+        assert self.pathway.length == 2
+
+    def test_static_pathway_extension_velocity(self):
+        self.pB.set_pos(self.pA, 2*self.N.x)
+        assert self.pathway.extension_velocity == 0
+
+    def test_static_pathway_to_loads(self):
+        self.pB.set_pos(self.pA, 2*self.N.x)
+        expected = [
+            (self.pA, - self.F*self.N.x),
+            (self.pB, self.F*self.N.x),
+        ]
+        assert self.pathway.to_loads(self.F) == expected
+
+    def test_2D_pathway_length(self):
+        self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
+        expected = 2*sqrt(self.q1**2)
+        assert self.pathway.length == expected
+
+    def test_2D_pathway_extension_velocity(self):
+        self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
+        expected = 2*sqrt(self.q1**2)*self.q1d/self.q1
+        assert self.pathway.extension_velocity == expected
+
+    def test_2D_pathway_to_loads(self):
+        self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
+        expected = [
+            (self.pA, - self.F*(self.q1 / sqrt(self.q1**2))*self.N.x),
+            (self.pB, self.F*(self.q1 / sqrt(self.q1**2))*self.N.x),
+        ]
+        assert self.pathway.to_loads(self.F) == expected
+
+    def test_3D_pathway_length(self):
+        self.pB.set_pos(
+            self.pA,
+            self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
+        )
+        expected = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
+        assert simplify(self.pathway.length - expected) == 0
+
+    def test_3D_pathway_extension_velocity(self):
+        self.pB.set_pos(
+            self.pA,
+            self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
+        )
+        length = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
+        expected = (
+            self.q1*self.q1d/length
+            + self.q2*self.q2d/length
+            + 4*self.q3*self.q3d/length
+        )
+        assert simplify(self.pathway.extension_velocity - expected) == 0
+
+    def test_3D_pathway_to_loads(self):
+        self.pB.set_pos(
+            self.pA,
+            self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
+        )
+        length = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
+        pO_force = (
+            - self.F*self.q1*self.N.x/length
+            + self.F*self.q2*self.N.y/length
+            - 2*self.F*self.q3*self.N.z/length
+        )
+        pI_force = (
+            self.F*self.q1*self.N.x/length
+            - self.F*self.q2*self.N.y/length
+            + 2*self.F*self.q3*self.N.z/length
+        )
+        expected = [
+            (self.pA, pO_force),
+            (self.pB, pI_force),
+        ]
+        assert self.pathway.to_loads(self.F) == expected
+
+
+class TestObstacleSetPathway:
+
+    def test_is_pathway_base_subclass(self):
+        assert issubclass(ObstacleSetPathway, PathwayBase)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'num_attachments, attachments',
+        [
+            (3, [Point(name) for name in ('pO', 'pA', 'pI')]),
+            (4, [Point(name) for name in ('pO', 'pA', 'pB', 'pI')]),
+            (5, [Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pI')]),
+            (6, [Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pD', 'pI')]),
+        ]
+    )
+    def test_valid_constructor(num_attachments, attachments):
+        instance = ObstacleSetPathway(*attachments)
+        assert isinstance(instance, ObstacleSetPathway)
+        assert hasattr(instance, 'attachments')
+        assert len(instance.attachments) == num_attachments
+        for attachment in instance.attachments:
+            assert isinstance(attachment, Point)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments',
+        [[Point('pO')], [Point('pO'), Point('pI')]],
+    )
+    def test_invalid_constructor_attachments_incorrect_number(attachments):
+        with pytest.raises(ValueError):
+            _ = ObstacleSetPathway(*attachments)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments',
+        [
+            (None, Point('pA'), Point('pI')),
+            (Point('pO'), None, Point('pI')),
+            (Point('pO'), Point('pA'), None),
+        ]
+    )
+    def test_invalid_constructor_attachments_not_point(attachments):
+        with pytest.raises(TypeError):
+            _ = WrappingPathway(*attachments)  # type: ignore
+
+    def test_properties_are_immutable(self):
+        pathway = ObstacleSetPathway(Point('pO'), Point('pA'), Point('pI'))
+        with pytest.raises(AttributeError):
+            pathway.attachments = None  # type: ignore
+        with pytest.raises(TypeError):
+            pathway.attachments[0] = None  # type: ignore
+        with pytest.raises(TypeError):
+            pathway.attachments[1] = None  # type: ignore
+        with pytest.raises(TypeError):
+            pathway.attachments[-1] = None  # type: ignore
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments, expected',
+        [
+            (
+                [Point(name) for name in ('pO', 'pA', 'pI')],
+                'ObstacleSetPathway(pO, pA, pI)'
+            ),
+            (
+                [Point(name) for name in ('pO', 'pA', 'pB', 'pI')],
+                'ObstacleSetPathway(pO, pA, pB, pI)'
+            ),
+            (
+                [Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pI')],
+                'ObstacleSetPathway(pO, pA, pB, pC, pI)'
+            ),
+        ]
+    )
+    def test_repr(attachments, expected):
+        pathway = ObstacleSetPathway(*attachments)
+        assert repr(pathway) == expected
+
+    @pytest.fixture(autouse=True)
+    def _obstacle_set_pathway_fixture(self):
+        self.N = ReferenceFrame('N')
+        self.pO = Point('pO')
+        self.pI = Point('pI')
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.q = dynamicsymbols('q')
+        self.qd = dynamicsymbols('q', 1)
+        self.F = Symbol('F')
+
+    def test_static_pathway_length(self):
+        self.pA.set_pos(self.pO, self.N.x)
+        self.pB.set_pos(self.pO, self.N.y)
+        self.pI.set_pos(self.pO, self.N.z)
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        assert pathway.length == 1 + 2 * sqrt(2)
+
+    def test_static_pathway_extension_velocity(self):
+        self.pA.set_pos(self.pO, self.N.x)
+        self.pB.set_pos(self.pO, self.N.y)
+        self.pI.set_pos(self.pO, self.N.z)
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        assert pathway.extension_velocity == 0
+
+    def test_static_pathway_to_loads(self):
+        self.pA.set_pos(self.pO, self.N.x)
+        self.pB.set_pos(self.pO, self.N.y)
+        self.pI.set_pos(self.pO, self.N.z)
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        expected = [
+            Force(self.pO, -self.F * self.N.x),
+            Force(self.pA, self.F * self.N.x),
+            Force(self.pA, self.F * sqrt(2) / 2 * (self.N.x - self.N.y)),
+            Force(self.pB, self.F * sqrt(2) / 2 * (self.N.y - self.N.x)),
+            Force(self.pB, self.F * sqrt(2) / 2 * (self.N.y - self.N.z)),
+            Force(self.pI, self.F * sqrt(2) / 2 * (self.N.z - self.N.y)),
+        ]
+        assert pathway.to_loads(self.F) == expected
+
+    def test_2D_pathway_length(self):
+        self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
+        self.pB.set_pos(
+            self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
+        )
+        self.pI.set_pos(
+            self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
+        )
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        expected = 2 * sqrt(2) + sqrt(2 + 2*cos(self.q))
+        assert (pathway.length - expected).simplify() == 0
+
+    def test_2D_pathway_extension_velocity(self):
+        self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
+        self.pB.set_pos(
+            self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
+        )
+        self.pI.set_pos(
+            self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
+        )
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        expected = - (sqrt(2) * sin(self.q) * self.qd) / (2 * sqrt(cos(self.q) + 1))
+        assert (pathway.extension_velocity - expected).simplify() == 0
+
+    def test_2D_pathway_to_loads(self):
+        self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
+        self.pB.set_pos(
+            self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
+        )
+        self.pI.set_pos(
+            self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
+        )
+        pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
+        pO_pA_force_vec = sqrt(2) / 2 * (self.N.x + self.N.y)
+        pA_pB_force_vec = (
+            - sqrt(2 * cos(self.q) + 2) / 2 * self.N.x
+            + sqrt(2) * sin(self.q) / (2 * sqrt(cos(self.q) + 1)) * self.N.y
+        )
+        pB_pI_force_vec = cos(self.q + pi/4) * self.N.x - sin(self.q + pi/4) * self.N.y
+        expected = [
+            Force(self.pO, self.F * pO_pA_force_vec),
+            Force(self.pA, -self.F * pO_pA_force_vec),
+            Force(self.pA, self.F * pA_pB_force_vec),
+            Force(self.pB, -self.F * pA_pB_force_vec),
+            Force(self.pB, self.F * pB_pI_force_vec),
+            Force(self.pI, -self.F * pB_pI_force_vec),
+        ]
+        assert _simplify_loads(pathway.to_loads(self.F)) == expected
+
+
+class TestWrappingPathway:
+
+    def test_is_pathway_base_subclass(self):
+        assert issubclass(WrappingPathway, PathwayBase)
+
+    @pytest.fixture(autouse=True)
+    def _wrapping_pathway_fixture(self):
+        self.pA = Point('pA')
+        self.pB = Point('pB')
+        self.r = Symbol('r', positive=True)
+        self.pO = Point('pO')
+        self.N = ReferenceFrame('N')
+        self.ax = self.N.z
+        self.sphere = WrappingSphere(self.r, self.pO)
+        self.cylinder = WrappingCylinder(self.r, self.pO, self.ax)
+        self.pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
+        self.F = Symbol('F')
+
+    def test_valid_constructor(self):
+        instance = WrappingPathway(self.pA, self.pB, self.cylinder)
+        assert isinstance(instance, WrappingPathway)
+        assert hasattr(instance, 'attachments')
+        assert len(instance.attachments) == 2
+        assert isinstance(instance.attachments[0], Point)
+        assert instance.attachments[0] == self.pA
+        assert isinstance(instance.attachments[1], Point)
+        assert instance.attachments[1] == self.pB
+        assert hasattr(instance, 'geometry')
+        assert isinstance(instance.geometry, WrappingGeometryBase)
+        assert instance.geometry == self.cylinder
+
+    @pytest.mark.parametrize(
+        'attachments',
+        [
+            (Point('pA'), ),
+            (Point('pA'), Point('pB'), Point('pZ')),
+        ]
+    )
+    def test_invalid_constructor_attachments_incorrect_number(self, attachments):
+        with pytest.raises(TypeError):
+            _ = WrappingPathway(*attachments, self.cylinder)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'attachments',
+        [
+            (None, Point('pB')),
+            (Point('pA'), None),
+        ]
+    )
+    def test_invalid_constructor_attachments_not_point(attachments):
+        with pytest.raises(TypeError):
+            _ = WrappingPathway(*attachments)
+
+    def test_invalid_constructor_geometry_is_not_supplied(self):
+        with pytest.raises(TypeError):
+            _ = WrappingPathway(self.pA, self.pB)
+
+    @pytest.mark.parametrize(
+        'geometry',
+        [
+            Symbol('r'),
+            dynamicsymbols('q'),
+            ReferenceFrame('N'),
+            ReferenceFrame('N').x,
+        ]
+    )
+    def test_invalid_geometry_not_geometry(self, geometry):
+        with pytest.raises(TypeError):
+            _ = WrappingPathway(self.pA, self.pB, geometry)
+
+    def test_attachments_property_is_immutable(self):
+        with pytest.raises(TypeError):
+            self.pathway.attachments[0] = self.pB
+        with pytest.raises(TypeError):
+            self.pathway.attachments[1] = self.pA
+
+    def test_geometry_property_is_immutable(self):
+        with pytest.raises(AttributeError):
+            self.pathway.geometry = None
+
+    def test_repr(self):
+        expected = (
+            f'WrappingPathway(pA, pB, '
+            f'geometry={self.cylinder!r})'
+        )
+        assert repr(self.pathway) == expected
+
+    @staticmethod
+    def _expand_pos_to_vec(pos, frame):
+        return sum(mag*unit for (mag, unit) in zip(pos, frame))
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec, factor',
+        [
+            ((1, 0, 0), (0, 1, 0), pi/2),
+            ((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0), 3*pi/4),
+            ((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0), pi/3),
+        ]
+    )
+    def test_static_pathway_on_sphere_length(self, pA_vec, pB_vec, factor):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.sphere)
+        expected = factor*self.r
+        assert simplify(pathway.length - expected) == 0
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec, factor',
+        [
+            ((1, 0, 0), (0, 1, 0), Rational(1, 2)*pi),
+            ((1, 0, 0), (-1, 0, 0), pi),
+            ((-1, 0, 0), (1, 0, 0), pi),
+            ((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0), 5*pi/4),
+            ((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0), pi/3),
+            (
+                (0, 1, 0),
+                (sqrt(2)*Rational(1, 2), -sqrt(2)*Rational(1, 2), 1),
+                sqrt(1 + (Rational(5, 4)*pi)**2),
+            ),
+            (
+                (1, 0, 0),
+                (Rational(1, 2), sqrt(3)*Rational(1, 2), 1),
+                sqrt(1 + (Rational(1, 3)*pi)**2),
+            ),
+        ]
+    )
+    def test_static_pathway_on_cylinder_length(self, pA_vec, pB_vec, factor):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
+        expected = factor*sqrt(self.r**2)
+        assert simplify(pathway.length - expected) == 0
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec',
+        [
+            ((1, 0, 0), (0, 1, 0)),
+            ((0, 1, 0), (sqrt(2)*Rational(1, 2), -sqrt(2)*Rational(1, 2), 0)),
+            ((1, 0, 0), (Rational(1, 2), sqrt(3)*Rational(1, 2), 0)),
+        ]
+    )
+    def test_static_pathway_on_sphere_extension_velocity(self, pA_vec, pB_vec):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.sphere)
+        assert pathway.extension_velocity == 0
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec',
+        [
+            ((1, 0, 0), (0, 1, 0)),
+            ((1, 0, 0), (-1, 0, 0)),
+            ((-1, 0, 0), (1, 0, 0)),
+            ((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0)),
+            ((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0)),
+            ((0, 1, 0), (sqrt(2)*Rational(1, 2), -sqrt(2)/2, 1)),
+            ((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 1)),
+        ]
+    )
+    def test_static_pathway_on_cylinder_extension_velocity(self, pA_vec, pB_vec):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
+        assert pathway.extension_velocity == 0
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec, pA_vec_expected, pB_vec_expected, pO_vec_expected',
+        (
+            ((1, 0, 0), (0, 1, 0), (0, 1, 0), (1, 0, 0), (-1, -1, 0)),
+            (
+                (0, 1, 0),
+                (sqrt(2)/2, -sqrt(2)/2, 0),
+                (1, 0, 0),
+                (sqrt(2)/2, sqrt(2)/2, 0),
+                (-1 - sqrt(2)/2, -sqrt(2)/2, 0)
+            ),
+            (
+                (1, 0, 0),
+                (Rational(1, 2), sqrt(3)/2, 0),
+                (0, 1, 0),
+                (sqrt(3)/2, -Rational(1, 2), 0),
+                (-sqrt(3)/2, Rational(1, 2) - 1, 0),
+            ),
+        )
+    )
+    def test_static_pathway_on_sphere_to_loads(
+        self,
+        pA_vec,
+        pB_vec,
+        pA_vec_expected,
+        pB_vec_expected,
+        pO_vec_expected,
+    ):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.sphere)
+
+        pA_vec_expected = sum(
+            mag*unit for (mag, unit) in zip(pA_vec_expected, self.N)
+        )
+        pB_vec_expected = sum(
+            mag*unit for (mag, unit) in zip(pB_vec_expected, self.N)
+        )
+        pO_vec_expected = sum(
+            mag*unit for (mag, unit) in zip(pO_vec_expected, self.N)
+        )
+        expected = [
+            Force(self.pA, self.F*(self.r**3/sqrt(self.r**6))*pA_vec_expected),
+            Force(self.pB, self.F*(self.r**3/sqrt(self.r**6))*pB_vec_expected),
+            Force(self.pO, self.F*(self.r**3/sqrt(self.r**6))*pO_vec_expected),
+        ]
+        assert pathway.to_loads(self.F) == expected
+
+    @pytest.mark.parametrize(
+        'pA_vec, pB_vec, pA_vec_expected, pB_vec_expected, pO_vec_expected',
+        (
+            ((1, 0, 0), (0, 1, 0), (0, 1, 0), (1, 0, 0), (-1, -1, 0)),
+            ((1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, 1, 0), (0, -2, 0)),
+            ((-1, 0, 0), (1, 0, 0), (0, -1, 0), (0, -1, 0), (0, 2, 0)),
+            (
+                (0, 1, 0),
+                (sqrt(2)/2, -sqrt(2)/2, 0),
+                (-1, 0, 0),
+                (-sqrt(2)/2, -sqrt(2)/2, 0),
+                (1 + sqrt(2)/2, sqrt(2)/2, 0)
+            ),
+            (
+                (1, 0, 0),
+                (Rational(1, 2), sqrt(3)/2, 0),
+                (0, 1, 0),
+                (sqrt(3)/2, -Rational(1, 2), 0),
+                (-sqrt(3)/2, Rational(1, 2) - 1, 0),
+            ),
+            (
+                (1, 0, 0),
+                (sqrt(2)/2, sqrt(2)/2, 0),
+                (0, 1, 0),
+                (sqrt(2)/2, -sqrt(2)/2, 0),
+                (-sqrt(2)/2, sqrt(2)/2 - 1, 0),
+            ),
+            ((0, 1, 0), (0, 1, 1), (0, 0, 1), (0, 0, -1), (0, 0, 0)),
+            (
+                (0, 1, 0),
+                (sqrt(2)/2, -sqrt(2)/2, 1),
+                (-5*pi/sqrt(16 + 25*pi**2), 0, 4/sqrt(16 + 25*pi**2)),
+                (
+                    -5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
+                    -5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
+                    -4/sqrt(16 + 25*pi**2),
+                ),
+                (
+                    5*(sqrt(2) + 2)*pi/(2*sqrt(16 + 25*pi**2)),
+                    5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
+                    0,
+                ),
+            ),
+        )
+    )
+    def test_static_pathway_on_cylinder_to_loads(
+        self,
+        pA_vec,
+        pB_vec,
+        pA_vec_expected,
+        pB_vec_expected,
+        pO_vec_expected,
+    ):
+        pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
+        pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
+        self.pA.set_pos(self.pO, self.r*pA_vec)
+        self.pB.set_pos(self.pO, self.r*pB_vec)
+        pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
+
+        pA_force_expected = self.F*self._expand_pos_to_vec(pA_vec_expected,
+                                                           self.N)
+        pB_force_expected = self.F*self._expand_pos_to_vec(pB_vec_expected,
+                                                           self.N)
+        pO_force_expected = self.F*self._expand_pos_to_vec(pO_vec_expected,
+                                                           self.N)
+        expected = [
+            Force(self.pA, pA_force_expected),
+            Force(self.pB, pB_force_expected),
+            Force(self.pO, pO_force_expected),
+        ]
+        assert _simplify_loads(pathway.to_loads(self.F)) == expected
+
+    def test_2D_pathway_on_cylinder_length(self):
+        q = dynamicsymbols('q')
+        pA_pos = self.r*self.N.x
+        pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
+        self.pA.set_pos(self.pO, pA_pos)
+        self.pB.set_pos(self.pO, pB_pos)
+        expected = self.r*sqrt(q**2)
+        assert simplify(self.pathway.length - expected) == 0
+
+    def test_2D_pathway_on_cylinder_extension_velocity(self):
+        q = dynamicsymbols('q')
+        qd = dynamicsymbols('q', 1)
+        pA_pos = self.r*self.N.x
+        pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
+        self.pA.set_pos(self.pO, pA_pos)
+        self.pB.set_pos(self.pO, pB_pos)
+        expected = self.r*(sqrt(q**2)/q)*qd
+        assert simplify(self.pathway.extension_velocity - expected) == 0
+
+    def test_2D_pathway_on_cylinder_to_loads(self):
+        q = dynamicsymbols('q')
+        pA_pos = self.r*self.N.x
+        pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
+        self.pA.set_pos(self.pO, pA_pos)
+        self.pB.set_pos(self.pO, pB_pos)
+
+        pA_force = self.F*self.N.y
+        pB_force = self.F*(sin(q)*self.N.x - cos(q)*self.N.y)
+        pO_force = self.F*(-sin(q)*self.N.x + (cos(q) - 1)*self.N.y)
+        expected = [
+            Force(self.pA, pA_force),
+            Force(self.pB, pB_force),
+            Force(self.pO, pO_force),
+        ]
+
+        loads = _simplify_loads(self.pathway.to_loads(self.F))
+        assert loads == expected

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_rigidbody.py

@@ -0,0 +1,184 @@
+from sympy.physics.mechanics import Point, ReferenceFrame, Dyadic, RigidBody
+from sympy.physics.mechanics import dynamicsymbols, outer, inertia, Inertia
+from sympy.physics.mechanics import inertia_of_point_mass
+from sympy import expand, zeros, simplify, symbols
+from sympy.testing.pytest import raises, warns_deprecated_sympy
+
+
+def test_rigidbody_default():
+    # Test default
+    b = RigidBody('B')
+    I = inertia(b.frame, *symbols('B_ixx B_iyy B_izz B_ixy B_iyz B_izx'))
+    assert b.name == 'B'
+    assert b.mass == symbols('B_mass')
+    assert b.masscenter.name == 'B_masscenter'
+    assert b.inertia == (I, b.masscenter)
+    assert b.central_inertia == I
+    assert b.frame.name == 'B_frame'
+    assert b.__str__() == 'B'
+    assert b.__repr__() == (
+        "RigidBody('B', masscenter=B_masscenter, frame=B_frame, mass=B_mass, "
+        "inertia=Inertia(dyadic=B_ixx*(B_frame.x|B_frame.x) + "
+        "B_ixy*(B_frame.x|B_frame.y) + B_izx*(B_frame.x|B_frame.z) + "
+        "B_ixy*(B_frame.y|B_frame.x) + B_iyy*(B_frame.y|B_frame.y) + "
+        "B_iyz*(B_frame.y|B_frame.z) + B_izx*(B_frame.z|B_frame.x) + "
+        "B_iyz*(B_frame.z|B_frame.y) + B_izz*(B_frame.z|B_frame.z), "
+        "point=B_masscenter))")
+
+
+def test_rigidbody():
+    m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega')
+    A = ReferenceFrame('A')
+    A2 = ReferenceFrame('A2')
+    P = Point('P')
+    P2 = Point('P2')
+    I = Dyadic(0)
+    I2 = Dyadic(0)
+    B = RigidBody('B', P, A, m, (I, P))
+    assert B.mass == m
+    assert B.frame == A
+    assert B.masscenter == P
+    assert B.inertia == (I, B.masscenter)
+
+    B.mass = m2
+    B.frame = A2
+    B.masscenter = P2
+    B.inertia = (I2, B.masscenter)
+    raises(TypeError, lambda: RigidBody(P, P, A, m, (I, P)))
+    raises(TypeError, lambda: RigidBody('B', P, P, m, (I, P)))
+    raises(TypeError, lambda: RigidBody('B', P, A, m, (P, P)))
+    raises(TypeError, lambda: RigidBody('B', P, A, m, (I, I)))
+    assert B.__str__() == 'B'
+    assert B.mass == m2
+    assert B.frame == A2
+    assert B.masscenter == P2
+    assert B.inertia == (I2, B.masscenter)
+    assert isinstance(B.inertia, Inertia)
+
+    # Testing linear momentum function assuming A2 is the inertial frame
+    N = ReferenceFrame('N')
+    P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
+    assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
+
+
+def test_rigidbody2():
+    M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
+    N = ReferenceFrame('N')
+    b = ReferenceFrame('b')
+    b.set_ang_vel(N, omega * b.x)
+    P = Point('P')
+    I = outer(b.x, b.x)
+    Inertia_tuple = (I, P)
+    B = RigidBody('B', P, b, M, Inertia_tuple)
+    P.set_vel(N, v * b.x)
+    assert B.angular_momentum(P, N) == omega * b.x
+    O = Point('O')
+    O.set_vel(N, v * b.x)
+    P.set_pos(O, r * b.y)
+    assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
+    B.potential_energy = M * g * h
+    assert B.potential_energy == M * g * h
+    assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
+
+
+def test_rigidbody3():
+    q1, q2, q3, q4 = dynamicsymbols('q1:5')
+    p1, p2, p3 = symbols('p1:4')
+    m = symbols('m')
+
+    A = ReferenceFrame('A')
+    B = A.orientnew('B', 'axis', [q1, A.x])
+    O = Point('O')
+    O.set_vel(A, q2*A.x + q3*A.y + q4*A.z)
+    P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z)
+    P.v2pt_theory(O, A, B)
+    I = outer(B.x, B.x)
+
+    rb1 = RigidBody('rb1', P, B, m, (I, P))
+    # I_S/O = I_S/S* + I_S*/O
+    rb2 = RigidBody('rb2', P, B, m,
+                    (I + inertia_of_point_mass(m, P.pos_from(O), B), O))
+
+    assert rb1.central_inertia == rb2.central_inertia
+    assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
+
+
+def test_pendulum_angular_momentum():
+    """Consider a pendulum of length OA = 2a, of mass m as a rigid body of
+    center of mass G (OG = a) which turn around (O,z). The angle between the
+    reference frame R and the rod is q.  The inertia of the body is I =
+    (G,0,ma^2/3,ma^2/3). """
+
+    m, a = symbols('m, a')
+    q = dynamicsymbols('q')
+
+    R = ReferenceFrame('R')
+    R1 = R.orientnew('R1', 'Axis', [q, R.z])
+    R1.set_ang_vel(R, q.diff() * R.z)
+
+    I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)
+
+    O = Point('O')
+
+    A = O.locatenew('A', 2*a * R1.x)
+    G = O.locatenew('G', a * R1.x)
+
+    S = RigidBody('S', G, R1, m, (I, G))
+
+    O.set_vel(R, 0)
+    A.v2pt_theory(O, R, R1)
+    G.v2pt_theory(O, R, R1)
+
+    assert (4 * m * a**2 / 3 * q.diff() * R.z -
+            S.angular_momentum(O, R).express(R)) == 0
+
+
+def test_rigidbody_inertia():
+    N = ReferenceFrame('N')
+    m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
+    Io = inertia(N, Ix, Iy, Iz)
+    o = Point('o')
+    p = o.locatenew('p', a * N.x + b * N.y)
+    R = RigidBody('R', o, N, m, (Io, p))
+    I_check = inertia(N, Ix - b ** 2 * m, Iy - a ** 2 * m,
+                      Iz - m * (a ** 2 + b ** 2), m * a * b)
+    assert isinstance(R.inertia, Inertia)
+    assert R.inertia == (Io, p)
+    assert R.central_inertia == I_check
+    R.central_inertia = Io
+    assert R.inertia == (Io, o)
+    assert R.central_inertia == Io
+    R.inertia = (Io, p)
+    assert R.inertia == (Io, p)
+    assert R.central_inertia == I_check
+    # parse Inertia object
+    R.inertia = Inertia(Io, o)
+    assert R.inertia == (Io, o)
+
+
+def test_parallel_axis():
+    N = ReferenceFrame('N')
+    m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
+    Io = inertia(N, Ix, Iy, Iz)
+    o = Point('o')
+    p = o.locatenew('p', a * N.x + b * N.y)
+    R = RigidBody('R', o, N, m, (Io, o))
+    Ip = R.parallel_axis(p)
+    Ip_expected = inertia(N, Ix + m * b**2, Iy + m * a**2,
+                          Iz + m * (a**2 + b**2), ixy=-m * a * b)
+    assert Ip == Ip_expected
+    # Reference frame from which the parallel axis is viewed should not matter
+    A = ReferenceFrame('A')
+    A.orient_axis(N, N.z, 1)
+    assert simplify(
+        (R.parallel_axis(p, A) - Ip_expected).to_matrix(A)) == zeros(3, 3)
+
+
+def test_deprecated_set_potential_energy():
+    m, g, h = symbols('m g h')
+    A = ReferenceFrame('A')
+    P = Point('P')
+    I = Dyadic(0)
+    B = RigidBody('B', P, A, m, (I, P))
+    with warns_deprecated_sympy():
+        B.set_potential_energy(m*g*h)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_system.py

@@ -0,0 +1,245 @@
+from sympy import symbols, Matrix, atan, zeros
+from sympy.simplify.simplify import simplify
+from sympy.physics.mechanics import (dynamicsymbols, Particle, Point,
+                                     ReferenceFrame, SymbolicSystem)
+from sympy.testing.pytest import raises
+
+# This class is going to be tested using a simple pendulum set up in x and y
+# coordinates
+x, y, u, v, lam = dynamicsymbols('x y u v lambda')
+m, l, g = symbols('m l g')
+
+# Set up the different forms the equations can take
+#       [1] Explicit form where the kinematics and dynamics are combined
+#           x' = F(x, t, r, p)
+#
+#       [2] Implicit form where the kinematics and dynamics are combined
+#           M(x, p) x' = F(x, t, r, p)
+#
+#       [3] Implicit form where the kinematics and dynamics are separate
+#           M(q, p) u' = F(q, u, t, r, p)
+#           q' = G(q, u, t, r, p)
+dyn_implicit_mat = Matrix([[1, 0, -x/m],
+                           [0, 1, -y/m],
+                           [0, 0, l**2/m]])
+
+dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g*y])
+
+comb_implicit_mat = Matrix([[1, 0, 0, 0, 0],
+                            [0, 1, 0, 0, 0],
+                            [0, 0, 1, 0, -x/m],
+                            [0, 0, 0, 1, -y/m],
+                            [0, 0, 0, 0, l**2/m]])
+
+comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g*y])
+
+kin_explicit_rhs = Matrix([u, v])
+
+comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs)
+
+# Set up a body and load to pass into the system
+theta = atan(x/y)
+N = ReferenceFrame('N')
+A = N.orientnew('A', 'Axis', [theta, N.z])
+O = Point('O')
+P = O.locatenew('P', l * A.x)
+
+Pa = Particle('Pa', P, m)
+
+bodies = [Pa]
+loads = [(P, g * m * N.x)]
+
+# Set up some output equations to be given to SymbolicSystem
+# Change to make these fit the pendulum
+PE = symbols("PE")
+out_eqns = {PE: m*g*(l+y)}
+
+# Set up remaining arguments that can be passed to SymbolicSystem
+alg_con = [2]
+alg_con_full = [4]
+coordinates = (x, y, lam)
+speeds = (u, v)
+states = (x, y, u, v, lam)
+coord_idxs = (0, 1)
+speed_idxs = (2, 3)
+
+
+def test_form_1():
+    symsystem1 = SymbolicSystem(states, comb_explicit_rhs,
+                                alg_con=alg_con_full, output_eqns=out_eqns,
+                                coord_idxs=coord_idxs, speed_idxs=speed_idxs,
+                                bodies=bodies, loads=loads)
+
+    assert symsystem1.coordinates == Matrix([x, y])
+    assert symsystem1.speeds == Matrix([u, v])
+    assert symsystem1.states == Matrix([x, y, u, v, lam])
+
+    assert symsystem1.alg_con == [4]
+
+    inter = comb_explicit_rhs
+    assert simplify(symsystem1.comb_explicit_rhs - inter) == zeros(5, 1)
+
+    assert set(symsystem1.dynamic_symbols()) == {y, v, lam, u, x}
+    assert type(symsystem1.dynamic_symbols()) == tuple
+    assert set(symsystem1.constant_symbols()) == {l, g, m}
+    assert type(symsystem1.constant_symbols()) == tuple
+
+    assert symsystem1.output_eqns == out_eqns
+
+    assert symsystem1.bodies == (Pa,)
+    assert symsystem1.loads == ((P, g * m * N.x),)
+
+
+def test_form_2():
+    symsystem2 = SymbolicSystem(coordinates, comb_implicit_rhs, speeds=speeds,
+                                mass_matrix=comb_implicit_mat,
+                                alg_con=alg_con_full, output_eqns=out_eqns,
+                                bodies=bodies, loads=loads)
+
+    assert symsystem2.coordinates == Matrix([x, y, lam])
+    assert symsystem2.speeds == Matrix([u, v])
+    assert symsystem2.states == Matrix([x, y, lam, u, v])
+
+    assert symsystem2.alg_con == [4]
+
+    inter = comb_implicit_rhs
+    assert simplify(symsystem2.comb_implicit_rhs - inter) == zeros(5, 1)
+    assert simplify(symsystem2.comb_implicit_mat-comb_implicit_mat) == zeros(5)
+
+    assert set(symsystem2.dynamic_symbols()) == {y, v, lam, u, x}
+    assert type(symsystem2.dynamic_symbols()) == tuple
+    assert set(symsystem2.constant_symbols()) == {l, g, m}
+    assert type(symsystem2.constant_symbols()) == tuple
+
+    inter = comb_explicit_rhs
+    symsystem2.compute_explicit_form()
+    assert simplify(symsystem2.comb_explicit_rhs - inter) == zeros(5, 1)
+
+
+    assert symsystem2.output_eqns == out_eqns
+
+    assert symsystem2.bodies == (Pa,)
+    assert symsystem2.loads == ((P, g * m * N.x),)
+
+
+def test_form_3():
+    symsystem3 = SymbolicSystem(states, dyn_implicit_rhs,
+                                mass_matrix=dyn_implicit_mat,
+                                coordinate_derivatives=kin_explicit_rhs,
+                                alg_con=alg_con, coord_idxs=coord_idxs,
+                                speed_idxs=speed_idxs, bodies=bodies,
+                                loads=loads)
+
+    assert symsystem3.coordinates == Matrix([x, y])
+    assert symsystem3.speeds == Matrix([u, v])
+    assert symsystem3.states == Matrix([x, y, u, v, lam])
+
+    assert symsystem3.alg_con == [4]
+
+    inter1 = kin_explicit_rhs
+    inter2 = dyn_implicit_rhs
+    assert simplify(symsystem3.kin_explicit_rhs - inter1) == zeros(2, 1)
+    assert simplify(symsystem3.dyn_implicit_mat - dyn_implicit_mat) == zeros(3)
+    assert simplify(symsystem3.dyn_implicit_rhs - inter2) == zeros(3, 1)
+
+    inter = comb_implicit_rhs
+    assert simplify(symsystem3.comb_implicit_rhs - inter) == zeros(5, 1)
+    assert simplify(symsystem3.comb_implicit_mat-comb_implicit_mat) == zeros(5)
+
+    inter = comb_explicit_rhs
+    symsystem3.compute_explicit_form()
+    assert simplify(symsystem3.comb_explicit_rhs - inter) == zeros(5, 1)
+
+    assert set(symsystem3.dynamic_symbols()) == {y, v, lam, u, x}
+    assert type(symsystem3.dynamic_symbols()) == tuple
+    assert set(symsystem3.constant_symbols()) == {l, g, m}
+    assert type(symsystem3.constant_symbols()) == tuple
+
+    assert symsystem3.output_eqns == {}
+
+    assert symsystem3.bodies == (Pa,)
+    assert symsystem3.loads == ((P, g * m * N.x),)
+
+
+def test_property_attributes():
+    symsystem = SymbolicSystem(states, comb_explicit_rhs,
+                               alg_con=alg_con_full, output_eqns=out_eqns,
+                               coord_idxs=coord_idxs, speed_idxs=speed_idxs,
+                               bodies=bodies, loads=loads)
+
+    with raises(AttributeError):
+        symsystem.bodies = 42
+    with raises(AttributeError):
+        symsystem.coordinates = 42
+    with raises(AttributeError):
+        symsystem.dyn_implicit_rhs = 42
+    with raises(AttributeError):
+        symsystem.comb_implicit_rhs = 42
+    with raises(AttributeError):
+        symsystem.loads = 42
+    with raises(AttributeError):
+        symsystem.dyn_implicit_mat = 42
+    with raises(AttributeError):
+        symsystem.comb_implicit_mat = 42
+    with raises(AttributeError):
+        symsystem.kin_explicit_rhs = 42
+    with raises(AttributeError):
+        symsystem.comb_explicit_rhs = 42
+    with raises(AttributeError):
+        symsystem.speeds = 42
+    with raises(AttributeError):
+        symsystem.states = 42
+    with raises(AttributeError):
+        symsystem.alg_con = 42
+
+
+def test_not_specified_errors():
+    """This test will cover errors that arise from trying to access attributes
+    that were not specified upon object creation or were specified on creation
+    and the user tries to recalculate them."""
+    # Trying to access form 2 when form 1 given
+    # Trying to access form 3 when form 2 given
+
+    symsystem1 = SymbolicSystem(states, comb_explicit_rhs)
+
+    with raises(AttributeError):
+        symsystem1.comb_implicit_mat
+    with raises(AttributeError):
+        symsystem1.comb_implicit_rhs
+    with raises(AttributeError):
+        symsystem1.dyn_implicit_mat
+    with raises(AttributeError):
+        symsystem1.dyn_implicit_rhs
+    with raises(AttributeError):
+        symsystem1.kin_explicit_rhs
+    with raises(AttributeError):
+        symsystem1.compute_explicit_form()
+
+    symsystem2 = SymbolicSystem(coordinates, comb_implicit_rhs, speeds=speeds,
+                                mass_matrix=comb_implicit_mat)
+
+    with raises(AttributeError):
+        symsystem2.dyn_implicit_mat
+    with raises(AttributeError):
+        symsystem2.dyn_implicit_rhs
+    with raises(AttributeError):
+        symsystem2.kin_explicit_rhs
+
+    # Attribute error when trying to access coordinates and speeds when only the
+    # states were given.
+    with raises(AttributeError):
+        symsystem1.coordinates
+    with raises(AttributeError):
+        symsystem1.speeds
+
+    # Attribute error when trying to access bodies and loads when they are not
+    # given
+    with raises(AttributeError):
+        symsystem1.bodies
+    with raises(AttributeError):
+        symsystem1.loads
+
+    # Attribute error when trying to access comb_explicit_rhs before it was
+    # calculated
+    with raises(AttributeError):
+        symsystem2.comb_explicit_rhs

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_system_class.py

@@ -0,0 +1,831 @@
+import pytest
+
+from sympy.core.symbol import symbols
+from sympy.core.sympify import sympify
+from sympy.functions.elementary.trigonometric import cos, sin
+from sympy.matrices.dense import eye, zeros
+from sympy.matrices.immutable import ImmutableMatrix
+from sympy.physics.mechanics import (
+    Force, KanesMethod, LagrangesMethod, Particle, PinJoint, Point,
+    PrismaticJoint, ReferenceFrame, RigidBody, Torque, TorqueActuator, System,
+    dynamicsymbols)
+from sympy.simplify.simplify import simplify
+from sympy.solvers.solvers import solve
+
+t = dynamicsymbols._t  # type: ignore
+q = dynamicsymbols('q:6')  # type: ignore
+qd = dynamicsymbols('q:6', 1)  # type: ignore
+u = dynamicsymbols('u:6')  # type: ignore
+ua = dynamicsymbols('ua:3')  # type: ignore
+
+
+class TestSystemBase:
+    @pytest.fixture()
+    def _empty_system_setup(self):
+        self.system = System(ReferenceFrame('frame'), Point('fixed_point'))
+
+    def _empty_system_check(self, exclude=()):
+        matrices = ('q_ind', 'q_dep', 'q', 'u_ind', 'u_dep', 'u', 'u_aux',
+                    'kdes', 'holonomic_constraints', 'nonholonomic_constraints')
+        tuples = ('loads', 'bodies', 'joints', 'actuators')
+        for attr in matrices:
+            if attr not in exclude:
+                assert getattr(self.system, attr)[:] == []
+        for attr in tuples:
+            if attr not in exclude:
+                assert getattr(self.system, attr) == ()
+        if 'eom_method' not in exclude:
+            assert self.system.eom_method is None
+
+    def _create_filled_system(self, with_speeds=True):
+        self.system = System(ReferenceFrame('frame'), Point('fixed_point'))
+        u = dynamicsymbols('u:6') if with_speeds else qd
+        self.bodies = symbols('rb1:5', cls=RigidBody)
+        self.joints = (
+            PinJoint('J1', self.bodies[0], self.bodies[1], q[0], u[0]),
+            PrismaticJoint('J2', self.bodies[1], self.bodies[2], q[1], u[1]),
+            PinJoint('J3', self.bodies[2], self.bodies[3], q[2], u[2])
+        )
+        self.system.add_joints(*self.joints)
+        self.system.add_coordinates(q[3], independent=[False])
+        self.system.add_speeds(u[3], independent=False)
+        if with_speeds:
+            self.system.add_kdes(u[3] - qd[3])
+            self.system.add_auxiliary_speeds(ua[0], ua[1])
+        self.system.add_holonomic_constraints(q[2] - q[0] + q[1])
+        self.system.add_nonholonomic_constraints(u[3] - qd[1] + u[2])
+        self.system.u_ind = u[:2]
+        self.system.u_dep = u[2:4]
+        self.q_ind, self.q_dep = self.system.q_ind[:], self.system.q_dep[:]
+        self.u_ind, self.u_dep = self.system.u_ind[:], self.system.u_dep[:]
+        self.kdes = self.system.kdes[:]
+        self.hc = self.system.holonomic_constraints[:]
+        self.vc = self.system.velocity_constraints[:]
+        self.nhc = self.system.nonholonomic_constraints[:]
+
+    @pytest.fixture()
+    def _filled_system_setup(self):
+        self._create_filled_system(with_speeds=True)
+
+    @pytest.fixture()
+    def _filled_system_setup_no_speeds(self):
+        self._create_filled_system(with_speeds=False)
+
+    def _filled_system_check(self, exclude=()):
+        assert 'q_ind' in exclude or self.system.q_ind[:] == q[:3]
+        assert 'q_dep' in exclude or self.system.q_dep[:] == [q[3]]
+        assert 'q' in exclude or self.system.q[:] == q[:4]
+        assert 'u_ind' in exclude or self.system.u_ind[:] == u[:2]
+        assert 'u_dep' in exclude or self.system.u_dep[:] == u[2:4]
+        assert 'u' in exclude or self.system.u[:] == u[:4]
+        assert 'u_aux' in exclude or self.system.u_aux[:] == ua[:2]
+        assert 'kdes' in exclude or self.system.kdes[:] == [
+            ui - qdi for ui, qdi in zip(u[:4], qd[:4])]
+        assert ('holonomic_constraints' in exclude or
+                self.system.holonomic_constraints[:] == [q[2] - q[0] + q[1]])
+        assert ('nonholonomic_constraints' in exclude or
+                self.system.nonholonomic_constraints[:] == [u[3] - qd[1] + u[2]]
+                )
+        assert ('velocity_constraints' in exclude or
+                self.system.velocity_constraints[:] == [
+                    qd[2] - qd[0] + qd[1], u[3] - qd[1] + u[2]])
+        assert ('bodies' in exclude or
+                self.system.bodies == tuple(self.bodies))
+        assert ('joints' in exclude or
+                self.system.joints == tuple(self.joints))
+
+    @pytest.fixture()
+    def _moving_point_mass(self, _empty_system_setup):
+        self.system.q_ind = q[0]
+        self.system.u_ind = u[0]
+        self.system.kdes = u[0] - q[0].diff(t)
+        p = Particle('p', mass=symbols('m'))
+        self.system.add_bodies(p)
+        p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x)
+
+
+class TestSystem(TestSystemBase):
+    def test_empty_system(self, _empty_system_setup):
+        self._empty_system_check()
+        self.system.validate_system()
+
+    def test_filled_system(self, _filled_system_setup):
+        self._filled_system_check()
+        self.system.validate_system()
+
+    @pytest.mark.parametrize('frame', [None, ReferenceFrame('frame')])
+    @pytest.mark.parametrize('fixed_point', [None, Point('fixed_point')])
+    def test_init(self, frame, fixed_point):
+        if fixed_point is None and frame is None:
+            self.system = System()
+        else:
+            self.system = System(frame, fixed_point)
+        if fixed_point is None:
+            assert self.system.fixed_point.name == 'inertial_point'
+        else:
+            assert self.system.fixed_point == fixed_point
+        if frame is None:
+            assert self.system.frame.name == 'inertial_frame'
+        else:
+            assert self.system.frame == frame
+        self._empty_system_check()
+        assert isinstance(self.system.q_ind, ImmutableMatrix)
+        assert isinstance(self.system.q_dep, ImmutableMatrix)
+        assert isinstance(self.system.q, ImmutableMatrix)
+        assert isinstance(self.system.u_ind, ImmutableMatrix)
+        assert isinstance(self.system.u_dep, ImmutableMatrix)
+        assert isinstance(self.system.u, ImmutableMatrix)
+        assert isinstance(self.system.kdes, ImmutableMatrix)
+        assert isinstance(self.system.holonomic_constraints, ImmutableMatrix)
+        assert isinstance(self.system.nonholonomic_constraints, ImmutableMatrix)
+
+    def test_from_newtonian_rigid_body(self):
+        rb = RigidBody('body')
+        self.system = System.from_newtonian(rb)
+        assert self.system.fixed_point == rb.masscenter
+        assert self.system.frame == rb.frame
+        self._empty_system_check(exclude=('bodies',))
+        self.system.bodies = (rb,)
+
+    def test_from_newtonian_particle(self):
+        pt = Particle('particle')
+        with pytest.raises(TypeError):
+            System.from_newtonian(pt)
+
+    @pytest.mark.parametrize('args, kwargs, exp_q_ind, exp_q_dep, exp_q', [
+        (q[:3], {}, q[:3], [], q[:3]),
+        (q[:3], {'independent': True}, q[:3], [], q[:3]),
+        (q[:3], {'independent': False}, [], q[:3], q[:3]),
+        (q[:3], {'independent': [True, False, True]}, [q[0], q[2]], [q[1]],
+         [q[0], q[2], q[1]]),
+    ])
+    def test_coordinates(self, _empty_system_setup, args, kwargs,
+                         exp_q_ind, exp_q_dep, exp_q):
+        # Test add_coordinates
+        self.system.add_coordinates(*args, **kwargs)
+        assert self.system.q_ind[:] == exp_q_ind
+        assert self.system.q_dep[:] == exp_q_dep
+        assert self.system.q[:] == exp_q
+        self._empty_system_check(exclude=('q_ind', 'q_dep', 'q'))
+        # Test setter for q_ind and q_dep
+        self.system.q_ind = exp_q_ind
+        self.system.q_dep = exp_q_dep
+        assert self.system.q_ind[:] == exp_q_ind
+        assert self.system.q_dep[:] == exp_q_dep
+        assert self.system.q[:] == exp_q
+        self._empty_system_check(exclude=('q_ind', 'q_dep', 'q'))
+
+    @pytest.mark.parametrize('func', ['add_coordinates', 'add_speeds'])
+    @pytest.mark.parametrize('args, kwargs', [
+        ((q[0], q[5]), {}),
+        ((u[0], u[5]), {}),
+        ((q[0],), {'independent': False}),
+        ((u[0],), {'independent': False}),
+        ((u[0], q[5]), {}),
+        ((symbols('a'), q[5]), {}),
+    ])
+    def test_coordinates_speeds_invalid(self, _filled_system_setup, func, args,
+                                        kwargs):
+        with pytest.raises(ValueError):
+            getattr(self.system, func)(*args, **kwargs)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('args, kwargs, exp_u_ind, exp_u_dep, exp_u', [
+        (u[:3], {}, u[:3], [], u[:3]),
+        (u[:3], {'independent': True}, u[:3], [], u[:3]),
+        (u[:3], {'independent': False}, [], u[:3], u[:3]),
+        (u[:3], {'independent': [True, False, True]}, [u[0], u[2]], [u[1]],
+         [u[0], u[2], u[1]]),
+    ])
+    def test_speeds(self, _empty_system_setup, args, kwargs, exp_u_ind,
+                    exp_u_dep, exp_u):
+        # Test add_speeds
+        self.system.add_speeds(*args, **kwargs)
+        assert self.system.u_ind[:] == exp_u_ind
+        assert self.system.u_dep[:] == exp_u_dep
+        assert self.system.u[:] == exp_u
+        self._empty_system_check(exclude=('u_ind', 'u_dep', 'u'))
+        # Test setter for u_ind and u_dep
+        self.system.u_ind = exp_u_ind
+        self.system.u_dep = exp_u_dep
+        assert self.system.u_ind[:] == exp_u_ind
+        assert self.system.u_dep[:] == exp_u_dep
+        assert self.system.u[:] == exp_u
+        self._empty_system_check(exclude=('u_ind', 'u_dep', 'u'))
+
+    @pytest.mark.parametrize('args, kwargs, exp_u_aux', [
+        (ua[:3], {}, ua[:3]),
+    ])
+    def test_auxiliary_speeds(self, _empty_system_setup, args, kwargs,
+                              exp_u_aux):
+        # Test add_speeds
+        self.system.add_auxiliary_speeds(*args, **kwargs)
+        assert self.system.u_aux[:] == exp_u_aux
+        self._empty_system_check(exclude=('u_aux',))
+        # Test setter for u_ind and u_dep
+        self.system.u_aux = exp_u_aux
+        assert self.system.u_aux[:] == exp_u_aux
+        self._empty_system_check(exclude=('u_aux',))
+
+    @pytest.mark.parametrize('args, kwargs', [
+        ((ua[2], q[0]), {}),
+        ((ua[2], u[1]), {}),
+        ((ua[0], ua[2]), {}),
+        ((symbols('a'), ua[2]), {}),
+    ])
+    def test_auxiliary_invalid(self, _filled_system_setup, args, kwargs):
+        with pytest.raises(ValueError):
+            self.system.add_auxiliary_speeds(*args, **kwargs)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('prop, add_func, args, kwargs', [
+        ('q_ind', 'add_coordinates', (q[0],), {}),
+        ('q_dep', 'add_coordinates', (q[3],), {'independent': False}),
+        ('u_ind', 'add_speeds', (u[0],), {}),
+        ('u_dep', 'add_speeds', (u[3],), {'independent': False}),
+        ('u_aux', 'add_auxiliary_speeds', (ua[2],), {}),
+        ('kdes', 'add_kdes', (qd[0] - u[0],), {}),
+        ('holonomic_constraints', 'add_holonomic_constraints',
+         (q[0] - q[1],), {}),
+        ('nonholonomic_constraints', 'add_nonholonomic_constraints',
+         (u[0] - u[1],), {}),
+        ('bodies', 'add_bodies', (RigidBody('body'),), {}),
+        ('loads', 'add_loads', (Force(Point('P'), ReferenceFrame('N').x),), {}),
+        ('actuators', 'add_actuators', (TorqueActuator(
+            symbols('T'), ReferenceFrame('N').x, ReferenceFrame('A')),), {}),
+    ])
+    def test_add_after_reset(self, _filled_system_setup, prop, add_func, args,
+                             kwargs):
+        setattr(self.system, prop, ())
+        exclude = (prop, 'q', 'u')
+        if prop in ('holonomic_constraints', 'nonholonomic_constraints'):
+            exclude += ('velocity_constraints',)
+        self._filled_system_check(exclude=exclude)
+        assert list(getattr(self.system, prop)[:]) == []
+        getattr(self.system, add_func)(*args, **kwargs)
+        assert list(getattr(self.system, prop)[:]) == list(args)
+
+    @pytest.mark.parametrize('prop, add_func, value, error', [
+        ('q_ind', 'add_coordinates', symbols('a'), ValueError),
+        ('q_dep', 'add_coordinates', symbols('a'), ValueError),
+        ('u_ind', 'add_speeds', symbols('a'), ValueError),
+        ('u_dep', 'add_speeds', symbols('a'), ValueError),
+        ('u_aux', 'add_auxiliary_speeds', symbols('a'), ValueError),
+        ('kdes', 'add_kdes', 7, TypeError),
+        ('holonomic_constraints', 'add_holonomic_constraints', 7, TypeError),
+        ('nonholonomic_constraints', 'add_nonholonomic_constraints', 7,
+         TypeError),
+        ('bodies', 'add_bodies', symbols('a'), TypeError),
+        ('loads', 'add_loads', symbols('a'), TypeError),
+        ('actuators', 'add_actuators', symbols('a'), TypeError),
+    ])
+    def test_type_error(self, _filled_system_setup, prop, add_func, value,
+                        error):
+        with pytest.raises(error):
+            getattr(self.system, add_func)(value)
+        with pytest.raises(error):
+            setattr(self.system, prop, value)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('args, kwargs, exp_kdes', [
+        ((), {}, [ui - qdi for ui, qdi in zip(u[:4], qd[:4])]),
+        ((u[4] - qd[4], u[5] - qd[5]), {},
+         [ui - qdi for ui, qdi in zip(u[:6], qd[:6])]),
+    ])
+    def test_kdes(self, _filled_system_setup, args, kwargs, exp_kdes):
+        # Test add_speeds
+        self.system.add_kdes(*args, **kwargs)
+        self._filled_system_check(exclude=('kdes',))
+        assert self.system.kdes[:] == exp_kdes
+        # Test setter for kdes
+        self.system.kdes = exp_kdes
+        self._filled_system_check(exclude=('kdes',))
+        assert self.system.kdes[:] == exp_kdes
+
+    @pytest.mark.parametrize('args, kwargs', [
+        ((u[0] - qd[0], u[4] - qd[4]), {}),
+        ((-(u[0] - qd[0]), u[4] - qd[4]), {}),
+        (([u[0] - u[0], u[4] - qd[4]]), {}),
+    ])
+    def test_kdes_invalid(self, _filled_system_setup, args, kwargs):
+        with pytest.raises(ValueError):
+            self.system.add_kdes(*args, **kwargs)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('args, kwargs, exp_con', [
+        ((), {}, [q[2] - q[0] + q[1]]),
+        ((q[4] - q[5], q[5] + q[3]), {},
+         [q[2] - q[0] + q[1], q[4] - q[5], q[5] + q[3]]),
+    ])
+    def test_holonomic_constraints(self, _filled_system_setup, args, kwargs,
+                                   exp_con):
+        exclude = ('holonomic_constraints', 'velocity_constraints')
+        exp_vel_con = [c.diff(t) for c in exp_con] + self.nhc
+        # Test add_holonomic_constraints
+        self.system.add_holonomic_constraints(*args, **kwargs)
+        self._filled_system_check(exclude=exclude)
+        assert self.system.holonomic_constraints[:] == exp_con
+        assert self.system.velocity_constraints[:] == exp_vel_con
+        # Test setter for holonomic_constraints
+        self.system.holonomic_constraints = exp_con
+        self._filled_system_check(exclude=exclude)
+        assert self.system.holonomic_constraints[:] == exp_con
+        assert self.system.velocity_constraints[:] == exp_vel_con
+
+    @pytest.mark.parametrize('args, kwargs', [
+        ((q[2] - q[0] + q[1], q[4] - q[3]), {}),
+        ((-(q[2] - q[0] + q[1]), q[4] - q[3]), {}),
+        ((q[0] - q[0], q[4] - q[3]), {}),
+    ])
+    def test_holonomic_constraints_invalid(self, _filled_system_setup, args,
+                                           kwargs):
+        with pytest.raises(ValueError):
+            self.system.add_holonomic_constraints(*args, **kwargs)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('args, kwargs, exp_con', [
+        ((), {}, [u[3] - qd[1] + u[2]]),
+        ((u[4] - u[5], u[5] + u[3]), {},
+         [u[3] - qd[1] + u[2], u[4] - u[5], u[5] + u[3]]),
+    ])
+    def test_nonholonomic_constraints(self, _filled_system_setup, args, kwargs,
+                                      exp_con):
+        exclude = ('nonholonomic_constraints', 'velocity_constraints')
+        exp_vel_con = self.vc[:len(self.hc)] + exp_con
+        # Test add_nonholonomic_constraints
+        self.system.add_nonholonomic_constraints(*args, **kwargs)
+        self._filled_system_check(exclude=exclude)
+        assert self.system.nonholonomic_constraints[:] == exp_con
+        assert self.system.velocity_constraints[:] == exp_vel_con
+        # Test setter for nonholonomic_constraints
+        self.system.nonholonomic_constraints = exp_con
+        self._filled_system_check(exclude=exclude)
+        assert self.system.nonholonomic_constraints[:] == exp_con
+        assert self.system.velocity_constraints[:] == exp_vel_con
+
+    @pytest.mark.parametrize('args, kwargs', [
+        ((u[3] - qd[1] + u[2], u[4] - u[3]), {}),
+        ((-(u[3] - qd[1] + u[2]), u[4] - u[3]), {}),
+        ((u[0] - u[0], u[4] - u[3]), {}),
+        (([u[0] - u[0], u[4] - u[3]]), {}),
+    ])
+    def test_nonholonomic_constraints_invalid(self, _filled_system_setup, args,
+                                              kwargs):
+        with pytest.raises(ValueError):
+            self.system.add_nonholonomic_constraints(*args, **kwargs)
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('constraints, expected', [
+        ([], []),
+        (qd[2] - qd[0] + qd[1], [qd[2] - qd[0] + qd[1]]),
+        ([qd[2] + qd[1], u[2] - u[1]], [qd[2] + qd[1], u[2] - u[1]]),
+    ])
+    def test_velocity_constraints_overwrite(self, _filled_system_setup,
+                                            constraints, expected):
+        self.system.velocity_constraints = constraints
+        self._filled_system_check(exclude=('velocity_constraints',))
+        assert self.system.velocity_constraints[:] == expected
+
+    def test_velocity_constraints_back_to_auto(self, _filled_system_setup):
+        self.system.velocity_constraints = qd[3] - qd[2]
+        self._filled_system_check(exclude=('velocity_constraints',))
+        assert self.system.velocity_constraints[:] == [qd[3] - qd[2]]
+        self.system.velocity_constraints = None
+        self._filled_system_check()
+
+    def test_bodies(self, _filled_system_setup):
+        rb1, rb2 = RigidBody('rb1'), RigidBody('rb2')
+        p1, p2 = Particle('p1'), Particle('p2')
+        self.system.add_bodies(rb1, p1)
+        assert self.system.bodies == (*self.bodies, rb1, p1)
+        self.system.add_bodies(p2)
+        assert self.system.bodies == (*self.bodies, rb1, p1, p2)
+        self.system.bodies = []
+        assert self.system.bodies == ()
+        self.system.bodies = p2
+        assert self.system.bodies == (p2,)
+        symb = symbols('symb')
+        pytest.raises(TypeError, lambda: self.system.add_bodies(symb))
+        pytest.raises(ValueError, lambda: self.system.add_bodies(p2))
+        with pytest.raises(TypeError):
+            self.system.bodies = (rb1, rb2, p1, p2, symb)
+        assert self.system.bodies == (p2,)
+
+    def test_add_loads(self):
+        system = System()
+        N, A = ReferenceFrame('N'), ReferenceFrame('A')
+        rb1 = RigidBody('rb1', frame=N)
+        mc1 = Point('mc1')
+        p1 = Particle('p1', mc1)
+        system.add_loads(Torque(rb1, N.x), (mc1, A.x), Force(p1, A.x))
+        assert system.loads == ((N, N.x), (mc1, A.x), (mc1, A.x))
+        system.loads = [(A, A.x)]
+        assert system.loads == ((A, A.x),)
+        pytest.raises(ValueError, lambda: system.add_loads((N, N.x, N.y)))
+        with pytest.raises(TypeError):
+            system.loads = (N, N.x)
+        assert system.loads == ((A, A.x),)
+
+    def test_add_actuators(self):
+        system = System()
+        N, A = ReferenceFrame('N'), ReferenceFrame('A')
+        act1 = TorqueActuator(symbols('T1'), N.x, N)
+        act2 = TorqueActuator(symbols('T2'), N.y, N, A)
+        system.add_actuators(act1)
+        assert system.actuators == (act1,)
+        assert system.loads == ()
+        system.actuators = (act2,)
+        assert system.actuators == (act2,)
+
+    def test_add_joints(self):
+        q1, q2, q3, q4, u1, u2, u3 = dynamicsymbols('q1:5 u1:4')
+        rb1, rb2, rb3, rb4, rb5 = symbols('rb1:6', cls=RigidBody)
+        J1 = PinJoint('J1', rb1, rb2, q1, u1)
+        J2 = PrismaticJoint('J2', rb2, rb3, q2, u2)
+        J3 = PinJoint('J3', rb3, rb4, q3, u3)
+        J_lag = PinJoint('J_lag', rb4, rb5, q4, q4.diff(t))
+        system = System()
+        system.add_joints(J1)
+        assert system.joints == (J1,)
+        assert system.bodies == (rb1, rb2)
+        assert system.q_ind == ImmutableMatrix([q1])
+        assert system.u_ind == ImmutableMatrix([u1])
+        assert system.kdes == ImmutableMatrix([u1 - q1.diff(t)])
+        system.add_bodies(rb4)
+        system.add_coordinates(q3)
+        system.add_kdes(u3 - q3.diff(t))
+        system.add_joints(J3)
+        assert system.joints == (J1, J3)
+        assert system.bodies == (rb1, rb2, rb4, rb3)
+        assert system.q_ind == ImmutableMatrix([q1, q3])
+        assert system.u_ind == ImmutableMatrix([u1, u3])
+        assert system.kdes == ImmutableMatrix(
+            [u1 - q1.diff(t), u3 - q3.diff(t)])
+        system.add_kdes(-(u2 - q2.diff(t)))
+        system.add_joints(J2)
+        assert system.joints == (J1, J3, J2)
+        assert system.bodies == (rb1, rb2, rb4, rb3)
+        assert system.q_ind == ImmutableMatrix([q1, q3, q2])
+        assert system.u_ind == ImmutableMatrix([u1, u3, u2])
+        assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t),
+                                               -(u2 - q2.diff(t))])
+        system.add_joints(J_lag)
+        assert system.joints == (J1, J3, J2, J_lag)
+        assert system.bodies == (rb1, rb2, rb4, rb3, rb5)
+        assert system.q_ind == ImmutableMatrix([q1, q3, q2, q4])
+        assert system.u_ind == ImmutableMatrix([u1, u3, u2, q4.diff(t)])
+        assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t),
+                                               -(u2 - q2.diff(t))])
+        assert system.q_dep[:] == []
+        assert system.u_dep[:] == []
+        pytest.raises(ValueError, lambda: system.add_joints(J2))
+        pytest.raises(TypeError, lambda: system.add_joints(rb1))
+
+    def test_joints_setter(self, _filled_system_setup):
+        self.system.joints = self.joints[1:]
+        assert self.system.joints == self.joints[1:]
+        self._filled_system_check(exclude=('joints',))
+        self.system.q_ind = ()
+        self.system.u_ind = ()
+        self.system.joints = self.joints
+        self._filled_system_check()
+
+    @pytest.mark.parametrize('name, joint_index', [
+        ('J1', 0),
+        ('J2', 1),
+        ('not_existing', None),
+    ])
+    def test_get_joint(self, _filled_system_setup, name, joint_index):
+        joint = self.system.get_joint(name)
+        if joint_index is None:
+            assert joint is None
+        else:
+            assert joint == self.joints[joint_index]
+
+    @pytest.mark.parametrize('name, body_index', [
+        ('rb1', 0),
+        ('rb3', 2),
+        ('not_existing', None),
+    ])
+    def test_get_body(self, _filled_system_setup, name, body_index):
+        body = self.system.get_body(name)
+        if body_index is None:
+            assert body is None
+        else:
+            assert body == self.bodies[body_index]
+
+    @pytest.mark.parametrize('eom_method', [KanesMethod, LagrangesMethod])
+    def test_form_eoms_calls_subclass(self, _moving_point_mass, eom_method):
+        class MyMethod(eom_method):
+            pass
+
+        self.system.form_eoms(eom_method=MyMethod)
+        assert isinstance(self.system.eom_method, MyMethod)
+
+    @pytest.mark.parametrize('kwargs, expected', [
+        ({}, ImmutableMatrix([[-1, 0], [0, symbols('m')]])),
+        ({'explicit_kinematics': True}, ImmutableMatrix([[1, 0],
+                                                         [0, symbols('m')]])),
+    ])
+    def test_system_kane_form_eoms_kwargs(self, _moving_point_mass, kwargs,
+                                          expected):
+        self.system.form_eoms(**kwargs)
+        assert self.system.mass_matrix_full == expected
+
+    @pytest.mark.parametrize('kwargs, mm, gm', [
+        ({}, ImmutableMatrix([[1, 0], [0, symbols('m')]]),
+         ImmutableMatrix([q[0].diff(t), 0])),
+    ])
+    def test_system_lagrange_form_eoms_kwargs(self, _moving_point_mass, kwargs,
+                                              mm, gm):
+        self.system.form_eoms(eom_method=LagrangesMethod, **kwargs)
+        assert self.system.mass_matrix_full == mm
+        assert self.system.forcing_full == gm
+
+    @pytest.mark.parametrize('eom_method, kwargs, error', [
+        (KanesMethod, {'non_existing_kwarg': 1}, TypeError),
+        (LagrangesMethod, {'non_existing_kwarg': 1}, TypeError),
+        (KanesMethod, {'bodies': []}, ValueError),
+        (KanesMethod, {'kd_eqs': []}, ValueError),
+        (LagrangesMethod, {'bodies': []}, ValueError),
+        (LagrangesMethod, {'Lagrangian': 1}, ValueError),
+    ])
+    def test_form_eoms_kwargs_errors(self, _empty_system_setup, eom_method,
+                                     kwargs, error):
+        self.system.q_ind = q[0]
+        p = Particle('p', mass=symbols('m'))
+        self.system.add_bodies(p)
+        p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x)
+        with pytest.raises(error):
+            self.system.form_eoms(eom_method=eom_method, **kwargs)
+
+
+class TestValidateSystem(TestSystemBase):
+    @pytest.mark.parametrize('valid_method, invalid_method, with_speeds', [
+        (KanesMethod, LagrangesMethod, True),
+        (LagrangesMethod, KanesMethod, False)
+    ])
+    def test_only_valid(self, valid_method, invalid_method, with_speeds):
+        self._create_filled_system(with_speeds=with_speeds)
+        self.system.validate_system(valid_method)
+        # Test Lagrange should fail due to the usage of generalized speeds
+        with pytest.raises(ValueError):
+            self.system.validate_system(invalid_method)
+
+    @pytest.mark.parametrize('method, with_speeds', [
+        (KanesMethod, True), (LagrangesMethod, False)])
+    def test_missing_joint_coordinate(self, method, with_speeds):
+        self._create_filled_system(with_speeds=with_speeds)
+        self.system.q_ind = self.q_ind[1:]
+        self.system.u_ind = self.u_ind[:-1]
+        self.system.kdes = self.kdes[:-1]
+        pytest.raises(ValueError, lambda: self.system.validate_system(method))
+
+    def test_missing_joint_speed(self, _filled_system_setup):
+        self.system.q_ind = self.q_ind[:-1]
+        self.system.u_ind = self.u_ind[1:]
+        self.system.kdes = self.kdes[:-1]
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+
+    def test_missing_joint_kdes(self, _filled_system_setup):
+        self.system.kdes = self.kdes[1:]
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+
+    def test_negative_joint_kdes(self, _filled_system_setup):
+        self.system.kdes = [-self.kdes[0]] + self.kdes[1:]
+        self.system.validate_system()
+
+    @pytest.mark.parametrize('method, with_speeds', [
+        (KanesMethod, True), (LagrangesMethod, False)])
+    def test_missing_holonomic_constraint(self, method, with_speeds):
+        self._create_filled_system(with_speeds=with_speeds)
+        self.system.holonomic_constraints = []
+        self.system.nonholonomic_constraints = self.nhc + [
+            self.u_ind[1] - self.u_dep[0] + self.u_ind[0]]
+        pytest.raises(ValueError, lambda: self.system.validate_system(method))
+        self.system.q_dep = []
+        self.system.q_ind = self.q_ind + self.q_dep
+        self.system.validate_system(method)
+
+    def test_missing_nonholonomic_constraint(self, _filled_system_setup):
+        self.system.nonholonomic_constraints = []
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+        self.system.u_dep = self.u_dep[1]
+        self.system.u_ind = self.u_ind + [self.u_dep[0]]
+        self.system.validate_system()
+
+    def test_number_of_coordinates_speeds(self, _filled_system_setup):
+        # Test more speeds than coordinates
+        self.system.u_ind = self.u_ind + [u[5]]
+        self.system.kdes = self.kdes + [u[5] - qd[5]]
+        self.system.validate_system()
+        # Test more coordinates than speeds
+        self.system.q_ind = self.q_ind
+        self.system.u_ind = self.u_ind[:-1]
+        self.system.kdes = self.kdes[:-1]
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+
+    def test_number_of_kdes(self, _filled_system_setup):
+        # Test wrong number of kdes
+        self.system.kdes = self.kdes[:-1]
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+        self.system.kdes = self.kdes + [u[2] + u[1] - qd[2]]
+        pytest.raises(ValueError, lambda: self.system.validate_system())
+
+    def test_duplicates(self, _filled_system_setup):
+        # This is basically a redundant feature, which should never fail
+        self.system.validate_system(check_duplicates=True)
+
+    def test_speeds_in_lagrange(self, _filled_system_setup_no_speeds):
+        self.system.u_ind = u[:len(self.u_ind)]
+        with pytest.raises(ValueError):
+            self.system.validate_system(LagrangesMethod)
+        self.system.u_ind = []
+        self.system.validate_system(LagrangesMethod)
+        self.system.u_aux = ua
+        with pytest.raises(ValueError):
+            self.system.validate_system(LagrangesMethod)
+        self.system.u_aux = []
+        self.system.validate_system(LagrangesMethod)
+        self.system.add_joints(
+            PinJoint('Ju', RigidBody('rbu1'), RigidBody('rbu2')))
+        self.system.u_ind = []
+        with pytest.raises(ValueError):
+            self.system.validate_system(LagrangesMethod)
+
+
+class TestSystemExamples:
+    def test_cart_pendulum_kanes(self):
+        # This example is the same as in the top documentation of System
+        # Added a spring to the cart
+        g, l, mc, mp, k = symbols('g l mc mp k')
+        F, qp, qc, up, uc = dynamicsymbols('F qp qc up uc')
+        rail = RigidBody('rail')
+        cart = RigidBody('cart', mass=mc)
+        bob = Particle('bob', mass=mp)
+        bob_frame = ReferenceFrame('bob_frame')
+        system = System.from_newtonian(rail)
+        assert system.bodies == (rail,)
+        assert system.frame == rail.frame
+        assert system.fixed_point == rail.masscenter
+        slider = PrismaticJoint('slider', rail, cart, qc, uc, joint_axis=rail.x)
+        pin = PinJoint('pin', cart, bob, qp, up, joint_axis=cart.z,
+                       child_interframe=bob_frame, child_point=l * bob_frame.y)
+        system.add_joints(slider, pin)
+        assert system.joints == (slider, pin)
+        assert system.get_joint('slider') == slider
+        assert system.get_body('bob') == bob
+        system.apply_uniform_gravity(-g * system.y)
+        system.add_loads((cart.masscenter, F * rail.x))
+        system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart))
+        system.validate_system()
+        system.form_eoms()
+        assert isinstance(system.eom_method, KanesMethod)
+        assert (simplify(system.mass_matrix - ImmutableMatrix(
+            [[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]]))
+                == zeros(2, 2))
+        assert (simplify(system.forcing - ImmutableMatrix([
+            [mp * l * up ** 2 * sin(qp) + F],
+            [-mp * g * l * sin(qp) + k * qp]])) == zeros(2, 1))
+
+        system.add_holonomic_constraints(
+            sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x)))
+        assert system.eom_method is None
+        system.q_ind, system.q_dep = qp, qc
+        system.u_ind, system.u_dep = up, uc
+        system.validate_system()
+
+        # Computed solution based on manually solving the constraints
+        subs = {qc: -l * sin(qp),
+                uc: -l * cos(qp) * up,
+                uc.diff(t): l * (up ** 2 * sin(qp) - up.diff(t) * cos(qp))}
+        upd_expected = (
+            (-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * up ** 2 / 2
+             - l * mp * sin(2 * qp) * up ** 2 / 2 - F * cos(qp)) /
+            (l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2)))
+        upd_sol = tuple(solve(system.form_eoms().xreplace(subs),
+                              up.diff(t)).values())[0]
+        assert simplify(upd_sol - upd_expected) == 0
+        assert isinstance(system.eom_method, KanesMethod)
+
+        # Test other output
+        Mk = -ImmutableMatrix([[0, 1], [1, 0]])
+        gk = -ImmutableMatrix([uc, up])
+        Md = ImmutableMatrix([[-l ** 2 * mp * cos(qp) ** 2 + l ** 2 * mp,
+                               l * mp * cos(qp) - l * (mc + mp) * cos(qp)],
+                              [l * cos(qp), 1]])
+        gd = ImmutableMatrix(
+            [[-g * l * mp * sin(qp) + k * qp - l ** 2 * mp * up ** 2 * sin(qp) *
+              cos(qp) - l * F * cos(qp)], [l * up ** 2 * sin(qp)]])
+        Mm = (Mk.row_join(zeros(2, 2))).col_join(zeros(2, 2).row_join(Md))
+        gm = gk.col_join(gd)
+        assert simplify(system.mass_matrix - Md) == zeros(2, 2)
+        assert simplify(system.forcing - gd) == zeros(2, 1)
+        assert simplify(system.mass_matrix_full - Mm) == zeros(4, 4)
+        assert simplify(system.forcing_full - gm) == zeros(4, 1)
+
+    def test_cart_pendulum_lagrange(self):
+        # Lagrange version of test_cart_pendulus_kanes
+        # Added a spring to the cart
+        g, l, mc, mp, k = symbols('g l mc mp k')
+        F, qp, qc = dynamicsymbols('F qp qc')
+        qpd, qcd = dynamicsymbols('qp qc', 1)
+        rail = RigidBody('rail')
+        cart = RigidBody('cart', mass=mc)
+        bob = Particle('bob', mass=mp)
+        bob_frame = ReferenceFrame('bob_frame')
+        system = System.from_newtonian(rail)
+        assert system.bodies == (rail,)
+        assert system.frame == rail.frame
+        assert system.fixed_point == rail.masscenter
+        slider = PrismaticJoint('slider', rail, cart, qc, qcd,
+                                joint_axis=rail.x)
+        pin = PinJoint('pin', cart, bob, qp, qpd, joint_axis=cart.z,
+                       child_interframe=bob_frame, child_point=l * bob_frame.y)
+        system.add_joints(slider, pin)
+        assert system.joints == (slider, pin)
+        assert system.get_joint('slider') == slider
+        assert system.get_body('bob') == bob
+        for body in system.bodies:
+            body.potential_energy = body.mass * g * body.masscenter.pos_from(
+                system.fixed_point).dot(system.y)
+        system.add_loads((cart.masscenter, F * rail.x))
+        system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart))
+        system.validate_system(LagrangesMethod)
+        system.form_eoms(LagrangesMethod)
+        assert (simplify(system.mass_matrix - ImmutableMatrix(
+            [[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]]))
+                == zeros(2, 2))
+        assert (simplify(system.forcing - ImmutableMatrix([
+            [mp * l * qpd ** 2 * sin(qp) + F], [-mp * g * l * sin(qp) + k * qp]]
+        )) == zeros(2, 1))
+
+        system.add_holonomic_constraints(
+            sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x)))
+        assert system.eom_method is None
+        system.q_ind, system.q_dep = qp, qc
+
+        # Computed solution based on manually solving the constraints
+        subs = {qc: -l * sin(qp),
+                qcd: -l * cos(qp) * qpd,
+                qcd.diff(t): l * (qpd ** 2 * sin(qp) - qpd.diff(t) * cos(qp))}
+        qpdd_expected = (
+            (-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * qpd ** 2 /
+             2 - l * mp * sin(2 * qp) * qpd ** 2 / 2 - F * cos(qp)) /
+            (l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2)))
+        eoms = system.form_eoms(LagrangesMethod)
+        lam1 = system.eom_method.lam_vec[0]
+        lam1_sol = system.eom_method.solve_multipliers()[lam1]
+        qpdd_sol = solve(eoms[0].xreplace({lam1: lam1_sol}).xreplace(subs),
+                         qpd.diff(t))[0]
+        assert simplify(qpdd_sol - qpdd_expected) == 0
+        assert isinstance(system.eom_method, LagrangesMethod)
+
+        # Test other output
+        Md = ImmutableMatrix([[l ** 2 * mp, l * mp * cos(qp), -l * cos(qp)],
+                              [l * mp * cos(qp), mc + mp, -1]])
+        gd = ImmutableMatrix(
+            [[-g * l * mp * sin(qp) + k * qp],
+             [l * mp * sin(qp) * qpd ** 2 + F]])
+        Mm = (eye(2).row_join(zeros(2, 3))).col_join(zeros(3, 2).row_join(
+            Md.col_join(ImmutableMatrix([l * cos(qp), 1, 0]).T)))
+        gm = ImmutableMatrix([qpd, qcd] + gd[:] + [l * sin(qp) * qpd ** 2])
+        assert simplify(system.mass_matrix - Md) == zeros(2, 3)
+        assert simplify(system.forcing - gd) == zeros(2, 1)
+        assert simplify(system.mass_matrix_full - Mm) == zeros(5, 5)
+        assert simplify(system.forcing_full - gm) == zeros(5, 1)
+
+    def test_box_on_ground(self):
+        # Particle sliding on ground with friction. The applied force is assumed
+        # to be positive and to be higher than the friction force.
+        g, m, mu = symbols('g m mu')
+        q, u, ua = dynamicsymbols('q u ua')
+        N, F = dynamicsymbols('N F', positive=True)
+        P = Particle("P", mass=m)
+        system = System()
+        system.add_bodies(P)
+        P.masscenter.set_pos(system.fixed_point, q * system.x)
+        P.masscenter.set_vel(system.frame, u * system.x + ua * system.y)
+        system.q_ind, system.u_ind, system.u_aux = [q], [u], [ua]
+        system.kdes = [q.diff(t) - u]
+        system.apply_uniform_gravity(-g * system.y)
+        system.add_loads(
+            Force(P, N * system.y),
+            Force(P, F * system.x - mu * N * system.x))
+        system.validate_system()
+        system.form_eoms()
+
+        # Test other output
+        Mk = ImmutableMatrix([1])
+        gk = ImmutableMatrix([u])
+        Md = ImmutableMatrix([m])
+        gd = ImmutableMatrix([F - mu * N])
+        Mm = (Mk.row_join(zeros(1, 1))).col_join(zeros(1, 1).row_join(Md))
+        gm = gk.col_join(gd)
+        aux_eqs = ImmutableMatrix([N - m * g])
+        assert simplify(system.mass_matrix - Md) == zeros(1, 1)
+        assert simplify(system.forcing - gd) == zeros(1, 1)
+        assert simplify(system.mass_matrix_full - Mm) == zeros(2, 2)
+        assert simplify(system.forcing_full - gm) == zeros(2, 1)
+        assert simplify(system.eom_method.auxiliary_eqs - aux_eqs
+                        ) == zeros(1, 1)

lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_wrapping_geometry.py

@@ -0,0 +1,363 @@
+"""Tests for the ``sympy.physics.mechanics.wrapping_geometry.py`` module."""
+
+import pytest
+
+from sympy import (
+    Integer,
+    Rational,
+    S,
+    Symbol,
+    acos,
+    cos,
+    pi,
+    sin,
+    sqrt,
+)
+from sympy.core.relational import Eq
+from sympy.physics.mechanics import (
+    Point,
+    ReferenceFrame,
+    WrappingCylinder,
+    WrappingSphere,
+    dynamicsymbols,
+)
+from sympy.simplify.simplify import simplify
+
+
+r = Symbol('r', positive=True)
+x = Symbol('x')
+q = dynamicsymbols('q')
+N = ReferenceFrame('N')
+
+
+class TestWrappingSphere:
+
+    @staticmethod
+    def test_valid_constructor():
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+        assert isinstance(sphere, WrappingSphere)
+        assert hasattr(sphere, 'radius')
+        assert sphere.radius == r
+        assert hasattr(sphere, 'point')
+        assert sphere.point == pO
+
+    @staticmethod
+    @pytest.mark.parametrize('position', [S.Zero, Integer(2)*r*N.x])
+    def test_geodesic_length_point_not_on_surface_invalid(position):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position)
+        p2 = Point('p2')
+        p2.set_pos(pO, position)
+
+        error_msg = r'point .* does not lie on the surface of'
+        with pytest.raises(ValueError, match=error_msg):
+            sphere.geodesic_length(p1, p2)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'position_1, position_2, expected',
+        [
+            (r*N.x, r*N.x, S.Zero),
+            (r*N.x, r*N.y, S.Half*pi*r),
+            (r*N.x, r*-N.x, pi*r),
+            (r*-N.x, r*N.x, pi*r),
+            (r*N.x, r*sqrt(2)*S.Half*(N.x + N.y), Rational(1, 4)*pi*r),
+            (
+                r*sqrt(2)*S.Half*(N.x + N.y),
+                r*sqrt(3)*Rational(1, 3)*(N.x + N.y + N.z),
+                r*acos(sqrt(6)*Rational(1, 3)),
+            ),
+        ]
+    )
+    def test_geodesic_length(position_1, position_2, expected):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position_1)
+        p2 = Point('p2')
+        p2.set_pos(pO, position_2)
+
+        assert simplify(Eq(sphere.geodesic_length(p1, p2), expected))
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'position_1, position_2, vector_1, vector_2',
+        [
+            (r * N.x, r * N.y, N.y, N.x),
+            (r * N.x, -r * N.y, -N.y, N.x),
+            (
+                r * N.y,
+                sqrt(2)/2 * r * N.x - sqrt(2)/2 * r * N.y,
+                N.x,
+                sqrt(2)/2 * N.x + sqrt(2)/2 * N.y,
+            ),
+            (
+                r * N.x,
+                r / 2 * N.x + sqrt(3)/2 * r * N.y,
+                N.y,
+                sqrt(3)/2 * N.x - 1/2 * N.y,
+            ),
+            (
+                r * N.x,
+                sqrt(2)/2 * r * N.x + sqrt(2)/2 * r * N.y,
+                N.y,
+                sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
+            ),
+        ]
+    )
+    def test_geodesic_end_vectors(position_1, position_2, vector_1, vector_2):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position_1)
+        p2 = Point('p2')
+        p2.set_pos(pO, position_2)
+
+        expected = (vector_1, vector_2)
+
+        assert sphere.geodesic_end_vectors(p1, p2) == expected
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'position',
+        [r * N.x, r * cos(q) * N.x + r * sin(q) * N.y]
+    )
+    def test_geodesic_end_vectors_invalid_coincident(position):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position)
+        p2 = Point('p2')
+        p2.set_pos(pO, position)
+
+        with pytest.raises(ValueError):
+            _ = sphere.geodesic_end_vectors(p1, p2)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'position_1, position_2',
+        [
+            (r * N.x, -r * N.x),
+            (-r * N.y, r * N.y),
+            (
+                r * cos(q) * N.x + r * sin(q) * N.y,
+                -r * cos(q) * N.x - r * sin(q) * N.y,
+            )
+        ]
+    )
+    def test_geodesic_end_vectors_invalid_diametrically_opposite(
+        position_1,
+        position_2,
+    ):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        sphere = WrappingSphere(r, pO)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position_1)
+        p2 = Point('p2')
+        p2.set_pos(pO, position_2)
+
+        with pytest.raises(ValueError):
+            _ = sphere.geodesic_end_vectors(p1, p2)
+
+
+class TestWrappingCylinder:
+
+    @staticmethod
+    def test_valid_constructor():
+        N = ReferenceFrame('N')
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, N.x)
+        assert isinstance(cylinder, WrappingCylinder)
+        assert hasattr(cylinder, 'radius')
+        assert cylinder.radius == r
+        assert hasattr(cylinder, 'point')
+        assert cylinder.point == pO
+        assert hasattr(cylinder, 'axis')
+        assert cylinder.axis == N.x
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'position, expected',
+        [
+            (S.Zero, False),
+            (r*N.y, True),
+            (r*N.z, True),
+            (r*(N.y + N.z).normalize(), True),
+            (Integer(2)*r*N.y, False),
+            (r*(N.x + N.y), True),
+            (r*(Integer(2)*N.x + N.y), True),
+            (Integer(2)*N.x + r*(Integer(2)*N.y + N.z).normalize(), True),
+            (r*(cos(q)*N.y + sin(q)*N.z), True)
+        ]
+    )
+    def test_point_is_on_surface(position, expected):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, N.x)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position)
+
+        assert cylinder.point_on_surface(p1) is expected
+
+    @staticmethod
+    @pytest.mark.parametrize('position', [S.Zero, Integer(2)*r*N.y])
+    def test_geodesic_length_point_not_on_surface_invalid(position):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, N.x)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position)
+        p2 = Point('p2')
+        p2.set_pos(pO, position)
+
+        error_msg = r'point .* does not lie on the surface of'
+        with pytest.raises(ValueError, match=error_msg):
+            cylinder.geodesic_length(p1, p2)
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'axis, position_1, position_2, expected',
+        [
+            (N.x, r*N.y, r*N.y, S.Zero),
+            (N.x, r*N.y, N.x + r*N.y, S.One),
+            (N.x, r*N.y, -x*N.x + r*N.y, sqrt(x**2)),
+            (-N.x, r*N.y, x*N.x + r*N.y, sqrt(x**2)),
+            (N.x, r*N.y, r*N.z, S.Half*pi*sqrt(r**2)),
+            (-N.x, r*N.y, r*N.z, Integer(3)*S.Half*pi*sqrt(r**2)),
+            (N.x, r*N.z, r*N.y, Integer(3)*S.Half*pi*sqrt(r**2)),
+            (-N.x, r*N.z, r*N.y, S.Half*pi*sqrt(r**2)),
+            (N.x, r*N.y, r*(cos(q)*N.y + sin(q)*N.z), sqrt(r**2*q**2)),
+            (
+                -N.x, r*N.y,
+                r*(cos(q)*N.y + sin(q)*N.z),
+                sqrt(r**2*(Integer(2)*pi - q)**2),
+            ),
+        ]
+    )
+    def test_geodesic_length(axis, position_1, position_2, expected):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, axis)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position_1)
+        p2 = Point('p2')
+        p2.set_pos(pO, position_2)
+
+        assert simplify(Eq(cylinder.geodesic_length(p1, p2), expected))
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'axis, position_1, position_2, vector_1, vector_2',
+        [
+            (N.z, r * N.x, r * N.y, N.y, N.x),
+            (N.z, r * N.x, -r * N.x, N.y, N.y),
+            (N.z, -r * N.x, r * N.x, -N.y, -N.y),
+            (-N.z, r * N.x, -r * N.x, -N.y, -N.y),
+            (-N.z, -r * N.x, r * N.x, N.y, N.y),
+            (N.z, r * N.x, -r * N.y, N.y, -N.x),
+            (
+                N.z,
+                r * N.y,
+                sqrt(2)/2 * r * N.x - sqrt(2)/2 * r * N.y,
+                - N.x,
+                - sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
+            ),
+            (
+                N.z,
+                r * N.x,
+                r / 2 * N.x + sqrt(3)/2 * r * N.y,
+                N.y,
+                sqrt(3)/2 * N.x - 1/2 * N.y,
+            ),
+            (
+                N.z,
+                r * N.x,
+                sqrt(2)/2 * r * N.x + sqrt(2)/2 * r * N.y,
+                N.y,
+                sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
+            ),
+            (
+                N.z,
+                r * N.x,
+                r * N.x + N.z,
+                N.z,
+                -N.z,
+            ),
+            (
+                N.z,
+                r * N.x,
+                r * N.y + pi/2 * r * N.z,
+                sqrt(2)/2 * N.y + sqrt(2)/2 * N.z,
+                sqrt(2)/2 * N.x - sqrt(2)/2 * N.z,
+            ),
+            (
+                N.z,
+                r * N.x,
+                r * cos(q) * N.x + r * sin(q) * N.y,
+                N.y,
+                sin(q) * N.x - cos(q) * N.y,
+            ),
+        ]
+    )
+    def test_geodesic_end_vectors(
+        axis,
+        position_1,
+        position_2,
+        vector_1,
+        vector_2,
+    ):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, axis)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position_1)
+        p2 = Point('p2')
+        p2.set_pos(pO, position_2)
+
+        expected = (vector_1, vector_2)
+        end_vectors = tuple(
+            end_vector.simplify()
+            for end_vector in cylinder.geodesic_end_vectors(p1, p2)
+        )
+
+        assert end_vectors == expected
+
+    @staticmethod
+    @pytest.mark.parametrize(
+        'axis, position',
+        [
+            (N.z, r * N.x),
+            (N.z, r * cos(q) * N.x + r * sin(q) * N.y + N.z),
+        ]
+    )
+    def test_geodesic_end_vectors_invalid_coincident(axis, position):
+        r = Symbol('r', positive=True)
+        pO = Point('pO')
+        cylinder = WrappingCylinder(r, pO, axis)
+
+        p1 = Point('p1')
+        p1.set_pos(pO, position)
+        p2 = Point('p2')
+        p2.set_pos(pO, position)
+
+        with pytest.raises(ValueError):
+            _ = cylinder.geodesic_end_vectors(p1, p2)