def __init__(self, location, i_body, j_body, i_rot, j_rot):
super().__init__(location, i_body, j_body)
self.type = 'universal joint'
self.name = self.name + '_uni'
self.nc = 4
self.i_rot = vector(i_rot)
self.j_rot = vector(j_rot)
self.i_rot_i = self.i_body.dcm.T.dot(self.i_rot)
self.j_rot_j = self.j_body.dcm.T.dot(self.j_rot)
self.angle = np.sin(
np.radians(vector(self.i_rot).angle_between(vector(self.j_rot))))
if abs(self.angle) <= 1e-7:
# collinear axis
self.u_irf = orient_along_axis(self.i_rot_i)
jax_j = j_body.dcm.T.dot(i_body.dcm.dot(self.u_irf[:, 0]))
self.u_jrf = orient_along_axis(self.j_rot_j, vector(jax_j))
self.h_i = vector(self.u_irf[:, 1]).a
self.h_j = vector(self.u_jrf[:, 0]).a
else:
# Angled axis
#print('angled')
ax = (vector(i_rot).cross(vector(j_rot))).unit
ax_i = i_body.dcm.T.dot(ax)
ax_j = j_body.dcm.T.dot(ax)
self.u_irf = orient_along_axis(self.i_rot_i, i_vector=ax_i)
self.u_jrf = orient_along_axis(self.j_rot_j, i_vector=ax_j)
self.h_i = vector(self.u_irf[:, 1]).a
self.h_j = vector(self.u_jrf[:, 0]).a
def __init__(self, p1, p2, p3, thickness, density=7.7):
l1 = (p1 - p2).mag # assuming this is the base, where p3 is the vertix
l2 = (p1 - p3).mag
l3 = (p2 - p3).mag
p = (l1 + l2 + l3) / 2 # half the premiter
# the normal height of the vertix from the base
self.height = l2 * np.sin(
np.deg2rad(vector.angle_between(p1 - p2, p1 - p3)))
# offset of vertex from base start point projected on base
a = (p1 - p3).dot(p1 - p2)
self.area = np.sqrt(p * (p - l1) * (p - l2) * (p - l3))
self.volume = self.area * thickness
self.mass = density * self.volume * 1e-3
normal_vector = vector.normal(p1.a, p2.a, p3.a)
self.cm = 1 / 3 * (p1 + p2 + p3) + normal_vector * thickness / 2
# creating a centroidal reference frame with z-axis normal to triangle
# plane and x-axis oriented with the selected base vector
self.C = orient_along_axis(normal_vector, p1 - p2)
# calculating the principle inertia properties "moment of areas"
Ixc = (l1 * self.height**3) / 36
Iyc = ((l1**3 * self.height) - (l1**2 * self.height * a) +
(l1 * self.height * a**2)) / 36
Izc = ((l1**3 * self.height) - (l1**2 * self.height * a) +
(l1 * self.height * a**2) + (l1 * self.height**3)) / 36
# calculating moment of inertia from the moment of area
self.J = density * thickness * 1e-3 * np.diag([Ixc, Iyc, Izc])
def __init__(self, location, i_body, j_body, axis=[0, 0, 1]):
# defining the basic attributes in the joint class
self.name = location.name
self.i_body = i_body
self.j_body = j_body
self.type = "None" # for representing the joint type in subclasses
self.typ = location.typ
self._loc = location
# defining the joint axis which is needed for the different types of joints
self.axis = axis
self.frame = orient_along_axis(axis)
self.u_irf = i_body.dcm.T.dot(self.frame)
self.u_jrf = j_body.dcm.T.dot(self.frame)
# joint locations wrt to each body origin cm referenced to the body reference
self.u_i = i_body.dcm.T.dot(self._loc - i_body.R)
self.u_j = j_body.dcm.T.dot(self._loc - j_body.R)
# axes to be used in the cylindrical and revolute joints classes
self.vii = vector(self.u_irf[:, 0])
self.vij = vector(self.u_irf[:, 1])
self.vik = vector(self.u_irf[:, 2])
self.vji = vector(self.u_jrf[:, 0])
self.vjj = vector(self.u_jrf[:, 1])
self.vjk = vector(self.u_jrf[:, 2])
def __init__(self, p1, p2, mass):
self.p1 = p1
self.p2 = p2
self.mass = mass
self.axis = p2 - p1
self.l = self.axis.mag
self.cm = p1 + 0.5 * self.axis
Jxx = Jyy = (self.mass / 12) * self.l**2
Jzz = 0
self.J = sc.sparse.diags([Jxx, Jyy, Jzz])
self.C = orient_along_axis(self.axis)
def __init__(self, p1, p2, do, di=0):
self.p1 = p1
self.p2 = p2
self.axis = p2 - p1
self.l = self.axis.mag
self.mass = 7.7 * np.pi * (do**2 - di**2) * self.l * 1e-3
self.cm = p1 + 0.5 * self.axis
Jxx = Jyy = (self.mass / 12) * (3 * do**2 + 3 * di**2 + self.l**2)
Jzz = (self.mass / 2) * (do**2 + di**2)
self.J = np.diag([Jxx, Jyy, Jzz])
self.C = orient_along_axis(self.axis)