def __init__(self, MBD_system, parent=None): """ Constructor """ super(KinematicAnalysis, self).__init__(MBD_system, parent) # parent self._parent = parent # DAE fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self)
def __init__(self, MBD_system, parent=None): """ Constructor """ super(SolveDynamicAnalysis, self).__init__(parent) # parent self._parent = parent # states of simulations self.running = False self.stopped = False self.finished = False self.failed = False self._no_error = False # signals self.step_signal = stepSignal() self.finished_signal = FinishedSignal() self.repaintGL_signal = RepaintGLSignal() self.energy_signal = EnergySignal() self.error_time_integration_signal = ErrorTimeIntegrationSignal() self.filename_signal = solutionFilenameSignal() self.save_screenshot_signal = SaveScreenshot() self.solution_signal = SolutionSignal() # MBD system object self.MBD_system = MBD_system # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # DAE fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) # simulation settings - properties # self.integrationMethod = "RKF" # self.MBD_system.integrationMethod # update opengl widget every N-th step self.step = 0 self.update_opengl_widget_step_count = 0 self.update_opengl_widget_every_Nth_step = self.MBD_system.updateEveryIthStep self.save_screenshot_step_count = 0 self.save_screenshot_every_Nth_step = 2 * self.update_opengl_widget_every_Nth_step # info properties self.start_time_simulation_info_in_sec_UTC = [] self.start_time_simulation_info = [] self.end_time_simulation_info_in_sec_UTC = [] self.end_time_simulation_info = [] self.simulation_time_info_in_sec_UTC = [] self.simulation_time_info = [] self.stop_time_simulation_info_in_sec_UTC = [] self.stop_time_simulation_info = [] self.FLAG = 1 self.FLAG_contact = 0
def solve(self): """ Solves system of ode with dormand-prince order 5 method with runge-kutta algorithm """ # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # ode fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) self.start_solve() self.start_time_simulation_info_in_sec_UTC = time.time() self.start_time_simulation_info = datetime.datetime.fromtimestamp(self.start_time_simulation_info_in_sec_UTC).strftime("%H:%M:%S %d.%m.%Y") print "Simulation started: ", self.start_time_simulation_info # create mass and inverse mass matrix (only once) self.DAE_fun.evaluate_M() # create array of initial conditions for differential equations if not self.MBD_system.q0_created: self.MBD_system.evaluate_q0() # get initial conditions q0 = self._MBD_system.evaluate_q0() self.FLAG = 1 # solution containers self._solution_containers() self.simulation_id = 0 # 0 - no contact # 1 - contact detected # -1 - contact already happened - reduce integration step self.step = 0 self.h_contact = self._MBD_system.Hmin if self.MBD_system.integrationMethod.title() == "ABAM": pass elif self.MBD_system.integrationMethod.title() == "Runge-Kutta": self.solve_ODE_RK(t_0=self._MBD_system.t_0, t_n=self._MBD_system.t_n, q0=q0, absTol=self._MBD_system.absTol, relTol=self._MBD_system.relTol, Hmax=self._MBD_system.Hmax, Hmin=self._MBD_system.Hmin) elif self.MBD_system.integrationMethod.title() == "Euler": self.solve_ODE_Euler(t_0=self._MBD_system.t_0, t_n=self._MBD_system.t_n, q0=q0, Hmax=self._MBD_system.Hmax) else: raise ValueError, "Selected integration method not supported! Method: %s"%self.MBD_system.integrationMethod
def solve(self): """ Euler method for time integration :param t_0: :param t_n: :param q0: :param h: """ # solution containers self._solution_containers() # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # ode fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) self.start_solve() self.start_time_simulation_info_in_sec_UTC = time.time() self.start_time_simulation_info = datetime.datetime.fromtimestamp(self.start_time_simulation_info_in_sec_UTC).strftime("%H:%M:%S %d.%m.%Y") print "Simulation started: ", self.start_time_simulation_info # create array of initial conditions for differential equations # get initial conditions q0 = self._MBD_system.evaluate_q0() self.FLAG = 1 self.simulation_id = 0 # 0 - no contact # +1 - contact detected # -1 - contact already happened - reduce integration step self.step = 0 self.h_contact = self._MBD_system.Hmin self._append_to_file_step = 0 # constant time step vector self.t_constant_time_step = np.arange(0, self._MBD_system.t_n + self._MBD_system.Hmax, self._MBD_system.Hmax) self.t_level = 0 self.t_0 = self._dt = self._MBD_system.t_0 self.t_n = self._MBD_system.t_n t = self.t_0 w = q0 self.Hmax = h = self._MBD_system.Hmax self.R = 0 while self.FLAG == 1: if self.stopped: self.refresh(t=t, q=w) self.stop_solve() self.FLAG = 0 if t >= self.t_n: self.FLAG = 0 self.finished = True self.refresh(t=t, q=w) self.t = t = t + h self.h = h # print "-------------------------------------" # print self.t, t, h # error control self.absError = self.evaluate_error(self.t, self.h) if self.absError <= self.absTol: w = w + h * self.DAE_fun.evaluate_dq(t, w) # solve contacts self.FLAG_contact = self._evaluate_contacts(t, w) # evaluate mechanical energy of system self._energy_t = self._mechanical_energy(w) self._energy_delta = self._energy_t - self._solution_data._energy_solution_container[-1] else: self.t = t = t - h self.h = h / 2. # track data h, t, w = self._track_data(h, t, w) # simulation informations self._info(t, w) # check time step h, self._error = self.check_time_step(t, h, w) if self.finished or self.failed or self.stopped: self.finished_solve() # save data to file # if self.MBD_system._solution_save_options.lower() != "discard": self.write_simulation_solution_to_file()
class DynamicAnalysis(object): """ classdocs """ __simulation_id = itertools.count(0) def __init__(self, MBD_system, parent=None): """ Constructor """ # super(DynamicAnalysis, self).__init__(parent) # parent self._parent = parent # states of simulation self.running = False self.stopped = False self.finished = False self.failed = False self._error = False # signals self.step_signal = StepSignal() self.finished_signal = FinishedSignal() self.refresh_signal = RefreshSignal() self.energy_signal = EnergySignal() self.error_time_integration_signal = ErrorTimeIntegrationSignal() self.filename_signal = SolutionFilenameSignal() self.save_screenshot_signal = SaveScreenshot() self.solution_signal = SolutionSignal() self.signal_simulation_status = SignalSimulationStatus() # display active self._active_scene = True self.update_visualization_widget = True # MBD system object self.MBD_system = MBD_system # copy MBD object # self._MBD_system = copy.copy(self.MBD_system) # DAE fun object self.DAE_fun = None # define simulation attributes self.simulation_error = SimulationError() self.simulation_id = None self.t_0 = 0. self.t_n = None self.t = None self.time = 0. self.h = None self.Hmax = None self.Hmin = None self.Hcontact = 0. self.Houtput = 0. self.stepsNumber = None self.absTol = None self.relTol = None self.errorControl = True self.error = None self.absError = None self.relError = None # list of position coordinates of max elements of every degree of freedom self.q0 = [] self.q_max = [] self.iterNum = 0 self.step = 0 self.progress = 0 self.FLAG = 1 self.FLAG_contact = 0 self.FLAG_contact_finish = 0 self._t_FLAG1 = None self.h_level = 0 self.dh = 0. self._steps_after_contact = 0 self._dh_constant_steps = 0 # energy data variables self._energy_delta = 0. self._mechanical_energy = 0. self._kinetic_energy = 0. self._potential_energy = 0. self._elastic_strain_energy = 0. # size of vectors # size of vector q that is assembled from position and velocity vector self.q_N = None # size of vector q of position coordinates only self.q_n = None # solution container object self._solution_data = None self.write_to_file = True # visualization properties self.refresh_visualization_widget_step_count = 0 # simulation settings - properties # self.integrationMethod = "RKF" # self.MBD_system.integrationMethod # update visualization widget every N-th step if self.MBD_system is not None: self._dt = self.MBD_system._dt self.updateEveryIthStep = self.MBD_system.updateEveryIthStep else: self._dt = 1E-3 self.updateEveryIthStep = 1 self.t_constant_time_step = None self.save_screenshot_step_count = 0 self.save_screenshot_every_Nth_step = 2 * self.updateEveryIthStep # info properties self.start_time_simulation_info_in_sec_UTC = [] self.start_time_simulation_info = [] self.end_time_simulation_info_in_sec_UTC = [] self.end_time_simulation_info = [] self.simulation_time_info_in_sec_UTC = [] self.simulation_time_info = [] self.stop_time_simulation_info_in_sec_UTC = [] self.stop_time_simulation_info = [] self.start_time_simulation_info_UTC = None # solution data settings # write to file during simulation self._append_to_file_every_Nth_step = 20 self._append_to_file_step = 0 self._append_to_file_during_simulation = True self._append_to_file_keep_last_Nth_steps_in_memory = 10 def create_DAE(self): """ :return: """ self.DAE_fun = DAEfun(self.MBD_system, parent=self) def _solution_containers(self): """ Function initializes solution containers object attributes to save solution data during numerical integration process :return: """ # solution container variable if hasattr(self.MBD_system, "evaluate_q0"): self.q0 = self.MBD_system.evaluate_q0() self.q_max = self.get_q(self.q0) # size of q and qdq self.q_N = len(self.q0) self.q_n = self.q_N / 2 # self.q_sol_matrix = np.array([self.q0]) # self.t_vector = np.array([0]) # self.R_vector = np.array([0]) # self.step_counter_vector = np.array([0]).astype(int) # self.step_size_vector = np.array([self._MBD_system.Hmax]) # self.h_level_vector = np.array([0]).astype(int) # self.energy_data_vector = np.array([self._mechanical_energy(self.q0)]) self.contact_status_vector = np.array([0]).astype(int) # create solution data object to store data at each simulation start self._solution_data = SolutionData(MBD_item=self.MBD_system) # clear temp solution file self._solution_data._clear_file() # append solution so list of solutions of MBD system if hasattr(self.MBD_system, "solutions"): self.MBD_system.solutions.append(self._solution_data) def evaluate_localError(self, q, q_new): """ According to: On an implementation of the Hilber-Hughes-Taylor method in the context of index 3 differential-algebraic equations of multibody dynamics :return: """ self.evaluate_q_max() x = q_new / self.q_max e = abs(self.beta - (1 / (6 * (1 + self.alpha)))) * ( self.h**2 / np.sqrt(self.q_n)) * np.linalg.norm(x, ord=2) return e def evaluate_q_max(self, w): """ Function to store max element of every coordinate during simulation to evaluate weighted norm :param q: :return: """ # get only position vector q = self.get_q(w) if isinstance(q, np.ndarray): for i in range(0, len(q)): if abs(q[i]) > abs(self.q_max[i]): self.q_max[i] = np.max(np.array([1., abs(q[i])])) else: self.q_max[i] = np.max(np.array([1., abs(self.q_max[i])])) else: self.q_max = np.max(np.array([1., abs(q)])) def get_q(self, w): """ :param w: :return: """ if self.q_n is None: self.q_N = len(self.q0) self.q_n = self.q_N / 2 if isinstance(w, (np.ndarray, list)): q = w[0:self.q_n] else: q = w return q def evaluate_energy_delta(self, energy_t): """ :return: """ return energy_t - self._solution_data._mechanical_energy_solution_container[ -1] def start_solve(self): """ Start solver (time integration process) """ # set step size if isinstance(self._MBD_system, MBDsystem): self.Hmin = self._MBD_system.Hmin self.Hmax = self._MBD_system.Hmax self.Hcontact = self._MBD_system.Hcontact self.Houtput = self._MBD_system.Houtput if self.t_n is None and self._MBD_system.stepsNumber is None: self.t_n = self.Hmax # set step level self.h_min_level = 0 self.h_max_level = np.trunc(np.log2(self.Hmax / self.Hmin)) if self.Hcontact > 0.: self.h_contact_level = np.trunc(np.log2(self.Hmax / self.Hcontact)) if self.Hcontact > self.Hmax: self.Hcontact = self.Hmax if self.Houtput > 0.: self.h_output_level = np.trunc(np.log2(self.Hmax / self.Houtput)) # solution id self.simulation_id = self.__simulation_id.next() # create mass and inverse mass matrix (only once) if hasattr(self.DAE_fun, "preprocessing"): self.DAE_fun.preprocessing() # set solution data to container of initial values if self._MBD_system is not None: self._MBD_system.evaluate_q0() self.q0 = self._MBD_system.evaluate_q0() self.absTol = self._MBD_system.absTol self.relTol = self._MBD_system.relTol self.errorControl = self._MBD_system.errorControl Em0, Ek0, Ep0, Ees0 = self._evaluate_mechanical_energy(self.q0) self._solution_data._set_initial_data(Em0, Ek0, Ep0, Ees0, self.q0) self.signal_simulation_status.signal_simulation_status.emit("Running") logging.getLogger("DyS_logger").info( "Simulation started, simulation id: %s" % self.simulation_id) self.running = True self.stopped = False self.finished = False self.start_time_simulation_info_UTC = self.MBD_system.start_time_simulation_info_in_sec_UTC = time.time( ) # self.start_time_simulation_info = datetime.datetime.fromtimestamp(self.start_time_simulation_info_in_sec_UTC).strftime("%H:%M:%S %d.%m.%Y") self.start_time_simulation_info = QtCore.QDateTime() print "Simulation started: ", self.start_time_simulation_info.toString( ) def stop_solve(self): """ Stop solver (time integration process) - interput by user """ # get solution data to one variable # solution_data = self.collect_solution_data() # append solution data to file self._solution_data._append_data_to_temp_file() self.stopped = True self.running = False self.stop_time_simulation_info_in_sec_UTC = self.MBD_system.end_time_simulation_info_in_sec_UTC = time.time( ) self.stop_time_simulation_info = datetime.datetime.fromtimestamp( self.stop_time_simulation_info_in_sec_UTC).strftime( "%H:%M:%S %d.%m.%Y") self.signal_simulation_status.signal_simulation_status.emit("Stopped") logging.getLogger("DyS_logger").info( "Simulation stopped by user at simulation time:%s", self.t) def finished_solve(self): """ Method sets some parameters when integration process is finished :return: """ if self.update_visualization_widget: self._refresh(t=self.t, step=self.step, h=self.h) if self.finished: self.solution_signal.solution_data.emit(id(self._solution_data), self._solution_data._name) logging.getLogger("DyS_logger").info( "Simulation finished successfully!") self.finished_signal.signal_finished.emit() self.signal_simulation_status.signal_simulation_status.emit( "Finished") self.end_time_simulation_info_in_sec_UTC = self.MBD_system.end_time_simulation_info_in_sec_UTC = time.time( ) # set booleans for states self.finished = True self.running = False def simulation_failed(self): """ :return: """ self.signal_simulation_status.signal_simulation_status.emit("Failed") logging.getLogger("DyS_logger").info("Simulation failed!") def check_error(self): """ :return: """ return False def _info(self, t, w): """ :return: """ # update coordinates and angles of all bodies self._update_coordinates_and_angles_of_all_bodies(t, w) # update visualization self.refresh(t, w) # self.step += 1 # step_counter = np.append(step_counter, self.step) # self.refresh_visualization_widget_step_count += 1 self.save_screenshot_step_count += 1 # step number signal self.step_signal.signal_step.emit(self.step) # energy data signal self.energy_signal.signal_energy.emit(self._mechanical_energy, self._energy_delta) # simulation progress if self.t_n is not None: self.progress = t / self.t_n if self.t_n == np.inf: self.progress = float(self.step) / float(self.stepsNumber) def _track_data(self, h, t, q): """ :return: """ # save values at next time step to matrix of solution (q_matrix) if not self.check_error() and self.FLAG_contact in [0, 1]: # evaluate q_max self.evaluate_q_max(q) self.step += 1 self._solution_data._track_data(self.step, self._mechanical_energy, self._kinetic_energy, self._potential_energy, self._elastic_strain_energy, self.absError, h, t, self.h_level, self.FLAG_contact, self.iterNum, q) if self.MBD_system is not None: for contact in self.MBD_system.contacts: contact._track_data(self.step, h, t) for force in self.MBD_system.forces: force._track_data(self.step, h, t) for spring in self.MBD_system.springs: spring._track_data(self.step, h, t) for joint in self.MBD_system.joints: joint._track_data(self.step, h, t) for measure in self.MBD_system.measures: measure._track_data(self.step, h, t, q) # check is step number is reached to append data to file if self._append_to_file_step == self._append_to_file_every_Nth_step - 1 and self._append_to_file_during_simulation: # collect solution data to one variable # solution_data = self.collect_solution_data() # append solution data to file self._solution_data._append_data_to_temp_file() # print "solution data appended!" # remove all but last value in array/matrix # self._reset_solution_data_after_append_to_temp_file() # counter self._append_to_file_step = 0 else: self._append_to_file_step += 1 # reduce step size and continue integration from previous step elif self.FLAG_contact == -1 or self.check_error(): # go to one before last solution q = self._solution_data._q_solution_container[-1] # q = self._solution_data. # remove last stored value in matrix # self.q_sol_matrix = np.delete(self.q_sol_matrix, -1, axis=0) # store time that is too large self._t_FLAG1 = t if t > 0: t -= h # t = self._solution_data._t_solution_container[-1] - self._solution_data._h_level_solution_container[-1] # # level os step size # self.t_level += 1 # self.step -= 1 # step_num = self.step # step_counter[-1] = self.step self._solution_data._mechanical_energy_solution_container[ -1] = self._mechanical_energy else: raise ValueError, "Wrong parameters!" return h, t, q def _check_contacts_status(self): """ :return: """ FLAG_contact = 0 self.contact_status_list = np.zeros(len(self.MBD_system.contacts), dtype="int") for i, contact in enumerate(self.MBD_system.contacts): self.contact_status_list[i] = contact.status if (self.contact_status_list == 0).all(): FLAG_contact = 0 if (self.contact_status_list == -1).any(): FLAG_contact = -1 if (self.contact_status_list == 1).any(): FLAG_contact = 1 return FLAG_contact def _evaluate_contacts(self, t, q): """ Function solves contacts. Finds overlap pairs of (sub)AABB and calculates actual distances between node and line (edge). If distance is below userdefined limit contact is present and contact equations are solved. :type self: object Args: q vector of positions and velocities (translational and rotational) Returns: q_ vector of new calculated velocities (positions are equal before and after contact status - 0 - contact is not detected continue with integration to next time step 1 - contact is detected (has happened) return to previous time step solution and split time step size 2 - contact is detected (will happen) split time step size Raises: None """ # print "step=", self.step, "t =", t, 'h_level=', self.h_level # print 'step=', self.step # predefine array of zeros if hasattr(self.MBD_system, "contacts"): self.contact_status_list = np.zeros(len(self.MBD_system.contacts), dtype="int") # check for contact of every contact pair for i, contact in enumerate(self.MBD_system.contacts): if contact.active: # print "contact._contact_point_found =", contact._contact_point_found, "t =", t if not contact._contact_point_found: # function recursively loops through all potential AABB objects in AABB tree of each body # and if two AABB objects overlap, a new overlap object is created # that has both overlapping AABB objects # general contact if contact._contact_type in [ "general", "roughness profile" ]: # adds simulation data to contact object # reset to empty list a contact object attribute of list of overlap pairs, # as this is not possible in the next line (function) due to the recursion contact.AABB_list_of_overlap_pairs = [] # check for overlap and if overlap is present build a overlap pair object contact.check_for_overlap(q, contact.AABB_i, contact.AABB_j) # if list of overlap object pairs is not empty, check for contact if contact.AABB_list_of_overlap_pairs != []: self.contact_status_list[ i] = contact.check_for_contact( self.step, t, q) else: contact.no_overlap(self.step, t, q) # clearance joints of contact elif contact._contact_type.lower( ) in contact._contact_types: self.contact_status_list[ i] = contact.check_for_contact( self.step, t, q) else: self.FLAG = 0 QtGui.QMessageBox.critical( self._parent, "Error!", "Contact type not correct: %s" % contact._type + ". \nDefine correct contact type!", QtGui.QMessageBox.Ok) raise ValueError, "Contact type not correct: %s" % contact._type, "\n Define correct contact type!" else: pass # adds simulation data to contact object as this is not possible in the next line because of the recursion self.contact_status_list[i] = contact.contact_update( self.step, t, q) # no contacts if (self.contact_status_list == 0).all() and self.MBD_system.contacts: FLAG_contact = 0 callerframerecord = inspect.stack()[ 0] # 0 represents this line # 1 represents line at caller frame = callerframerecord[0] # print "NO CONTACT" # print "simulation sleep" info = inspect.getframeinfo(frame) # print "file:", info.filename # print "function:", info.function+"()" # time.sleep(100) # if self._solution_data._contact_status_solution_container[-1] == 1 and self.FLAG_contact_finish == 1: # FLAG_contact = -1 # self.FLAG_contact_finish -= 1 return FLAG_contact # contact has already happened - reduce time step if (self.contact_status_list == -1).any(): FLAG_contact = -1 # 0-no contact, 1-contact has already happened - reduce time step return FLAG_contact # contact if (self.contact_status_list == 1).any(): FLAG_contact = 1 # self.FLAG_contact_finish = 1 # logging.getLogger("DyS_logger").info("Contact detected at simulation time:%s \nbody_i=%s \nbody_j=%s", t, self.MBD_system.bodies[contact.body_id_i]._name, self.MBD_system.bodies[contact.body_id_j]._name) # logging.getLogger("DyS_logger").info("Contact detected at simulation time:%s "%t) # repaint opengl widget # self.repaintGL_signal.signal_repaintGL.emit() # solve contacts - evaluate contact forces # self.DAE_fun.solve_contacts(t, q) # logging.getLogger("DyS_logger").info("Contact calculation finished") return FLAG_contact else: FLAG_contact = 0 else: FLAG_contact = 0 return FLAG_contact def _evaluate_mechanical_energy(self, q): """ """ if hasattr(self.MBD_system, "evaluate_mechanical_energy"): _mechanical_energy, _kinetic_energy, _potential_energy, _elastic_strain_energy = self.MBD_system.evaluate_mechanical_energy( q=q) else: _mechanical_energy = _kinetic_energy = _potential_energy = _elastic_strain_energy = 0. return _mechanical_energy, _kinetic_energy, _potential_energy, _elastic_strain_energy def check_time_step(self, t, h, w): """ Function checks time step size """ error = False self.dh = 0 # if at least one contact is present in the MBD system if self.FLAG_contact == 1: self.dh = 0 # print 'h_level=', self.h_level # condition to increase speed for calculationg contact conditions if self._dh_constant_steps == 0: if self.h_level - 1 >= self.h_contact_level: for h_lev in np.arange(self.h_level): if np.around((self.t / self.Hmax * 2**(h_lev)), decimals=5) % 1 == 0: # dh = -self.h_level + h_lev dh = -1 self.h_level += dh if self.h_level < self.h_contact_level: self.h_level = self.h_contact_level break else: self.h_level = self.h_contact_level else: self._dh_constant_steps -= 1 # at least one contact has current initial penetration depth larger than initial minimum specified depth by user elif self.FLAG_contact == -1 or self.absError > self.absTol: if self.h_level + 1 <= self.h_max_level: self.dh = 1 self._dh_constant_steps = 8 else: error = True print "-----------------------------------------" for contact in self.MBD_system.contacts: print "contact id =", contact.contact_id print "delta =", contact._delta print "step =", self.step # print "t =", self.t # print "h_level =", self.h_level # print "Hmin =", self.Hmin print Warning, "Hmin exceeded!" # contact is finished and the integration is started from previous time step with smalled step size elif self._solution_data._contact_status_solution_container[-1] == 1: self.h_level = int( max(self._solution_data._h_level_solution_container)) # print 'ELIF h_level', self.h_level # no contact is present at current time step, integration step size can be increased else: if self._dh_constant_steps == 0: if self.h_level - 1 >= self.h_min_level: # print "np.arange(self.h_level) =", np.arange(self.h_level) for h_lev in np.arange(self.h_level): if np.around((self.t / self.Hmax * 2**(h_lev)), decimals=5) % 1 == 0: # dh = self.h_level - h_lev dh = 1 if self.step < self._steps_after_contact: _n = self.step else: _n = self._steps_after_contact _status = self._solution_data._contact_status_solution_container[ -_n - 1:-1] if (_status == np.zeros(_n)).all(): self.dh = -dh else: self.dh = 0 break else: self._dh_constant_steps -= 1 self.h_level += self.dh h = self._evaluate_time_step(self.h_level) # time step size is limited by max value if h > self.Hmax: h = self.Hmax self.h_level = 0 if h < self.Hmin and self.FLAG_contact != 1: h = self.Hmin error = True print "-----------------------------------------" for contact in self.MBD_system.contacts: print "contact id =", contact.contact_id print "delta =", contact._delta # print "t =", self.t # print "h_level =", self.h_level # print "Hmin =", self.Hmin print Warning, "Hmin exceeded!" # multiple of time step # m = t * (2.**self.t_level) / self.Hmax # next_level = fractions.gcd((2**self.t_level), m) # print "next_level =", next_level # time.sleep(100) return h, error def _evaluate_time_step(self, level): """ """ # print "self.Hmax =", self.Hmax # print "level =", level # print "2.**level =", 2.**level h = self.Hmax / (2**level) # print "h =", h return h def write_simulation_solution_to_file(self): """ Function writes solution data to solution data object """ print "write_simulation_solution_to_file()" # write solution data and info to solution data object if hasattr(self.MBD_system, "_solution_filetype"): self._solution_data.set_filetype( self.MBD_system._solution_filetype) else: self._solution_data.set_filetype(".sol") self.solution_filename = 'solution_data_%02d' % self.simulation_id #+self.MBD_system._solution_filetype # add to temp file # data = self.collect_solution_data() self._solution_data._append_data_to_temp_file() # read from temp file if os.path.isfile(self._solution_data._temp_filename): self._solution_data._solution_data_from_temp_file() if self.MBD_system._save_options == "save to new": # check filename if already exists self.solution_filename = check_filename(self.solution_filename) # set solution data filename self._solution_data.setName(self.solution_filename) # add data to object attribute self._solution_data.finished = True # print "self.MBD_system._solution_save_options =", self.MBD_system._solution_save_options if self.MBD_system._solution_save_options != "discard": # write data to file self._solution_data.write_to_file() # emit signal # self.solution_signal.solution_data.emit(id(self._solution_data), self._solution_data._filename) # write contact values of every simulation time step to file for contact in self.MBD_system.contacts: if contact._solution_save_options != "discard" and contact.active: contact.write_to_file() for joint in self.MBD_system.joints: if joint._solution_save_options != "discard" and joint.active: joint.write_to_file() def collect_solution_data(self): """ :return: """ # join in one ndarray (matrix) solution_data = np.hstack( (np.array([self.step_counter_vector]).T, np.array([self.energy_data_vector]).T, np.array([self.R_vector]).T, np.array([self.step_size_vector]).T, np.array([self.t_vector]).T, np.array([self.h_level_vector]).T, np.array([self.contact_status_vector]).T, self.q_sol_matrix)) return solution_data def _reset_solution_data_after_append_to_temp_file(self): """ Function removes all elements in array except last item (row) :return: """ for data_name in [ "step_counter_vector", "energy_data_vector", "R_vector", "step_size_vector", "t_vector", "h_level_vector", "contact_status_vector" ]: data = getattr(self, data_name) new_data = np.array( data[self._append_to_file_keep_last_Nth_steps_in_memory:]) setattr(self, data_name, new_data) # solution matrix new_data = self.q_sol_matrix[ self._append_to_file_keep_last_Nth_steps_in_memory:, :] self.q_sol_matrix = new_data def save_solution_data(self): """ :return: """ if os.path.isfile(self._solution_data._temp_filename): self._solution_data._solution_data_from_temp_file() # self._solution_data.add_data(data) def load_simulation_solution_from_file(self, filename): """ Function load solution_data from file. """ # self.finished_signal.signal_finished.emit("Animation") self.signal_simulation_status.signal_simulation_status.emit( "Animation") if os.path.exists(filename): self.solution_data = np.loadtxt(str(filename), skiprows=2) return self.solution_data def _update_coordinates_and_angles_of_all_bodies(self, t, q): """ Function updates MBD_system data before display """ # self.MBD_system.update_coordinates_and_angles_of_all_bodies(q) self.MBD_system.update_vtk_data(t, q) def refresh(self, t, q): """ Update visualization widget at preset time steps """ if self._active_scene and self.update_visualization_widget: # update display on every i-th step if self._parent._update_display_type == "step": self.refresh_visualization_widget_step_count += 1 if self.refresh_visualization_widget_step_count == self.updateEveryIthStep or self.finished or self.stopped: self._refresh(t, self.step, self.h) self.time = t self.save_updated_screenshot_to_file(self.time) # reset step counter to update opengl widget self.refresh_visualization_widget_step_count = 0 # update display on simulation time interval dt if self._parent._update_display_type == "dt": _dt = t - self._dt if _dt >= self._parent._dt: self._refresh(t, self.step, self.h) # set new value self._dt = t return None def _refresh(self, t, step, h): """ :return: """ # update visualization widget self.MBD_system.update_simulation_properties(time=t, step_num=step, h=h) # emit signal if self._active_scene: self.refresh_signal.signal_refresh.emit() def save_updated_screenshot_to_file(self, t): """ """ if hasattr(self.MBD_system, "save_screenshots"): if self.MBD_system.save_screenshots: if (self.save_screenshot_step_count == self.save_screenshot_every_Nth_step) or ( t == self.t_0) or (self.finished): self.screenshot_filename_abs_path = os.path.join( self.MBD_system.saved_screenshots_folder_abs_path, str("step=" + "%010d" % self.step)) self.save_screenshot_signal.signal_saveScreenshot.emit() self.save_screenshot_step_count = 0 def restore_initial_condition(self): """ Function restores initial conditions and displays it with opengl widget. """ self.step = 0 self.MBD_system._restore_initial_conditions() self.step_signal.signal_step.emit(self.step) self.energy_signal.signal_energy.emit(0., 0.) self.MBD_system.time = 0. self.MBD_system.step_num = 0 self.MBD_system.h = self.MBD_system.Hmax self.refresh_signal.signal_refresh.emit() self.refresh(t=0, q=self.MBD_system.q0)
def create_DAE(self): """ :return: """ self.DAE_fun = DAEfun(self.MBD_system, parent=self)
def __init__(self, MBD_system, parent=None): """ Constructor """ # super(DynamicAnalysis, self).__init__(parent) # parent self._parent = parent # states of simulation self.running = False self.stopped = False self.finished = False self.failed = False self._error = False self.errorControl = True # signals self.step_signal = StepSignal() self.finished_signal = FinishedSignal() self.refresh_signal = RefreshSignal() self.energy_signal = EnergySignal() self.error_time_integration_signal = ErrorTimeIntegrationSignal() self.filename_signal = SolutionFilenameSignal() self.save_screenshot_signal = SaveScreenshot() self.solution_signal = SolutionSignal() self.signal_simulation_status = SignalSimulationStatus() # display active self._active_scene = True # MBD system object self.MBD_system = MBD_system # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # DAE fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) # define simulation attributes self.simulation_id = None self.h_contact = None self.t_0 = 0. self.t_n = None self.stepsNumber = None self.absTol = None self.relTol = None self.absError = None self.relError = None self.progress = 0 self._energy_t = 0. self._energy_delta = 0. self.h_min_level = None self.h_max_level = None self.h_contact_level = None # simulation settings - properties # self.integrationMethod = "RKF" # self.MBD_system.integrationMethod # update opengl widget every N-th step self._dt = self.MBD_system._dt self.step = 0 self.t = 0. self.update_opengl_widget_step_count = 0 self.update_opengl_widget_every_Nth_step = self.MBD_system.updateEveryIthStep self.t_constant_time_step = None self.save_screenshot_step_count = 0 self.save_screenshot_every_Nth_step = 2 * self.update_opengl_widget_every_Nth_step # info properties self.start_time_simulation_info_in_sec_UTC = [] self.start_time_simulation_info = [] self.end_time_simulation_info_in_sec_UTC = [] self.end_time_simulation_info = [] self.simulation_time_info_in_sec_UTC = [] self.simulation_time_info = [] self.stop_time_simulation_info_in_sec_UTC = [] self.stop_time_simulation_info = [] self.FLAG = 1 self.FLAG_contact = 0 self.FLAG_contact_finish = 0 # level of integration step size - time self.h_level = 0 self.dh = 0 self._steps_after_contact = 0 self._dh_constant_steps = 0 # solution data settings # write to file during simulation self._append_to_file_every_Nth_step = 20 self._append_to_file_step = 0 self._append_to_file_during_simulation = True self._append_to_file_keep_last_Nth_steps_in_memory = 10
def solve(self, q0=[], t_n=None, absTol=1E-9, maxIter=10): """ :return: """ # redefine solution containers self._solution_containers() # copy MBD object self.MBD_system.setSolver(self) # self._MBD_system = copy.copy(self.MBD_system) self._MBD_system = self.MBD_system # set solver self._MBD_system.setSolver(self) # ode fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) self.start_solve() self.beta = self.evaluate_beta(self.alpha) self.gamma = self.evaluate_gamma(self.alpha) self.t_n = t_n if self.t_n is not None and self.stepsNumber is not None: self.Hmax = self.t_n / self.stepsNumber if q0: self.q0 = q0 if t_n is not None: self.t_n = t_n self.h = h = self.Hmax t = self.t_0 w = self.q0 q = self.get_q(w) while self.FLAG == 1 and not self._error and not self.DAE_fun.error: self.h = h K1 = w + 0.5 * h * self.DAE_fun.evaluate_dq(t, w) w0 = K1 i_iter = 0 self.FLAG_iter = 0 while self.FLAG_iter == 0: w = w0 - ( (w0 - (0.5 * h * self.DAE_fun.evaluate_dq(t + h, w0)) - K1) / (1. - 0.5 * h * self.DAE_fun.evaluate_Jacobian(t + h, w0))) self.absError = self.evaluate_error(w0, w) #optimize.newton(f, x0, fprime=None, args=(y,), tol=1.48e-08, maxiter=50) if self.absError < self.absTol: self.FLAG_iter = 1 else: i_iter += 1 w0 = w if i_iter > maxIter: raise ValueError, "The maximum number of iterations exceeded" break if (t >= self.t_n) or (self.step == self.stepsNumber): self.FLAG = 0 self.finished = True self.refresh(t=t, q=w) else: t = t + h # track - store simulation state data of different MBD system objects h, t, w = self._track_data(h, t, w)
class IntegrateHHT_I3(DynamicAnalysis): """ classdocs """ def __init__(self, MBD_system, parent=None): """ Constructor """ super(IntegrateHHT_I3, self).__init__(MBD_system, parent) # iterative process properties self.FLAG_iter = None self.J = None # parameters self.gamma = 0.5 self.beta = 0.25 self.alpha = 0. def evaluate_beta(self, alpha): """ :return: """ beta = ((1. - alpha)**2) / 4. return beta def evaluate_alpha(self, alpha): """ If alpha is 0, no numerical damping is used :param alpha: :return: """ if alpha < -0.3: alpha = -0.3 if alpha > 0.: alpha = 0. return alpha def evaluate_gamma(self, alpha): """ :param alpha: :return: """ gamma = 0.5 + alpha return gamma def evaluate_error(self, q, q_new): """ :return: """ self.evaluate_q_max(q_new) x = q_new / self.q_max # error e = abs(self.beta - (1. / (6. * (1. + self.alpha)))) * ( self.h**2 / np.sqrt(self.q_n)) * np.linalg.norm(x, ord=2) return e def solve(self, q0=[], t_n=None, absTol=1E-9, maxIter=10): """ :return: """ # redefine solution containers self._solution_containers() # copy MBD object self.MBD_system.setSolver(self) # self._MBD_system = copy.copy(self.MBD_system) self._MBD_system = self.MBD_system # set solver self._MBD_system.setSolver(self) # ode fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) self.start_solve() self.beta = self.evaluate_beta(self.alpha) self.gamma = self.evaluate_gamma(self.alpha) self.t_n = t_n if self.t_n is not None and self.stepsNumber is not None: self.Hmax = self.t_n / self.stepsNumber if q0: self.q0 = q0 if t_n is not None: self.t_n = t_n self.h = h = self.Hmax t = self.t_0 w = self.q0 q = self.get_q(w) while self.FLAG == 1 and not self._error and not self.DAE_fun.error: self.h = h K1 = w + 0.5 * h * self.DAE_fun.evaluate_dq(t, w) w0 = K1 i_iter = 0 self.FLAG_iter = 0 while self.FLAG_iter == 0: w = w0 - ( (w0 - (0.5 * h * self.DAE_fun.evaluate_dq(t + h, w0)) - K1) / (1. - 0.5 * h * self.DAE_fun.evaluate_Jacobian(t + h, w0))) self.absError = self.evaluate_error(w0, w) #optimize.newton(f, x0, fprime=None, args=(y,), tol=1.48e-08, maxiter=50) if self.absError < self.absTol: self.FLAG_iter = 1 else: i_iter += 1 w0 = w if i_iter > maxIter: raise ValueError, "The maximum number of iterations exceeded" break if (t >= self.t_n) or (self.step == self.stepsNumber): self.FLAG = 0 self.finished = True self.refresh(t=t, q=w) else: t = t + h # track - store simulation state data of different MBD system objects h, t, w = self._track_data(h, t, w) def evaluate_Jacobian(self, t, q): """ :return: """ if self.J is None: self.J = self.DAE_fun.evaluate_Jacobian(t, q) return self.J
class KinematicAnalysis(DynamicAnalysis): """ classdocs """ def __init__(self, MBD_system, parent=None): """ Constructor """ super(KinematicAnalysis, self).__init__(MBD_system, parent) # parent self._parent = parent # DAE fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) def solve(self): """ Solves a kinematic system """ print "solve()" # pprint(vars(self)) # start solve self.start_solve() self.simulation_id = 0 self.FLAG = 1 # scipy.optimize.newton() t = 0. h = self._MBD_system.Hmax self.t_n = self.MBD_system.t_n # initial approximation q = self.q_sol = self.MBD_system.evaluate_q0() # use positions vector (translational and rotational) q = q[0:len(q) / 2] # us # solution containers self._solution_containers() # size of C, C_q, C_t vectors, matrix self.DAE_fun._C_q_size() # options to track data self.q_sol_matrix = np.array([q]) self._energy_t = 0 self._energy_delta = np.nan # check before run if not self.DAE_fun.check_C(q, t): self.FLAG = 1 else: print "Error found before simulation run!" self.FLAG = 0 self.finished_solve() # kinematic analysis while self.FLAG == 1: # print "----------------------------------------" # print "t =", t if self.stopped: # self.update_GL_(t=t, q=w) self.stop_solve() self.FLAG = 0 if t >= self.t_n: self.FLAG = 0 self.finished = True print "finished!" # self.update_GL_(t=t, q=w) if self.finished or self.failed: self.finished_solve() # evaluate C vector # C = self.DAE_fun.evaluate_C(q, t) # print "C =", len(C) # print C # C_q = self.DAE_fun.evaluate_C_q(q) # print "C_q =" # print C_q # print "q (in) =", q q = scipy.optimize.fsolve(self.DAE_fun.evaluate_C, q, args=(t), fprime=self.DAE_fun.evaluate_C_q, xtol=self.MBD_system.TOL_dq_i, maxfev=50) # q = scipy.optimize.newton(self.DAE_fun.evaluate_C, q, fprime=self.DAE_fun.evaluate_C_q, tol=self.MBD_system.TOL_dq_i, maxiter=50) # print "q (out) =", q # evaluate C_q matrix C_q = self.DAE_fun.evaluate_C_q(q, t) # print "C_q =" # print C_q # evaluate C_t vector C_t = self.DAE_fun.evaluate_C_t(q, t) # print "C_t =", C_t # solve linear system of equations for dq dq = np.linalg.solve(C_q, -C_t) # assemble new q vector q=[q, dq] _q = np.concatenate([q, dq]) self.q_sol = _q # evaluate vector Q_d Q_d = self.DAE_fun.evaluate_Q_d(_q) # solve linear system of equations for ddq ddq = np.linalg.solve(C_q, Q_d) # track data self.R = np.nan self._track_data(h, t, q) self._info(t, q) # increase time step t = t + h self.t = t # integration finished - end time reached successfully if self.finished or self.failed: self.finished_solve() # save data to file self.write_simulation_solution_to_file()
class SolveDynamicAnalysis(QObject): # Thread, QObject """ classdocs """ __simulation_id = itertools.count(0) def __init__(self, MBD_system, parent=None): """ Constructor """ super(SolveDynamicAnalysis, self).__init__(parent) # parent self._parent = parent # states of simulations self.running = False self.stopped = False self.finished = False self.failed = False self._no_error = False # signals self.step_signal = stepSignal() self.finished_signal = FinishedSignal() self.repaintGL_signal = RepaintGLSignal() self.energy_signal = EnergySignal() self.error_time_integration_signal = ErrorTimeIntegrationSignal() self.filename_signal = solutionFilenameSignal() self.save_screenshot_signal = SaveScreenshot() self.solution_signal = SolutionSignal() # MBD system object self.MBD_system = MBD_system # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # DAE fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) # simulation settings - properties # self.integrationMethod = "RKF" # self.MBD_system.integrationMethod # update opengl widget every N-th step self.step = 0 self.update_opengl_widget_step_count = 0 self.update_opengl_widget_every_Nth_step = self.MBD_system.updateEveryIthStep self.save_screenshot_step_count = 0 self.save_screenshot_every_Nth_step = 2 * self.update_opengl_widget_every_Nth_step # info properties self.start_time_simulation_info_in_sec_UTC = [] self.start_time_simulation_info = [] self.end_time_simulation_info_in_sec_UTC = [] self.end_time_simulation_info = [] self.simulation_time_info_in_sec_UTC = [] self.simulation_time_info = [] self.stop_time_simulation_info_in_sec_UTC = [] self.stop_time_simulation_info = [] self.FLAG = 1 self.FLAG_contact = 0 def _solution_containers(self): """ :return: """ # solution container variable self.q0 = self._MBD_system.evaluate_q0() self.q_sol_matrix = np.array([self.q0]) self.t_vector = np.array([[0]]) self.R_vector = np.array([0]) self.step_counter = np.array([0]) self.step_size = np.array([self._MBD_system.Hmax]) self.energy_data = np.array([self._mechanical_energy(self.q0)]) def solve(self): """ Solves system of ode with dormand-prince order 5 method with runge-kutta algorithm """ # copy MBD object self._MBD_system = copy.copy(self.MBD_system) # ode fun object self.DAE_fun = DAEfun(self._MBD_system, parent=self) self.start_solve() self.start_time_simulation_info_in_sec_UTC = time.time() self.start_time_simulation_info = datetime.datetime.fromtimestamp(self.start_time_simulation_info_in_sec_UTC).strftime("%H:%M:%S %d.%m.%Y") print "Simulation started: ", self.start_time_simulation_info # create mass and inverse mass matrix (only once) self.DAE_fun.evaluate_M() # create array of initial conditions for differential equations if not self.MBD_system.q0_created: self.MBD_system.evaluate_q0() # get initial conditions q0 = self._MBD_system.evaluate_q0() self.FLAG = 1 # solution containers self._solution_containers() self.simulation_id = 0 # 0 - no contact # 1 - contact detected # -1 - contact already happened - reduce integration step self.step = 0 self.h_contact = self._MBD_system.Hmin if self.MBD_system.integrationMethod.title() == "ABAM": pass elif self.MBD_system.integrationMethod.title() == "Runge-Kutta": self.solve_ODE_RK(t_0=self._MBD_system.t_0, t_n=self._MBD_system.t_n, q0=q0, absTol=self._MBD_system.absTol, relTol=self._MBD_system.relTol, Hmax=self._MBD_system.Hmax, Hmin=self._MBD_system.Hmin) elif self.MBD_system.integrationMethod.title() == "Euler": self.solve_ODE_Euler(t_0=self._MBD_system.t_0, t_n=self._MBD_system.t_n, q0=q0, Hmax=self._MBD_system.Hmax) else: raise ValueError, "Selected integration method not supported! Method: %s"%self.MBD_system.integrationMethod def solve_ODE_Euler(self, t_0, t_n, q0, Hmax): """ Euler method for time integration :param t_0: :param t_n: :param q0: :param h: :return: """ self.t_0 = self._dt = t_0 self.t_n = t_n t = t_0 w = q0 self.Hmax = h = Hmax self.R = 0 while self.FLAG == 1: if self.stopped: self.updateGL(t=t, q=w) self.stop_solve() self.FLAG = 0 if t >= self.t_n: self.FLAG = 0 self.finished = True self.updateGL(t=t, q=w) self.t = t = t + h # print "-------------------------------------" # print "t =", t, "step =", self.step w = w + h * self.DAE_fun.evaluate_dq(h, t, w) # print "q =", w # solve contacts here # time.sleep(100) self.FLAG_contact = self.__evaluate_contacts(t, w) # print "self.FLAG_contact =", self.FLAG_contact # evaluate mechanical energy of system self._energy_t = self._mechanical_energy(w) # evaluate difference of mechanical energy of a system between this and prevouis time step self._energy_delta = self._energy_t - self.energy_data[-1] # self.__evaluate_contacts(h, t, w) h, t, w = self._track_data(h, t, w) # print "track data" self._info(t, w) # check time step h = self.check_time_step(h, w) if self.finished or self.failed: self.finished_solve() # save data to file if self.MBD_system._save_options.lower() != "discard": self.write_simulation_solution_to_file() def solve_ODE_RK(self, t_0, t_n, q0, absTol, relTol, Hmax, Hmin): """ RKF - Runge-Kutta-Fehlberg method for time integration Based on Numerical Analysis 9th ed Burden & Faires Args: t_0 - t_n - q0 - absTol - Hmax - Hmin - """ self.t_0 = self._dt = t_0 self.t_n = t_n t = t_0 w = q0 # logging.getLogger("DyS_logger").info("Size of MBD system in bytes %s" % sys.getsizeof(self.MBD_system)) logging.getLogger("DyS_logger").info("Simulation started with initial conditions q0:\n%s" % q0) self.Hmax = Hmax self.Hmin = Hmin h = Hmax h_contact = self._MBD_system.Hmin self._t_FLAG1 = Hmax # print "absTol =", absTol # self.update_opengl_widget_every_Nth_step = 1*((t_n - t_0)/Hmax) # print "self.update_opengl_widget_every_Nth_step =", self.update_opengl_widget_every_Nth_step self.save_updated_GL_screenshot_to_file(self.t_0) np.set_printoptions(precision=20, threshold=1000, edgeitems=True, linewidth=1000, suppress=False, formatter={'float': '{: 10.9e}'.format}) while self.FLAG == 1: # print "-------------------------------------" # print "step =", self.step, "contact =", self.FLAG_contact # print "t =", t, if self.stopped: self.updateGL(t=t, q=w) self.stop_solve() self.FLAG = 0 if self.FLAG_contact == 1: h = h_contact elif self.FLAG_contact == 0: h = Hmax # print "h =", h, # print "self.FLAG_contact =", self.FLAG_contact # print "--------------------------------" # print "h =", h # print "t =", t # print "w =", w # print "self.DAE_fun.evaluate_dq(h, t, w) =", self.DAE_fun.evaluate_dq(h, t, w) K1 = h * self.DAE_fun.evaluate_dq(h, t, w) K2 = h * self.DAE_fun.evaluate_dq(h, t + (1 / 4.) * h, w + (1 / 4.) * K1) K3 = h * self.DAE_fun.evaluate_dq(h, t + (3 / 8.) * h, w + (3 / 32.) * K1 + (9 / 32.) * K2) K4 = h * self.DAE_fun.evaluate_dq(h, t + (12 / 13.) * h, w + (1932 / 2197.) * K1 - (7200 / 2197.) * K2 + (7296 / 2197.) * K3) K5 = h * self.DAE_fun.evaluate_dq(h, t + h, w + (439 / 216.) * K1 - 8 * K2 + (3680 / 513.) * K3 - (845 / 4104.) * K4) K6 = h * self.DAE_fun.evaluate_dq(h, t + (1 / 2.) * h, w - (8 / 27.) * K1 + 2 * K2 - (3544 / 2565.) * K3 + (1859 / 4104.) * K4 - (11 / 40.) * K5) self.R = (1. / h) * np.linalg.norm((1 / 360.) * K1 - (128 / 4275.) * K3 - (2197 / 75240.) * K4 + (1 / 50.) * K5 + (2 / 55.) * K6) self.R = absTol # if calculated difference is less the absTolerance limit accept the calculated solution if self.R <= absTol or self.FLAG_contact == 1: self.t = t = t + h # print "i =", self.step, "t =", t # self._parent.ui.simulation_progressBar.setValue(int(1/self.t)) # print "t =", t, "h =", h, w = w + (25. / 216.) * K1 + (1408. / 2565.) * K3 + (2197. / 4104.) * K4 - (1. / 5.) * K5 # if self.step > 220: # print self.step, w # print "-----------------------------------" # check time step size # t = self.check_time_step(t, w) # print "t_out =", t # print "t = %1.3e" % t # , "absTol =%10.3E" % R_, "h =%10.3E" % h, "step =", self.step # __print_options = np.get_printoptions() # np.set_printoptions(precision=10, threshold=1000, edgeitems=True, linewidth=1000, suppress=False, formatter={'float': '{: 10.9e}'.format}) # np.set_printoptions(**__print_options) # solve contacts here # print "t =", t # time.sleep(100) self.FLAG_contact = self.__evaluate_contacts(t, w) # print "self.FLAG_contact =", self.FLAG_contact # evaluate mechanical energy of system self._energy_t = self._mechanical_energy(w) self._energy_delta = self._energy_t - self.energy_data[-1] h, t, w = self._track_data(h, t, w) # evaluate difference of mechanical energy of a system between this and prevouis time step self._info(t, w) # check time step h = self.check_time_step(h, w) # evaluate error and change step size if needed # delta = 0.84 * (absTol / R) ** (1 / 4.) # # # if self.FLAG_contact == 0 or self.FLAG_contact == 1: # if delta <= 0.1: # h = 0.1 * h # elif delta >= 4: # h = 4 * h # else: # h = delta * h # # if h > Hmax: # h = Hmax # # else: # # new step size - h has to be smaller than the last calculated over impact time # # print "t =", t # # print "h =", h # # print "t+h =", t+h # # print "self._t_FLAG1 =", self._t_FLAG1 # if t + h >= self._t_FLAG1: # h = (self._t_FLAG1 - t)/2 # # print "h =", h # # if end time is reached stop/break integration process if t >= self.t_n: self.FLAG = 0 self.finished = True self.updateGL(t=t, q=w) # # # reduce step size to get to final time t_n # elif t + h > t_n: # h = t_n - t # # # break integration as minimum step size is exceeded # elif h < Hmin and R > absTol: # self.failed = True # self.FLAG = 0 # print "t =", t # print "absTol =", absTol # print "R =", R # print "h =", h # h = Hmin # self.error_time_integration_signal.signal_time_integration_error.emit() # # print "Hmin exceeded! Procedure failed!" # time step after contact is constant and very small value if self.FLAG_contact == 1: h = self.Hmin # if h_after_contact < 10 * Hmin: # h = 10 * Hmin # else: # h = Hmax # integration finished - end time reached successfully if self.finished or self.failed or self.stopped: self.finished_solve() # save data to file self.write_simulation_solution_to_file() def _info(self, t, w): """ :return: """ # update coordinates and angles of all bodies self.__update_coordinates_and_angles_of_all_bodies(w) # update opengl display self.updateGL(t, w) # self.step += 1 # step_counter = np.append(step_counter, self.step) self.update_opengl_widget_step_count += 1 self.save_screenshot_step_count += 1 # step number signal self.step_signal.signal_step.emit(self.step) # energy data signal self.energy_signal.signal_energy.emit(self._energy_t, self._energy_delta) def _track_data(self, h, t, q): """ :return: """ # save values at next time step to matrix of solution (q_matrix) if self.FLAG_contact == 0 or self.FLAG_contact == 1: # append solution at i-th time to solution matrix self.q_sol = self.q_sol_matrix = np.vstack((self.q_sol_matrix, np.array(q))) self.step_size = np.vstack((self.step_size, h)) self.step += 1 step_num = self.step # append current step number to step counter history array self.step_counter = np.append(self.step_counter, self.step) # add to vector self.t_vector = np.vstack((self.t_vector, t)) self.R_vector = np.vstack((self.R_vector, self.R)) # append current mechanical energy to array self.energy_data = np.append(self.energy_data, self._energy_t) for contact in self.MBD_system.contacts: contact._track_data(self.step, h, t, q) # reduce step size and continue integration from previous step elif self.FLAG_contact == -1: # go to one before last solution q = self.q_sol_matrix[-1, :] # store time that is too large self._t_FLAG1 = t t = self.t_vector[-1, 0] # , 0 # reduce step size h = 0.5 * h # self.step -= 1 # step_num = self.step # step_counter[-1] = self.step self.energy_data[-1] = self._energy_t return h, t, q def __evaluate_contacts(self, t, q): """ Function solves contacts. Finds overlap pairs of (sub)AABB and calculates actual distances between node and line (edge). If distance is below userdefined limit contact is present and contact equations are solved. :type self: object Args: q vector of positions and velocities (translational and rotational) Returns: q_ vector of new calculated velocities (positions are equal before and after contact status - 0 - contact is not detected continue with integration to next time step 1 - contact is detected (has happened) return to previous time step solution and split time step size 2 - contact is detected (will happen) split time step size Raises: None """ # print "---------------------------------" # print "t =", t, "step =", self.step, self.__update_coordinates_and_angles_of_all_bodies(q) self.contact_status_list = [] # check for contact of every contact pair for contact in self.MBD_system.contacts: contact.data_tracker(t, self.step) # print "contact._contact_point_found =", contact._contact_point_found, "t =", t if not contact._contact_point_found: # function recursively loops through all potential AABB objects in AABB tree of each body # and if two AABB objects overlap, a new overlap object is created # that has both overlapping AABB objects # general contact if contact._type == "General": # adds simulation data to contact object # reset to empty list a contact object attribute of list of overlap pairs, # as this is not possible in the next line (function) due to the recursion contact.AABB_list_of_overlap_pairs = [] # check for overlap and if overlap is present build a overlap pair object contact.check_for_overlap(q, contact.AABB_i, contact.AABB_j) # if list of overlap object pairs is not empty, check for contact if contact.AABB_list_of_overlap_pairs is not []: status = contact.check_for_contact(q) self.contact_status_list.append(status) else: contact.no_overlap() # revolute clearance joint contact elif contact._type.lower() == "revolute clearance joint" or contact._type.lower() == "contact sphere-sphere" or contact._type.lower() == "contact plane-sphere" or contact._type.lower() == "pin-slot clearance joint linear": status = contact.check_for_contact(q) # print "t =", t, "status(check_for_contact) =", status self.contact_status_list.append(status) else: self.FLAG = 0 QtGui.QMessageBox.critical(self._parent, "Error!", "Contact type not correct: %s"%contact._type+". \nDefine correct contact type!", QtGui.QMessageBox.Ok) raise ValueError, "Contact type not correct: %s"%contact._type, "\n Define correct contact type!" else: # adds simulation data to contact object as this is not possible in the next line because of the recursion # print "contact.update_status()" # print "contact_update() @ solve_ODE" status = contact.contact_update(self.step, t, q) # print "status (from contact_update) =", status self.contact_status_list.append(status) self.contact_status_list = np.array(self.contact_status_list) # print "self.contact_status_list =", self.contact_status_list # no contacts if (self.contact_status_list == 0).all(): FLAG_contact = 0 callerframerecord = inspect.stack()[0] # 0 represents this line # 1 represents line at caller frame = callerframerecord[0] # print "NO CONTACT" # print "simulation sleep" info = inspect.getframeinfo(frame) # print "file:", info.filename # print "function:", info.function+"()" # time.sleep(100) return FLAG_contact # contact if (self.contact_status_list == 1).any(): FLAG_contact = 1 # logging.getLogger("DyS_logger").info("Contact detected at simulation time:%s \nbody_i=%s \nbody_j=%s", t, self.MBD_system.bodies[contact.body_id_i]._name, self.MBD_system.bodies[contact.body_id_j]._name) # logging.getLogger("DyS_logger").info("Contact detected at simulation time:%s "%t) # repaint opengl widget # self.repaintGL_signal.signal_repaintGL.emit() # solve contacts - construct contact forces # self.DAE_fun.solve_contacts(t, q) # logging.getLogger("DyS_logger").info("Contact calculation finished") return FLAG_contact # contact has already happened - reduce time step if (self.contact_status_list == -1).any(): FLAG_contact = -1 # 0-no contact, 1-contact has already happened - reduce time step return FLAG_contact else: FLAG_contact = 0 return FLAG_contact def _mechanical_energy(self, q): """ """ # predefine zero array _energy = np.zeros(self.MBD_system.number_of_bodies) # energy of all bodies for i, body in enumerate(self.MBD_system.bodies): _q = q2R_i(q, body.body_id) _dq = np.append(q2dR_i(q, body.body_id), q2dtheta_i(q, body.body_id)) # body energy # body_energy = 0.5 * (body.mass * (_dq**2) + body.J_zz * (_omega**2)) + (body.mass * self.MBD_system.gravity * _q[1]) body_energy = body.mechanical_energy(q=_q, dq=_dq, gravity=self.MBD_system.gravity) _energy[i] = body_energy # energy of normal contact forces _energy_of_contacts = np.zeros(len(self.MBD_system.contacts)) # for i, contact in enumerate(self.MBD_system.contacts): # _energy_of_contacts[i] = contact.mechanical_energy() # total mechanical energy energy = np.sum(_energy) + np.sum(_energy_of_contacts) return energy def check_time_step(self, h, q): """ """ # _t_max = [] # for contact in self.MBD_system.contacts: # if contact.AABB_list_of_overlap_pairs != []: # for body_id in contact.contact_body_id_list: # # _t = self.MBD_system.bodies[body_id].skin_thickness / np.linalg.norm(q2dR_i(q, body_id) + q2dtheta_i(q, body_id) * self.MBD_system.bodies[body_id].uP_i_max, ord=2) # _t_max.append(_t) # # t_max = min(_t_max) # if t > t_max: # return t_max # else: # return t # else: # return t # if h < self.Hmin: # h = self.Hmin if self.FLAG_contact == 1: h = self.h_contact elif self.FLAG_contact == -1: h = h else: h = self.Hmax return h def write_simulation_solution_to_file(self): """ Function writes solution data to solution data object """ print "write_simulation_solution_to_file()" solution_data = self.collect_solution_data() # write solution data and info to solution data object self._solution_data.set_filetype(self.MBD_system._solution_filetype) self.solution_filename = 'solution_data_%02d'%self.simulation_id#+self.MBD_system._solution_filetype if self.MBD_system._save_options == "save to new": # check filename if already exists self.solution_filename = check_filename(self.solution_filename) # set solution data filename self._solution_data.setName(self.solution_filename) # add data to object attribute self._solution_data.add_data(solution_data) self._solution_data.finished = True # print "exe1" # print "self.MBD_system._solution_save_options =", self.MBD_system._solution_save_options if self.MBD_system._solution_save_options != "discard": print "exe2" # write data to file self._solution_data.write_to_file() # emit signal # self.solution_signal.solution_data.emit(id(self._solution_data), self._solution_data._filename) # write contact values of every simulation time step to file for contact in self.MBD_system.contacts: if contact._solution_save_options != "discard": contact.write_to_file() def collect_solution_data(self): """ :return: """ # reshape data to join in one matrix # step _step = np.array([self.step_counter]).T # energy _energy_data = np.array([self.energy_data]).T # add all solution data to one variable solution_data = np.hstack((_step, _energy_data, self.R_vector, self.step_size, self.t_vector, self.q_sol_matrix)) return solution_data def save_solution_data(self): """ :return: """ data = self.collect_solution_data() self._solution_data.add_data(data) def start_solve(self): """ Start solver (time integration process) """ self.simulation_id = self.__simulation_id.next() # create solution data object to store data at each simulation start self._solution_data = SolutionData(MBD_system=self.MBD_system)#parent=self.MBD_system.Solution print "self._solution_data._name =", self._solution_data._name self.MBD_system.solutions.append(self._solution_data) logging.getLogger("DyS_logger").info("Simulation started, simulation id: %s"%self.simulation_id) self.running = True self.stopped = False self.finished = False def stop_solve(self): """ Stop solver (time integration process) """ self.stopped = True self.running = False self.stop_time_simulation_info_in_sec_UTC = time.time() self.stop_time_simulation_info = datetime.datetime.fromtimestamp(self.stop_time_simulation_info_in_sec_UTC).strftime("%H:%M:%S %d.%m.%Y") logging.getLogger("DyS_logger").info("Simulation stopped by user at simulation time:%s", self.t) def finished_solve(self): """ Method sets some parameters when integration process is finished :return: """ if self.finished: self.save_solution_data() self.solution_signal.solution_data.emit(id(self._solution_data), self._solution_data._name) logging.getLogger("DyS_logger").info("Simulation finished successfully!") self.finished_signal.signal_finished.emit("Finished") else: logging.getLogger("DyS_logger").info("Simulation failed!") # set booleans self.finished = True self.running = False def load_simulation_solution_from_file(self, filename): """ Function load solution_data from file. """ self.finished_signal.signal_finished.emit("Animation") if os.path.exists(filename): self.solution_data = np.loadtxt(str(filename), skiprows=2) return self.solution_data def __update_coordinates_and_angles_of_all_bodies(self, q): """ Function updates MBD_system data before display """ self.MBD_system.update_coordinates_and_angles_of_all_bodies(q) def updateGL(self, t, q): """ Update opengl widget at preset time steps """ # update display on every i-th step if self._parent._update_display_type == "step": if self.update_opengl_widget_step_count == self.update_opengl_widget_every_Nth_step or self.finished or self.stopped: self._updateGL() self.time = t self.save_updated_GL_screenshot_to_file(self.time) # reset step counter to update opengl widget self.update_opengl_widget_step_count = 0 # update display on simulation time interval dt if self._parent._update_display_type == "dt": _t = t - self._dt if _t >= self._parent._dt: self._updateGL(t, self.step) # set new value self._dt = t return None def _updateGL(self, t, step): """ :return: """ # update opengl widget self.MBD_system.update_simulation_properties(time=t, step_num=step) # emit signal self.repaintGL_signal.signal_repaintGL.emit() def save_updated_GL_screenshot_to_file(self, t): """ """ if self.MBD_system.save_screenshots: if (self.save_screenshot_step_count == self.save_screenshot_every_Nth_step) or (t == self.t_0) or (self.finished): self.screenshot_filename_abs_path = os.path.join(self.MBD_system.saved_screenshots_folder_abs_path, str("step=" + "%010d" % self.step)) self.save_screenshot_signal.signal_saveScreenshot.emit() self.save_screenshot_step_count = 0 def restore_initial_condition(self): """ Function restores initial conditions and displays it with opengl widget. """ print "restore_initial_condition()" self.step = 0 self.MBD_system._restore_initial_conditions() self.repaintGL_signal.signal_repaintGL.emit() self.updateGL(t=0, q=self.MBD_system.q0)
class IntegrateRKF45(DynamicAnalysis): """ classdocs """ def __init__(self, MBD_system, parent=None): """ Constructor """ super(IntegrateRKF45, self).__init__(MBD_system, parent) def evaluate_absError(self, w, w_new): """ :return: """ # e = (1. / self.h) * np.linalg.norm(((1./360.)*K1 - (128./4275.)*K3 - (2197./75240.)*K4 + (1./50.)*K5 + (2./55.)*K6), ord=2) # w_new = self._evaluate_w_new(w, K1, K2, K3, K4, K5, K6) e = self.evaluate_error_HHT_I3(w, w_new) # if e < 1E-11: # e = 1E-11 return e def evaluate_error_HHT_I3(self, w, w_new): """ :return: """ q_new = self.get_q(w_new) self.evaluate_q_max(w_new) x = q_new / self.q_max # error e = (self.h**2 / np.sqrt(self.q_n)) * np.linalg.norm( x, ord=2) #(self.h**2 / np.sqrt(self.q_n)) * np.linalg.norm(x, ord=2) return e def evaluate_error_RKF45(self, K1, K2, K3, K4, K5, K6): """ :return: """ d = (1. / self.h) * np.abs( np.array((1. / 360.) * K1 - (128. / 4275.) * K3 - (2197. / 75240.) * K4 + (1. / 50.) * K5 + (2. / 55.) * K6)) e = np.max(np.abs(self.get_q(d))) return e def _evaluate_w_new(self, w, K1, K2, K3, K4, K5, K6): """ :return: """ w_new = w + (25. / 216.) * K1 + (1408. / 2565.) * K3 + ( 2197. / 4104.) * K4 - (1. / 5.) * K5 return w_new def check_error(self): """ :return: """ if self.absError is np.nan: self.simulation_error.setError("Evaluated error is NaN!") return True if self.absError > self.absTol: return True else: return False def check_time_step(self, t, h, w, R=None): """ :param t: :param h: :param w: :return: """ if self.FLAG_contact == 0: delta = 0.84 * (self.absTol / np.max(R))**(1. / 4.) if delta <= 0.1: h = 0.1 * h elif delta >= 4.: h = 4. * h else: h = delta * h if h > self.Hmax: h = self.Hmax if t >= self.t_n: self.FLAG = 0 elif t + h > self.t_n: h = self.t_n - t elif h < self.Hmin: print "h =", h print "self.Hmin =", self.Hmin print "self.absTol =", self.absTol print "self.absError =", self.absError # h = self.Hmin self.simulation_error.setWarning("Hmin exceeded!") # error = True # h = self.Hmin # print "Hmin exceeded!" # self.FLAG = 0 if self.FLAG_contact == 1: h = self.Hcontact if self.FLAG_contact == -1: h = 0.5 * h if h < self.Hmin and t < self.t_n - self.Hmin: print "h =", h print "self.Hmin =", self.Hmin print "self.absTol =", self.absTol print "self.absError =", self.absError print "self.contact_status_list =", self.contact_status_list self.simulation_error.setError("Hmin exceeded! FLAG contact is: " + str(self.FLAG_contact), q=w) # error = True # h = self.Hmin return h, self.simulation_error.error def solve(self, q0=[], t_n=None, absTol=1E-9): """ Solves system of ode with order 5 method with runge-kutta algorithm """ # redefine solution containers self._solution_containers() # copy MBD object if hasattr(self.MBD_system, "setSolver"): self.MBD_system.setSolver(self) self._MBD_system = self.MBD_system # set solver self._MBD_system.setSolver(self) # dae fun object if self.DAE_fun is None: self.DAE_fun = DAEfun(self._MBD_system, parent=self) if q0: self.q0 = q0 self.start_solve() # create array of initial conditions for differential equations # get initial conditions if hasattr(self._MBD_system, "q0"): self._MBD_system.q0 = self.q0 if t_n is not None: self.t_n = t_n # evaluate stiffness matrix if hasattr(self.DAE_fun, "evaluate_K"): self.DAE_fun.evaluate_K(self.q0) self.simulation_id = 0 # 0 - no contact # +1 - contact detected # -1 - contact already happened - reduce integration step self.step = 0 if hasattr(self._MBD_system, "Hmin"): self.h_contact = self._MBD_system.Hmin self._append_to_file_step = 0 self.t_0 = self._dt = 0 self.FLAG = 1 if hasattr(self._MBD_system, "t_n"): if self._MBD_system.t_n is not None: self.t_n = self._MBD_system.t_n if hasattr(self._MBD_system, "stepsNumber"): self.stepsNumber = self._MBD_system.stepsNumber t = self.t_0 w = self.q0 if self.t_n is None and self.stepsNumber is not None: self.t_n = np.inf # logging.getLogger("DyS_logger").info("Size of MBD system in bytes %s" % sys.getsizeof(self.MBD_system)) logging.getLogger("DyS_logger").info( "Simulation started with initial conditions q0:\n%s" % self.q0) h = self.Hmax self._t_FLAG1 = self.Hmax # print "absTol =", absTol # self.update_opengl_widget_every_Nth_step = 1*((t_n - t_0)/Hmax) # print "self.update_opengl_widget_every_Nth_step =", self.update_opengl_widget_every_Nth_step self.save_updated_screenshot_to_file(self.t_0) # np.set_printoptions(precision=20, threshold=1000, edgeitems=True, linewidth=1000, suppress=False, formatter={'float': '{: 10.9e}'.format}) while self.FLAG == 1 and not self._error and not self.DAE_fun.error: # time.sleep(.001) # if self.step > 1415: # print "----------------------" # print "step =", self.step, "t =", t, "h =", h#"self.FLAG_contact =", self.FLAG_contact # print "step =", self.step, "h =", h, "t =", t # print "step =", self.step self.h = h self.t = t # print "self.h =", self.h K1 = h * self.DAE_fun.evaluate_dq(t, w) K2 = h * self.DAE_fun.evaluate_dq(t + (1. / 4.) * h, w + (1. / 4.) * K1) K3 = h * self.DAE_fun.evaluate_dq( t + (3. / 8.) * h, w + (3. / 32.) * K1 + (9. / 32.) * K2) K4 = h * self.DAE_fun.evaluate_dq( t + (12. / 13.) * h, w + (1932. / 2197.) * K1 - (7200. / 2197.) * K2 + (7296. / 2197.) * K3) K5 = h * self.DAE_fun.evaluate_dq( t + h, w + (439. / 216.) * K1 - 8. * K2 + (3680. / 513.) * K3 - (845. / 4104.) * K4) K6 = h * self.DAE_fun.evaluate_dq( t + (1. / 2.) * h, w - (8. / 27.) * K1 + 2. * K2 - (3544. / 2565.) * K3 + (1859. / 4104.) * K4 - (11. / 40.) * K5) w_new = self._evaluate_w_new(w, K1, K2, K3, K4, K5, K6) # self.absError = (1. / h) * np.linalg.norm((1. / 360.) * K1 - (128. / 4275.) * K3 - (2197. / 75240.) * K4 + (1. / 50.) * K5 + (2. / 55.) * K6) # self.absError = self.evaluate_error_HHT_I3(w, w_new, K1, K2, K3, K4, K5, K6) self.absError = self.evaluate_absError(w, w_new) #self.absError = (1. / h) * np.linalg.norm((1. / 360.) * K1 - (128. / 4275.) * K3 - (2197. / 75240.) * K4 + (1. / 50.) * K5 + (2. / 55.) * K6) #self.absError = 0.1*self.absTol # self.R = absTol # print "E_RKF45 =", self.evaluate_error_RKF45(K1, K2, K3, K4, K5, K6), "E_HHT_I3 =", self.absError # if calculated difference is less the absolute tolerance limit and accept the calculated solution # if not self.errorControl: # self.absError = self.absTol # if (self.absError <= self.absTol) and (self.FLAG_contact in [0, 1]): # print "self.check_error() =", self.check_error() # solve contacts self.FLAG_contact = self._evaluate_contacts(t, w_new) if not self.check_error(): #and (self.FLAG_contact in [0, 1]) # next time point self.t = t = t + h # value of vector of variables at next time step w = w_new if self.FLAG_contact in [0, 1]: # evaluate mechanical energy of system self._mechanical_energy, self._kinetic_energy, self._potential_energy, self._elastic_strain_energy = self._evaluate_mechanical_energy( w) self._energy_delta = self.evaluate_energy_delta( self._mechanical_energy) else: self.t, self.step, self._mechanical_energy, self._energy_delta = self._use_last_accapted_solution( t, self.step) else: self.t, self.step, self._mechanical_energy, self._energy_delta = self._use_last_accapted_solution( t, self.step) t = self.t # w = w # self.step = self.step # self._mechanical_energy = 0. # self._energy_delta = 0. # track - store simulation state data of different MBD system objects # print "h1 =", h h, t, w = self._track_data(h, t, w) # print "h2 =", h # check time step if self.errorControl: h, self._error = self.check_time_step(t, h, w, R=self.absError) else: print UserWarning, "Error control dissabled! Results may be wrong!" self._error = False # print "h3 =", h # process information of integration process if not self.check_error() and (self.FLAG_contact in [ 0, 1 ]) and self.update_visualization_widget: self._info(t, w) # check if finished if (t >= self.t_n) or (self.step == self.stepsNumber): self.FLAG = 0 self.finished = True self.refresh(t=t, q=w) # integration finished - end time reached successfully if self.finished or self.failed: self.refresh(t=t, q=w) self.finished_solve() self.FLAG = 0 if self.stopped: self.refresh(t=t, q=w) self.stop_solve() self.FLAG = 0 if np.isnan(t): self._error = True self.failed = True print "t is NaN! Check Hmax!" if self._error or self.failed or self.simulation_error.error: print "self._error =", self._error print "self.failed =", self.failed print "self.DAE_fun.error =", self.DAE_fun.error self.simulation_failed() print self.simulation_error.info print Warning, "Simulation failed!" # save data to file if self.write_to_file: self.write_simulation_solution_to_file() def _use_last_accapted_solution(self, t, step): """ :return: """ energy_t = 0. energy_delta = 0. return t, step, energy_t, energy_delta