def test_particle():
m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
P = Point('P')
P2 = Point('P2')
p = Particle('pa', P, m)
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame('N')
O = Point('O')
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r *v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.set_potential_energy(m * g * h)
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(
N) in [m2*(v1**2 + v2**2 + v3**2)/2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2]
def test_particle():
m, m2, v1, v2, v3, r = symbols('m m2 v1 v2 v3 r')
P = Point('P')
P2 = Point('P2')
p = Particle('pa', P, m)
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame('N')
O = Point('O')
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
assert p.linearmomentum(N) == m2 * v1 * N.x
assert p.angularmomentum(O, N) == -m2 * r *v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linearmomentum(N) == m2 * v2 * N.y
assert p.angularmomentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linearmomentum(N) == m2 * v3 * N.z
assert p.angularmomentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linearmomentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angularmomentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
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 second_order_system():
# from sympy.printing.pycode import NumPyPrinter, pycode
coordinates = dynamicsymbols('q:1') # Generalized coordinates
speeds = dynamicsymbols('u:1') # Generalized speeds
# Force applied to the cart
cart_thrust = dynamicsymbols('thrust')
m = sp.symbols('m:1') # Mass of each bob
g, t = sp.symbols('g t')
# Gravity and time
ref_frame = ReferenceFrame('I') # Inertial reference frame
origin = Point('O') # Origin point
origin.set_vel(ref_frame, 0) # Origin's velocity is zero
P0 = Point('P0') # Hinge point of top link
# Set the position of P0
P0.set_pos(origin, coordinates[0] * ref_frame.x)
P0.set_vel(ref_frame, speeds[0] * ref_frame.x) # Set the velocity of P0
Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0
# List to hold the n + 1 frames
frames = [ref_frame]
points = [P0] # List to hold the n + 1 points
# List to hold the n + 1 particles
particles = [Pa0]
# List to hold the n + 1 applied forces, including the input force, f
applied_forces = [(P0, cart_thrust * ref_frame.x - m[0] * g *
ref_frame.y)]
# List to hold kinematic ODE's
kindiffs = [coordinates[0].diff(t) - speeds[0]]
# Initialize the object
kane = KanesMethod(ref_frame, q_ind=coordinates,
u_ind=speeds, kd_eqs=kindiffs)
# Generate EoM's fr + frstar = 0
fr, frstar = kane.kanes_equations(particles, applied_forces)
state = coordinates + speeds
gain = [cart_thrust]
kindiff_dict = kane.kindiffdict()
M = kane.mass_matrix_full.subs(kindiff_dict)
F = kane.forcing_full.subs(kindiff_dict)
static_parameters = [g, m[0]]
transfer = M.inv() * F
return DynamicSystem(state, gain, static_parameters, transfer)
def test_rigidbody2():
M, v, r, omega = dynamicsymbols('M v r omega')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer (b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angularmomentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z
def test_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.potential_energy = M * g * h
assert B.potential_energy == M * g * h
assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
def test_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.set_potential_energy(M * g * h)
assert B.potential_energy == M * g * h
assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S(1))
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S.One)
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass():
a = ReferenceFrame("a")
m = symbols("m", real=True)
p1 = Particle("p1", Point("p1_pt"), S.One)
p2 = Particle("p2", Point("p2_pt"), S(2))
p3 = Particle("p3", Point("p3_pt"), S(3))
p4 = Particle("p4", Point("p4_pt"), m)
b_f = ReferenceFrame("b_f")
b_cm = Point("b_cm")
mb = symbols("mb")
b = RigidBody("b", b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o = Point("o")
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = (5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb /
(m + mb + 6) * a.z)
assert point_o.pos_from(p1.point) - expr == 0
def test_particle():
m, m2, v1, v2, v3, r, g, h = symbols("m m2 v1 v2 v3 r g h")
P = Point("P")
P2 = Point("P2")
p = Particle("pa", P, m)
assert p.__str__() == "pa"
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame("N")
O = Point("O")
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
raises(TypeError, lambda: Particle(P, P, m))
raises(TypeError, lambda: Particle("pa", m, m))
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.potential_energy = m * g * h
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(N) in [
m2 * (v1**2 + v2**2 + v3**2) / 2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2,
]
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()
def frame_viz(Origin, frame, l=1, r=0.08):
""" """
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
from sympy.physics.mechanics import Point
#
X_frame = frame.orientnew('ffx', 'Axis', (-np.pi/2, frame.z) ) # Make y be x
Z_frame = frame.orientnew('ffz', 'Axis', (+np.pi/2, frame.x) ) # Make y be z
X_shape = Cylinder(radius=r, length=l, color='red') # Cylinder are along y
Y_shape = Cylinder(radius=r, length=l, color='green')
Z_shape = Cylinder(radius=r, length=l, color='blue')
X_center=Point('X'); X_center.set_pos(Origin, l/2 * X_frame.y)
Y_center=Point('Y'); Y_center.set_pos(Origin, l/2 * frame.y)
Z_center=Point('Z'); Z_center.set_pos(Origin, l/2 * Z_frame.y)
X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape)
Y_viz_frame = VisualizationFrame( 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 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
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
s_frame = ReferenceFrame('S')
#Sets up symbols for joint angles
theta_s = dynamicsymbols('theta_s')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
s_frame.orient(inertial_frame, 'Axis', (theta_s, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
S = Point('S')
s_length = symbols('l_s')
T = Point('T')
T.set_pos(S, s_length*s_frame.y)
#Sets up the angular velocities
omega_s = dynamicsymbols('omega_s')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_s - theta_s.diff()]
#Sets up the rotational axes of the angular velocities
s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z)
#Sets up the linear velocities of the points on the linkages
S.set_vel(inertial_frame, 0)
T.v2pt_theory(S, inertial_frame, s_frame)
#Sets up the masses of the linkages
s_mass = symbols('m_s')
one_frame = ReferenceFrame('1')
two_frame = ReferenceFrame('2')
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))
#Sets up points for the joints and places them relative to each other
one = Point('1')
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)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'axis', [q2, A.z])
C = N.orientnew('C', 'Axis', [theta, N.z])
# Joints location and speed
J0 = Point('J0')
J0.set_vel(N, 0)
J1 = J0.locatenew('J1', bb * A.y)
J2 = J1.locatenew('J2', ob * B.y)
J1.v2pt_theory(J0, N, A)
J2.v2pt_theory(J1, N, B)
#Pedal position and speed
O = Point('O')
O.set_pos(J0, -d2 * N.x + d1 * N.y)
O.set_vel(N, 0 * N.z)
S = O.locatenew('S', -ta *
C.y) # hoek van pedaal vanaf het hoogste punt van de cirkel
S.v2pt_theory(O, N, C)
# Centres of mass and speed
P = J0.locatenew('P', 0.5 * bb * A.y)
R = J1.locatenew('R', 0.5 * ob * B.y)
T = O.locatenew('T', 0.5 * ta * C.y)
P.v2pt_theory(J0, N, A)
R.v2pt_theory(J1, N, B)
T.v2pt_theory(O, N, C)
# Bodies
IP = inertia(A, 1 / 12 * m * (3 * r**2 + bb**2), 0,
body_frame = ReferenceFrame("B")
body_frame.orient(l_leg_frame, "Axis", (theta2, l_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
l_leg_length, hip_width = symbols("l_L, h_W")
l_ankle = Point("LA")
l_hip = Point("LH")
l_hip.set_pos(l_ankle, l_leg_length * l_leg_frame.y)
r_hip = Point("RH")
r_hip.set_pos(l_hip, -1 * hip_width * body_frame.x)
# Center of mass locations
# ------------------------
l_leg_com_length, body_com_length, r_leg_com_length, body_com_height = symbols("d_L, d_B, d_R, d_BH")
l_leg_mass_center = Point("LL_o")
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_height * body_frame.y)
# ===============
# Joints
# ------
a_length, b_length, c_length = symbols('l_a, l_b, l_c')
a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c')
A = Point('A')
B = Point('B')
C = Point('C')
D = Point('D')
com_global = Point('G_o')
com_bc = Point('BC_o')
B.set_pos(A, a_length * a_frame.y)
C.set_pos(B, b_length * b_frame.y)
D.set_pos(C, c_length * c_frame.y)
t_ai = simplify(B.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_bi = simplify(C.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ci = simplify(D.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ba = simplify(C.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
t_ca = simplify(D.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
com_global_coords = (t_ai*a_mass + t_bi*b_mass + t_ci*c_mass)/(a_mass+b_mass+c_mass)
com_bc_coords = (t_ba*b_mass + t_ba*c_mass)/(b_mass+c_mass)
theta_go = atan(com_global_coords[0]/com_global_coords[1])
theta_bco = atan(com_bc_coords[0]/com_bc_coords[1])
vol_C))
print('\nC* = {0}'.format(pCs.pos_from(pC_O)))
print('C* = {0}'.format(eval_vec(pCs.pos_from(pC_O))))
## FIND CENTER OF MASS OF D
vol_D = pi*a**3/4
pD_O = Point('D_O')
pDs = pD_O.locatenew('D*', (a*N.z + a/2*N.y +
(N.x - N.z)*(centroid_sector.subs(
theta_pi_4) * sin(pi/4))))
print('\nD* = {0}'.format(pDs.pos_from(pD_O)))
print('D* = {0}'.format(eval_vec(pDs.pos_from(pD_O))))
## FIND CENTER OF MASS OF ASSEMBLY
pO = Point('O')
pA_O.set_pos(pO, 2*a*N.x - (a+b)*N.z)
pB_O.set_pos(pO, 2*a*N.x - a*N.z)
pC_O.set_pos(pO, -(a+b)*N.z)
pD_O.set_pos(pO, -a*N.z)
density_A = 7800
density_B = 17.00
density_C = 2700
density_D = 8400
mass_A = vol_A * density_A
mass_B = vol_B * density_B
mass_C = vol_C * density_C
mass_D = vol_D * density_D
pms = pO.locatenew('m*', ((pAs.pos_from(pO)*mass_A + pBs.pos_from(pO)*mass_B +
pCs.pos_from(pO)*mass_C + pDs.pos_from(pO)*mass_D) /
j2_frame = ReferenceFrame('J2')
#declare dynamic symbols for the three joints
theta0, theta1, theta2 = dynamicsymbols('theta0, theta1, theta2')
#orient frames
j0_frame.orient(inertial_frame, 'Axis', (theta0, inertial_frame.y))
j1_frame.orient(j0_frame, 'Axis', (theta1, j0_frame.z))
j2_frame.orient(j1_frame, 'Axis', (theta2, j1_frame.x))
#TODO figure out better name for joint points
#init joints
joint0 = Point('j0')
j0_length = symbols('l_j0')
joint1 = Point('j1')
joint1.set_pos(joint0, j0_length * j0_frame.y)
j1_length = symbols('l_j1')
joint2 = Point('j2')
joint2.set_pos(joint1, j1_length * j1_frame.y)
#create End Effector
EE = Point('E')
#COM locations
j0_com_length, j1_com_length, j2_com_length = symbols('d_j0, d_j1, d_j2')
j0_mass_center = Point('j0_o')
j0_mass_center.set_pos(joint0, j0_com_length * j0_frame.y)
j1_mass_center = Point('j1_o')
j1_mass_center.set_pos(joint1, j1_com_length * j1_frame.y)
j2_mass_center = Point('j2_o')
j2_mass_center.set_pos(joint2, j2_com_length * j2_frame.y)
EE.set_pos(joint2, j2_com_length * 2 * j2_frame.y)
n = 1
q = dynamicsymbols('q:' + str(n + 1)) # Generalized coordinates
u = dynamicsymbols('u:' + str(n + 1)) # Generalized speeds
f = dynamicsymbols('f') # Force applied to the cart
m = symbols('m:' + str(n + 1)) # Mass of each bob
length = symbols('l:' + str(n)) # Length of each link
g, t = symbols('g t') # Gravity and time
ref_frame = ReferenceFrame('I') # Inertial reference frame
origin = Point('O') # Origin point
origin.set_vel(ref_frame, 0) # Origin's velocity is zero
P0 = Point('P0') # Hinge point of top link
P0.set_pos(origin, q[0] * ref_frame.x) # Set the position of P0
P0.set_vel(ref_frame, u[0] * ref_frame.x) # Set the velocity of P0
Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0
# List to hold the n + 1 frames
frames = [ref_frame]
points = [P0] # List to hold the n + 1 points
particles = [Pa0] # List to hold the n + 1 particles
# List to hold the n + 1 applied forces, including the input force, f
forces = [
(P0, f * ref_frame.x - m[0] * g * ref_frame.y - 0.01 * q[0] * ref_frame.x)
]
kindiffs = [q[0].diff(t) - u[0]] # List to hold kinematic ODE's
for i in range(n):
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
ankle = Point('A')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
# Center of mass locations
# ------------------------
lower_leg_com_length, upper_leg_com_length = symbols('d_L, d_U')
lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)
upper_leg_mass_center = Point('U_o')
upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
# Define kinematical differential equations
# =========================================
two_frame_dp2.orient(one_frame_dp2, "Axis", (theta2_dp2, one_frame_dp2.z))
# Point Locations
# ===============
# Joints
# ------
one_length_dp1, one_length_dp2, two_length_dp2 = symbols("l_1dp1, l_1dp2, l_2dp2")
one_dp1 = Point("1_dp1")
one_dp2 = Point("1_dp2")
two_dp1 = Point("2_dp1")
two_dp2 = Point("2_dp2")
two_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y)
two_dp2.set_pos(one_dp2, one_length_dp2 * one_frame_dp2.y)
three_dp2 = Point("3_dp2")
three_dp2.set_pos(two_dp2, two_length_dp2 * two_frame_dp2.y)
# Center of mass locations
# ------------------------
one_com_xdp1, one_com_ydp1, two_com_xdp1, two_com_ydp1 = dynamicsymbols(
"1_comx_dp1, 1_comy_dp1, 2_comx_dp1, 2_comy_dp1"
)
one_mass_center_dp1 = Point("1_odp1")
one_mass_center_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y)
body_frame = ReferenceFrame("B")
body_frame.orient(crotch_frame, "Axis", (theta4, crotch_frame.z))
# Point Locations
# ===============
# Joints
# ------
l_leg_length, hip_width, body_height = symbols("l_L, h_W, b_H")
l_ankle = Point("LA")
l_hip = Point("LH")
l_hip.set_pos(l_ankle, l_leg_length * l_leg_frame.y)
r_hip = Point("RH")
r_hip.set_pos(l_hip, hip_width * crotch_frame.x)
body_middle = Point("B_m")
body_middle.set_pos(l_hip, (hip_width / 2) * crotch_frame.x)
waist = Point("W")
waist.set_pos(body_middle, body_height * crotch_frame.y)
# Center of mass locations
# ------------------------
l_leg_com_length, r_leg_com_length, crotch_com_height, body_com_height = symbols("d_L, d_R, d_C, d_B")
torso_frame = ReferenceFrame('T')
torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
ankle = Point('A')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
# Center of mass locations
# ------------------------
lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols('d_L, d_U, d_T')
lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)
upper_leg_mass_center = Point('U_o')
upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
def get_model(model_name, **opts):
"""
model: string defining model
opts: dictionary of options with keys:
see _defaultOpts
"""
for k, v in _defaultOpts.items():
if k not in opts.keys():
opts[k] = v
for k, v in opts.items():
if k not in _defaultOpts.keys():
raise Exception(
'Key {} not supported for model options.'.format(k))
#print(opts)
# Extract info from model name
sFnd = model_name.split('T')[0][1:]
if len(sFnd) == 1:
nDOF_fnd = int(sFnd[0])
bFndDOFs = [False] * 6
bFndDOFs[0] = nDOF_fnd >= 1 # x
bFndDOFs[4] = nDOF_fnd >= 2 # phiy
bFndDOFs[2] = nDOF_fnd == 3 or nDOF_fnd == 6 # z
bFndDOFs[1] = nDOF_fnd >= 5 # y
bFndDOFs[3] = nDOF_fnd >= 5 # phi_x
bFndDOFs[5] = nDOF_fnd >= 5 # phi_z
else:
bFndDOFs = [s == '1' for s in sFnd]
nDOF_fnd = sum(bFndDOFs)
nDOF_twr = int(model_name.split('T')[1][0])
bFullRNA = model_name.find('RNA') == -1
bBlades = False # Rotor
nDOF_nac = 0
nDOF_sft = 0
nDOF_bld = 0
if bFullRNA:
nDOF_nac = int(model_name.split('N')[1][0])
nDOF_sft = int(model_name.split('S')[1][0])
bBlades = model_name.find('B') > 0
if bBlades:
nDOF_bld = model_name.split('B')[1][0]
else:
nDOF_bld = 0
#print('fnd',','.join(['1' if b else '0' for b in bFndDOFs]), 'twr',nDOF_twr, 'nac',nDOF_nac, 'sft',nDOF_sft, 'bld',nDOF_bld)
# --------------------------------------------------------------------------------}
# --- Isolated bodies
# --------------------------------------------------------------------------------{
# Reference frame
ref = YAMSInertialBody('E')
# Foundation, floater, always rigid for now
if (not opts['floating']) or opts['mergeFndTwr']:
fnd = None # the floater is merged with the twr, or we are not floating
else:
fnd = YAMSRigidBody('F', rho_G=[0, 0, z_FG], J_diag=True)
# Tower
if nDOF_twr == 0:
# Ridid tower
twr = YAMSRigidBody('T', rho_G=[0, 0, z_TG], J_diag=True)
twrDOFs = []
twrSpeeds = []
elif nDOF_twr <= 4:
# Flexible tower
twr = YAMSFlexibleBody('T',
nDOF_twr,
directions=opts['twrDOFDir'],
orderMM=opts['orderMM'],
orderH=opts['orderH'],
predefined_kind='twr-z')
twrDOFs = twr.q
twrSpeeds = twr.qd
# Nacelle rotor assembly
if bFullRNA:
# Nacelle
nac = YAMSRigidBody('N', rho_G=[x_NG, 0, z_NG], J_cross=True)
# Shaft
#...
# Individual blades or rotor
if bBlades:
# Individual blades
raise NotImplementedError()
else:
# Rotor
rot = YAMSRigidBody('R', rho_G=[0, 0, 0], J_diag=True)
rot.inertia = (inertia(rot.frame, Jxx_R, JO_R, JO_R), rot.origin
) # defining inertia at orign
else:
# Nacelle
#nac = YAMSRigidBody('RNA', rho_G = [x_RNAG ,0, z_RNAG], J_diag=True)
nac = YAMSRigidBody('RNA', rho_G=[x_RNAG, 0, z_RNAG], J_cross=True)
rot = None
# --------------------------------------------------------------------------------}
# --- Body DOFs
# --------------------------------------------------------------------------------{
# Fnd
if (not opts['floating']):
fndDOFs = []
fndSpeeds = []
else:
fndDOFsAll = [x, y, z, phi_x, phi_y, phi_z]
fndSpeedsAll = [xd, yd, zd, omega_x_T, omega_y_T, omega_z_T]
fndDOFs = [dof for active, dof in zip(bFndDOFs, fndDOFsAll) if active]
fndSpeeds = [
dof for active, dof in zip(bFndDOFs, fndSpeedsAll) if active
]
# Twr
# Nac
if nDOF_nac == 2:
opts['yaw'] = 'dynamic'
opts['tilt'] = 'dynamic'
if opts['tiltShaft'] and opts['tilt'] == 'dynamic':
raise Exception('Cannot do tiltshaft with tilt dynamic')
yawDOF = {'zero': 0, 'fixed': theta_yaw, 'dynamic': q_yaw}[opts['yaw']]
tiltDOF = {'zero': 0, 'fixed': theta_tilt, 'dynamic': q_tilt}[opts['tilt']]
nacDOFs = []
nacSpeeds = []
nacKDEqSubs = []
if opts['yaw'] == 'dynamic':
nacDOFs += [q_yaw]
nacSpeeds += [qd_yaw]
nacKDEqSubs += [(qd_yaw, diff(q_yaw, time))]
if opts['tilt'] == 'dynamic':
nacDOFs += [q_tilt]
nacSpeeds += [qd_tilt]
nacKDEqSubs += [(qd_tilt, diff(q_tilt, time))]
nacDOFsAct = (opts['yaw'] == 'dynamic', opts['tilt'] == 'dynamic')
if nDOF_nac == 0:
if not (nacDOFsAct == (False, False)):
raise Exception(
'If nDOF_nac is 0, yaw and tilt needs to be "fixed" or "zero"')
elif nDOF_nac == 1:
if not (nacDOFsAct == (True, False) or nacDOFsAct == (False, True)):
raise Exception(
'If nDOF_nac is 1, yaw or tilt needs to be "dynamic"')
else:
if not (nacDOFsAct == (True, True)):
raise Exception(
'If nDOF_nac is 2, yaw and tilt needs to be "dynamic"')
# Shaft
sftDOFs = []
sftSpeeds = []
if bFullRNA:
if nDOF_sft == 1:
sftDOFs = [psi]
sftSpeeds = [omega_x_R]
elif nDOF_sft == 0:
pass
else:
raise Exception('nDOF shaft should be 0 or 1')
# Blade Rotor
if bBlades:
pass
else:
pass
coordinates = fndDOFs + twrDOFs + nacDOFs + sftDOFs
speeds = fndSpeeds + twrSpeeds + nacSpeeds + sftSpeeds # Order determine eq order
# --------------------------------------------------------------------------------}
# --- Connections between bodies
# --------------------------------------------------------------------------------{
z_OT = Symbol('z_OT')
if opts['floating']:
rel_pos = [0, 0, 0]
rel_pos[0] = x if bFndDOFs[0] else 0
rel_pos[1] = y if bFndDOFs[1] else 0
rel_pos[2] = z + z_OT if bFndDOFs[2] else z_OT
rots = [0, 0, 0]
rots[0] = phi_x if bFndDOFs[3] else 0
rots[1] = phi_y if bFndDOFs[4] else 0
rots[2] = phi_z if bFndDOFs[5] else 0
if nDOF_fnd == 0:
#print('Rigid connection ref twr', rel_pos)
ref.connectTo(twr, type='Rigid', rel_pos=rel_pos)
elif nDOF_fnd == 1:
#print('Constraint connection ref twr')
ref.connectTo(twr,
type='Free',
rel_pos=(x, 0, z_OT),
rot_amounts=(0, x * symbols('nu'), 0),
rot_order='XYZ')
else:
#print('Free connection ref twr', rel_pos, rots)
ref.connectTo(
twr,
type='Free',
rel_pos=rel_pos,
rot_amounts=rots,
rot_order='XYZ'
) #NOTE: rot order is not "optimal".. phi_x should be last
#ref.connectTo(twr, type='Free' , rel_pos=rel_pos, rot_amounts=(rots[2],rots[1],rots[0]), rot_order='ZYX') #NOTE: rot order is not "optimal".. phi_x should be last
else:
#print('Rigid connection ref twr')
ref.connectTo(twr, type='Rigid', rel_pos=(0, 0, 0))
# Rigid connection between twr and fnd if fnd exists
if fnd is not None:
#print('Rigid connection twr fnd')
if nDOF_twr == 0:
twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F
else:
twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F
if nDOF_twr == 0:
# Tower rigid -> Rigid connection to nacelle
# TODO TODO L_T or twr.L
#if nDOF_nac==0:
#print('Rigid connection twr nac')
#else:
#print('Dynamic connection twr nac')
if opts['tiltShaft']:
# Shaft will be tilted, not nacelle
twr.connectTo(nac,
type='Rigid',
rel_pos=(0, 0, L_T),
rot_amounts=(yawDOF, 0, 0),
rot_order='ZYX')
else:
# Nacelle is tilted
twr.connectTo(nac,
type='Rigid',
rel_pos=(0, 0, L_T),
rot_amounts=(yawDOF, tiltDOF, 0),
rot_order='ZYX')
else:
# Flexible tower -> Flexible connection to nacelle
#print('Flexible connection twr nac')
if opts['tiltShaft']:
twr.connectToTip(nac,
type='Joint',
rel_pos=(0, 0, twr.L),
rot_amounts=(yawDOF, 0, 0),
rot_order='ZYX',
rot_type_elastic=opts['rot_elastic_type'],
doSubs=opts['rot_elastic_subs'])
else:
twr.connectToTip(nac,
type='Joint',
rel_pos=(0, 0, twr.L),
rot_amounts=(yawDOF, tiltDOF, 0),
rot_order='ZYX',
rot_type_elastic=opts['rot_elastic_type'],
doSubs=opts['rot_elastic_subs'])
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
if opts['tiltShaft']:
if nDOF_sft == 0:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, tiltDOF, 0),
rot_order='ZYX')
else:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, tiltDOF, psi),
rot_order='ZYX')
else:
if nDOF_sft == 0:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, 0, 0),
rot_order='ZYX')
else:
nac.connectTo(rot,
type='Joint',
rel_pos=(x_NR, 0, z_NR),
rot_amounts=(0, 0, psi),
rot_order='ZYX')
# --- Defining Body rotational velocities
omega_TE = twr.ang_vel_in(
ref) # Angular velocity of nacelle in inertial frame
omega_NT = nac.ang_vel_in(
twr.frame) # Angular velocity of nacelle in inertial frame
if rot is not None:
omega_RN = rot.ang_vel_in(
nac.frame
) # Angular velocity of rotor wrt Nacelle (omega_R-omega_N)
# --- Kinetics
body_loads = []
bodies = []
g = symbols('g')
g_vect = -g * ref.frame.z
# Foundation/floater loads
if fnd is not None:
bodies += [fnd]
grav_F = (fnd.masscenter, -fnd.mass * g * ref.frame.z)
# Point of application for Buoyancy and mooring
P_B = twr.origin.locatenew('P_B',
z_TB * fnd.frame.z) # <<<< Measured from T
P_M = twr.origin.locatenew('P_M',
z_TM * fnd.frame.z) # <<<< Measured from T
P_B.v2pt_theory(twr.origin, ref.frame, twr.frame)
# PB & T are fixed in e_T
P_M.v2pt_theory(twr.origin, ref.frame, twr.frame)
# PM & T are fixed in e_T
if opts['fnd_loads']:
K_Mx, K_My, K_Mz = symbols(
'K_x_M, K_y_M, K_z_M') # Mooring restoring
K_Mphix, K_Mphiy, K_Mphiz = symbols(
'K_phi_x_M, K_phi_y_M, K_phi_z_M') # Mooring restoring
F_B = dynamicsymbols('F_B') # Buoyancy force
#print('>>> Adding fnd loads. NOTE: reference might need to be adapted')
# Buoyancy
body_loads += [(fnd, (P_B, F_B * ref.frame.z))]
## Restoring mooring and torques
fr = 0
fr += -K_Mx * x * ref.frame.x if bFndDOFs[0] else 0
fr += -K_My * y * ref.frame.y if bFndDOFs[1] else 0
fr += -K_Mz * z * ref.frame.z if bFndDOFs[2] else 0
Mr += -K_MPhix * phi_x * ref.frame.x if bFndDOFs[3] else 0
Mr += -K_MPhiy * phi_y * ref.frame.y if bFndDOFs[4] else 0
Mr += -K_MPhiz * phi_z * ref.frame.z if bFndDOFs[5] else 0
body_loads += [(fnd, (P_M, fr))]
body_loads += [(fnd, (fnd.frame, Mr))]
body_loads += [(fnd, grav_F)]
# Tower loads
grav_T = (twr.masscenter, -twr.mass * g * ref.frame.z)
bodies += [twr]
body_loads += [(twr, grav_T)]
# Nacelle loads
grav_N = (nac.masscenter, -nac.mass * g * ref.frame.z)
bodies += [nac]
body_loads += [(nac, grav_N)]
T_a = dynamicsymbols('T_a') # NOTE NOTE
#T_a = Function('T_a')(dynamicsymbols._t, *coordinates, *speeds) # NOTE: to introduce it in the linearization, add coordinates
M_ax, M_ay, M_az = dynamicsymbols('M_x_a, M_y_a, M_z_a') # Aero torques
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
# Rotor loads
grav_R = (rot.masscenter, -M_R * g * ref.frame.z)
bodies += [rot]
body_loads += [(rot, grav_R)]
# NOTE: loads on rot, but expressed in N frame
if opts['tiltShaft']:
# TODO actually tilt shaft, introduce non rotating shaft body
if opts['aero_forces']:
thrustR = (rot.origin, T_a * cos(tiltDOF) * nac.frame.x -
T_a * sin(tiltDOF) * nac.frame.z)
if opts['aero_torques']:
M_a_R = (nac.frame, M_ax * nac.frame.x * 0) # TODO TODO
#print('>>> WARNING tilt shaft aero moments not implemented') # TODO
body_loads += [(nac, M_a_R)]
else:
thrustR = (rot.origin, T_a * nac.frame.x)
#thrustR = (rot.origin, T_a * rot.frame.x )
#M_a_R = (rot.frame, M_ax*rot.frame.x + M_ay*rot.frame.y + M_az*rot.frame.z) # TODO TODO TODO introduce a non rotating shaft
if opts['aero_torques']:
M_a_R = (nac.frame, M_ax * nac.frame.x +
M_ay * nac.frame.y + M_az * nac.frame.z)
body_loads += [(nac, M_a_R)]
body_loads += [(rot, thrustR)]
else:
# RNA loads, point load at R
R = Point('R')
R.set_pos(nac.origin, x_NR * nac.frame.x + z_NR * nac.frame.z)
R.set_vel(nac.frame, 0 * nac.frame.x)
R.v2pt_theory(nac.origin, ref.frame, nac.frame)
#thrustN = (nac.masscenter, T * nac.frame.x)
if opts['tiltShaft']:
thrustN = (R, T_a * cos(tiltDOF) * nac.frame.x -
T_a * sin(tiltDOF) * nac.frame.z)
else:
thrustN = (R, T_a * nac.frame.x)
if opts['aero_forces']:
body_loads += [(nac, thrustN)]
if opts['aero_torques']:
#print('>>> Adding aero torques')
if opts['tiltShaft']:
# NOTE: for a rigid RNA we keep only M_y and M_z, no shaft torque
x_tilted = cos(tiltDOF) * nac.frame.x - sin(
tiltDOF) * nac.frame.z
z_tilted = cos(tiltDOF) * nac.frame.y + sin(
tiltDOF) * nac.frame.x
M_a_N = (nac.frame, M_ay * nac.frame.y + M_az * z_tilted)
else:
M_a_N = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y +
M_az * nac.frame.z)
body_loads += [(nac, M_a_N)]
# --------------------------------------------------------------------------------}
# --- Kinematic equations
# --------------------------------------------------------------------------------{
kdeqsSubs = []
# --- Fnd
if not opts['floating']:
pass
else:
# Kdeqs for fnd:
# : (xd, diff(x,time)) and (omega_y_T, diff(phi_y,time))
fndVelAll = [diff(x, time), diff(y, time), diff(z, time)]
if opts['linRot']:
fndVelAll += [
diff(phi_x, time),
diff(phi_y, time),
diff(phi_z, time)
]
else:
#print('>>>>>>>> TODO sort out which frame')
#fndVelAll +=[ omega_TE.dot(ref.frame.x).simplify(), omega_TE.dot(ref.frame.y).simplify(), omega_TE.dot(ref.frame.z).simplify()]
fndVelAll += [
omega_TE.dot(twr.frame.x).simplify(),
omega_TE.dot(twr.frame.y).simplify(),
omega_TE.dot(twr.frame.z).simplify()
]
kdeqsSubs += [(fndSpeedsAll[i], fndVelAll[i])
for i, dof in enumerate(bFndDOFs) if dof]
# --- Twr
if nDOF_twr == 0:
pass
else:
kdeqsSubs += [(twr.qd[i], twr.qdot[i]) for i, _ in enumerate(twr.q)]
# --- Nac
kdeqsSubs += nacKDEqSubs
# --- Shaft
if bFullRNA:
if bBlades:
raise NotImplementedError()
else:
if nDOF_sft == 1:
#print('>>>>>>>> TODO sort out which frame')
# I believe we should use omega_RE
kdeqsSubs += [(omega_x_R, omega_RN.dot(rot.frame.x).simplify())
]
# --- Create a YAMS wrapper model
model = YAMSModel(name=model_name)
model.opts = opts
model.ref = ref
model.bodies = bodies
model.body_loads = body_loads
model.coordinates = coordinates
model.speeds = speeds
model.kdeqsSubs = kdeqsSubs
#print(model)
model.fnd = fnd
model.twr = twr
model.nac = nac
model.rot = rot
#model.sft=sft
#model.bld=bld
model.g_vect = g_vect
# Small angles
model.smallAnglesFnd = [phi_x, phi_y, phi_z]
if nDOF_twr > 0:
if opts['rot_elastic_smallAngle']:
model.smallAnglesTwr = twr.vcList
else:
model.smallAnglesTwr = []
else:
model.smallAnglesTwr = []
model.smallAnglesNac = []
if opts['yaw'] == 'dynamic':
model.smallAnglesNac += [q_yaw]
if opts['tilt'] == 'dynamic':
model.smallAnglesNac += [q_tilt]
model.smallAngles = model.smallAnglesFnd + model.smallAnglesTwr + model.smallAnglesNac
# Shape normalization
if nDOF_twr > 0:
model.shapeNormSubs = [(v, 1) for v in twr.ucList]
else:
model.shapeNormSubs = []
return model
c_frame = ReferenceFrame('C')
#Sets up symbols for joint angles
theta_a, theta_b, theta_c = dynamicsymbols('theta_a, theta_b, theta_c')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
b_frame.orient(a_frame, 'Axis', (theta_b, a_frame.z))
c_frame.orient(b_frame, 'Axis', (theta_c, b_frame.z))
#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
C = Point('C')
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
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))
# Point Locations
# ===============
# Joints
# ------
one_length = symbols('l_1')
one = Point('1')
two = Point('2')
two.set_pos(one, one_length * one_frame.y)
# Center of mass locations
# ------------------------
one_com_x, one_com_y, two_com_x, two_com_y = dynamicsymbols('1_comx, 1_comy, 2_comx, 2_comy')
one_mass_center = Point('1_o')
one_mass_center.set_pos(one, one_com_x*inertial_frame.x + one_com_y*inertial_frame.y)
two_mass_center = Point('2_o')
two_mass_center.set_pos(two, two_com_x*inertial_frame.x + two_com_y*inertial_frame.y)
# Define kinematical differential equations
# =========================================
leg_frame = ReferenceFrame('L')
body_frame = ReferenceFrame('B')
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
body_frame.orient(leg_frame, 'Axis', (theta2, leg_frame.z))
#Sets up points for the joints and places them relative to each other
ankle = Point('A')
leg_length = symbols('l_L')
waist = Point('W')
waist.set_pos(ankle, leg_length*leg_frame.y)
body = Point('B')
body_length = symbols('l_B')
body.set_pos(waist, body_length*body_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
leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
leg_frame.ang_vel_in(inertial_frame)
body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z)
body_frame.ang_vel_in(inertial_frame)
# Referenciais
# Referencial Inercial
B0 = ReferenceFrame('B0')
# Referencial móvel: theta_1 em relação a B0.z
B1 = ReferenceFrame('B1')
B1.orient(B0, 'Axis', [THETA_1, B0.z])
# Referencial móvel: theta_2 em relação a B1.z
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis', [THETA_2, B1.z])
# Pontos e Centros de Massa
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, L_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, R_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, R_2 * B2.x)
CM_2.v2pt_theory(O, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 1
E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1))
I_2 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 2
E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
RRTethCy = symbols('RRTethCy')
RRTethCz = symbols('RRTethCz')
LFTethLx = symbols('LFTethLx') # Tether lower positions
LFTethLy = symbols('LFTethLy')
LFTethLz = symbols('LFTethLz')
RFTethLx = symbols('RFTethLx')
RFTethLy = symbols('RFTethLy')
RFTethLz = symbols('RFTethLz')
LRTethHy = symbols('LRTethHy') # Tether housing positions
RRTethHy = symbols('RRTethHy')
#___________________________________________________________________________________________________________________________________________________
# Set Chassis Positions
ChassisC.set_pos(GlobalCoord0,(ChassisDeltax * inertial_frame.x + ChassisDeltay * inertial_frame.y + ChassisDeltaz * inertial_frame.z)) # Chassis
CGpos.set_pos(ChassisC,(CGposx * Chassis_frame.x + CGposy * Chassis_frame.y + CGposz * Chassis_frame.z)) # CG Position
LUCAMP.set_pos(ChassisC,(LUCAMPx * Chassis_frame.x + LUCAMPy * Chassis_frame.y + LUCAMPz * Chassis_frame.z)) # Control Arm Mount Midpoints
RUCAMP.set_pos(ChassisC,(RUCAMPx * Chassis_frame.x + RUCAMPy * Chassis_frame.y + RUCAMPz * Chassis_frame.z))
LLCAMP.set_pos(ChassisC,(LLCAMPx * Chassis_frame.x + LLCAMPy * Chassis_frame.y + LLCAMPz * Chassis_frame.z))
RLCAMP.set_pos(ChassisC,(RLCAMPx * Chassis_frame.x + RLCAMPy * Chassis_frame.y + RLCAMPz * Chassis_frame.z))
LUBC.set_pos(ChassisC,(LUBCx * Chassis_frame.x + LUBCy * Chassis_frame.y + LUBCz * Chassis_frame.z)) # Four bar chassis positions
RUBC.set_pos(ChassisC,(RUBCx * Chassis_frame.x + RUBCy * Chassis_frame.y + RUBCz * Chassis_frame.z))
LLBC.set_pos(ChassisC,(LLBCx * Chassis_frame.x + LLBCy * Chassis_frame.y + LLBCz * Chassis_frame.z))
RLBC.set_pos(ChassisC,(RLBCx * Chassis_frame.x + RLBCy * Chassis_frame.y + RLBCz * Chassis_frame.z))
FifthShkC.set_pos(ChassisC,(FifthShkCx * Chassis_frame.x + FifthShkCy * Chassis_frame.y + FifthShkCz * Chassis_frame.z)) # Fifth coil chassis position
JbarC.set_pos(ChassisC,(JbarCx * Chassis_frame.x + JbarCy * Chassis_frame.y + JbarCz * Chassis_frame.z)) # Jbar chassis position
LFShkC.set_pos(ChassisC,(LFShkCx * Chassis_frame.x + LFShkCy * Chassis_frame.y + LFShkCz * Chassis_frame.z)) # Shock chassis positions
RFShkC.set_pos(ChassisC,(RFShkCx * Chassis_frame.x + RFShkCy * Chassis_frame.y + RFShkCz * Chassis_frame.z))
LRShkC.set_pos(ChassisC,(LRShkCx * Chassis_frame.x + LRShkCy * Chassis_frame.y + LRShkCz * Chassis_frame.z))
RRShkC.set_pos(ChassisC,(RRShkCx * Chassis_frame.x + RRShkCy * Chassis_frame.y + RRShkCz * Chassis_frame.z))
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Sets up the masses of the linkages
one_mass, two_mass = symbols('m_1, m_2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))
#Sets up points for the joints and places them relative to each other
one = Point('1')
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)
com = Point('c')
com.set_pos(two, (two_length)*(two_mass/(one_mass + two_mass))*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)
torso_frame = ReferenceFrame('T')
torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
ankle = Point('A')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
# Center of mass locations
# ------------------------
lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols(
'd_L, d_U, d_T')
lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)
upper_leg_mass_center = Point('U_o')
upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
pCs_3.pos_from(pC_O) * vol_C_3 + pCs_4.pos_from(pC_O) * vol_C_4) /
vol_C))
print('\nC* = {0}'.format(pCs.pos_from(pC_O)))
print('C* = {0}'.format(eval_vec(pCs.pos_from(pC_O))))
## FIND CENTER OF MASS OF D
vol_D = pi * a**3 / 4
pD_O = Point('D_O')
pDs = pD_O.locatenew('D*', (a * N.z + a / 2 * N.y + (N.x - N.z) *
(centroid_sector.subs(theta_pi_4) * sin(pi / 4))))
print('\nD* = {0}'.format(pDs.pos_from(pD_O)))
print('D* = {0}'.format(eval_vec(pDs.pos_from(pD_O))))
## FIND CENTER OF MASS OF ASSEMBLY
pO = Point('O')
pA_O.set_pos(pO, 2 * a * N.x - (a + b) * N.z)
pB_O.set_pos(pO, 2 * a * N.x - a * N.z)
pC_O.set_pos(pO, -(a + b) * N.z)
pD_O.set_pos(pO, -a * N.z)
density_A = 7800
density_B = 17.00
density_C = 2700
density_D = 8400
mass_A = vol_A * density_A
mass_B = vol_B * density_B
mass_C = vol_C * density_C
mass_D = vol_D * density_D
pms = pO.locatenew('m*',
((pAs.pos_from(pO) * mass_A + pBs.pos_from(pO) * mass_B +
# these can be arbitray points on each of you robots links
# later these points are visualized, so you can verify the correct build of your robot
origin = Point('Origin')
ankle_left = Point('AnkleLeft')
knee_left = Point('KneeLeft')
hip_left = Point('HipLeft')
hip_center = Point('HipCenter')
hip_right = Point('HipRight')
knee_right = Point('KneeRight')
ankle_right = Point('AnkleRight')
# here go the lengths of your robots links
lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3')
ankle_left.set_pos(origin, (0 * inertial_frame.y) + (0 * inertial_frame.x))
#ankle_left.pos_from(origin).express(inertial_frame).simplify()
knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y)
#knee_left.pos_from(origin).express(inertial_frame).simplify()
hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y)
#hip_left.pos_from(origin).express(inertial_frame).simplify()
hip_center.set_pos(hip_left, hip_length / 2 * hip_frame.x)
#hip_center.pos_from(origin).express(inertial_frame).simplify()
hip_right.set_pos(hip_center, hip_length / 2 * hip_frame.x)
#hip_right.pos_from(origin).express(inertial_frame).simplify()
knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)
# these can be arbitray points on each of you robots links
# later these points are visualized, so you can verify the correct build of your robot
origin = Point('Origin')
ankle_left = Point('AnkleLeft')
knee_left = Point('KneeLeft')
hip_left = Point('HipLeft')
hip_center = Point('HipCenter')
hip_right = Point('HipRight')
knee_right = Point('KneeRight')
ankle_right = Point('AnkleRight')
# here go the lengths of your robots links
lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3')
ankle_left.set_pos(origin, (0 * inertial_frame.y)+(0 * inertial_frame.x))
#ankle_left.pos_from(origin).express(inertial_frame).simplify()
knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y)
#knee_left.pos_from(origin).express(inertial_frame).simplify()
hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y)
#hip_left.pos_from(origin).express(inertial_frame).simplify()
hip_center.set_pos(hip_left, hip_length/2 * hip_frame.x)
#hip_center.pos_from(origin).express(inertial_frame).simplify()
hip_right.set_pos(hip_center, hip_length/2 * hip_frame.x)
#hip_right.pos_from(origin).express(inertial_frame).simplify()
knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)
a_frame = ReferenceFrame('A')
base_frame = ReferenceFrame('Base')
#Sets up symbols for joint angles
theta_a = dynamicsymbols('theta_a')
theta_base = dynamicsymbols('theta_base')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
base_frame.orient(inertial_frame, 'Axis', (theta_base, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
#Sets up the angular velocities
omega_base, omega_a = dynamicsymbols('omega_b, omega_a')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff()]
#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z)
#Sets up the linear velocities of the points on the linkages
base_vel_x, base_vel_y = dynamicsymbols('v_bx, v_by')
A.set_vel(inertial_frame, base_vel_x*base_frame.x + base_vel_y*base_frame.y)
B.v2pt_theory(A, inertial_frame, a_frame)
#Sets up the masses of the linkages
one_frame = ReferenceFrame('1')
two_frame = ReferenceFrame('2')
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))
#Sets up points for the joints and places them relative to each other
one = Point('1')
one_length = symbols('l_1')
two = Point('2')
two.set_pos(one, one_length*one_frame.y)
three = Point('3')
two_length_x = symbols('l_2x')
two_length_y = symbols('l_2y')
three.set_pos(two, two_length_x*inertial_frame.x + two_length_y*inertial_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)
