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)