def run(self, structural_step=None, dt=None):
if structural_step is None:
structural_step = self.data.structure.timestep_info[-1]
if structural_step.mb_dict is not None:
MBdict = structural_step.mb_dict
else:
MBdict = self.data.structure.ini_mb_dict
if dt is None:
dt = ct.c_float(0.)
self.lc_list = lagrangeconstraints.initialize_constraints(MBdict)
self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
# TODO: only working for constant forces
MB_beam, MB_tstep = mb.split_multibody(self.data.structure,
structural_step, MBdict, 0)
q = np.zeros((self.sys_size + self.num_LM_eq, ),
dtype=ct.c_double,
order='F')
dqdt = np.zeros((self.sys_size + self.num_LM_eq, ),
dtype=ct.c_double,
order='F')
dqddt = np.zeros((self.sys_size + self.num_LM_eq, ),
dtype=ct.c_double,
order='F')
mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt)
# Lagrange multipliers parameters
num_LM_eq = self.num_LM_eq
Lambda = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F')
# Lambda_dot = np.zeros((num_LM_eq,), dtype=ct.c_double, order='F')
Dq_old = 0.
Dq = np.zeros((self.sys_size, ))
for iLoadStep in range(0, self.settings['num_load_steps'].value + 1):
iter = -1
# delta = settings.min_delta + 1.
converged = False
while converged == False:
iter += 1
if (iter == self.settings['max_iterations'].value - 1):
cout.cout_wrap(("Residual is: %f" % np.amax(np.abs(Dq))),
4)
cout.cout_wrap("Static equations did not converge", 4)
break
MB_K, MB_Q = self.assembly_MB_eq_system(
MB_beam, MB_tstep, Lambda, MBdict, iLoadStep)
Dq = np.linalg.solve(MB_K, -MB_Q)
# Dq *= 0.7
q += Dq
mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
if (iter > 0):
if (np.amax(np.abs(Dq)) < Dq_old):
converged = True
if iter == 0:
Dq_old = np.amax(np.array([1., np.amax(
np.abs(Dq))])) * self.settings['min_delta'].value
lagrangeconstraints.postprocess(self.lc_list, MB_beam,
MB_tstep, "static")
self.compute_forces_constraints(MB_beam, MB_tstep, Lambda)
if self.settings['gravity_on']:
for ibody in range(len(MB_beam)):
xbeamlib.cbeam3_correct_gravity_forces(MB_beam[ibody],
MB_tstep[ibody],
self.settings)
mb.merge_multibody(MB_tstep, MB_beam, self.data.structure,
structural_step, MBdict, 0.)
# # Initialize
# q = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
# dqdt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
# dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
#
# # Predictor step
# mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt)
#
# q += dt*dqdt + (0.5 - self.beta)*dt*dt*dqddt
# dqdt += (1.0 - self.gamma)*dt*dqddt
# dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
# if not num_LM_eq == 0:
# Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
# Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
# else:
# Lambda = 0
# Lambda_dot = 0
#
# # Newmark-beta iterations
# old_Dq = 1.0
# LM_old_Dq = 1.0
#
# converged = False
# for iter in range(self.settings['max_iterations'].value):
# # Check if the maximum of iterations has been reached
# if (iter == self.settings['max_iterations'].value - 1):
# print('Solver did not converge in ', iter, ' iterations.')
# print('res = ', res)
# print('LM_res = ', LM_res)
# import pdb; pdb.set_trace()
# break
#
# # Update positions and velocities
# mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
# MB_Asys, MB_Q = self.assembly_MB_eq_system(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot, MBdict)
#
# # Compute the correction
# # ADC next line not necessary
# # Dq = np.zeros((self.sys_size+num_LM_eq,), dtype=ct.c_double, order='F')
# # MB_Asys_balanced, T = scipy.linalg.matrix_balance(MB_Asys)
# # invT = np.matrix(T).I
# # MB_Q_balanced = np.dot(invT, MB_Q).T
#
# Dq = np.linalg.solve(MB_Asys, -MB_Q)
# # least squares solver
# # Dq = np.linalg.lstsq(np.dot(MB_Asys_balanced, invT), -MB_Q_balanced, rcond=None)[0]
#
# # Evaluate convergence
# if (iter > 0):
# res = np.max(np.abs(Dq[0:self.sys_size]))/old_Dq
# if not num_LM_eq == 0:
# LM_res = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))/LM_old_Dq
# else:
# LM_res = 0.0
# if (res < self.settings['min_delta'].value) and (LM_res < self.settings['min_delta'].value*1e-2):
# converged = True
#
# # Compute variables from previous values and increments
# # TODO:decide If I want other way of updating lambda
# # this for least sq
# # q[:, np.newaxis] += Dq
# # dqdt[:, np.newaxis] += self.gamma/(self.beta*dt)*Dq
# # dqddt[:, np.newaxis] += 1.0/(self.beta*dt*dt)*Dq
#
# # this for direct solver
# q += Dq
# dqdt += self.gamma/(self.beta*dt)*Dq
# dqddt += 1.0/(self.beta*dt*dt)*Dq
#
# if not num_LM_eq == 0:
# Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
# Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
# else:
# Lambda = 0
# Lambda_dot = 0
#
# if converged:
# break
#
# if iter == 0:
# old_Dq = np.max(np.abs(Dq[0:self.sys_size]))
# if old_Dq < 1.0:
# old_Dq = 1.0
# if num_LM_eq:
# LM_old_Dq = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))
# else:
# LM_old_Dq = 1.0
#
# mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
# # end: comment time stepping
#
# # End of Newmark-beta iterations
# self.integrate_position(MB_beam, MB_tstep, dt)
# # lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, MBdict, "dynamic")
# lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "dynamic")
# self.compute_forces_constraints(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot)
# if self.settings['gravity_on']:
# for ibody in range(len(MB_beam)):
# xbeamlib.cbeam3_correct_gravity_forces(MB_beam[ibody], MB_tstep[ibody], self.settings)
# mb.merge_multibody(MB_tstep, MB_beam, self.data.structure, structural_step, MBdict, dt)
# structural_step.q[:] = q[:self.sys_size].copy()
# structural_step.dqdt[:] = dqdt[:self.sys_size].copy()
# structural_step.dqddt[:] = dqddt[:self.sys_size].copy()
return self.data
def run(self, structural_step=None, dt=None):
if structural_step is None:
structural_step = self.data.structure.timestep_info[-1]
if structural_step.mb_dict is not None:
MBdict = structural_step.mb_dict
else:
MBdict = self.data.structure.ini_mb_dict
if dt is None:
dt = self.settings['dt'].value
else:
self.settings['dt'] = ct.c_float(dt)
self.lc_list = lagrangeconstraints.initialize_constraints(MBdict)
self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
# TODO: only working for constant forces
MB_beam, MB_tstep = mb.split_multibody(self.data.structure,
structural_step, MBdict,
self.data.ts)
# Lagrange multipliers parameters
num_LM_eq = self.num_LM_eq
Lambda = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F')
Lambda_dot = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F')
# Initialize
q = np.zeros((self.sys_size + num_LM_eq, ),
dtype=ct.c_double,
order='F')
dqdt = np.zeros((self.sys_size + num_LM_eq, ),
dtype=ct.c_double,
order='F')
dqddt = np.zeros((self.sys_size + num_LM_eq, ),
dtype=ct.c_double,
order='F')
# Predictor step
mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt)
q += dt * dqdt + (0.5 - self.beta) * dt * dt * dqddt
dqdt += (1.0 - self.gamma) * dt * dqddt
dqddt = np.zeros((self.sys_size + num_LM_eq, ),
dtype=ct.c_double,
order='F')
if not num_LM_eq == 0:
Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double,
copy=True,
order='F')
Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double,
copy=True,
order='F')
else:
Lambda = 0
Lambda_dot = 0
# Newmark-beta iterations
old_Dq = 1.0
LM_old_Dq = 1.0
converged = False
for iteration in range(self.settings['max_iterations'].value):
# Check if the maximum of iterations has been reached
if iteration == self.settings['max_iterations'].value - 1:
error = (
'Solver did not converge in %d iterations.\n res = %e \n LM_res = %e'
% (iteration, res, LM_res))
raise exc.NotConvergedSolver(error)
# Update positions and velocities
mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
MB_Asys, MB_Q = self.assembly_MB_eq_system(MB_beam, MB_tstep,
self.data.ts, dt,
Lambda, Lambda_dot,
MBdict)
# Compute the correction
# ADC next line not necessary
# Dq = np.zeros((self.sys_size+num_LM_eq,), dtype=ct.c_double, order='F')
# MB_Asys_balanced, T = scipy.linalg.matrix_balance(MB_Asys)
# invT = np.matrix(T).I
# MB_Q_balanced = np.dot(invT, MB_Q).T
Dq = np.linalg.solve(MB_Asys, -MB_Q)
# least squares solver
# Dq = np.linalg.lstsq(np.dot(MB_Asys_balanced, invT), -MB_Q_balanced, rcond=None)[0]
# Evaluate convergence
if iteration:
res = np.max(np.abs(Dq[0:self.sys_size])) / old_Dq
if np.isnan(res):
raise exc.NotConvergedSolver('Multibody res = NaN')
if num_LM_eq:
LM_res = np.max(
np.abs(Dq[self.sys_size:self.sys_size +
num_LM_eq])) / LM_old_Dq
else:
LM_res = 0.0
if (res < self.settings['min_delta'].value) and (
LM_res < self.settings['min_delta'].value):
converged = True
# Compute variables from previous values and increments
# TODO:decide If I want other way of updating lambda
# this for least sq
# q[:, np.newaxis] += Dq
# dqdt[:, np.newaxis] += self.gamma/(self.beta*dt)*Dq
# dqddt[:, np.newaxis] += 1.0/(self.beta*dt*dt)*Dq
# this for direct solver
q += Dq
dqdt += self.gamma / (self.beta * dt) * Dq
dqddt += 1.0 / (self.beta * dt * dt) * Dq
if not num_LM_eq == 0:
Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double,
copy=True,
order='F')
Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double,
copy=True,
order='F')
else:
Lambda = 0
Lambda_dot = 0
if converged:
break
if not iteration:
old_Dq = np.max(np.abs(Dq[0:self.sys_size]))
if old_Dq < 1.0:
old_Dq = 1.0
if num_LM_eq:
LM_old_Dq = np.max(
np.abs(Dq[self.sys_size:self.sys_size + num_LM_eq]))
else:
LM_old_Dq = 1.0
mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
# end: comment time stepping
# End of Newmark-beta iterations
self.integrate_position(MB_beam, MB_tstep, dt)
# lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, MBdict, "dynamic")
lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep,
"dynamic")
self.compute_forces_constraints(MB_beam, MB_tstep, self.data.ts, dt,
Lambda, Lambda_dot)
if self.settings['gravity_on']:
for ibody in range(len(MB_beam)):
xbeamlib.cbeam3_correct_gravity_forces(MB_beam[ibody],
MB_tstep[ibody],
self.settings)
mb.merge_multibody(MB_tstep, MB_beam, self.data.structure,
structural_step, MBdict, dt)
# structural_step.q[:] = q[:self.sys_size].copy()
# structural_step.dqdt[:] = dqdt[:self.sys_size].copy()
# structural_step.dqddt[:] = dqddt[:self.sys_size].copy()
return self.data