def test_linearize_pendulum_lagrange_minimal(): q1 = dynamicsymbols('q1') # angle of pendulum q1d = dynamicsymbols('q1', 1) # Angular velocity L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum A = N.orientnew('A', 'axis', [q1, N.z]) A.set_ang_vel(N, q1d * N.z) # Locate point P relative to the origin N* P = pN.locatenew('P', L * A.x) P.v2pt_theory(pN, N, A) pP = Particle('pP', P, m) # Solve for eom with Lagranges method Lag = Lagrangian(N, pP) LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m * g * N.x)], frame=N) LM.form_lagranges_equations() # Linearize A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True) assert _simplify_matrix(A) == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]]) assert B == Matrix([])
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_linearize_pendulum_lagrange_nonminimal(): q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose World Frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2 / q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Create point P, the pendulum mass P = pN.locatenew('P1', q1 * N.x + q2 * N.y) P.set_vel(N, P.pos_from(pN).dt(N)) pP = Particle('pP', P, m) # Constraint Equations f_c = Matrix([q1**2 + q2**2 - L**2]) # Calculate the lagrangian, and form the equations of motion Lag = Lagrangian(N, pP) LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m * g * N.x)], frame=N) LM.form_lagranges_equations() # Compose operating point op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0} # Solve for multiplier operating point lam_op = LM.solve_multipliers(op_point=op_point) op_point.update(lam_op) # Perform the Linearization A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d], op_point=op_point, A_and_B=True) assert _simplify_matrix(A) == Matrix([[0, 1], [-9.8 / L, 0]]) assert B == Matrix([])
def test_slidingjoint_arbitrary_axis(): x, v = dynamicsymbols('x_S, v_S') N, A, P, C = _generate_body() PrismaticJoint('S', 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, -1, 0], [0, 0, 1]]) assert C.masscenter.pos_from(P.masscenter) == x * N.x assert C.masscenter.pos_from(P.masscenter).express(A).simplify() == -x * A.x assert C.masscenter.vel(N) == v * N.x assert C.masscenter.vel(N).express(A) == -v * 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_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, -1, 0], [1, 0, 0], [0, 0, 1]]) assert C.masscenter.vel(N) == v * N.x assert C.masscenter.vel(N).express(A) == v * A.y assert C.masscenter.pos_from(P.masscenter) == x*N.x - A.x assert C.masscenter.pos_from(P.masscenter).express(N).simplify() == x*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_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], [-1, 0, 0], [0, 0, 1]]) assert C.masscenter.vel(N) == v * N.y assert C.masscenter.vel(N).express(A) == v * A.x assert C.masscenter.pos_from(P.masscenter) == N.x + x*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_joint_pos=N.x, child_joint_pos=A.x, child_axis=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) == (x + 1)*N.x - A.x assert C.masscenter.pos_from(P.masscenter).express(N) == (x - sqrt(2)/2 + 1)*N.x + sqrt(2)/2*N.y assert C.masscenter.vel(N).express(A) == v * (A.x + A.y)/sqrt(2) assert C.masscenter.vel(N) == v*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_joint_pos=N.x, child_joint_pos=A.x, child_axis=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) == (x + 1)*N.x - A.x assert C.masscenter.pos_from(P.masscenter).express(N) == \ (x - sqrt(3)/3 + 1)*N.x + sqrt(3)/3*N.y - sqrt(3)/3*N.z assert C.masscenter.vel(N) == v*N.x assert C.masscenter.vel(N).express(A) == sqrt(3)*v/3*A.x + sqrt(3)*v/3*A.y - sqrt(3)*v/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_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) assert (N.x-N.y+N.z).angle_between(A.x+A.y-A.z) == 0 #Axis are aligned 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)*x/3)*N.x - sqrt(3)*x/3*N.y + sqrt(3)*x/3*N.z - n*A.x assert C.masscenter.pos_from(P.masscenter).express(N) == \ (m + n/3 + sqrt(3)*x/3)*N.x + (2*n/3 - sqrt(3)*x/3)*N.y + (-2*n/3 + sqrt(3)*x/3)*N.z assert C.masscenter.vel(N) == sqrt(3)*v/3*N.x - sqrt(3)*v/3*N.y + sqrt(3)*v/3*N.z assert C.masscenter.vel(N).express(A) == sqrt(3)*v/3*A.x + sqrt(3)*v/3*A.y - sqrt(3)*v/3*A.z assert A.ang_vel_in(N) == 0 assert N.ang_vel_in(A) == 0
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
def simplify(self): """Returns a simplified Vector.""" d = {} for v in self.args: d[v[1]] = _simplify_matrix(v[0]) return Vector(d)