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
