def test_complete_simple_double_pendulum(): q1, q2 = dynamicsymbols('q1 q2') u1, u2 = dynamicsymbols('u1 u2') m, l, g = symbols('m l g') C = Body('C') # ceiling PartP = Body('P', mass=m) PartR = Body('R', mass=m) J1 = PinJoint('J1', C, PartP, speeds=u1, coordinates=q1, child_joint_pos=-l*PartP.x, parent_axis=C.z, child_axis=PartP.z) J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2, child_joint_pos=-l*PartR.x, parent_axis=PartP.z, child_axis=PartR.z) PartP.apply_force(m*g*C.x) PartR.apply_force(m*g*C.x) method = JointsMethod(C, J1, J2) method.form_eoms() assert expand(method.mass_matrix_full) == Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 2*l**2*m*cos(q2) + 3*l**2*m, l**2*m*cos(q2) + l**2*m], [0, 0, l**2*m*cos(q2) + l**2*m, l**2*m]]) assert trigsimp(method.forcing_full) == trigsimp(Matrix([[u1], [u2], [-g*l*m*(sin(q1 + q2) + sin(q1)) - g*l*m*sin(q1) + l**2*m*(2*u1 + u2)*u2*sin(q2)], [-g*l*m*sin(q1 + q2) - l**2*m*u1**2*sin(q2)]]))
def test_pinjoint(): P = Body('P') C = Body('C') l, m = symbols('l m') theta, omega = dynamicsymbols('theta_J, omega_J') Pj = PinJoint('J', P, C) assert Pj.name == 'J' assert Pj.parent == P assert Pj.child == C assert Pj.coordinates == [theta] assert Pj.speeds == [omega] assert Pj.kdes == [omega - theta.diff(t)] assert Pj.parent_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.__str__() == 'PinJoint: J parent: P child: C' P1 = Body('P1') C1 = Body('C1') J1 = PinJoint('J1', P1, C1, parent_joint_pos=l*P1.frame.x, child_joint_pos=m*C1.frame.y, parent_axis=P1.frame.z) assert J1._parent_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)
def test_chaos_pendulum(): #https://www.pydy.org/examples/chaos_pendulum.html mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g = symbols('mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g') theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha') A = ReferenceFrame('A') B = ReferenceFrame('B') rod = Body('rod', mass=mA, frame=A, central_inertia=inertia(A, IAxx, IAxx, 0)) plate = Body('plate', mass=mB, frame=B, central_inertia=inertia(B, IBxx, IByy, IBzz)) C = Body('C') J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega, child_joint_pos=-lA*rod.z, parent_axis=C.y, child_axis=rod.y) J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha, parent_joint_pos=(lB-lA)*rod.z, parent_axis=rod.z, child_axis=plate.z) rod.apply_force(mA*g*C.z) plate.apply_force(mB*g*C.z) method = JointsMethod(C, J1, J2) method.form_eoms() MM = method.mass_matrix forcing = method.forcing rhs = MM.LUsolve(forcing) xd = (-2 * IBxx * alpha * omega * sin(phi) * cos(phi) + 2 * IByy * alpha * omega * sin(phi) * cos(phi) - g * lA * mA * sin(theta) - g * lB * mB * sin(theta)) / (IAxx + IBxx * sin(phi)**2 + IByy * cos(phi)**2 + lA**2 * mA + lB**2 * mB) assert (rhs[0] - xd).simplify() == 0 xd = (IBxx - IByy) * omega**2 * sin(phi) * cos(phi) / IBzz assert (rhs[1] - xd).simplify() == 0
def test_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_joint_pos=lA * A.z, parent_axis=N.y, child_axis=A.y) J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha, parent_joint_pos=lC * A.z, parent_axis=A.z, child_axis=B.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 == [omega - theta.diff(t)] assert J2.kdes == [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_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_joint_pos=-l * A.x, parent_axis=C.frame.z, child_axis=PartP.frame.z) J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2, child_joint_pos=-l * B.x, parent_axis=PartP.frame.z, child_axis=PartR.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 == [u1 - q1.diff(t)] assert J2.kdes == [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_jointmethod_duplicate_coordinates_speeds(): P = Body('P') C = Body('C') T = Body('T') q, u = dynamicsymbols('q u') P1 = PinJoint('P1', P, C, q) P2 = PrismaticJoint('P2', C, T, q) raises(ValueError, lambda: JointsMethod(P, P1, P2)) P1 = PinJoint('P1', P, C, speeds=u) P2 = PrismaticJoint('P2', C, T, speeds=u) raises(ValueError, lambda: JointsMethod(P, P1, P2)) P1 = PinJoint('P1', P, C, q, u) P2 = PrismaticJoint('P2', C, T, q, u) raises(ValueError, lambda: JointsMethod(P, P1, P2))
def test_simple_pedulum(): l, m, g = symbols('l m g') C = Body('C') b = Body('b', mass=m) q = dynamicsymbols('q') P = PinJoint('P', C, b, speeds=q.diff(t), coordinates=q, child_joint_pos = -l*b.x, parent_axis=C.z, child_axis=b.z) b.potential_energy = - m * g * l * cos(q) method = JointsMethod(C, P) method.form_eoms(LagrangesMethod) rhs = method.rhs() assert rhs[1] == -g*sin(q)/l
def test_jointsmethod(): P = Body('P') C = Body('C') Pin = PinJoint('P1', P, C) C_ixx, g = symbols('C_ixx g') theta, omega = dynamicsymbols('theta_P1, omega_P1') P.apply_force(g * P.y) method = JointsMethod(P, Pin) assert method.frame == P.frame assert method.bodies == [C, P] assert method.loads == [(P.masscenter, g * P.frame.y)] assert method.q == [theta] assert method.u == [omega] assert method.kdes == [omega - theta.diff()] soln = method.form_eoms() assert soln == Matrix([[-C_ixx * omega.diff()]]) assert method.forcing_full == Matrix([[omega], [0]]) assert method.mass_matrix_full == Matrix([[1, 0], [0, C_ixx]]) assert isinstance(method.method, KanesMethod)
def test_pinjoint_pi(): _, _, P, C = _generate_body() J = PinJoint('J', P, C, child_axis=-C.frame.x) assert J._generate_vector() == P.frame.z _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.y, child_axis=-C.frame.y) assert J._generate_vector() == P.frame.x _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.z, child_axis=-C.frame.z) assert J._generate_vector() == P.frame.y _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.x+P.frame.y, child_axis=-C.frame.y-C.frame.x) assert J._generate_vector() == P.frame.z _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.y+P.frame.z, child_axis=-C.frame.y-C.frame.z) assert J._generate_vector() == P.frame.x _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.x+P.frame.z, child_axis=-C.frame.z-C.frame.x) assert J._generate_vector() == P.frame.y _, _, P, C = _generate_body() J = PinJoint('J', P, C, parent_axis=P.frame.x+P.frame.y+P.frame.z, child_axis=-C.frame.x-C.frame.y-C.frame.z) assert J._generate_vector() == P.frame.y - P.frame.z
def test_pinjoint_arbitrary_axis(): theta, omega = dynamicsymbols('theta_J, omega_J') # When the bodies are attached though masscenters but axess are opposite. N, A, P, C = _generate_body() 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(theta), -sin(theta)], [0, -sin(theta), cos(theta)]]) assert A.ang_vel_in(N) == omega*N.x assert A.ang_vel_in(N).magnitude() == sqrt(omega**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_axis=A.y, child_joint_pos=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(theta), -sin(theta)], [1, 0, 0], [0, -sin(theta), cos(theta)]]) assert A.ang_vel_in(N) == omega*N.x assert A.ang_vel_in(N).express(A) == omega * A.y assert A.ang_vel_in(N).magnitude() == sqrt(omega**2) angle = A.ang_vel_in(N).angle_between(A.y) assert angle.xreplace({omega: 1}) == 0 assert C.masscenter.vel(N) == omega*A.z assert C.masscenter.pos_from(P.masscenter) == -A.x assert (C.masscenter.pos_from(P.masscenter).express(N).simplify() == cos(theta)*N.y + sin(theta)*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_axis=N.y, parent_joint_pos=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(theta), 0, sin(theta)], [sin(theta), 0, cos(theta)]]) assert A.ang_vel_in(N) == omega*N.y assert A.ang_vel_in(N).express(A) == omega*A.x assert A.ang_vel_in(N).magnitude() == sqrt(omega**2) angle = A.ang_vel_in(N).angle_between(A.x) assert angle.xreplace({omega: 1}) == 0 assert C.masscenter.vel(N).simplify() == - omega*N.z 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_joint_pos=N.x, child_joint_pos=A.x, child_axis=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(theta)/2, -sqrt(2)*sin(theta)/2], [sqrt(2)/2, sqrt(2)*cos(theta)/2, sqrt(2)*sin(theta)/2], [0, -sin(theta), cos(theta)]]) assert A.ang_vel_in(N) == omega*N.x assert (A.ang_vel_in(N).express(A).simplify() == (omega*A.x + omega*A.y)/sqrt(2)) assert A.ang_vel_in(N).magnitude() == sqrt(omega**2) angle = A.ang_vel_in(N).angle_between(A.x + A.y) assert angle.xreplace({omega: 1}) == 0 assert C.masscenter.vel(N).simplify() == (omega * 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(theta)/2*N.y + sqrt(2)*sin(theta)/2*N.z) assert (C.masscenter.vel(N).express(N).simplify() == -sqrt(2)*omega*sin(theta)/2*N.y + sqrt(2)*omega*cos(theta)/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_joint_pos=N.x, child_joint_pos=A.x, child_axis=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(theta + pi/4)/3, sqrt(6)*cos(theta + pi/4)/3], [sqrt(3)/3, sqrt(6)*cos(theta + pi/12)/3, sqrt(6)*sin(theta + pi/12)/3], [-sqrt(3)/3, sqrt(6)*cos(theta + 5*pi/12)/3, sqrt(6)*sin(theta + 5*pi/12)/3]]) assert A.ang_vel_in(N) == omega*N.x assert A.ang_vel_in(N).express(A).simplify() == (omega*A.x + omega*A.y - omega*A.z)/sqrt(3) assert A.ang_vel_in(N).magnitude() == sqrt(omega**2) angle = A.ang_vel_in(N).angle_between(A.x + A.y-A.z) assert angle.xreplace({omega: 1}) == 0 assert C.masscenter.vel(N).simplify() == (omega*A.y + omega*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(theta + pi/4)/3*N.y - sqrt(6)*cos(theta + pi/4)/3*N.z) assert (C.masscenter.vel(N).express(N).simplify() == sqrt(6)*omega*cos(theta + pi/4)/3*N.y + sqrt(6)*omega*sin(theta + 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_joint_pos=m*N.x, child_joint_pos=n*A.x, child_axis=A.x+A.y-A.z, parent_axis=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(theta)/3 - S(1)/3)*N.x + (S(1)/3 - 4*sin(theta + pi/6)/3)*N.y + (4*cos(theta + pi/3)/3 - S(1)/3)*N.z) assert _simplify_matrix(A.dcm(N)) == Matrix([ [S(1)/3 - 2*cos(theta)/3, -2*sin(theta + pi/6)/3 - S(1)/3, 2*cos(theta + pi/3)/3 + S(1)/3], [2*cos(theta + pi/3)/3 + S(1)/3, 2*cos(theta)/3 - S(1)/3, 2*sin(theta + pi/6)/3 + S(1)/3], [-2*sin(theta + pi/6)/3 - S(1)/3, 2*cos(theta + pi/3)/3 + S(1)/3, 2*cos(theta)/3 - S(1)/3]]) assert A.ang_vel_in(N) == (omega*N.x - omega*N.y + omega*N.z)/sqrt(3) assert A.ang_vel_in(N).express(A).simplify() == (omega*A.x + omega*A.y - omega*A.z)/sqrt(3) assert A.ang_vel_in(N).magnitude() == sqrt(omega**2) angle = A.ang_vel_in(N).angle_between(A.x+A.y-A.z) assert angle.xreplace({omega: 1}) == 0 assert (C.masscenter.vel(N).simplify() == (m*omega*N.y + m*omega*N.z + n*omega*A.y + n*omega*A.z)/sqrt(3)) 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(theta) - 1)/3)*N.x + n*(2*sin(theta + pi/6) + 1)/3*N.y - n*(2*cos(theta + pi/3) + 1)/3*N.z) assert (C.masscenter.vel(N).express(N).simplify() == -2*n*omega*sin(theta)/3*N.x + (sqrt(3)*m + 2*n*cos(theta + pi/6))*omega/3*N.y + (sqrt(3)*m + 2*n*sin(theta + pi/3))*omega/3*N.z) assert expand_mul(C.masscenter.vel(N).angle_between(m*N.x - n*A.x)) == pi/2