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