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)