diff --git a/docs/boundary_conditions.rst b/docs/boundary_conditions.rst index 37ddb2e5..af23bef9 100644 --- a/docs/boundary_conditions.rst +++ b/docs/boundary_conditions.rst @@ -105,9 +105,18 @@ ____________________________________________ fedoo.MPC -Avdanced BC and constraints +Advanced BC and constraints ============================== +Fedoo also provides higher-level boundary-condition objects for common +distributed loads and constraint patterns. Distributed loads are converted to +external Neumann forces, while the kinematic constraints below are based on +MPC equations and can be added directly to ``pb.bc``. + + +Distributed loads +_________________ + Distributed loads are defined by dedicated assembly objects. The recommended way to apply them is to add the load assembly to the problem boundary conditions. If the assembly provides an ``as_neumann()`` method, @@ -147,18 +156,118 @@ boundary-condition force. fedoo.constraint.Pressure fedoo.constraint.SurfaceForce -Some advanced constraints based on multiple linearized MPC are available in -fedoo. They can be created and added to the problem with the pb.bc.add method. + +Kinematic MPC constraints +_________________________ + +Rigid tie +~~~~~~~~~ + +The :py:class:`fedoo.constraint.RigidTie` constraint couples a set of nodes to +a rigid-body motion. It creates global degrees of freedom for rigid +translation and rotation, then eliminates the selected nodal displacement DOFs +through MPC equations. Prescribing ``RigidDisp`` or ``RigidRot`` therefore +drives the whole selected node set as a rigid object. + +This constraint is useful to impose a strict rigid connection, for instance to +drive a face from a reference rigid point. It is more restrictive than +:py:class:`fedoo.constraint.MeanMotion`, because all selected nodes follow the +same rigid-body kinematics and local warping of the selected surface is +suppressed. .. autosummary:: :toctree: generated/ :template: custom-class-template.rst - fedoo.constraint.PeriodicBC fedoo.constraint.RigidTie fedoo.constraint.RigidTie2D +Mean motion control +~~~~~~~~~~~~~~~~~~~ + +The :py:class:`fedoo.constraint.MeanMotion` constraint is a less +restrictive alternative to :py:class:`fedoo.constraint.RigidTie`. It extracts +selected components of the best-fit motion of a node set or surface mesh in a +mean sense, without forcing every selected node to follow a rigid motion. The +selected surface can therefore deform locally while selected mean translation +and rotation components are controlled. + +The ``components`` argument is required and defines which global DOFs are +created. Accepted component names include ``"RotX"``, ``"MeanRotX"``, +``"DispZ"`` and ``"MeanDispZ"``. The vector aliases ``"Rot"``/``"MeanRot"`` +and ``"Disp"``/``"MeanDisp"`` select all rotation or displacement components +available in the current modeling dimension. In 2D, the rotation vector +contains only ``MeanRotZ``. + +When a surface mesh is passed, nodal weights are computed from the element +area, or from the element length in 2D. These weights define an area-weighted +least-squares projection of the displacement field onto rigid-body modes. + +For displacement-only mean control, select only displacement components: + +.. code-block:: python + + mean_disp = fd.constraint.MeanMotion(surface_mesh, components="DispZ") + pb.bc.add(mean_disp) + pb.bc.add("Dirichlet", "MeanDispZ", -delta) + +When ``finite_rotation`` is left to ``None`` (the default), rotational +components use finite rotations automatically if geometrical nonlinearity is +enabled on the problem. Otherwise, the rotation components are small-rotation +vector components. The finite-rotation mode uses a rotation-vector convention +and linearizes the constraint at each Newton iteration using the derivative of +the rotation matrix with respect to the rotation vector. + +.. code-block:: python + + face_motion = fd.constraint.MeanMotion( + surface_mesh, + components="RotZ", + ) + pb.bc.add(face_motion) + pb.bc.add("Dirichlet", face_motion.node_by_variable["MeanRotZ"], "MeanRotZ", angle) + +.. code-block:: python + + face_motion = fd.constraint.MeanMotion( + surface_mesh, + components=["RotZ", "DispZ"], + ) + pb.bc.add(face_motion) + pb.bc.add("Dirichlet", "MeanRotZ", angle) + pb.bc.add("Dirichlet", "MeanDispZ", -delta) + +The reactions on the created global DOFs are generalized forces. For instance, +the reaction associated with ``MeanRotZ`` is the conjugate moment around the +mean-motion center. + +.. autosummary:: + :toctree: generated/ + :template: custom-class-template.rst + + fedoo.constraint.MeanMotion + + +Periodic boundary conditions +____________________________ + +The :py:class:`fedoo.constraint.PeriodicBC` constraint creates MPC equations +between opposite boundaries so that their displacement fluctuations are +periodic. It is mainly used for representative volume elements and +homogenization problems, where the boundary kinematics must be compatible with +a prescribed macroscopic strain or displacement gradient. + +The constraint can build the required node pairings from opposite node sets +and then be added to ``pb.bc`` like the other MPC-based constraints. + +.. autosummary:: + :toctree: generated/ + :template: custom-class-template.rst + + fedoo.constraint.PeriodicBC + + Contact ============================== diff --git a/examples/01-simple/spherical_shell_compression.py b/examples/01-simple/spherical_shell_compression.py index 8799d2ce..126a4ea1 100644 --- a/examples/01-simple/spherical_shell_compression.py +++ b/examples/01-simple/spherical_shell_compression.py @@ -70,7 +70,7 @@ # assembly = solid_assembly -pb = fd.problem.NonLinear(assembly, nlgeom=False) +pb = fd.problem.Linear(assembly) pb.bc.add(pressure_assembly) nodes = mesh.nodes @@ -82,7 +82,7 @@ pb.bc.add("Dirichlet", node_b, ["DispY", "DispZ"], 0) pb.bc.add("Dirichlet", node_c, "DispZ", 0) -pb.nlsolve() +pb.solve() ############################################################################### # Extract the results: diff --git a/examples/02-constraints/mean_rigid_motion_torsion.py b/examples/02-constraints/mean_rigid_motion_torsion.py new file mode 100644 index 00000000..1267ce6e --- /dev/null +++ b/examples/02-constraints/mean_rigid_motion_torsion.py @@ -0,0 +1,167 @@ +""" +Mean motion torsion +~~~~~~~~~~~~~~~~~~~ + +Compare two ways of imposing a torsion on the right face of a cube: + +* ``RigidTie`` enforces a rigid motion of every node on the right face. +* ``MeanMotion`` prescribes only the best-fit mean rotation of the face, + so local warping of the right face remains possible. +""" + +import numpy as np +import pyvista as pv +import fedoo as fd + + +fd.ModelingSpace("3D") + +NLGEOM = "UL" +E = 200e3 +nu = 0.3 +L = 2.0 +ANGLE = np.pi / 3 # 0.10 +PRINT_INFO = 0 + + +def make_model(label): + mesh = fd.mesh.box_mesh( + nx=8, + ny=12, + nz=12, + x_min=0.0, + x_max=L, + y_min=-1.0, + y_max=1.0, + z_min=-1.0, + z_max=1.0, + elm_type="hex8", + name=f"torsion_{label}_mesh", + ) + + material = fd.constitutivelaw.ElasticIsotrop(E, nu, name=f"material_{label}") + wf = fd.weakform.StressEquilibrium(material, nlgeom=NLGEOM, name=f"wf_{label}") + assembly = fd.Assembly.create(wf, mesh, "hex8", name=f"assembly_{label}") + + pb = fd.problem.NonLinear(assembly, nlgeom=NLGEOM, name=f"problem_{label}") + # pb.set_nr_criterion("Displacement", err0=1, tol=1e-3, max_subiter=20) + return mesh, assembly, pb + + +def solve_rigid_tie_case(): + mesh, assembly, pb = make_model("rigid") + left = mesh.find_nodes("X", mesh.bounding_box.xmin) + right = mesh.find_nodes("X", mesh.bounding_box.xmax) + + pb.bc.add(fd.constraint.RigidTie(right)) + pb.bc.add("Dirichlet", left, "Disp", 0.0) + pb.bc.add("Dirichlet", "RigidRotX", ANGLE) + + pb.nlsolve(dt=0.1, tmax=1.0, update_dt=True, print_info=PRINT_INFO) + + moment = pb.get_ext_forces("RigidRotX")[0] + disp = pb.get_dof_solution("Disp") + return pb, assembly, moment + + +def solve_mean_motion_case(): + mesh, assembly, pb = make_model("mean") + left = mesh.find_nodes("X", mesh.bounding_box.xmin) + right = mesh.find_nodes("X", mesh.bounding_box.xmax) + right_surface = fd.mesh.extract_surface(mesh, node_set=right, reduce_order=False) + + mean_motion = fd.constraint.MeanMotion( + right_surface, + components=["Rot"], + ) + pb.bc.add(mean_motion) + pb.bc.add("Dirichlet", left, "Disp", 0.0) + pb.bc.add("Dirichlet", "MeanRotX", ANGLE) + # pb.bc.add("Dirichlet", ["MeanDispX", "MeanDispY", "MeanDispZ"], [0.4, -1, 0]) + + pb.nlsolve(dt=0.1, tmax=1.0, update_dt=True, print_info=PRINT_INFO) + + moment = pb.get_ext_forces("MeanRotX")[mean_motion.node_by_variable["MeanRotX"]] + disp = pb.get_dof_solution("Disp") + return pb, assembly, moment + + +rigid_pb, rigid_assembly, rigid_moment = solve_rigid_tie_case() +mean_pb, mean_assembly, mean_moment = solve_mean_motion_case() + + +############################################################################### +# Plot the two constraints side by side +# ------------------------------------- +# +# The deformed shapes are colored by the axial displacement ``DispX``. The +# rigid tie keeps the right face rigid, while the mean-motion constraint only +# enforces the best-fit rotation and lets the face warp locally. + +rigid_res = rigid_pb.get_results(rigid_assembly, ["Disp", "Stress"], "Node") +mean_res = mean_pb.get_results(mean_assembly, ["Disp", "Stress"], "Node") + +disp_x_rigid = rigid_res.get_data("Disp", component="X", data_type="Node") +disp_x_mean = mean_res.get_data("Disp", component="X", data_type="Node") +disp_x_lim = [ + min(np.min(disp_x_rigid), np.min(disp_x_mean)), + max(np.max(disp_x_rigid), np.max(disp_x_mean)), +] + +plotter = pv.Plotter(shape=(1, 2), window_size=(1400, 650)) + +plotter.subplot(0, 0) +rigid_res.plot( + "Disp", + component="X", + data_type="Node", + scale=1, + clim=disp_x_lim, + cmap="coolwarm", + show_edges=True, + show_scalar_bar=False, + lock_view=True, + plotter=plotter, + title="RigidTie", +) +plotter.hide_axes() +plotter.add_text( + "Rigid face kinematics\n" f"Mx = {rigid_moment:.3e}\n", + position="lower_left", + font_size=10, +) + +plotter.subplot(0, 1) +mean_res.plot( + "Disp", + component="X", + data_type="Node", + scale=1, + clim=disp_x_lim, + cmap="coolwarm", + show_edges=True, + show_scalar_bar=False, + lock_view=True, + plotter=plotter, + title="MeanMotion", +) +plotter.hide_axes() +plotter.add_text( + "Best-fit mean rotation\n" f"Mx = {mean_moment:.3e}\n", + position="lower_left", + font_size=10, +) +plotter.add_scalar_bar( + title="DispX", + vertical=True, + position_x=0.91, + position_y=0.18, + height=0.62, + width=0.04, + title_font_size=18, + label_font_size=14, +) + +plotter.link_views() +plotter.view_isometric() +plotter.show() diff --git a/fedoo/constraint/__init__.py b/fedoo/constraint/__init__.py index 92061ad8..b0034446 100644 --- a/fedoo/constraint/__init__.py +++ b/fedoo/constraint/__init__.py @@ -7,3 +7,4 @@ SurfaceForce, ) from .ipc_contact import IPCContact, IPCSelfContact +from .mean_motion import MeanMotion diff --git a/fedoo/constraint/mean_motion.py b/fedoo/constraint/mean_motion.py new file mode 100644 index 00000000..4e9df0e3 --- /dev/null +++ b/fedoo/constraint/mean_motion.py @@ -0,0 +1,722 @@ +"""Mean motion constraint.""" + +from numbers import Number + +import numpy as np +from scipy.spatial import cKDTree +from simcoon import Rotation as SimRotation +from simcoon import dR_drotvec + +from fedoo.core.boundary_conditions import BCBase, ListBC, MPC + + +_DISP_VECTOR = "Disp" +_MEAN_DISP_VECTOR = "MeanDisp" +_MEAN_ROT_VECTOR = "MeanRot" + + +class MeanMotion(BCBase): + """Constraint defining selected global DOFs equal to a mean motion. + + The constraint projects the displacement field of selected nodes onto the + rigid-body modes using a weighted least-squares fit. Selected fitted + translations are exposed through the global vector ``MeanDisp`` and + selected fitted rotations are exposed through the global vector ``MeanRot``. + Prescribing these global DOFs with standard Dirichlet boundary conditions + drives the selected face in a mean sense while still allowing local + deformation. + + Parameters + ---------- + node_set : str, int, array_like, Mesh or None, optional + Nodes used to compute the mean motion. If a mesh is given, its + elements are used to compute area/length nodal weights automatically. + If ``None`` all problem mesh nodes are used. + components : str or list[str] + Mean-motion components exposed as global DOFs. Accepted aliases include + ``"RotX"``, ``"MeanRotX"``, ``"DispZ"``, ``"MeanDispZ"``, ``"Rot"``, + ``"MeanRot"``, ``"Disp"`` and ``"MeanDisp"``. Vector aliases expand to + all components available in the current modeling dimension. + weights : array_like or None, optional + Nodal weights. By default, weights are area/length based when + ``node_set`` is a mesh, otherwise all selected nodes have equal weight. + center : int, array_like or None, optional + Rotation center. If ``None``, the weighted centroid of the selected + nodes is used. If an int is given, the corresponding problem mesh node + is used. + surface_mesh : Mesh or None, optional + Surface mesh used to define both selected nodes and area/length weights. + This is equivalent to passing the mesh as ``node_set``. + normalize_weights : bool, default=True + If True, normalize weights so that their sum is one. + finite_rotation : bool or None, default=None + If ``None``, finite rotation is enabled automatically when geometrical + nonlinearity is active on the problem and at least one rotation + component is selected. If False, rotations are small-rotation vector + components and the constraint is linear. If True, rotations are finite + rotation-vector components and the constraint is linearized at each + Newton iteration using ``simcoon.dR_drotvec``. + name : str, optional + Constraint name. + """ + + def __init__( + self, + node_set=None, + components=None, + weights=None, + center=None, + surface_mesh=None, + normalize_weights=True, + finite_rotation=None, + name="Mean motion", + ): + super().__init__(name) + self.bc_type = "MeanMotion" + if surface_mesh is None and _looks_like_mesh(node_set): + surface_mesh = node_set + node_set = None + self.node_set = node_set + self.components = components + self.weights = weights + self.center = center + self.surface_mesh = surface_mesh + self.normalize_weights = normalize_weights + self.finite_rotation = finite_rotation + + self.nodes = None + self._weights = None + self._disp_variables = None + self._mean_disp_variables = None + self._mean_rot_variables = None + self._full_mean_disp_variables = None + self._full_mean_rot_variables = None + self._full_mean_variables = None + self._mean_variables = None + self._mode_indices = None + self._node_by_variable = None + self._projection = None + self._mode_matrix = None + self._node_coords = None + self._phys_dof_index = None + self.node_disp = None + self.node_rot = None + + @property + def node_by_variable(self): + """Map global variable names to their local global-DOF index.""" + return self._node_by_variable + + def str_condensed(self): + if self.name == "": + return f"{self.bc_type} -> {', '.join(self._mean_variables or [])}" + return ( + f"{self.bc_type} (name = '{self.name}') -> " + f"{', '.join(self._mean_variables or [])}" + ) + + def initialize(self, problem): + self._disp_variables = self._get_disp_variables(problem) + n_dim = len(self._disp_variables) + + if n_dim == 2: + self._full_mean_disp_variables = [ + f"{_MEAN_DISP_VECTOR}X", + f"{_MEAN_DISP_VECTOR}Y", + ] + self._full_mean_rot_variables = [f"{_MEAN_ROT_VECTOR}Z"] + elif n_dim == 3: + self._full_mean_disp_variables = [ + f"{_MEAN_DISP_VECTOR}X", + f"{_MEAN_DISP_VECTOR}Y", + f"{_MEAN_DISP_VECTOR}Z", + ] + self._full_mean_rot_variables = [ + f"{_MEAN_ROT_VECTOR}X", + f"{_MEAN_ROT_VECTOR}Y", + f"{_MEAN_ROT_VECTOR}Z", + ] + else: + raise NotImplementedError("MeanMotion is implemented in 2D and 3D.") + + self.nodes, self._weights = self._get_nodes_and_weights(problem) + self.center = self._get_center(problem) + self._node_coords = problem.mesh.nodes[self.nodes, :n_dim] + self._projection, self._mode_matrix = self._build_projection(problem) + + self._full_mean_variables = ( + self._full_mean_disp_variables + self._full_mean_rot_variables + ) + normalized = self._normalize_components(n_dim) + self._mean_disp_variables = [ + var for var in normalized if var in self._full_mean_disp_variables + ] + self._mean_rot_variables = [ + var for var in normalized if var in self._full_mean_rot_variables + ] + self._mean_variables = self._mean_disp_variables + self._mean_rot_variables + self._mode_indices = [ + self._full_mean_variables.index(var) for var in self._mean_variables + ] + + n_nodes = problem.mesh.n_nodes + self._phys_dof_index = np.array( + [ + problem.space.variable_rank(var) * n_nodes + node + for node in self.nodes + for var in self._disp_variables + ], + dtype=int, + ) + + has_rotation = bool(self._mean_rot_variables) + if self.finite_rotation is None: + self.finite_rotation = bool(problem.nlgeom and has_rotation) + if self.finite_rotation: + self._keep_at_end = True + # finite rotation is nonlinear: the MPCs must be relinearized every + # Newton iteration, independently of problem.nlgeom (a user may force + # finite_rotation=True on a geometrically linear problem). + self._update_during_inc = True + + self._node_by_variable = {} + if self._mean_disp_variables: + self.node_disp = problem.add_global_dof( + self._mean_disp_variables, 1, vector_name=_MEAN_DISP_VECTOR + ) + self._node_by_variable.update( + {var: self.node_disp for var in self._mean_disp_variables} + ) + if self._mean_rot_variables: + self.node_rot = problem.add_global_dof( + self._mean_rot_variables, 1, vector_name=_MEAN_ROT_VECTOR + ) + self._node_by_variable.update( + {var: self.node_rot for var in self._mean_rot_variables} + ) + + def generate(self, problem, t_fact=1, t_fact_old=None): + res = ListBC() + if self.finite_rotation: + self._project_finite_dirichlet_mean_motion(problem, t_fact, t_fact_old) + u0 = self._get_total_physical_values(problem) + q0 = self._get_full_total_mean_values( + problem, u0, controlled=self._finite_dirichlet_mask(problem) + ) + coeff_u, coeff_q, residual = self._build_finite_incremental_linearization( + problem, q0, u0 + ) + # Track the constraint out-of-balance so the NR loop does not report + # convergence while the mean-motion fit is still unsatisfied. + problem._bc_residual_norm = max( + getattr(problem, "_bc_residual_norm", 0.0), + float(np.linalg.norm(residual)), + ) + used_slave_cols = set() + for mode_index, mean_var in enumerate(self._mean_variables): + res.append( + self._make_linearized_mpc( + coeff_u[mode_index], + coeff_q[mode_index], + residual[mode_index], + mean_var, + used_slave_cols, + ) + ) + else: + used_slave_cols = set() + for mode_index, mean_var in enumerate(self._mean_variables): + res.append( + self._make_mpc( + self._mode_indices[mode_index], mean_var, used_slave_cols + ) + ) + + res.initialize(problem) + return res.generate(problem, t_fact, t_fact_old) + + def _normalize_components(self, n_dim): + if self.components is None: + raise ValueError("MeanMotion requires an explicit components argument.") + + if isinstance(self.components, str): + components = [self.components] + else: + components = list(self.components) + + if len(components) == 0: + raise ValueError("MeanMotion requires at least one component.") + + disp_suffixes = ["X", "Y", "Z"][:n_dim] + rot_suffixes = ["Z"] if n_dim == 2 else ["X", "Y", "Z"] + allowed = set(self._full_mean_variables) + + normalized = [] + for component in components: + for variable in self._expand_component_alias( + component, disp_suffixes, rot_suffixes + ): + if variable not in allowed: + raise ValueError( + f"Unknown MeanMotion component '{component}' for " + f"{n_dim}D problem." + ) + if variable not in normalized: + normalized.append(variable) + + return normalized + + def _expand_component_alias(self, component, disp_suffixes, rot_suffixes): + vector_aliases = { + "Disp": self._full_mean_disp_variables, + "MeanDisp": self._full_mean_disp_variables, + "Rot": self._full_mean_rot_variables, + "MeanRot": self._full_mean_rot_variables, + } + if component in vector_aliases: + return list(vector_aliases[component]) + + for prefix, vector_name, suffixes in [ + ("Disp", _MEAN_DISP_VECTOR, disp_suffixes), + ("MeanDisp", _MEAN_DISP_VECTOR, disp_suffixes), + ("Rot", _MEAN_ROT_VECTOR, rot_suffixes), + ("MeanRot", _MEAN_ROT_VECTOR, rot_suffixes), + ]: + if component.startswith(prefix): + suffix = component[len(prefix) :] + if suffix in suffixes: + return [f"{vector_name}{suffix}"] + + return [component] + + def _get_nodes_and_weights(self, problem): + if self.surface_mesh is not None: + nodes, weights = self._get_surface_nodes_and_weights(problem) + elif self.node_set is None: + nodes = np.arange(problem.mesh.n_nodes) + weights = self._get_explicit_or_uniform_weights(len(nodes)) + elif isinstance(self.node_set, str): + nodes = np.asarray(problem.mesh.node_sets[self.node_set], dtype=int).ravel() + weights = self._get_explicit_or_uniform_weights(len(nodes)) + elif isinstance(self.node_set, Number): + nodes = np.array([self.node_set], dtype=int) + weights = self._get_explicit_or_uniform_weights(len(nodes)) + else: + nodes = np.asarray(self.node_set, dtype=int).ravel() + weights = self._get_explicit_or_uniform_weights(len(nodes)) + + if len(nodes) == 0: + raise ValueError("MeanMotion requires at least one node.") + if not np.any(np.abs(weights) > 0): + raise ValueError("At least one mean-rigid-motion weight must be non-zero.") + + if self.normalize_weights: + weights_sum = np.sum(weights) + if weights_sum == 0: + raise ValueError("Cannot normalize weights with a zero sum.") + weights = weights / weights_sum + + return nodes, weights + + def _get_explicit_or_uniform_weights(self, n_nodes): + if self.weights is None: + return np.ones(n_nodes, dtype=float) + + weights = np.asarray(self.weights, dtype=float).ravel() + if len(weights) != n_nodes: + raise ValueError("weights must have the same length as the selected nodes.") + return weights + + def _get_surface_nodes_and_weights(self, problem): + if self.weights is not None: + raise ValueError( + "weights should not be given when surface_mesh is used. Pass a " + "node_set instead to provide explicit nodal weights." + ) + + surface_nodes = self._map_surface_nodes_to_problem(problem) + elements = surface_nodes[np.asarray(self.surface_mesh.elements, dtype=int)] + measures = self.surface_mesh.get_element_volumes() + + nodes_per_elm = elements.shape[1] + weights = np.zeros(problem.mesh.n_nodes) + np.add.at( + weights, + elements.ravel(), + np.repeat(measures / nodes_per_elm, nodes_per_elm), + ) + + nodes = np.flatnonzero(weights) + return nodes.astype(int), weights[nodes] + + def _map_surface_nodes_to_problem(self, problem): + surface_nodes = np.asarray(self.surface_mesh.nodes) + if surface_nodes.shape == problem.mesh.nodes.shape and np.allclose( + surface_nodes, problem.mesh.nodes + ): + return np.arange(problem.mesh.n_nodes) + + distances, mapped_nodes = cKDTree(problem.mesh.nodes).query(surface_nodes) + if not np.allclose(distances, 0.0): + raise ValueError( + "surface_mesh nodes could not be mapped to the problem mesh. " + "Use a surface mesh extracted from the problem mesh or pass " + "node_set and weights explicitly." + ) + return mapped_nodes.astype(int) + + def _get_center(self, problem): + if self.center is None: + return np.average( + problem.mesh.nodes[self.nodes], axis=0, weights=self._weights + ) + if np.isscalar(self.center): + return np.asarray(problem.mesh.nodes[int(self.center)]) + return np.asarray(self.center, dtype=float) + + def _build_projection(self, problem): + n_dim = len(self._disp_variables) + n_modes = n_dim + (1 if n_dim == 2 else 3) + n_phys = len(self.nodes) * n_dim + weighted_modes = np.zeros((n_modes, n_modes)) + rhs = np.zeros((n_modes, n_phys)) + mode_matrix = np.zeros((n_phys, n_modes)) + + coords = problem.mesh.nodes[self.nodes, :n_dim] - self.center[:n_dim] + for i, (coord, weight) in enumerate(zip(coords, self._weights)): + b = _rigid_motion_block(coord, n_dim) + row = slice(i * n_dim, (i + 1) * n_dim) + mode_matrix[row, :] = b + weighted_modes += weight * b.T @ b + rhs[:, row] = weight * b.T + + rank = np.linalg.matrix_rank(weighted_modes) + if rank < n_modes: + raise ValueError( + "Selected nodes cannot define all requested mean-motion modes. " + f"Rank is {rank}, expected {n_modes}." + ) + + return np.linalg.solve(weighted_modes, rhs), mode_matrix + + def _get_current_mean_values(self, problem, t_fact, t_fact_old): + sol = problem.get_dof_solution() + if np.isscalar(sol) and sol == 0: + values = np.zeros(len(self._mean_variables)) + else: + values = np.array( + [ + sol[self._global_dof_index(problem, var)] + for var in self._mean_variables + ] + ) + + if not (np.isscalar(problem._Xbc) and problem._Xbc == 0): + values += np.array( + [ + problem._Xbc[self._global_dof_index(problem, var)] + for var in self._mean_variables + ] + ) + + return values + + def _get_full_total_mean_values(self, problem, u0, controlled=None): + q = self._fit_finite_mean_motion(problem, u0) + if not self._mean_variables: + return q + + if controlled is None: + controlled = np.ones(len(self._mean_variables), dtype=bool) + elif not np.any(controlled): + return q + + sol = problem.get_dof_solution() + if np.isscalar(sol) and sol == 0: + selected = np.zeros(len(self._mean_variables)) + else: + selected = np.array( + [ + sol[self._global_dof_index(problem, var)] + for var in self._mean_variables + ] + ) + + if not (np.isscalar(problem._Xbc) and problem._Xbc == 0): + selected += np.array( + [ + problem._Xbc[self._global_dof_index(problem, var)] + for var in self._mean_variables + ] + ) + + for selected_index, is_controlled in enumerate(controlled): + if is_controlled: + q[self._mode_indices[selected_index]] = selected[selected_index] + return q + + def _get_total_physical_values(self, problem): + sol = problem.get_dof_solution() + if np.isscalar(sol) and sol == 0: + return np.zeros(len(self._phys_dof_index)) + return np.asarray(sol)[self._phys_dof_index] + + def _global_dof_index(self, problem, variable): + return ( + problem.n_node_dof + + problem.global_dof.indice_start(variable) + + self._node_by_variable[variable] + ) + + def _build_finite_incremental_linearization(self, problem, q0, u0): + n_dim = len(self._disp_variables) + j_mat, pred = self._finite_tangent_and_prediction(problem, q0) + weights = np.repeat(self._weights, n_dim) + residual_full = (j_mat.T * weights) @ (pred - u0) + coeff_u_full = -(j_mat.T * weights) + coeff_q_full = (j_mat.T * weights) @ j_mat + + return ( + coeff_u_full[self._mode_indices], + coeff_q_full[np.ix_(self._mode_indices, self._mode_indices)], + residual_full[self._mode_indices], + ) + + def _project_finite_dirichlet_mean_motion(self, problem, t_fact, t_fact_old): + if ( + not hasattr(problem, "_dU") + or np.isscalar(problem._dU) + or np.array_equal(problem._dU, 0) + ): + return + + controlled = self._finite_dirichlet_mask(problem) + if not np.any(controlled): + return + + u0 = self._get_total_physical_values(problem) + q_fit = self._fit_finite_mean_motion(problem, u0) + q_target = q_fit.copy() + prescribed = self._get_current_mean_values(problem, t_fact, t_fact_old) + for selected_index, is_controlled in enumerate(controlled): + if is_controlled: + q_target[self._mode_indices[selected_index]] = prescribed[ + selected_index + ] + + correction = self._finite_rigid_displacement(problem, q_target) + correction -= self._finite_rigid_displacement(problem, q_fit) + problem._dU[self._phys_dof_index] += correction + + def _finite_dirichlet_mask(self, problem): + controlled = np.zeros(len(self._mean_variables), dtype=bool) + + for bc in problem.bc.list_all(): + if getattr(bc, "bc_type", None) != "Dirichlet": + continue + variable = getattr(bc, "variable_name", None) + if variable not in self._mean_variables: + continue + controlled[self._mean_variables.index(variable)] = True + + return controlled + + def _fit_finite_mean_motion(self, problem, u0): + n_dim = len(self._disp_variables) + initial = problem.mesh.nodes[self.nodes, :n_dim] + current = initial + u0.reshape(len(self.nodes), n_dim) + + mean_initial = np.average(initial, axis=0, weights=self._weights) + mean_current = np.average(current, axis=0, weights=self._weights) + initial_centered = initial - mean_initial + current_centered = current - mean_current + + if n_dim == 2: + cov = (initial_centered * self._weights[:, None]).T @ current_centered + rotation_angle = np.arctan2(cov[0, 1] - cov[1, 0], cov[0, 0] + cov[1, 1]) + cos_angle = np.cos(rotation_angle) + sin_angle = np.sin(rotation_angle) + rotation = np.array([[cos_angle, -sin_angle], [sin_angle, cos_angle]]) + trans = ( + mean_current + - self.center[:2] + - rotation @ (mean_initial - self.center[:2]) + ) + return np.array([trans[0], trans[1], rotation_angle]) + + cov = (initial_centered * self._weights[:, None]).T @ current_centered + u_svd, _, vt = np.linalg.svd(cov) + rotation = vt.T @ u_svd.T + if np.linalg.det(rotation) < 0: + vt[-1] *= -1 + rotation = vt.T @ u_svd.T + + trans = ( + mean_current - self.center[:3] - rotation @ (mean_initial - self.center[:3]) + ) + rotvec = SimRotation.from_matrix(rotation).as_rotvec() + return np.r_[trans, rotvec] + + def _finite_rigid_displacement(self, problem, q): + _, pred = self._finite_tangent_and_prediction(problem, q) + return pred + + def _build_finite_linearization(self, problem, q0, u0): + j_mat, pred = self._finite_tangent_and_prediction(problem, q0) + return self._build_finite_linearization_from_tangent(j_mat, pred, q0, u0) + + def _build_finite_linearization_from_tangent(self, j_mat, pred, q0, u0): + n_dim = len(self._disp_variables) + weights = np.repeat(self._weights, n_dim) + residual = pred - u0 + + coeff_u = -(j_mat.T * weights) + coeff_q = (j_mat.T * weights) @ j_mat + h0 = (j_mat.T * weights) @ residual + constants = h0 - coeff_u @ u0 - coeff_q @ q0 + return coeff_u, coeff_q, constants + + def _finite_tangent_and_prediction(self, problem, q): + # rotation and its derivative are node-independent, so the prediction + # and Jacobian use broadcasted ops (called every Newton iteration). + n_dim = len(self._disp_variables) + n_modes = len(self._full_mean_variables) + n_phys = len(self.nodes) * n_dim + j_mat = np.zeros((n_phys, n_modes)) + + if n_dim == 2: + trans = q[:2] + rotvec = np.array([0.0, 0.0, q[2]]) + rotation = SimRotation.from_rotvec(rotvec).as_matrix()[:2, :2] + drot = dR_drotvec(rotvec)[:2, :2, 2] + coords = problem.mesh.nodes[self.nodes, :2] - self.center[:2] + pred = (trans + coords @ rotation.T - coords).ravel() + j_mat[:, :2] = np.tile(np.eye(2), (len(coords), 1)) + j_mat[:, 2] = (coords @ drot.T).ravel() + return j_mat, pred + + trans = q[:3] + rotvec = q[3:] + rotation = SimRotation.from_rotvec(rotvec).as_matrix() + drot = dR_drotvec(rotvec) + coords = problem.mesh.nodes[self.nodes, :3] - self.center[:3] + pred = (trans + coords @ rotation.T - coords).ravel() + j_mat[:, :3] = np.tile(np.eye(3), (len(coords), 1)) + j_mat[:, 3:] = np.einsum("klr,ml->mkr", drot, coords).reshape(n_phys, 3) + return j_mat, pred + + def _get_disp_variables(self, problem): + if _DISP_VECTOR in problem.space.list_vectors(): + return [ + problem.space.variable_name(var_rank) + for var_rank in problem.space.get_rank_vector(_DISP_VECTOR) + ] + + if _DISP_VECTOR in problem.space.list_variables(): + return [_DISP_VECTOR] + + raise ValueError(f"Variable or vector '{_DISP_VECTOR}' doesn't exist.") + + def _make_mpc(self, row_index, mean_var, used_slave_cols=None): + # ``row_index`` indexes ``_projection`` in the full rigid-body-mode + # order; callers map a selected-subset index through ``_mode_indices``. + if used_slave_cols is None: + used_slave_cols = set() + coeffs = self._projection[row_index] + slave_col = self._select_physical_slave(coeffs, used_slave_cols, mean_var) + used_slave_cols.add(slave_col) + if coeffs[slave_col] == 0: + raise ValueError(f"Mean-motion mode '{mean_var}' has no physical support.") + + n_dim = len(self._disp_variables) + slave_node_index, slave_var_index = divmod(slave_col, n_dim) + node_sets = [ + [int(self.nodes[slave_node_index])], + [self._node_by_variable[mean_var]], + ] + variables = [self._disp_variables[slave_var_index], mean_var] + factors = [[-float(coeffs[slave_col])], [1.0]] + + for col, coeff in enumerate(coeffs): + if col == slave_col or coeff == 0: + continue + node_index, var_index = divmod(col, n_dim) + node_sets.append([int(self.nodes[node_index])]) + variables.append(self._disp_variables[var_index]) + factors.append([-float(coeff)]) + + return MPC(node_sets, variables, factors) + + def _make_linearized_mpc( + self, coeff_u, coeff_q, constant, mean_var, used_slave_cols + ): + slave_col = self._select_physical_slave(coeff_u, used_slave_cols, mean_var) + used_slave_cols.add(slave_col) + + n_dim = len(self._disp_variables) + slave_node_index, slave_var_index = divmod(slave_col, n_dim) + node_sets = [[int(self.nodes[slave_node_index])]] + variables = [self._disp_variables[slave_var_index]] + factors = [float(coeff_u[slave_col])] + + for i, coeff in enumerate(coeff_q): + if coeff == 0: + continue + node_sets.append([self._node_by_variable[self._mean_variables[i]]]) + variables.append(self._mean_variables[i]) + factors.append(float(coeff)) + + for col, coeff in enumerate(coeff_u): + if col == slave_col or coeff == 0: + continue + node_index, var_index = divmod(col, n_dim) + node_sets.append([int(self.nodes[node_index])]) + variables.append(self._disp_variables[var_index]) + factors.append(float(coeff)) + + return MPC(node_sets, variables, factors, constant=float(constant)) + + def _select_physical_slave(self, coeff_u, used_slave_cols, mean_var): + abs_coeff = np.abs(coeff_u) + available = np.ones(len(abs_coeff), dtype=bool) + if used_slave_cols: + available[list(used_slave_cols)] = False + available &= abs_coeff > 0 + + if np.any(available): + max_coeff = np.max(abs_coeff[available]) + candidate_cols = np.flatnonzero( + available & np.isclose(abs_coeff, max_coeff) + ) + n_dim = len(self._disp_variables) + candidate_node_indices = candidate_cols // n_dim + distances = np.linalg.norm( + self._node_coords[candidate_node_indices] - self.center[:n_dim], + axis=1, + ) + return int(candidate_cols[np.argmin(distances)]) + + raise ValueError(f"Mean-motion mode '{mean_var}' has no physical support.") + + +def _looks_like_mesh(value): + return ( + hasattr(value, "nodes") + and hasattr(value, "elements") + and hasattr(value, "elm_type") + ) + + +def _rigid_motion_block(coord, n_dim): + if n_dim == 2: + x, y = coord + return np.array([[1.0, 0.0, -y], [0.0, 1.0, x]]) + + x, y, z = coord + return np.array( + [ + [1.0, 0.0, 0.0, 0.0, z, -y], + [0.0, 1.0, 0.0, -z, 0.0, x], + [0.0, 0.0, 1.0, y, -x, 0.0], + ] + ) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 9e42dd3d..14658604 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -4,9 +4,8 @@ import numpy as np from fedoo.core.boundary_conditions import BCBase, MPC, ListBC - - -from scipy.spatial.transform import Rotation +from simcoon import Rotation +from simcoon import dR_drotvec class RigidTie(BCBase): @@ -46,15 +45,9 @@ class RigidTie(BCBase): Definition of rotations ----------------------- - A convention needs to be defined the orders of rotations. - The convention used in this class is: First rotation around X, then - rotation around Y' (Y' being the new Y afte r rotation around X) - and finaly the rotation arould Z" (Z" beging the new Z after the 2 first - rotations). - - We can note that this convention can also be interpreted using global axis - not attached to the solid by applying first the rotation around Z, then the - rotation around Y and finally, the rotation around X. + ``RigidRotX``, ``RigidRotY`` and ``RigidRotZ`` use a rotation-vector + convention. The vector direction defines the rotation axis and its norm is + the rotation angle. Notes @@ -173,16 +166,9 @@ def generate(self, problem, t_fact=1, t_fact_old=None): ) disp_ref = dof_ref[:3] # reference displacement - angles = dof_ref[3:] # rotation angle + rotvec = dof_ref[3:] - sin = np.sin(angles) - cos = np.cos(angles) - - R = Rotation.from_euler("XYZ", angles).as_matrix() - # #or - # R = np.array([[cos[1]*cos[2], -cos[1]*sin[2], sin[1]], - # [cos[0]*sin[2] + cos[2]*sin[0]*sin[1], cos[0]*cos[2]-sin[0]*sin[1]*sin[2], -cos[1]*sin[0]], - # [sin[0]*sin[2] - cos[0]*cos[2]*sin[1], cos[2]*sin[0]+cos[0]*sin[1]*sin[2], cos[0]*cos[1]]] ) + R = Rotation.from_rotvec(rotvec).as_matrix() # Correct displacement of slave nodes to be consistent with the master nodes new_disp = ( @@ -201,60 +187,11 @@ def generate(self, problem, t_fact=1, t_fact_old=None): ) # approche incrémentale: - dR_drx = np.array( - [ - [0, 0, 0], - [ - -sin[0] * sin[2] + cos[2] * cos[0] * sin[1], - -sin[0] * cos[2] - cos[0] * sin[1] * sin[2], - -cos[1] * cos[0], - ], - [ - cos[0] * sin[2] + sin[0] * cos[2] * sin[1], - cos[2] * cos[0] - sin[0] * sin[1] * sin[2], - -sin[0] * cos[1], - ], - ] - ) - - dR_dry = np.array( - [ - [-sin[1] * cos[2], +sin[1] * sin[2], cos[1]], - [ - cos[2] * sin[0] * cos[1], - -sin[0] * cos[1] * sin[2], - sin[1] * sin[0], - ], - [ - -cos[0] * cos[2] * cos[1], - cos[0] * cos[1] * sin[2], - -cos[0] * sin[1], - ], - ] - ) - - dR_drz = np.array( - [ - [-cos[1] * sin[2], -cos[1] * cos[2], 0], - [ - cos[0] * cos[2] - sin[2] * sin[0] * sin[1], - -cos[0] * sin[2] - sin[0] * sin[1] * cos[2], - 0, - ], - [ - sin[0] * cos[2] + cos[0] * sin[2] * sin[1], - -sin[2] * sin[0] + cos[0] * sin[1] * cos[2], - 0, - ], - ] - ) - + dR = dR_drotvec(rotvec) crd = mesh.nodes[list_nodes] - self.center - du_drx = crd @ dR_drx.T - du_dry = crd @ dR_dry.T - du_drz = ( - crd @ dR_drz.T - ) # shape = (nnodes, nvar) with nvar = 3 in 3d (ux, uy, uz) + du_drx = crd @ dR[:, :, 0].T + du_dry = crd @ dR[:, :, 1].T + du_drz = crd @ dR[:, :, 2].T #### MPC #### diff --git a/fedoo/core/boundary_conditions.py b/fedoo/core/boundary_conditions.py index db9189b8..f1ddc4b4 100644 --- a/fedoo/core/boundary_conditions.py +++ b/fedoo/core/boundary_conditions.py @@ -199,10 +199,7 @@ def add(self, *args, **kargs): return bc else: # define a boundary condition type_bc = args[0] - if len(args) == 3 and ( - args[1] in self._problem._global_dof._variable - or args[1] in self._problem._global_dof._vector - ): + if len(args) == 3 and self._is_global_dof_variable_or_vector(args[1]): node_set = [0] variable = args[1] value = args[2] @@ -227,6 +224,18 @@ def add(self, *args, **kargs): self.append(bc) return bc + def _is_global_dof_variable_or_vector(self, variable): + if isinstance(variable, str): + return ( + variable in self._problem._global_dof._variable + or variable in self._problem._global_dof._vector + ) + + if isinstance(variable, (list, tuple)): + return all(self._is_global_dof_variable_or_vector(var) for var in variable) + + return False + def _extract_vartiables_from_vector(self, variable): # return a list in any cases if variable in self._problem.space.list_vectors(): @@ -643,8 +652,7 @@ def time_func(t_fact): self.time_func = time_func - # can be a float or an array or None ! if DefaultInitialvalue is None, start_value can be modified by the Problem - # self._start_constant_default = self.start_constant = start_constant + self.start_constant = start_constant self.list_variables = list_variables self.list_factors = list_factors @@ -696,7 +704,7 @@ def generate(self, problem, t_fact, t_fact_old=None): factor = BoundaryCondition._get_factor(self, t_fact, t_fact_old) if factor == 0: self._current_value = 0 - elif self.start_value is None: + elif self.start_constant is None: self._current_value = factor * value else: # in case there is an initial value start_value = -self.start_constant / self.list_factors[0] diff --git a/fedoo/core/problem.py b/fedoo/core/problem.py index e4500e57..81602c35 100644 --- a/fedoo/core/problem.py +++ b/fedoo/core/problem.py @@ -344,6 +344,7 @@ def set_dof_solution(self, name, value): def apply_boundary_conditions(self, t_fact=1, t_fact_old=None): n_dof = self.n_dof self._Xbc = np.zeros(n_dof) + self._bc_residual_norm = 0.0 F = np.zeros(n_dof) build_mpc = False diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index 97c7cb33..fe3e554f 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -887,6 +887,10 @@ def solve_time_increment(self, max_subiter=None, tol_nr=None): error < tol_nr and subiter >= self._nr_min_subiter and self._boundary_is_0 + # constraints such as MeanMotion publish the out-of-balance on + # their own nonlinear equations here; gate convergence on it so + # an unsatisfied constraint cannot report a converged increment. + and getattr(self, "_bc_residual_norm", 0.0) < 10 * tol_nr ): self._t_fact_inc = None return 1, subiter, error @@ -1186,21 +1190,20 @@ def nlsolve( ) if update_dt: dt *= 0.25 - # if self.print_info > 0: - # print( - # "NR failed to converge (err: {:.5f}) - reduce the time increment to {:.5f}".format( - # error, dt - # ) - # ) if dt < dt_min: + # roll back to the last converged increment so accessors + # return a clean state after the abort + self.to_start() raise RuntimeError( "Current time step is inferior to the specified minimal time step (dt_min)" ) + # Roll back the failed increment (to_start at the top of the loop) restart = True continue else: + self.to_start() raise RuntimeError( "Newton Raphson iteration has not converged - Reduce the time step or use update_dt = True" ) diff --git a/tests/test_mean_motion_regression.py b/tests/test_mean_motion_regression.py new file mode 100644 index 00000000..35907a44 --- /dev/null +++ b/tests/test_mean_motion_regression.py @@ -0,0 +1,78 @@ +"""Regression guards for MeanMotion bugs. + +These complement the main ``test_mean_motion.py`` suite. They lock in the +linear (small-rotation) path's projection-row bug: a component selection that +is not the leading rigid-body modes in natural order (e.g. ``components="DispZ"`` +or a reordered list) must still drive the *selected* mean components — not the +wrong projection row. Both cases run a full ``nlsolve`` and check the enforced +mean of the physical field, which the wrong-row bug silently violated. +""" + +import numpy as np + +import fedoo as fd + + +def _box(label): + """Small clamped-left cube with a MeanMotion-ready right surface.""" + fd.ModelingSpace("3D") + mesh = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=3, + x_min=0.0, + x_max=2.0, + y_min=-1.0, + y_max=1.0, + z_min=-1.0, + z_max=1.0, + elm_type="hex8", + name=f"mmreg_{label}_mesh", + ) + mat = fd.constitutivelaw.ElasticIsotrop(200e3, 0.3, name=f"mmreg_{label}_mat") + wf = fd.weakform.StressEquilibrium(mat, name=f"mmreg_{label}_wf") + assemb = fd.Assembly.create(wf, mesh, "hex8", name=f"mmreg_{label}_asm") + pb = fd.problem.NonLinear(assemb, name=f"mmreg_{label}_pb") + + left = mesh.find_nodes("X", mesh.bounding_box.xmin) + right = mesh.find_nodes("X", mesh.bounding_box.xmax) + surf = fd.mesh.extract_surface(mesh, node_set=right, reduce_order=False) + return mesh, pb, left, surf + + +def _face_disp(pb, nodes): + """(n_face, 3) displacement of the selected nodes.""" + return pb.get_dof_solution("Disp").T[nodes] + + +def test_mean_disp_single_non_leading_component_enforced(): + # components="DispZ": selected subset index 0 maps to full-mode index 2. + # The wrong-row bug tied MeanDispZ to the DispX fit instead. + mesh, pb, left, surf = _box("dispz") + mm = fd.constraint.MeanMotion(surf, components="DispZ") + pb.bc.add(mm) + pb.bc.add("Dirichlet", left, "Disp", 0.0) + target = -0.30 + pb.bc.add("Dirichlet", "MeanDispZ", target) + pb.nlsolve(dt=1.0, tmax=1.0, print_info=0) + + u = _face_disp(pb, mm.nodes) + assert abs(np.average(u[:, 2], weights=mm._weights) - target) < 1e-8 + # the unselected x/y means stay free (not accidentally pinned) + assert abs(np.average(u[:, 0], weights=mm._weights)) < 1e-6 + + +def test_mean_disp_reordered_multi_component_enforced(): + # Reordered selection -> _mode_indices = [2, 0]; each selected mean must + # reach its own target, confirming the subset->full-mode row mapping. + mesh, pb, left, surf = _box("dispzx") + mm = fd.constraint.MeanMotion(surf, components=["DispZ", "DispX"]) + pb.bc.add(mm) + pb.bc.add("Dirichlet", left, "Disp", 0.0) + pb.bc.add("Dirichlet", "MeanDispZ", -0.20) + pb.bc.add("Dirichlet", "MeanDispX", 0.10) + pb.nlsolve(dt=1.0, tmax=1.0, print_info=0) + + u = _face_disp(pb, mm.nodes) + assert abs(np.average(u[:, 2], weights=mm._weights) - (-0.20)) < 1e-8 + assert abs(np.average(u[:, 0], weights=mm._weights) - 0.10) < 1e-8 diff --git a/tests/test_rigid_tie_rotvec.py b/tests/test_rigid_tie_rotvec.py new file mode 100644 index 00000000..795225ba --- /dev/null +++ b/tests/test_rigid_tie_rotvec.py @@ -0,0 +1,40 @@ +import numpy as np +from scipy import sparse +from simcoon import Rotation + +import fedoo as fd + + +def test_rigid_tie_rotvec_matches_single_component_x_torsion(): + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + + nodes = np.array( + [ + [0.0, -1.0, -1.0], + [0.0, 1.0, -1.0], + [0.0, 1.0, 1.0], + [0.0, -1.0, 1.0], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + pb = fd.Problem(A=sparse.eye(mesh.n_nodes * space.nvar), mesh=mesh, space=space) + + tied_nodes = np.arange(mesh.n_nodes) + control = fd.constraint.RigidTie(tied_nodes) + pb.bc.add(control) + angle = 0.8 + pb.bc.add("Dirichlet", "RigidRotX", angle) + pb._U = np.zeros(pb.n_dof) + pb._dU = np.zeros(pb.n_dof) + + pb.apply_boundary_conditions() + + center = 0.5 * (nodes.min(axis=0) + nodes.max(axis=0)) + rotation = Rotation.from_rotvec([angle, 0.0, 0.0]).as_matrix() + expected = (nodes - center) @ rotation.T + center - nodes + + np.testing.assert_allclose(pb._dU[control._disp_indices], expected, atol=1e-12)