I am done

rl/Lib/site-packages/sympy/physics/mechanics/__init__.py

@@ -0,0 +1,90 @@
+__all__ = [
+    'vector',
+
+    'CoordinateSym', 'ReferenceFrame', 'Dyadic', 'Vector', 'Point', 'cross',
+    'dot', 'express', 'time_derivative', 'outer', 'kinematic_equations',
+    'get_motion_params', 'partial_velocity', 'dynamicsymbols', 'vprint',
+    'vsstrrepr', 'vsprint', 'vpprint', 'vlatex', 'init_vprinting', 'curl',
+    'divergence', 'gradient', 'is_conservative', 'is_solenoidal',
+    'scalar_potential', 'scalar_potential_difference',
+
+    'KanesMethod',
+
+    'RigidBody',
+
+    'linear_momentum', 'angular_momentum', 'kinetic_energy', 'potential_energy',
+    'Lagrangian', 'mechanics_printing', 'mprint', 'msprint', 'mpprint',
+    'mlatex', 'msubs', 'find_dynamicsymbols',
+
+    'inertia', 'inertia_of_point_mass', 'Inertia',
+
+    'Force', 'Torque',
+
+    'Particle',
+
+    'LagrangesMethod',
+
+    'Linearizer',
+
+    'Body',
+
+    'SymbolicSystem', 'System',
+
+    'PinJoint', 'PrismaticJoint', 'CylindricalJoint', 'PlanarJoint',
+    'SphericalJoint', 'WeldJoint',
+
+    'JointsMethod',
+
+    'WrappingCylinder', 'WrappingGeometryBase', 'WrappingSphere',
+
+    'PathwayBase', 'LinearPathway', 'ObstacleSetPathway', 'WrappingPathway',
+
+    'ActuatorBase', 'ForceActuator', 'LinearDamper', 'LinearSpring',
+    'TorqueActuator', 'DuffingSpring'
+]
+
+from sympy.physics import vector
+
+from sympy.physics.vector import (CoordinateSym, ReferenceFrame, Dyadic, Vector, Point,
+        cross, dot, express, time_derivative, outer, kinematic_equations,
+        get_motion_params, partial_velocity, dynamicsymbols, vprint,
+        vsstrrepr, vsprint, vpprint, vlatex, init_vprinting, curl, divergence,
+        gradient, is_conservative, is_solenoidal, scalar_potential,
+        scalar_potential_difference)
+
+from .kane import KanesMethod
+
+from .rigidbody import RigidBody
+
+from .functions import (linear_momentum, angular_momentum, kinetic_energy,
+                        potential_energy, Lagrangian, mechanics_printing,
+                        mprint, msprint, mpprint, mlatex, msubs,
+                        find_dynamicsymbols)
+
+from .inertia import inertia, inertia_of_point_mass, Inertia
+
+from .loads import Force, Torque
+
+from .particle import Particle
+
+from .lagrange import LagrangesMethod
+
+from .linearize import Linearizer
+
+from .body import Body
+
+from .system import SymbolicSystem, System
+
+from .jointsmethod import JointsMethod
+
+from .joint import (PinJoint, PrismaticJoint, CylindricalJoint, PlanarJoint,
+                    SphericalJoint, WeldJoint)
+
+from .wrapping_geometry import (WrappingCylinder, WrappingGeometryBase,
+                                WrappingSphere)
+
+from .pathway import (PathwayBase, LinearPathway, ObstacleSetPathway,
+                      WrappingPathway)
+
+from .actuator import (ActuatorBase, ForceActuator, LinearDamper, LinearSpring,
+                       TorqueActuator, DuffingSpring)

rl/Lib/site-packages/sympy/physics/mechanics/actuator.py

