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