1145 lines
52 KiB
Python
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
|