switching to high quality piper tts and added label translations

2026-01-29 23:48:19 +01:00
commit d80c619df9
3934 changed files with 1451600 additions and 0 deletions
@@ -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
@@ -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)]
@@ -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)
@@ -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))
@@ -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)
@@ -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()))
@@ -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)
@@ -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.
@@ -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))
@@ -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)
@@ -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 accurately formed with Lagrange method (see issue #8626)
+
+def test_lagrange_2forces():
+    ### Equations for two damped springs in series 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
@@ -0,0 +1,41 @@
+from sympy import symbols, sin, cos
+from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
+            KanesMethod)
+from sympy.testing import pytest
+from sympy.solvers.solveset import NonlinearError
+
+def test_linearity_of_motion_constraints():
+    # Test that an error is raised by KanesMethod if nonlinear velocity
+    # constraints are supplied.
+    # It is a simple pendulum.
+    t = dynamicsymbols._t
+    N, A = ReferenceFrame('N'), ReferenceFrame('A')
+    O, P = Point('O'), Point('P')
+    O.set_vel(N, 0)
+
+    l = symbols('l')
+    q, x, y, u, ux, uy = dynamicsymbols('q x y u ux uy')
+
+    A.orient_axis(N, q, N.z)
+    A.set_ang_vel(N, u * N.z)
+    P.set_pos(O, -l * A.y)
+    P.v2pt_theory(O, N, A)
+
+    kd = [u - q.diff(t), ux - x.diff(t), uy - y.diff(t)]
+    config_constr = [x - l * sin(q), y - l * cos(q)]
+
+    q_ind = [q]
+    q_dep = [x, y]
+    u_ind = [u]
+    u_dep = [ux, uy]
+
+    # Make sure an error is raised if nonlinear velocity constraints are
+    # supplied.
+    speed_constr = [ux - l * q.diff(t) * cos(q), sin(uy) +
+        l * q.diff(t) * sin(q)]
+
+    with pytest.raises(NonlinearError):
+        KanesMethod(N, q_ind=q_ind, q_dependent=q_dep, u_ind=u_ind,
+            u_dependent=u_dep, kd_eqs=kd,
+            configuration_constraints=config_constr,
+            velocity_constraints=speed_constr)
@@ -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
@@ -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]))
@@ -0,0 +1,5 @@
+from sympy.physics.mechanics.method import _Methods
+from sympy.testing.pytest import raises
+
+def test_method():
+    raises(TypeError, lambda: _Methods())
@@ -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])
@@ -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)
@@ -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
@@ -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)
@@ -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
@@ -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)
@@ -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)