ai-content-maker/.venv/Lib/site-packages/sympy/physics/mechanics/tests/test_joint.py

1145 lines
52 KiB
Python

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