def acceleration(self):
omega_tilde = tilde(self.body.omega)
omega_dot_tilde = tilde(self.body.omega_dot)
t1 = self.body.r_ddot
t2 = omega_tilde @ omega_tilde @ self.body.A @ self.vec
t3 = omega_dot_tilde @ self.body.A @ self.vec
accel = t1 + t2 + t3
return accel
def reaction(self, body = 'i'):
"""Calculate reaction forces and torques at the joint location
The reaction forces and torques are computed about the center of gravity
automatically but it would be more useful to translate the torques to
to actually be about the joint so that we could use it to determine the
torque required by a motor. To do this we do the following:
Tg = rxF + T_joint --> T_joint = Tg - r x F
Where Tg is the net torque that the contraint creates about the center
of gravity and is created by a combination of a moment about the joint
plus the effects of the reaction for F being applied at the joint. To
determine the effects of just the moment at the joint, we must subtract
off the contribution of the force at some lever arm from the net torque
"""
#calculate reaction forces about center of gravity
Tg = self.reaction_torque(body) #net torque about center of gravity
Fg = self.reaction_force(body) #net force located at center of gravity
if body.lower() == 'j':
r = self.body_j.A @ self.sq_j_bar
else:
r = self.body_i.A @ self.sp_i_bar
T_joint = Tg - tilde(r) @ Fg
return T_joint, Fg
def A(self):
e0 = self.p[0]
e = self.p[1::]
e_tilde = tilde(e)
A_mat = (2 * e0**2 - 1) * np.eye(3) + 2 * (e @ e.T + e0 * e_tilde)
return A_mat
def B(self, p, a_bar):
"""Returns 3x4 matrix that is created by the B operator.
p = the time varying vector
a_bar = the constant local vector"""
e0 = p[0]
e = p[1::]
e_tilde = tilde(e)
a_bar_tilde = tilde(a_bar)
t0 = e0 * np.eye(3) + e_tilde
v1 = t0 @ a_bar
m1 = e @ a_bar.T - t0 @ a_bar_tilde
M = 2 * np.hstack((v1, m1))
return M
def G_dot(self):
#comes from slide 12 of lecture 7
e0_dot = self.p_dot[0]
e_dot = self.p_dot[1::]
e_dot_tilde = tilde(e_dot)
G_dot_mat = np.zeros((3, 4))
G_dot_mat[:, 0] = -e_dot.flatten()
G_dot_mat[:, 1::] = -e_dot_tilde + e0_dot * np.eye(3)
return G_dot_mat
def G(self):
#comes from slide 12 of lecture 7
e0 = self.p[0]
e = self.p[1::]
e_tilde = tilde(e)
G_mat = np.zeros((3, 4))
G_mat[:, 0] = -e.flatten()
G_mat[:, 1::] = -e_tilde + e0 * np.eye(3)
return G_mat
def resulting_torque(self, t, frame):
#frame is rigid_bodies frame that the force is being applied to
F = self.f(t) #vector object
force = F.to_local(frame) #bring it into the bodies local frame
T = tilde(self.loc) @ force #torque in local frame as np array
#make torque into actual vector object
T = rigidbody.Vector(T, frame)
return T
def velocity(self):
vel = self.body.r_dot + tilde(self.body.omega) @ self.body.A @ self.vec
return vel