def setUp(self): M_mat = np.array([[1, -1, 0], [-1, 1.2, -1.5], [0, -1.5, 2]], dtype=float) K_mat = np.array([[2, -1, 0], [-1, 2, -1.5], [0, -1.5, 3]], dtype=float) D_mat = 0.2 * M_mat + 0.1 * K_mat self.M_unconstr = csr_matrix(M_mat) self.D_unconstr = csr_matrix(D_mat) self.K_unconstr = csr_matrix(K_mat) self.f_int_unconstr = np.array([1, 2, 3], dtype=float) self.f_ext_unconstr = np.array([3, 4, 5], dtype=float) def M(u, du, t): return self.M_unconstr def h(u, du, t): return self.f_int_unconstr def p(u, du, t): return self.f_ext_unconstr def h_q(u, du, t): return self.K_unconstr def h_dq(u, du, t): return self.D_unconstr def g_holo(u, t): return np.array(u[0], dtype=float, ndmin=1) def B_holo(u, t): return csr_matrix(np.array([[1, 0, 0]], dtype=float, ndmin=2)) self.no_of_constraints = 1 self.no_of_dofs_unconstrained = 3 self.M_func = M self.h_func = h self.p_func = p self.h_q_func = h_q self.h_dq_func = h_dq self.g_holo_func = g_holo self.B_holo_func = B_holo self.formulation = SparseLagrangeMultiplierConstraintFormulation(self.no_of_dofs_unconstrained, self.M_func, self.h_func, self.B_holo_func, self.p_func, self.h_q_func, self.h_dq_func, g_func=self.g_holo_func)
def test_formulation_without_constraints(self): def B(u, t): return csr_matrix((0, 3)) def g_holo(u, t): return np.array([], ndmin=1) formulation = SparseLagrangeMultiplierConstraintFormulation(self.no_of_dofs_unconstrained, self.M_func, self.h_func, B, self.p_func, self.h_q_func, self.h_dq_func, g_func=g_holo) x = np.arange(formulation.dimension, dtype=float) dx = x.copy() self.assertEqual(formulation._no_of_constraints, 0.0) K_actual = formulation.K(x, dx, 0.0) assert_array_equal(K_actual.todense(), self.K_unconstr.todense()) D_actual = formulation.D(x, dx, 0.0) assert_array_equal(D_actual.todense(), self.D_unconstr.todense()) M_actual = formulation.M(x, dx, 0.0) assert_array_equal(M_actual.todense(), self.M_unconstr.todense()) F_int_actual = formulation.f_int(x, dx, 0.0) u = x[:self.no_of_dofs_unconstrained] du = dx[:self.no_of_dofs_unconstrained] F_int_desired = self.h_func(u, du, 0.0) assert_array_equal(F_int_actual, F_int_desired) F_ext_actual = formulation.f_ext(x, dx, 0.0) F_ext_desired = self.p_func(u, du, 0.0) assert_array_equal(F_ext_actual, F_ext_desired)
def test_pendulum_time_integration(self): """ Test that computes the time integration of the pendulum """ formulation_lagrange = SparseLagrangeMultiplierConstraintFormulation( self.cm.no_of_dofs_unconstrained, self.M_raw_func, self.h_func, self.B_func, self.p_func, self.dh_dq, self.dh_ddq, g_func=self.g_func) formulation_nullspace = NullspaceConstraintFormulation( self.cm.no_of_dofs_unconstrained, self.M_raw_func, self.h_func, self.B_func, self.p_func, self.dh_dq, self.dh_ddq, g_func=self.g_func, a_func=self.a_func) sol_lagrange = AmfeSolution() sol_nullspace = AmfeSolution() formulations = [(formulation_lagrange, sol_lagrange), (formulation_nullspace, sol_nullspace)] # Define state: 90deg to the right q0 = np.array([0.0, 0.0, self.L, self.L]) dq0 = np.array([0.0, 0.0, 0.0, 0.0]) nonlinear_solver = NewtonRaphson() for (formulation, sol) in formulations: def write_callback(t, x, dx, ddx): u = formulation.u(x, t) du = formulation.du(x, dx, t) ddu = formulation.ddu(x, dx, ddx, t) sol.write_timestep(t, u, du, ddu) integrator = GeneralizedAlpha(formulation.M, formulation.f_int, formulation.f_ext, formulation.K, formulation.D, alpha_m=0.0) integrator.dt = 0.025 integrator.nonlinear_solver_func = nonlinear_solver.solve integrator.nonlinear_solver_options = {'rtol': 1e-7, 'atol': 1e-6} # Initialize first timestep t = 0.0 t_end = 0.5 x = np.zeros(formulation.dimension) dx = np.zeros(formulation.dimension) ddx = np.zeros(formulation.dimension) x[:4] = q0 dx[:4] = dq0 # Call write timestep for initial conditions write_callback(t, x, dx, ddx) # Run Loop while t < t_end: t, x, dx, ddx = integrator.step(t, x, dx, ddx) write_callback(t, x, dx, ddx) def plot_pendulum_path(u_plot): plt.scatter(self.X[2] + [u[2] for u in u_plot], self.X[3] + [u[3] for u in u_plot]) plt.title( 'Pendulum-test: Positional curve of rigid pendulum under gravitation' ) return # UNCOMMENT THESE LINES IF YOU LIKE TO SEE A TRAJECTORY (THIS CAN NOT BE DONE FOR GITLAB-RUNNER # plot_pendulum_path(sol_lagrange.q) # plot_pendulum_path(sol_nullspace.q) # plt.show() # test if nullspace formulation is almost equal to lagrangian # and test if constraint is not violated for q_lagrange, q_nullspace in zip(sol_lagrange.q, sol_nullspace.q): assert_allclose(q_nullspace, q_lagrange, atol=1e-1) x_lagrange = self.X.reshape(-1) + q_lagrange x_nullspace = self.X.reshape(-1) + q_nullspace assert_allclose(np.array( [np.linalg.norm(x_lagrange[2:] - x_lagrange[:2])]), np.array([self.L]), atol=1e-3) assert_allclose(np.array( [np.linalg.norm(x_nullspace[2:] - x_nullspace[:2])]), np.array([self.L]), atol=1e-1)
def test_pendulum_pure_lagrange(self): """ Test that tests a pure lagrange formulation. """ formulation = SparseLagrangeMultiplierConstraintFormulation( self.cm.no_of_dofs_unconstrained, self.M_raw_func, self.h_func, self.B_func, self.p_func, self.dh_dq, self.dh_ddq, g_func=self.g_func) # Define state: 90deg to the right x = np.array([0.0, 0.0, self.L, self.L, 0.0, 0.0, 0.0]) unconstrained_u_desired = np.array([0.0, 0.0, self.L, self.L]) unconstrained_u_actual = formulation.u(x, 0.0) # test if unconstrained u is correctly calculated assert_array_equal(unconstrained_u_actual, unconstrained_u_desired) # Check if g function is zero for different positions # -90 deg x = np.array([0.0, 0.0, -self.L, self.L, 0.0, 0.0, 0.0]) F1 = formulation.f_int(x, np.zeros_like(x), 0.0) F2 = formulation.f_int(x, np.zeros_like(x), 2.0) F3 = formulation.f_int(x, x, 2.0) assert_array_equal(F1[-3:], np.array([0.0, 0.0, 0.0])) assert_array_equal(F2[-3:], np.array([0.0, 0.0, 0.0])) assert_array_equal(F3[-3:], np.array([0.0, 0.0, 0.0])) x = np.array([0.0, 0.0, 0.0, -0.1 * self.L, 0.0, 0.0, 0.0]) F1 = formulation.f_int(x, np.zeros_like(x), 0.0) # Violated constraint, thus g > 0.0 self.assertGreater(np.linalg.norm(F1[-3:]), 0.0) # Test jacobians of constraint formulation by comparing with finite difference approximation delta = 0.00000001 x = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) K_actual = formulation.K(x, x, 0.0).todense() K_finite_difference = np.zeros_like(K_actual) for i in range(len(x)): x_plus = x.copy() x_plus[i] = x_plus[i] + delta x_minus = x.copy() x_minus[i] = x_minus[i] - delta F_plus = formulation.f_int(x_plus, x, 0.0).copy() - formulation.f_ext( x_plus, x, 0.0).copy() F_minus = formulation.f_int(x_minus, x, 0.0) - formulation.f_ext( x_minus, x, 0.0) K_finite_difference[:, i] = (F_plus - F_minus).reshape( -1, 1) / (2 * delta) assert_allclose(K_actual, K_finite_difference, rtol=1e-7)
def test_pendulum_time_integration(self): """ Test that computes the time integration of the pendulum """ formulation_lagrange = SparseLagrangeMultiplierConstraintFormulation(self.cm.no_of_dofs_unconstrained, self.M_raw_func, self.h_func, self.B_func, self.p_func, self.dh_dq, self.dh_ddq, g_func=self.g_func) formulation_nullspace = NullspaceConstraintFormulation(self.cm.no_of_dofs_unconstrained, self.M_raw_func, self.h_func, self.B_func, self.p_func, self.dh_dq, self.dh_ddq, g_func=self.g_func, a_func=self.a_func) sol_lagrange = AmfeSolution() sol_nullspace = AmfeSolution() phi_analytical = [] formulations = [(formulation_lagrange, sol_lagrange), (formulation_nullspace, sol_nullspace)] # Define state: 90deg to the right q0 = np.array([0.0, 0.0, self.L, self.L]) dq0 = np.array([0.0, 0.0, 0.0, 0.0]) nonlinear_solver = NewtonRaphson() for (formulation, sol) in formulations: def write_callback(t, x, dx, ddx): u = formulation.u(x, t) du = formulation.du(x, dx, t) ddu = formulation.ddu(x, dx, ddx, t) sol.write_timestep(t, u, du, ddu) integrator = GeneralizedAlpha(formulation.M, formulation.f_int, formulation.f_ext, formulation.K, formulation.D, alpha_m=0.0) integrator.dt = 0.025 integration_stepper = NonlinearIntegrationStepper(integrator) integration_stepper.nonlinear_solver_func = nonlinear_solver.solve integration_stepper.nonlinear_solver_options = {'rtol': 1e-7, 'atol': 1e-6} # Initialize first timestep t = 0.0 t_end = 0.5 x = np.zeros(formulation.dimension) dx = np.zeros(formulation.dimension) ddx = np.zeros(formulation.dimension) x[:4] = q0 dx[:4] = dq0 # Call write timestep for initial conditions write_callback(t, x, dx, ddx) i = 0 # Run Loop while t < t_end: phi_analytical.append(np.pi / 2 * np.cos(np.sqrt(self.g / self.L) * t)) t, x, dx, ddx = integration_stepper.step(t, x, dx, ddx) write_callback(t, x, dx, ddx) i += 1 phi_analytical.append(np.pi / 2 * np.cos(np.sqrt(self.g / self.L) * t)) def plot_pendulum_path(u_plot, label_name): return plt.scatter(self.X[2]+[u[2] for u in u_plot], self.X[3]+[u[3] for u in u_plot], label=label_name) plt.title('Pendulum-test: Positional curve of rigid pendulum under gravitation') lagrange = plot_pendulum_path(sol_lagrange.q, 'Lagrange') nullspace = plot_pendulum_path(sol_nullspace.q, 'Nullspace') # ANALYTICAL SOLUTION FOR SMALL ANGLES #analytical = plt.scatter([self.L*np.sin(phi) for phi in phi_analytical], [-self.L*np.cos(phi) for phi in phi_analytical], label='Analytical') axes = plt.gca() axes.set_xlim([-2.1, 2.1]) axes.set_ylim([-2.1, 2.1]) plt.legend(handles=[lagrange, nullspace])#, analytical]) # UNCOMMENT NEXT LINE IF YOU LIKE TO SEE A TRAJECTORY (THIS CAN NOT BE DONE FOR GITLAB-RUNNER) # plt.show() # test if nullspace formulation is almost equal to lagrangian # and test if constraint is not violated for q_lagrange, q_nullspace in zip(sol_lagrange.q, sol_nullspace.q): assert_allclose(q_nullspace, q_lagrange, atol=1e-1) x_lagrange = self.X.reshape(-1) + q_lagrange x_nullspace = self.X.reshape(-1) + q_nullspace assert_allclose(np.array([np.linalg.norm(x_lagrange[2:] - x_lagrange[:2])]), np.array([self.L]), atol=1e-3) assert_allclose(np.array([np.linalg.norm(x_nullspace[2:] - x_nullspace[:2])]), np.array([self.L]), atol=1e-1)
def _create_constraint_formulation(mechanical_system, component, formulation, **formulation_options): """ Internal method that creates a constraint formulation for a mechanical system combined with the constraints from a component Parameters ---------- mechanical_system : MechanicalSystem Mechanical system whose matrices M, K, D, f_int, f_ext will be used in the constraint formulation component : amfe.component.StructuralComponent Structural Component having constraint functions, such as g_holo, B, b, a. formulation : str {'boolean', 'lagrange', 'nullspace_elimination'} String describing the constraint formulation that shall be used formulation_options : dict options passed to the set_options method of the constraint formulation Returns ------- constraint_formulation : amfe.constraint.ConstraintFormulation A ConstraintFormulation object that applies the constraints on the mechanical system. """ no_of_dofs_unconstrained = mechanical_system.dimension if formulation == 'boolean': constraint_formulation = BooleanEliminationConstraintFormulation( no_of_dofs_unconstrained, mechanical_system.M, mechanical_system.f_int, component.B, mechanical_system.f_ext, mechanical_system.K, mechanical_system.D, g_func=component.g_holo) elif formulation == 'lagrange': constraint_formulation = SparseLagrangeMultiplierConstraintFormulation( no_of_dofs_unconstrained, mechanical_system.M, mechanical_system.f_int, component.B, mechanical_system.f_ext, mechanical_system.K, mechanical_system.D, g_func=component.g_holo) elif formulation == 'nullspace_elimination': constraint_formulation = NullspaceConstraintFormulation( no_of_dofs_unconstrained, mechanical_system.M, mechanical_system.f_int, component.B, mechanical_system.f_ext, mechanical_system.K, mechanical_system.D, g_func=component.g_holo, b_func=component.b, a_func=component.a) else: raise ValueError('formulation not valid') if formulation_options is not None: constraint_formulation.set_options(**formulation_options) return constraint_formulation
class SparseLagrangeFormulationTest(TestCase): def setUp(self): M_mat = np.array([[1, -1, 0], [-1, 1.2, -1.5], [0, -1.5, 2]], dtype=float) K_mat = np.array([[2, -1, 0], [-1, 2, -1.5], [0, -1.5, 3]], dtype=float) D_mat = 0.2 * M_mat + 0.1 * K_mat self.M_unconstr = csr_matrix(M_mat) self.D_unconstr = csr_matrix(D_mat) self.K_unconstr = csr_matrix(K_mat) self.f_int_unconstr = np.array([1, 2, 3], dtype=float) self.f_ext_unconstr = np.array([3, 4, 5], dtype=float) def M(u, du, t): return self.M_unconstr def h(u, du, t): return self.f_int_unconstr def p(u, du, t): return self.f_ext_unconstr def h_q(u, du, t): return self.K_unconstr def h_dq(u, du, t): return self.D_unconstr def g_holo(u, t): return np.array(u[0], dtype=float, ndmin=1) def B_holo(u, t): return csr_matrix(np.array([[1, 0, 0]], dtype=float, ndmin=2)) self.no_of_constraints = 1 self.no_of_dofs_unconstrained = 3 self.M_func = M self.h_func = h self.p_func = p self.h_q_func = h_q self.h_dq_func = h_dq self.g_holo_func = g_holo self.B_holo_func = B_holo self.formulation = SparseLagrangeMultiplierConstraintFormulation(self.no_of_dofs_unconstrained, self.M_func, self.h_func, self.B_holo_func, self.p_func, self.h_q_func, self.h_dq_func, g_func=self.g_holo_func) def tearDown(self): self.formulation = None def test_no_of_dofs_unconstrained(self): self.assertEqual(self.formulation.no_of_dofs_unconstrained, self.no_of_dofs_unconstrained) self.formulation.no_of_dofs_unconstrained = 5 self.assertEqual(self.formulation.no_of_dofs_unconstrained, 5) def test_dimension(self): self.assertEqual(self.formulation.dimension, self.no_of_dofs_unconstrained + self.no_of_constraints) def test_update(self): # Just test if update function works self.formulation.update() def test_recover_u_du_ddu_lambda(self): x = np.arange(self.formulation.dimension, dtype=float) dx = x.copy() + 1.0 ddx = dx.copy() + 1.0 u, du, ddu = self.formulation.recover(x, dx, ddx, 5.0) assert_array_equal(u, x[:self.no_of_dofs_unconstrained]) assert_array_equal(du, dx[:self.no_of_dofs_unconstrained]) assert_array_equal(ddu, ddx[:self.no_of_dofs_unconstrained]) u = self.formulation.u(x, 2.0) du = self.formulation.du(x, dx, 3.0) ddu = self.formulation.ddu(x, dx, ddx, 6.0) lagrange_multiplier = self.formulation.lagrange_multiplier(x, 6.0) assert_array_equal(u, x[:self.no_of_dofs_unconstrained]) assert_array_equal(du, dx[:self.no_of_dofs_unconstrained]) assert_array_equal(ddu, ddx[:self.no_of_dofs_unconstrained]) assert_array_equal(lagrange_multiplier, x[self.no_of_dofs_unconstrained:]) def test_M(self): x = np.arange(self.formulation.dimension, dtype=float) dx = x.copy() M_desired = spvstack((sphstack((self.M_unconstr, csr_matrix((self.no_of_dofs_unconstrained, self.no_of_constraints))), format='csr'), sphstack((csr_matrix((self.no_of_constraints, self.no_of_dofs_unconstrained)), csr_matrix((1, 1))), format='csr')), format='csr') M_actual = self.formulation.M(x, dx, 0.0) assert_array_equal(M_actual.todense(), M_desired.todense()) def test_f_int(self): x = np.arange(self.no_of_dofs_unconstrained + self.no_of_constraints, dtype=float) + 1.0 dx = x.copy() + 1.0 u = x[:self.no_of_dofs_unconstrained] du = dx[:self.no_of_dofs_unconstrained] t = 0.0 F_desired = np.concatenate((self.h_func(u, du, t)+self.B_holo_func(u, t).T.dot(x[self.no_of_dofs_unconstrained:]), +self.g_holo_func(u, t))) F_actual = self.formulation.f_int(x, dx, t) assert_array_equal(F_actual, F_desired) def test_f_ext(self): x = np.arange(self.no_of_dofs_unconstrained + self.no_of_constraints, dtype=float) + 1.0 dx = x.copy() + 1.0 u = x[:self.no_of_dofs_unconstrained] du = dx[:self.no_of_dofs_unconstrained] t = 0.0 F_desired = np.concatenate( (self.p_func(u, du, t), np.zeros(self.no_of_constraints))) F_actual = self.formulation.f_ext(x, dx, t) assert_array_equal(F_actual, F_desired) def test_D(self): x = np.arange(self.formulation.dimension, dtype=float) dx = x.copy() D_desired = spvstack((sphstack((self.D_unconstr, csr_matrix((self.no_of_dofs_unconstrained, self.no_of_constraints))), format='csr'), sphstack((csr_matrix((self.no_of_constraints, self.no_of_dofs_unconstrained)), csr_matrix((1, 1))), format='csr')), format='csr') D_actual = self.formulation.D(x, dx, 0.0) assert_array_equal(D_actual.todense(), D_desired.todense()) def test_K(self): x = np.arange(self.formulation.dimension, dtype=float) dx = x.copy() K_desired = spvstack((sphstack((self.K_unconstr, self.B_holo_func(x[:self.no_of_dofs_unconstrained], 0.0).T), format='csr'), sphstack((self.B_holo_func(x[:self.no_of_dofs_unconstrained], 0.0), csr_matrix((1, 1))), format='csr')), format='csr') K_actual = self.formulation.K(x, dx, 0.0) assert_array_equal(K_actual.todense(), K_desired.todense()) def test_formulation_without_constraints(self): def B(u, t): return csr_matrix((0, 3)) def g_holo(u, t): return np.array([], ndmin=1) formulation = SparseLagrangeMultiplierConstraintFormulation(self.no_of_dofs_unconstrained, self.M_func, self.h_func, B, self.p_func, self.h_q_func, self.h_dq_func, g_func=g_holo) x = np.arange(formulation.dimension, dtype=float) dx = x.copy() self.assertEqual(formulation._no_of_constraints, 0.0) K_actual = formulation.K(x, dx, 0.0) assert_array_equal(K_actual.todense(), self.K_unconstr.todense()) D_actual = formulation.D(x, dx, 0.0) assert_array_equal(D_actual.todense(), self.D_unconstr.todense()) M_actual = formulation.M(x, dx, 0.0) assert_array_equal(M_actual.todense(), self.M_unconstr.todense()) F_int_actual = formulation.f_int(x, dx, 0.0) u = x[:self.no_of_dofs_unconstrained] du = dx[:self.no_of_dofs_unconstrained] F_int_desired = self.h_func(u, du, 0.0) assert_array_equal(F_int_actual, F_int_desired) F_ext_actual = formulation.f_ext(x, dx, 0.0) F_ext_desired = self.p_func(u, du, 0.0) assert_array_equal(F_ext_actual, F_ext_desired) def test_scaling(self): x0 = np.zeros(self.formulation.dimension) x0[0] = 3.141 dx0 = np.zeros(self.formulation.dimension) h0 = self.h_func(x0, dx0, 0.0) F0 = np.zeros(self.formulation.dimension) F0[:len(h0)] = h0 # Test only constraint equation scaling self.formulation.set_options(scaling=2.0) F_scale_2 = self.formulation.f_int(x0, dx0, 0.0).copy() self.formulation.set_options(scaling=4.0) F_scale_4 = self.formulation.f_int(x0, dx0, 0.0).copy() assert_array_equal((F_scale_2-F0)/2.0, (F_scale_4-F0)/4.0) # Test B.T lambda scaling x0 = np.zeros(self.formulation.dimension) x0[-1] = 2.7 self.formulation.set_options(scaling=2.0) F_scale_2 = self.formulation.f_int(x0, dx0, 0.0).copy() self.formulation.set_options(scaling=4.0) F_scale_4 = self.formulation.f_int(x0, dx0, 0.0).copy() assert_array_equal((F_scale_2-F0)/2.0, (F_scale_4-F0)/4.0) # Test stiffness matrix scaling K0 = self.K_unconstr K0_full = np.zeros((self.formulation.dimension, self.formulation.dimension)) for i in range(self.K_unconstr.shape[0]): K0_full[i, :self.K_unconstr.shape[1]] = K0[i, :].toarray()[:] x0 = np.zeros(self.formulation.dimension) x0[-1] = 2.7 x0[0] = 3.141 self.formulation.set_options(scaling=2.0) K_scale_2 = self.formulation.K(x0, dx0, 0.0).copy() self.formulation.set_options(scaling=4.0) K_scale_4 = self.formulation.K(x0, dx0, 0.0).copy() assert_array_equal((K_scale_2 - K0_full) / 2.0, (K_scale_4 - K0_full) / 4.0) def test_penalty(self): scale1 = 2.0 scale2 = 4.0 x0 = np.zeros(self.formulation.dimension) x0[-1] = 2.7 x0[0] = 3.141 dx0 = np.zeros(self.formulation.dimension) B = self.B_holo_func(x0[:-1], 0.0) BTB = B.T.dot(B) KBTB = np.zeros((self.formulation.dimension, self.formulation.dimension)) for i in range(BTB.shape[0]): KBTB[i, :BTB.shape[1]] = BTB.toarray()[i, :] BTg = B.T.dot(self.g_holo_func(x0[:-1], 0.0)) FBTg = np.zeros(self.formulation.dimension) FBTg[:len(BTg)] = BTg self.formulation.set_options(penalty=scale1) K_pen_2 = self.formulation.K(x0, dx0, 0.0).copy() F_pen_2 = self.formulation.f_int(x0, dx0, 0.0).copy() self.formulation.set_options(penalty=scale2) K_pen_4 = self.formulation.K(x0, dx0, 0.0).copy() F_pen_4 = self.formulation.f_int(x0, dx0, 0.0).copy() assert_allclose(K_pen_2 - scale1 * KBTB, K_pen_4 - scale2 * KBTB) assert_allclose(F_pen_2 - scale1 * FBTg, F_pen_4 - scale2 * FBTg)