def main():
print = lambda x: sympy.pprint(x, use_unicode=False, wrap_line=False)
# init_session()
# init_printing()
theta, phi, psi = dynamicsymbols('theta, phi, psi')
inertial_frame = ReferenceFrame('O')
gyro_frame = ReferenceFrame('e')
gyro_frame.orient(inertial_frame, 'Body', (phi, theta, psi), 'ZXZ')
print('>>>>')
print('>>>>')
omega = gyro_frame.ang_vel_in(
inertial_frame) # Angular velocity of a frame in another
print(omega.to_matrix(gyro_frame))
def test_ang_vel():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
assert A.ang_vel_in(N) == (q1d)*A.z
assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z
assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z
A2 = N.orientnew('A2', 'Axis', [q4, N.y])
assert N.ang_vel_in(N) == 0
assert N.ang_vel_in(A) == -q1d*N.z
assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x
assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y
assert N.ang_vel_in(A2) == -q4d*N.y
assert A.ang_vel_in(N) == q1d*N.z
assert A.ang_vel_in(A) == 0
assert A.ang_vel_in(B) == - q2d*B.x
assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y
assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y
assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x
assert B.ang_vel_in(A) == q2d*A.x
assert B.ang_vel_in(B) == 0
assert B.ang_vel_in(C) == -q3d*B.y
assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y
assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y
assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y
assert C.ang_vel_in(B) == q3d*B.y
assert C.ang_vel_in(C) == 0
assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y
assert A2.ang_vel_in(N) == q4d*A2.y
assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z
assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x
assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y
assert A2.ang_vel_in(A2) == 0
C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z)
assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z
assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z
assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y
assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y
q0 = dynamicsymbols('q0')
q0d = dynamicsymbols('q0', 1)
E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3))
assert E.ang_vel_in(N) == (
2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x +
2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y +
2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z)
F = N.orientnew('F', 'Body', (q1, q2, q3), '313')
assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x +
(sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z)
G = N.orientnew('G', 'Axis', (q1, N.x + N.y))
assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
l_leg_mass_center.set_pos(l_ankle, l_leg_com_length*l_leg_frame.y)
body_mass_center = Point('B_o')
body_mass_center.set_pos(l_hip, body_com_length*body_frame.y)
r_leg_mass_center = Point('RL_o')
r_leg_mass_center.set_pos(r_hip, l_leg_com_length*r_leg_frame.y)
#Sets up the angular velocities
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
omega2 - theta2.diff(),
omega3 - theta3.diff()]
#Sets up the rotational axes of the angular velocities
l_leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
l_leg_frame.ang_vel_in(inertial_frame)
body_frame.set_ang_vel(l_leg_frame, omega2*inertial_frame.z)
body_frame.ang_vel_in(inertial_frame)
r_leg_frame.set_ang_vel(body_frame, omega3*inertial_frame.z)
r_leg_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
l_ankle.set_vel(inertial_frame, 0)
l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_leg_mass_center.vel(inertial_frame)
l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_hip.vel(inertial_frame)
body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)
body_mass_center.vel(inertial_frame)
r_hip.v2pt_theory(l_hip, inertial_frame, body_frame)
r_hip.vel(inertial_frame)
def test_ang_vel():
q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4")
q1d, q2d, q3d, q4d = dynamicsymbols("q1 q2 q3 q4", 1)
N = ReferenceFrame("N")
A = N.orientnew("A", "Axis", [q1, N.z])
B = A.orientnew("B", "Axis", [q2, A.x])
C = B.orientnew("C", "Axis", [q3, B.y])
D = N.orientnew("D", "Axis", [q4, N.y])
u1, u2, u3 = dynamicsymbols("u1 u2 u3")
assert A.ang_vel_in(N) == (q1d) * A.z
assert B.ang_vel_in(N) == (q2d) * B.x + (q1d) * A.z
assert C.ang_vel_in(N) == (q3d) * C.y + (q2d) * B.x + (q1d) * A.z
A2 = N.orientnew("A2", "Axis", [q4, N.y])
assert N.ang_vel_in(N) == 0
assert N.ang_vel_in(A) == -q1d * N.z
assert N.ang_vel_in(B) == -q1d * A.z - q2d * B.x
assert N.ang_vel_in(C) == -q1d * A.z - q2d * B.x - q3d * B.y
assert N.ang_vel_in(A2) == -q4d * N.y
assert A.ang_vel_in(N) == q1d * N.z
assert A.ang_vel_in(A) == 0
assert A.ang_vel_in(B) == -q2d * B.x
assert A.ang_vel_in(C) == -q2d * B.x - q3d * B.y
assert A.ang_vel_in(A2) == q1d * N.z - q4d * N.y
assert B.ang_vel_in(N) == q1d * A.z + q2d * A.x
assert B.ang_vel_in(A) == q2d * A.x
assert B.ang_vel_in(B) == 0
assert B.ang_vel_in(C) == -q3d * B.y
assert B.ang_vel_in(A2) == q1d * A.z + q2d * A.x - q4d * N.y
assert C.ang_vel_in(N) == q1d * A.z + q2d * A.x + q3d * B.y
assert C.ang_vel_in(A) == q2d * A.x + q3d * C.y
assert C.ang_vel_in(B) == q3d * B.y
assert C.ang_vel_in(C) == 0
assert C.ang_vel_in(A2) == q1d * A.z + q2d * A.x + q3d * B.y - q4d * N.y
assert A2.ang_vel_in(N) == q4d * A2.y
assert A2.ang_vel_in(A) == q4d * A2.y - q1d * N.z
assert A2.ang_vel_in(B) == q4d * N.y - q1d * A.z - q2d * A.x
assert A2.ang_vel_in(C) == q4d * N.y - q1d * A.z - q2d * A.x - q3d * B.y
assert A2.ang_vel_in(A2) == 0
C.set_ang_vel(N, u1 * C.x + u2 * C.y + u3 * C.z)
assert C.ang_vel_in(N) == (u1) * C.x + (u2) * C.y + (u3) * C.z
assert N.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z
assert C.ang_vel_in(D) == (u1) * C.x + (u2) * C.y + (u3) * C.z + (-q4d) * D.y
assert D.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z + (q4d) * D.y
q0 = dynamicsymbols("q0")
q0d = dynamicsymbols("q0", 1)
E = N.orientnew("E", "Quaternion", (q0, q1, q2, q3))
assert E.ang_vel_in(N) == (
2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x
+ 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y
+ 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z
)
F = N.orientnew("F", "Body", (q1, q2, q3), "313")
assert F.ang_vel_in(N) == (
(sin(q2) * sin(q3) * q1d + cos(q3) * q2d) * F.x
+ (sin(q2) * cos(q3) * q1d - sin(q3) * q2d) * F.y
+ (cos(q2) * q1d + q3d) * F.z
)
G = N.orientnew("G", "Axis", (q1, N.x + N.y))
assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
def test_ang_vel():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
assert A.ang_vel_in(N) == (q1d)*A.z
assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z
assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z
A2 = N.orientnew('A2', 'Axis', [q4, N.y])
assert N.ang_vel_in(N) == 0
assert N.ang_vel_in(A) == -q1d*N.z
assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x
assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y
assert N.ang_vel_in(A2) == -q4d*N.y
assert A.ang_vel_in(N) == q1d*N.z
assert A.ang_vel_in(A) == 0
assert A.ang_vel_in(B) == - q2d*B.x
assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y
assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y
assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x
assert B.ang_vel_in(A) == q2d*A.x
assert B.ang_vel_in(B) == 0
assert B.ang_vel_in(C) == -q3d*B.y
assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y
assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y
assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y
assert C.ang_vel_in(B) == q3d*B.y
assert C.ang_vel_in(C) == 0
assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y
assert A2.ang_vel_in(N) == q4d*A2.y
assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z
assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x
assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y
assert A2.ang_vel_in(A2) == 0
C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z)
assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z
assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z
assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y
assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y
q0 = dynamicsymbols('q0')
q0d = dynamicsymbols('q0', 1)
E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3))
assert E.ang_vel_in(N) == (
2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x +
2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y +
2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z)
F = N.orientnew('F', 'Body', (q1, q2, q3), '313')
assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x +
(sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z)
G = N.orientnew('G', 'Axis', (q1, N.x + N.y))
assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2 * m * r**2 / 5, 2 * m * r**2 / 5, 2 * m * r**2 / 5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(
expand((rbC.kinetic_energy(R) + 4 * rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2 / 2))))
from sympy import sin, cos, pi, integrate, Matrix
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10')
c, R = symbols('c R')
x, y, z, r, phi, theta = symbols('x y z r phi theta')
# --- Reference Frames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z)
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A))
pC_star = Point('C*')
pC_star.set_vel(C, 0)
# since radius of cavity is very small, assume C* has zero velocity in B
pC_star.set_vel(B, 0)
pC_star.set_vel(A, u7 * B.x + u8 * B.y + u9 * B.z)
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# point on B
pP_prime = pP.locatenew("P'", 0)
b_length = symbols('l_b')
C.set_pos(B, b_length*b_frame.y)
D = Point('D')
c_length = symbols('l_c')
D.set_pos(C, c_length*c_frame.y)
#Sets up the angular velocities
omega_a, omega_b, omega_c = dynamicsymbols('omega_a, omega_b, omega_c')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff(),
omega_b - theta_b.diff(),
omega_c - theta_c.diff()]
#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z)
a_frame.ang_vel_in(inertial_frame)
b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z)
b_frame.ang_vel_in(inertial_frame)
c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z)
c_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
B.vel(inertial_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
C.vel(inertial_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
D.vel(inertial_frame)
#Sets up the masses of the linkages
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10')
c, R = symbols('c R')
x, y, z, r, phi, theta = symbols('x y z r phi theta')
# --- Reference Frames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z)
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A))
pC_star = Point('C*')
pC_star.set_vel(C, 0)
# since radius of cavity is very small, assume C* has zero velocity in B
pC_star.set_vel(B, 0)
pC_star.set_vel(A, u7 * B.x + u8*B.y + u9*B.z)
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# point on B
pP_prime = pP.locatenew("P'", 0)
class YAMSBody(object):
def __init__(self, name):
"""
Origin point have no velocities in the body frame!
"""
self.frame = ReferenceFrame('e_' + name)
self.origin = Point('O_' + name)
self.masscenter = Point('G_' + name)
self.name = name
self.origin.set_vel(self.frame, 0 * self.frame.x)
self.parent = None # Parent body, assuming a tree structure
self.children = [] # children bodies
self.inertial_frame = None # storing the typical inertial frame use for computation
self.inertial_origin = None # storing the typical inertial frame use for computation
def __repr__(self):
s = '<{} object "{}" with attributes:>\n'.format(
type(self).__name__, self.name)
s += ' - origin: {}\n'.format(self.origin)
s += ' - frame: {}\n'.format(self.frame)
return s
def ang_vel_in(self, frame_or_body):
""" Angular velocity of body wrt to another frame or body
This is just a wrapper for the ReferenceFrame ang_vel_in function
"""
if isinstance(frame_or_body, ReferenceFrame):
return self.frame.ang_vel_in(frame_or_body)
else:
if issubclass(type(frame_or_body), YAMSBody):
return self.frame.ang_vel_in(frame_or_body.frame)
else:
raise Exception(
'Unknown class type, use ReferenceFrame of YAMSBody as argument'
)
def connectTo(parent,
child,
type='Rigid',
rel_pos=None,
rot_type='Body',
rot_amounts=None,
rot_order=None,
dynamicAllowed=False):
# register parent/child relationship
child.parent = parent
parent.children.append(child)
if isinstance(parent, YAMSInertialBody):
parent.inertial_frame = parent.frame
child.inertial_frame = parent.frame
parent.inertial_origin = parent.origin
child.inertial_origin = parent.origin
else:
if parent.inertial_frame is None:
raise Exception(
'Parent body was not connected to an inertial frame. Bodies needs to be connected in order, starting from inertial frame.'
)
else:
child.inertial_frame = parent.inertial_frame # the same frame is used for all connected bodies
child.inertial_origin = parent.origin
if rel_pos is None or len(rel_pos) != 3:
raise Exception('rel_pos needs to be an array of size 3')
pos = 0 * parent.frame.x
vel = 0 * parent.frame.x
t = dynamicsymbols._t
if type == 'Free':
# --- "Free", "floating" connection
if not isinstance(parent, YAMSInertialBody):
raise Exception(
'Parent needs to be inertial body for a free connection')
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
vel += diff(d, t) * e
elif type == 'Rigid':
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
if exprHasFunction(d) and not dynamicAllowed:
raise Exception(
'Position variable cannot be a dynamic variable for a rigid connection: variable {}'
.format(d))
if dynamicAllowed:
vel += diff(d, t) * e
elif type == 'Joint':
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
if exprHasFunction(d) and not dynamicAllowed:
raise Exception(
'Position variable cannot be a dynamic variable for a joint connection, variable: {}'
.format(d))
if dynamicAllowed:
vel += diff(d, t) * e
# Orientation
if rot_amounts is None:
raise Exception(
'rot_amounts needs to be provided with Joint connection')
for d in rot_amounts:
if d != 0 and not exprHasFunction(d):
raise Exception(
'Rotation amount variable should be a dynamic variable for a joint connection, variable: {}'
.format(d))
else:
raise Exception('Unsupported joint type: {}'.format(type))
# Orientation (creating a path connecting frames together)
if rot_amounts is None:
child.frame.orient(parent.frame, 'Axis', (0, parent.frame.x))
else:
if rot_type in ['Body', 'Space']:
child.frame.orient(parent.frame, rot_type, rot_amounts,
rot_order) # <<<
else:
child.frame.orient(parent.frame, rot_type, rot_amounts) # <<<
# Position of child origin wrt parent origin
child.origin.set_pos(parent.origin, pos)
# Velocity of child origin frame wrt parent frame (0 for rigid or joint)
child.origin.set_vel(parent.frame, vel)
# Velocity of child masscenter wrt parent frame, based on origin vel (NOTE: for rigid body only, should be overriden for flexible body)
child.masscenter.v2pt_theory(child.origin, parent.frame, child.frame)
# Velocity of child origin wrt inertial frame, using parent origin/frame as intermediate
child.origin.v1pt_theory(parent.origin, child.inertial_frame,
parent.frame)
# Velocity of child masscenter wrt inertial frame, using parent origin/frame as intermediate
child.masscenter.v1pt_theory(parent.origin, child.inertial_frame,
parent.frame)
#r_OB = child.origin.pos_from(child.inertial_origin)
#vel_OB = r_OB.diff(t, child.inertial_frame)
# --------------------------------------------------------------------------------}
# --- Visualization
# --------------------------------------------------------------------------------{
def vizOrigin(self, radius=1.0, color='black', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.origin,
Sphere(color=color, radius=radius))
def vizCOG(self, radius=1.0, color='red', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.masscenter,
Sphere(color=color, radius=radius))
def vizFrame(self, radius=0.1, length=1.0, format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
from sympy.physics.mechanics import Point
X_frame = self.frame.orientnew(
'ffx', 'Axis', (-np.pi / 2, self.frame.z)) # Make y be x
Z_frame = self.frame.orientnew(
'ffz', 'Axis', (+np.pi / 2, self.frame.x)) # Make y be z
X_shape = Cylinder(radius=radius, length=length,
color='red') # Cylinder are along y
Y_shape = Cylinder(radius=radius, length=length, color='green')
Z_shape = Cylinder(radius=radius, length=length, color='blue')
X_center = Point('X')
X_center.set_pos(self.origin, length / 2 * X_frame.y)
Y_center = Point('Y')
Y_center.set_pos(self.origin, length / 2 * self.frame.y)
Z_center = Point('Z')
Z_center.set_pos(self.origin, length / 2 * Z_frame.y)
X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape)
Y_viz_frame = VisualizationFrame(self.frame, Y_center, Y_shape)
Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape)
return X_viz_frame, Y_viz_frame, Z_viz_frame
def vizAsCylinder(self,
radius,
length,
axis='z',
color='blue',
offset=0,
format='pydy'):
""" """
if format == 'pydy':
# pydy cylinder is along y and centered at the middle of the cylinder
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
if axis == 'y':
e = self.frame
a = self.frame.y
elif axis == 'z':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.x))
a = self.frame.z
elif axis == 'x':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.z))
a = self.frame.x
shape = Cylinder(radius=radius, length=length, color=color)
center = Point('CC_' + self.name)
center.set_pos(self.origin, (length / 2 + offset) * a)
return VisualizationFrame(e, center, shape)
else:
raise NotImplementedError()
def vizAsRotor(self,
radius=0.1,
length=1,
nB=3,
axis='x',
color='white',
format='pydy'):
# --- Bodies visualization
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
blade_shape = Cylinder(radius=radius, length=length, color=color)
viz = []
if axis == 'x':
for iB in np.arange(nB):
frame = self.frame.orientnew(
'b', 'Axis', (-np.pi / 2 + (iB - 1) * 2 * np.pi / nB,
self.frame.x)) # Y pointing along blade
center = Point('RB')
center.set_pos(self.origin, length / 2 * frame.y)
viz.append(VisualizationFrame(frame, center, blade_shape))
return viz
else:
raise NotImplementedError()
one_length = symbols('l_1')
two = Point('2')
two.set_pos(one, one_length*one_frame.y)
three = Point('3')
two_length = symbols('l_2')
three.set_pos(two, two_length*two_frame.y)
#Sets up the angular velocities
omega1, omega2 = dynamicsymbols('omega1, omega2')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
omega2 - theta2.diff()]
#Sets up the rotational axes of the angular velocities
one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
one_frame.ang_vel_in(inertial_frame)
two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z)
two_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
#one.set_vel(inertial_frame, 0)
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
three.v2pt_theory(two, inertial_frame, two_frame)
three.vel(inertial_frame)
#Sets up the masses of the linkages
one_mass, two_mass = symbols('m_1, m_2')
#Defines the linkages as particles
twoP = Particle('twoP', two, one_mass)
pO = Point('O')
pS_star = pO.locatenew('S*', b*A.y)
pS_hat = pS_star.locatenew('S^', -r*B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1', -r*A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r*A.y) # S2 touches vertical wall of the race
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
# Since S is rolling against C, v_S^_C = 0.
# Cone has only angular velocity in R.z direction.
vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A]
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc_map = solve(vc, u)
# Pure rolling between S and C, dot(ω_C_S, B.y) = 0.
b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0]
print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r))))
b_expected = r*(1 + sin(theta))/(cos(theta) - sin(theta))
assert trigsimp(b_val - b_expected) == 0
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(expand((rbC.kinetic_energy(R) +
4*rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2/2))))
from __future__ import print_function, division
from sympy import symbols, simplify
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
inertial_frame = ReferenceFrame('I')
lower_leg_frame = ReferenceFrame('L')
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
lower_leg_frame.dcm(inertial_frame)
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
print(lower_leg_frame.ang_vel_in(inertial_frame))
# Define kinematical differential equations
# =========================================
omega1_dp1, omega2_dp1, omega1_dp2, omega2_dp2 = dynamicsymbols("omega_1dp1, omega_2dp1, omega_1dp2, omega_2dp2")
time = symbols("t")
kinematical_differential_equations_dp1 = [omega1_dp1 - theta1_dp1.diff(time), omega2_dp1 - theta2_dp2.diff(time)]
kinematical_differential_equations_dp2 = [omega1_dp2 - theta1_dp2.diff(time), omega2_dp2 - theta2_dp2.diff(time)]
# Angular Velocities
# ==================
one_frame_dp1.set_ang_vel(inertial_frame_dp1, omega1_dp1 * inertial_frame_dp1.z)
one_frame_dp1.ang_vel_in(inertial_frame_dp1)
one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z)
one_frame_dp2.ang_vel_in(inertial_frame_dp2)
two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z)
two_frame_dp1.ang_vel_in(inertial_frame_dp1)
two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z)
two_frame_dp2.ang_vel_in(inertial_frame_dp2)
# Linear Velocities
# =================
one_dp1.set_vel(inertial_frame_dp1, 0)
one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel)
pS_star = pO.locatenew('S*', b * A.y)
pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1',
-r * A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
# Since S is rolling against C, v_S^_C = 0.
# Cone has only angular velocity in R.z direction.
vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A]
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc_map = solve(vc, u)
# Pure rolling between S and C, dot(ω_C_S, B.y) = 0.
b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0]
print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r))))
b_expected = r * (1 + sin(theta)) / (cos(theta) - sin(theta))
assert trigsimp(b_val - b_expected) == 0