@@ -0,0 +1,992 @@
+"""Implementations of actuators for linked force and torque application."""
+
+from abc import ABC, abstractmethod
+
+from sympy import S, sympify
+from sympy.physics.mechanics.joint import PinJoint
+from sympy.physics.mechanics.loads import Torque
+from sympy.physics.mechanics.pathway import PathwayBase
+from sympy.physics.mechanics.rigidbody import RigidBody
+from sympy.physics.vector import ReferenceFrame, Vector
+
+
+__all__ = [
+    'ActuatorBase',
+    'ForceActuator',
+    'LinearDamper',
+    'LinearSpring',
+    'TorqueActuator',
+    'DuffingSpring'
+]
+
+
+class ActuatorBase(ABC):
+    """Abstract base class for all actuator classes to inherit from.
+
+    Notes
+    =====
+
+    Instances of this class cannot be directly instantiated by users. However,
+    it can be used to created custom actuator types through subclassing.
+
+    """
+
+    def __init__(self):
+        """Initializer for ``ActuatorBase``."""
+        pass
+
+    @abstractmethod
+    def to_loads(self):
+        """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 an actuator."""
+        return f'{self.__class__.__name__}()'
+
+
+class ForceActuator(ActuatorBase):
+    """Force-producing actuator.
+
+    Explanation
+    ===========
+
+    A ``ForceActuator`` is an actuator that produces a (expansile) force along
+    its length.
+
+    A force actuator uses a pathway instance to determine the direction and
+    number of forces that it applies to a system. Consider the simplest case
+    where a ``LinearPathway`` instance is used. This pathway is made up of two
+    points that can move relative to each other, and results in a pair of equal
+    and opposite forces acting on the endpoints. 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, this is the
+    meaning of "expansile" in this context. The following diagram shows the
+    positive force sense and the distance between the points::
+
+       P           Q
+       o<--- F --->o
+       |           |
+       |<--l(t)--->|
+
+    Examples
+    ========
+
+    To construct an actuator, an expression (or symbol) must be supplied to
+    represent the force it can produce, alongside a pathway specifying its line
+    of action. Let's also create a global reference frame and spatially fix one
+    of the points in it while setting the other to be positioned such that it
+    can freely move in the frame's x direction specified by the coordinate
+    ``q``.
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import (ForceActuator, LinearPathway,
+    ...     Point, ReferenceFrame)
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> N = ReferenceFrame('N')
+    >>> q = dynamicsymbols('q')
+    >>> force = symbols('F')
+    >>> pA, pB = Point('pA'), Point('pB')
+    >>> pA.set_vel(N, 0)
+    >>> pB.set_pos(pA, q*N.x)
+    >>> pB.pos_from(pA)
+    q(t)*N.x
+    >>> linear_pathway = LinearPathway(pA, pB)
+    >>> actuator = ForceActuator(force, linear_pathway)
+    >>> actuator
+    ForceActuator(F, LinearPathway(pA, pB))
+
+    Parameters
+    ==========
+
+    force : Expr
+        The scalar expression defining the (expansile) force that the actuator
+        produces.
+    pathway : PathwayBase
+        The pathway that the actuator follows. This must be an instance of a
+        concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+
+    """
+
+    def __init__(self, force, pathway):
+        """Initializer for ``ForceActuator``.
+
+        Parameters
+        ==========
+
+        force : Expr
+            The scalar expression defining the (expansile) force that the
+            actuator produces.
+        pathway : PathwayBase
+            The pathway that the actuator follows. This must be an instance of
+            a concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+
+        """
+        self.force = force
+        self.pathway = pathway
+
+    @property
+    def force(self):
+        """The magnitude of the force produced by the actuator."""
+        return self._force
+
+    @force.setter
+    def force(self, force):
+        if hasattr(self, '_force'):
+            msg = (
+                f'Can\'t set attribute `force` to {repr(force)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        self._force = sympify(force, strict=True)
+
+    @property
+    def pathway(self):
+        """The ``Pathway`` defining the actuator's line of action."""
+        return self._pathway
+
+    @pathway.setter
+    def pathway(self, pathway):
+        if hasattr(self, '_pathway'):
+            msg = (
+                f'Can\'t set attribute `pathway` to {repr(pathway)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        if not isinstance(pathway, PathwayBase):
+            msg = (
+                f'Value {repr(pathway)} passed to `pathway` was of type '
+                f'{type(pathway)}, must be {PathwayBase}.'
+            )
+            raise TypeError(msg)
+        self._pathway = pathway
+
+    def to_loads(self):
+        """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 by a force
+        actuator that follows a linear pathway. In this example we'll assume
+        that the force actuator is being used to model a simple linear spring.
+        First, create a linear pathway 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)
+        >>> pathway = LinearPathway(pA, pB)
+
+        Now create a symbol ``k`` to describe the spring's stiffness and
+        instantiate a force actuator that produces a (contractile) force
+        proportional to both the spring's stiffness and the pathway's length.
+        Note that actuator classes use the sign convention that expansile
+        forces are positive, so for a spring to produce a contractile force the
+        spring force needs to be calculated as the negative for the stiffness
+        multiplied by the length.
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import ForceActuator
+        >>> stiffness = symbols('k')
+        >>> spring_force = -stiffness*pathway.length
+        >>> spring = ForceActuator(spring_force, pathway)
+
+        The forces produced by the spring can be generated in the list of loads
+        form that ``KanesMethod`` (and other equations of motion methods)
+        requires by calling the ``to_loads`` method.
+
+        >>> spring.to_loads()
+        [(pA, k*q(t)*N.x), (pB, - k*q(t)*N.x)]
+
+        A simple linear damper can be modeled in a similar way. Create another
+        symbol ``c`` to describe the dampers damping coefficient. This time
+        instantiate a force actuator that produces a force proportional to both
+        the damper's damping coefficient and the pathway's extension velocity.
+        Note that the damping force is negative as it acts in the opposite
+        direction to which the damper is changing in length.
+
+        >>> damping_coefficient = symbols('c')
+        >>> damping_force = -damping_coefficient*pathway.extension_velocity
+        >>> damper = ForceActuator(damping_force, pathway)
+
+        Again, the forces produces by the damper can be generated by calling
+        the ``to_loads`` method.
+
+        >>> damper.to_loads()
+        [(pA, c*Derivative(q(t), t)*N.x), (pB, - c*Derivative(q(t), t)*N.x)]
+
+        """
+        return self.pathway.to_loads(self.force)
+
+    def __repr__(self):
+        """Representation of a ``ForceActuator``."""
+        return f'{self.__class__.__name__}({self.force}, {self.pathway})'
+
+
+class LinearSpring(ForceActuator):
+    """A spring with its spring force as a linear function of its length.
+
+    Explanation
+    ===========
+
+    Note that the "linear" in the name ``LinearSpring`` refers to the fact that
+    the spring force is a linear function of the springs length. I.e. for a
+    linear spring with stiffness ``k``, distance between its ends of ``x``, and
+    an equilibrium length of ``0``, the spring force will be ``-k*x``, which is
+    a linear function in ``x``. To create a spring that follows a linear, or
+    straight, pathway between its two ends, a ``LinearPathway`` instance needs
+    to be passed to the ``pathway`` parameter.
+
+    A ``LinearSpring`` is a subclass of ``ForceActuator`` and so follows the
+    same sign conventions for length, extension velocity, and the direction of
+    the forces it applies to its points of attachment on bodies. The sign
+    convention for the direction of forces is such that, for the case where a
+    linear spring is instantiated with a ``LinearPathway`` instance as its
+    pathway, they act to push the two ends of the spring away from one another.
+    Because springs produces a contractile force and acts to pull the two ends
+    together towards the equilibrium length when stretched, the scalar portion
+    of the forces on the endpoint are negative in order to flip the sign of the
+    forces on the endpoints when converted into vector quantities. The
+    following diagram shows the positive force sense and the distance between
+    the points::
+
+       P           Q
+       o<--- F --->o
+       |           |
+       |<--l(t)--->|
+
+    Examples
+    ========
+
+    To construct a linear spring, an expression (or symbol) must be supplied to
+    represent the stiffness (spring constant) of the spring, alongside a
+    pathway specifying its line of action. Let's also create a global reference
+    frame and spatially fix one of the points in it while setting the other to
+    be positioned such that it can freely move in the frame's x direction
+    specified by the coordinate ``q``.
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import (LinearPathway, LinearSpring,
+    ...     Point, ReferenceFrame)
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> N = ReferenceFrame('N')
+    >>> q = dynamicsymbols('q')
+    >>> stiffness = symbols('k')
+    >>> pA, pB = Point('pA'), Point('pB')
+    >>> pA.set_vel(N, 0)
+    >>> pB.set_pos(pA, q*N.x)
+    >>> pB.pos_from(pA)
+    q(t)*N.x
+    >>> linear_pathway = LinearPathway(pA, pB)
+    >>> spring = LinearSpring(stiffness, linear_pathway)
+    >>> spring
+    LinearSpring(k, LinearPathway(pA, pB))
+
+    This spring will produce a force that is proportional to both its stiffness
+    and the pathway's length. Note that this force is negative as SymPy's sign
+    convention for actuators is that negative forces are contractile.
+
+    >>> spring.force
+    -k*sqrt(q(t)**2)
+
+    To create a linear spring with a non-zero equilibrium length, an expression
+    (or symbol) can be passed to the ``equilibrium_length`` parameter on
+    construction on a ``LinearSpring`` instance. Let's create a symbol ``l``
+    to denote a non-zero equilibrium length and create another linear spring.
+
+    >>> l = symbols('l')
+    >>> spring = LinearSpring(stiffness, linear_pathway, equilibrium_length=l)
+    >>> spring
+    LinearSpring(k, LinearPathway(pA, pB), equilibrium_length=l)
+
+    The spring force of this new spring is again proportional to both its
+    stiffness and the pathway's length. However, the spring will not produce
+    any force when ``q(t)`` equals ``l``. Note that the force will become
+    expansile when ``q(t)`` is less than ``l``, as expected.
+
+    >>> spring.force
+    -k*(-l + sqrt(q(t)**2))
+
+    Parameters
+    ==========
+
+    stiffness : Expr
+        The spring constant.
+    pathway : PathwayBase
+        The pathway that the actuator follows. This must be an instance of a
+        concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+    equilibrium_length : Expr, optional
+        The length at which the spring is in equilibrium, i.e. it produces no
+        force. The default value is 0, i.e. the spring force is a linear
+        function of the pathway's length with no constant offset.
+
+    See Also
+    ========
+
+    ForceActuator: force-producing actuator (superclass of ``LinearSpring``).
+    LinearPathway: straight-line pathway between a pair of points.
+
+    """
+
+    def __init__(self, stiffness, pathway, equilibrium_length=S.Zero):
+        """Initializer for ``LinearSpring``.
+
+        Parameters
+        ==========
+
+        stiffness : Expr
+            The spring constant.
+        pathway : PathwayBase
+            The pathway that the actuator follows. This must be an instance of
+            a concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+        equilibrium_length : Expr, optional
+            The length at which the spring is in equilibrium, i.e. it produces
+            no force. The default value is 0, i.e. the spring force is a linear
+            function of the pathway's length with no constant offset.
+
+        """
+        self.stiffness = stiffness
+        self.pathway = pathway
+        self.equilibrium_length = equilibrium_length
+
+    @property
+    def force(self):
+        """The spring force produced by the linear spring."""
+        return -self.stiffness*(self.pathway.length - self.equilibrium_length)
+
+    @force.setter
+    def force(self, force):
+        raise AttributeError('Can\'t set computed attribute `force`.')
+
+    @property
+    def stiffness(self):
+        """The spring constant for the linear spring."""
+        return self._stiffness
+
+    @stiffness.setter
+    def stiffness(self, stiffness):
+        if hasattr(self, '_stiffness'):
+            msg = (
+                f'Can\'t set attribute `stiffness` to {repr(stiffness)} as it '
+                f'is immutable.'
+            )
+            raise AttributeError(msg)
+        self._stiffness = sympify(stiffness, strict=True)
+
+    @property
+    def equilibrium_length(self):
+        """The length of the spring at which it produces no force."""
+        return self._equilibrium_length
+
+    @equilibrium_length.setter
+    def equilibrium_length(self, equilibrium_length):
+        if hasattr(self, '_equilibrium_length'):
+            msg = (
+                f'Can\'t set attribute `equilibrium_length` to '
+                f'{repr(equilibrium_length)} as it is immutable.'
+            )
+            raise AttributeError(msg)
+        self._equilibrium_length = sympify(equilibrium_length, strict=True)
+
+    def __repr__(self):
+        """Representation of a ``LinearSpring``."""
+        string = f'{self.__class__.__name__}({self.stiffness}, {self.pathway}'
+        if self.equilibrium_length == S.Zero:
+            string += ')'
+        else:
+            string += f', equilibrium_length={self.equilibrium_length})'
+        return string
+
+
+class LinearDamper(ForceActuator):
+    """A damper whose force is a linear function of its extension velocity.
+
+    Explanation
+    ===========
+
+    Note that the "linear" in the name ``LinearDamper`` refers to the fact that
+    the damping force is a linear function of the damper's rate of change in
+    its length. I.e. for a linear damper with damping ``c`` and extension
+    velocity ``v``, the damping force will be ``-c*v``, which is a linear
+    function in ``v``. To create a damper that follows a linear, or straight,
+    pathway between its two ends, a ``LinearPathway`` instance needs to be
+    passed to the ``pathway`` parameter.
+
+    A ``LinearDamper`` is a subclass of ``ForceActuator`` and so follows the
+    same sign conventions for length, extension velocity, and the direction of
+    the forces it applies to its points of attachment on bodies. The sign
+    convention for the direction of forces is such that, for the case where a
+    linear damper is instantiated with a ``LinearPathway`` instance as its
+    pathway, they act to push the two ends of the damper away from one another.
+    Because dampers produce a force that opposes the direction of change in
+    length, when extension velocity is positive the scalar portions of the
+    forces applied at the two endpoints are negative in order to flip the sign
+    of the forces on the endpoints wen converted into vector quantities. When
+    extension velocity is negative (i.e. when the damper is shortening), the
+    scalar portions of the fofces applied are also negative so that the signs
+    cancel producing forces on the endpoints that are in the same direction as
+    the positive sign convention for the forces at the endpoints of the pathway
+    (i.e. they act to push the endpoints away from one another). The following
+    diagram shows the positive force sense and the distance between the
+    points::
+
+       P           Q
+       o<--- F --->o
+       |           |
+       |<--l(t)--->|
+
+    Examples
+    ========
+
+    To construct a linear damper, an expression (or symbol) must be supplied to
+    represent the damping coefficient of the damper (we'll use the symbol
+    ``c``), alongside a pathway specifying its line of action. Let's also
+    create a global reference frame and spatially fix one of the points in it
+    while setting the other to be positioned such that it can freely move in
+    the frame's x direction specified by the coordinate ``q``. The velocity
+    that the two points move away from one another can be specified by the
+    coordinate ``u`` where ``u`` is the first time derivative of ``q``
+    (i.e., ``u = Derivative(q(t), t)``).
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import (LinearDamper, LinearPathway,
+    ...     Point, ReferenceFrame)
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> N = ReferenceFrame('N')
+    >>> q = dynamicsymbols('q')
+    >>> damping = symbols('c')
+    >>> pA, pB = Point('pA'), Point('pB')
+    >>> pA.set_vel(N, 0)
+    >>> pB.set_pos(pA, q*N.x)
+    >>> pB.pos_from(pA)
+    q(t)*N.x
+    >>> pB.vel(N)
+    Derivative(q(t), t)*N.x
+    >>> linear_pathway = LinearPathway(pA, pB)
+    >>> damper = LinearDamper(damping, linear_pathway)
+    >>> damper
+    LinearDamper(c, LinearPathway(pA, pB))
+
+    This damper will produce a force that is proportional to both its damping
+    coefficient and the pathway's extension length. Note that this force is
+    negative as SymPy's sign convention for actuators is that negative forces
+    are contractile and the damping force of the damper will oppose the
+    direction of length change.
+
+    >>> damper.force
+    -c*sqrt(q(t)**2)*Derivative(q(t), t)/q(t)
+
+    Parameters
+    ==========
+
+    damping : Expr
+        The damping constant.
+    pathway : PathwayBase
+        The pathway that the actuator follows. This must be an instance of a
+        concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+
+    See Also
+    ========
+
+    ForceActuator: force-producing actuator (superclass of ``LinearDamper``).
+    LinearPathway: straight-line pathway between a pair of points.
+
+    """
+
+    def __init__(self, damping, pathway):
+        """Initializer for ``LinearDamper``.
+
+        Parameters
+        ==========
+
+        damping : Expr
+            The damping constant.
+        pathway : PathwayBase
+            The pathway that the actuator follows. This must be an instance of
+            a concrete subclass of ``PathwayBase``, e.g. ``LinearPathway``.
+
+        """
+        self.damping = damping
+        self.pathway = pathway
+
+    @property
+    def force(self):
+        """The damping force produced by the linear damper."""
+        return -self.damping*self.pathway.extension_velocity
+
+    @force.setter
+    def force(self, force):
+        raise AttributeError('Can\'t set computed attribute `force`.')
+
+    @property
+    def damping(self):
+        """The damping constant for the linear damper."""
+        return self._damping
+
+    @damping.setter
+    def damping(self, damping):
+        if hasattr(self, '_damping'):
+            msg = (
+                f'Can\'t set attribute `damping` to {repr(damping)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        self._damping = sympify(damping, strict=True)
+
+    def __repr__(self):
+        """Representation of a ``LinearDamper``."""
+        return f'{self.__class__.__name__}({self.damping}, {self.pathway})'
+
+
+class TorqueActuator(ActuatorBase):
+    """Torque-producing actuator.
+
+    Explanation
+    ===========
+
+    A ``TorqueActuator`` is an actuator that produces a pair of equal and
+    opposite torques on a pair of bodies.
+
+    Examples
+    ========
+
+    To construct a torque actuator, an expression (or symbol) must be supplied
+    to represent the torque it can produce, alongside a vector specifying the
+    axis about which the torque will act, and a pair of frames on which the
+    torque will act.
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import (ReferenceFrame, RigidBody,
+    ...     TorqueActuator)
+    >>> N = ReferenceFrame('N')
+    >>> A = ReferenceFrame('A')
+    >>> torque = symbols('T')
+    >>> axis = N.z
+    >>> parent = RigidBody('parent', frame=N)
+    >>> child = RigidBody('child', frame=A)
+    >>> bodies = (child, parent)
+    >>> actuator = TorqueActuator(torque, axis, *bodies)
+    >>> actuator
+    TorqueActuator(T, axis=N.z, target_frame=A, reaction_frame=N)
+
+    Note that because torques actually act on frames, not bodies,
+    ``TorqueActuator`` will extract the frame associated with a ``RigidBody``
+    when one is passed instead of a ``ReferenceFrame``.
+
+    Parameters
+    ==========
+
+    torque : Expr
+        The scalar expression defining the torque that the actuator produces.
+    axis : Vector
+        The axis about which the actuator applies torques.
+    target_frame : ReferenceFrame | RigidBody
+        The primary frame on which the actuator will apply the torque.
+    reaction_frame : ReferenceFrame | RigidBody | None
+        The secondary frame on which the actuator will apply the torque. Note
+        that the (equal and opposite) reaction torque is applied to this frame.
+
+    """
+
+    def __init__(self, torque, axis, target_frame, reaction_frame=None):
+        """Initializer for ``TorqueActuator``.
+
+        Parameters
+        ==========
+
+        torque : Expr
+            The scalar expression defining the torque that the actuator
+            produces.
+        axis : Vector
+            The axis about which the actuator applies torques.
+        target_frame : ReferenceFrame | RigidBody
+            The primary frame on which the actuator will apply the torque.
+        reaction_frame : ReferenceFrame | RigidBody | None
+           The secondary frame on which the actuator will apply the torque.
+           Note that the (equal and opposite) reaction torque is applied to
+           this frame.
+
+        """
+        self.torque = torque
+        self.axis = axis
+        self.target_frame = target_frame
+        self.reaction_frame = reaction_frame
+
+    @classmethod
+    def at_pin_joint(cls, torque, pin_joint):
+        """Alternate construtor to instantiate from a ``PinJoint`` instance.
+
+        Examples
+        ========
+
+        To create a pin joint the ``PinJoint`` class requires a name, parent
+        body, and child body to be passed to its constructor. It is also
+        possible to control the joint axis using the ``joint_axis`` keyword
+        argument. In this example let's use the parent body's reference frame's
+        z-axis as the joint axis.
+
+        >>> from sympy.physics.mechanics import (PinJoint, ReferenceFrame,
+        ...     RigidBody, TorqueActuator)
+        >>> N = ReferenceFrame('N')
+        >>> A = ReferenceFrame('A')
+        >>> parent = RigidBody('parent', frame=N)
+        >>> child = RigidBody('child', frame=A)
+        >>> pin_joint = PinJoint(
+        ...     'pin',
+        ...     parent,
+        ...     child,
+        ...     joint_axis=N.z,
+        ... )
+
+        Let's also create a symbol ``T`` that will represent the torque applied
+        by the torque actuator.
+
+        >>> from sympy import symbols
+        >>> torque = symbols('T')
+
+        To create the torque actuator from the ``torque`` and ``pin_joint``
+        variables previously instantiated, these can be passed to the alternate
+        constructor class method ``at_pin_joint`` of the ``TorqueActuator``
+        class. It should be noted that a positive torque will cause a positive
+        displacement of the joint coordinate or that the torque is applied on
+        the child body with a reaction torque on the parent.
+
+        >>> actuator = TorqueActuator.at_pin_joint(torque, pin_joint)
+        >>> actuator
+        TorqueActuator(T, axis=N.z, target_frame=A, reaction_frame=N)
+
+        Parameters
+        ==========
+
+        torque : Expr
+            The scalar expression defining the torque that the actuator
+            produces.
+        pin_joint : PinJoint
+            The pin joint, and by association the parent and child bodies, on
+            which the torque actuator will act. The pair of bodies acted upon
+            by the torque actuator are the parent and child bodies of the pin
+            joint, with the child acting as the reaction body. The pin joint's
+            axis is used as the axis about which the torque actuator will apply
+            its torque.
+
+        """
+        if not isinstance(pin_joint, PinJoint):
+            msg = (
+                f'Value {repr(pin_joint)} passed to `pin_joint` was of type '
+                f'{type(pin_joint)}, must be {PinJoint}.'
+            )
+            raise TypeError(msg)
+        return cls(
+            torque,
+            pin_joint.joint_axis,
+            pin_joint.child_interframe,
+            pin_joint.parent_interframe,
+        )
+
+    @property
+    def torque(self):
+        """The magnitude of the torque produced by the actuator."""
+        return self._torque
+
+    @torque.setter
+    def torque(self, torque):
+        if hasattr(self, '_torque'):
+            msg = (
+                f'Can\'t set attribute `torque` to {repr(torque)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        self._torque = sympify(torque, strict=True)
+
+    @property
+    def axis(self):
+        """The axis about which the torque acts."""
+        return self._axis
+
+    @axis.setter
+    def axis(self, axis):
+        if hasattr(self, '_axis'):
+            msg = (
+                f'Can\'t set attribute `axis` to {repr(axis)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        if not isinstance(axis, Vector):
+            msg = (
+                f'Value {repr(axis)} passed to `axis` was of type '
+                f'{type(axis)}, must be {Vector}.'
+            )
+            raise TypeError(msg)
+        self._axis = axis
+
+    @property
+    def target_frame(self):
+        """The primary reference frames on which the torque will act."""
+        return self._target_frame
+
+    @target_frame.setter
+    def target_frame(self, target_frame):
+        if hasattr(self, '_target_frame'):
+            msg = (
+                f'Can\'t set attribute `target_frame` to {repr(target_frame)} '
+                f'as it is immutable.'
+            )
+            raise AttributeError(msg)
+        if isinstance(target_frame, RigidBody):
+            target_frame = target_frame.frame
+        elif not isinstance(target_frame, ReferenceFrame):
+            msg = (
+                f'Value {repr(target_frame)} passed to `target_frame` was of '
+                f'type {type(target_frame)}, must be {ReferenceFrame}.'
+            )
+            raise TypeError(msg)
+        self._target_frame = target_frame
+
+    @property
+    def reaction_frame(self):
+        """The primary reference frames on which the torque will act."""
+        return self._reaction_frame
+
+    @reaction_frame.setter
+    def reaction_frame(self, reaction_frame):
+        if hasattr(self, '_reaction_frame'):
+            msg = (
+                f'Can\'t set attribute `reaction_frame` to '
+                f'{repr(reaction_frame)} as it is immutable.'
+            )
+            raise AttributeError(msg)
+        if isinstance(reaction_frame, RigidBody):
+            reaction_frame = reaction_frame.frame
+        elif (
+            not isinstance(reaction_frame, ReferenceFrame)
+            and reaction_frame is not None
+        ):
+            msg = (
+                f'Value {repr(reaction_frame)} passed to `reaction_frame` was '
+                f'of type {type(reaction_frame)}, must be {ReferenceFrame}.'
+            )
+            raise TypeError(msg)
+        self._reaction_frame = reaction_frame
+
+    def to_loads(self):
+        """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 by a torque
+        actuator that acts on a pair of bodies attached by a pin joint.
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import (PinJoint, ReferenceFrame,
+        ...     RigidBody, TorqueActuator)
+        >>> torque = symbols('T')
+        >>> N = ReferenceFrame('N')
+        >>> A = ReferenceFrame('A')
+        >>> parent = RigidBody('parent', frame=N)
+        >>> child = RigidBody('child', frame=A)
+        >>> pin_joint = PinJoint(
+        ...     'pin',
+        ...     parent,
+        ...     child,
+        ...     joint_axis=N.z,
+        ... )
+        >>> actuator = TorqueActuator.at_pin_joint(torque, pin_joint)
+
+        The forces produces by the damper can be generated by calling the
+        ``to_loads`` method.
+
+        >>> actuator.to_loads()
+        [(A, T*N.z), (N, - T*N.z)]
+
+        Alternatively, if a torque actuator is created without a reaction frame
+        then the loads returned by the ``to_loads`` method will contain just
+        the single load acting on the target frame.
+
+        >>> actuator = TorqueActuator(torque, N.z, N)
+        >>> actuator.to_loads()
+        [(N, T*N.z)]
+
+        """
+        loads = [
+            Torque(self.target_frame, self.torque*self.axis),
+        ]
+        if self.reaction_frame is not None:
+            loads.append(Torque(self.reaction_frame, -self.torque*self.axis))
+        return loads
+
+    def __repr__(self):
+        """Representation of a ``TorqueActuator``."""
+        string = (
+            f'{self.__class__.__name__}({self.torque}, axis={self.axis}, '
+            f'target_frame={self.target_frame}'
+        )
+        if self.reaction_frame is not None:
+            string += f', reaction_frame={self.reaction_frame})'
+        else:
+            string += ')'
+        return string
+
+
+class DuffingSpring(ForceActuator):
+    """A nonlinear spring based on the Duffing equation.
+
+    Explanation
+    ===========
+
+    Here, ``DuffingSpring`` represents the force exerted by a nonlinear spring based on the Duffing equation:
+    F = -beta*x-alpha*x**3, where x is the displacement from the equilibrium position, beta is the linear spring constant,
+    and alpha is the coefficient for the nonlinear cubic term.
+
+    Parameters
+    ==========
+
+    linear_stiffness : Expr
+        The linear stiffness coefficient (beta).
+    nonlinear_stiffness : Expr
+        The nonlinear stiffness coefficient (alpha).
+    pathway : PathwayBase
+        The pathway that the actuator follows.
+    equilibrium_length : Expr, optional
+        The length at which the spring is in equilibrium (x).
+    """
+
+    def __init__(self, linear_stiffness, nonlinear_stiffness, pathway, equilibrium_length=S.Zero):
+        self.linear_stiffness = sympify(linear_stiffness, strict=True)
+        self.nonlinear_stiffness = sympify(nonlinear_stiffness, strict=True)
+        self.equilibrium_length = sympify(equilibrium_length, strict=True)
+
+        if not isinstance(pathway, PathwayBase):
+            raise TypeError("pathway must be an instance of PathwayBase.")
+        self._pathway = pathway
+
+    @property
+    def linear_stiffness(self):
+        return self._linear_stiffness
+
+    @linear_stiffness.setter
+    def linear_stiffness(self, linear_stiffness):
+        if hasattr(self, '_linear_stiffness'):
+            msg = (
+                f'Can\'t set attribute `linear_stiffness` to '
+                f'{repr(linear_stiffness)} as it is immutable.'
+            )
+            raise AttributeError(msg)
+        self._linear_stiffness = sympify(linear_stiffness, strict=True)
+
+    @property
+    def nonlinear_stiffness(self):
+        return self._nonlinear_stiffness
+
+    @nonlinear_stiffness.setter
+    def nonlinear_stiffness(self, nonlinear_stiffness):
+        if hasattr(self, '_nonlinear_stiffness'):
+            msg = (
+                f'Can\'t set attribute `nonlinear_stiffness` to '
+                f'{repr(nonlinear_stiffness)} as it is immutable.'
+            )
+            raise AttributeError(msg)
+        self._nonlinear_stiffness = sympify(nonlinear_stiffness, strict=True)
+
+    @property
+    def pathway(self):
+        return self._pathway
+
+    @pathway.setter
+    def pathway(self, pathway):
+        if hasattr(self, '_pathway'):
+            msg = (
+                f'Can\'t set attribute `pathway` to {repr(pathway)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        if not isinstance(pathway, PathwayBase):
+            msg = (
+                f'Value {repr(pathway)} passed to `pathway` was of type '
+                f'{type(pathway)}, must be {PathwayBase}.'
+            )
+            raise TypeError(msg)
+        self._pathway = pathway
+
+    @property
+    def equilibrium_length(self):
+        return self._equilibrium_length
+
+    @equilibrium_length.setter
+    def equilibrium_length(self, equilibrium_length):
+        if hasattr(self, '_equilibrium_length'):
+            msg = (
+                f'Can\'t set attribute `equilibrium_length` to '
+                f'{repr(equilibrium_length)} as it is immutable.'
+            )
+            raise AttributeError(msg)
+        self._equilibrium_length = sympify(equilibrium_length, strict=True)
+
+    @property
+    def force(self):
+        """The force produced by the Duffing spring."""
+        displacement = self.pathway.length - self.equilibrium_length
+        return -self.linear_stiffness * displacement - self.nonlinear_stiffness * displacement**3
+
+    @force.setter
+    def force(self, force):
+        if hasattr(self, '_force'):
+            msg = (
+                f'Can\'t set attribute `force` to {repr(force)} as it is '
+                f'immutable.'
+            )
+            raise AttributeError(msg)
+        self._force = sympify(force, strict=True)
+
+    def __repr__(self):
+        return (f"{self.__class__.__name__}("
+                f"{self.linear_stiffness}, {self.nonlinear_stiffness}, {self.pathway}, "
+                f"equilibrium_length={self.equilibrium_length})")

rl/Lib/site-packages/sympy/physics/mechanics/body.py

@@ -0,0 +1,710 @@
+from sympy import Symbol
+from sympy.physics.vector import Point, Vector, ReferenceFrame, Dyadic
+from sympy.physics.mechanics import RigidBody, Particle, Inertia
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.utilities.exceptions import sympy_deprecation_warning
+
+__all__ = ['Body']
+
+
+# XXX: We use type:ignore because the classes RigidBody and Particle have
+# inconsistent parallel axis methods that take different numbers of arguments.
+class Body(RigidBody, Particle):  # type: ignore
+    """
+    Body is a common representation of either a RigidBody or a Particle SymPy
+    object depending on what is passed in during initialization. If a mass is
+    passed in and central_inertia is left as None, the Particle object is
+    created. Otherwise a RigidBody object will be created.
+
+    .. deprecated:: 1.13
+        The Body class is deprecated. Its functionality is captured by
+        :class:`~.RigidBody` and :class:`~.Particle`.
+
+    Explanation
+    ===========
+
+    The attributes that Body possesses will be the same as a Particle instance
+    or a Rigid Body instance depending on which was created. Additional
+    attributes are listed below.
+
+    Attributes
+    ==========
+
+    name : string
+        The body's name
+    masscenter : Point
+        The point which represents the center of mass of the rigid body
+    frame : ReferenceFrame
+        The reference frame which the body is fixed in
+    mass : Sympifyable
+        The body's mass
+    inertia : (Dyadic, Point)
+        The body's inertia around its center of mass. This attribute is specific
+        to the rigid body form of Body and is left undefined for the Particle
+        form
+    loads : iterable
+        This list contains information on the different loads acting on the
+        Body. Forces are listed as a (point, vector) tuple and torques are
+        listed as (reference frame, vector) tuples.
+
+    Parameters
+    ==========
+
+    name : String
+        Defines the name of the body. It is used as the base for defining
+        body specific properties.
+    masscenter : Point, optional
+        A point that represents the center of mass of the body or particle.
+        If no point is given, a point is generated.
+    mass : Sympifyable, optional
+        A Sympifyable object which represents the mass of the body. If no
+        mass is passed, one is generated.
+    frame : ReferenceFrame, optional
+        The ReferenceFrame that represents the reference frame of the body.
+        If no frame is given, a frame is generated.
+    central_inertia : Dyadic, optional
+        Central inertia dyadic of the body. If none is passed while creating
+        RigidBody, a default inertia is generated.
+
+    Examples
+    ========
+
+    As Body has been deprecated, the following examples are for illustrative
+    purposes only. The functionality of Body is fully captured by
+    :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+    warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+
+    Default behaviour. This results in the creation of a RigidBody object for
+    which the mass, mass center, frame and inertia attributes are given default
+    values. ::
+
+        >>> from sympy.physics.mechanics import Body
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     body = Body('name_of_body')
+
+    This next example demonstrates the code required to specify all of the
+    values of the Body object. Note this will also create a RigidBody version of
+    the Body object. ::
+
+        >>> from sympy import Symbol
+        >>> from sympy.physics.mechanics import ReferenceFrame, Point, inertia
+        >>> from sympy.physics.mechanics import Body
+        >>> mass = Symbol('mass')
+        >>> masscenter = Point('masscenter')
+        >>> frame = ReferenceFrame('frame')
+        >>> ixx = Symbol('ixx')
+        >>> body_inertia = inertia(frame, ixx, 0, 0)
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     body = Body('name_of_body', masscenter, mass, frame, body_inertia)
+
+    The minimal code required to create a Particle version of the Body object
+    involves simply passing in a name and a mass. ::
+
+        >>> from sympy import Symbol
+        >>> from sympy.physics.mechanics import Body
+        >>> mass = Symbol('mass')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     body = Body('name_of_body', mass=mass)
+
+    The Particle version of the Body object can also receive a masscenter point
+    and a reference frame, just not an inertia.
+    """
+
+    def __init__(self, name, masscenter=None, mass=None, frame=None,
+                 central_inertia=None):
+        sympy_deprecation_warning(
+            """
+            Support for the Body class has been removed, as its functionality is
+            fully captured by RigidBody and Particle.
+            """,
+            deprecated_since_version="1.13",
+            active_deprecations_target="deprecated-mechanics-body-class"
+        )
+
+        self._loads = []
+
+        if frame is None:
+            frame = ReferenceFrame(name + '_frame')
+
+        if masscenter is None:
+            masscenter = Point(name + '_masscenter')
+
+        if central_inertia is None and mass is None:
+            ixx = Symbol(name + '_ixx')
+            iyy = Symbol(name + '_iyy')
+            izz = Symbol(name + '_izz')
+            izx = Symbol(name + '_izx')
+            ixy = Symbol(name + '_ixy')
+            iyz = Symbol(name + '_iyz')
+            _inertia = Inertia.from_inertia_scalars(masscenter, frame, ixx, iyy,
+                                                    izz, ixy, iyz, izx)
+        else:
+            _inertia = (central_inertia, masscenter)
+
+        if mass is None:
+            _mass = Symbol(name + '_mass')
+        else:
+            _mass = mass
+
+        masscenter.set_vel(frame, 0)
+
+        # If user passes masscenter and mass then a particle is created
+        # otherwise a rigidbody. As a result a body may or may not have inertia.
+        # Note: BodyBase.__init__ is used to prevent problems with super() calls in
+        # Particle and RigidBody arising due to multiple inheritance.
+        if central_inertia is None and mass is not None:
+            BodyBase.__init__(self, name, masscenter, _mass)
+            self.frame = frame
+            self._central_inertia = Dyadic(0)
+        else:
+            BodyBase.__init__(self, name, masscenter, _mass)
+            self.frame = frame
+            self.inertia = _inertia
+
+    def __repr__(self):
+        if self.is_rigidbody:
+            return RigidBody.__repr__(self)
+        return Particle.__repr__(self)
+
+    @property
+    def loads(self):
+        return self._loads
+
+    @property
+    def x(self):
+        """The basis Vector for the Body, in the x direction."""
+        return self.frame.x
+
+    @property
+    def y(self):
+        """The basis Vector for the Body, in the y direction."""
+        return self.frame.y
+
+    @property
+    def z(self):
+        """The basis Vector for the Body, in the z direction."""
+        return self.frame.z
+
+    @property
+    def inertia(self):
+        """The body's inertia about a point; stored as (Dyadic, Point)."""
+        if self.is_rigidbody:
+            return RigidBody.inertia.fget(self)
+        return (self.central_inertia, self.masscenter)
+
+    @inertia.setter
+    def inertia(self, I):
+        RigidBody.inertia.fset(self, I)
+
+    @property
+    def is_rigidbody(self):
+        if hasattr(self, '_inertia'):
+            return True
+        return False
+
+    def kinetic_energy(self, frame):
+        """Kinetic energy of the body.
+
+        Parameters
+        ==========
+
+        frame : ReferenceFrame or Body
+            The Body's angular velocity and the velocity of it's mass
+            center are typically defined with respect to an inertial frame but
+            any relevant frame in which the velocities are known can be supplied.
+
+        Examples
+        ========
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body, ReferenceFrame, Point
+        >>> from sympy import symbols
+        >>> m, v, r, omega = symbols('m v r omega')
+        >>> N = ReferenceFrame('N')
+        >>> O = Point('O')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     P = Body('P', masscenter=O, mass=m)
+        >>> P.masscenter.set_vel(N, v * N.y)
+        >>> P.kinetic_energy(N)
+        m*v**2/2
+
+        >>> N = ReferenceFrame('N')
+        >>> b = ReferenceFrame('b')
+        >>> b.set_ang_vel(N, omega * b.x)
+        >>> P = Point('P')
+        >>> P.set_vel(N, v * N.x)
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B', masscenter=P, frame=b)
+        >>> B.kinetic_energy(N)
+        B_ixx*omega**2/2 + B_mass*v**2/2
+
+        See Also
+        ========
+
+        sympy.physics.mechanics : Particle, RigidBody
+
+        """
+        if isinstance(frame, Body):
+            frame = Body.frame
+        if self.is_rigidbody:
+            return RigidBody(self.name, self.masscenter, self.frame, self.mass,
+                            (self.central_inertia, self.masscenter)).kinetic_energy(frame)
+        return Particle(self.name, self.masscenter, self.mass).kinetic_energy(frame)
+
+    def apply_force(self, force, point=None, reaction_body=None, reaction_point=None):
+        """Add force to the body(s).
+
+        Explanation
+        ===========
+
+        Applies the force on self or equal and opposite forces on
+        self and other body if both are given on the desired point on the bodies.
+        The force applied on other body is taken opposite of self, i.e, -force.
+
+        Parameters
+        ==========
+
+        force: Vector
+            The force to be applied.
+        point: Point, optional
+            The point on self on which force is applied.
+            By default self's masscenter.
+        reaction_body: Body, optional
+            Second body on which equal and opposite force
+            is to be applied.
+        reaction_point : Point, optional
+            The point on other body on which equal and opposite
+            force is applied. By default masscenter of other body.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import Body, Point, dynamicsymbols
+        >>> m, g = symbols('m g')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B')
+        >>> force1 = m*g*B.z
+        >>> B.apply_force(force1) #Applying force on B's masscenter
+        >>> B.loads
+        [(B_masscenter, g*m*B_frame.z)]
+
+        We can also remove some part of force from any point on the body by
+        adding the opposite force to the body on that point.
+
+        >>> f1, f2 = dynamicsymbols('f1 f2')
+        >>> P = Point('P') #Considering point P on body B
+        >>> B.apply_force(f1*B.x + f2*B.y, P)
+        >>> B.loads
+        [(B_masscenter, g*m*B_frame.z), (P, f1(t)*B_frame.x + f2(t)*B_frame.y)]
+
+        Let's remove f1 from point P on body B.
+
+        >>> B.apply_force(-f1*B.x, P)
+        >>> B.loads
+        [(B_masscenter, g*m*B_frame.z), (P, f2(t)*B_frame.y)]
+
+        To further demonstrate the use of ``apply_force`` attribute,
+        consider two bodies connected through a spring.
+
+        >>> from sympy.physics.mechanics import Body, dynamicsymbols
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     N = Body('N') #Newtonion Frame
+        >>> x = dynamicsymbols('x')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B1 = Body('B1')
+        ...     B2 = Body('B2')
+        >>> spring_force = x*N.x
+
+        Now let's apply equal and opposite spring force to the bodies.
+
+        >>> P1 = Point('P1')
+        >>> P2 = Point('P2')
+        >>> B1.apply_force(spring_force, point=P1, reaction_body=B2, reaction_point=P2)
+
+        We can check the loads(forces) applied to bodies now.
+
+        >>> B1.loads
+        [(P1, x(t)*N_frame.x)]
+        >>> B2.loads
+        [(P2, - x(t)*N_frame.x)]
+
+        Notes
+        =====
+
+        If a new force is applied to a body on a point which already has some
+        force applied on it, then the new force is added to the already applied
+        force on that point.
+
+        """
+
+        if not isinstance(point, Point):
+            if point is None:
+                point = self.masscenter  # masscenter
+            else:
+                raise TypeError("Force must be applied to a point on the body.")
+        if not isinstance(force, Vector):
+            raise TypeError("Force must be a vector.")
+
+        if reaction_body is not None:
+            reaction_body.apply_force(-force, point=reaction_point)
+
+        for load in self._loads:
+            if point in load:
+                force += load[1]
+                self._loads.remove(load)
+                break
+
+        self._loads.append((point, force))
+
+    def apply_torque(self, torque, reaction_body=None):
+        """Add torque to the body(s).
+
+        Explanation
+        ===========
+
+        Applies the torque on self or equal and opposite torques on
+        self and other body if both are given.
+        The torque applied on other body is taken opposite of self,
+        i.e, -torque.
+
+        Parameters
+        ==========
+
+        torque: Vector
+            The torque to be applied.
+        reaction_body: Body, optional
+            Second body on which equal and opposite torque
+            is to be applied.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import Body, dynamicsymbols
+        >>> t = symbols('t')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B')
+        >>> torque1 = t*B.z
+        >>> B.apply_torque(torque1)
+        >>> B.loads
+        [(B_frame, t*B_frame.z)]
+
+        We can also remove some part of torque from the body by
+        adding the opposite torque to the body.
+
+        >>> t1, t2 = dynamicsymbols('t1 t2')
+        >>> B.apply_torque(t1*B.x + t2*B.y)
+        >>> B.loads
+        [(B_frame, t1(t)*B_frame.x + t2(t)*B_frame.y + t*B_frame.z)]
+
+        Let's remove t1 from Body B.
+
+        >>> B.apply_torque(-t1*B.x)
+        >>> B.loads
+        [(B_frame, t2(t)*B_frame.y + t*B_frame.z)]
+
+        To further demonstrate the use, let us consider two bodies such that
+        a torque `T` is acting on one body, and `-T` on the other.
+
+        >>> from sympy.physics.mechanics import Body, dynamicsymbols
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     N = Body('N') #Newtonion frame
+        ...     B1 = Body('B1')
+        ...     B2 = Body('B2')
+        >>> v = dynamicsymbols('v')
+        >>> T = v*N.y #Torque
+
+        Now let's apply equal and opposite torque to the bodies.
+
+        >>> B1.apply_torque(T, B2)
+
+        We can check the loads (torques) applied to bodies now.
+
+        >>> B1.loads
+        [(B1_frame, v(t)*N_frame.y)]
+        >>> B2.loads
+        [(B2_frame, - v(t)*N_frame.y)]
+
+        Notes
+        =====
+
+        If a new torque is applied on body which already has some torque applied on it,
+        then the new torque is added to the previous torque about the body's frame.
+
+        """
+
+        if not isinstance(torque, Vector):
+            raise TypeError("A Vector must be supplied to add torque.")
+
+        if reaction_body is not None:
+            reaction_body.apply_torque(-torque)
+
+        for load in self._loads:
+            if self.frame in load:
+                torque += load[1]
+                self._loads.remove(load)
+                break
+        self._loads.append((self.frame, torque))
+
+    def clear_loads(self):
+        """
+        Clears the Body's loads list.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B')
+        >>> force = B.x + B.y
+        >>> B.apply_force(force)
+        >>> B.loads
+        [(B_masscenter, B_frame.x + B_frame.y)]
+        >>> B.clear_loads()
+        >>> B.loads
+        []
+
+        """
+
+        self._loads = []
+
+    def remove_load(self, about=None):
+        """
+        Remove load about a point or frame.
+
+        Parameters
+        ==========
+
+        about : Point or ReferenceFrame, optional
+            The point about which force is applied,
+            and is to be removed.
+            If about is None, then the torque about
+            self's frame is removed.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body, Point
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B')
+        >>> P = Point('P')
+        >>> f1 = B.x
+        >>> f2 = B.y
+        >>> B.apply_force(f1)
+        >>> B.apply_force(f2, P)
+        >>> B.loads
+        [(B_masscenter, B_frame.x), (P, B_frame.y)]
+
+        >>> B.remove_load(P)
+        >>> B.loads
+        [(B_masscenter, B_frame.x)]
+
+        """
+
+        if about is not None:
+            if not isinstance(about, Point):
+                raise TypeError('Load is applied about Point or ReferenceFrame.')
+        else:
+            about = self.frame
+
+        for load in self._loads:
+            if about in load:
+                self._loads.remove(load)
+                break
+
+    def masscenter_vel(self, body):
+        """
+        Returns the velocity of the mass center with respect to the provided
+        rigid body or reference frame.
+
+        Parameters
+        ==========
+
+        body: Body or ReferenceFrame
+            The rigid body or reference frame to calculate the velocity in.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     A = Body('A')
+        ...     B = Body('B')
+        >>> A.masscenter.set_vel(B.frame, 5*B.frame.x)
+        >>> A.masscenter_vel(B)
+        5*B_frame.x
+        >>> A.masscenter_vel(B.frame)
+        5*B_frame.x
+
+        """
+
+        if isinstance(body,  ReferenceFrame):
+            frame=body
+        elif isinstance(body, Body):
+            frame = body.frame
+        return self.masscenter.vel(frame)
+
+    def ang_vel_in(self, body):
+        """
+        Returns this body's angular velocity with respect to the provided
+        rigid body or reference frame.
+
+        Parameters
+        ==========
+
+        body: Body or ReferenceFrame
+            The rigid body or reference frame to calculate the angular velocity in.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body, ReferenceFrame
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     A = Body('A')
+        >>> N = ReferenceFrame('N')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     B = Body('B', frame=N)
+        >>> A.frame.set_ang_vel(N, 5*N.x)
+        >>> A.ang_vel_in(B)
+        5*N.x
+        >>> A.ang_vel_in(N)
+        5*N.x
+
+        """
+
+        if isinstance(body,  ReferenceFrame):
+            frame=body
+        elif isinstance(body, Body):
+            frame = body.frame
+        return self.frame.ang_vel_in(frame)
+
+    def dcm(self, body):
+        """
+        Returns the direction cosine matrix of this body relative to the
+        provided rigid body or reference frame.
+
+        Parameters
+        ==========
+
+        body: Body or ReferenceFrame
+            The rigid body or reference frame to calculate the dcm.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     A = Body('A')
+        ...     B = Body('B')
+        >>> A.frame.orient_axis(B.frame, B.frame.x, 5)
+        >>> A.dcm(B)
+        Matrix([
+        [1,       0,      0],
+        [0,  cos(5), sin(5)],
+        [0, -sin(5), cos(5)]])
+        >>> A.dcm(B.frame)
+        Matrix([
+        [1,       0,      0],
+        [0,  cos(5), sin(5)],
+        [0, -sin(5), cos(5)]])
+
+        """
+
+        if isinstance(body,  ReferenceFrame):
+            frame=body
+        elif isinstance(body, Body):
+            frame = body.frame
+        return self.frame.dcm(frame)
+
+    def parallel_axis(self, point, frame=None):
+        """Returns the inertia dyadic of the body with respect to another
+        point.
+
+        Parameters
+        ==========
+
+        point : sympy.physics.vector.Point
+            The point to express the inertia dyadic about.
+        frame : sympy.physics.vector.ReferenceFrame
+            The reference frame used to construct the dyadic.
+
+        Returns
+        =======
+
+        inertia : sympy.physics.vector.Dyadic
+            The inertia dyadic of the rigid body expressed about the provided
+            point.
+
+        Example
+        =======
+
+        As Body has been deprecated, the following examples are for illustrative
+        purposes only. The functionality of Body is fully captured by
+        :class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
+        warning we can use the ignore_warnings context manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+        >>> from sympy.physics.mechanics import Body
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     A = Body('A')
+        >>> P = A.masscenter.locatenew('point', 3 * A.x + 5 * A.y)
+        >>> A.parallel_axis(P).to_matrix(A.frame)
+        Matrix([
+        [A_ixx + 25*A_mass, A_ixy - 15*A_mass,             A_izx],
+        [A_ixy - 15*A_mass,  A_iyy + 9*A_mass,             A_iyz],
+        [            A_izx,             A_iyz, A_izz + 34*A_mass]])
+
+        """
+        if self.is_rigidbody:
+            return RigidBody.parallel_axis(self, point, frame)
+        return Particle.parallel_axis(self, point, frame)

rl/Lib/site-packages/sympy/physics/mechanics/body_base.py

@@ -0,0 +1,94 @@
+from abc import ABC, abstractmethod
+from sympy import Symbol, sympify
+from sympy.physics.vector import Point
+
+__all__ = ['BodyBase']
+
+
+class BodyBase(ABC):
+    """Abstract class for body type objects."""
+    def __init__(self, name, masscenter=None, mass=None):
+        # Note: If frame=None, no auto-generated frame is created, because a
+        # Particle does not need to have a frame by default.
+        if not isinstance(name, str):
+            raise TypeError('Supply a valid name.')
+        self._name = name
+        if mass is None:
+            mass = Symbol(f'{name}_mass')
+        if masscenter is None:
+            masscenter = Point(f'{name}_masscenter')
+        self.mass = mass
+        self.masscenter = masscenter
+        self.potential_energy = 0
+        self.points = []
+
+    def __str__(self):
+        return self.name
+
+    def __repr__(self):
+        return (f'{self.__class__.__name__}({repr(self.name)}, masscenter='
+                f'{repr(self.masscenter)}, mass={repr(self.mass)})')
+
+    @property
+    def name(self):
+        """The name of the body."""
+        return self._name
+
+    @property
+    def masscenter(self):
+        """The body's center of mass."""
+        return self._masscenter
+
+    @masscenter.setter
+    def masscenter(self, point):
+        if not isinstance(point, Point):
+            raise TypeError("The body's center of mass must be a Point object.")
+        self._masscenter = point
+
+    @property
+    def mass(self):
+        """The body's mass."""
+        return self._mass
+
+    @mass.setter
+    def mass(self, mass):
+        self._mass = sympify(mass)
+
+    @property
+    def potential_energy(self):
+        """The potential energy of the body.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Particle, Point
+        >>> from sympy import symbols
+        >>> m, g, h = symbols('m g h')
+        >>> O = Point('O')
+        >>> P = Particle('P', O, m)
+        >>> P.potential_energy = m * g * h
+        >>> P.potential_energy
+        g*h*m
+
+        """
+        return self._potential_energy
+
+    @potential_energy.setter
+    def potential_energy(self, scalar):
+        self._potential_energy = sympify(scalar)
+
+    @abstractmethod
+    def kinetic_energy(self, frame):
+        pass
+
+    @abstractmethod
+    def linear_momentum(self, frame):
+        pass
+
+    @abstractmethod
+    def angular_momentum(self, point, frame):
+        pass
+
+    @abstractmethod
+    def parallel_axis(self, point, frame):
+        pass

rl/Lib/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)

rl/Lib/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__

rl/Lib/site-packages/sympy/physics/mechanics/jointsmethod.py

@@ -0,0 +1,318 @@
+from sympy.physics.mechanics import (Body, Lagrangian, KanesMethod, LagrangesMethod,
+                                    RigidBody, Particle)
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.physics.mechanics.method import _Methods
+from sympy import Matrix
+from sympy.utilities.exceptions import sympy_deprecation_warning
+
+__all__ = ['JointsMethod']
+
+
+class JointsMethod(_Methods):
+    """Method for formulating the equations of motion using a set of interconnected bodies with joints.
+
+    .. deprecated:: 1.13
+        The JointsMethod class is deprecated. Its functionality has been
+        replaced by the new :class:`~.System` class.
+
+    Parameters
+    ==========
+
+    newtonion : Body or ReferenceFrame
+        The newtonion(inertial) frame.
+    *joints : Joint
+        The joints in the system
+
+    Attributes
+    ==========
+
+    q, u : iterable
+        Iterable of the generalized coordinates and speeds
+    bodies : iterable
+        Iterable of Body objects in the system.
+    loads : iterable
+        Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
+        describing the forces on the system.
+    mass_matrix : Matrix, shape(n, n)
+        The system's mass matrix
+    forcing : Matrix, shape(n, 1)
+        The system's forcing vector
+    mass_matrix_full : Matrix, shape(2*n, 2*n)
+        The "mass matrix" for the u's and q's
+    forcing_full : Matrix, shape(2*n, 1)
+        The "forcing vector" for the u's and q's
+    method : KanesMethod or Lagrange's method
+        Method's object.
+    kdes : iterable
+        Iterable of kde in they system.
+
+    Examples
+    ========
+
+    As Body and JointsMethod have been deprecated, the following examples are
+    for illustrative purposes only. The functionality of Body is fully captured
+    by :class:`~.RigidBody` and :class:`~.Particle` and the functionality of
+    JointsMethod is fully captured by :class:`~.System`. To ignore the
+    deprecation warning we can use the ignore_warnings context manager.
+
+    >>> from sympy.utilities.exceptions import ignore_warnings
+
+    This is a simple example for a one degree of freedom translational
+    spring-mass-damper.
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import Body, JointsMethod, PrismaticJoint
+    >>> from sympy.physics.vector import dynamicsymbols
+    >>> c, k = symbols('c k')
+    >>> x, v = dynamicsymbols('x v')
+    >>> with ignore_warnings(DeprecationWarning):
+    ...     wall = Body('W')
+    ...     body = Body('B')
+    >>> J = PrismaticJoint('J', wall, body, coordinates=x, speeds=v)
+    >>> wall.apply_force(c*v*wall.x, reaction_body=body)
+    >>> wall.apply_force(k*x*wall.x, reaction_body=body)
+    >>> with ignore_warnings(DeprecationWarning):
+    ...     method = JointsMethod(wall, J)
+    >>> method.form_eoms()
+    Matrix([[-B_mass*Derivative(v(t), t) - c*v(t) - k*x(t)]])
+    >>> M = method.mass_matrix_full
+    >>> F = method.forcing_full
+    >>> rhs = M.LUsolve(F)
+    >>> rhs
+    Matrix([
+    [                     v(t)],
+    [(-c*v(t) - k*x(t))/B_mass]])
+
+    Notes
+    =====
+
+    ``JointsMethod`` currently only works with systems that do not have any
+    configuration or motion constraints.
+
+    """
+
+    def __init__(self, newtonion, *joints):
+        sympy_deprecation_warning(
+            """
+            The JointsMethod class is deprecated.
+            Its functionality has been replaced by the new System class.
+            """,
+            deprecated_since_version="1.13",
+            active_deprecations_target="deprecated-mechanics-jointsmethod"
+        )
+        if isinstance(newtonion, BodyBase):
+            self.frame = newtonion.frame
+        else:
+            self.frame = newtonion
+
+        self._joints = joints
+        self._bodies = self._generate_bodylist()
+        self._loads = self._generate_loadlist()
+        self._q = self._generate_q()
+        self._u = self._generate_u()
+        self._kdes = self._generate_kdes()
+
+        self._method = None
+
+    @property
+    def bodies(self):
+        """List of bodies in they system."""
+        return self._bodies
+
+    @property
+    def loads(self):
+        """List of loads on the system."""
+        return self._loads
+
+    @property
+    def q(self):
+        """List of the generalized coordinates."""
+        return self._q
+
+    @property
+    def u(self):
+        """List of the generalized speeds."""
+        return self._u
+
+    @property
+    def kdes(self):
+        """List of the generalized coordinates."""
+        return self._kdes
+
+    @property
+    def forcing_full(self):
+        """The "forcing vector" for the u's and q's."""
+        return self.method.forcing_full
+
+    @property
+    def mass_matrix_full(self):
+        """The "mass matrix" for the u's and q's."""
+        return self.method.mass_matrix_full
+
+    @property
+    def mass_matrix(self):
+        """The system's mass matrix."""
+        return self.method.mass_matrix
+
+    @property
+    def forcing(self):
+        """The system's forcing vector."""
+        return self.method.forcing
+
+    @property
+    def method(self):
+        """Object of method used to form equations of systems."""
+        return self._method
+
+    def _generate_bodylist(self):
+        bodies = []
+        for joint in self._joints:
+            if joint.child not in bodies:
+                bodies.append(joint.child)
+            if joint.parent not in bodies:
+                bodies.append(joint.parent)
+        return bodies
+
+    def _generate_loadlist(self):
+        load_list = []
+        for body in self.bodies:
+            if isinstance(body, Body):
+                load_list.extend(body.loads)
+        return load_list
+
+    def _generate_q(self):
+        q_ind = []
+        for joint in self._joints:
+            for coordinate in joint.coordinates:
+                if coordinate in q_ind:
+                    raise ValueError('Coordinates of joints should be unique.')
+                q_ind.append(coordinate)
+        return Matrix(q_ind)
+
+    def _generate_u(self):
+        u_ind = []
+        for joint in self._joints:
+            for speed in joint.speeds:
+                if speed in u_ind:
+                    raise ValueError('Speeds of joints should be unique.')
+                u_ind.append(speed)
+        return Matrix(u_ind)
+
+    def _generate_kdes(self):
+        kd_ind = Matrix(1, 0, []).T
+        for joint in self._joints:
+            kd_ind = kd_ind.col_join(joint.kdes)
+        return kd_ind
+
+    def _convert_bodies(self):
+        # Convert `Body` to `Particle` and `RigidBody`
+        bodylist = []
+        for body in self.bodies:
+            if not isinstance(body, Body):
+                bodylist.append(body)
+                continue
+            if body.is_rigidbody:
+                rb = RigidBody(body.name, body.masscenter, body.frame, body.mass,
+                    (body.central_inertia, body.masscenter))
+                rb.potential_energy = body.potential_energy
+                bodylist.append(rb)
+            else:
+                part = Particle(body.name, body.masscenter, body.mass)
+                part.potential_energy = body.potential_energy
+                bodylist.append(part)
+        return bodylist
+
+    def form_eoms(self, method=KanesMethod):
+        """Method to form system's equation of motions.
+
+        Parameters
+        ==========
+
+        method : Class
+            Class name of method.
+
+        Returns
+        ========
+
+        Matrix
+            Vector of equations of motions.
+
+        Examples
+        ========
+
+        As Body and JointsMethod have been deprecated, the following examples
+        are for illustrative purposes only. The functionality of Body is fully
+        captured by :class:`~.RigidBody` and :class:`~.Particle` and the
+        functionality of JointsMethod is fully captured by :class:`~.System`. To
+        ignore the deprecation warning we can use the ignore_warnings context
+        manager.
+
+        >>> from sympy.utilities.exceptions import ignore_warnings
+
+        This is a simple example for a one degree of freedom translational
+        spring-mass-damper.
+
+        >>> from sympy import S, symbols
+        >>> from sympy.physics.mechanics import LagrangesMethod, dynamicsymbols, Body
+        >>> from sympy.physics.mechanics import PrismaticJoint, JointsMethod
+        >>> q = dynamicsymbols('q')
+        >>> qd = dynamicsymbols('q', 1)
+        >>> m, k, b = symbols('m k b')
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     wall = Body('W')
+        ...     part = Body('P', mass=m)
+        >>> part.potential_energy = k * q**2 / S(2)
+        >>> J = PrismaticJoint('J', wall, part, coordinates=q, speeds=qd)
+        >>> wall.apply_force(b * qd * wall.x, reaction_body=part)
+        >>> with ignore_warnings(DeprecationWarning):
+        ...     method = JointsMethod(wall, J)
+        >>> method.form_eoms(LagrangesMethod)
+        Matrix([[b*Derivative(q(t), t) + k*q(t) + m*Derivative(q(t), (t, 2))]])
+
+        We can also solve for the states using the 'rhs' method.
+
+        >>> method.rhs()
+        Matrix([
+        [                Derivative(q(t), t)],
+        [(-b*Derivative(q(t), t) - k*q(t))/m]])
+
+        """
+
+        bodylist = self._convert_bodies()
+        if issubclass(method, LagrangesMethod): #LagrangesMethod or similar
+            L = Lagrangian(self.frame, *bodylist)
+            self._method = method(L, self.q, self.loads, bodylist, self.frame)
+        else: #KanesMethod or similar
+            self._method = method(self.frame, q_ind=self.q, u_ind=self.u, kd_eqs=self.kdes,
+                                    forcelist=self.loads, bodies=bodylist)
+        soln = self.method._form_eoms()
+        return soln
+
+    def rhs(self, inv_method=None):
+        """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`
+
+        Returns
+        ========
+
+        Matrix
+            Numerically solvable equations.
+
+        See Also
+        ========
+
+        sympy.physics.mechanics.kane.KanesMethod.rhs:
+            KanesMethod's rhs function.
+        sympy.physics.mechanics.lagrange.LagrangesMethod.rhs:
+            LagrangesMethod's rhs function.
+
+        """
+
+        return self.method.rhs(inv_method=inv_method)

rl/Lib/site-packages/sympy/physics/mechanics/kane.py

@@ -0,0 +1,860 @@
+from sympy import zeros, Matrix, diff, eye
+from sympy.core.sorting import default_sort_key
+from sympy.physics.vector import (ReferenceFrame, dynamicsymbols,
+                                  partial_velocity)
+from sympy.physics.mechanics.method import _Methods
+from sympy.physics.mechanics.particle import Particle
+from sympy.physics.mechanics.rigidbody import RigidBody
+from sympy.physics.mechanics.functions import (msubs, find_dynamicsymbols,
+                                               _f_list_parser,
+                                               _validate_coordinates,
+                                               _parse_linear_solver)
+from sympy.physics.mechanics.linearize import Linearizer
+from sympy.utilities.iterables import iterable
+
+__all__ = ['KanesMethod']
+
+
+class KanesMethod(_Methods):
+    r"""Kane's method object.
+
+    Explanation
+    ===========
+
+    This object is used to do the "book-keeping" as you go through and form
+    equations of motion in the way Kane presents in:
+    Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill
+
+    The attributes are for equations in the form [M] udot = forcing.
+
+    Attributes
+    ==========
+
+    q, u : Matrix
+        Matrices of the generalized coordinates and speeds
+    bodies : iterable
+        Iterable of Particle and RigidBody objects in the system.
+    loads : iterable
+        Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
+        describing the forces on the system.
+    auxiliary_eqs : Matrix
+        If applicable, the set of auxiliary Kane's
+        equations used to solve for non-contributing
+        forces.
+    mass_matrix : Matrix
+        The system's dynamics mass matrix: [k_d; k_dnh]
+    forcing : Matrix
+        The system's dynamics forcing vector: -[f_d; f_dnh]
+    mass_matrix_kin : Matrix
+        The "mass matrix" for kinematic differential equations: k_kqdot
+    forcing_kin : Matrix
+        The forcing vector for kinematic differential equations: -(k_ku*u + f_k)
+    mass_matrix_full : Matrix
+        The "mass matrix" for the u's and q's with dynamics and kinematics
+    forcing_full : Matrix
+        The "forcing vector" for the u's and q's with dynamics and kinematics
+
+    Parameters
+    ==========
+
+    frame : ReferenceFrame
+        The inertial reference frame for the system.
+    q_ind : iterable of dynamicsymbols
+        Independent generalized coordinates.
+    u_ind : iterable of dynamicsymbols
+        Independent generalized speeds.
+    kd_eqs : iterable of Expr, optional
+        Kinematic differential equations, which linearly relate the generalized
+        speeds to the time-derivatives of the generalized coordinates.
+    q_dependent : iterable of dynamicsymbols, optional
+        Dependent generalized coordinates.
+    configuration_constraints : iterable of Expr, optional
+        Constraints on the system's configuration, i.e. holonomic constraints.
+    u_dependent : iterable of dynamicsymbols, optional
+        Dependent generalized speeds.
+    velocity_constraints : iterable of Expr, optional
+        Constraints on the system's velocity, i.e. the combination of the
+        nonholonomic constraints and the time-derivative of the holonomic
+        constraints.
+    acceleration_constraints : iterable of Expr, optional
+        Constraints on the system's acceleration, by default these are the
+        time-derivative of the velocity constraints.
+    u_auxiliary : iterable of dynamicsymbols, optional
+        Auxiliary generalized speeds.
+    bodies : iterable of Particle and/or RigidBody, optional
+        The particles and rigid bodies in the system.
+    forcelist : iterable of tuple[Point | ReferenceFrame, Vector], optional
+        Forces and torques applied on the system.
+    explicit_kinematics : bool
+        Boolean whether the mass matrices and forcing vectors should use the
+        explicit form (default) or implicit form for kinematics.
+        See the notes for more details.
+    kd_eqs_solver : str, callable
+        Method used to solve the kinematic differential equations. 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 ``f(A, rhs)``, where it solves the
+        equations and returns the solution. The default utilizes LU solve. See
+        the notes for more information.
+    constraint_solver : str, callable
+        Method used to solve the velocity constraints. 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 ``f(A, rhs)``, where it solves the
+        equations and returns the solution. The default utilizes LU solve. See
+        the notes for more information.
+
+    Notes
+    =====
+
+    The mass matrices and forcing vectors related to kinematic equations
+    are given in the explicit form by default. In other words, the kinematic
+    mass matrix is $\mathbf{k_{k\dot{q}}} = \mathbf{I}$.
+    In order to get the implicit form of those matrices/vectors, you can set the
+    ``explicit_kinematics`` attribute to ``False``. So $\mathbf{k_{k\dot{q}}}$
+    is not necessarily an identity matrix. This can provide more compact
+    equations for non-simple kinematics.
+
+    Two linear solvers can be supplied to ``KanesMethod``: one for solving the
+    kinematic differential equations and one to solve the velocity constraints.
+    Both of these sets of equations can be expressed as a linear system ``Ax = rhs``,
+    which have to be solved in order to obtain the equations of motion.
+
+    The default solver ``'LU'``, which stands for LU solve, results relatively low
+    number of operations. The weakness of this method is that it can result in zero
+    division errors.
+
+    If zero divisions are encountered, a possible solver which may solve the problem
+    is ``"CRAMER"``. This method uses Cramer's rule to solve the system. This method
+    is slower and results in more operations than the default solver. However it only
+    uses a single division by default per entry of the solution.
+
+    While a valid list of solvers can be found at
+    :meth:`sympy.matrices.matrixbase.MatrixBase.solve`, it is also possible to supply a
+    `callable`. This way it is possible to use a different solver routine. If the
+    kinematic differential equations are not too complex it can be worth it to simplify
+    the solution by using ``lambda A, b: simplify(Matrix.LUsolve(A, b))``. Another
+    option solver one may use is :func:`sympy.solvers.solveset.linsolve`. This can be
+    done using `lambda A, b: tuple(linsolve((A, b)))[0]`, where we select the first
+    solution as our system should have only one unique solution.
+
+    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 speeds and coordinates and their
+    derivatives.
+    Then we create a point and set its velocity in a frame.
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame
+        >>> from sympy.physics.mechanics import Point, Particle, KanesMethod
+        >>> 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)
+
+    Next we need to arrange/store information in the way that KanesMethod
+    requires. The kinematic differential equations should be an iterable of
+    expressions. A list of forces/torques must be constructed, where each entry
+    in the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where
+    the Vectors represent the Force or Torque.
+    Next a particle needs to be created, and it needs to have a point and mass
+    assigned to it.
+    Finally, a list of all bodies and particles needs to be created.
+
+        >>> kd = [qd - u]
+        >>> FL = [(P, (-k * q - c * u) * N.x)]
+        >>> pa = Particle('pa', P, m)
+        >>> BL = [pa]
+
+    Finally we can generate the equations of motion.
+    First we create the KanesMethod object and supply an inertial frame,
+    coordinates, generalized speeds, and the kinematic differential equations.
+    Additional quantities such as configuration and motion constraints,
+    dependent coordinates and speeds, and auxiliary speeds are also supplied
+    here (see the online documentation).
+    Next we form FR* and FR to complete: Fr + Fr* = 0.
+    We have the equations of motion at this point.
+    It makes sense to rearrange them though, so we calculate the mass matrix and
+    the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is
+    the mass matrix, udot is a vector of the time derivatives of the
+    generalized speeds, and forcing is a vector representing "forcing" terms.
+
+        >>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
+        >>> (fr, frstar) = KM.kanes_equations(BL, FL)
+        >>> MM = KM.mass_matrix
+        >>> forcing = KM.forcing
+        >>> rhs = MM.inv() * forcing
+        >>> rhs
+        Matrix([[(-c*u(t) - k*q(t))/m]])
+        >>> KM.linearize(A_and_B=True)[0]
+        Matrix([
+        [   0,    1],
+        [-k/m, -c/m]])
+
+    Please look at the documentation pages for more information on how to
+    perform linearization and how to deal with dependent coordinates & speeds,
+    and how do deal with bringing non-contributing forces into evidence.
+
+    """
+
+    def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None,
+                 configuration_constraints=None, u_dependent=None,
+                 velocity_constraints=None, acceleration_constraints=None,
+                 u_auxiliary=None, bodies=None, forcelist=None,
+                 explicit_kinematics=True, kd_eqs_solver='LU',
+                 constraint_solver='LU'):
+
+        """Please read the online documentation. """
+        if not q_ind:
+            q_ind = [dynamicsymbols('dummy_q')]
+            kd_eqs = [dynamicsymbols('dummy_kd')]
+
+        if not isinstance(frame, ReferenceFrame):
+            raise TypeError('An inertial ReferenceFrame must be supplied')
+        self._inertial = frame
+
+        self._fr = None
+        self._frstar = None
+
+        self._forcelist = forcelist
+        self._bodylist = bodies
+
+        self.explicit_kinematics = explicit_kinematics
+        self._constraint_solver = constraint_solver
+        self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent,
+                u_auxiliary)
+        _validate_coordinates(self.q, self.u)
+        self._initialize_kindiffeq_matrices(kd_eqs, kd_eqs_solver)
+        self._initialize_constraint_matrices(
+            configuration_constraints, velocity_constraints,
+            acceleration_constraints, constraint_solver)
+
+    def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
+        """Initialize the coordinate and speed vectors."""
+
+        none_handler = lambda x: Matrix(x) if x else Matrix()
+
+        # Initialize generalized coordinates
+        q_dep = none_handler(q_dep)
+        if not iterable(q_ind):
+            raise TypeError('Generalized coordinates must be an iterable.')
+        if not iterable(q_dep):
+            raise TypeError('Dependent coordinates must be an iterable.')
+        q_ind = Matrix(q_ind)
+        self._qdep = q_dep
+        self._q = Matrix([q_ind, q_dep])
+        self._qdot = self.q.diff(dynamicsymbols._t)
+
+        # Initialize generalized speeds
+        u_dep = none_handler(u_dep)
+        if not iterable(u_ind):
+            raise TypeError('Generalized speeds must be an iterable.')
+        if not iterable(u_dep):
+            raise TypeError('Dependent speeds must be an iterable.')
+        u_ind = Matrix(u_ind)
+        self._udep = u_dep
+        self._u = Matrix([u_ind, u_dep])
+        self._udot = self.u.diff(dynamicsymbols._t)
+        self._uaux = none_handler(u_aux)
+
+    def _initialize_constraint_matrices(self, config, vel, acc, linear_solver='LU'):
+        """Initializes constraint matrices."""
+        linear_solver = _parse_linear_solver(linear_solver)
+        # Define vector dimensions
+        o = len(self.u)
+        m = len(self._udep)
+        p = o - m
+        none_handler = lambda x: Matrix(x) if x else Matrix()
+
+        # Initialize configuration constraints
+        config = none_handler(config)
+        if len(self._qdep) != len(config):
+            raise ValueError('There must be an equal number of dependent '
+                             'coordinates and configuration constraints.')
+        self._f_h = none_handler(config)
+
+        # Initialize velocity and acceleration constraints
+        vel = none_handler(vel)
+        acc = none_handler(acc)
+        if len(vel) != m:
+            raise ValueError('There must be an equal number of dependent '
+                             'speeds and velocity constraints.')
+        if acc and (len(acc) != m):
+            raise ValueError('There must be an equal number of dependent '
+                             'speeds and acceleration constraints.')
+        if vel:
+            u_zero = dict.fromkeys(self.u, 0)
+            udot_zero = dict.fromkeys(self._udot, 0)
+
+            # When calling kanes_equations, another class instance will be
+            # created if auxiliary u's are present. In this case, the
+            # computation of kinetic differential equation matrices will be
+            # skipped as this was computed during the original KanesMethod
+            # object, and the qd_u_map will not be available.
+            if self._qdot_u_map is not None:
+                vel = msubs(vel, self._qdot_u_map)
+
+            self._f_nh = msubs(vel, u_zero)
+            self._k_nh = (vel - self._f_nh).jacobian(self.u)
+            # If no acceleration constraints given, calculate them.
+            if not acc:
+                _f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
+                          self._f_nh.diff(dynamicsymbols._t))
+                if self._qdot_u_map is not None:
+                    _f_dnh = msubs(_f_dnh, self._qdot_u_map)
+                self._f_dnh = _f_dnh
+                self._k_dnh = self._k_nh
+            else:
+                if self._qdot_u_map is not None:
+                    acc = msubs(acc, self._qdot_u_map)
+                self._f_dnh = msubs(acc, udot_zero)
+                self._k_dnh = (acc - self._f_dnh).jacobian(self._udot)
+
+            # Form of non-holonomic constraints is B*u + C = 0.
+            # We partition B into independent and dependent columns:
+            # Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
+            # to independent speeds as: udep = Ars*uind, neglecting the C term.
+            B_ind = self._k_nh[:, :p]
+            B_dep = self._k_nh[:, p:o]
+            self._Ars = -linear_solver(B_dep, B_ind)
+        else:
+            self._f_nh = Matrix()
+            self._k_nh = Matrix()
+            self._f_dnh = Matrix()
+            self._k_dnh = Matrix()
+            self._Ars = Matrix()
+
+    def _initialize_kindiffeq_matrices(self, kdeqs, linear_solver='LU'):
+        """Initialize the kinematic differential equation matrices.
+
+        Parameters
+        ==========
+        kdeqs : sequence of sympy expressions
+            Kinematic differential equations in the form of f(u,q',q,t) where
+            f() = 0. The equations have to be linear in the generalized
+            coordinates and generalized speeds.
+
+        """
+        linear_solver = _parse_linear_solver(linear_solver)
+        if kdeqs:
+            if len(self.q) != len(kdeqs):
+                raise ValueError('There must be an equal number of kinematic '
+                                 'differential equations and coordinates.')
+
+            u = self.u
+            qdot = self._qdot
+
+            kdeqs = Matrix(kdeqs)
+
+            u_zero = dict.fromkeys(u, 0)
+            uaux_zero = dict.fromkeys(self._uaux, 0)
+            qdot_zero = dict.fromkeys(qdot, 0)
+
+            # Extract the linear coefficient matrices as per the following
+            # equation:
+            #
+            # k_ku(q,t)*u(t) + k_kqdot(q,t)*q'(t) + f_k(q,t) = 0
+            #
+            k_ku = kdeqs.jacobian(u)
+            k_kqdot = kdeqs.jacobian(qdot)
+            f_k = kdeqs.xreplace(u_zero).xreplace(qdot_zero)
+
+            # The kinematic differential equations should be linear in both q'
+            # and u, so check for u and q' in the components.
+            dy_syms = find_dynamicsymbols(k_ku.row_join(k_kqdot).row_join(f_k))
+            nonlin_vars = [vari for vari in u[:] + qdot[:] if vari in dy_syms]
+            if nonlin_vars:
+                msg = ('The provided kinematic differential equations are '
+                       'nonlinear in {}. They must be linear in the '
+                       'generalized speeds and derivatives of the generalized '
+                       'coordinates.')
+                raise ValueError(msg.format(nonlin_vars))
+
+            self._f_k_implicit = f_k.xreplace(uaux_zero)
+            self._k_ku_implicit = k_ku.xreplace(uaux_zero)
+            self._k_kqdot_implicit = k_kqdot
+
+            # Solve for q'(t) such that the coefficient matrices are now in
+            # this form:
+            #
+            # k_kqdot^-1*k_ku*u(t) + I*q'(t) + k_kqdot^-1*f_k = 0
+            #
+            # NOTE : Solving the kinematic differential equations here is not
+            # necessary and prevents the equations from being provided in fully
+            # implicit form.
+            f_k_explicit = linear_solver(k_kqdot, f_k)
+            k_ku_explicit = linear_solver(k_kqdot, k_ku)
+            self._qdot_u_map = dict(zip(qdot, -(k_ku_explicit*u + f_k_explicit)))
+
+            self._f_k = f_k_explicit.xreplace(uaux_zero)
+            self._k_ku = k_ku_explicit.xreplace(uaux_zero)
+            self._k_kqdot = eye(len(qdot))
+
+        else:
+            self._qdot_u_map = None
+            self._f_k_implicit = self._f_k = Matrix()
+            self._k_ku_implicit = self._k_ku = Matrix()
+            self._k_kqdot_implicit = self._k_kqdot = Matrix()
+
+    def _form_fr(self, fl):
+        """Form the generalized active force."""
+        if fl is not None and (len(fl) == 0 or not iterable(fl)):
+            raise ValueError('Force pairs must be supplied in an '
+                'non-empty iterable or None.')
+
+        N = self._inertial
+        # pull out relevant velocities for constructing partial velocities
+        vel_list, f_list = _f_list_parser(fl, N)
+        vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
+        f_list = [msubs(i, self._qdot_u_map) for i in f_list]
+
+        # Fill Fr with dot product of partial velocities and forces
+        o = len(self.u)
+        b = len(f_list)
+        FR = zeros(o, 1)
+        partials = partial_velocity(vel_list, self.u, N)
+        for i in range(o):
+            FR[i] = sum(partials[j][i].dot(f_list[j]) for j in range(b))
+
+        # In case there are dependent speeds
+        if self._udep:
+            p = o - len(self._udep)
+            FRtilde = FR[:p, 0]
+            FRold = FR[p:o, 0]
+            FRtilde += self._Ars.T * FRold
+            FR = FRtilde
+
+        self._forcelist = fl
+        self._fr = FR
+        return FR
+
+    def _form_frstar(self, bl):
+        """Form the generalized inertia force."""
+
+        if not iterable(bl):
+            raise TypeError('Bodies must be supplied in an iterable.')
+
+        t = dynamicsymbols._t
+        N = self._inertial
+        # Dicts setting things to zero
+        udot_zero = dict.fromkeys(self._udot, 0)
+        uaux_zero = dict.fromkeys(self._uaux, 0)
+        uauxdot = [diff(i, t) for i in self._uaux]
+        uauxdot_zero = dict.fromkeys(uauxdot, 0)
+        # Dictionary of q' and q'' to u and u'
+        q_ddot_u_map = {k.diff(t): v.diff(t).xreplace(
+            self._qdot_u_map) for (k, v) in self._qdot_u_map.items()}
+        q_ddot_u_map.update(self._qdot_u_map)
+
+        # Fill up the list of partials: format is a list with num elements
+        # equal to number of entries in body list. Each of these elements is a
+        # list - either of length 1 for the translational components of
+        # particles or of length 2 for the translational and rotational
+        # components of rigid bodies. The inner most list is the list of
+        # partial velocities.
+        def get_partial_velocity(body):
+            if isinstance(body, RigidBody):
+                vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
+            elif isinstance(body, Particle):
+                vlist = [body.point.vel(N),]
+            else:
+                raise TypeError('The body list may only contain either '
+                                'RigidBody or Particle as list elements.')
+            v = [msubs(vel, self._qdot_u_map) for vel in vlist]
+            return partial_velocity(v, self.u, N)
+        partials = [get_partial_velocity(body) for body in bl]
+
+        # Compute fr_star in two components:
+        # fr_star = -(MM*u' + nonMM)
+        o = len(self.u)
+        MM = zeros(o, o)
+        nonMM = zeros(o, 1)
+        zero_uaux = lambda expr: msubs(expr, uaux_zero)
+        zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
+        for i, body in enumerate(bl):
+            if isinstance(body, RigidBody):
+                M = zero_uaux(body.mass)
+                I = zero_uaux(body.central_inertia)
+                vel = zero_uaux(body.masscenter.vel(N))
+                omega = zero_uaux(body.frame.ang_vel_in(N))
+                acc = zero_udot_uaux(body.masscenter.acc(N))
+                inertial_force = (M.diff(t) * vel + M * acc)
+                inertial_torque = zero_uaux((I.dt(body.frame).dot(omega)) +
+                    msubs(I.dot(body.frame.ang_acc_in(N)), udot_zero) +
+                    (omega.cross(I.dot(omega))))
+                for j in range(o):
+                    tmp_vel = zero_uaux(partials[i][0][j])
+                    tmp_ang = zero_uaux(I.dot(partials[i][1][j]))
+                    for k in range(o):
+                        # translational
+                        MM[j, k] += M*tmp_vel.dot(partials[i][0][k])
+                        # rotational
+                        MM[j, k] += tmp_ang.dot(partials[i][1][k])
+                    nonMM[j] += inertial_force.dot(partials[i][0][j])
+                    nonMM[j] += inertial_torque.dot(partials[i][1][j])
+            else:
+                M = zero_uaux(body.mass)
+                vel = zero_uaux(body.point.vel(N))
+                acc = zero_udot_uaux(body.point.acc(N))
+                inertial_force = (M.diff(t) * vel + M * acc)
+                for j in range(o):
+                    temp = zero_uaux(partials[i][0][j])
+                    for k in range(o):
+                        MM[j, k] += M*temp.dot(partials[i][0][k])
+                    nonMM[j] += inertial_force.dot(partials[i][0][j])
+        # Compose fr_star out of MM and nonMM
+        MM = zero_uaux(msubs(MM, q_ddot_u_map))
+        nonMM = msubs(msubs(nonMM, q_ddot_u_map),
+                udot_zero, uauxdot_zero, uaux_zero)
+        fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)
+
+        # If there are dependent speeds, we need to find fr_star_tilde
+        if self._udep:
+            p = o - len(self._udep)
+            fr_star_ind = fr_star[:p, 0]
+            fr_star_dep = fr_star[p:o, 0]
+            fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
+            # Apply the same to MM
+            MMi = MM[:p, :]
+            MMd = MM[p:o, :]
+            MM = MMi + (self._Ars.T * MMd)
+            # Apply the same to nonMM
+            nonMM = nonMM[:p, :] + (self._Ars.T * nonMM[p:o, :])
+
+        self._bodylist = bl
+        self._frstar = fr_star
+        self._k_d = MM
+        self._f_d = -(self._fr - nonMM)
+        return fr_star
+
+    def to_linearizer(self, linear_solver='LU'):
+        """Returns an instance of the Linearizer class, initiated from the
+        data in the KanesMethod 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
+        ==========
+        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`.
+
+        """
+
+        if (self._fr is None) or (self._frstar is None):
+            raise ValueError('Need to compute Fr, Fr* first.')
+
+        # Get required equation components. The Kane's method class breaks
+        # these into pieces. Need to reassemble
+        f_c = self._f_h
+        if self._f_nh and self._k_nh:
+            f_v = self._f_nh + self._k_nh*Matrix(self.u)
+        else:
+            f_v = Matrix()
+        if self._f_dnh and self._k_dnh:
+            f_a = self._f_dnh + self._k_dnh*Matrix(self._udot)
+        else:
+            f_a = Matrix()
+        # Dicts to sub to zero, for splitting up expressions
+        u_zero = dict.fromkeys(self.u, 0)
+        ud_zero = dict.fromkeys(self._udot, 0)
+        qd_zero = dict.fromkeys(self._qdot, 0)
+        qd_u_zero = dict.fromkeys(Matrix([self._qdot, self.u]), 0)
+        # Break the kinematic differential eqs apart into f_0 and f_1
+        f_0 = msubs(self._f_k, u_zero) + self._k_kqdot*Matrix(self._qdot)
+        f_1 = msubs(self._f_k, qd_zero) + self._k_ku*Matrix(self.u)
+        # Break the dynamic differential eqs into f_2 and f_3
+        f_2 = msubs(self._frstar, qd_u_zero)
+        f_3 = msubs(self._frstar, ud_zero) + self._fr
+        f_4 = zeros(len(f_2), 1)
+
+        # Get the required vector components
+        q = self.q
+        u = self.u
+        if self._qdep:
+            q_i = q[:-len(self._qdep)]
+        else:
+            q_i = q
+        q_d = self._qdep
+        if self._udep:
+            u_i = u[:-len(self._udep)]
+        else:
+            u_i = u
+        u_d = self._udep
+
+        # Form dictionary to set auxiliary speeds & their derivatives to 0.
+        uaux = self._uaux
+        uauxdot = uaux.diff(dynamicsymbols._t)
+        uaux_zero = dict.fromkeys(Matrix([uaux, uauxdot]), 0)
+
+        # Checking for dynamic symbols outside the dynamic differential
+        # equations; throws error if there is.
+        sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
+        if any(find_dynamicsymbols(i, sym_list) for i in [self._k_kqdot,
+                self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]):
+            raise ValueError('Cannot have dynamicsymbols outside dynamic \
+                             forcing vector.')
+
+        # Find all other dynamic symbols, forming the forcing vector r.
+        # Sort r to make it canonical.
+        r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list))
+        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, linear_solver=linear_solver)
+
+    # TODO : Remove `new_method` after 1.1 has been released.
+    def linearize(self, *, new_method=None, linear_solver='LU', **kwargs):
+        """ Linearize the equations of motion about a symbolic operating point.
+
+        Parameters
+        ==========
+        new_method
+            Deprecated, does nothing and will be removed.
+        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(linear_solver=linear_solver)
+        result = linearizer.linearize(**kwargs)
+        return result + (linearizer.r,)
+
+    def kanes_equations(self, bodies=None, loads=None):
+        """ Method to form Kane's equations, Fr + Fr* = 0.
+
+        Explanation
+        ===========
+
+        Returns (Fr, Fr*). In the case where auxiliary generalized speeds are
+        present (say, s auxiliary speeds, o generalized speeds, and m motion
+        constraints) the length of the returned vectors will be o - m + s in
+        length. The first o - m equations will be the constrained Kane's
+        equations, then the s auxiliary Kane's equations. These auxiliary
+        equations can be accessed with the auxiliary_eqs property.
+
+        Parameters
+        ==========
+
+        bodies : iterable
+            An iterable of all RigidBody's and Particle's in the system.
+            A system must have at least one body.
+        loads : iterable
+            Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector)
+            tuples which represent the force at a point or torque on a frame.
+            Must be either a non-empty iterable of tuples or None which corresponds
+            to a system with no constraints.
+        """
+        if bodies is None:
+            bodies = self.bodies
+        if  loads is None and self._forcelist is not None:
+            loads = self._forcelist
+        if loads == []:
+            loads = None
+        if not self._k_kqdot:
+            raise AttributeError('Create an instance of KanesMethod with '
+                    'kinematic differential equations to use this method.')
+        fr = self._form_fr(loads)
+        frstar = self._form_frstar(bodies)
+        if self._uaux:
+            if not self._udep:
+                km = KanesMethod(self._inertial, self.q, self._uaux,
+                             u_auxiliary=self._uaux, constraint_solver=self._constraint_solver)
+            else:
+                km = KanesMethod(self._inertial, self.q, self._uaux,
+                        u_auxiliary=self._uaux, u_dependent=self._udep,
+                        velocity_constraints=(self._k_nh * self.u +
+                        self._f_nh),
+                        acceleration_constraints=(self._k_dnh * self._udot +
+                        self._f_dnh),
+                        constraint_solver=self._constraint_solver
+                        )
+            km._qdot_u_map = self._qdot_u_map
+            self._km = km
+            fraux = km._form_fr(loads)
+            frstaraux = km._form_frstar(bodies)
+            self._aux_eq = fraux + frstaraux
+            self._fr = fr.col_join(fraux)
+            self._frstar = frstar.col_join(frstaraux)
+        return (self._fr, self._frstar)
+
+    def _form_eoms(self):
+        fr, frstar = self.kanes_equations(self.bodylist, self.forcelist)
+        return fr + frstar
+
+    def rhs(self, inv_method=None):
+        """Returns the system's equations of motion in first order form. The
+        output is the right hand side of::
+
+           x' = |q'| =: f(q, u, r, p, t)
+                |u'|
+
+        The right hand side is what is needed by most numerical ODE
+        integrators.
+
+        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`
+
+        """
+        rhs = zeros(len(self.q) + len(self.u), 1)
+        kdes = self.kindiffdict()
+        for i, q_i in enumerate(self.q):
+            rhs[i] = kdes[q_i.diff()]
+
+        if inv_method is None:
+            rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing)
+        else:
+            rhs[len(self.q):, 0] = (self.mass_matrix.inv(inv_method,
+                                                         try_block_diag=True) *
+                                    self.forcing)
+
+        return rhs
+
+    def kindiffdict(self):
+        """Returns a dictionary mapping q' to u."""
+        if not self._qdot_u_map:
+            raise AttributeError('Create an instance of KanesMethod with '
+                    'kinematic differential equations to use this method.')
+        return self._qdot_u_map
+
+    @property
+    def auxiliary_eqs(self):
+        """A matrix containing the auxiliary equations."""
+        if not self._fr or not self._frstar:
+            raise ValueError('Need to compute Fr, Fr* first.')
+        if not self._uaux:
+            raise ValueError('No auxiliary speeds have been declared.')
+        return self._aux_eq
+
+    @property
+    def mass_matrix_kin(self):
+        r"""The kinematic "mass matrix" $\mathbf{k_{k\dot{q}}}$ of the system."""
+        return self._k_kqdot if self.explicit_kinematics else self._k_kqdot_implicit
+
+    @property
+    def forcing_kin(self):
+        """The kinematic "forcing vector" of the system."""
+        if self.explicit_kinematics:
+            return -(self._k_ku * Matrix(self.u) + self._f_k)
+        else:
+            return -(self._k_ku_implicit * Matrix(self.u) + self._f_k_implicit)
+
+    @property
+    def mass_matrix(self):
+        """The mass matrix of the system."""
+        if not self._fr or not self._frstar:
+            raise ValueError('Need to compute Fr, Fr* first.')
+        return Matrix([self._k_d, self._k_dnh])
+
+    @property
+    def forcing(self):
+        """The forcing vector of the system."""
+        if not self._fr or not self._frstar:
+            raise ValueError('Need to compute Fr, Fr* first.')
+        return -Matrix([self._f_d, self._f_dnh])
+
+    @property
+    def mass_matrix_full(self):
+        """The mass matrix of the system, augmented by the kinematic
+        differential equations in explicit or implicit form."""
+        if not self._fr or not self._frstar:
+            raise ValueError('Need to compute Fr, Fr* first.')
+        o, n = len(self.u), len(self.q)
+        return (self.mass_matrix_kin.row_join(zeros(n, o))).col_join(
+            zeros(o, n).row_join(self.mass_matrix))
+
+    @property
+    def forcing_full(self):
+        """The forcing vector of the system, augmented by the kinematic
+        differential equations in explicit or implicit form."""
+        return Matrix([self.forcing_kin, self.forcing])
+
+    @property
+    def q(self):
+        return self._q
+
+    @property
+    def u(self):
+        return self._u
+
+    @property
+    def bodylist(self):
+        return self._bodylist
+
+    @property
+    def forcelist(self):
+        return self._forcelist
+
+    @property
+    def bodies(self):
+        return self._bodylist
+
+    @property
+    def loads(self):
+        return self._forcelist

rl/Lib/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

rl/Lib/site-packages/sympy/physics/mechanics/linearize.py

@@ -0,0 +1,474 @@
+__all__ = ['Linearizer']
+
+from sympy import Matrix, eye, zeros
+from sympy.core.symbol import Dummy
+from sympy.utilities.iterables import flatten
+from sympy.physics.vector import dynamicsymbols
+from sympy.physics.mechanics.functions import msubs, _parse_linear_solver
+
+from collections import namedtuple
+from collections.abc import Iterable
+
+
+class Linearizer:
+    """This object holds the general model form for a dynamic system. This
+    model is used for computing the linearized form of the system, while
+    properly dealing with constraints leading to  dependent coordinates and
+    speeds. The notation and method is described in [1]_.
+
+    Attributes
+    ==========
+
+    f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix
+        Matrices holding the general system form.
+    q, u, r : Matrix
+        Matrices holding the generalized coordinates, speeds, and
+        input vectors.
+    q_i, u_i : Matrix
+        Matrices of the independent generalized coordinates and speeds.
+    q_d, u_d : Matrix
+        Matrices of the dependent generalized coordinates and speeds.
+    perm_mat : Matrix
+        Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T
+
+    References
+    ==========
+
+    .. [1] D. L. Peterson, G. Gede, and M. Hubbard, "Symbolic linearization of
+           equations of motion of constrained multibody systems," Multibody
+           Syst Dyn, vol. 33, no. 2, pp. 143-161, Feb. 2015, doi:
+           10.1007/s11044-014-9436-5.
+
+    """
+
+    def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None,
+                 q_d=None, u_i=None, u_d=None, r=None, lams=None,
+                 linear_solver='LU'):
+        """
+        Parameters
+        ==========
+
+        f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like
+            System of equations holding the general system form.
+            Supply empty array or Matrix if the parameter
+            does not exist.
+        q : array_like
+            The generalized coordinates.
+        u : array_like
+            The generalized speeds
+        q_i, u_i : array_like, optional
+            The independent generalized coordinates and speeds.
+        q_d, u_d : array_like, optional
+            The dependent generalized coordinates and speeds.
+        r : array_like, optional
+            The input variables.
+        lams : array_like, optional
+            The lagrange multipliers
+        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.
+
+        """
+        self.linear_solver = _parse_linear_solver(linear_solver)
+
+        # Generalized equation form
+        self.f_0 = Matrix(f_0)
+        self.f_1 = Matrix(f_1)
+        self.f_2 = Matrix(f_2)
+        self.f_3 = Matrix(f_3)
+        self.f_4 = Matrix(f_4)
+        self.f_c = Matrix(f_c)
+        self.f_v = Matrix(f_v)
+        self.f_a = Matrix(f_a)
+
+        # Generalized equation variables
+        self.q = Matrix(q)
+        self.u = Matrix(u)
+        none_handler = lambda x: Matrix(x) if x else Matrix()
+        self.q_i = none_handler(q_i)
+        self.q_d = none_handler(q_d)
+        self.u_i = none_handler(u_i)
+        self.u_d = none_handler(u_d)
+        self.r = none_handler(r)
+        self.lams = none_handler(lams)
+
+        # Derivatives of generalized equation variables
+        self._qd = self.q.diff(dynamicsymbols._t)
+        self._ud = self.u.diff(dynamicsymbols._t)
+        # If the user doesn't actually use generalized variables, and the
+        # qd and u vectors have any intersecting variables, this can cause
+        # problems. We'll fix this with some hackery, and Dummy variables
+        dup_vars = set(self._qd).intersection(self.u)
+        self._qd_dup = Matrix([var if var not in dup_vars else Dummy() for var
+                               in self._qd])
+
+        # Derive dimesion terms
+        l = len(self.f_c)
+        m = len(self.f_v)
+        n = len(self.q)
+        o = len(self.u)
+        s = len(self.r)
+        k = len(self.lams)
+        dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k'])
+        self._dims = dims(l, m, n, o, s, k)
+
+        self._Pq = None
+        self._Pqi = None
+        self._Pqd = None
+        self._Pu = None
+        self._Pui = None
+        self._Pud = None
+        self._C_0 = None
+        self._C_1 = None
+        self._C_2 = None
+        self.perm_mat = None
+
+        self._setup_done = False
+
+    def _setup(self):
+        # Calculations here only need to be run once. They are moved out of
+        # the __init__ method to increase the speed of Linearizer creation.
+        self._form_permutation_matrices()
+        self._form_block_matrices()
+        self._form_coefficient_matrices()
+        self._setup_done = True
+
+    def _form_permutation_matrices(self):
+        """Form the permutation matrices Pq and Pu."""
+
+        # Extract dimension variables
+        l, m, n, o, s, k = self._dims
+        # Compute permutation matrices
+        if n != 0:
+            self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
+            if l > 0:
+                self._Pqi = self._Pq[:, :-l]
+                self._Pqd = self._Pq[:, -l:]
+            else:
+                self._Pqi = self._Pq
+                self._Pqd = Matrix()
+        if o != 0:
+            self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
+            if m > 0:
+                self._Pui = self._Pu[:, :-m]
+                self._Pud = self._Pu[:, -m:]
+            else:
+                self._Pui = self._Pu
+                self._Pud = Matrix()
+        # Compute combination permutation matrix for computing A and B
+        P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
+        P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
+        if P_col1:
+            if P_col2:
+                self.perm_mat = P_col1.row_join(P_col2)
+            else:
+                self.perm_mat = P_col1
+        else:
+            self.perm_mat = P_col2
+
+    def _form_coefficient_matrices(self):
+        """Form the coefficient matrices C_0, C_1, and C_2."""
+
+        # Extract dimension variables
+        l, m, n, o, s, k = self._dims
+        # Build up the coefficient matrices C_0, C_1, and C_2
+        # If there are configuration constraints (l > 0), form C_0 as normal.
+        # If not, C_0 is I_(nxn). Note that this works even if n=0
+        if l > 0:
+            f_c_jac_q = self.f_c.jacobian(self.q)
+            self._C_0 = (eye(n) - self._Pqd *
+                         self.linear_solver(f_c_jac_q*self._Pqd,
+                                            f_c_jac_q))*self._Pqi
+        else:
+            self._C_0 = eye(n)
+        # If there are motion constraints (m > 0), form C_1 and C_2 as normal.
+        # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
+        # o = 0.
+        if m > 0:
+            f_v_jac_u = self.f_v.jacobian(self.u)
+            temp = f_v_jac_u * self._Pud
+            if n != 0:
+                f_v_jac_q = self.f_v.jacobian(self.q)
+                self._C_1 = -self._Pud * self.linear_solver(temp, f_v_jac_q)
+            else:
+                self._C_1 = zeros(o, n)
+            self._C_2 = (eye(o) - self._Pud *
+                         self.linear_solver(temp, f_v_jac_u))*self._Pui
+        else:
+            self._C_1 = zeros(o, n)
+            self._C_2 = eye(o)
+
+    def _form_block_matrices(self):
+        """Form the block matrices for composing M, A, and B."""
+
+        # Extract dimension variables
+        l, m, n, o, s, k = self._dims
+        # Block Matrix Definitions. These are only defined if under certain
+        # conditions. If undefined, an empty matrix is used instead
+        if n != 0:
+            self._M_qq = self.f_0.jacobian(self._qd)
+            self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q)
+        else:
+            self._M_qq = Matrix()
+            self._A_qq = Matrix()
+        if n != 0 and m != 0:
+            self._M_uqc = self.f_a.jacobian(self._qd_dup)
+            self._A_uqc = -self.f_a.jacobian(self.q)
+        else:
+            self._M_uqc = Matrix()
+            self._A_uqc = Matrix()
+        if n != 0 and o - m + k != 0:
+            self._M_uqd = self.f_3.jacobian(self._qd_dup)
+            self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q)
+        else:
+            self._M_uqd = Matrix()
+            self._A_uqd = Matrix()
+        if o != 0 and m != 0:
+            self._M_uuc = self.f_a.jacobian(self._ud)
+            self._A_uuc = -self.f_a.jacobian(self.u)
+        else:
+            self._M_uuc = Matrix()
+            self._A_uuc = Matrix()
+        if o != 0 and o - m + k != 0:
+            self._M_uud = self.f_2.jacobian(self._ud)
+            self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u)
+        else:
+            self._M_uud = Matrix()
+            self._A_uud = Matrix()
+        if o != 0 and n != 0:
+            self._A_qu = -self.f_1.jacobian(self.u)
+        else:
+            self._A_qu = Matrix()
+        if k != 0 and o - m + k != 0:
+            self._M_uld = self.f_4.jacobian(self.lams)
+        else:
+            self._M_uld = Matrix()
+        if s != 0 and o - m + k != 0:
+            self._B_u = -self.f_3.jacobian(self.r)
+        else:
+            self._B_u = Matrix()
+
+    def linearize(self, op_point=None, A_and_B=False, simplify=False):
+        """Linearize the system about the operating point. Note that
+        q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
+        These may be either symbolic or numeric.
+
+        Parameters
+        ==========
+        op_point : dict or iterable of dicts, optional
+            Dictionary or iterable of dictionaries containing the operating
+            point conditions for all or a subset of the generalized
+            coordinates, generalized speeds, and time derivatives of the
+            generalized speeds. These will be substituted into the linearized
+            system before the linearization is complete. Leave set to ``None``
+            if you want the operating point to be an arbitrary set of symbols.
+            Note that any reduction in symbols (whether substituted for numbers
+            or expressions with a common parameter) will result in faster
+            runtime.
+        A_and_B : bool, optional
+            If A_and_B=False (default), (M, A, B) is returned and of
+            A_and_B=True, (A, B) is returned. See below.
+        simplify : bool, optional
+            Determines if returned values are simplified before return.
+            For large expressions this may be time consuming. Default is False.
+
+        Returns
+        =======
+        M, A, B : Matrices, ``A_and_B=False``
+            Matrices from the implicit form:
+                ``[M]*[q', u']^T = [A]*[q_ind, u_ind]^T + [B]*r``
+        A, B : Matrices, ``A_and_B=True``
+            Matrices from the explicit form:
+                ``[q_ind', u_ind']^T = [A]*[q_ind, u_ind]^T + [B]*r``
+
+        Notes
+        =====
+
+        Note that the process of solving with A_and_B=True 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. More values may then be substituted in to these matrices later
+        on. The state space form can then be found as A = P.T*M.LUsolve(A), B =
+        P.T*M.LUsolve(B), where P = Linearizer.perm_mat.
+
+        """
+
+        # Run the setup if needed:
+        if not self._setup_done:
+            self._setup()
+
+        # Compose dict of operating conditions
+        if isinstance(op_point, dict):
+            op_point_dict = op_point
+        elif isinstance(op_point, Iterable):
+            op_point_dict = {}
+            for op in op_point:
+                op_point_dict.update(op)
+        else:
+            op_point_dict = {}
+
+        # Extract dimension variables
+        l, m, n, o, s, k = self._dims
+
+        # Rename terms to shorten expressions
+        M_qq = self._M_qq
+        M_uqc = self._M_uqc
+        M_uqd = self._M_uqd
+        M_uuc = self._M_uuc
+        M_uud = self._M_uud
+        M_uld = self._M_uld
+        A_qq = self._A_qq
+        A_uqc = self._A_uqc
+        A_uqd = self._A_uqd
+        A_qu = self._A_qu
+        A_uuc = self._A_uuc
+        A_uud = self._A_uud
+        B_u = self._B_u
+        C_0 = self._C_0
+        C_1 = self._C_1
+        C_2 = self._C_2
+
+        # Build up Mass Matrix
+        #     |M_qq    0_nxo   0_nxk|
+        # M = |M_uqc   M_uuc   0_mxk|
+        #     |M_uqd   M_uud   M_uld|
+        if o != 0:
+            col2 = Matrix([zeros(n, o), M_uuc, M_uud])
+        if k != 0:
+            col3 = Matrix([zeros(n + m, k), M_uld])
+        if n != 0:
+            col1 = Matrix([M_qq, M_uqc, M_uqd])
+            if o != 0 and k != 0:
+                M = col1.row_join(col2).row_join(col3)
+            elif o != 0:
+                M = col1.row_join(col2)
+            else:
+                M = col1
+        elif k != 0:
+            M = col2.row_join(col3)
+        else:
+            M = col2
+        M_eq = msubs(M, op_point_dict)
+
+        # Build up state coefficient matrix A
+        #     |(A_qq + A_qu*C_1)*C_0       A_qu*C_2|
+        # A = |(A_uqc + A_uuc*C_1)*C_0    A_uuc*C_2|
+        #     |(A_uqd + A_uud*C_1)*C_0    A_uud*C_2|
+        # Col 1 is only defined if n != 0
+        if n != 0:
+            r1c1 = A_qq
+            if o != 0:
+                r1c1 += (A_qu * C_1)
+            r1c1 = r1c1 * C_0
+            if m != 0:
+                r2c1 = A_uqc
+                if o != 0:
+                    r2c1 += (A_uuc * C_1)
+                r2c1 = r2c1 * C_0
+            else:
+                r2c1 = Matrix()
+            if o - m + k != 0:
+                r3c1 = A_uqd
+                if o != 0:
+                    r3c1 += (A_uud * C_1)
+                r3c1 = r3c1 * C_0
+            else:
+                r3c1 = Matrix()
+            col1 = Matrix([r1c1, r2c1, r3c1])
+        else:
+            col1 = Matrix()
+        # Col 2 is only defined if o != 0
+        if o != 0:
+            if n != 0:
+                r1c2 = A_qu * C_2
+            else:
+                r1c2 = Matrix()
+            if m != 0:
+                r2c2 = A_uuc * C_2
+            else:
+                r2c2 = Matrix()
+            if o - m + k != 0:
+                r3c2 = A_uud * C_2
+            else:
+                r3c2 = Matrix()
+            col2 = Matrix([r1c2, r2c2, r3c2])
+        else:
+            col2 = Matrix()
+        if col1:
+            if col2:
+                Amat = col1.row_join(col2)
+            else:
+                Amat = col1
+        else:
+            Amat = col2
+        Amat_eq = msubs(Amat, op_point_dict)
+
+        # Build up the B matrix if there are forcing variables
+        #     |0_(n + m)xs|
+        # B = |B_u        |
+        if s != 0 and o - m + k != 0:
+            Bmat = zeros(n + m, s).col_join(B_u)
+            Bmat_eq = msubs(Bmat, op_point_dict)
+        else:
+            Bmat_eq = Matrix()
+
+        # kwarg A_and_B indicates to return  A, B for forming the equation
+        # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
+        if A_and_B:
+            A_cont = self.perm_mat.T * self.linear_solver(M_eq, Amat_eq)
+            if Bmat_eq:
+                B_cont = self.perm_mat.T * self.linear_solver(M_eq, Bmat_eq)
+            else:
+                # Bmat = Matrix([]), so no need to sub
+                B_cont = Bmat_eq
+            if simplify:
+                A_cont.simplify()
+                B_cont.simplify()
+            return A_cont, B_cont
+        # Otherwise return M, A, B for forming the equation
+        # [M]dx = [A]x + [B]r, where x = [q, u]^T
+        else:
+            if simplify:
+                M_eq.simplify()
+                Amat_eq.simplify()
+                Bmat_eq.simplify()
+            return M_eq, Amat_eq, Bmat_eq
+
+
+def permutation_matrix(orig_vec, per_vec):
+    """Compute the permutation matrix to change order of
+    orig_vec into order of per_vec.
+
+    Parameters
+    ==========
+
+    orig_vec : array_like
+        Symbols in original ordering.
+    per_vec : array_like
+        Symbols in new ordering.
+
+    Returns
+    =======
+
+    p_matrix : Matrix
+        Permutation matrix such that orig_vec == (p_matrix * per_vec).
+    """
+    if not isinstance(orig_vec, (list, tuple)):
+        orig_vec = flatten(orig_vec)
+    if not isinstance(per_vec, (list, tuple)):
+        per_vec = flatten(per_vec)
+    if set(orig_vec) != set(per_vec):
+        raise ValueError("orig_vec and per_vec must be the same length, "
+                         "and contain the same symbols.")
+    ind_list = [orig_vec.index(i) for i in per_vec]
+    p_matrix = zeros(len(orig_vec))
+    for i, j in enumerate(ind_list):
+        p_matrix[i, j] = 1
+    return p_matrix

rl/Lib/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.')

rl/Lib/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.")

rl/Lib/site-packages/sympy/physics/mechanics/models.py

@@ -0,0 +1,230 @@
+#!/usr/bin/env python
+"""This module contains some sample symbolic models used for testing and
+examples."""
+
+# Internal imports
+from sympy.core import backend as sm
+import sympy.physics.mechanics as me
+
+
+def multi_mass_spring_damper(n=1, apply_gravity=False,
+                             apply_external_forces=False):
+    r"""Returns a system containing the symbolic equations of motion and
+    associated variables for a simple multi-degree of freedom point mass,
+    spring, damper system with optional gravitational and external
+    specified forces. For example, a two mass system under the influence of
+    gravity and external forces looks like:
+
+    ::
+
+        ----------------
+         |     |     |   | g
+         \    | |    |   V
+      k0 /    --- c0 |
+         |     |     | x0, v0
+        ---------    V
+        |  m0   | -----
+        ---------    |
+         | |   |     |
+         \ v  | |    |
+      k1 / f0 --- c1 |
+         |     |     | x1, v1
+        ---------    V
+        |  m1   | -----
+        ---------
+           | f1
+           V
+
+    Parameters
+    ==========
+
+    n : integer
+        The number of masses in the serial chain.
+    apply_gravity : boolean
+        If true, gravity will be applied to each mass.
+    apply_external_forces : boolean
+        If true, a time varying external force will be applied to each mass.
+
+    Returns
+    =======
+
+    kane : sympy.physics.mechanics.kane.KanesMethod
+        A KanesMethod object.
+
+    """
+
+    mass = sm.symbols('m:{}'.format(n))
+    stiffness = sm.symbols('k:{}'.format(n))
+    damping = sm.symbols('c:{}'.format(n))
+
+    acceleration_due_to_gravity = sm.symbols('g')
+
+    coordinates = me.dynamicsymbols('x:{}'.format(n))
+    speeds = me.dynamicsymbols('v:{}'.format(n))
+    specifieds = me.dynamicsymbols('f:{}'.format(n))
+
+    ceiling = me.ReferenceFrame('N')
+    origin = me.Point('origin')
+    origin.set_vel(ceiling, 0)
+
+    points = [origin]
+    kinematic_equations = []
+    particles = []
+    forces = []
+
+    for i in range(n):
+
+        center = points[-1].locatenew('center{}'.format(i),
+                                      coordinates[i] * ceiling.x)
+        center.set_vel(ceiling, points[-1].vel(ceiling) +
+                       speeds[i] * ceiling.x)
+        points.append(center)
+
+        block = me.Particle('block{}'.format(i), center, mass[i])
+
+        kinematic_equations.append(speeds[i] - coordinates[i].diff())
+
+        total_force = (-stiffness[i] * coordinates[i] -
+                       damping[i] * speeds[i])
+        try:
+            total_force += (stiffness[i + 1] * coordinates[i + 1] +
+                            damping[i + 1] * speeds[i + 1])
+        except IndexError:  # no force from below on last mass
+            pass
+
+        if apply_gravity:
+            total_force += mass[i] * acceleration_due_to_gravity
+
+        if apply_external_forces:
+            total_force += specifieds[i]
+
+        forces.append((center, total_force * ceiling.x))
+
+        particles.append(block)
+
+    kane = me.KanesMethod(ceiling, q_ind=coordinates, u_ind=speeds,
+                          kd_eqs=kinematic_equations)
+    kane.kanes_equations(particles, forces)
+
+    return kane
+
+
+def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
+    r"""Returns the system containing the symbolic first order equations of
+    motion for a 2D n-link pendulum on a sliding cart under the influence of
+    gravity.
+
+    ::
+
+                  |
+         o    y   v
+          \ 0 ^   g
+           \  |
+          --\-|----
+          |  \|   |
+      F-> |   o --|---> x
+          |       |
+          ---------
+           o     o
+
+    Parameters
+    ==========
+
+    n : integer
+        The number of links in the pendulum.
+    cart_force : boolean, default=True
+        If true an external specified lateral force is applied to the cart.
+    joint_torques : boolean, default=False
+        If true joint torques will be added as specified inputs at each
+        joint.
+
+    Returns
+    =======
+
+    kane : sympy.physics.mechanics.kane.KanesMethod
+        A KanesMethod object.
+
+    Notes
+    =====
+
+    The degrees of freedom of the system are n + 1, i.e. one for each
+    pendulum link and one for the lateral motion of the cart.
+
+    M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
+
+    The joint angles are all defined relative to the ground where the x axis
+    defines the ground line and the y axis points up. The joint torques are
+    applied between each adjacent link and the between the cart and the
+    lower link where a positive torque corresponds to positive angle.
+
+    """
+    if n <= 0:
+        raise ValueError('The number of links must be a positive integer.')
+
+    q = me.dynamicsymbols('q:{}'.format(n + 1))
+    u = me.dynamicsymbols('u:{}'.format(n + 1))
+
+    if joint_torques is True:
+        T = me.dynamicsymbols('T1:{}'.format(n + 1))
+
+    m = sm.symbols('m:{}'.format(n + 1))
+    l = sm.symbols('l:{}'.format(n))
+    g, t = sm.symbols('g t')
+
+    I = me.ReferenceFrame('I')
+    O = me.Point('O')
+    O.set_vel(I, 0)
+
+    P0 = me.Point('P0')
+    P0.set_pos(O, q[0] * I.x)
+    P0.set_vel(I, u[0] * I.x)
+    Pa0 = me.Particle('Pa0', P0, m[0])
+
+    frames = [I]
+    points = [P0]
+    particles = [Pa0]
+    forces = [(P0, -m[0] * g * I.y)]
+    kindiffs = [q[0].diff(t) - u[0]]
+
+    if cart_force is True or joint_torques is True:
+        specified = []
+    else:
+        specified = None
+
+    for i in range(n):
+        Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
+        Bi.set_ang_vel(I, u[i + 1] * I.z)
+        frames.append(Bi)
+
+        Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
+        Pi.v2pt_theory(points[-1], I, Bi)
+        points.append(Pi)
+
+        Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
+        particles.append(Pai)
+
+        forces.append((Pi, -m[i + 1] * g * I.y))
+
+        if joint_torques is True:
+
+            specified.append(T[i])
+
+            if i == 0:
+                forces.append((I, -T[i] * I.z))
+
+            if i == n - 1:
+                forces.append((Bi, T[i] * I.z))
+            else:
+                forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
+
+        kindiffs.append(q[i + 1].diff(t) - u[i + 1])
+
+    if cart_force is True:
+        F = me.dynamicsymbols('F')
+        forces.append((P0, F * I.x))
+        specified.append(F)
+
+    kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
+    kane.kanes_equations(particles, forces)
+
+    return kane

rl/Lib/site-packages/sympy/physics/mechanics/particle.py

@@ -0,0 +1,209 @@
+from sympy import S
+from sympy.physics.vector import cross, dot
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.physics.mechanics.inertia import inertia_of_point_mass
+from sympy.utilities.exceptions import sympy_deprecation_warning
+
+__all__ = ['Particle']
+
+
+class Particle(BodyBase):
+    """A particle.
+
+    Explanation
+    ===========
+
+    Particles have a non-zero mass and lack spatial extension; they take up no
+    space.
+
+    Values need to be supplied on initialization, but can be changed later.
+
+    Parameters
+    ==========
+
+    name : str
+        Name of particle
+    point : Point
+        A physics/mechanics Point which represents the position, velocity, and
+        acceleration of this Particle
+    mass : Sympifyable
+        A SymPy expression representing the Particle's mass
+    potential_energy : Sympifyable
+        The potential energy of the Particle.
+
+    Examples
+    ========
+
+    >>> from sympy.physics.mechanics import Particle, Point
+    >>> from sympy import Symbol
+    >>> po = Point('po')
+    >>> m = Symbol('m')
+    >>> pa = Particle('pa', po, m)
+    >>> # Or you could change these later
+    >>> pa.mass = m
+    >>> pa.point = po
+
+    """
+    point = BodyBase.masscenter
+
+    def __init__(self, name, point=None, mass=None):
+        super().__init__(name, point, mass)
+
+    def linear_momentum(self, frame):
+        """Linear momentum of the particle.
+
+        Explanation
+        ===========
+
+        The linear momentum L, of a particle P, with respect to frame N is
+        given by:
+
+        L = m * v
+
+        where m is the mass of the particle, and v is the velocity of the
+        particle in the frame N.
+
+        Parameters
+        ==========
+
+        frame : ReferenceFrame
+            The frame in which linear momentum is desired.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
+        >>> from sympy.physics.mechanics import dynamicsymbols
+        >>> from sympy.physics.vector import init_vprinting
+        >>> init_vprinting(pretty_print=False)
+        >>> m, v = dynamicsymbols('m v')
+        >>> N = ReferenceFrame('N')
+        >>> P = Point('P')
+        >>> A = Particle('A', P, m)
+        >>> P.set_vel(N, v * N.x)
+        >>> A.linear_momentum(N)
+        m*v*N.x
+
+        """
+
+        return self.mass * self.point.vel(frame)
+
+    def angular_momentum(self, point, frame):
+        """Angular momentum of the particle about the point.
+
+        Explanation
+        ===========
+
+        The angular momentum H, about some point O of a particle, P, is given
+        by:
+
+        ``H = cross(r, m * v)``
+
+        where r is the position vector from point O to the particle P, m is
+        the mass of the particle, and v is the velocity of the particle in
+        the inertial frame, N.
+
+        Parameters
+        ==========
+
+        point : Point
+            The point about which angular momentum of the particle is desired.
+
+        frame : ReferenceFrame
+            The frame in which angular momentum is desired.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
+        >>> from sympy.physics.mechanics import dynamicsymbols
+        >>> from sympy.physics.vector import init_vprinting
+        >>> init_vprinting(pretty_print=False)
+        >>> m, v, r = dynamicsymbols('m v r')
+        >>> N = ReferenceFrame('N')
+        >>> O = Point('O')
+        >>> A = O.locatenew('A', r * N.x)
+        >>> P = Particle('P', A, m)
+        >>> P.point.set_vel(N, v * N.y)
+        >>> P.angular_momentum(O, N)
+        m*r*v*N.z
+
+        """
+
+        return cross(self.point.pos_from(point),
+                     self.mass * self.point.vel(frame))
+
+    def kinetic_energy(self, frame):
+        """Kinetic energy of the particle.
+
+        Explanation
+        ===========
+
+        The kinetic energy, T, of a particle, P, is given by:
+
+        ``T = 1/2 (dot(m * v, v))``
+
+        where m is the mass of particle P, and v is the velocity of the
+        particle in the supplied ReferenceFrame.
+
+        Parameters
+        ==========
+
+        frame : ReferenceFrame
+            The Particle's velocity is typically defined with respect to
+            an inertial frame but any relevant frame in which the velocity is
+            known can be supplied.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
+        >>> from sympy import symbols
+        >>> m, v, r = symbols('m v r')
+        >>> N = ReferenceFrame('N')
+        >>> O = Point('O')
+        >>> P = Particle('P', O, m)
+        >>> P.point.set_vel(N, v * N.y)
+        >>> P.kinetic_energy(N)
+        m*v**2/2
+
+        """
+
+        return S.Half * self.mass * dot(self.point.vel(frame),
+                                        self.point.vel(frame))
+
+    def set_potential_energy(self, scalar):
+        sympy_deprecation_warning(
+            """
+The sympy.physics.mechanics.Particle.set_potential_energy()
+method is deprecated. Instead use
+
+    P.potential_energy = scalar
+            """,
+        deprecated_since_version="1.5",
+        active_deprecations_target="deprecated-set-potential-energy",
+        )
+        self.potential_energy = scalar
+
+    def parallel_axis(self, point, frame):
+        """Returns an inertia dyadic of the particle with respect to another
+        point and frame.
+
+        Parameters
+        ==========
+
+        point : sympy.physics.vector.Point
+            The point to express the inertia dyadic about.
+        frame : sympy.physics.vector.ReferenceFrame
+            The reference frame used to construct the dyadic.
+
+        Returns
+        =======
+
+        inertia : sympy.physics.vector.Dyadic
+            The inertia dyadic of the particle expressed about the provided
+            point and frame.
+
+        """
+        return inertia_of_point_mass(self.mass, self.point.pos_from(point),
+                                     frame)

rl/Lib/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)

rl/Lib/site-packages/sympy/physics/mechanics/rigidbody.py

@@ -0,0 +1,314 @@
+from sympy import Symbol, S
+from sympy.physics.vector import ReferenceFrame, Dyadic, Point, dot
+from sympy.physics.mechanics.body_base import BodyBase
+from sympy.physics.mechanics.inertia import inertia_of_point_mass, Inertia
+from sympy.utilities.exceptions import sympy_deprecation_warning
+
+__all__ = ['RigidBody']
+
+
+class RigidBody(BodyBase):
+    """An idealized rigid body.
+
+    Explanation
+    ===========
+
+    This is essentially a container which holds the various components which
+    describe a rigid body: a name, mass, center of mass, reference frame, and
+    inertia.
+
+    All of these need to be supplied on creation, but can be changed
+    afterwards.
+
+    Attributes
+    ==========
+
+    name : string
+        The body's name.
+    masscenter : Point
+        The point which represents the center of mass of the rigid body.
+    frame : ReferenceFrame
+        The ReferenceFrame which the rigid body is fixed in.
+    mass : Sympifyable
+        The body's mass.
+    inertia : (Dyadic, Point)
+        The body's inertia about a point; stored in a tuple as shown above.
+    potential_energy : Sympifyable
+        The potential energy of the RigidBody.
+
+    Examples
+    ========
+
+    >>> from sympy import Symbol
+    >>> from sympy.physics.mechanics import ReferenceFrame, Point, RigidBody
+    >>> from sympy.physics.mechanics import outer
+    >>> m = Symbol('m')
+    >>> A = ReferenceFrame('A')
+    >>> P = Point('P')
+    >>> I = outer (A.x, A.x)
+    >>> inertia_tuple = (I, P)
+    >>> B = RigidBody('B', P, A, m, inertia_tuple)
+    >>> # Or you could change them afterwards
+    >>> m2 = Symbol('m2')
+    >>> B.mass = m2
+
+    """
+
+    def __init__(self, name, masscenter=None, frame=None, mass=None,
+                 inertia=None):
+        super().__init__(name, masscenter, mass)
+        if frame is None:
+            frame = ReferenceFrame(f'{name}_frame')
+        self.frame = frame
+        if inertia is None:
+            ixx = Symbol(f'{name}_ixx')
+            iyy = Symbol(f'{name}_iyy')
+            izz = Symbol(f'{name}_izz')
+            izx = Symbol(f'{name}_izx')
+            ixy = Symbol(f'{name}_ixy')
+            iyz = Symbol(f'{name}_iyz')
+            inertia = Inertia.from_inertia_scalars(self.masscenter, self.frame,
+                                                   ixx, iyy, izz, ixy, iyz, izx)
+        self.inertia = inertia
+
+    def __repr__(self):
+        return (f'{self.__class__.__name__}({repr(self.name)}, masscenter='
+                f'{repr(self.masscenter)}, frame={repr(self.frame)}, mass='
+                f'{repr(self.mass)}, inertia={repr(self.inertia)})')
+
+    @property
+    def frame(self):
+        """The ReferenceFrame fixed to the body."""
+        return self._frame
+
+    @frame.setter
+    def frame(self, F):
+        if not isinstance(F, ReferenceFrame):
+            raise TypeError("RigidBody frame must be a ReferenceFrame object.")
+        self._frame = F
+
+    @property
+    def x(self):
+        """The basis Vector for the body, in the x direction. """
+        return self.frame.x
+
+    @property
+    def y(self):
+        """The basis Vector for the body, in the y direction. """
+        return self.frame.y
+
+    @property
+    def z(self):
+        """The basis Vector for the body, in the z direction. """
+        return self.frame.z
+
+    @property
+    def inertia(self):
+        """The body's inertia about a point; stored as (Dyadic, Point)."""
+        return self._inertia
+
+    @inertia.setter
+    def inertia(self, I):
+        # check if I is of the form (Dyadic, Point)
+        if len(I) != 2 or not isinstance(I[0], Dyadic) or not isinstance(I[1], Point):
+            raise TypeError("RigidBody inertia must be a tuple of the form (Dyadic, Point).")
+
+        self._inertia = Inertia(I[0], I[1])
+        # have I S/O, want I S/S*
+        # I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O
+        # I_S/S* = I_S/O - I_S*/O
+        I_Ss_O = inertia_of_point_mass(self.mass,
+                                       self.masscenter.pos_from(I[1]),
+                                       self.frame)
+        self._central_inertia = I[0] - I_Ss_O
+
+    @property
+    def central_inertia(self):
+        """The body's central inertia dyadic."""
+        return self._central_inertia
+
+    @central_inertia.setter
+    def central_inertia(self, I):
+        if not isinstance(I, Dyadic):
+            raise TypeError("RigidBody inertia must be a Dyadic object.")
+        self.inertia = Inertia(I, self.masscenter)
+
+    def linear_momentum(self, frame):
+        """ Linear momentum of the rigid body.
+
+        Explanation
+        ===========
+
+        The linear momentum L, of a rigid body B, with respect to frame N is
+        given by:
+
+        ``L = m * v``
+
+        where m is the mass of the rigid body, and v is the velocity of the mass
+        center of B in the frame N.
+
+        Parameters
+        ==========
+
+        frame : ReferenceFrame
+            The frame in which linear momentum is desired.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
+        >>> from sympy.physics.mechanics import RigidBody, dynamicsymbols
+        >>> from sympy.physics.vector import init_vprinting
+        >>> init_vprinting(pretty_print=False)
+        >>> m, v = dynamicsymbols('m v')
+        >>> N = ReferenceFrame('N')
+        >>> P = Point('P')
+        >>> P.set_vel(N, v * N.x)
+        >>> I = outer (N.x, N.x)
+        >>> Inertia_tuple = (I, P)
+        >>> B = RigidBody('B', P, N, m, Inertia_tuple)
+        >>> B.linear_momentum(N)
+        m*v*N.x
+
+        """
+
+        return self.mass * self.masscenter.vel(frame)
+
+    def angular_momentum(self, point, frame):
+        """Returns the angular momentum of the rigid body about a point in the
+        given frame.
+
+        Explanation
+        ===========
+
+        The angular momentum H of a rigid body B about some point O in a frame N
+        is given by:
+
+        ``H = dot(I, w) + cross(r, m * v)``
+
+        where I and m are the central inertia dyadic and mass of rigid body B, w
+        is the angular velocity of body B in the frame N, r is the position
+        vector from point O to the mass center of B, and v is the velocity of
+        the mass center in the frame N.
+
+        Parameters
+        ==========
+
+        point : Point
+            The point about which angular momentum is desired.
+        frame : ReferenceFrame
+            The frame in which angular momentum is desired.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
+        >>> from sympy.physics.mechanics import RigidBody, dynamicsymbols
+        >>> from sympy.physics.vector import init_vprinting
+        >>> init_vprinting(pretty_print=False)
+        >>> m, v, r, omega = dynamicsymbols('m v r omega')
+        >>> N = ReferenceFrame('N')
+        >>> b = ReferenceFrame('b')
+        >>> b.set_ang_vel(N, omega * b.x)
+        >>> P = Point('P')
+        >>> P.set_vel(N, 1 * N.x)
+        >>> I = outer(b.x, b.x)
+        >>> B = RigidBody('B', P, b, m, (I, P))
+        >>> B.angular_momentum(P, N)
+        omega*b.x
+
+        """
+        I = self.central_inertia
+        w = self.frame.ang_vel_in(frame)
+        m = self.mass
+        r = self.masscenter.pos_from(point)
+        v = self.masscenter.vel(frame)
+
+        return I.dot(w) + r.cross(m * v)
+
+    def kinetic_energy(self, frame):
+        """Kinetic energy of the rigid body.
+
+        Explanation
+        ===========
+
+        The kinetic energy, T, of a rigid body, B, is given by:
+
+        ``T = 1/2 * (dot(dot(I, w), w) + dot(m * v, v))``
+
+        where I and m are the central inertia dyadic and mass of rigid body B
+        respectively, w is the body's angular velocity, and v is the velocity of
+        the body's mass center in the supplied ReferenceFrame.
+
+        Parameters
+        ==========
+
+        frame : ReferenceFrame
+            The RigidBody's angular velocity and the velocity of it's mass
+            center are typically defined with respect to an inertial frame but
+            any relevant frame in which the velocities are known can be
+            supplied.
+
+        Examples
+        ========
+
+        >>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
+        >>> from sympy.physics.mechanics import RigidBody
+        >>> from sympy import symbols
+        >>> m, v, r, omega = symbols('m v r omega')
+        >>> N = ReferenceFrame('N')
+        >>> b = ReferenceFrame('b')
+        >>> b.set_ang_vel(N, omega * b.x)
+        >>> P = Point('P')
+        >>> P.set_vel(N, v * N.x)
+        >>> I = outer (b.x, b.x)
+        >>> inertia_tuple = (I, P)
+        >>> B = RigidBody('B', P, b, m, inertia_tuple)
+        >>> B.kinetic_energy(N)
+        m*v**2/2 + omega**2/2
+
+        """
+
+        rotational_KE = S.Half * dot(
+            self.frame.ang_vel_in(frame),
+            dot(self.central_inertia, self.frame.ang_vel_in(frame)))
+        translational_KE = S.Half * self.mass * dot(self.masscenter.vel(frame),
+                                                    self.masscenter.vel(frame))
+        return rotational_KE + translational_KE
+
+    def set_potential_energy(self, scalar):
+        sympy_deprecation_warning(
+            """
+The sympy.physics.mechanics.RigidBody.set_potential_energy()
+method is deprecated. Instead use
+
+    B.potential_energy = scalar
+            """,
+        deprecated_since_version="1.5",
+        active_deprecations_target="deprecated-set-potential-energy",
+        )
+        self.potential_energy = scalar
+
+    def parallel_axis(self, point, frame=None):
+        """Returns the inertia dyadic of the body with respect to another point.
+
+        Parameters
+        ==========
+
+        point : sympy.physics.vector.Point
+            The point to express the inertia dyadic about.
+        frame : sympy.physics.vector.ReferenceFrame
+            The reference frame used to construct the dyadic.
+
+        Returns
+        =======
+
+        inertia : sympy.physics.vector.Dyadic
+            The inertia dyadic of the rigid body expressed about the provided
+            point.
+
+        """
+        if frame is None:
+            frame = self.frame
+        return self.central_inertia + inertia_of_point_mass(
+            self.mass, self.masscenter.pos_from(point), frame)

rl/Lib/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}"

rl/Lib/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

rl/Lib/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)]

rl/Lib/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)

rl/Lib/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))

rl/Lib/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)

rl/Lib/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()))

rl/Lib/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)

rl/Lib/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.

rl/Lib/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))

rl/Lib/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)

rl/Lib/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

rl/Lib/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

rl/Lib/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]))

rl/Lib/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())

rl/Lib/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])

rl/Lib/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)

rl/Lib/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

rl/Lib/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)

rl/Lib/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

rl/Lib/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)

rl/Lib/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)

rl/Lib/site-packages/sympy/physics/mechanics/wrapping_geometry.py

@@ -0,0 +1,641 @@
+"""Geometry objects for use by wrapping pathways."""
+
+from abc import ABC, abstractmethod
+
+from sympy import Integer, acos, pi, sqrt, sympify, tan
+from sympy.core.relational import Eq
+from sympy.functions.elementary.trigonometric import atan2
+from sympy.polys.polytools import cancel
+from sympy.physics.vector import Vector, dot
+from sympy.simplify.simplify import trigsimp
+
+
+__all__ = [
+    'WrappingGeometryBase',
+    'WrappingCylinder',
+    'WrappingSphere',
+]
+
+
+class WrappingGeometryBase(ABC):
+    """Abstract base class for all geometry classes to inherit from.
+
+    Notes
+    =====
+
+    Instances of this class cannot be directly instantiated by users. However,
+    it can be used to created custom geometry types through subclassing.
+
+    """
+
+    @property
+    @abstractmethod
+    def point(cls):
+        """The point with which the geometry is associated."""
+        pass
+
+    @abstractmethod
+    def point_on_surface(self, point):
+        """Returns ``True`` if a point is on the geometry's surface.
+
+        Parameters
+        ==========
+        point : Point
+            The point for which it's to be ascertained if it's on the
+            geometry's surface or not.
+
+        """
+        pass
+
+    @abstractmethod
+    def geodesic_length(self, point_1, point_2):
+        """Returns the shortest distance between two points on a geometry's
+        surface.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            The point from which the geodesic length should be calculated.
+        point_2 : Point
+            The point to which the geodesic length should be calculated.
+
+        """
+        pass
+
+    @abstractmethod
+    def geodesic_end_vectors(self, point_1, point_2):
+        """The vectors parallel to the geodesic at the two end points.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            The point from which the geodesic originates.
+        point_2 : Point
+            The point at which the geodesic terminates.
+
+        """
+        pass
+
+    def __repr__(self):
+        """Default representation of a geometry object."""
+        return f'{self.__class__.__name__}()'
+
+
+class WrappingSphere(WrappingGeometryBase):
+    """A solid spherical object.
+
+    Explanation
+    ===========
+
+    A wrapping geometry that allows for circular arcs to be defined between
+    pairs of points. These paths are always geodetic (the shortest possible).
+
+    Examples
+    ========
+
+    To create a ``WrappingSphere`` instance, a ``Symbol`` denoting its radius
+    and ``Point`` at which its center will be located are needed:
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import Point, WrappingSphere
+    >>> r = symbols('r')
+    >>> pO = Point('pO')
+
+    A sphere with radius ``r`` centered on ``pO`` can be instantiated with:
+
+    >>> WrappingSphere(r, pO)
+    WrappingSphere(radius=r, point=pO)
+
+    Parameters
+    ==========
+
+    radius : Symbol
+        Radius of the sphere. This symbol must represent a value that is
+        positive and constant, i.e. it cannot be a dynamic symbol, nor can it
+        be an expression.
+    point : Point
+        A point at which the sphere is centered.
+
+    See Also
+    ========
+
+    WrappingCylinder: Cylindrical geometry where the wrapping direction can be
+        defined.
+
+    """
+
+    def __init__(self, radius, point):
+        """Initializer for ``WrappingSphere``.
+
+        Parameters
+        ==========
+
+        radius : Symbol
+            The radius of the sphere.
+        point : Point
+            A point on which the sphere is centered.
+
+        """
+        self.radius = radius
+        self.point = point
+
+    @property
+    def radius(self):
+        """Radius of the sphere."""
+        return self._radius
+
+    @radius.setter
+    def radius(self, radius):
+        self._radius = radius
+
+    @property
+    def point(self):
+        """A point on which the sphere is centered."""
+        return self._point
+
+    @point.setter
+    def point(self, point):
+        self._point = point
+
+    def point_on_surface(self, point):
+        """Returns ``True`` if a point is on the sphere's surface.
+
+        Parameters
+        ==========
+
+        point : Point
+            The point for which it's to be ascertained if it's on the sphere's
+            surface or not. This point's position relative to the sphere's
+            center must be a simple expression involving the radius of the
+            sphere, otherwise this check will likely not work.
+
+        """
+        point_vector = point.pos_from(self.point)
+        if isinstance(point_vector, Vector):
+            point_radius_squared = dot(point_vector, point_vector)
+        else:
+            point_radius_squared = point_vector**2
+        return Eq(point_radius_squared, self.radius**2) == True
+
+    def geodesic_length(self, point_1, point_2):
+        r"""Returns the shortest distance between two points on the sphere's
+        surface.
+
+        Explanation
+        ===========
+
+        The geodesic length, i.e. the shortest arc along the surface of a
+        sphere, connecting two points can be calculated using the formula:
+
+        .. math::
+
+           l = \arccos\left(\mathbf{v}_1 \cdot \mathbf{v}_2\right)
+
+        where $\mathbf{v}_1$ and $\mathbf{v}_2$ are the unit vectors from the
+        sphere's center to the first and second points on the sphere's surface
+        respectively. Note that the actual path that the geodesic will take is
+        undefined when the two points are directly opposite one another.
+
+        Examples
+        ========
+
+        A geodesic length can only be calculated between two points on the
+        sphere's surface. Firstly, a ``WrappingSphere`` instance must be
+        created along with two points that will lie on its surface:
+
+        >>> from sympy import symbols
+        >>> from sympy.physics.mechanics import (Point, ReferenceFrame,
+        ...     WrappingSphere)
+        >>> N = ReferenceFrame('N')
+        >>> r = symbols('r')
+        >>> pO = Point('pO')
+        >>> pO.set_vel(N, 0)
+        >>> sphere = WrappingSphere(r, pO)
+        >>> p1 = Point('p1')
+        >>> p2 = Point('p2')
+
+        Let's assume that ``p1`` lies at a distance of ``r`` in the ``N.x``
+        direction from ``pO`` and that ``p2`` is located on the sphere's
+        surface in the ``N.y + N.z`` direction from ``pO``. These positions can
+        be set with:
+
+        >>> p1.set_pos(pO, r*N.x)
+        >>> p1.pos_from(pO)
+        r*N.x
+        >>> p2.set_pos(pO, r*(N.y + N.z).normalize())
+        >>> p2.pos_from(pO)
+        sqrt(2)*r/2*N.y + sqrt(2)*r/2*N.z
+
+        The geodesic length, which is in this case is a quarter of the sphere's
+        circumference, can be calculated using the ``geodesic_length`` method:
+
+        >>> sphere.geodesic_length(p1, p2)
+        pi*r/2
+
+        If the ``geodesic_length`` method is passed an argument, the ``Point``
+        that doesn't lie on the sphere's surface then a ``ValueError`` is
+        raised because it's not possible to calculate a value in this case.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            Point from which the geodesic length should be calculated.
+        point_2 : Point
+            Point to which the geodesic length should be calculated.
+
+        """
+        for point in (point_1, point_2):
+            if not self.point_on_surface(point):
+                msg = (
+                    f'Geodesic length cannot be calculated as point {point} '
+                    f'with radius {point.pos_from(self.point).magnitude()} '
+                    f'from the sphere\'s center {self.point} does not lie on '
+                    f'the surface of {self} with radius {self.radius}.'
+                )
+                raise ValueError(msg)
+        point_1_vector = point_1.pos_from(self.point).normalize()
+        point_2_vector = point_2.pos_from(self.point).normalize()
+        central_angle = acos(point_2_vector.dot(point_1_vector))
+        geodesic_length = self.radius*central_angle
+        return geodesic_length
+
+    def geodesic_end_vectors(self, point_1, point_2):
+        """The vectors parallel to the geodesic at the two end points.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            The point from which the geodesic originates.
+        point_2 : Point
+            The point at which the geodesic terminates.
+
+        """
+        pA, pB = point_1, point_2
+        pO = self.point
+        pA_vec = pA.pos_from(pO)
+        pB_vec = pB.pos_from(pO)
+
+        if pA_vec.cross(pB_vec) == 0:
+            msg = (
+                f'Can\'t compute geodesic end vectors for the pair of points '
+                f'{pA} and {pB} on a sphere {self} as they are diametrically '
+                f'opposed, thus the geodesic is not defined.'
+            )
+            raise ValueError(msg)
+
+        return (
+            pA_vec.cross(pB.pos_from(pA)).cross(pA_vec).normalize(),
+            pB_vec.cross(pA.pos_from(pB)).cross(pB_vec).normalize(),
+        )
+
+    def __repr__(self):
+        """Representation of a ``WrappingSphere``."""
+        return (
+            f'{self.__class__.__name__}(radius={self.radius}, '
+            f'point={self.point})'
+        )
+
+
+class WrappingCylinder(WrappingGeometryBase):
+    """A solid (infinite) cylindrical object.
+
+    Explanation
+    ===========
+
+    A wrapping geometry that allows for circular arcs to be defined between
+    pairs of points. These paths are always geodetic (the shortest possible) in
+    the sense that they will be a straight line on the unwrapped cylinder's
+    surface. However, it is also possible for a direction to be specified, i.e.
+    paths can be influenced such that they either wrap along the shortest side
+    or the longest side of the cylinder. To define these directions, rotations
+    are in the positive direction following the right-hand rule.
+
+    Examples
+    ========
+
+    To create a ``WrappingCylinder`` instance, a ``Symbol`` denoting its
+    radius, a ``Vector`` defining its axis, and a ``Point`` through which its
+    axis passes are needed:
+
+    >>> from sympy import symbols
+    >>> from sympy.physics.mechanics import (Point, ReferenceFrame,
+    ...     WrappingCylinder)
+    >>> N = ReferenceFrame('N')
+    >>> r = symbols('r')
+    >>> pO = Point('pO')
+    >>> ax = N.x
+
+    A cylinder with radius ``r``, and axis parallel to ``N.x`` passing through
+    ``pO`` can be instantiated with:
+
+    >>> WrappingCylinder(r, pO, ax)
+    WrappingCylinder(radius=r, point=pO, axis=N.x)
+
+    Parameters
+    ==========
+
+    radius : Symbol
+        The radius of the cylinder.
+    point : Point
+        A point through which the cylinder's axis passes.
+    axis : Vector
+        The axis along which the cylinder is aligned.
+
+    See Also
+    ========
+
+    WrappingSphere: Spherical geometry where the wrapping direction is always
+        geodetic.
+
+    """
+
+    def __init__(self, radius, point, axis):
+        """Initializer for ``WrappingCylinder``.
+
+        Parameters
+        ==========
+
+        radius : Symbol
+            The radius of the cylinder. This symbol must represent a value that
+            is positive and constant, i.e. it cannot be a dynamic symbol.
+        point : Point
+            A point through which the cylinder's axis passes.
+        axis : Vector
+            The axis along which the cylinder is aligned.
+
+        """
+        self.radius = radius
+        self.point = point
+        self.axis = axis
+
+    @property
+    def radius(self):
+        """Radius of the cylinder."""
+        return self._radius
+
+    @radius.setter
+    def radius(self, radius):
+        self._radius = radius
+
+    @property
+    def point(self):
+        """A point through which the cylinder's axis passes."""
+        return self._point
+
+    @point.setter
+    def point(self, point):
+        self._point = point
+
+    @property
+    def axis(self):
+        """Axis along which the cylinder is aligned."""
+        return self._axis
+
+    @axis.setter
+    def axis(self, axis):
+        self._axis = axis.normalize()
+
+    def point_on_surface(self, point):
+        """Returns ``True`` if a point is on the cylinder's surface.
+
+        Parameters
+        ==========
+
+        point : Point
+            The point for which it's to be ascertained if it's on the
+            cylinder's surface or not. This point's position relative to the
+            cylinder's axis must be a simple expression involving the radius of
+            the sphere, otherwise this check will likely not work.
+
+        """
+        relative_position = point.pos_from(self.point)
+        parallel = relative_position.dot(self.axis) * self.axis
+        point_vector = relative_position - parallel
+        if isinstance(point_vector, Vector):
+            point_radius_squared = dot(point_vector, point_vector)
+        else:
+            point_radius_squared = point_vector**2
+        return Eq(trigsimp(point_radius_squared), self.radius**2) == True
+
+    def geodesic_length(self, point_1, point_2):
+        """The shortest distance between two points on a geometry's surface.
+
+        Explanation
+        ===========
+
+        The geodesic length, i.e. the shortest arc along the surface of a
+        cylinder, connecting two points. It can be calculated using Pythagoras'
+        theorem. The first short side is the distance between the two points on
+        the cylinder's surface parallel to the cylinder's axis. The second
+        short side is the arc of a circle between the two points of the
+        cylinder's surface perpendicular to the cylinder's axis. The resulting
+        hypotenuse is the geodesic length.
+
+        Examples
+        ========
+
+        A geodesic length can only be calculated between two points on the
+        cylinder's surface. Firstly, a ``WrappingCylinder`` instance must be
+        created along with two points that will lie on its surface:
+
+        >>> from sympy import symbols, cos, sin
+        >>> from sympy.physics.mechanics import (Point, ReferenceFrame,
+        ...     WrappingCylinder, dynamicsymbols)
+        >>> N = ReferenceFrame('N')
+        >>> r = symbols('r')
+        >>> pO = Point('pO')
+        >>> pO.set_vel(N, 0)
+        >>> cylinder = WrappingCylinder(r, pO, N.x)
+        >>> p1 = Point('p1')
+        >>> p2 = Point('p2')
+
+        Let's assume that ``p1`` is located at ``N.x + r*N.y`` relative to
+        ``pO`` and that ``p2`` is located at ``r*(cos(q)*N.y + sin(q)*N.z)``
+        relative to ``pO``, where ``q(t)`` is a generalized coordinate
+        specifying the angle rotated around the ``N.x`` axis according to the
+        right-hand rule where ``N.y`` is zero. These positions can be set with:
+
+        >>> q = dynamicsymbols('q')
+        >>> p1.set_pos(pO, N.x + r*N.y)
+        >>> p1.pos_from(pO)
+        N.x + r*N.y
+        >>> p2.set_pos(pO, r*(cos(q)*N.y + sin(q)*N.z).normalize())
+        >>> p2.pos_from(pO).simplify()
+        r*cos(q(t))*N.y + r*sin(q(t))*N.z
+
+        The geodesic length, which is in this case a is the hypotenuse of a
+        right triangle where the other two side lengths are ``1`` (parallel to
+        the cylinder's axis) and ``r*q(t)`` (parallel to the cylinder's cross
+        section), can be calculated using the ``geodesic_length`` method:
+
+        >>> cylinder.geodesic_length(p1, p2).simplify()
+        sqrt(r**2*q(t)**2 + 1)
+
+        If the ``geodesic_length`` method is passed an argument ``Point`` that
+        doesn't lie on the sphere's surface then a ``ValueError`` is raised
+        because it's not possible to calculate a value in this case.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            Point from which the geodesic length should be calculated.
+        point_2 : Point
+            Point to which the geodesic length should be calculated.
+
+        """
+        for point in (point_1, point_2):
+            if not self.point_on_surface(point):
+                msg = (
+                    f'Geodesic length cannot be calculated as point {point} '
+                    f'with radius {point.pos_from(self.point).magnitude()} '
+                    f'from the cylinder\'s center {self.point} does not lie on '
+                    f'the surface of {self} with radius {self.radius} and axis '
+                    f'{self.axis}.'
+                )
+                raise ValueError(msg)
+
+        relative_position = point_2.pos_from(point_1)
+        parallel_length = relative_position.dot(self.axis)
+
+        point_1_relative_position = point_1.pos_from(self.point)
+        point_1_perpendicular_vector = (
+            point_1_relative_position
+            - point_1_relative_position.dot(self.axis)*self.axis
+        ).normalize()
+
+        point_2_relative_position = point_2.pos_from(self.point)
+        point_2_perpendicular_vector = (
+            point_2_relative_position
+            - point_2_relative_position.dot(self.axis)*self.axis
+        ).normalize()
+
+        central_angle = _directional_atan(
+            cancel(point_1_perpendicular_vector
+                .cross(point_2_perpendicular_vector)
+                .dot(self.axis)),
+            cancel(point_1_perpendicular_vector.dot(point_2_perpendicular_vector)),
+        )
+
+        planar_arc_length = self.radius*central_angle
+        geodesic_length = sqrt(parallel_length**2 + planar_arc_length**2)
+        return geodesic_length
+
+    def geodesic_end_vectors(self, point_1, point_2):
+        """The vectors parallel to the geodesic at the two end points.
+
+        Parameters
+        ==========
+
+        point_1 : Point
+            The point from which the geodesic originates.
+        point_2 : Point
+            The point at which the geodesic terminates.
+
+        """
+        point_1_from_origin_point = point_1.pos_from(self.point)
+        point_2_from_origin_point = point_2.pos_from(self.point)
+
+        if point_1_from_origin_point == point_2_from_origin_point:
+            msg = (
+                f'Cannot compute geodesic end vectors for coincident points '
+                f'{point_1} and {point_2} as no geodesic exists.'
+            )
+            raise ValueError(msg)
+
+        point_1_parallel = point_1_from_origin_point.dot(self.axis) * self.axis
+        point_2_parallel = point_2_from_origin_point.dot(self.axis) * self.axis
+        point_1_normal = (point_1_from_origin_point - point_1_parallel)
+        point_2_normal = (point_2_from_origin_point - point_2_parallel)
+
+        if point_1_normal == point_2_normal:
+            point_1_perpendicular = Vector(0)
+            point_2_perpendicular = Vector(0)
+        else:
+            point_1_perpendicular = self.axis.cross(point_1_normal).normalize()
+            point_2_perpendicular = -self.axis.cross(point_2_normal).normalize()
+
+        geodesic_length = self.geodesic_length(point_1, point_2)
+        relative_position = point_2.pos_from(point_1)
+        parallel_length = relative_position.dot(self.axis)
+        planar_arc_length = sqrt(geodesic_length**2 - parallel_length**2)
+
+        point_1_vector = (
+            planar_arc_length * point_1_perpendicular
+            + parallel_length * self.axis
+        ).normalize()
+        point_2_vector = (
+            planar_arc_length * point_2_perpendicular
+            - parallel_length * self.axis
+        ).normalize()
+
+        return (point_1_vector, point_2_vector)
+
+    def __repr__(self):
+        """Representation of a ``WrappingCylinder``."""
+        return (
+            f'{self.__class__.__name__}(radius={self.radius}, '
+            f'point={self.point}, axis={self.axis})'
+        )
+
+
+def _directional_atan(numerator, denominator):
+    """Compute atan in a directional sense as required for geodesics.
+
+    Explanation
+    ===========
+
+    To be able to control the direction of the geodesic length along the
+    surface of a cylinder a dedicated arctangent function is needed that
+    properly handles the directionality of different case. This function
+    ensures that the central angle is always positive but shifting the case
+    where ``atan2`` would return a negative angle to be centered around
+    ``2*pi``.
+
+    Notes
+    =====
+
+    This function only handles very specific cases, i.e. the ones that are
+    expected to be encountered when calculating symbolic geodesics on uniformly
+    curved surfaces. As such, ``NotImplemented`` errors can be raised in many
+    cases. This function is named with a leader underscore to indicate that it
+    only aims to provide very specific functionality within the private scope
+    of this module.
+
+    """
+
+    if numerator.is_number and denominator.is_number:
+        angle = atan2(numerator, denominator)
+        if angle < 0:
+            angle += 2 * pi
+    elif numerator.is_number:
+        msg = (
+            f'Cannot compute a directional atan when the numerator {numerator} '
+            f'is numeric and the denominator {denominator} is symbolic.'
+        )
+        raise NotImplementedError(msg)
+    elif denominator.is_number:
+        msg = (
+            f'Cannot compute a directional atan when the numerator {numerator} '
+            f'is symbolic and the denominator {denominator} is numeric.'
+        )
+        raise NotImplementedError(msg)
+    else:
+        ratio = sympify(trigsimp(numerator / denominator))
+        if isinstance(ratio, tan):
+            angle = ratio.args[0]
+        elif (
+            ratio.is_Mul
+            and ratio.args[0] == Integer(-1)
+            and isinstance(ratio.args[1], tan)
+        ):
+            angle = 2 * pi - ratio.args[1].args[0]
+        else:
+            msg = f'Cannot compute a directional atan for the value {ratio}.'
+            raise NotImplementedError(msg)
+
+    return angle