def __init__(self, body_i, ai_bar, body_j, aj_bar, constraint_func=None):
super().__init__(body_i, body_j, constraint_func) #pass arguments to
#parent class
#constructor
#store both of the local coordinate vectors ensuring they are both
#numpy arrays
self.ai_bar = column(ai_bar)
self.aj_bar = column(aj_bar)
def __init__(self,
body_i,
sp_i_bar,
body_j,
sq_j_bar,
constraint_func=None):
super().__init__(body_i, body_j, constraint_func) #pass arguments to
#parent class
#constructor
#store the local vectors ensuring they are numpy column vectors
self.sp_i_bar = column(sp_i_bar)
self.sq_j_bar = column(sq_j_bar)
def set_orientation(self, p):
if len(p) != 4:
raise ValueError('p must be of length 4')
else:
self.p = column(p)
def set_ang_accel(self, p_ddot):
if len(p_ddot) != 4:
raise ValueError('p_ddot must be of length 4')
else:
self.p_ddot = column(p_ddot)
def C_vel(self, vel_history):
"""vel_history is an array where the rows correspond to a variable and the
columns correspond to a time step. Earlier (lower index) columns correspond
to older data"""
C = -vel_history @ self.alpha_arr
return column(C)
def set_ang_vel_from_omega(self, omega):
#comes from slide 16 of lecture 7
omega = column(omega)
p_dot = 0.5 * self.E.T @ omega
self.set_ang_vel(p_dot)
def set_ang_vel_from_omega_bar(self, omega_bar):
#comes from slide 16 of lecture 7
omega_bar = column(omega_bar)
p_dot = 0.5 * self.G.T @ omega_bar
self.set_ang_vel(p_dot)
def accel_rhs(self):
gamma = np.zeros((self.num_constraints + self.num_bodies, 1))
gamma[0:self.num_constraints] = self.gamma()
gamma[self.num_constraints::] = self.gamma_euler()
return column(gamma)
def __init__(self, vec, frame):
"""
vec: the vector represented in the reference frame "frame"
frame: the local frame that the input vector is represented in (can be
the global frame)
"""
self.frame = frame
self.vec = column(vec)
def __init__(self, f, loc):
"""
f: a function that takes time as an input and returns a vector object
representing the force vector
loc: an array like marking the location of where the force is being
applied on the rigid body
"""
self.f = f
self.loc = column(loc)
def vel_rhs(self):
rhs = np.zeros((self.num_constraints + self.num_bodies, 1))
rhs[0:self.num_constraints] = self.nu()
#nu for euler parameters equals zero and we have already preallocate
#the array full of zeros so there's no need to worry about the bottom
#of the array
return column(rhs)
def _set_lagrange_params(self, lagrange, lagrange_euler):
self.lagrange = lagrange
self.lagrange_euler = lagrange_euler
idx = 0
for constraint in self.constraints:
dof = constraint.DOF_constrained
constraint.lagrange = column(lagrange[idx:idx + dof])
idx += dof
def __init__(self,
m,
J,
r=None,
p=None,
r_dot=None,
p_dot=None,
r_ddot=None,
p_ddot=None,
idx=None,
name=None):
self.m = m
self.J = np.diag(J)
self.idx = idx
if name == None:
name = str(idx)
#set default values to zero if none are provided
if r is None:
r = [0, 0, 0]
if r_dot is None:
r_dot = [0, 0, 0]
if r_ddot is None:
r_ddot = [0, 0, 0]
#set attributes ensuring they are all column vectors
self.r = column(r)
self.r_dot = column(r_dot)
self.r_ddot = column(r_ddot)
super().__init__(p, p_dot, p_ddot)
self.forces = []
self.torques = []
def gamma(self):
gamma_arr = np.zeros((self.num_constraints, 1))
old_idx = 0
#add all algebraic constraint equations to phi vector
for constraint in self.constraints:
new_idx = old_idx + constraint.DOF_constrained
gamma_arr[old_idx:new_idx] = constraint.accel_rhs(self.t)
old_idx = new_idx
return column(gamma_arr)
def __init__(self, gcon_list, body_i, body_j, sp_i_bar = None, sq_j_bar = None):
self.gcon_list = gcon_list
self.body_i = body_i
self.body_j = body_j
self.i_idx = body_i.idx #index of body i in bodies list at system level
self.j_idx = body_j.idx #index of body j in bodies list at system level
#the following attributes are the location of the joint relative to each
#body's center of gravity. It is used for computing reaction forces
#about the joint. If no locations are given, the vectors default to zero
#vectors and the 'reaction()' method will simply return the reactons
#about the center of gravity.
if sp_i_bar == None:
#if a location for the joint
sp_i_bar = np.zeros((3,1))
sq_j_bar = np.zeros((3,1))
else:
self.sp_i_bar = column(sp_i_bar) #location of joint on body i
self.sq_j_bar = column(sq_j_bar) #location of joint on body j
self.lagrange = None
def __init__(self, body_i, ai_bar, bi_bar, body_j, aj_bar, bj_bar, k,
theta_0, c, h):
"""
ai_bar: axis about which the torque is applied to body i (unit vector)
bi_bar: perpendicular to ai_bar and used with bj_bar to define rotation
angle theta_ij
aj_bar: axis about which the torque is applied to body j (unit vector)
bj_bar: perpendicular to aj_bar and used with bi_bar to define rotation
angle theta_ij
k: spring constant of actuator
theta_0: zero_tension angle
c: damping coefficient
h: function that describes the effects of an actuator (hydraulic, electric, etc.)
must be of the form h(theta, theta_dot, t)
t: time
"""
self.body_i = body_i
self.body_j = body_j
self.ai_bar = normalize(column(ai_bar))
self.bi_bar = normalize(column(bi_bar))
self.aj_bar = normalize(column(aj_bar))
self.bj_bar = normalize(column(bj_bar))
self.k = k
self.theta_0 = theta_0
self.c = c
self.h = h
self.theta_old = 0
self.n = 0 #number of full revolutions
def set_order(self, order):
self.order = order
beta_arr = [1, 2 / 3, 6 / 11, 12 / 25, 60 / 137, 60 / 147]
self.beta_0 = beta_arr[order - 1]
#note that earlier indices corresponds to most recent points
alpha_lst = [[-1], [-4 / 3, 1 / 3], [-18 / 11, 9 / 11, -2 / 11],
[-48 / 25, 36 / 25, -16 / 25, 3 / 25],
[-300 / 137, 300 / 137, -200 / 137, 75 / 137, -12 / 137],
[
-360 / 147, 450 / 147, -400 / 147, 225 / 147,
-72 / 147, 10 / 147
]]
#set the alpha array and reverse the order so that earlier indices
#correspond to the oldest points
alpha_coeff = alpha_lst[order - 1]
alpha_coeff.reverse()
self.alpha_arr = column(alpha_coeff)
def __init__(self, vec, body):
self.vec = column(vec)
self.body = body
def C_pos(self, pos_history, vel_history):
C = -pos_history @ self.alpha_arr + self.beta_0 * self.h * self.C_vel(
vel_history)
return column(C)
def initialize(self):
"""Initial conditions for position and velocity must be given. This
Function calculates the accelerations and lagrange multipliers for time
t = 0, sets the accelerations for each of the bodies, sets the lagrange
multipliers of the system for t = 0 and then takes a snapshot of the
history of the system at t = 0"""
self.is_initialized = True
#initialization comes from slide 16 of L15
nb = self.num_bodies
nc = self.num_constraints
N = 8 * nb + nc
LHS = np.zeros((N, N))
RHS = np.zeros((N, 1))
#formulate LHS
col_offset1 = 3 * nb
col_offset2 = col_offset1 + 4 * nb
col_offset3 = col_offset2 + nb
row_offset1 = 3 * nb
row_offset2 = row_offset1 + 4 * nb
row_offset3 = row_offset2 + nb
#row 1
LHS[0:row_offset1, 0:col_offset1] = self.M()
LHS[0:row_offset1, col_offset3::] = self.partial_r().T
#row 2
LHS[row_offset1:row_offset2, col_offset1:col_offset2] = self.J()
LHS[row_offset1:row_offset2, col_offset2:col_offset3] = self.P().T
LHS[row_offset1:row_offset2, col_offset3::] = self.partial_p().T
#row 3
LHS[row_offset2:row_offset3, col_offset1:col_offset2] = self.P()
#row 4
LHS[row_offset3::, 0:col_offset1] = self.partial_r()
LHS[row_offset3::, col_offset1:col_offset2] = self.partial_p()
#formulate RHS
RHS[0:row_offset1] = self.generalized_forces()
RHS[row_offset1:row_offset2] = self.generalized_torques()
RHS[row_offset2:row_offset3] = self.gamma_euler()
RHS[row_offset3::] = self.gamma()
sol_0 = np.linalg.solve(LHS, RHS)
r_ddot_0 = column(sol_0[0:row_offset1])
p_ddot_0 = column(sol_0[row_offset1:row_offset2])
lagrange_euler = column(sol_0[row_offset2:row_offset3])
lagrange = column(sol_0[row_offset3::])
self._set_generalized_coords(r_ddot_0, p_ddot_0, level=2)
self._set_lagrange_params(lagrange, lagrange_euler)
self.history_set()
def step(self, tol=1e-3):
nb = self.num_bodies
nc = self.num_constraints
delta_mag = 2 * tol
#STEP 0: PRIME NEW STEP
#initialize the problem if it has not been already
if not self.is_initialized:
self.initialize()
#step time forward one step and increment the step counter
self.t += self.h
self.step_num += 1
#set order of solver based on how much history is present
history_cnt = len(self.history['r'])
order = min(self.order, history_cnt)
self.solver.set_order(order)
#grab history
r_h = self._get_history_array(key='r', order=order)
p_h = self._get_history_array(key='p', order=order)
r_dot_h = self._get_history_array(key='r_dot', order=order)
p_dot_h = self._get_history_array(key='p_dot', order=order)
r_ddot, p_ddot = self._get_generalized_coords(level=2)
sol = np.zeros((8 * nb + nc, 1))
offset1 = 3 * nb
offset2 = offset1 + 4 * nb
offset3 = offset2 + nb
sol[0:offset1] = r_ddot
sol[offset1:offset2] = p_ddot
sol[offset2:offset3] = self.lagrange_euler
sol[offset3::] = self.lagrange
max_iter = 30
iter_count = 0
# psi = self.sensitivity()
while delta_mag > tol and iter_count < max_iter:
#STEP 1: COMPUTE POS AND VEL USING MOST RECENT ACCELERATIONS
#step level 0 quantities
r_n = self.solver.pos_step(accel=r_ddot,
pos_history=r_h,
vel_history=r_dot_h)
p_n = self.solver.pos_step(accel=p_ddot,
pos_history=p_h,
vel_history=p_dot_h)
self._set_generalized_coords(r_n, p_n, level=0)
#step level 1 quantities
r_dot_n = self.solver.vel_step(accel=r_ddot, vel_history=r_dot_h)
p_dot_n = self.solver.vel_step(accel=p_ddot, vel_history=p_dot_h)
self._set_generalized_coords(r_dot_n, p_dot_n, level=1)
#STEP 2: COMPUTE RESIDUAL
gn = self.residual()
#STEP 3: SOLVE LINEAR SYSTEM FOR CORRECTION
psi = self.sensitivity()
delta = np.linalg.solve(psi, -gn)
#STEP 4: IMPROVE APPROXIMATE SOLUTION
sol = sol + delta
#set system parameters from new solution
r_ddot = sol[0:offset1]
p_ddot = sol[offset1:offset2]
self.lagrange_euler = column(sol[offset2:offset3])
self.lagrange = column(sol[offset3::])
self._set_generalized_coords(r_ddot, p_ddot, level=2)
#STEP 5: CHECK STOPPING CRITERION. IF SMALL ENOUGH STEP IS DONE
delta_mag = np.linalg.norm(delta)
# if iter_count < 5:
# print(delta_mag)
iter_count += 1
#use last acceleration data to set position and velocity level coordinates
#step level 0 quantities
r_n = self.solver.pos_step(accel=r_ddot,
pos_history=r_h,
vel_history=r_dot_h)
p_n = self.solver.pos_step(accel=p_ddot,
pos_history=p_h,
vel_history=p_dot_h)
self._set_generalized_coords(r_n, p_n, level=0)
#step level 1 quantities
r_dot_n = self.solver.vel_step(accel=r_ddot, vel_history=r_dot_h)
p_dot_n = self.solver.vel_step(accel=p_ddot, vel_history=p_dot_h)
self._set_generalized_coords(r_dot_n, p_dot_n, level=1)
#snap-shot into system history
self.history_set()
if history_cnt >= self.order:
#delete the oldest item out of history if we have enough data
self.history_delete_oldest()
#set system lagrange parameters
lagrange_euler = sol[offset2:offset3]
lagrange = sol[offset3::]
self._set_lagrange_params(lagrange, lagrange_euler)
def set_accel(self, r_ddot):
self.r_ddot = column(r_ddot)
def set_vel(self, r_dot):
self.r_dot = column(r_dot)
def set_position(self, r):
self.r = column(r)
