From fdfe0ccb9244e164169f03fd08db2d4e9268db43 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Sat, 28 Mar 2026 23:37:33 +0100 Subject: [PATCH 01/21] Add rigid-body dynamics, IPC integration, and examples Introduce full rigid-body support and example simulations. Added a new rigid_body module (RigidBody, RigidBodyAssembly) implementing 6-DOF dynamics, quaternion-based large-rotation updates, direct 6x6 Newmark solver and IPC contact handling (J^T@F projection). Extended IPCContact to support rigid bodies: add_rigid_body, scatter-matrix mapping of surface DOFs to global rigid DOFs using exact rotation derivatives, runtime rebuild of the scatter matrix, and various checks/adjustments for gradients, padding and CCD/OGC incompatibilities. Export RigidBody from fedoo.constraint. Add example scripts demonstrating free-fall and bouncing rigid bodies (sphere, bunny, cow) with PyVista animations and plots. Removed a macOS .DS_Store file. These changes enable mixed rigid/deformable IPC contact and fast standalone rigid-body integration for validation and demos. --- examples/rigid_body_bounce_ipc.py | 181 +++++++ examples/rigid_body_bunny_bounce.py | 179 +++++++ examples/rigid_body_cow_bounce.py | 177 +++++++ examples/rigid_body_freefall.py | 167 ++++++ fedoo/.DS_Store | Bin 6148 -> 0 bytes fedoo/constraint/__init__.py | 1 + fedoo/constraint/ipc_contact.py | 204 ++++++- fedoo/constraint/rigid_body.py | 794 ++++++++++++++++++++++++++++ fedoo/constraint/rigid_tie.py | 255 ++++++--- fedoo/problem/nl_newmark.py | 131 ++--- fedoo/problem/non_linear.py | 18 + 11 files changed, 1945 insertions(+), 162 deletions(-) create mode 100644 examples/rigid_body_bounce_ipc.py create mode 100644 examples/rigid_body_bunny_bounce.py create mode 100644 examples/rigid_body_cow_bounce.py create mode 100644 examples/rigid_body_freefall.py delete mode 100644 fedoo/.DS_Store create mode 100644 fedoo/constraint/rigid_body.py diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py new file mode 100644 index 00000000..36cf7a02 --- /dev/null +++ b/examples/rigid_body_bounce_ipc.py @@ -0,0 +1,181 @@ +"""Rigid body bouncing on a plane — 6x6 solver + IPC contact + damping. + +Uses RigidBody.solve() for direct 6-DOF Newmark integration. +Comparison with analytical elastic bounce. +""" + +import sys +import time +import numpy as np +import pyvista as pv + +sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") +import fedoo as fd + +g = 9.81 +mass = 1.0 +radius = 0.1 +z0 = 0.5 +dt = 5e-4 +t_end = 2.0 + +print("=" * 60) +print("SPHERE BOUNCE — 6x6 Newmark + IPC + Rayleigh damping") +print(f" m={mass}kg, z0={z0}m, r={radius}m, dt={dt}s") +print("=" * 60) + +# Modeling space +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Meshes +pv_ball = pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=10, phi_resolution=10 +) +ball_mesh = fd.Mesh.from_pyvista(pv_ball) +pv_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, +) +plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) + +# Rigid body +inertia = (2 / 5) * mass * radius**2 * np.eye(3) +body = fd.constraint.RigidBody( + ball_mesh, mass=mass, inertia_tensor=inertia, center_of_mass=np.array([0, 0, z0]) +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.0) +body.enable_ipc_contact(plane_mesh, dhat=0.01, kappa=1e8) + +print(f" Ball: {ball_mesh.n_nodes} nodes, IPC kappa=1e8, Rayleigh alpha=1.0") + +# Solve with 6x6 direct solver +z_hist = [z0] +t_hist = [0.0] + + +def collect(t, q, v): + z_hist.append(z0 + q[2]) + t_hist.append(t) + + +t0 = time.time() +q, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) +elapsed = time.time() - t0 + +t_hist = np.array(t_hist) +z_hist = np.array(z_hist) +print( + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" +) +print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") + + +# Analytical elastic bounce +def analytical_bounce(t_arr, z0, g, r): + z = np.empty_like(t_arr) + zc, vc, tc = z0, 0.0, 0.0 + for i, t in enumerate(t_arr): + dt_l = t - tc + z_val = zc + vc * dt_l - 0.5 * g * dt_l**2 + v_val = vc - g * dt_l + if z_val <= r and v_val < 0: + disc = vc**2 + 2 * g * (zc - r) + if disc >= 0: + dt_c = (vc + np.sqrt(disc)) / g + vc = -(vc - g * dt_c) + zc, tc = r, tc + dt_c + dt_l = t - tc + z_val = zc + vc * dt_l - 0.5 * g * dt_l**2 + z[i] = z_val + return z + + +z_analytical = analytical_bounce(t_hist, z0, g, radius) + +# PyVista animation +print("\nGenerating PyVista animation...") +out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" +gif_path = f"{out_dir}/rigid_body_bounce_ipc.gif" + +fps = 25 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_hist), frame_skip) + +sphere = pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=20, phi_resolution=20 +) +pts_ref = sphere.points.copy() +vis_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[800, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh(sphere, color="steelblue", smooth_shading=True) +pl.camera_position = [(1.2, -1.2, 0.8), (0, 0, 0.25), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +for i in frame_indices: + sphere.points[:] = pts_ref + np.array([[0, 0, z_hist[i] - z0]]) + sphere.GetPoints().Modified() + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m 6x6 Newmark+IPC", + position="upper_edge", + font_size=11, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") + +# Plot +try: + import matplotlib.pyplot as plt + + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 7), sharex=True) + ax1.plot(t_hist, z_hist, "b-", linewidth=1, label="Fedoo 6x6 (IPC+Rayleigh)") + ax1.plot( + t_hist, + z_analytical, + "r--", + linewidth=0.8, + alpha=0.6, + label="Analytical elastic", + ) + ax1.axhline( + y=radius, color="grey", linestyle=":", alpha=0.5, label=f"contact z={radius}m" + ) + ax1.set_ylabel("z (m)") + ax1.set_title( + f"Sphere bounce — {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + ) + ax1.legend() + ax1.grid(True, alpha=0.3) + ax2.plot(t_hist, z_hist - z_analytical, "g-", linewidth=0.8) + ax2.set_ylabel("z_fedoo - z_analytical (m)") + ax2.set_xlabel("t (s)") + ax2.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig(f"{out_dir}/rigid_body_bounce_ipc.png", dpi=150) + plt.close() + print(f" Saved: {out_dir}/rigid_body_bounce_ipc.png") +except ImportError: + pass diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py new file mode 100644 index 00000000..6fadac1b --- /dev/null +++ b/examples/rigid_body_bunny_bounce.py @@ -0,0 +1,179 @@ +"""Stanford bunny (convex hull) bouncing on a plane — 6x6 solver + IPC. + +The bunny_coarse mesh has 658 open edges, so we use its convex hull +(watertight, manifold). The shape is simplified but still asymmetric, +producing interesting tumbling on impact. +""" + +import sys +import time +import numpy as np +import pyvista as pv + +sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") +import fedoo as fd + +g = 9.81 +dt = 5e-4 +t_end = 2.0 + +print("=" * 60) +print("BUNNY BOUNCE — 6x6 Newmark + IPC contact") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Build watertight bunny from convex hull of decimated full bunny +pv_raw = pv.examples.download_bunny().decimate(0.95) +pv_bunny = pv_raw.delaunay_3d().extract_surface().triangulate().clean() + +# Scale and position +bounds = pv_bunny.bounds +size = max(bounds[1] - bounds[0], bounds[3] - bounds[2], bounds[5] - bounds[4]) +pv_bunny = pv_bunny.scale(0.25 / size, inplace=False) +pv_bunny = pv_bunny.translate( + [-pv_bunny.center[0], -pv_bunny.center[1], -pv_bunny.bounds[4] + 0.4], + inplace=False, +) +pv_bunny = pv_bunny.compute_normals(consistent_normals=True, auto_orient_normals=True) + +bunny_mesh = fd.Mesh.from_pyvista(pv_bunny) +print(f" Bunny: {bunny_mesh.n_nodes} nodes, {bunny_mesh.n_elements} faces") +print(f" Manifold: {pv_bunny.is_manifold}, Open edges: {pv_bunny.n_open_edges}") + +# Plane +pv_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, +) +plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) + +# Rigid body — box inertia approximation +mass = 0.5 +bb = pv_bunny.bounds +lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] +I_approx = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) + +body = fd.constraint.RigidBody( + bunny_mesh, + mass=mass, + inertia_tensor=I_approx, + center_of_mass=np.array(pv_bunny.center), + name="Bunny", +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.0) +body.enable_ipc_contact(plane_mesh, dhat=0.008, kappa=1e8) + +print(f" mass={mass}kg, center_z={body.center_of_mass[2]:.3f}m") +print(f" dt={dt}s → {int(t_end/dt)} steps") + +# Solve — store full q (6 DOFs) for rotation animation +q_hist = [np.zeros(6)] +t_hist = [0.0] + + +def collect(t, q, v): + q_hist.append(q.copy()) + t_hist.append(t) + + +t0 = time.time() +q_final, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) +elapsed = time.time() - t0 + +t_hist = np.array(t_hist) +q_hist = np.array(q_hist) +z_hist = body.center_of_mass[2] + q_hist[:, 2] +print( + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" +) +print(f" z_min={z_hist.min():.4f}m") +print( + f" max rotation (deg): rx={np.degrees(q_hist[:,3].max()):.1f} ry={np.degrees(q_hist[:,4].max()):.1f} rz={np.degrees(q_hist[:,5].max()):.1f}" +) + +# Animation +print("\nGenerating PyVista animation...") +out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" +gif_path = f"{out_dir}/rigid_body_bunny_bounce.gif" + +fps = 25 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_hist), frame_skip) + +vis = pv_bunny.copy() +pts_ref = vis.points.copy() +vis_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[900, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh(vis, color="sandybrown", smooth_shading=True) +pl.camera_position = [(1.0, -1.0, 0.7), (0, 0, 0.2), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + +center = body.center_of_mass +for i in frame_indices: + q_i = q_hist[i] + disp = q_i[:3] + angles = q_i[3:] + + # Apply full rigid body transform: rotate around center, then translate + R = Rotation.from_euler("XYZ", angles).as_matrix() + pts_rotated = (pts_ref - center) @ R.T + center + disp + vis.points[:] = pts_rotated + vis.GetPoints().Modified() + + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m rx={np.degrees(angles[0]):.1f}°", + position="upper_edge", + font_size=10, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") + +# Plot +try: + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(12, 4)) + ax.plot(t_hist, z_hist, "b-", linewidth=1) + ax.axhline(y=0, color="grey", linestyle=":", alpha=0.5) + ax.set_xlabel("t (s)") + ax.set_ylabel("z (m)") + ax.set_title( + f"Bunny bounce — {bunny_mesh.n_nodes} nodes, {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + ) + ax.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig(f"{out_dir}/rigid_body_bunny_bounce.png", dpi=150) + plt.close() + print(f" Saved: {out_dir}/rigid_body_bunny_bounce.png") +except ImportError: + pass diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py new file mode 100644 index 00000000..b599e3c8 --- /dev/null +++ b/examples/rigid_body_cow_bounce.py @@ -0,0 +1,177 @@ +"""Cow bouncing on a plane — 6x6 Newmark + IPC contact. + +The cow mesh from PyVista is watertight and manifold (2903 nodes). +""" + +import sys +import time +import numpy as np +import pyvista as pv + +sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") +import fedoo as fd + +g = 9.81 +dt = 5e-4 +t_end = 3.0 + +print("=" * 60) +print("COW BOUNCE — 6x6 Newmark + IPC contact") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Cow mesh — watertight, manifold +pv_cow = pv.examples.download_cow().triangulate().clean() +print( + f" Raw cow: {pv_cow.n_points} pts, manifold={pv_cow.is_manifold}, open_edges={pv_cow.n_open_edges}" +) + +# Scale to ~0.3m and position above ground +bounds = pv_cow.bounds +size = max(bounds[1] - bounds[0], bounds[3] - bounds[2], bounds[5] - bounds[4]) +pv_cow = pv_cow.scale(0.3 / size, inplace=False) +pv_cow = pv_cow.translate( + [-pv_cow.center[0], -pv_cow.center[1], -pv_cow.bounds[4] + 0.5], + inplace=False, +) +# Decimate for speed +pv_cow = pv_cow.decimate(0.9).triangulate().clean() +pv_cow = pv_cow.compute_normals(consistent_normals=True, auto_orient_normals=True) + +cow_mesh = fd.Mesh.from_pyvista(pv_cow) +print(f" Decimated cow: {cow_mesh.n_nodes} nodes, {cow_mesh.n_elements} faces") + +# Plane +pv_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=2.0, + j_size=2.0, + i_resolution=8, + j_resolution=8, +) +plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) + +# Rigid body with box-approximate inertia +mass = 1.0 +bb = pv_cow.bounds +lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] +I_approx = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) + +body = fd.constraint.RigidBody( + cow_mesh, + mass=mass, + inertia_tensor=I_approx, + center_of_mass=np.array(pv_cow.center), + name="Cow", +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.5) +body.enable_ipc_contact(plane_mesh, dhat=0.01) # kappa auto-tuned + +print(f" mass={mass}kg, center_z={body.center_of_mass[2]:.3f}m") +print(f" inertia diag: {np.diag(I_approx)}") +print(f" dt={dt}s → {int(t_end/dt)} steps") + +# Solve +q_hist = [np.zeros(6)] +t_hist = [0.0] + + +def collect(t, q, v): + q_hist.append(q.copy()) + t_hist.append(t) + + +t0 = time.time() +q, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) +elapsed = time.time() - t0 + +t_hist = np.array(t_hist) +q_hist = np.array(q_hist) +z_hist = body.center_of_mass[2] + q_hist[:, 2] + +print( + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" +) +print(f" z_min={z_hist.min():.4f}m") +print( + f" max rotation: rx={np.degrees(q_hist[:,3].max()):.0f}° ry={np.degrees(q_hist[:,4].max()):.0f}° rz={np.degrees(q_hist[:,5].max()):.0f}°" +) + +# Animation +print("\nGenerating PyVista animation...") +out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" +gif_path = f"{out_dir}/rigid_body_cow_bounce.gif" + +fps = 25 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_hist), frame_skip) +print(f" {len(frame_indices)} frames") + +vis_cow = pv_cow.copy() +pts_ref = vis_cow.points.copy() +center = body.center_of_mass +vis_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=2.0, + j_size=2.0, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[900, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh(vis_cow, color="sandybrown", smooth_shading=True) +pl.camera_position = [(1.5, -1.5, 1.0), (0, 0, 0.25), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + +for i in frame_indices: + qi = q_hist[i] + R = Rotation.from_euler("XYZ", qi[3:]).as_matrix() + vis_cow.points[:] = (pts_ref - center) @ R.T + center + qi[:3] + vis_cow.GetPoints().Modified() + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m", + position="upper_edge", + font_size=11, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") + +# Plot +try: + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(12, 4)) + ax.plot(t_hist, z_hist, "b-", linewidth=1) + ax.axhline(y=0, color="grey", linestyle=":", alpha=0.5) + ax.set_xlabel("t (s)") + ax.set_ylabel("z (m)") + ax.set_title( + f"Cow bounce — {cow_mesh.n_nodes} nodes, {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + ) + ax.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig(f"{out_dir}/rigid_body_cow_bounce.png", dpi=150) + plt.close() + print(f" Saved: {out_dir}/rigid_body_cow_bounce.png") +except ImportError: + pass diff --git a/examples/rigid_body_freefall.py b/examples/rigid_body_freefall.py new file mode 100644 index 00000000..36fa81cf --- /dev/null +++ b/examples/rigid_body_freefall.py @@ -0,0 +1,167 @@ +"""Rigid body free fall — validation of RigidBody + NonLinearNewmark. + +A rigid sphere falls under gravity. The result is compared with the +analytical solution z(t) = z0 - 0.5*g*t^2. + +Demonstrates the RigidBody API: +- fd.constraint.RigidBody(mesh, mass, inertia_tensor) +- body.set_force([0, 0, -m*g]) +- body.assembly as both Stiffness and Mass in NonLinearNewmark +- PyVista animation of the result +""" + +import sys +import numpy as np +import pyvista as pv + +sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") +import fedoo as fd + +# ============================================================================== +# Parameters +# ============================================================================== +g = 9.81 +mass = 1.0 +radius = 0.1 +z0 = 1.0 +dt = 1e-3 +t_end = 0.6 + +print("=" * 60) +print("RIGID BODY FREE FALL — Fedoo validation") +print(f" Fedoo {fd.__version__}, m={mass}kg, z0={z0}m, r={radius}m, dt={dt}s") +print("=" * 60) + +# ============================================================================== +# Setup +# ============================================================================== +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Sphere mesh via PyVista +pv_sphere = pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=12, phi_resolution=12 +) +mesh = fd.Mesh.from_pyvista(pv_sphere) +print(f" Mesh: {mesh.n_nodes} nodes, {mesh.n_elements} elements") + +# Rigid body +inertia = (2.0 / 5.0) * mass * radius**2 * np.eye(3) +body = fd.constraint.RigidBody(mesh, mass=mass, inertia_tensor=inertia, name="Ball") +body.set_force([0, 0, -mass * g]) + +# Problem +pb = fd.problem.NonLinearNewmark( + body.assembly, body.assembly, 0.25, 0.5, name="FreeFall" +) +body.add_to_problem(pb) + +# ============================================================================== +# Solve step by step +# ============================================================================== +print(" Solving...") +n_steps = int(t_end / dt) +z_history = [z0] +t_history = [0.0] + +pb.initialize() + +idx_z = ( + pb.n_node_dof + + pb._global_dof.indice_start("RigidDispZ") + + body.constraint.node_cd[2] +) + +for step in range(n_steps): + t = (step + 1) * dt + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + + dz = pb.get_dof_solution()[idx_z] + z_history.append(z0 + dz) + t_history.append(t) + + if step % 100 == 0: + z_ana = z0 - 0.5 * g * t**2 + print(f" t={t:.3f}s z={z0+dz:.4f}m (analytical: {z_ana:.4f}m)") + +t_history = np.array(t_history) +z_history = np.array(z_history) +z_analytical = z0 - 0.5 * g * t_history**2 +error = np.max(np.abs(z_history - z_analytical)) +print(f"\n Max error |z_num - z_analytical| = {error:.2e} m") +print(f" {'PASS' if error < 0.01 else 'FAIL'}") + +# ============================================================================== +# PyVista animation +# ============================================================================== +print("\nGenerating PyVista animation...") +out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" +gif_path = f"{out_dir}/rigid_body_freefall.gif" + +fps = 25 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_history), frame_skip) +print(f" {len(frame_indices)} frames at {fps} fps") + +sphere = pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=20, phi_resolution=20 +) +pts_ref = sphere.points.copy() +plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=2.0, + j_size=2.0, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[800, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(plane, color="lightgrey", opacity=0.7, show_edges=True) +pl.add_mesh(sphere, color="steelblue", smooth_shading=True) +pl.camera_position = [(2.0, -2.0, 1.5), (0, 0, 0.5), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +for i in frame_indices: + z = z_history[i] + t = t_history[i] + sphere.points[:] = pts_ref + np.array([[0, 0, z - z0]]) + sphere.GetPoints().Modified() + pl.add_text( + f"t = {t:.3f}s | z = {z:.3f}m | Fedoo RigidBody", + position="upper_edge", + font_size=11, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") + +# Trajectory plot +try: + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(10, 4)) + ax.plot(t_history, z_history, "b-", linewidth=1.5, label="Fedoo Newmark") + ax.plot(t_history, z_analytical, "r--", linewidth=1, label="Analytical") + ax.set_xlabel("t (s)") + ax.set_ylabel("z (m)") + ax.set_title("Rigid body free fall — Fedoo validation") + ax.legend() + ax.grid(True, alpha=0.3) + plt.tight_layout() + png_path = f"{out_dir}/rigid_body_freefall.png" + plt.savefig(png_path, dpi=150) + plt.close() + print(f" Saved: {png_path}") +except ImportError: + pass diff --git a/fedoo/.DS_Store b/fedoo/.DS_Store deleted file mode 100644 index 5008ddfcf53c02e82d7eee2e57c38e5672ef89f6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeH~Jr2S!425mzP>H1@V-^m;4Wg<&0T*E43hX&L&p$$qDprKhvt+--jT7}7np#A3 zem<@ulZcFPQ@L2!n>{z**++&mCkOWA81W14cNZlEfg7;MkzE(HCqgga^y>{tEnwC%0;vJ&^%eQ zLs35+`xjp>T0 0: + is_rigid = np.isin(surface_node_indices, list(self._rigid_node_set)) + else: + is_rigid = np.zeros(n_surf, dtype=bool) + deformable_mask = ~is_rigid + + # --- Deformable nodes: identity mapping --- + deform_local = np.where(deformable_mask)[0] + if len(deform_local) > 0: + deform_global = surface_node_indices[deform_local] + for d in range(ndim): + all_rows.append(disp_ranks[d] * n_nodes + deform_global) + all_cols.append(deform_local * ndim + d) + all_data.append(np.ones(len(deform_local))) + + # --- Rigid body nodes: contact Jacobian --- + for node_set, rb in self._rigid_bodies: + rt = rb.constraint + dof_indices = rb.assembly._dof_indices # (6,) global DOF positions + + # Find which surface indices belong to this rigid body + mask = np.isin(surface_node_indices, node_set) + local_indices = np.where(mask)[0] + if len(local_indices) == 0: + continue + + n_rb = len(local_indices) + + # Get exact rotation derivatives from RigidTie + if pb is not None: + dof_ref, _ = rt._get_dof_ref(pb) + angles = dof_ref[3:] + center_disp = dof_ref[:3] + else: + angles = np.zeros(3) + center_disp = np.zeros(3) + + R, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) + center = rt.center # initial center position + + # Lever arms in reference frame + if vertices is not None: + # Use current positions to get lever arms + vertex_pos = vertices[local_indices] # (n_rb, 3) + current_center = center + center_disp + r = vertex_pos - current_center # (n_rb, 3) + else: + # Use rest positions + r_ref = self._rest_positions[local_indices] - center + r = (R @ r_ref.T).T # rotate to current frame + + # Translation part: J_i[:3, :3] = I_3 + for d in range(ndim): + all_rows.append(np.full(n_rb, dof_indices[d])) + all_cols.append(local_indices * ndim + d) + all_data.append(np.ones(n_rb)) + + # Rotation part: exact derivatives from RigidTie + # J_i[d, 3+k] = (dR_drk @ r_ref_i)[d] + # where r_ref = vertex_ref - center (in reference config) + r_ref = self._rest_positions[local_indices] - center # (n_rb, 3) + du_drx = r_ref @ dR_drx.T # (n_rb, 3) + du_dry = r_ref @ dR_dry.T + du_drz = r_ref @ dR_drz.T + + for d in range(ndim): + # RotX contribution + nonzero = np.abs(du_drx[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[3])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_drx[nonzero, d]) + # RotY contribution + nonzero = np.abs(du_dry[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[4])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_dry[nonzero, d]) + # RotZ contribution + nonzero = np.abs(du_drz[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[5])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_drz[nonzero, d]) + + rows = np.concatenate(all_rows) if all_rows else np.array([], dtype=int) + cols = np.concatenate(all_cols) if all_cols else np.array([], dtype=int) + data = np.concatenate(all_data) if all_data else np.array([]) - for d in range(ndim): - start = d * n_surf - end = (d + 1) * n_surf - # fedoo blocked row: disp_ranks[d] * n_nodes + global_node - rows[start:end] = disp_ranks[d] * n_nodes + surface_node_indices - # ipctk interleaved col: local_i * ndim + d - cols[start:end] = np.arange(n_surf) * ndim + d - - data = np.ones(n_surf * ndim) P = sparse.csc_matrix( (data, (rows, cols)), - shape=(nvar * n_nodes, n_surf * ndim), + shape=(n_dof, n_surf * ndim), ) return P @@ -345,9 +470,18 @@ def _get_elastic_gradient_on_surface(self): if grad_energy_full is None: return None - # Truncate to mesh DOFs (remove global DOFs like PeriodicBC) - n_mesh_dof = self.space.nvar * self.mesh.n_nodes - grad_energy_full = grad_energy_full[:n_mesh_dof] + # When rigid bodies are present, P includes global DOF rows, + # so keep the full vector. Otherwise truncate to mesh DOFs. + if not self._rigid_bodies: + n_mesh_dof = self.space.nvar * self.mesh.n_nodes + grad_energy_full = grad_energy_full[:n_mesh_dof] + + # Ensure dimension matches P rows + n_P_rows = self._scatter_matrix.shape[0] + if len(grad_energy_full) < n_P_rows: + grad_energy_full = np.pad( + grad_energy_full, (0, n_P_rows - len(grad_energy_full)) + ) # Project to surface DOFs: P^T @ full_grad -> surface_grad return self._scatter_matrix.T @ grad_energy_full @@ -472,8 +606,10 @@ def _compute_ipc_contributions(self, vertices, compute="all"): ) self.global_matrix += P @ fric_hess_surf @ P.T - # Pad with zeros to account for extra global DOFs (e.g. PeriodicBC) - if self._n_global_dof > 0: + # Pad with zeros to account for extra global DOFs (e.g. PeriodicBC). + # When rigid bodies are present, P already maps to n_dof (including + # global DOFs), so no padding is needed. + if self._n_global_dof > 0 and not self._rigid_bodies: if compute != "matrix": self.global_vector = np.pad(self.global_vector, (0, self._n_global_dof)) if compute != "vector": @@ -772,9 +908,25 @@ def initialize(self, pb): # Rest positions of surface vertices self._rest_positions = self.mesh.nodes[self._surface_node_indices] + # Build rigid body node lookup set + if self._rigid_bodies: + self._rigid_node_set = set() + for node_set, rb in self._rigid_bodies: + self._rigid_node_set.update(node_set.tolist()) + + # Validate: CCD and OGC not supported with rigid bodies + if self._use_ccd: + raise ValueError( + "use_ccd=True is not yet supported with rigid bodies. " + "Use use_ccd=False (default)." + ) + if self._use_ogc: + raise ValueError("use_ogc=True is not supported with rigid bodies.") + # Build scatter matrix for DOF mapping (ipctk surface -> fedoo full) + vertices = self._get_current_vertices(pb) self._scatter_matrix = self._build_scatter_matrix( - self._surface_node_indices, self.mesh.n_nodes, ndim + self._surface_node_indices, self.mesh.n_nodes, ndim, pb, vertices ) # Create broad phase instance @@ -976,6 +1128,16 @@ def update(self, pb, compute="all"): vertices = self._get_current_vertices(pb) self._last_vertices = vertices + # Rebuild scatter matrix for rigid bodies (Jacobian depends on position) + if self._rigid_bodies: + self._scatter_matrix = self._build_scatter_matrix( + self._surface_node_indices, + self.mesh.n_nodes, + self.space.ndim, + pb, + vertices, + ) + # Rebuild collision set self._collisions.build( self._collision_mesh, diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py new file mode 100644 index 00000000..5c776cc0 --- /dev/null +++ b/fedoo/constraint/rigid_body.py @@ -0,0 +1,794 @@ +"""Rigid body support for large-displacement dynamic simulation. + +Provides: +- ``RigidBodyAssembly``: AssemblyBase that contributes a 6x6 mass matrix and + a generalized force vector on the rigid body's global DOFs. +- ``RigidBody``: Facade that coordinates a RigidTie constraint (kinematics) + with a RigidBodyAssembly (dynamics) into a single user-facing object. + Optionally handles IPC contact via direct Jacobian projection (J^T @ F). + +Large rotations +--------------- +Rotations are handled exactly (no small-angle approximation) via a +multiplicative quaternion update (using ``simcoon.Rotation``): + +- The total rotation is stored as a quaternion ``Q_base`` in the RigidTie. +- The rotation DOFs ``[rx, ry, rz]`` represent a small Euler increment + from the current base state. +- At each converged time step, the increment is composed into the + quaternion: ``Q_base = R_inc * Q_base`` (quaternion multiplication). +- This avoids gimbal lock and supports arbitrarily large rotations. + +The contact Jacobian ``J = [I_3 | dR/d(angle) @ r_ref]`` uses the exact +rotation derivatives from ``RigidTie._compute_rotation()``, not the +infinitesimal skew-symmetric approximation. +""" + +import numpy as np +from scipy import sparse +from fedoo.core.base import AssemblyBase +from fedoo.constraint.rigid_tie import RigidTie + +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + + +class RigidBodyAssembly(AssemblyBase): + """Assembly contributing rigid body mass and generalized forces. + + Injects a 6x6 mass matrix and a 6-component force vector at the global + DOF positions created by a :class:`RigidTie` constraint. + + The mass matrix has the structure:: + + M = [[m*I_3, 0 ], + [ 0, J_global]] + + where ``J_global = Q.apply_tensor(J_body)`` is the inertia tensor rotated + to the current configuration via the RigidTie's quaternion. + + Parameters + ---------- + mass : float + Total mass of the rigid body. + inertia_tensor : array_like, shape (3, 3) + Inertia tensor about the center of mass in the body-fixed frame. + rigid_tie : RigidTie + The associated rigid tie constraint. + mesh : fedoo.Mesh, optional + Mesh (needed by the Fedoo Problem constructor). + space : ModelingSpace, optional + Modeling space. + name : str, optional + Name of the assembly. + """ + + def __init__( + self, + mass, + inertia_tensor, + rigid_tie, + mesh=None, + space=None, + name="RigidBodyAssembly", + ): + if space is None: + from fedoo.core.modelingspace import ModelingSpace + + space = ModelingSpace.get_active() + AssemblyBase.__init__(self, name, space) + self.mass = float(mass) + self.inertia_body = np.asarray(inertia_tensor, dtype=float) + self.rigid_tie = rigid_tie + self.force = np.zeros(6) + self.mesh = mesh + + self._dof_indices = None + self._pb_ref = None + + # IPC contact state (set by RigidBody.enable_ipc_contact) + self._ipc_collision_mesh = None + self._ipc_collisions = None + self._ipc_barrier = None + self._ipc_kappa = None + self._ipc_kappa_auto = False + self._ipc_dhat = None + self._ipc_broad_phase = None + self._ipc_rest_positions = None + self._ipc_n_body = 0 + self._ipc_obstacle_nodes = None + self._contact_force = np.zeros(6) + self._contact_stiffness = np.zeros((6, 6)) + + def initialize(self, pb): + """Extract global DOF indices from the problem.""" + rt = self.rigid_tie + self._dof_indices = np.array( + [ + pb.n_node_dof + + pb._global_dof.indice_start(rt.var_cd[i]) + + rt.node_cd[i] + for i in range(6) + ] + ) + self._pb_ref = pb + if self.mesh is None: + self.mesh = pb.mesh + + @property + def dof_indices(self): + """Global DOF indices [Fx,Fy,Fz,Mx,My,Mz] in the problem.""" + return self._dof_indices + + # ------------------------------------------------------------------ + # IPC contact: Jacobian and force/stiffness computation + # ------------------------------------------------------------------ + + def _build_ipc_jacobian(self, rt, angles): + """Build contact Jacobian J mapping 6 rigid DOFs to all vertex DOFs. + + Uses exact rotation derivatives from RigidTie for consistency. + + Returns + ------- + J : ndarray, shape (n_all*3, 6) + Body vertices use exact derivatives; obstacle rows are zero. + """ + R, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) + r_ref = self._ipc_rest_positions - rt.center + du_drx = r_ref @ dR_drx.T + du_dry = r_ref @ dR_dry.T + du_drz = r_ref @ dR_drz.T + + n_body = self._ipc_n_body + n_all = n_body + len(self._ipc_obstacle_nodes) + J = np.zeros((n_all * 3, 6)) + for d in range(3): + J[d : n_body * 3 : 3, d] = 1.0 + J[d : n_body * 3 : 3, 3] = du_drx[:, d] + J[d : n_body * 3 : 3, 4] = du_dry[:, d] + J[d : n_body * 3 : 3, 5] = du_drz[:, d] + return J + + def _ipc_vertices(self, q, rt): + """Compute all vertex positions (body + obstacle) from q.""" + R, _, _, _ = rt._compute_rotation(q[3:]) + r_ref = self._ipc_rest_positions - rt.center + body_verts = r_ref @ R.T + rt.center + q[:3] + return np.vstack([body_verts, self._ipc_obstacle_nodes]) + + def compute_contact(self, q, rt, compute="all"): + """Compute IPC contact force and/or stiffness on 6 rigid DOFs. + + Parameters + ---------- + q : ndarray (6,) + Rigid body DOFs [dx, dy, dz, rx, ry, rz]. + rt : RigidTie + Associated rigid tie constraint. + compute : str + ``"all"`` (force + stiffness), ``"force"`` (skip hessian), + or ``"stiffness"``. + + Returns + ------- + force : ndarray (6,) + stiffness : ndarray (6, 6) + """ + if self._ipc_collision_mesh is None: + self._contact_force[:] = 0 + self._contact_stiffness[:] = 0 + return self._contact_force, self._contact_stiffness + + vertices = self._ipc_vertices(q, rt) + self._ipc_collisions.build( + self._ipc_collision_mesh, + vertices, + self._ipc_dhat, + broad_phase=self._ipc_broad_phase, + ) + + if len(self._ipc_collisions) == 0: + self._contact_force[:] = 0 + self._contact_stiffness[:] = 0 + return self._contact_force, self._contact_stiffness + + from fedoo.constraint.ipc_contact import _import_ipctk + + ipctk = _import_ipctk() + + J = self._build_ipc_jacobian(rt, q[3:]) + + # Gradient → force + grad = self._ipc_barrier.gradient( + self._ipc_collisions, self._ipc_collision_mesh, vertices + ) + self._contact_force = -self._ipc_kappa * (J.T @ grad) + + # Hessian → stiffness (skipped in line-search for efficiency) + if compute in ("all", "stiffness"): + try: + hess = self._ipc_barrier.hessian( + self._ipc_collisions, + self._ipc_collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, + ) + except RuntimeError: + hess = self._ipc_barrier.hessian( + self._ipc_collisions, + self._ipc_collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, + ) + self._contact_stiffness = self._ipc_kappa * (J.T @ (hess @ J)) + else: + self._contact_stiffness[:] = 0 + + return self._contact_force, self._contact_stiffness + + # ------------------------------------------------------------------ + # Fedoo assembly interface (for use with NonLinearNewmark) + # ------------------------------------------------------------------ + + def assemble_global_mat(self, compute="all"): + if self._dof_indices is None: + return + + n = ( + self._pb_ref.n_dof + if self._pb_ref is not None + else ( + max( + self.mesh.n_nodes * self.space.nvar, + int(self._dof_indices.max()) + 1, + ) + ) + ) + idx = self._dof_indices + + if compute in ("all", "matrix"): + M = np.zeros((6, 6)) + M[0, 0] = M[1, 1] = M[2, 2] = self.mass + Q = self.rigid_tie.Q_total + M[3:, 3:] = ( + Q.apply_tensor(self.inertia_body) + if Q is not None + else self.inertia_body + ) + + self.global_matrix = sparse.csr_matrix( + (M.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), + shape=(n, n), + ) + + if compute in ("all", "vector"): + vec = np.zeros(n) + vec[idx] = self.force + self._contact_force + self.global_vector = vec + + def update(self, pb, compute="all"): + self._pb_ref = pb + self.assemble_global_mat(compute) + + def set_start(self, pb): + self._pb_ref = pb + + def to_start(self, pb): + pass + + +class RigidBody: + """Rigid body for large-displacement dynamic simulation in Fedoo. + + Coordinates a :class:`RigidTie` constraint (kinematics: MPC coupling + of surface nodes to 6 rigid DOFs) with a :class:`RigidBodyAssembly` + (dynamics: 6x6 mass matrix and generalized force vector). + + Supports arbitrarily large translations and rotations via quaternion- + based multiplicative update (no gimbal lock, no small-angle + approximation). IPC barrier contact is handled via direct Jacobian + projection ``J^T @ F`` in the 6-DOF space. + + Two solver modes: + - ``body.solve(dt, tmax)`` — direct 6x6 Newmark integrator (fast, + standalone, no Fedoo Problem needed). + - ``body.add_to_problem(pb)`` — integrate with Fedoo's + NonLinearNewmark for mixed rigid-deformable problems. + + Parameters + ---------- + mesh : fedoo.Mesh + Surface mesh of the rigid body (tri3 for IPC contact). + mass : float, optional + Total mass. Required if ``density`` is not given. + density : float, optional + Material density. Computes mass and inertia from mesh. + inertia_tensor : array_like, shape (3, 3), optional + Inertia tensor about center of mass in body frame. + center_of_mass : array_like, shape (3,), optional + Center of mass. Default: mean of mesh nodes. + use_quaternion : bool, optional + Quaternion-based rotation in RigidTie (default True). + name : str, optional + Name of the rigid body. + + Example + ------- + .. code-block:: python + + import fedoo as fd + import numpy as np + + fd.ModelingSpace("3D") + sphere_mesh = fd.Mesh.from_pyvista(pv.Sphere(radius=0.1)) + + body = fd.constraint.RigidBody(sphere_mesh, mass=1.0, + inertia_tensor=0.004*np.eye(3)) + body.set_force([0, 0, -9.81]) + body.set_rayleigh_damping(1.0) + body.enable_ipc_contact(plane_mesh, dhat=0.01, kappa=1e8) + + q, v, a = body.solve(dt=5e-4, tmax=2.0) + """ + + def __init__( + self, + mesh, + mass=None, + density=None, + inertia_tensor=None, + center_of_mass=None, + use_quaternion=True, + name="RigidBody", + ): + self.mesh = mesh + self.name = name + + if center_of_mass is None: + center_of_mass = mesh.nodes.mean(axis=0) + center_of_mass = np.asarray(center_of_mass, dtype=float) + + if density is not None: + vol = mesh.get_volume() + if mass is None: + mass = density * vol + if inertia_tensor is None: + inertia_tensor = self._compute_inertia(mesh, density, center_of_mass) + else: + if mass is None: + raise ValueError("Either mass or density must be provided.") + if inertia_tensor is None: + raise ValueError("inertia_tensor required when density is not given.") + + self.mass = float(mass) + self.center_of_mass = center_of_mass + self.inertia_tensor = np.asarray(inertia_tensor, dtype=float) + + self.constraint = RigidTie( + np.arange(mesh.n_nodes), + center=center_of_mass, + use_quaternion=use_quaternion, + name=f"{name}_tie", + ) + self.assembly = RigidBodyAssembly( + self.mass, + self.inertia_tensor, + self.constraint, + mesh=mesh, + name=f"{name}_asm", + ) + self._rayleigh_alpha = 0.0 + + # ------------------------------------------------------------------ + # Forces and damping + # ------------------------------------------------------------------ + + def set_force(self, force): + """Set external force [Fx, Fy, Fz] on translational DOFs.""" + self.assembly.force[:3] = force + + def set_torque(self, torque): + """Set external torque [Mx, My, Mz] on rotational DOFs.""" + self.assembly.force[3:] = torque + + def set_generalized_force(self, f): + """Set full generalized force [Fx, Fy, Fz, Mx, My, Mz].""" + self.assembly.force[:] = f + + def set_rayleigh_damping(self, alpha): + """Set mass-proportional damping: C = alpha * M.""" + self._rayleigh_alpha = float(alpha) + + # ------------------------------------------------------------------ + # IPC contact + # ------------------------------------------------------------------ + + def enable_ipc_contact(self, obstacle_mesh, dhat=0.01, kappa=None): + """Enable IPC barrier contact with an obstacle surface. + + Parameters + ---------- + obstacle_mesh : fedoo.Mesh + Surface mesh of the obstacle (tri3). + dhat : float + Barrier activation distance (meters). + kappa : float or None + Barrier stiffness. If None (default), automatically tuned at + first contact to balance external forces and barrier gradient. + """ + from fedoo.constraint.ipc_contact import _import_ipctk + + ipctk = _import_ipctk() + + body_nodes = self.mesh.nodes + obst_nodes = obstacle_mesh.nodes + n_body = len(body_nodes) + + if self.mesh.elements.shape[1] != 3: + raise ValueError("Body mesh must be tri3 for IPC contact.") + if obstacle_mesh.elements.shape[1] != 3: + raise ValueError("Obstacle mesh must be tri3 for IPC contact.") + + all_nodes = np.vstack([body_nodes, obst_nodes]) + all_elms = np.vstack([self.mesh.elements, obstacle_mesh.elements + n_body]) + + edges = ipctk.edges(all_elms) + cm = ipctk.CollisionMesh(all_nodes, edges, all_elms) + + # Only body↔obstacle collisions (no self-contact within rigid body) + patches = np.zeros(len(all_nodes), dtype=np.int32) + patches[n_body:] = 1 + cm.can_collide = ipctk.VertexPatchesCanCollide(patches) + + asm = self.assembly + asm._ipc_collision_mesh = cm + asm._ipc_collisions = ipctk.NormalCollisions() + asm._ipc_barrier = ipctk.BarrierPotential(dhat) + if kappa is None: + kappa = 1e9 + asm._ipc_kappa = kappa + asm._ipc_kappa_auto = False + asm._ipc_dhat = dhat + asm._ipc_broad_phase = ipctk.HashGrid() + asm._ipc_rest_positions = body_nodes.copy() + asm._ipc_n_body = n_body + asm._ipc_obstacle_nodes = obst_nodes.copy() + + # ------------------------------------------------------------------ + # Fedoo integration + # ------------------------------------------------------------------ + + def add_to_problem(self, pb): + """Register the rigid body constraint with a Fedoo problem.""" + pb.bc.add(self.constraint) + + @property + def Q_total(self): + """Current total rotation as a Rotation object.""" + return self.constraint.Q_total + + # ------------------------------------------------------------------ + # Direct 6x6 Newmark solver + # ------------------------------------------------------------------ + + @staticmethod + def _ccd_filter(asm, rt, q_current, dq_step): + """Limit dq_step so the trajectory is collision-free (Tight-Inclusion CCD). + + Computes the maximal alpha in [0, 1] such that the linear motion + from q_current to q_current + alpha*dq_step produces no collision, + then returns alpha * dq_step. + """ + from fedoo.constraint.ipc_contact import _import_ipctk + + ipctk = _import_ipctk() + + v0 = asm._ipc_vertices(q_current, rt) + v1 = asm._ipc_vertices(q_current + dq_step, rt) + + alpha = ipctk.compute_collision_free_stepsize( + asm._ipc_collision_mesh, + v0, + v1, + broad_phase=asm._ipc_broad_phase, + ) + + if alpha < 1.0: + # Leave a small margin (0.9 factor) to stay inside barrier zone + return 0.9 * alpha * dq_step + return dq_step + + def solve(self, dt, tmax, t0=0, callback=None, print_info=True): + """Solve rigid body dynamics with a direct 6x6 Newmark integrator. + + Bypasses the Fedoo FEM solver — solves directly in the 6-DOF + rigid body space with IPC contact and Rayleigh damping. + + Parameters + ---------- + dt : float + Time step. + tmax : float + End time. + t0 : float + Start time. + callback : callable, optional + Called at each step: ``callback(t, q, v)``. + print_info : bool + Print progress. + + Returns + ------- + q : ndarray (6,) + Final DOFs [dx, dy, dz, rx, ry, rz]. + v : ndarray (6,) + Final velocity. + a : ndarray (6,) + Final acceleration. + """ + n_steps = int(round((tmax - t0) / dt)) + q = np.zeros(6) + v = np.zeros(6) + a = np.zeros(6) + beta, gamma = 0.25, 0.5 + rt = self.constraint + asm = self.assembly + + # Ensure quaternion state is initialized + if not hasattr(rt, "_Q_base") or rt._Q_base is None: + rt._Q_base = Rotation.identity() + rt._Q_base_backup = Rotation.identity() + rt._angles_at_base = np.zeros(3) + + nr_tol = 1e-10 * max(np.linalg.norm(asm.force), 1.0) + + for step in range(n_steps): + t = t0 + (step + 1) * dt + + # Mass matrix + M = np.zeros((6, 6)) + M[0, 0] = M[1, 1] = M[2, 2] = self.mass + Q = rt._Q_base + M[3:, 3:] = ( + Q.apply_tensor(self.inertia_tensor) + if Q is not None + else self.inertia_tensor + ) + + # Damping + C = ( + self._rayleigh_alpha * M + if self._rayleigh_alpha > 0 + else np.zeros((6, 6)) + ) + + # Initial prediction + F_c, K_c = asm.compute_contact(q, rt, compute="all") + A_eff = K_c + (1 / (beta * dt**2)) * M + (gamma / (beta * dt)) * C + v_pred = (1 - gamma / beta) * v + dt * (1 - gamma / (2 * beta)) * a + rhs = ( + asm.force + + F_c + + M @ (-(1 / (beta * dt)) * v - (0.5 / beta - 1) * a) + + C @ (-v_pred) + ) + + try: + dq = np.linalg.solve(A_eff, rhs) + except np.linalg.LinAlgError: + if print_info: + print(f" Singular at t={t:.4f}s") + break + + # CCD: limit step to collision-free trajectory + if asm._ipc_collision_mesh is not None: + dq = self._ccd_filter(asm, rt, q, dq) + + # Newton-Raphson with line search + for nr_iter in range(20): + F_c, K_c = asm.compute_contact(q + dq, rt, compute="all") + new_a = ( + (1 / (beta * dt**2)) * dq + - (1 / (beta * dt)) * v + - (0.5 / beta - 1) * a + ) + new_v = v + dt * ((1 - gamma) * a + gamma * new_a) + res = asm.force + F_c - M @ new_a - C @ new_v + res_norm = np.linalg.norm(res) + + if res_norm < nr_tol: + break + + A_eff = K_c + (1 / (beta * dt**2)) * M + (gamma / (beta * dt)) * C + try: + ddq = np.linalg.solve(A_eff, res) + except np.linalg.LinAlgError: + break + + # CCD-filtered line search + ddq_safe = self._ccd_filter(asm, rt, q + dq, ddq) + alpha_ls = 1.0 + for _ in range(8): + dq_test = dq + alpha_ls * ddq_safe + F_test, _ = asm.compute_contact(q + dq_test, rt, compute="force") + a_test = ( + (1 / (beta * dt**2)) * dq_test + - (1 / (beta * dt)) * v + - (0.5 / beta - 1) * a + ) + v_test = v + dt * ((1 - gamma) * a + gamma * a_test) + if ( + np.linalg.norm(asm.force + F_test - M @ a_test - C @ v_test) + < res_norm + ): + break + alpha_ls *= 0.5 + dq = dq + alpha_ls * ddq_safe + + # Update state + new_a = ( + (1 / (beta * dt**2)) * dq - (1 / (beta * dt)) * v - (0.5 / beta - 1) * a + ) + v = v + dt * ((1 - gamma) * a + gamma * new_a) + a = new_a + q = q + dq + + # Absorb rotation into quaternion base + delta = q[3:] - rt._angles_at_base + if np.any(np.abs(delta) > 1e-14): + rt._Q_base = Rotation.from_euler("XYZ", delta) * rt._Q_base + rt._angles_at_base = q[3:].copy() + + # Adaptive barrier stiffness via ipctk heuristics + if asm._ipc_collision_mesh is not None and len(asm._ipc_collisions) > 0: + min_dist = asm._ipc_collisions.compute_minimum_distance( + asm._ipc_collision_mesh, + asm._ipc_vertices(q, rt), + ) + if not hasattr(asm, "_prev_min_dist"): + # First contact: initialize kappa + from fedoo.constraint.ipc_contact import _import_ipctk + + _ipctk = _import_ipctk() + bbox_diag = np.linalg.norm( + asm._ipc_collision_mesh.rest_positions.max(axis=0) + - asm._ipc_collision_mesh.rest_positions.min(axis=0) + ) + grad_barrier = asm._ipc_barrier.gradient( + asm._ipc_collisions, + asm._ipc_collision_mesh, + asm._ipc_vertices(q, rt), + ) + J = asm._build_ipc_jacobian(rt, q[3:]) + grad_b_proj = J.T @ grad_barrier + kappa_init, kappa_max = _ipctk.initial_barrier_stiffness( + bbox_diag, + asm._ipc_barrier.barrier, + asm._ipc_dhat, + self.mass, + asm.force, + grad_b_proj, + ) + asm._ipc_kappa = max(asm._ipc_kappa, kappa_init) + asm._ipc_kappa_max = kappa_max + asm._bbox_diag = bbox_diag + asm._prev_min_dist = min_dist + else: + from fedoo.constraint.ipc_contact import _import_ipctk + + _ipctk = _import_ipctk() + asm._ipc_kappa = _ipctk.update_barrier_stiffness( + asm._prev_min_dist, + min_dist, + asm._ipc_kappa_max, + asm._ipc_kappa, + asm._bbox_diag, + ) + asm._prev_min_dist = min_dist + + if callback is not None: + callback(t, q, v) + + if print_info and step % max(1, n_steps // 20) == 0: + n_c = len(asm._ipc_collisions) if asm._ipc_collisions is not None else 0 + print( + f" t={t:.3f}s z={self.center_of_mass[2]+q[2]:.4f}m contacts={n_c}" + ) + + return q, v, a + + # ------------------------------------------------------------------ + # Inertia computation + # ------------------------------------------------------------------ + + @staticmethod + def _compute_inertia(mesh, density, center_of_mass): + """Compute inertia tensor from a closed triangulated surface mesh. + + Uses the divergence theorem (Mirtich 1996). + """ + nodes = mesh.nodes - center_of_mass + elements = mesh.elements + + if elements.shape[1] != 3: + raise ValueError( + "Automatic inertia computation requires tri3 elements. " + f"Got elements with {elements.shape[1]} nodes." + ) + + v0 = nodes[elements[:, 0]] + v1 = nodes[elements[:, 1]] + v2 = nodes[elements[:, 2]] + + normals = np.cross(v1 - v0, v2 - v0) + + x2 = ( + v0[:, 0] ** 2 + + v1[:, 0] ** 2 + + v2[:, 0] ** 2 + + v0[:, 0] * v1[:, 0] + + v0[:, 0] * v2[:, 0] + + v1[:, 0] * v2[:, 0] + ) + y2 = ( + v0[:, 1] ** 2 + + v1[:, 1] ** 2 + + v2[:, 1] ** 2 + + v0[:, 1] * v1[:, 1] + + v0[:, 1] * v2[:, 1] + + v1[:, 1] * v2[:, 1] + ) + z2 = ( + v0[:, 2] ** 2 + + v1[:, 2] ** 2 + + v2[:, 2] ** 2 + + v0[:, 2] * v1[:, 2] + + v0[:, 2] * v2[:, 2] + + v1[:, 2] * v2[:, 2] + ) + + xy = ( + 2 * (v0[:, 0] * v0[:, 1] + v1[:, 0] * v1[:, 1] + v2[:, 0] * v2[:, 1]) + + v0[:, 0] * v1[:, 1] + + v0[:, 1] * v1[:, 0] + + v0[:, 0] * v2[:, 1] + + v0[:, 1] * v2[:, 0] + + v1[:, 0] * v2[:, 1] + + v1[:, 1] * v2[:, 0] + ) + xz = ( + 2 * (v0[:, 0] * v0[:, 2] + v1[:, 0] * v1[:, 2] + v2[:, 0] * v2[:, 2]) + + v0[:, 0] * v1[:, 2] + + v0[:, 2] * v1[:, 0] + + v0[:, 0] * v2[:, 2] + + v0[:, 2] * v2[:, 0] + + v1[:, 0] * v2[:, 2] + + v1[:, 2] * v2[:, 0] + ) + yz = ( + 2 * (v0[:, 1] * v0[:, 2] + v1[:, 1] * v1[:, 2] + v2[:, 1] * v2[:, 2]) + + v0[:, 1] * v1[:, 2] + + v0[:, 2] * v1[:, 1] + + v0[:, 1] * v2[:, 2] + + v0[:, 2] * v2[:, 1] + + v1[:, 1] * v2[:, 2] + + v1[:, 2] * v2[:, 1] + ) + + Ixx = np.sum(normals[:, 0] * x2) * density / 60.0 + Iyy = np.sum(normals[:, 1] * y2) * density / 60.0 + Izz = np.sum(normals[:, 2] * z2) * density / 60.0 + Ixy = np.sum(normals[:, 0] * xy) * density / 120.0 + Ixz = np.sum(normals[:, 0] * xz) * density / 120.0 + Iyz = np.sum(normals[:, 1] * yz) * density / 120.0 + + return np.array( + [ + [Iyy + Izz, -Ixy, -Ixz], + [-Ixy, Ixx + Izz, -Iyz], + [-Ixz, -Iyz, Ixx + Iyy], + ] + ) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 8cccacb8..f26c9154 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -3,8 +3,10 @@ import numpy as np from fedoo.core.boundary_conditions import BCBase, MPC, ListBC - -from scipy.spatial.transform import Rotation +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation class RigidTie(BCBase): @@ -22,7 +24,7 @@ class RigidTie(BCBase): * "RigidDisp" = ["RigidDispX", "RigidDispY", "RigidDispZ"] for rigid displacement * "RigidRot" = ["RigidRotX", "RigidRotY", "RigidRotZ"] for rigid rotation - If several RigidTime constraints are used with the same problem, all the previous + If several RigidTie constraints are used with the same problem, all the previous variables will contains several dof, the indice of the dof being associated to order in which the constraint has been added. For instance, pb.global_dof['RigidDispX'][0] will be associated to the first @@ -33,11 +35,17 @@ class RigidTie(BCBase): ---------- list_nodes: list (int) or 1d np.array List of nodes that will be eliminated considering a rigid body tie. - center: int of np.array[float] with shape = (3), optional + center: int or np.array[float] with shape = (3), optional If center is an int, the rotation center will be initialized at the coordinates of the corresponding node in the mesh. If center is an array (or list, ...) it contains the initial coordinates of the rotation center. By default, the - center is set at the midle of the rigid sets. + center is set at the middle of the rigid sets. + use_quaternion: bool, optional + If True (default), use a multiplicative quaternion update to avoid + gimbal lock for large rotations. The rotation DOFs (RigidRotX/Y/Z) + are interpreted as incremental Euler angles relative to a quaternion + base state that is updated at each converged increment. + If False, use the original Euler angle formulation. name : str, optional Name of the created boundary condition. The default is "Rigid Tie". @@ -46,14 +54,27 @@ class RigidTie(BCBase): ----------------------- 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 + rotation around Y' (Y' being the new Y after rotation around X) + and finally the rotation around Z" (Z" being 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. + When ``use_quaternion=True`` (default), the total rotation is stored as a + quaternion (via ``simcoon.Rotation``) using a multiplicative update: + + - The Euler angle DOFs represent a **small incremental** rotation from + the current quaternion base state ``Q_base``. + - At each converged increment, the increment is composed: + ``Q_base = Rotation.from_euler("XYZ", delta) * Q_base`` + - The total rotation is exact for arbitrarily large angles — no gimbal + lock, no small-angle approximation. + - The rotation derivatives ``dR/d(angle)`` used in the MPC linearization + are evaluated at the small increment (well-conditioned), then composed + with ``R_base`` via the chain rule for exact consistency. + Notes ----- @@ -81,9 +102,10 @@ class RigidTie(BCBase): rigid_tie = fd.constraint.RigidTie(right_face) """ - def __init__(self, list_nodes, center=None, name="Rigid Tie"): + def __init__(self, list_nodes, center=None, use_quaternion=True, name="Rigid Tie"): self.list_nodes = list_nodes self.center = center + self.use_quaternion = use_quaternion self.bc_type = "RigidTie" BCBase.__init__(self, name) self._keep_at_end = True @@ -140,56 +162,60 @@ def initialize(self, problem): + self.list_nodes[:, None] ) - def generate(self, problem, t_fact=1, t_fact_old=None): - mesh = problem.mesh - var_cd = self.var_cd - node_cd = self.node_cd - list_nodes = self.list_nodes + # Quaternion base state for multiplicative rotation update + if self.use_quaternion: + self._Q_base = Rotation.identity() + self._Q_base_backup = Rotation.identity() + self._angles_at_base = np.zeros(3) + def _get_dof_ref(self, problem): + """Read current values of the 6 rigid DOFs from the problem.""" dof_cd = [ problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) + + problem._global_dof.indice_start(self.var_cd[i]) + + self.node_cd[i] + for i in range(6) ] - - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(6), dof_cd + return np.array([xbc[dof] for dof in dof_cd]), dof_cd else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) - - disp_ref = dof_ref[:3] # reference displacement - angles = dof_ref[3:] # rotation angle - - 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]]] ) - - # Correct displacement of slave nodes to be consistent with the master nodes - new_disp = ( - (mesh.nodes[list_nodes] - self.center) @ R.T - + self.center - + disp_ref - - mesh.nodes[list_nodes] - ) - - if not (np.array_equal(problem._dU, 0)): - if np.array_equal(problem._U, 0): - problem._dU[self._disp_indices] = new_disp - else: - problem._dU[self._disp_indices] = ( - new_disp - problem._U[self._disp_indices] - ) + if np.isscalar(xbc) and xbc == 0: + return np.array([dof_sol[dof] for dof in dof_cd]), dof_cd + return np.array([dof_sol[dof] + xbc[dof] for dof in dof_cd]), dof_cd + + def _compute_rotation(self, angles): + """Compute total rotation matrix and Euler derivatives. + + When use_quaternion=True, the rotation is composed multiplicatively: + R_total = R_inc(delta) * Q_base + where delta = angles - angles_at_base is always small. + + Returns R (3x3), dR_drx, dR_dry, dR_drz (3x3 each). + """ + if self.use_quaternion and hasattr(self, "_Q_base"): + delta = angles - self._angles_at_base + if np.any(np.isnan(delta)) or np.any(np.isinf(delta)): + delta = np.zeros(3) + R_inc = Rotation.from_euler("XYZ", delta) + R_total = R_inc * self._Q_base + R = R_total.as_matrix() + R_base_mat = self._Q_base.as_matrix() + + # Derivatives of R_inc at delta, composed with R_base + sin = np.sin(delta) + cos = np.cos(delta) + else: + R = Rotation.from_euler("XYZ", angles).as_matrix() + R_base_mat = np.eye(3) + sin = np.sin(angles) + cos = np.cos(angles) - # approche incrémentale: - dR_drx = np.array( + # Euler XYZ derivatives (same formulas, evaluated at delta or angles) + dRinc_drx = np.array( [ [0, 0, 0], [ @@ -204,24 +230,14 @@ def generate(self, problem, t_fact=1, t_fact_old=None): ], ] ) - - dR_dry = np.array( + dRinc_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], - ], + [-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( + dRinc_drz = np.array( [ [-cos[1] * sin[2], -cos[1] * cos[2], 0], [ @@ -237,22 +253,105 @@ def generate(self, problem, t_fact=1, t_fact_old=None): ] ) + # Chain rule: dR_total/d(angle_i) = dR_inc/d(angle_i) @ R_base + dR_drx = dRinc_drx @ R_base_mat + dR_dry = dRinc_dry @ R_base_mat + dR_drz = dRinc_drz @ R_base_mat + + return R, dR_drx, dR_dry, dR_drz + + def _compute_slave_disp(self, problem, disp_ref, R): + """Compute and write slave node displacements from rigid body state.""" + mesh = problem.mesh + list_nodes = self.list_nodes + new_disp = ( + (mesh.nodes[list_nodes] - self.center) @ R.T + + self.center + + disp_ref + - mesh.nodes[list_nodes] + ) + if not np.array_equal(problem._dU, 0): + if np.array_equal(problem._U, 0): + problem._dU[self._disp_indices] = new_disp + else: + problem._dU[self._disp_indices] = ( + new_disp - problem._U[self._disp_indices] + ) + return new_disp + + def pre_update(self, problem): + """Refresh slave node positions in _dU before assembly update. + + Called by the solver BEFORE assembly.update() so that other assemblies + (e.g. IPCContact) see the correct geometry of the rigid body surface. + """ + dof_ref, _ = self._get_dof_ref(problem) + disp_ref = dof_ref[:3] + angles = dof_ref[3:] + R, _, _, _ = self._compute_rotation(angles) + self._compute_slave_disp(problem, disp_ref, R) + + def set_start(self, problem): + """Absorb the converged incremental rotation into the quaternion base. + + Called by the solver after a converged increment, before _dU is reset. + """ + if not self.use_quaternion or not hasattr(self, "_Q_base"): + return + dof_ref, _ = self._get_dof_ref(problem) + angles = dof_ref[3:] + if np.any(np.isnan(angles)) or np.any(np.isinf(angles)): + self._Q_base_backup = self._Q_base + return + delta = angles - self._angles_at_base + if not np.allclose(delta, 0, atol=1e-15): + R_inc = Rotation.from_euler("XYZ", delta) + self._Q_base_backup = self._Q_base + self._Q_base = R_inc * self._Q_base + self._angles_at_base = angles.copy() + else: + self._Q_base_backup = self._Q_base + + def to_start_bc(self, problem): + """Revert the quaternion base after a failed increment. + + Called by the solver when an increment does not converge and + the state must be rolled back. + """ + if not self.use_quaternion: + return + self._Q_base = self._Q_base_backup + + @property + def Q_total(self): + """Current total rotation as a Rotation object (quaternion-backed).""" + if self.use_quaternion: + return self._Q_base + return None + + def generate(self, problem, t_fact=1, t_fact_old=None): + mesh = problem.mesh + var_cd = self.var_cd + node_cd = self.node_cd + list_nodes = self.list_nodes + + dof_ref, dof_cd = self._get_dof_ref(problem) + disp_ref = dof_ref[:3] + angles = dof_ref[3:] + + # Compute rotation and derivatives + R, dR_drx, dR_dry, dR_drz = self._compute_rotation(angles) + + # Set slave node displacements + self._compute_slave_disp(problem, disp_ref, R) + + # MPC linearization 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_drz = crd @ dR_drz.T #### MPC #### - - # dU - dU_ref - du_drx*drx_ref - du_dry*dry_ref - du_drz*drz_ref = 0 - # with shapes: dU, du_drx, ... -> (nnodes, nvar) - dU_ref -> (nvar), drx_ref, ... -> scalar - # dU are associated to eliminated dof and should be different than ref dof - # or - # dUx - dUx_ref - du_drx[:,0]*drx_ref - du_dry[:,0]*dry_ref - du_drz[:,0]*drz_ref = 0 - # dUy - dUy_ref - du_drx[1]*drx_ref - du_dry[1]*dry_ref - du_drz[1]*drz_ref = 0 - # dUz - dUz_ref - du_drx[2]*drx_ref - du_dry[2]*dry_ref - du_drz[2]*drz_ref = 0 res = ListBC() res.append( MPC( diff --git a/fedoo/problem/nl_newmark.py b/fedoo/problem/nl_newmark.py index 17aa42d3..e4a5743d 100644 --- a/fedoo/problem/nl_newmark.py +++ b/fedoo/problem/nl_newmark.py @@ -52,28 +52,35 @@ def __init__( self.__Velocity = 0 self.__Acceleration = 0 + def _get_damping_matrix(self): + """Compute the damping matrix C from Rayleigh model or explicit assembly. + + Returns None if no damping is configured. + Rayleigh damping: C = alpha * M + beta * K + """ + if self.__RayleighDamping is not None: + return ( + self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() + + self.__RayleighDamping[1] + * self.__StiffnessAssembly.get_global_matrix() + ) + elif self.__DampingAssembly is not None: + return self.__DampingAssembly.get_global_matrix() + return None + def updateA(self): # internal function to be used when modifying M, K or C dt = self.dtime - if self.__DampingAssembly is None: - self.set_A( - self.__StiffnessAssembly.get_global_matrix() - + 1 / (self.__Beta * (dt**2)) * self.__MassAssembly.get_global_matrix() - ) - else: - if self.__RayleighDamping is not None: - # In this case, self.__RayleighDamping = [alpha, beta] - DampMatrix = ( - self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() - + self.__RayleighDamping[1] - * self.__StiffnessAssembly.get_global_matrix() - ) - else: - DampMatrix = self.__DampingAssembly.get_global_matrix() + K = self.__StiffnessAssembly.get_global_matrix() + M = self.__MassAssembly.get_global_matrix() + C = self._get_damping_matrix() + if C is None: + self.set_A(K + 1 / (self.__Beta * (dt**2)) * M) + else: self.set_A( - self.__StiffnessAssembly.get_global_matrix() - + 1 / (self.__Beta * (dt**2)) * self.__MassAssembly.get_global_matrix() - + self.__Gamma / (self.__Beta * dt) * DampMatrix + K + + 1 / (self.__Beta * (dt**2)) * M + + self.__Gamma / (self.__Beta * dt) * C ) def updateD(self, start=False): @@ -87,7 +94,6 @@ def updateD(self, start=False): self.set_D(0) return else: - # DeltaDisp = self._NonLinear__TotalDisplacementOld - self._NonLinear__TotalDisplacementStart DeltaDisp = self._dU D = ( @@ -99,32 +105,17 @@ def updateD(self, start=False): ) + self.__StiffnessAssembly.get_global_vector() ) - if self.__DampingAssembly is not None: - if self.__RayleighDamping is not None: - # In this case, self.__RayleighDamping = [alpha, beta] - DampMatrix = ( - self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() - + self.__RayleighDamping[1] - * self.__StiffnessAssembly.get_global_matrix() - ) - else: - DampMatrix = self.__DampingAssembly.get_global_matrix() - - assert 0, "Non linear Dynamic problem with damping needs to be checked" - # need to be cheched - - # new_velocity = dt * 0.5*(2 - self.gamma/self.beta)*acceleration +(ou -) - # self.gamma/(self.beta*dt)) * delta_disp - # (1 - self.gamma/(self.beta))*velocity - - # D += DampMatrix * (-new_velocity) - # check if same as below - - D += DampMatrix * ( - (self.__Gamma / (self.__Beta * dt)) * DisplacementStart - + (self.__Gamma / self.__Beta - 1) * self.__Velocity - + (0.5 * dt * (self.__Gamma / self.__Beta - 2)) * self.__Acceleration + + # Damping contribution: D += -C * v_new + # where v_new = (1 - γ/β)*V + (γ/(β*dt))*ΔU + dt*(1 - γ/(2β))*A + C = self._get_damping_matrix() + if C is not None: + new_velocity = ( + (1 - self.__Gamma / self.__Beta) * self.__Velocity + + (self.__Gamma / (self.__Beta * dt)) * DeltaDisp + + dt * (1 - self.__Gamma / (2 * self.__Beta)) * self.__Acceleration ) + D += C @ (-new_velocity) self.set_D(D) @@ -136,9 +127,15 @@ def initialize(self): self.__DampingAssembly.initialize(self) def to_start(self): - self._dU = 0 + # Notify constraints of reverted increment + for bc in self.bc: + if hasattr(bc, "to_start_bc"): + bc.to_start_bc(self) - self._err0 = self.nr_parameters["err0"] # initial error for NR error estimation + self._dU = 0 + self._nr_min_subiter = 0 + self._t_fact_inc = None + self._err0 = self.nr_parameters["err0"] self.__MassAssembly.to_start(self) self.__StiffnessAssembly.to_start(self) @@ -146,36 +143,38 @@ def to_start(self): # self.__DampingAssembly.to_start() def set_start(self, save_results=False, callback=None): - dt = self.dtime ### dt is the time step of the previous increment + dt = self.dtime + self._nr_min_subiter = 0 if not (np.isscalar(self._dU) and self._dU == 0): + # Notify constraints of accepted increment (before _dU is reset) + for bc in self.bc: + if hasattr(bc, "set_start"): + bc.set_start(self) + # update velocity and acceleration - NewAcceleration = (1 / self.__Beta / (dt**2)) * ( + new_acceleration = (1 / self.__Beta / (dt**2)) * ( self._dU - dt * self.__Velocity ) - 1 / self.__Beta * (0.5 - self.__Beta) * self.__Acceleration self.__Velocity += dt * ( (1 - self.__Gamma) * self.__Acceleration - + self.__Gamma * NewAcceleration + + self.__Gamma * new_acceleration ) - self.__Acceleration = NewAcceleration + self.__Acceleration = new_acceleration + + # Save results and callback BEFORE _dU is reset + if save_results: + self.save_results(self._NonLinearBase__compteurOutput) + self._NonLinearBase__compteurOutput += 1 + if callback is not None: + if self.exec_callback_at_each_iter or save_results: + callback(self) self._U += self._dU self._dU = 0 - self._err0 = self.nr_parameters["err0"] # initial error for NR error estimation + self._err0 = self.nr_parameters["err0"] self.__MassAssembly.set_start(self) self.__StiffnessAssembly.set_start(self) - # if self.__DampingAssembly is not None: - # self.__DampingAssembly.set_start(self,dt) - - # Save results - if not (np.isscalar(self._dU) and self._dU == 0): - if save_results: - self.save_results(self.__compteurOutput) - self.__compteurOutput += 1 - - if callback is not None: - if self.exec_callback_at_each_iter or save_results: - callback(self) # def NewTimeIncrement(self, dt): # ### dt is the time step of the previous increment @@ -201,6 +200,12 @@ def update(self, compute="all", updateWeakForm=True): - Change in constitutive law (internal variable) Update the problem with the new assembled global matrix and global vector """ + # Pre-update hook: refresh slave positions (e.g. RigidTie) + if self.bc._update_during_inc: + for bc in self.bc: + if hasattr(bc, "pre_update"): + bc.pre_update(self) + if updateWeakForm == True: self.__StiffnessAssembly.update(self, compute) # self.__MassAssembly.update(self,dt,compute) diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index 23f26b9b..664f38d6 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -168,6 +168,11 @@ def set_start(self, save_results=False, callback=None): # dt not used for static problem self._nr_min_subiter = 0 # reset SDI for new increment if not (np.isscalar(self._dU) and self._dU == 0): + # Notify constraints of accepted increment (before _dU is reset) + for bc in self.bc: + if hasattr(bc, "set_start"): + bc.set_start(self) + self._U += self._dU self._dU = 0 self._err0 = self.nr_parameters[ @@ -190,6 +195,11 @@ def set_start(self, save_results=False, callback=None): self.__assembly.set_start(self) def to_start(self): + # Notify constraints of reverted increment + for bc in self.bc: + if hasattr(bc, "to_start_bc"): + bc.to_start_bc(self) + self._dU = 0 self._nr_min_subiter = 0 self._t_fact_inc = None @@ -218,6 +228,14 @@ def update(self, compute="all", updateWeakForm=True): - Change in constitutive law (internal variable) Don't Update the problem with the new assembled global matrix and global vector -> use UpdateA and UpdateD method for this purpose """ + # Pre-update hook: refresh slave positions (e.g. RigidTie) before + # assembly update so that other assemblies (e.g. IPCContact) see + # the correct geometry. + if self.bc._update_during_inc: + for bc in self.bc: + if hasattr(bc, "pre_update"): + bc.pre_update(self) + if updateWeakForm == True: self.__assembly.update(self, compute) else: From a18c15d69982a0728395a17307899a20f6a88ce0 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Mon, 30 Mar 2026 00:46:23 +0200 Subject: [PATCH 02/21] Use rotation vectors and exact derivatives MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace Euler-XYZ incremental rotations with rotation-vector (exponential map) usage across examples and core constraint code. Instances of Rotation.from_euler(..., "XYZ") were changed to Rotation.from_rotvec(...), and RigidTie now provides _dR_drotvec(...) to compute exact dR/dω derivatives (Rodrigues / Gallego & Yezzi) which are chained with the quaternion base. This removes Euler ordering and small-angle approximations, ensuring correct composition with Q_base and accurate linearization for MPC/constraint computations. --- examples/rigid_body_bunny_bounce.py | 2 +- examples/rigid_body_cow_bounce.py | 2 +- fedoo/constraint/rigid_body.py | 2 +- fedoo/constraint/rigid_tie.py | 146 +++++++++++++++++----------- 4 files changed, 94 insertions(+), 58 deletions(-) diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index 6fadac1b..5f4f60a5 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -140,7 +140,7 @@ def collect(t, q, v): angles = q_i[3:] # Apply full rigid body transform: rotate around center, then translate - R = Rotation.from_euler("XYZ", angles).as_matrix() + R = Rotation.from_rotvec(angles).as_matrix() pts_rotated = (pts_ref - center) @ R.T + center + disp vis.points[:] = pts_rotated vis.GetPoints().Modified() diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index b599e3c8..278d5ef7 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -140,7 +140,7 @@ def collect(t, q, v): for i in frame_indices: qi = q_hist[i] - R = Rotation.from_euler("XYZ", qi[3:]).as_matrix() + R = Rotation.from_rotvec(qi[3:]).as_matrix() vis_cow.points[:] = (pts_ref - center) @ R.T + center + qi[:3] vis_cow.GetPoints().Modified() pl.add_text( diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 5c776cc0..88306cd4 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -639,7 +639,7 @@ def solve(self, dt, tmax, t0=0, callback=None, print_info=True): # Absorb rotation into quaternion base delta = q[3:] - rt._angles_at_base if np.any(np.abs(delta) > 1e-14): - rt._Q_base = Rotation.from_euler("XYZ", delta) * rt._Q_base + rt._Q_base = Rotation.from_rotvec(delta) * rt._Q_base rt._angles_at_base = q[3:].copy() # Adaptive barrier stiffness via ipctk heuristics diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index f26c9154..256529b0 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -68,7 +68,7 @@ class RigidTie(BCBase): - The Euler angle DOFs represent a **small incremental** rotation from the current quaternion base state ``Q_base``. - At each converged increment, the increment is composed: - ``Q_base = Rotation.from_euler("XYZ", delta) * Q_base`` + ``Q_base = Rotation.from_rotvec(delta) * Q_base`` - The total rotation is exact for arbitrarily large angles — no gimbal lock, no small-angle approximation. - The rotation derivatives ``dR/d(angle)`` used in the MPC linearization @@ -188,77 +188,113 @@ def _get_dof_ref(self, problem): return np.array([dof_sol[dof] + xbc[dof] for dof in dof_cd]), dof_cd def _compute_rotation(self, angles): - """Compute total rotation matrix and Euler derivatives. + """Compute total rotation matrix and derivatives w.r.t. rotation DOFs. - When use_quaternion=True, the rotation is composed multiplicatively: - R_total = R_inc(delta) * Q_base - where delta = angles - angles_at_base is always small. + When ``use_quaternion=True``, the rotation is composed multiplicatively: + ``R_total = R_inc(delta) * Q_base`` where ``delta`` is always small. - Returns R (3x3), dR_drx, dR_dry, dR_drz (3x3 each). + The rotation DOFs are **rotation vector** components (exponential map): + ``rotvec = theta * axis``. This is convention-free (no XYZ ordering) + and consistent with Fedoo's beam elements. + + Derivatives ``dR/dω_k`` use the exact Rodrigues differentiation + (Gallego & Yezzi 2015). + + Returns R (3x3), dR_dw0, dR_dw1, dR_dw2 (3x3 each). """ if self.use_quaternion and hasattr(self, "_Q_base"): delta = angles - self._angles_at_base if np.any(np.isnan(delta)) or np.any(np.isinf(delta)): delta = np.zeros(3) - R_inc = Rotation.from_euler("XYZ", delta) + R_inc = Rotation.from_rotvec(delta) R_total = R_inc * self._Q_base R = R_total.as_matrix() R_base_mat = self._Q_base.as_matrix() - - # Derivatives of R_inc at delta, composed with R_base - sin = np.sin(delta) - cos = np.cos(delta) + omega = delta else: - R = Rotation.from_euler("XYZ", angles).as_matrix() + R = Rotation.from_rotvec(angles).as_matrix() R_base_mat = np.eye(3) - sin = np.sin(angles) - cos = np.cos(angles) + omega = angles - # Euler XYZ derivatives (same formulas, evaluated at delta or angles) - dRinc_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], - ], - ] - ) - dRinc_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]], - ] - ) - dRinc_drz = np.array( + # Exact derivatives dR_inc/dω_k via Rodrigues differentiation + dRinc = self._dR_drotvec(omega) + + # Chain rule: dR_total/dω_k = dR_inc/dω_k @ R_base + dR_dw0 = dRinc[0] @ R_base_mat + dR_dw1 = dRinc[1] @ R_base_mat + dR_dw2 = dRinc[2] @ R_base_mat + + return R, dR_dw0, dR_dw1, dR_dw2 + + @staticmethod + def _dR_drotvec(omega): + """Exact derivatives of R(ω) w.r.t. rotation vector components. + + Uses the Rodrigues formula differentiation: + ``R = I + (sin θ)/θ [ω]× + (1-cos θ)/θ² [ω]ײ`` + + For small ``||ω|| < 1e-10``, returns ``dR/dωₖ ≈ [eₖ]×`` (skew of + unit vector), which is exact in the limit. + + Parameters + ---------- + omega : ndarray (3,) + Rotation vector. + + Returns + ------- + dR : tuple of 3 ndarrays (3x3) + (dR/dω₀, dR/dω₁, dR/dω₂). + + References + ---------- + Gallego & Yezzi, "A Compact Formula for the Derivative of a 3-D + Rotation in Exponential Coordinates", J. Math. Imaging Vis., 2015. + """ + theta = np.linalg.norm(omega) + + # Skew-symmetric matrix of omega + W = 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, - ], + [0, -omega[2], omega[1]], + [omega[2], 0, -omega[0]], + [-omega[1], omega[0], 0], ] ) - # Chain rule: dR_total/d(angle_i) = dR_inc/d(angle_i) @ R_base - dR_drx = dRinc_drx @ R_base_mat - dR_dry = dRinc_dry @ R_base_mat - dR_drz = dRinc_drz @ R_base_mat + if theta < 1e-10: + # Small angle limit: dR/dωₖ = [eₖ]× + return ( + np.array([[0, 0, 0], [0, 0, -1], [0, 1, 0]]), + np.array([[0, 0, 1], [0, 0, 0], [-1, 0, 0]]), + np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]), + ) + + s, c = np.sin(theta), np.cos(theta) + t2 = theta * theta + + # Rodrigues coefficients and their derivatives w.r.t. θ + a = s / theta # sin(θ)/θ + b = (1 - c) / t2 # (1-cos(θ))/θ² + da = (c * theta - s) / t2 # d(sin(θ)/θ)/dθ + db = (s * theta - 2 * (1 - c)) / (t2 * theta) # d((1-cos(θ))/θ²)/dθ + + W2 = W @ W + e = np.eye(3) + dR = [] + for k in range(3): + # dW/dωₖ = [eₖ]× + dW = np.array( + [[0, -e[k, 2], e[k, 1]], [e[k, 2], 0, -e[k, 0]], [-e[k, 1], e[k, 0], 0]] + ) + # dW²/dωₖ = dW @ W + W @ dW + dW2 = dW @ W + W @ dW + # dθ/dωₖ = ωₖ / θ + dtk = omega[k] / theta + + dR.append(da * dtk * W + a * dW + db * dtk * W2 + b * dW2) - return R, dR_drx, dR_dry, dR_drz + return tuple(dR) def _compute_slave_disp(self, problem, disp_ref, R): """Compute and write slave node displacements from rigid body state.""" @@ -305,7 +341,7 @@ def set_start(self, problem): return delta = angles - self._angles_at_base if not np.allclose(delta, 0, atol=1e-15): - R_inc = Rotation.from_euler("XYZ", delta) + R_inc = Rotation.from_rotvec(delta) self._Q_base_backup = self._Q_base self._Q_base = R_inc * self._Q_base self._angles_at_base = angles.copy() From 6ef4ac93ca0cd2755cd2b99eeedcbe2179fd758f Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Wed, 15 Apr 2026 15:52:38 +0200 Subject: [PATCH 03/21] Rigid body assembly & examples: switch to NonLinear Replace the old direct 6x6 Newmark integrator with Fedoo's NonLinear-based workflow and update example scripts accordingly. Key changes: - RigidBodyAssembly: added Newmark parameters (beta, gamma), Rayleigh alpha, and 6-DOF Newmark state variables; implemented mass matrix helper, effective dynamic stiffness assembly (K_eff), residual computation, update(), set_start(), and to_start() to integrate rigid DOFs into the NonLinear solver flow (including IPC contact updates and damping). - RigidBody: solve() now wraps/creates a NonLinear problem and calls nlsolve; removed the bespoke 6x6 integrator and related CCD/line-search code. set_rayleigh_damping now syncs alpha into the assembly. - Examples (sphere, bunny, cow, freefall): migrated to use the NonLinear problem or return problem from body.solve; simplified mesh creation (fd.Mesh.from_pyvista), triangulation of planes, consistent animation output, and added try/except to import Rotation (simcoon or scipy fallback). Various cleanups to prints, timings, and frame handling. - Constitutivelaw: guard in ElasticAnisotropic to handle uninitialized/zero total_strain (avoid errors on first Newmark step). Overall this integrates rigid-body Newmark time integration into Fedoo's NonLinear solver, centralizes state management, and updates examples to the new API and improved mesh/animation handling. --- examples/rigid_body_bounce_ipc.py | 133 +++---- examples/rigid_body_bunny_bounce.py | 170 ++++----- examples/rigid_body_cow_bounce.py | 112 +++--- examples/rigid_body_freefall.py | 158 ++------ fedoo/constitutivelaw/elastic_anisotropic.py | 5 + fedoo/constraint/rigid_body.py | 357 +++++++------------ 6 files changed, 309 insertions(+), 626 deletions(-) diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index 36cf7a02..851478c6 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -1,7 +1,7 @@ -"""Rigid body bouncing on a plane — 6x6 solver + IPC contact + damping. +"""Sphere bouncing on a plane — NonLinear + IPC contact + Rayleigh damping. -Uses RigidBody.solve() for direct 6-DOF Newmark integration. -Comparison with analytical elastic bounce. +Uses RigidBody with Newmark integration built into the assembly, +solved by Fedoo's standard NonLinear solver. """ import sys @@ -12,6 +12,11 @@ sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + g = 9.81 mass = 1.0 radius = 0.1 @@ -20,92 +25,70 @@ t_end = 2.0 print("=" * 60) -print("SPHERE BOUNCE — 6x6 Newmark + IPC + Rayleigh damping") +print("SPHERE BOUNCE — NonLinear + IPC + Rayleigh") print(f" m={mass}kg, z0={z0}m, r={radius}m, dt={dt}s") print("=" * 60) -# Modeling space space = fd.ModelingSpace("3D") space.new_variable("DispX") space.new_variable("DispY") space.new_variable("DispZ") space.new_vector("Disp", ("DispX", "DispY", "DispZ")) -# Meshes -pv_ball = pv.Sphere( - radius=radius, center=(0, 0, z0), theta_resolution=10, phi_resolution=10 +ball_mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=radius, center=(0, 0, z0), theta_resolution=10, phi_resolution=10) ) -ball_mesh = fd.Mesh.from_pyvista(pv_ball) -pv_plane = pv.Plane( - center=(0, 0, 0), - direction=(0, 0, 1), - i_size=1.5, - j_size=1.5, - i_resolution=6, - j_resolution=6, +plane_mesh = fd.Mesh.from_pyvista( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, + ).triangulate() ) -plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) -# Rigid body -inertia = (2 / 5) * mass * radius**2 * np.eye(3) body = fd.constraint.RigidBody( - ball_mesh, mass=mass, inertia_tensor=inertia, center_of_mass=np.array([0, 0, z0]) + ball_mesh, + mass=mass, + inertia_tensor=(2 / 5) * mass * radius**2 * np.eye(3), + center_of_mass=np.array([0, 0, z0]), ) body.set_force([0, 0, -mass * g]) body.set_rayleigh_damping(1.0) body.enable_ipc_contact(plane_mesh, dhat=0.01, kappa=1e8) -print(f" Ball: {ball_mesh.n_nodes} nodes, IPC kappa=1e8, Rayleigh alpha=1.0") +# Solve using NonLinear via manual time stepping for trajectory collection +pb = fd.problem.NonLinear(body.assembly) +body.add_to_problem(pb) +pb.initialize() -# Solve with 6x6 direct solver +idx = body.assembly._dof_indices z_hist = [z0] t_hist = [0.0] - -def collect(t, q, v): - z_hist.append(z0 + q[2]) - t_hist.append(t) - - t0 = time.time() -q, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) -elapsed = time.time() - t0 +n_steps = int(round(t_end / dt)) +for step in range(n_steps): + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + dof = pb.get_dof_solution() + z_hist.append(z0 + dof[idx[2]]) + t_hist.append((step + 1) * dt) +elapsed = time.time() - t0 t_hist = np.array(t_hist) z_hist = np.array(z_hist) print( - f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" ) print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") - -# Analytical elastic bounce -def analytical_bounce(t_arr, z0, g, r): - z = np.empty_like(t_arr) - zc, vc, tc = z0, 0.0, 0.0 - for i, t in enumerate(t_arr): - dt_l = t - tc - z_val = zc + vc * dt_l - 0.5 * g * dt_l**2 - v_val = vc - g * dt_l - if z_val <= r and v_val < 0: - disc = vc**2 + 2 * g * (zc - r) - if disc >= 0: - dt_c = (vc + np.sqrt(disc)) / g - vc = -(vc - g * dt_c) - zc, tc = r, tc + dt_c - dt_l = t - tc - z_val = zc + vc * dt_l - 0.5 * g * dt_l**2 - z[i] = z_val - return z - - -z_analytical = analytical_bounce(t_hist, z0, g, radius) - -# PyVista animation -print("\nGenerating PyVista animation...") +# Animation out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" gif_path = f"{out_dir}/rigid_body_bounce_ipc.gif" - fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) @@ -134,7 +117,7 @@ def analytical_bounce(t_arr, z0, g, r): sphere.points[:] = pts_ref + np.array([[0, 0, z_hist[i] - z0]]) sphere.GetPoints().Modified() pl.add_text( - f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m 6x6 Newmark+IPC", + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m NonLinear+IPC", position="upper_edge", font_size=11, color="black", @@ -145,37 +128,3 @@ def analytical_bounce(t_arr, z0, g, r): pl.close() print(f" Saved: {gif_path}") - -# Plot -try: - import matplotlib.pyplot as plt - - fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 7), sharex=True) - ax1.plot(t_hist, z_hist, "b-", linewidth=1, label="Fedoo 6x6 (IPC+Rayleigh)") - ax1.plot( - t_hist, - z_analytical, - "r--", - linewidth=0.8, - alpha=0.6, - label="Analytical elastic", - ) - ax1.axhline( - y=radius, color="grey", linestyle=":", alpha=0.5, label=f"contact z={radius}m" - ) - ax1.set_ylabel("z (m)") - ax1.set_title( - f"Sphere bounce — {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" - ) - ax1.legend() - ax1.grid(True, alpha=0.3) - ax2.plot(t_hist, z_hist - z_analytical, "g-", linewidth=0.8) - ax2.set_ylabel("z_fedoo - z_analytical (m)") - ax2.set_xlabel("t (s)") - ax2.grid(True, alpha=0.3) - plt.tight_layout() - plt.savefig(f"{out_dir}/rigid_body_bounce_ipc.png", dpi=150) - plt.close() - print(f" Saved: {out_dir}/rigid_body_bounce_ipc.png") -except ImportError: - pass diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index 5f4f60a5..9afd8182 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -1,8 +1,7 @@ -"""Stanford bunny (convex hull) bouncing on a plane — 6x6 solver + IPC. +"""Bunny bouncing on a plane — NonLinear + IPC contact. -The bunny_coarse mesh has 658 open edges, so we use its convex hull -(watertight, manifold). The shape is simplified but still asymmetric, -producing interesting tumbling on impact. +Convex hull of Stanford bunny (watertight). Demonstrates tumbling +on impact with asymmetric geometry. """ import sys @@ -13,12 +12,17 @@ sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + g = 9.81 dt = 5e-4 t_end = 2.0 print("=" * 60) -print("BUNNY BOUNCE — 6x6 Newmark + IPC contact") +print("BUNNY BOUNCE — NonLinear + IPC contact") print("=" * 60) space = fd.ModelingSpace("3D") @@ -27,45 +31,41 @@ space.new_variable("DispZ") space.new_vector("Disp", ("DispX", "DispY", "DispZ")) -# Build watertight bunny from convex hull of decimated full bunny -pv_raw = pv.examples.download_bunny().decimate(0.95) +# Watertight bunny from convex hull +pv_raw = pv.examples.download_bunny().decimate(0.97) pv_bunny = pv_raw.delaunay_3d().extract_surface().triangulate().clean() - -# Scale and position -bounds = pv_bunny.bounds -size = max(bounds[1] - bounds[0], bounds[3] - bounds[2], bounds[5] - bounds[4]) -pv_bunny = pv_bunny.scale(0.25 / size, inplace=False) +s = max( + pv_bunny.bounds[1] - pv_bunny.bounds[0], + pv_bunny.bounds[3] - pv_bunny.bounds[2], + pv_bunny.bounds[5] - pv_bunny.bounds[4], +) +pv_bunny = pv_bunny.scale(0.25 / s, inplace=False) pv_bunny = pv_bunny.translate( - [-pv_bunny.center[0], -pv_bunny.center[1], -pv_bunny.bounds[4] + 0.4], - inplace=False, + [-pv_bunny.center[0], -pv_bunny.center[1], -pv_bunny.bounds[4] + 0.4], inplace=False ) pv_bunny = pv_bunny.compute_normals(consistent_normals=True, auto_orient_normals=True) bunny_mesh = fd.Mesh.from_pyvista(pv_bunny) -print(f" Bunny: {bunny_mesh.n_nodes} nodes, {bunny_mesh.n_elements} faces") -print(f" Manifold: {pv_bunny.is_manifold}, Open edges: {pv_bunny.n_open_edges}") - -# Plane -pv_plane = pv.Plane( - center=(0, 0, 0), - direction=(0, 0, 1), - i_size=1.5, - j_size=1.5, - i_resolution=6, - j_resolution=6, +plane_mesh = fd.Mesh.from_pyvista( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, + ).triangulate() ) -plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) -# Rigid body — box inertia approximation mass = 0.5 bb = pv_bunny.bounds lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] -I_approx = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) +I = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) body = fd.constraint.RigidBody( bunny_mesh, mass=mass, - inertia_tensor=I_approx, + inertia_tensor=I, center_of_mass=np.array(pv_bunny.center), name="Bunny", ) @@ -73,107 +73,79 @@ body.set_rayleigh_damping(1.0) body.enable_ipc_contact(plane_mesh, dhat=0.008, kappa=1e8) -print(f" mass={mass}kg, center_z={body.center_of_mass[2]:.3f}m") -print(f" dt={dt}s → {int(t_end/dt)} steps") +print(f" Bunny: {bunny_mesh.n_nodes} nodes, mass={mass}kg") + +# Solve with manual loop for trajectory +pb = fd.problem.NonLinear(body.assembly) +body.add_to_problem(pb) +pb.initialize() -# Solve — store full q (6 DOFs) for rotation animation +idx = body.assembly._dof_indices q_hist = [np.zeros(6)] t_hist = [0.0] - -def collect(t, q, v): - q_hist.append(q.copy()) - t_hist.append(t) - - t0 = time.time() -q_final, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) -elapsed = time.time() - t0 +n_steps = int(round(t_end / dt)) +for step in range(n_steps): + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + dof = pb.get_dof_solution() + q_hist.append(dof[idx].copy()) + t_hist.append((step + 1) * dt) +elapsed = time.time() - t0 t_hist = np.array(t_hist) q_hist = np.array(q_hist) z_hist = body.center_of_mass[2] + q_hist[:, 2] print( - f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + f" {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" ) print(f" z_min={z_hist.min():.4f}m") -print( - f" max rotation (deg): rx={np.degrees(q_hist[:,3].max()):.1f} ry={np.degrees(q_hist[:,4].max()):.1f} rz={np.degrees(q_hist[:,5].max()):.1f}" -) -# Animation -print("\nGenerating PyVista animation...") -out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" -gif_path = f"{out_dir}/rigid_body_bunny_bounce.gif" - -fps = 25 +# Animation (low res for file size) +out = "/Users/ychemisky/Documents/GitHub/fedoo/examples/rigid_body_bunny_bounce.gif" +fps = 15 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) vis = pv_bunny.copy() pts_ref = vis.points.copy() -vis_plane = pv.Plane( - center=(0, 0, 0), - direction=(0, 0, 1), - i_size=1.5, - j_size=1.5, - i_resolution=10, - j_resolution=10, -) +center = body.center_of_mass -pl = pv.Plotter(window_size=[900, 600], off_screen=True) +pl = pv.Plotter(window_size=[600, 400], off_screen=True) pl.set_background("white") -pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=8, + j_resolution=8, + ), + color="lightgrey", + opacity=0.8, + show_edges=True, +) pl.add_mesh(vis, color="sandybrown", smooth_shading=True) pl.camera_position = [(1.0, -1.0, 0.7), (0, 0, 0.2), (0, 0, 1)] -pl.open_gif(gif_path, fps=fps) +pl.open_gif(out, fps=fps) -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation - -center = body.center_of_mass for i in frame_indices: - q_i = q_hist[i] - disp = q_i[:3] - angles = q_i[3:] - - # Apply full rigid body transform: rotate around center, then translate - R = Rotation.from_rotvec(angles).as_matrix() - pts_rotated = (pts_ref - center) @ R.T + center + disp - vis.points[:] = pts_rotated + qi = q_hist[i] + R = Rotation.from_rotvec(qi[3:]).as_matrix() + vis.points[:] = (pts_ref - center) @ R.T + center + qi[:3] vis.GetPoints().Modified() - pl.add_text( - f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m rx={np.degrees(angles[0]):.1f}°", + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m", position="upper_edge", font_size=10, color="black", - name="title", + name="t", ) pl.render() pl.write_frame() pl.close() -print(f" Saved: {gif_path}") - -# Plot -try: - import matplotlib.pyplot as plt - - fig, ax = plt.subplots(figsize=(12, 4)) - ax.plot(t_hist, z_hist, "b-", linewidth=1) - ax.axhline(y=0, color="grey", linestyle=":", alpha=0.5) - ax.set_xlabel("t (s)") - ax.set_ylabel("z (m)") - ax.set_title( - f"Bunny bounce — {bunny_mesh.n_nodes} nodes, {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" - ) - ax.grid(True, alpha=0.3) - plt.tight_layout() - plt.savefig(f"{out_dir}/rigid_body_bunny_bounce.png", dpi=150) - plt.close() - print(f" Saved: {out_dir}/rigid_body_bunny_bounce.png") -except ImportError: - pass +print(f" Saved: {out}") diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 278d5ef7..74c1fbf4 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -1,6 +1,7 @@ -"""Cow bouncing on a plane — 6x6 Newmark + IPC contact. +"""Cow bouncing on a plane — NonLinear + IPC contact. -The cow mesh from PyVista is watertight and manifold (2903 nodes). +The cow mesh from PyVista is watertight (2903 nodes), decimated for speed. +Full rotation via quaternion, rotation vectors (no gimbal lock). """ import sys @@ -11,12 +12,17 @@ sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + g = 9.81 dt = 5e-4 -t_end = 3.0 +t_end = 2.0 print("=" * 60) -print("COW BOUNCE — 6x6 Newmark + IPC contact") +print("COW BOUNCE — NonLinear + IPC contact") print("=" * 60) space = fd.ModelingSpace("3D") @@ -25,26 +31,19 @@ space.new_variable("DispZ") space.new_vector("Disp", ("DispX", "DispY", "DispZ")) -# Cow mesh — watertight, manifold +# Cow mesh pv_cow = pv.examples.download_cow().triangulate().clean() -print( - f" Raw cow: {pv_cow.n_points} pts, manifold={pv_cow.is_manifold}, open_edges={pv_cow.n_open_edges}" +size = max( + pv_cow.bounds[1] - pv_cow.bounds[0], + pv_cow.bounds[3] - pv_cow.bounds[2], + pv_cow.bounds[5] - pv_cow.bounds[4], ) - -# Scale to ~0.3m and position above ground -bounds = pv_cow.bounds -size = max(bounds[1] - bounds[0], bounds[3] - bounds[2], bounds[5] - bounds[4]) pv_cow = pv_cow.scale(0.3 / size, inplace=False) pv_cow = pv_cow.translate( - [-pv_cow.center[0], -pv_cow.center[1], -pv_cow.bounds[4] + 0.5], - inplace=False, + [-pv_cow.center[0], -pv_cow.center[1], -pv_cow.bounds[4] + 0.5], inplace=False ) -# Decimate for speed pv_cow = pv_cow.decimate(0.9).triangulate().clean() -pv_cow = pv_cow.compute_normals(consistent_normals=True, auto_orient_normals=True) - cow_mesh = fd.Mesh.from_pyvista(pv_cow) -print(f" Decimated cow: {cow_mesh.n_nodes} nodes, {cow_mesh.n_elements} faces") # Plane pv_plane = pv.Plane( @@ -57,62 +56,68 @@ ) plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) -# Rigid body with box-approximate inertia +# Rigid body mass = 1.0 bb = pv_cow.bounds lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] -I_approx = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) +I = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) body = fd.constraint.RigidBody( cow_mesh, mass=mass, - inertia_tensor=I_approx, + inertia_tensor=I, center_of_mass=np.array(pv_cow.center), name="Cow", ) body.set_force([0, 0, -mass * g]) body.set_rayleigh_damping(1.5) -body.enable_ipc_contact(plane_mesh, dhat=0.01) # kappa auto-tuned +body.enable_ipc_contact(plane_mesh, dhat=0.01) + +print(f" Cow: {cow_mesh.n_nodes} nodes, mass={mass}kg") -print(f" mass={mass}kg, center_z={body.center_of_mass[2]:.3f}m") -print(f" inertia diag: {np.diag(I_approx)}") -print(f" dt={dt}s → {int(t_end/dt)} steps") +# Solve with NonLinear (manual stepping for trajectory) +pb = fd.problem.NonLinear(body.assembly) +body.add_to_problem(pb) +pb.initialize() -# Solve +idx = body.assembly._dof_indices q_hist = [np.zeros(6)] t_hist = [0.0] - - -def collect(t, q, v): - q_hist.append(q.copy()) - t_hist.append(t) - +n_steps = int(round(t_end / dt)) t0 = time.time() -q, v, a = body.solve(dt=dt, tmax=t_end, callback=collect, print_info=True) -elapsed = time.time() - t0 +for step in range(n_steps): + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + dof = pb.get_dof_solution() + q_hist.append(dof[idx].copy()) + t_hist.append((step + 1) * dt) + if step % 500 == 0: + n_c = ( + len(body.assembly._ipc_collisions) + if body.assembly._ipc_collisions is not None + else 0 + ) + print( + f" t={(step+1)*dt:.2f}s z={body.center_of_mass[2]+dof[idx[2]]:.3f}m contacts={n_c}" + ) +elapsed = time.time() - t0 t_hist = np.array(t_hist) q_hist = np.array(q_hist) z_hist = body.center_of_mass[2] + q_hist[:, 2] - print( - f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" ) print(f" z_min={z_hist.min():.4f}m") -print( - f" max rotation: rx={np.degrees(q_hist[:,3].max()):.0f}° ry={np.degrees(q_hist[:,4].max()):.0f}° rz={np.degrees(q_hist[:,5].max()):.0f}°" -) # Animation -print("\nGenerating PyVista animation...") out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" gif_path = f"{out_dir}/rigid_body_cow_bounce.gif" - fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) -print(f" {len(frame_indices)} frames") vis_cow = pv_cow.copy() pts_ref = vis_cow.points.copy() @@ -133,11 +138,6 @@ def collect(t, q, v): pl.camera_position = [(1.5, -1.5, 1.0), (0, 0, 0.25), (0, 0, 1)] pl.open_gif(gif_path, fps=fps) -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation - for i in frame_indices: qi = q_hist[i] R = Rotation.from_rotvec(qi[3:]).as_matrix() @@ -155,23 +155,3 @@ def collect(t, q, v): pl.close() print(f" Saved: {gif_path}") - -# Plot -try: - import matplotlib.pyplot as plt - - fig, ax = plt.subplots(figsize=(12, 4)) - ax.plot(t_hist, z_hist, "b-", linewidth=1) - ax.axhline(y=0, color="grey", linestyle=":", alpha=0.5) - ax.set_xlabel("t (s)") - ax.set_ylabel("z (m)") - ax.set_title( - f"Cow bounce — {cow_mesh.n_nodes} nodes, {elapsed:.1f}s ({elapsed/len(t_hist)*1000:.1f}ms/step)" - ) - ax.grid(True, alpha=0.3) - plt.tight_layout() - plt.savefig(f"{out_dir}/rigid_body_cow_bounce.png", dpi=150) - plt.close() - print(f" Saved: {out_dir}/rigid_body_cow_bounce.png") -except ImportError: - pass diff --git a/examples/rigid_body_freefall.py b/examples/rigid_body_freefall.py index 36fa81cf..d9e77567 100644 --- a/examples/rigid_body_freefall.py +++ b/examples/rigid_body_freefall.py @@ -1,25 +1,17 @@ -"""Rigid body free fall — validation of RigidBody + NonLinearNewmark. +"""Rigid body free fall — validation with NonLinear solver. -A rigid sphere falls under gravity. The result is compared with the -analytical solution z(t) = z0 - 0.5*g*t^2. - -Demonstrates the RigidBody API: -- fd.constraint.RigidBody(mesh, mass, inertia_tensor) -- body.set_force([0, 0, -m*g]) -- body.assembly as both Stiffness and Mass in NonLinearNewmark -- PyVista animation of the result +A rigid sphere falls under gravity. Compared with z(t) = z0 - 0.5*g*t^2. +Uses RigidBody + NonLinear (Fedoo's standard solver). """ import sys +import time import numpy as np import pyvista as pv sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd -# ============================================================================== -# Parameters -# ============================================================================== g = 9.81 mass = 1.0 radius = 0.1 @@ -29,139 +21,35 @@ print("=" * 60) print("RIGID BODY FREE FALL — Fedoo validation") -print(f" Fedoo {fd.__version__}, m={mass}kg, z0={z0}m, r={radius}m, dt={dt}s") +print(f" m={mass}kg, z0={z0}m, dt={dt}s") print("=" * 60) -# ============================================================================== -# Setup -# ============================================================================== space = fd.ModelingSpace("3D") space.new_variable("DispX") space.new_variable("DispY") space.new_variable("DispZ") space.new_vector("Disp", ("DispX", "DispY", "DispZ")) -# Sphere mesh via PyVista -pv_sphere = pv.Sphere( - radius=radius, center=(0, 0, z0), theta_resolution=12, phi_resolution=12 -) -mesh = fd.Mesh.from_pyvista(pv_sphere) -print(f" Mesh: {mesh.n_nodes} nodes, {mesh.n_elements} elements") - -# Rigid body -inertia = (2.0 / 5.0) * mass * radius**2 * np.eye(3) -body = fd.constraint.RigidBody(mesh, mass=mass, inertia_tensor=inertia, name="Ball") -body.set_force([0, 0, -mass * g]) - -# Problem -pb = fd.problem.NonLinearNewmark( - body.assembly, body.assembly, 0.25, 0.5, name="FreeFall" -) -body.add_to_problem(pb) - -# ============================================================================== -# Solve step by step -# ============================================================================== -print(" Solving...") -n_steps = int(t_end / dt) -z_history = [z0] -t_history = [0.0] - -pb.initialize() - -idx_z = ( - pb.n_node_dof - + pb._global_dof.indice_start("RigidDispZ") - + body.constraint.node_cd[2] -) - -for step in range(n_steps): - t = (step + 1) * dt - pb.dtime = dt - pb.solve_time_increment() - pb.set_start() - - dz = pb.get_dof_solution()[idx_z] - z_history.append(z0 + dz) - t_history.append(t) - - if step % 100 == 0: - z_ana = z0 - 0.5 * g * t**2 - print(f" t={t:.3f}s z={z0+dz:.4f}m (analytical: {z_ana:.4f}m)") - -t_history = np.array(t_history) -z_history = np.array(z_history) -z_analytical = z0 - 0.5 * g * t_history**2 -error = np.max(np.abs(z_history - z_analytical)) -print(f"\n Max error |z_num - z_analytical| = {error:.2e} m") -print(f" {'PASS' if error < 0.01 else 'FAIL'}") - -# ============================================================================== -# PyVista animation -# ============================================================================== -print("\nGenerating PyVista animation...") -out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" -gif_path = f"{out_dir}/rigid_body_freefall.gif" - -fps = 25 -frame_skip = max(1, int(1.0 / (fps * dt))) -frame_indices = np.arange(0, len(t_history), frame_skip) -print(f" {len(frame_indices)} frames at {fps} fps") - -sphere = pv.Sphere( - radius=radius, center=(0, 0, z0), theta_resolution=20, phi_resolution=20 +mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=radius, center=(0, 0, z0), theta_resolution=8, phi_resolution=8) ) -pts_ref = sphere.points.copy() -plane = pv.Plane( - center=(0, 0, 0), - direction=(0, 0, 1), - i_size=2.0, - j_size=2.0, - i_resolution=10, - j_resolution=10, +body = fd.constraint.RigidBody( + mesh, + mass=mass, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0, 0, z0]), ) +body.set_force([0, 0, -mass * g]) -pl = pv.Plotter(window_size=[800, 600], off_screen=True) -pl.set_background("white") -pl.add_mesh(plane, color="lightgrey", opacity=0.7, show_edges=True) -pl.add_mesh(sphere, color="steelblue", smooth_shading=True) -pl.camera_position = [(2.0, -2.0, 1.5), (0, 0, 0.5), (0, 0, 1)] -pl.open_gif(gif_path, fps=fps) - -for i in frame_indices: - z = z_history[i] - t = t_history[i] - sphere.points[:] = pts_ref + np.array([[0, 0, z - z0]]) - sphere.GetPoints().Modified() - pl.add_text( - f"t = {t:.3f}s | z = {z:.3f}m | Fedoo RigidBody", - position="upper_edge", - font_size=11, - color="black", - name="title", - ) - pl.render() - pl.write_frame() - -pl.close() -print(f" Saved: {gif_path}") +pb = body.solve(dt=dt, tmax=t_end, print_info=0) -# Trajectory plot -try: - import matplotlib.pyplot as plt +# Read trajectory from final state +idx = body.assembly._dof_indices +dof = pb.get_dof_solution() +dz_final = dof[idx[2]] +z_final = z0 + dz_final +z_analytical = z0 - 0.5 * g * t_end**2 - fig, ax = plt.subplots(figsize=(10, 4)) - ax.plot(t_history, z_history, "b-", linewidth=1.5, label="Fedoo Newmark") - ax.plot(t_history, z_analytical, "r--", linewidth=1, label="Analytical") - ax.set_xlabel("t (s)") - ax.set_ylabel("z (m)") - ax.set_title("Rigid body free fall — Fedoo validation") - ax.legend() - ax.grid(True, alpha=0.3) - plt.tight_layout() - png_path = f"{out_dir}/rigid_body_freefall.png" - plt.savefig(png_path, dpi=150) - plt.close() - print(f" Saved: {png_path}") -except ImportError: - pass +print(f" z_final = {z_final:.4f}m (analytical: {z_analytical:.4f}m)") +print(f" error = {abs(z_final - z_analytical):.2e}m") +print(f" {'PASS' if abs(z_final - z_analytical) < 0.01 else 'FAIL'}") diff --git a/fedoo/constitutivelaw/elastic_anisotropic.py b/fedoo/constitutivelaw/elastic_anisotropic.py index 9da4d299..e267d26d 100644 --- a/fedoo/constitutivelaw/elastic_anisotropic.py +++ b/fedoo/constitutivelaw/elastic_anisotropic.py @@ -46,6 +46,11 @@ def update(self, assembly, pb): else: total_strain = assembly.sv["Strain"] + # Handle uninitialized strain (e.g. first Newmark step with zero displacement) + if np.isscalar(total_strain) and total_strain == 0: + assembly.sv["Stress"] = 0 + return + assembly.sv["Stress"] = StressTensorList( [ sum( diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 88306cd4..05e5ea26 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -72,6 +72,8 @@ def __init__( rigid_tie, mesh=None, space=None, + beta=0.25, + gamma=0.5, name="RigidBodyAssembly", ): if space is None: @@ -84,10 +86,21 @@ def __init__( self.rigid_tie = rigid_tie self.force = np.zeros(6) self.mesh = mesh + self.beta = beta + self.gamma = gamma + self.rayleigh_alpha = 0.0 self._dof_indices = None self._pb_ref = None + # Newmark state variables (6-DOF) + self.sv = { + "Velocity": np.zeros(6), + "Acceleration": np.zeros(6), + "_DeltaDisp": np.zeros(6), + } + self.sv_start = dict(self.sv) + # IPC contact state (set by RigidBody.enable_ipc_contact) self._ipc_collision_mesh = None self._ipc_collisions = None @@ -230,54 +243,125 @@ def compute_contact(self, q, rt, compute="all"): return self._contact_force, self._contact_stiffness # ------------------------------------------------------------------ - # Fedoo assembly interface (for use with NonLinearNewmark) + # Fedoo assembly interface (compatible with NonLinear solver) # ------------------------------------------------------------------ + def _get_mass_matrix(self): + """6x6 mass matrix in global frame.""" + M = np.zeros((6, 6)) + M[0, 0] = M[1, 1] = M[2, 2] = self.mass + Q = self.rigid_tie.Q_total + M[3:, 3:] = ( + Q.apply_tensor(self.inertia_body) if Q is not None else self.inertia_body + ) + return M + + def _get_n_dof(self): + if self._pb_ref is not None: + return self._pb_ref.n_dof + return max( + self.mesh.n_nodes * self.space.nvar, + int(self._dof_indices.max()) + 1, + ) + def assemble_global_mat(self, compute="all"): + """Assemble effective dynamic stiffness and residual. + + Implements Newmark time integration directly on the 6 rigid DOFs: + - Matrix: K_eff = K_contact + (a0 + α·c0)·M + - Vector: D = F_ext + F_contact + M·(inertia residual) + C·(damping residual) + + where a0 = 1/(β·dt²), c0 = γ/(β·dt). + """ if self._dof_indices is None: return - n = ( - self._pb_ref.n_dof - if self._pb_ref is not None - else ( - max( - self.mesh.n_nodes * self.space.nvar, - int(self._dof_indices.max()) + 1, - ) - ) - ) + n = self._get_n_dof() idx = self._dof_indices + dt = self._pb_ref.dtime if self._pb_ref is not None else 1.0 - if compute in ("all", "matrix"): - M = np.zeros((6, 6)) - M[0, 0] = M[1, 1] = M[2, 2] = self.mass - Q = self.rigid_tie.Q_total - M[3:, 3:] = ( - Q.apply_tensor(self.inertia_body) - if Q is not None - else self.inertia_body - ) + M = self._get_mass_matrix() + a0 = 1.0 / (self.beta * dt**2) + c0 = self.gamma / (self.beta * dt) + alpha = self.rayleigh_alpha + if compute in ("all", "matrix"): + # K_eff = K_contact + (a0 + α·c0)·M + K_eff_6 = self._contact_stiffness + (a0 + alpha * c0) * M self.global_matrix = sparse.csr_matrix( - (M.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), + (K_eff_6.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), shape=(n, n), ) if compute in ("all", "vector"): + v_n = self.sv["Velocity"] + a_n = self.sv["Acceleration"] + delta_u = self.sv["_DeltaDisp"] + + # Predicted acceleration and velocity from Newmark + a_pred = a0 * (delta_u - dt * v_n) + (1 - 0.5 / self.beta) * a_n + v_pred = ( + (1 - self.gamma / self.beta) * v_n + + (c0 * delta_u) + + dt * (1 - self.gamma / (2 * self.beta)) * a_n + ) + + # Damping: C = α·M + C = alpha * M + + # Residual: D = F_ext + F_contact - M·a_pred - C·v_pred + D_6 = self.force + self._contact_force - M @ a_pred - C @ v_pred vec = np.zeros(n) - vec[idx] = self.force + self._contact_force + vec[idx] = D_6 self.global_vector = vec def update(self, pb, compute="all"): + """Update state from current displacement increment.""" self._pb_ref = pb + # Read accumulated displacement at rigid DOFs + dof_sol = pb.get_dof_solution() + if not (np.isscalar(dof_sol) and dof_sol == 0): + # _DeltaDisp = current total increment for this time step + if np.isscalar(pb._dU) and pb._dU == 0: + self.sv["_DeltaDisp"] = np.zeros(6) + else: + self.sv["_DeltaDisp"] = pb._dU[self._dof_indices] + + # Update IPC contact from current q + if self._ipc_collision_mesh is not None: + q = dof_sol[self._dof_indices] if not np.isscalar(dof_sol) else np.zeros(6) + self.compute_contact(q, self.rigid_tie, compute="all") + self.assemble_global_mat(compute) def set_start(self, pb): + """Accept converged increment: update velocity and acceleration.""" self._pb_ref = pb + dt = pb.dtime + delta_u = self.sv["_DeltaDisp"] + v_n = self.sv["Velocity"] + a_n = self.sv["Acceleration"] + + # Save for to_start rollback + self.sv_start = { + k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv.items() + } + + # Newmark velocity/acceleration update + new_a = ( + (1 / (self.beta * dt**2)) * delta_u + - (1 / (self.beta * dt)) * v_n + - (0.5 / self.beta - 1) * a_n + ) + self.sv["Velocity"] = v_n + dt * ((1 - self.gamma) * a_n + self.gamma * new_a) + self.sv["Acceleration"] = new_a + self.sv["_DeltaDisp"] = np.zeros(6) def to_start(self, pb): - pass + """Revert to start of failed increment.""" + self.sv = { + k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv_start.items() + } class RigidBody: @@ -401,6 +485,7 @@ def set_generalized_force(self, f): def set_rayleigh_damping(self, alpha): """Set mass-proportional damping: C = alpha * M.""" self._rayleigh_alpha = float(alpha) + self.assembly.rayleigh_alpha = float(alpha) # ------------------------------------------------------------------ # IPC contact @@ -471,41 +556,15 @@ def Q_total(self): return self.constraint.Q_total # ------------------------------------------------------------------ - # Direct 6x6 Newmark solver + # Convenience solver (wraps NonLinear) # ------------------------------------------------------------------ - @staticmethod - def _ccd_filter(asm, rt, q_current, dq_step): - """Limit dq_step so the trajectory is collision-free (Tight-Inclusion CCD). + def solve(self, dt, tmax, t0=0, print_info=1): + """Solve rigid body dynamics using Fedoo's NonLinear solver. - Computes the maximal alpha in [0, 1] such that the linear motion - from q_current to q_current + alpha*dq_step produces no collision, - then returns alpha * dq_step. - """ - from fedoo.constraint.ipc_contact import _import_ipctk - - ipctk = _import_ipctk() - - v0 = asm._ipc_vertices(q_current, rt) - v1 = asm._ipc_vertices(q_current + dq_step, rt) - - alpha = ipctk.compute_collision_free_stepsize( - asm._ipc_collision_mesh, - v0, - v1, - broad_phase=asm._ipc_broad_phase, - ) - - if alpha < 1.0: - # Leave a small margin (0.9 factor) to stay inside barrier zone - return 0.9 * alpha * dq_step - return dq_step - - def solve(self, dt, tmax, t0=0, callback=None, print_info=True): - """Solve rigid body dynamics with a direct 6x6 Newmark integrator. - - Bypasses the Fedoo FEM solver — solves directly in the 6-DOF - rigid body space with IPC contact and Rayleigh damping. + Creates an internal ``NonLinear`` problem and calls ``nlsolve``. + The ``RigidBodyAssembly`` handles Newmark time integration, + IPC contact, and Rayleigh damping internally. Parameters ---------- @@ -515,190 +574,20 @@ def solve(self, dt, tmax, t0=0, callback=None, print_info=True): End time. t0 : float Start time. - callback : callable, optional - Called at each step: ``callback(t, q, v)``. - print_info : bool - Print progress. + print_info : int + Verbosity (0=silent, 1=iterations). Returns ------- - q : ndarray (6,) - Final DOFs [dx, dy, dz, rx, ry, rz]. - v : ndarray (6,) - Final velocity. - a : ndarray (6,) - Final acceleration. + pb : NonLinear + The solved problem (access DOFs via ``pb.get_dof_solution()``). """ - n_steps = int(round((tmax - t0) / dt)) - q = np.zeros(6) - v = np.zeros(6) - a = np.zeros(6) - beta, gamma = 0.25, 0.5 - rt = self.constraint - asm = self.assembly - - # Ensure quaternion state is initialized - if not hasattr(rt, "_Q_base") or rt._Q_base is None: - rt._Q_base = Rotation.identity() - rt._Q_base_backup = Rotation.identity() - rt._angles_at_base = np.zeros(3) - - nr_tol = 1e-10 * max(np.linalg.norm(asm.force), 1.0) - - for step in range(n_steps): - t = t0 + (step + 1) * dt - - # Mass matrix - M = np.zeros((6, 6)) - M[0, 0] = M[1, 1] = M[2, 2] = self.mass - Q = rt._Q_base - M[3:, 3:] = ( - Q.apply_tensor(self.inertia_tensor) - if Q is not None - else self.inertia_tensor - ) - - # Damping - C = ( - self._rayleigh_alpha * M - if self._rayleigh_alpha > 0 - else np.zeros((6, 6)) - ) - - # Initial prediction - F_c, K_c = asm.compute_contact(q, rt, compute="all") - A_eff = K_c + (1 / (beta * dt**2)) * M + (gamma / (beta * dt)) * C - v_pred = (1 - gamma / beta) * v + dt * (1 - gamma / (2 * beta)) * a - rhs = ( - asm.force - + F_c - + M @ (-(1 / (beta * dt)) * v - (0.5 / beta - 1) * a) - + C @ (-v_pred) - ) - - try: - dq = np.linalg.solve(A_eff, rhs) - except np.linalg.LinAlgError: - if print_info: - print(f" Singular at t={t:.4f}s") - break - - # CCD: limit step to collision-free trajectory - if asm._ipc_collision_mesh is not None: - dq = self._ccd_filter(asm, rt, q, dq) - - # Newton-Raphson with line search - for nr_iter in range(20): - F_c, K_c = asm.compute_contact(q + dq, rt, compute="all") - new_a = ( - (1 / (beta * dt**2)) * dq - - (1 / (beta * dt)) * v - - (0.5 / beta - 1) * a - ) - new_v = v + dt * ((1 - gamma) * a + gamma * new_a) - res = asm.force + F_c - M @ new_a - C @ new_v - res_norm = np.linalg.norm(res) - - if res_norm < nr_tol: - break - - A_eff = K_c + (1 / (beta * dt**2)) * M + (gamma / (beta * dt)) * C - try: - ddq = np.linalg.solve(A_eff, res) - except np.linalg.LinAlgError: - break - - # CCD-filtered line search - ddq_safe = self._ccd_filter(asm, rt, q + dq, ddq) - alpha_ls = 1.0 - for _ in range(8): - dq_test = dq + alpha_ls * ddq_safe - F_test, _ = asm.compute_contact(q + dq_test, rt, compute="force") - a_test = ( - (1 / (beta * dt**2)) * dq_test - - (1 / (beta * dt)) * v - - (0.5 / beta - 1) * a - ) - v_test = v + dt * ((1 - gamma) * a + gamma * a_test) - if ( - np.linalg.norm(asm.force + F_test - M @ a_test - C @ v_test) - < res_norm - ): - break - alpha_ls *= 0.5 - dq = dq + alpha_ls * ddq_safe - - # Update state - new_a = ( - (1 / (beta * dt**2)) * dq - (1 / (beta * dt)) * v - (0.5 / beta - 1) * a - ) - v = v + dt * ((1 - gamma) * a + gamma * new_a) - a = new_a - q = q + dq - - # Absorb rotation into quaternion base - delta = q[3:] - rt._angles_at_base - if np.any(np.abs(delta) > 1e-14): - rt._Q_base = Rotation.from_rotvec(delta) * rt._Q_base - rt._angles_at_base = q[3:].copy() - - # Adaptive barrier stiffness via ipctk heuristics - if asm._ipc_collision_mesh is not None and len(asm._ipc_collisions) > 0: - min_dist = asm._ipc_collisions.compute_minimum_distance( - asm._ipc_collision_mesh, - asm._ipc_vertices(q, rt), - ) - if not hasattr(asm, "_prev_min_dist"): - # First contact: initialize kappa - from fedoo.constraint.ipc_contact import _import_ipctk - - _ipctk = _import_ipctk() - bbox_diag = np.linalg.norm( - asm._ipc_collision_mesh.rest_positions.max(axis=0) - - asm._ipc_collision_mesh.rest_positions.min(axis=0) - ) - grad_barrier = asm._ipc_barrier.gradient( - asm._ipc_collisions, - asm._ipc_collision_mesh, - asm._ipc_vertices(q, rt), - ) - J = asm._build_ipc_jacobian(rt, q[3:]) - grad_b_proj = J.T @ grad_barrier - kappa_init, kappa_max = _ipctk.initial_barrier_stiffness( - bbox_diag, - asm._ipc_barrier.barrier, - asm._ipc_dhat, - self.mass, - asm.force, - grad_b_proj, - ) - asm._ipc_kappa = max(asm._ipc_kappa, kappa_init) - asm._ipc_kappa_max = kappa_max - asm._bbox_diag = bbox_diag - asm._prev_min_dist = min_dist - else: - from fedoo.constraint.ipc_contact import _import_ipctk - - _ipctk = _import_ipctk() - asm._ipc_kappa = _ipctk.update_barrier_stiffness( - asm._prev_min_dist, - min_dist, - asm._ipc_kappa_max, - asm._ipc_kappa, - asm._bbox_diag, - ) - asm._prev_min_dist = min_dist - - if callback is not None: - callback(t, q, v) - - if print_info and step % max(1, n_steps // 20) == 0: - n_c = len(asm._ipc_collisions) if asm._ipc_collisions is not None else 0 - print( - f" t={t:.3f}s z={self.center_of_mass[2]+q[2]:.4f}m contacts={n_c}" - ) + from fedoo.problem.non_linear import NonLinear - return q, v, a + pb = NonLinear(self.assembly) + self.add_to_problem(pb) + pb.nlsolve(dt=dt, tmax=tmax, t0=t0, print_info=print_info, update_dt=False) + return pb # ------------------------------------------------------------------ # Inertia computation From 524350df3ecd6dc85e7387d2d0c9f12631334b52 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Tue, 21 Apr 2026 14:31:25 +0200 Subject: [PATCH 04/21] Refactor RigidBody, update examples, add tests Clean up example scripts to avoid hard-coded sys.path and use paths relative to each file for output GIFs. Simplify fedoo.constraint.rigid_body: remove simcoon import fallback, streamline Newmark state handling (use copies for sv_start), eliminate some unused attributes/comments (e.g. _ipc_kappa_auto, _rayleigh_alpha) and tidy IPC/contact and solver-related code paths. Wire rayleigh damping into assembly.rayleigh_alpha. Add unit tests covering rigid body free-fall, IPC kinematics/Jacobian, RigidTie derivative correctness, and quaternion rollback behavior. --- examples/rigid_body_bounce_ipc.py | 11 +-- examples/rigid_body_bunny_bounce.py | 5 +- examples/rigid_body_cow_bounce.py | 6 +- examples/rigid_body_freefall.py | 2 - fedoo/constraint/rigid_body.py | 60 +------------- tests/test_rigid_body_freefall.py | 86 ++++++++++++++++++++ tests/test_rigid_body_kinematics.py | 119 ++++++++++++++++++++++++++++ tests/test_rigid_tie_derivatives.py | 80 +++++++++++++++++++ tests/test_rigid_tie_rollback.py | 85 ++++++++++++++++++++ 9 files changed, 379 insertions(+), 75 deletions(-) create mode 100644 tests/test_rigid_body_freefall.py create mode 100644 tests/test_rigid_body_kinematics.py create mode 100644 tests/test_rigid_tie_derivatives.py create mode 100644 tests/test_rigid_tie_rollback.py diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index 851478c6..a10895b1 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -4,19 +4,13 @@ solved by Fedoo's standard NonLinear solver. """ -import sys +import os import time import numpy as np import pyvista as pv -sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation - g = 9.81 mass = 1.0 radius = 0.1 @@ -87,8 +81,7 @@ print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") # Animation -out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" -gif_path = f"{out_dir}/rigid_body_bounce_ipc.gif" +gif_path = os.path.join(os.path.dirname(__file__), "rigid_body_bounce_ipc.gif") fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index 9afd8182..ca81bd56 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -4,12 +4,11 @@ on impact with asymmetric geometry. """ -import sys +import os import time import numpy as np import pyvista as pv -sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd try: @@ -104,7 +103,7 @@ print(f" z_min={z_hist.min():.4f}m") # Animation (low res for file size) -out = "/Users/ychemisky/Documents/GitHub/fedoo/examples/rigid_body_bunny_bounce.gif" +out = os.path.join(os.path.dirname(__file__), "rigid_body_bunny_bounce.gif") fps = 15 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 74c1fbf4..561d191f 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -4,12 +4,11 @@ Full rotation via quaternion, rotation vectors (no gimbal lock). """ -import sys +import os import time import numpy as np import pyvista as pv -sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd try: @@ -113,8 +112,7 @@ print(f" z_min={z_hist.min():.4f}m") # Animation -out_dir = "/Users/ychemisky/Documents/GitHub/fedoo/examples" -gif_path = f"{out_dir}/rigid_body_cow_bounce.gif" +gif_path = os.path.join(os.path.dirname(__file__), "rigid_body_cow_bounce.gif") fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/examples/rigid_body_freefall.py b/examples/rigid_body_freefall.py index d9e77567..4d5bdc96 100644 --- a/examples/rigid_body_freefall.py +++ b/examples/rigid_body_freefall.py @@ -4,12 +4,10 @@ Uses RigidBody + NonLinear (Fedoo's standard solver). """ -import sys import time import numpy as np import pyvista as pv -sys.path.insert(0, "/Users/ychemisky/Documents/GitHub/fedoo") import fedoo as fd g = 9.81 diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 05e5ea26..da65cced 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -29,11 +29,6 @@ from fedoo.core.base import AssemblyBase from fedoo.constraint.rigid_tie import RigidTie -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation - class RigidBodyAssembly(AssemblyBase): """Assembly contributing rigid body mass and generalized forces. @@ -93,20 +88,17 @@ def __init__( self._dof_indices = None self._pb_ref = None - # Newmark state variables (6-DOF) self.sv = { "Velocity": np.zeros(6), "Acceleration": np.zeros(6), "_DeltaDisp": np.zeros(6), } - self.sv_start = dict(self.sv) + self.sv_start = {k: v.copy() for k, v in self.sv.items()} - # IPC contact state (set by RigidBody.enable_ipc_contact) self._ipc_collision_mesh = None self._ipc_collisions = None self._ipc_barrier = None self._ipc_kappa = None - self._ipc_kappa_auto = False self._ipc_dhat = None self._ipc_broad_phase = None self._ipc_rest_positions = None @@ -135,10 +127,6 @@ def dof_indices(self): """Global DOF indices [Fx,Fy,Fz,Mx,My,Mz] in the problem.""" return self._dof_indices - # ------------------------------------------------------------------ - # IPC contact: Jacobian and force/stiffness computation - # ------------------------------------------------------------------ - def _build_ipc_jacobian(self, rt, angles): """Build contact Jacobian J mapping 6 rigid DOFs to all vertex DOFs. @@ -214,13 +202,11 @@ def compute_contact(self, q, rt, compute="all"): J = self._build_ipc_jacobian(rt, q[3:]) - # Gradient → force grad = self._ipc_barrier.gradient( self._ipc_collisions, self._ipc_collision_mesh, vertices ) self._contact_force = -self._ipc_kappa * (J.T @ grad) - # Hessian → stiffness (skipped in line-search for efficiency) if compute in ("all", "stiffness"): try: hess = self._ipc_barrier.hessian( @@ -242,10 +228,6 @@ def compute_contact(self, q, rt, compute="all"): return self._contact_force, self._contact_stiffness - # ------------------------------------------------------------------ - # Fedoo assembly interface (compatible with NonLinear solver) - # ------------------------------------------------------------------ - def _get_mass_matrix(self): """6x6 mass matrix in global frame.""" M = np.zeros((6, 6)) @@ -286,7 +268,6 @@ def assemble_global_mat(self, compute="all"): alpha = self.rayleigh_alpha if compute in ("all", "matrix"): - # K_eff = K_contact + (a0 + α·c0)·M K_eff_6 = self._contact_stiffness + (a0 + alpha * c0) * M self.global_matrix = sparse.csr_matrix( (K_eff_6.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), @@ -298,18 +279,14 @@ def assemble_global_mat(self, compute="all"): a_n = self.sv["Acceleration"] delta_u = self.sv["_DeltaDisp"] - # Predicted acceleration and velocity from Newmark a_pred = a0 * (delta_u - dt * v_n) + (1 - 0.5 / self.beta) * a_n v_pred = ( (1 - self.gamma / self.beta) * v_n + (c0 * delta_u) + dt * (1 - self.gamma / (2 * self.beta)) * a_n ) - - # Damping: C = α·M C = alpha * M - # Residual: D = F_ext + F_contact - M·a_pred - C·v_pred D_6 = self.force + self._contact_force - M @ a_pred - C @ v_pred vec = np.zeros(n) vec[idx] = D_6 @@ -318,16 +295,13 @@ def assemble_global_mat(self, compute="all"): def update(self, pb, compute="all"): """Update state from current displacement increment.""" self._pb_ref = pb - # Read accumulated displacement at rigid DOFs dof_sol = pb.get_dof_solution() if not (np.isscalar(dof_sol) and dof_sol == 0): - # _DeltaDisp = current total increment for this time step if np.isscalar(pb._dU) and pb._dU == 0: self.sv["_DeltaDisp"] = np.zeros(6) else: self.sv["_DeltaDisp"] = pb._dU[self._dof_indices] - # Update IPC contact from current q if self._ipc_collision_mesh is not None: q = dof_sol[self._dof_indices] if not np.isscalar(dof_sol) else np.zeros(6) self.compute_contact(q, self.rigid_tie, compute="all") @@ -342,12 +316,10 @@ def set_start(self, pb): v_n = self.sv["Velocity"] a_n = self.sv["Acceleration"] - # Save for to_start rollback self.sv_start = { k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv.items() } - # Newmark velocity/acceleration update new_a = ( (1 / (self.beta * dt**2)) * delta_u - (1 / (self.beta * dt)) * v_n @@ -376,11 +348,8 @@ class RigidBody: approximation). IPC barrier contact is handled via direct Jacobian projection ``J^T @ F`` in the 6-DOF space. - Two solver modes: - - ``body.solve(dt, tmax)`` — direct 6x6 Newmark integrator (fast, - standalone, no Fedoo Problem needed). - - ``body.add_to_problem(pb)`` — integrate with Fedoo's - NonLinearNewmark for mixed rigid-deformable problems. + Solve via ``body.solve(dt, tmax)`` (wraps ``NonLinear``) or integrate + into an existing problem with ``body.add_to_problem(pb)``. Parameters ---------- @@ -464,11 +433,6 @@ def __init__( mesh=mesh, name=f"{name}_asm", ) - self._rayleigh_alpha = 0.0 - - # ------------------------------------------------------------------ - # Forces and damping - # ------------------------------------------------------------------ def set_force(self, force): """Set external force [Fx, Fy, Fz] on translational DOFs.""" @@ -484,13 +448,8 @@ def set_generalized_force(self, f): def set_rayleigh_damping(self, alpha): """Set mass-proportional damping: C = alpha * M.""" - self._rayleigh_alpha = float(alpha) self.assembly.rayleigh_alpha = float(alpha) - # ------------------------------------------------------------------ - # IPC contact - # ------------------------------------------------------------------ - def enable_ipc_contact(self, obstacle_mesh, dhat=0.01, kappa=None): """Enable IPC barrier contact with an obstacle surface. @@ -535,17 +494,12 @@ def enable_ipc_contact(self, obstacle_mesh, dhat=0.01, kappa=None): if kappa is None: kappa = 1e9 asm._ipc_kappa = kappa - asm._ipc_kappa_auto = False asm._ipc_dhat = dhat asm._ipc_broad_phase = ipctk.HashGrid() asm._ipc_rest_positions = body_nodes.copy() asm._ipc_n_body = n_body asm._ipc_obstacle_nodes = obst_nodes.copy() - # ------------------------------------------------------------------ - # Fedoo integration - # ------------------------------------------------------------------ - def add_to_problem(self, pb): """Register the rigid body constraint with a Fedoo problem.""" pb.bc.add(self.constraint) @@ -555,10 +509,6 @@ def Q_total(self): """Current total rotation as a Rotation object.""" return self.constraint.Q_total - # ------------------------------------------------------------------ - # Convenience solver (wraps NonLinear) - # ------------------------------------------------------------------ - def solve(self, dt, tmax, t0=0, print_info=1): """Solve rigid body dynamics using Fedoo's NonLinear solver. @@ -589,10 +539,6 @@ def solve(self, dt, tmax, t0=0, print_info=1): pb.nlsolve(dt=dt, tmax=tmax, t0=t0, print_info=print_info, update_dt=False) return pb - # ------------------------------------------------------------------ - # Inertia computation - # ------------------------------------------------------------------ - @staticmethod def _compute_inertia(mesh, density, center_of_mass): """Compute inertia tensor from a closed triangulated surface mesh. diff --git a/tests/test_rigid_body_freefall.py b/tests/test_rigid_body_freefall.py new file mode 100644 index 00000000..214a1c86 --- /dev/null +++ b/tests/test_rigid_body_freefall.py @@ -0,0 +1,86 @@ +"""Analytical validation of rigid body free fall under gravity.""" + +import numpy as np +import pytest + +import fedoo as fd + + +G = 9.81 + + +def _run_freefall(dt, t_end, mass=1.0, z0=1.0, radius=0.1): + try: + import pyvista as pv + except ImportError: # pragma: no cover + pytest.skip("pyvista not available") + + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + + mesh = fd.Mesh.from_pyvista( + pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=6, phi_resolution=6 + ) + ) + body = fd.constraint.RigidBody( + mesh, + mass=mass, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0.0, 0.0, z0]), + ) + body.set_force([0.0, 0.0, -mass * G]) + + pb = body.solve(dt=dt, tmax=t_end, print_info=0) + dof = pb.get_dof_solution() + idx = body.assembly._dof_indices + return z0 + dof[idx[2]] + + +@pytest.mark.parametrize("dt", [1e-3, 5e-4]) +def test_freefall_matches_analytical_solution(dt): + t_end = 0.3 + z_sim = _run_freefall(dt=dt, t_end=t_end) + z_exact = 1.0 - 0.5 * G * t_end**2 + # Newmark (β=0.25, γ=0.5) is energy-conserving and exact on quadratic + # trajectories up to round-off; 1e-4 m is ample margin. + assert abs(z_sim - z_exact) < 1e-4 + + +def test_freefall_no_horizontal_drift(): + t_end = 0.3 + try: + import pyvista as pv + except ImportError: # pragma: no cover + pytest.skip("pyvista not available") + + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + + mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=0.1, center=(0, 0, 1.0), theta_resolution=6, phi_resolution=6) + ) + body = fd.constraint.RigidBody( + mesh, + mass=1.0, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0.0, 0.0, 1.0]), + ) + body.set_force([0.0, 0.0, -G]) + + pb = body.solve(dt=1e-3, tmax=t_end, print_info=0) + dof = pb.get_dof_solution() + idx = body.assembly._dof_indices + dx, dy = dof[idx[0]], dof[idx[1]] + assert abs(dx) < 1e-10 + assert abs(dy) < 1e-10 + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py new file mode 100644 index 00000000..874b2a39 --- /dev/null +++ b/tests/test_rigid_body_kinematics.py @@ -0,0 +1,119 @@ +"""Unit tests for rigid body kinematics and the IPC contact Jacobian. + +Avoids ipctk dependencies by testing the RigidBodyAssembly geometry paths +(`_ipc_vertices`, `_build_ipc_jacobian`) against manual formulas. +""" + +import numpy as np +import pytest + +import fedoo as fd +from fedoo.constraint.rigid_body import RigidBodyAssembly + +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + + +@pytest.fixture +def space(): + return fd.ModelingSpace("3D") + + +def _make_assembly(rest_positions, obstacle_positions, center): + rt = fd.constraint.RigidTie( + np.arange(len(rest_positions)), center=center, use_quaternion=True + ) + rt.center = np.asarray(center, dtype=float) + asm = RigidBodyAssembly(mass=1.0, inertia_tensor=np.eye(3), rigid_tie=rt) + asm._ipc_rest_positions = np.asarray(rest_positions, dtype=float) + asm._ipc_obstacle_nodes = np.asarray(obstacle_positions, dtype=float) + asm._ipc_n_body = len(rest_positions) + return asm, rt + + +def test_ipc_vertices_pure_translation(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) + obst = np.array([[0.0, 0.0, -1.0]]) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + q = np.array([0.1, -0.2, 0.3, 0.0, 0.0, 0.0]) + verts = asm._ipc_vertices(q, rt) + + expected_body = rest + q[:3] + assert np.allclose(verts[:3], expected_body) + assert np.allclose(verts[3:], obst) + + +def test_ipc_vertices_pure_rotation_about_center(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + obst = np.zeros((0, 3)) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + # 90° about z — should rotate x into y, y into -x + q = np.array([0.0, 0.0, 0.0, 0.0, 0.0, np.pi / 2]) + verts = asm._ipc_vertices(q, rt) + + R_expected = Rotation.from_rotvec([0, 0, np.pi / 2]).as_matrix() + expected = rest @ R_expected.T + assert np.allclose(verts, expected, atol=1e-12) + + +def test_ipc_vertices_rotation_off_center(space): + rest = np.array([[1.0, 0.0, 0.0]]) + obst = np.zeros((0, 3)) + center = np.array([0.5, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + # 180° about z around (0.5,0,0) maps (1,0,0) -> (0,0,0) + q = np.array([0.0, 0.0, 0.0, 0.0, 0.0, np.pi]) + verts = asm._ipc_vertices(q, rt) + assert np.allclose(verts[0], [0.0, 0.0, 0.0], atol=1e-12) + + +def test_build_ipc_jacobian_structure(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) + obst = np.array([[0.0, 0.0, -1.0], [1.0, 0.0, -1.0]]) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + J = asm._build_ipc_jacobian(rt, np.zeros(3)) + + assert J.shape == (12, 6) + # Translation block: body rows are identity per vertex; obstacle rows zero. + for i in range(2): + for d in range(3): + assert J[i * 3 + d, d] == 1.0 + assert np.all(J[6:, :3] == 0.0) # obstacle rows zero in translation cols + assert np.all(J[6:, 3:] == 0.0) # obstacle rows zero in rotation cols + + +def test_build_ipc_jacobian_matches_finite_difference(space): + rest = np.array( + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 1.0, 0.0]] + ) + obst = np.zeros((0, 3)) + center = np.array([0.2, -0.1, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + angles0 = np.array([0.1, -0.15, 0.2]) + J = asm._build_ipc_jacobian(rt, angles0) + + # Numerically differentiate vertex positions w.r.t. q = [dx,dy,dz,rx,ry,rz] + def vertex_positions(q): + return asm._ipc_vertices(q, rt).ravel() + + q0 = np.concatenate([np.zeros(3), angles0]) + h = 1e-6 + for k in range(6): + eps = np.zeros(6) + eps[k] = h + du_dq = (vertex_positions(q0 + eps) - vertex_positions(q0 - eps)) / (2 * h) + assert np.allclose(J[:, k], du_dq, atol=1e-6), f"column {k} mismatch" + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_tie_derivatives.py b/tests/test_rigid_tie_derivatives.py new file mode 100644 index 00000000..fa26e957 --- /dev/null +++ b/tests/test_rigid_tie_derivatives.py @@ -0,0 +1,80 @@ +"""Finite-difference validation of RigidTie rotation derivatives.""" + +import numpy as np +import pytest + +from fedoo.constraint.rigid_tie import RigidTie + + +def _R(omega): + """Rotation matrix via the tie's own forward pass (non-quaternion path).""" + rt = RigidTie([0, 1], use_quaternion=False) + R, _, _, _ = rt._compute_rotation(np.asarray(omega, dtype=float)) + return R + + +def _dR_analytic(omega): + dR = RigidTie._dR_drotvec(np.asarray(omega, dtype=float)) + return np.stack(dR, axis=0) + + +def _dR_fd(omega, h=1e-6): + omega = np.asarray(omega, dtype=float) + out = np.empty((3, 3, 3)) + for k in range(3): + eps = np.zeros(3) + eps[k] = h + out[k] = (_R(omega + eps) - _R(omega - eps)) / (2 * h) + return out + + +@pytest.mark.parametrize( + "omega", + [ + np.zeros(3), + np.array([1e-12, 0.0, 0.0]), + np.array([0.1, 0.0, 0.0]), + np.array([0.0, 0.2, 0.0]), + np.array([0.0, 0.0, 0.3]), + np.array([0.3, -0.4, 0.5]), + np.array([np.pi / 4, np.pi / 6, -np.pi / 8]), + ], +) +def test_dR_drotvec_matches_finite_difference(omega): + analytic = _dR_analytic(omega) + numeric = _dR_fd(omega) + # FD error ~ O(h^2) with h=1e-6 → ~1e-8 in smooth regime + assert np.allclose(analytic, numeric, atol=1e-6) + + +def test_dR_drotvec_zero_limit_is_skew_basis(): + dR = RigidTie._dR_drotvec(np.zeros(3)) + expected = ( + np.array([[0, 0, 0], [0, 0, -1], [0, 1, 0]]), + np.array([[0, 0, 1], [0, 0, 0], [-1, 0, 0]]), + np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]), + ) + for got, exp in zip(dR, expected): + assert np.array_equal(got, exp) + + +def test_compute_rotation_identity_at_zero(): + rt = RigidTie([0, 1], use_quaternion=False) + R, _, _, _ = rt._compute_rotation(np.zeros(3)) + assert np.allclose(R, np.eye(3)) + + +def test_compute_rotation_is_orthogonal(): + rt = RigidTie([0, 1], use_quaternion=False) + for omega in [ + np.array([0.3, -0.4, 0.5]), + np.array([1.2, 0.0, 0.0]), + np.array([0.0, 2.5, -1.5]), + ]: + R, _, _, _ = rt._compute_rotation(omega) + assert np.allclose(R @ R.T, np.eye(3), atol=1e-12) + assert np.isclose(np.linalg.det(R), 1.0, atol=1e-12) + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py new file mode 100644 index 00000000..b8812580 --- /dev/null +++ b/tests/test_rigid_tie_rollback.py @@ -0,0 +1,85 @@ +"""Quaternion rollback: to_start_bc must exactly restore the pre-set_start state.""" + +import numpy as np +import pytest + +from fedoo.constraint.rigid_tie import RigidTie + +try: + from simcoon import Rotation +except ImportError: + from scipy.spatial.transform import Rotation + + +def _make_tie_with_fake_problem(angles_seq): + """Build a RigidTie wired to a fake problem that returns `angles_seq[i]` + on the i-th call to `_get_dof_ref`.""" + rt = RigidTie([0, 1], use_quaternion=True) + rt._Q_base = Rotation.identity() + rt._Q_base_backup = Rotation.identity() + rt._angles_at_base = np.zeros(3) + + calls = {"i": 0} + + def fake_get_dof_ref(_problem): + i = calls["i"] + calls["i"] += 1 + angles = np.asarray(angles_seq[i], dtype=float) + return np.concatenate([np.zeros(3), angles]), None + + rt._get_dof_ref = fake_get_dof_ref + return rt + + +def _quat_equal(a, b, atol=1e-14): + qa = a.as_quat() + qb = b.as_quat() + return np.allclose(qa, qb, atol=atol) or np.allclose(qa, -qb, atol=atol) + + +def test_to_start_bc_restores_identity_after_failed_increment(): + rt = _make_tie_with_fake_problem([[0.3, -0.4, 0.5]]) + rt.set_start(problem=None) + assert not _quat_equal(rt._Q_base, Rotation.identity()) + + rt.to_start_bc(problem=None) + assert _quat_equal(rt._Q_base, Rotation.identity()) + + +def test_two_successful_increments_compose_multiplicatively(): + a1 = np.array([0.1, 0.0, 0.0]) + a2 = np.array([0.1, 0.2, 0.0]) + rt = _make_tie_with_fake_problem([a1, a2]) + + rt.set_start(problem=None) + rt.set_start(problem=None) + + # Q_total should be R(a2 - a1) * R(a1) = composed rotations + expected = Rotation.from_rotvec(a2 - a1) * Rotation.from_rotvec(a1) + assert _quat_equal(rt._Q_base, expected) + assert np.array_equal(rt._angles_at_base, a2) + + +def test_rollback_after_converged_increment_reverts_to_prior_base(): + a1 = np.array([0.3, 0.0, 0.0]) + a2 = np.array([0.3, 0.4, 0.0]) + rt = _make_tie_with_fake_problem([a1, a2]) + + rt.set_start(problem=None) + q_after_first = rt._Q_base + + rt.set_start(problem=None) + rt.to_start_bc(problem=None) + + assert _quat_equal(rt._Q_base, q_after_first) + + +def test_nan_angles_do_not_mutate_base(): + rt = _make_tie_with_fake_problem([[np.nan, 0.0, 0.0]]) + q_before = rt._Q_base + rt.set_start(problem=None) + assert _quat_equal(rt._Q_base, q_before) + + +if __name__ == "__main__": + pytest.main([__file__]) From 8a32dba02ecfabc3e9c19fc22a8af62a4bbf0a00 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Thu, 23 Apr 2026 10:48:45 +0200 Subject: [PATCH 05/21] Fix GIF paths and initialize acceleration Use a safe __file__ fallback (os.getcwd()) when building GIF output paths in examples/rigid_body_bounce_ipc.py, examples/rigid_body_bunny_bounce.py, and examples/rigid_body_cow_bounce.py so they work in environments without __file__. In fedoo/constraint/rigid_body.py, initialize sv['Acceleration'] (and sv_start) by solving the mass matrix when the acceleration vector is all zeros, ensuring the rigid body starts with the correct initial acceleration derived from forces. --- examples/rigid_body_bounce_ipc.py | 3 ++- examples/rigid_body_bunny_bounce.py | 3 ++- examples/rigid_body_cow_bounce.py | 3 ++- fedoo/constraint/rigid_body.py | 5 +++++ 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index a10895b1..a604f047 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -81,7 +81,8 @@ print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") # Animation -gif_path = os.path.join(os.path.dirname(__file__), "rigid_body_bounce_ipc.gif") +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +gif_path = os.path.join(_here, "rigid_body_bounce_ipc.gif") fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index ca81bd56..71ea7a61 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -103,7 +103,8 @@ print(f" z_min={z_hist.min():.4f}m") # Animation (low res for file size) -out = os.path.join(os.path.dirname(__file__), "rigid_body_bunny_bounce.gif") +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +out = os.path.join(_here, "rigid_body_bunny_bounce.gif") fps = 15 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 561d191f..80ccaebb 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -112,7 +112,8 @@ print(f" z_min={z_hist.min():.4f}m") # Animation -gif_path = os.path.join(os.path.dirname(__file__), "rigid_body_cow_bounce.gif") +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +gif_path = os.path.join(_here, "rigid_body_cow_bounce.gif") fps = 25 frame_skip = max(1, int(1.0 / (fps * dt))) frame_indices = np.arange(0, len(t_hist), frame_skip) diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index da65cced..25153bad 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -122,6 +122,11 @@ def initialize(self, pb): if self.mesh is None: self.mesh = pb.mesh + if not np.any(self.sv["Acceleration"]): + M = self._get_mass_matrix() + self.sv["Acceleration"] = np.linalg.solve(M, self.force) + self.sv_start["Acceleration"] = self.sv["Acceleration"].copy() + @property def dof_indices(self): """Global DOF indices [Fx,Fy,Fz,Mx,My,Mz] in the problem.""" From bd53c63ad3a86d08f0728e0345d19d8109b362c9 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Thu, 23 Apr 2026 11:30:20 +0200 Subject: [PATCH 06/21] Skip computation when delta_u is zero Add an early return in RigidBodyAssembly (rigid_body.py) when delta_u has no non-zero entries (using np.any). This avoids unnecessary computation of new accelerations/velocities and prevents potential numerical noise or redundant state updates when there is no displacement change. --- fedoo/constraint/rigid_body.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 25153bad..9e6f1f81 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -325,6 +325,9 @@ def set_start(self, pb): k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv.items() } + if not np.any(delta_u): + return + new_a = ( (1 / (self.beta * dt**2)) * delta_u - (1 / (self.beta * dt)) * v_n From 49cf7e748e0c59d1b9197daee550786fc5d38000 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Sun, 26 Apr 2026 23:14:53 +0200 Subject: [PATCH 07/21] Use simcoon dR_drotvec; bump simcoon to 1.11.2 Delegate rotation-derivative computation to simcoon.dR_drotvec instead of the local Rodrigues differentiation routine. Add a fallback that sets _simcoon_dR_drotvec=None when simcoon is unavailable and raise a clear ImportError if derivatives are required. Also update pyproject.toml to require simcoon>=1.11.2 to ensure dR_drotvec is present. --- fedoo/constraint/rigid_tie.py | 69 +++++------------------------------ pyproject.toml | 4 +- 2 files changed, 12 insertions(+), 61 deletions(-) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 256529b0..c6278e23 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -4,10 +4,12 @@ from fedoo.core.boundary_conditions import BCBase, MPC, ListBC try: - from simcoon import Rotation + from simcoon import Rotation, dR_drotvec as _simcoon_dR_drotvec except ImportError: from scipy.spatial.transform import Rotation + _simcoon_dR_drotvec = None + class RigidTie(BCBase): """Constraint that eliminate dof assuming a rigid body tie between nodes. @@ -230,71 +232,20 @@ def _compute_rotation(self, angles): def _dR_drotvec(omega): """Exact derivatives of R(ω) w.r.t. rotation vector components. - Uses the Rodrigues formula differentiation: - ``R = I + (sin θ)/θ [ω]× + (1-cos θ)/θ² [ω]ײ`` - - For small ``||ω|| < 1e-10``, returns ``dR/dωₖ ≈ [eₖ]×`` (skew of - unit vector), which is exact in the limit. - - Parameters - ---------- - omega : ndarray (3,) - Rotation vector. + Delegates to simcoon's ``dR_drotvec`` (Gallego & Yezzi, 2015). Returns ------- dR : tuple of 3 ndarrays (3x3) (dR/dω₀, dR/dω₁, dR/dω₂). - - References - ---------- - Gallego & Yezzi, "A Compact Formula for the Derivative of a 3-D - Rotation in Exponential Coordinates", J. Math. Imaging Vis., 2015. """ - theta = np.linalg.norm(omega) - - # Skew-symmetric matrix of omega - W = np.array( - [ - [0, -omega[2], omega[1]], - [omega[2], 0, -omega[0]], - [-omega[1], omega[0], 0], - ] - ) - - if theta < 1e-10: - # Small angle limit: dR/dωₖ = [eₖ]× - return ( - np.array([[0, 0, 0], [0, 0, -1], [0, 1, 0]]), - np.array([[0, 0, 1], [0, 0, 0], [-1, 0, 0]]), - np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]), - ) - - s, c = np.sin(theta), np.cos(theta) - t2 = theta * theta - - # Rodrigues coefficients and their derivatives w.r.t. θ - a = s / theta # sin(θ)/θ - b = (1 - c) / t2 # (1-cos(θ))/θ² - da = (c * theta - s) / t2 # d(sin(θ)/θ)/dθ - db = (s * theta - 2 * (1 - c)) / (t2 * theta) # d((1-cos(θ))/θ²)/dθ - - W2 = W @ W - e = np.eye(3) - dR = [] - for k in range(3): - # dW/dωₖ = [eₖ]× - dW = np.array( - [[0, -e[k, 2], e[k, 1]], [e[k, 2], 0, -e[k, 0]], [-e[k, 1], e[k, 0], 0]] + if _simcoon_dR_drotvec is None: + raise ImportError( + "RigidTie rotation derivatives require simcoon>=1.11.2 " + "(provides simcoon.dR_drotvec)." ) - # dW²/dωₖ = dW @ W + W @ dW - dW2 = dW @ W + W @ dW - # dθ/dωₖ = ωₖ / θ - dtk = omega[k] / theta - - dR.append(da * dtk * W + a * dW + db * dtk * W2 + b * dW2) - - return tuple(dR) + cube = _simcoon_dR_drotvec(np.asarray(omega, dtype=float)) + return (cube[:, :, 0], cube[:, :, 1], cube[:, :, 2]) def _compute_slave_disp(self, problem, disp_ref, R): """Compute and write slave node displacements from rigid body state.""" diff --git a/pyproject.toml b/pyproject.toml index 88c5efb3..39cf5e4a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -27,7 +27,7 @@ classifiers = [ dependencies = [ 'numpy>=2.00', 'scipy', - 'simcoon>=1.11.0', + 'simcoon>=1.11.2', ] [project.optional-dependencies] @@ -42,7 +42,7 @@ plot = [ 'pyvista[io]', ] simcoon = [ - 'simcoon>=1.11.0' + 'simcoon>=1.11.2' ] test = [ 'pytest', From 6dd92f15cfac36d954e8e7c4f3f3f8103e860c59 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Mon, 27 Apr 2026 10:17:38 +0200 Subject: [PATCH 08/21] Update rigid_tie.py --- fedoo/constraint/rigid_tie.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index c6278e23..271b269a 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -3,12 +3,7 @@ import numpy as np from fedoo.core.boundary_conditions import BCBase, MPC, ListBC -try: - from simcoon import Rotation, dR_drotvec as _simcoon_dR_drotvec -except ImportError: - from scipy.spatial.transform import Rotation - - _simcoon_dR_drotvec = None +from simcoon import Rotation, dR_drotvec as _simcoon_dR_drotvec class RigidTie(BCBase): @@ -239,11 +234,6 @@ def _dR_drotvec(omega): dR : tuple of 3 ndarrays (3x3) (dR/dω₀, dR/dω₁, dR/dω₂). """ - if _simcoon_dR_drotvec is None: - raise ImportError( - "RigidTie rotation derivatives require simcoon>=1.11.2 " - "(provides simcoon.dR_drotvec)." - ) cube = _simcoon_dR_drotvec(np.asarray(omega, dtype=float)) return (cube[:, :, 0], cube[:, :, 1], cube[:, :, 2]) From 2d4519dd9c50a963cbf827e8fe2fe2eb82b777cd Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Tue, 12 May 2026 17:43:39 +0200 Subject: [PATCH 09/21] Remove redundant padding of IPC global DOFs Stop padding IPCContact global_matrix/global_vector for extra global DOFs. The scatter matrix P already maps to the full n_dof (mesh DOFs + global DOFs), so the previous padding inflated matrices to n_dof + n_global_dof and caused inconsistent-shape errors during assembly. Also add a regression test (tests/test_ipc_rigid_tie_shape.py) that exercises RigidTie+IPC and PeriodicBC+IPC cases to ensure assembled matrices/vectors have consistent shapes and no ValueError is raised. --- fedoo/constraint/ipc_contact.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index a351b39b..d83ea2c9 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -608,19 +608,10 @@ def _compute_ipc_contributions(self, vertices, compute="all"): ) self.global_matrix += P @ fric_hess_surf @ P.T - # Pad with zeros to account for extra global DOFs (e.g. PeriodicBC). - # When rigid bodies are present, P already maps to n_dof (including - # global DOFs), so no padding is needed. - if self._n_global_dof > 0 and not self._rigid_bodies: - if compute != "matrix": - self.global_vector = np.pad(self.global_vector, (0, self._n_global_dof)) - if compute != "vector": - self.global_matrix = sparse.block_diag( - [ - self.global_matrix, - sparse.csr_array((self._n_global_dof, self._n_global_dof)), - ], - ) + # P already has n_dof rows (mesh DOFs + global DOFs) — see + # _build_scatter_matrix — so P @ hess @ P.T and P @ grad are already + # the right size. No padding is needed for PeriodicBC, RigidTie, or + # registered RigidBody global DOFs. def _ccd_line_search(self, pb, dX): """Compute step size using CCD + energy-based backtracking. From b6b789b7835b1dd6320c183f1d7994c56dbb1779 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Tue, 26 May 2026 11:44:22 +0200 Subject: [PATCH 10/21] Support rigid+deformable IPC & rigid body updates Add support for wiring RigidBody instances into a shared IPCContact and improve rigid-body APIs and static-vs-dynamic handling. Key changes: - New example: examples/contact/ipc/rigid_deformable_punch.py demonstrating IPCContact.add_rigid_body with mixed rigid+deformable mesh. - Replace per-body enable_ipc_contact with RigidBody.set_static_obstacle for rigid-vs-static use; update example calls accordingly. - IPCContact.add_rigid_body signature changed to (rigid_body, surface_node_indices=None); body assembly is pointed to the shared mesh and scatter matrix computes exact rigid Jacobian. - IPCContact: rebuild scatter matrix handling, avoid double-padding global DOFs, clarify CCD/OGC compatibility, and refresh scatter on update/compute paths to keep tangents consistent. - RigidBody / RigidBodyAssembly: introduce dynamic flag (static mode disables Newmark inertia/damping), handle dt==0 safely, add tiny static regularisation, improve initialization and set_start logic, and make add_to_problem idempotent. - Mesh: add merge_coincident_nodes helper and set parent_node_indices on extract_elements to expose active node lists for submeshes. - Problem (Linear, NonLinear): auto-register RigidTie constraints from Assembly.sum so rigid ties are present before assembly initialization; fix NR step alpha handling to correctly apply Dirichlet increments and avoid leftover values. - Tests: add tests/tests for rigid-ipc coupling and shape-consistency (tests/test_ipc_rigid_tie_shape.py and tests/test_rigid_body_ipc_deformable.py) to guard matrix/vector shapes, Newton's third law, and proper kinematic coupling. These changes fix shape-mismatch bugs, ensure correct scatter/jacobian behaviour for rigid-slaved nodes, and provide clearer APIs for static vs shared IPC contact scenarios. --- examples/rigid_body_bounce_ipc.py | 3 +- examples/rigid_body_bunny_bounce.py | 3 +- examples/rigid_body_cow_bounce.py | 3 +- fedoo/constraint/ipc_contact.py | 137 ++++++++------ fedoo/constraint/rigid_body.py | 162 ++++++++++++---- fedoo/core/mesh.py | 64 ++++++- fedoo/problem/linear.py | 20 ++ fedoo/problem/non_linear.py | 50 ++++- tests/test_ipc_rigid_tie_shape.py | 184 ++++++++++++++++++ tests/test_rigid_body_ipc_deformable.py | 239 ++++++++++++++++++++++++ 10 files changed, 763 insertions(+), 102 deletions(-) create mode 100644 tests/test_ipc_rigid_tie_shape.py create mode 100644 tests/test_rigid_body_ipc_deformable.py diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index a604f047..7cc3a8fc 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -51,11 +51,10 @@ ) body.set_force([0, 0, -mass * g]) body.set_rayleigh_damping(1.0) -body.enable_ipc_contact(plane_mesh, dhat=0.01, kappa=1e8) +body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) # Solve using NonLinear via manual time stepping for trajectory collection pb = fd.problem.NonLinear(body.assembly) -body.add_to_problem(pb) pb.initialize() idx = body.assembly._dof_indices diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index 71ea7a61..ad29cc43 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -70,13 +70,12 @@ ) body.set_force([0, 0, -mass * g]) body.set_rayleigh_damping(1.0) -body.enable_ipc_contact(plane_mesh, dhat=0.008, kappa=1e8) +body.set_static_obstacle(plane_mesh, dhat=0.008, kappa=1e8) print(f" Bunny: {bunny_mesh.n_nodes} nodes, mass={mass}kg") # Solve with manual loop for trajectory pb = fd.problem.NonLinear(body.assembly) -body.add_to_problem(pb) pb.initialize() idx = body.assembly._dof_indices diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 80ccaebb..35888f69 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -70,13 +70,12 @@ ) body.set_force([0, 0, -mass * g]) body.set_rayleigh_damping(1.5) -body.enable_ipc_contact(plane_mesh, dhat=0.01) +body.set_static_obstacle(plane_mesh, dhat=0.01) print(f" Cow: {cow_mesh.n_nodes} nodes, mass={mass}kg") # Solve with NonLinear (manual stepping for trajectory) pb = fd.problem.NonLinear(body.assembly) -body.add_to_problem(pb) pb.initialize() idx = body.assembly._dof_indices diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index d83ea2c9..34bcd427 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -241,29 +241,53 @@ def __init__( self._rigid_bodies = [] self._rigid_node_set = None # set of rigid node indices (built in initialize) - def add_rigid_body(self, node_indices, rigid_body): - """Register surface nodes as belonging to a rigid body. + def add_rigid_body(self, rigid_body, surface_node_indices=None): + """Register a :class:`RigidBody` with this IPCContact. - When rigid bodies are registered, the scatter matrix maps their - surface DOFs directly onto the 6 rigid body global DOFs via the - contact Jacobian ``J = [I_3 | dR/d(angle)]``, instead of placing - forces at the per-node DOF positions. + Use this for rigid-vs-deformable (or rigid-vs-rigid) contact where + a single ``IPCContact`` carries the full collision mesh. The body's + surface nodes are routed onto the 6 rigid-body global DOFs by the + scatter matrix via the exact rigid Jacobian + ``J = [I_3 | dR/d(omega) @ r_ref]``, instead of placing forces at + the per-node mesh DOFs. - This is intended for mixed rigid-deformable contact problems where - rigid body surfaces and deformable surfaces are in the same - IPCContact assembly. For pure rigid-body problems, prefer - ``RigidBody.solve()`` which uses a faster direct 6x6 solver. + For rigid-vs-static contact (rigid body bouncing on a fixed floor), + use :meth:`RigidBody.set_static_obstacle` instead — that path uses + a private 6x6 fast route with no shared mesh. Parameters ---------- - node_indices : array_like - Global node indices in the full mesh that belong to this - rigid body's surface. rigid_body : RigidBody - The rigid body object (must have ``.constraint`` and - ``.assembly`` attributes). + The rigid body. When the body's nodes live inside this + IPCContact's mesh (stacked rigid-plus-deformable case), + construct it from a :meth:`Mesh.extract_elements` of the + shared mesh so the carried ``parent_node_indices`` wires + RigidTie to the correct parent DOFs. + ``rigid_body.constraint.list_nodes`` is read as the default + surface index set. + surface_node_indices : array_like, optional + Override for ``rigid_body.constraint.list_nodes``. Rarely + needed; useful only when the rigid body's surface is a strict + subset of its tie node set. """ - self._rigid_bodies.append((np.asarray(node_indices), rigid_body)) + if rigid_body.assembly._ipc_collision_mesh is not None: + raise RuntimeError( + f"{rigid_body.name}: cannot use set_static_obstacle() and " + "IPCContact.add_rigid_body() on the same body — choose " + "either the private rigid-vs-static path or the shared " + "IPCContact." + ) + if surface_node_indices is None: + surface_node_indices = np.asarray( + rigid_body.constraint.list_nodes, dtype=int + ) + else: + surface_node_indices = np.asarray(surface_node_indices, dtype=int) + # Point the body's RigidBodyAssembly at the shared problem mesh so + # ``Assembly.sum`` accepts (mass / inertia / Newmark stay 6x6 and + # are scattered at the 6 global DOFs regardless of mesh size). + rigid_body.assembly.mesh = self.mesh + self._rigid_bodies.append((surface_node_indices, rigid_body)) def _create_broad_phase(self): """Create an ipctk BroadPhase instance from the string name.""" @@ -282,9 +306,7 @@ def _create_broad_phase(self): ) return cls() - def _build_scatter_matrix( - self, surface_node_indices, n_nodes, ndim, pb=None, vertices=None - ): + def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim, pb=None): """Build sparse matrix mapping ipctk surface DOFs to fedoo full DOFs. ipctk uses interleaved layout for surface nodes: @@ -294,8 +316,10 @@ def _build_scatter_matrix( For deformable nodes, the mapping is a permutation (identity values). For rigid body nodes, the mapping uses the exact contact Jacobian - derived from the RigidTie rotation derivatives, projecting surface - DOFs directly onto the 6 rigid body global DOFs. + ``J = [I_3 | dR/d(omega) @ r_ref]`` derived from the RigidTie + rotation derivatives, projecting surface DOFs directly onto the 6 + rigid body global DOFs. Only ``r_ref`` (reference positions) + enters the Jacobian — current positions are not needed. Parameters ---------- @@ -307,9 +331,6 @@ def _build_scatter_matrix( Number of spatial dimensions (2 or 3). pb : Problem, optional Problem instance (needed for rigid body global DOF indices). - vertices : ndarray, optional - Current surface vertex positions, shape (n_surf, ndim). - Needed for rigid body Jacobian computation. Returns ------- @@ -356,29 +377,19 @@ def _build_scatter_matrix( n_rb = len(local_indices) - # Get exact rotation derivatives from RigidTie + # Get exact rotation derivatives from RigidTie. The Jacobian + # J = [I_3 | dR/d(omega) @ r_ref] uses only r_ref (reference + # positions) — current vertex positions and translation are + # not needed. if pb is not None: dof_ref, _ = rt._get_dof_ref(pb) angles = dof_ref[3:] - center_disp = dof_ref[:3] else: angles = np.zeros(3) - center_disp = np.zeros(3) - R, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) + _, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) center = rt.center # initial center position - # Lever arms in reference frame - if vertices is not None: - # Use current positions to get lever arms - vertex_pos = vertices[local_indices] # (n_rb, 3) - current_center = center + center_disp - r = vertex_pos - current_center # (n_rb, 3) - else: - # Use rest positions - r_ref = self._rest_positions[local_indices] - center - r = (R @ r_ref.T).T # rotate to current frame - # Translation part: J_i[:3, :3] = I_3 for d in range(ndim): all_rows.append(np.full(n_rb, dof_indices[d])) @@ -457,7 +468,13 @@ def _get_elastic_gradient_on_surface(self): if not hasattr(assembly, "_list_assembly"): return None - # Sum global vectors from all assemblies except this one + # Sum global vectors from all assemblies except this IPC one. + # RigidBodyAssembly safely returns zeros at init time (its + # assemble_global_mat guards against dt=0 internally), so we + # include it like any other assembly — its contribution at the + # 6 rigid DOFs is exactly the external/contact force balance, + # which is what kappa auto-tune wants to match against the + # barrier gradient. grad_energy_full = None for a in assembly._list_assembly: if a is self: @@ -645,10 +662,16 @@ def _ccd_line_search(self, pb, dX): # Current vertices vertices_current = self._get_current_vertices(pb) - # Compute displacement of surface nodes from dX (fedoo blocked layout) + # Surface displacement from dX (fedoo blocked layout). For + # rigid-tied slave nodes, ``_MatCB`` has already written the + # kinematic motion back into the mesh-DOF rows (see + # ``problem.Problem.solve``: ``X = MatCB @ X_free + Xbc``), so we + # can read the surface displacement directly regardless of + # whether a node is deformable or rigid-slaved. disp_ranks = np.array(self.space.get_rank_vector("Disp")) n_nodes = self.mesh.n_nodes - surf_disp = np.zeros((len(self._surface_node_indices), ndim)) + n_surf = len(self._surface_node_indices) + surf_disp = np.zeros((n_surf, ndim)) for d in range(ndim): surf_disp[:, d] = dX[disp_ranks[d] * n_nodes + self._surface_node_indices] @@ -839,8 +862,21 @@ def assemble_global_mat(self, compute="all"): Called by the solver when the tangent matrix needs reassembly without a full ``update()`` cycle. Uses vertex positions cached by the most recent ``update()`` or ``set_start()`` call. + + When rigid bodies are registered the scatter matrix depends on the + current rigid rotation; the MPC coefficients have just been + refreshed by ``update_boundary_conditions`` for this NR step, so + refresh the scatter here too to keep the contact tangent + consistent with the kinematic constraint. """ if self._last_vertices is not None: + if self._rigid_bodies and self._pb is not None: + self._scatter_matrix = self._build_scatter_matrix( + self._surface_node_indices, + self.mesh.n_nodes, + self.space.ndim, + self._pb, + ) self._compute_ipc_contributions(self._last_vertices, compute) def initialize(self, pb): @@ -906,19 +942,16 @@ def initialize(self, pb): for node_set, rb in self._rigid_bodies: self._rigid_node_set.update(node_set.tolist()) - # Validate: CCD and OGC not supported with rigid bodies - if self._use_ccd: - raise ValueError( - "use_ccd=True is not yet supported with rigid bodies. " - "Use use_ccd=False (default)." - ) + # OGC is incompatible with rigid bodies: per-vertex trust + # region cannot be reconciled with the 6-DOF rigid Jacobian + # reduction. CCD is supported — _ccd_line_search reconstructs + # rigid surface motion from dX[rigid_global_dofs] via J. if self._use_ogc: raise ValueError("use_ogc=True is not supported with rigid bodies.") # Build scatter matrix for DOF mapping (ipctk surface -> fedoo full) - vertices = self._get_current_vertices(pb) self._scatter_matrix = self._build_scatter_matrix( - self._surface_node_indices, self.mesh.n_nodes, ndim, pb, vertices + self._surface_node_indices, self.mesh.n_nodes, ndim, pb ) # Create broad phase instance @@ -1120,14 +1153,14 @@ def update(self, pb, compute="all"): vertices = self._get_current_vertices(pb) self._last_vertices = vertices - # Rebuild scatter matrix for rigid bodies (Jacobian depends on position) + # Rebuild scatter matrix for rigid bodies (Jacobian uses r_ref but + # depends on the current rotation, so refresh after every update) if self._rigid_bodies: self._scatter_matrix = self._build_scatter_matrix( self._surface_node_indices, self.mesh.n_nodes, self.space.ndim, pb, - vertices, ) # Rebuild collision set diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 9e6f1f81..1cbe9312 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -69,6 +69,7 @@ def __init__( space=None, beta=0.25, gamma=0.5, + dynamic=True, name="RigidBodyAssembly", ): if space is None: @@ -84,6 +85,14 @@ def __init__( self.beta = beta self.gamma = gamma self.rayleigh_alpha = 0.0 + # Static vs dynamic. In static mode, no Newmark inertia, no + # damping, no v/a state; only external force + contact tangent + # contribute to the 6 rigid DOFs. A tiny diagonal regularisation + # is added to K to keep the linear solve well-posed when the body + # is not yet in contact (otherwise the unconstrained free rigid + # DOFs would produce a singular K row). + self.dynamic = bool(dynamic) + self.static_regularisation = 1e-9 self._dof_indices = None self._pb_ref = None @@ -122,7 +131,7 @@ def initialize(self, pb): if self.mesh is None: self.mesh = pb.mesh - if not np.any(self.sv["Acceleration"]): + if self.dynamic and not np.any(self.sv["Acceleration"]): M = self._get_mass_matrix() self.sv["Acceleration"] = np.linalg.solve(M, self.force) self.sv_start["Acceleration"] = self.sv["Acceleration"].copy() @@ -252,20 +261,51 @@ def _get_n_dof(self): ) def assemble_global_mat(self, compute="all"): - """Assemble effective dynamic stiffness and residual. - - Implements Newmark time integration directly on the 6 rigid DOFs: - - Matrix: K_eff = K_contact + (a0 + α·c0)·M - - Vector: D = F_ext + F_contact + M·(inertia residual) + C·(damping residual) + """Assemble the 6×6 stiffness and residual at the rigid global DOFs. + Dynamic mode (Newmark): + K_eff = K_contact + (a0 + α·c0)·M + D = F_ext + F_contact − M·a_pred − C·v_pred where a0 = 1/(β·dt²), c0 = γ/(β·dt). + + Static mode: + K_eff = K_contact + ε·I_6 (ε tiny, only to break + singularity of fully-free + rigid DOFs without contact) + D = F_ext + F_contact """ if self._dof_indices is None: return n = self._get_n_dof() idx = self._dof_indices - dt = self._pb_ref.dtime if self._pb_ref is not None else 1.0 + + if not self.dynamic: + # Pure static — no Newmark, no v/a state. + if compute in ("all", "matrix"): + K_eff_6 = self._contact_stiffness + ( + self.static_regularisation * np.eye(6) + ) + self.global_matrix = sparse.csr_matrix( + (K_eff_6.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), + shape=(n, n), + ) + if compute in ("all", "vector"): + vec = np.zeros(n) + vec[idx] = self.force + self._contact_force + self.global_vector = vec + return + + # Dynamic mode: guard against dt=0 (init-time probes) by emitting + # zeros instead of dividing. This replaces the previous class-name + # filter band-aid in IPCContact._get_elastic_gradient_on_surface. + dt = self._pb_ref.dtime if self._pb_ref is not None else 0.0 + if dt == 0: + if compute in ("all", "matrix"): + self.global_matrix = sparse.csr_matrix((n, n)) + if compute in ("all", "vector"): + self.global_vector = np.zeros(n) + return M = self._get_mass_matrix() a0 = 1.0 / (self.beta * dt**2) @@ -316,15 +356,21 @@ def update(self, pb, compute="all"): def set_start(self, pb): """Accept converged increment: update velocity and acceleration.""" self._pb_ref = pb - dt = pb.dtime delta_u = self.sv["_DeltaDisp"] - v_n = self.sv["Velocity"] - a_n = self.sv["Acceleration"] self.sv_start = { k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv.items() } + # Static mode: no v/a state to update; just reset the increment. + if not self.dynamic: + self.sv["_DeltaDisp"] = np.zeros(6) + return + + dt = pb.dtime + v_n = self.sv["Velocity"] + a_n = self.sv["Acceleration"] + if not np.any(delta_u): return @@ -356,13 +402,19 @@ class RigidBody: approximation). IPC barrier contact is handled via direct Jacobian projection ``J^T @ F`` in the 6-DOF space. - Solve via ``body.solve(dt, tmax)`` (wraps ``NonLinear``) or integrate - into an existing problem with ``body.add_to_problem(pb)``. + Solve via ``body.solve(dt, tmax)`` (wraps ``NonLinear``) or include + ``body.assembly`` in an ``Assembly.sum`` — the kinematic tie is then + registered automatically when the problem is constructed. Parameters ---------- mesh : fedoo.Mesh - Surface mesh of the rigid body (tri3 for IPC contact). + Mesh of the rigid body. For a standalone body, this is its own + mesh (tri3 surface for IPC contact, or volume for inertia + integration). For a body that is one part of a larger stacked + problem mesh (rigid-vs-deformable IPC), pass the output of + :meth:`Mesh.extract_elements` — the carried ``parent_node_indices`` + attribute tells RigidTie which DOFs to slave in the parent. mass : float, optional Total mass. Required if ``density`` is not given. density : float, optional @@ -390,7 +442,7 @@ class RigidBody: inertia_tensor=0.004*np.eye(3)) body.set_force([0, 0, -9.81]) body.set_rayleigh_damping(1.0) - body.enable_ipc_contact(plane_mesh, dhat=0.01, kappa=1e8) + body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) q, v, a = body.solve(dt=5e-4, tmax=2.0) """ @@ -403,33 +455,59 @@ def __init__( inertia_tensor=None, center_of_mass=None, use_quaternion=True, + dynamic=True, name="RigidBody", ): self.mesh = mesh self.name = name + self.dynamic = bool(dynamic) if center_of_mass is None: - center_of_mass = mesh.nodes.mean(axis=0) + # Use only nodes actually referenced by elements — protects + # the default against ``extract_elements`` submeshes that + # carry the parent's full node array. + active = np.unique(mesh.elements.ravel()) + center_of_mass = mesh.nodes[active].mean(axis=0) center_of_mass = np.asarray(center_of_mass, dtype=float) - if density is not None: - vol = mesh.get_volume() - if mass is None: - mass = density * vol - if inertia_tensor is None: - inertia_tensor = self._compute_inertia(mesh, density, center_of_mass) + if self.dynamic: + # Mass and inertia are only used by the Newmark integrator. + # Static mode allows callers to omit them entirely. + if density is not None: + vol = mesh.get_volume() + if mass is None: + mass = density * vol + if inertia_tensor is None: + inertia_tensor = self._compute_inertia( + mesh, density, center_of_mass + ) + else: + if mass is None: + raise ValueError("Either mass or density must be provided.") + if inertia_tensor is None: + raise ValueError( + "inertia_tensor required when density is not given." + ) else: - if mass is None: - raise ValueError("Either mass or density must be provided.") - if inertia_tensor is None: - raise ValueError("inertia_tensor required when density is not given.") + mass = 0.0 if mass is None else mass + inertia_tensor = ( + np.zeros((3, 3)) if inertia_tensor is None else inertia_tensor + ) self.mass = float(mass) self.center_of_mass = center_of_mass self.inertia_tensor = np.asarray(inertia_tensor, dtype=float) + # When ``mesh`` came from ``Mesh.extract_elements`` it carries + # the parent-mesh active-node list used to slave the right DOFs + # in a stacked rigid-plus-deformable problem. A standalone + # body's mesh has no such attribute — every node is tied. + tie_node_indices = getattr(mesh, "parent_node_indices", None) + if tie_node_indices is None: + tie_node_indices = np.arange(mesh.n_nodes) + self.constraint = RigidTie( - np.arange(mesh.n_nodes), + tie_node_indices, center=center_of_mass, use_quaternion=use_quaternion, name=f"{name}_tie", @@ -439,6 +517,7 @@ def __init__( self.inertia_tensor, self.constraint, mesh=mesh, + dynamic=self.dynamic, name=f"{name}_asm", ) @@ -458,13 +537,25 @@ def set_rayleigh_damping(self, alpha): """Set mass-proportional damping: C = alpha * M.""" self.assembly.rayleigh_alpha = float(alpha) - def enable_ipc_contact(self, obstacle_mesh, dhat=0.01, kappa=None): - """Enable IPC barrier contact with an obstacle surface. + def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): + """Enable IPC barrier contact with a STATIC obstacle surface. + + Builds a private collision mesh and barrier on this rigid body's + :class:`RigidBodyAssembly`. The obstacle is snapshotted once at + setup and treated as frozen geometry — use this for rigid-vs-static + scenarios (e.g., a rigid body falling onto a fixed floor). + + For rigid-vs-deformable contact (e.g., a punch crushing an elastic + disc), build a shared :class:`IPCContact` over the union of all + surfaces and call ``ipc.add_rigid_body(body)`` instead. In that + path the deformable obstacle's vertex positions are tracked at + every NR iteration and the IPC reaction is scattered onto the + obstacle's mesh DOFs (Newton's 3rd law honoured). Parameters ---------- obstacle_mesh : fedoo.Mesh - Surface mesh of the obstacle (tri3). + Surface mesh of the obstacle (tri3), treated as static. dhat : float Barrier activation distance (meters). kappa : float or None @@ -509,8 +600,16 @@ def enable_ipc_contact(self, obstacle_mesh, dhat=0.01, kappa=None): asm._ipc_obstacle_nodes = obst_nodes.copy() def add_to_problem(self, pb): - """Register the rigid body constraint with a Fedoo problem.""" - pb.bc.add(self.constraint) + """Register the rigid body's kinematic tie with a Fedoo problem. + + Usually unnecessary — :class:`NonLinear` auto-registers ties from + any :class:`RigidBodyAssembly` it discovers in its assembly sum. + Kept as an explicit hook for advanced flows (custom problem + classes, late attachment) and made idempotent so calling it + after auto-registration is a no-op. + """ + if self.constraint not in pb.bc: + pb.bc.add(self.constraint) @property def Q_total(self): @@ -543,7 +642,6 @@ def solve(self, dt, tmax, t0=0, print_info=1): from fedoo.problem.non_linear import NonLinear pb = NonLinear(self.assembly) - self.add_to_problem(pb) pb.nlsolve(dt=dt, tmax=tmax, t0=t0, print_info=print_info, update_dt=False) return pb diff --git a/fedoo/core/mesh.py b/fedoo/core/mesh.py index 47ad8bb8..009ffffd 100644 --- a/fedoo/core/mesh.py +++ b/fedoo/core/mesh.py @@ -441,6 +441,54 @@ def find_coincident_nodes(self, tol: float = 1e-8) -> np.ndarray[int]: )[0] # indices of the first coincident nodes return np.array([ind_sorted[ind_coincident], ind_sorted[ind_coincident + 1]]).T + def merge_coincident_nodes(self, tol: float) -> int: + """Find and merge node pairs whose Euclidean distance is below ``tol``. + + Useful when stacking volumes from separate sources (e.g. several + STEP files meshed without a boolean fuse): the interface nodes are + geometrically close but distinct, and elements on either side + don't share connectivity. After merging, an interface node from + one phase and its counterpart from another become a single node, + and any constraint or RigidTie applied via the surviving index + propagates kinematically across the interface. + + The pairing is greedy by distance: closest pair first, each node + merged at most once. This guarantees no element collapse (no two + corners of the same tet end up merged to the same survivor). + + Parameters + ---------- + tol : float + Geometric distance threshold (same units as ``self.nodes``). + + Returns + ------- + int + Number of node pairs merged. + """ + from scipy.spatial import cKDTree + + tree = cKDTree(self.nodes) + pairs = tree.query_pairs(r=tol, output_type="ndarray") + if len(pairs) == 0: + return 0 + dists = np.linalg.norm( + self.nodes[pairs[:, 0]] - self.nodes[pairs[:, 1]], axis=1 + ) + order = np.argsort(dists) + used = np.zeros(self.n_nodes, dtype=bool) + accepted = [] + for k in order: + i, j = int(pairs[k, 0]), int(pairs[k, 1]) + if used[i] or used[j]: + continue + used[i] = used[j] = True + accepted.append((i, j)) + if not accepted: + return 0 + self.merge_nodes(np.asarray(accepted, dtype=int)) + return len(accepted) + def merge_nodes(self, node_couples: np.ndarray[int]) -> None: """ Merge some nodes @@ -567,9 +615,14 @@ def extract_elements(self, element_set: str | list, name: str = "") -> "Mesh": * The new mesh keep the former element_sets dict with only the extrated elements. * The element indices of the new mesh are not the same as the former one. - * The new mesh keep the initial nodes and node_sets. To also removed the - nodes, a simple solution is to use the method "remove_isolated_nodes" - with the new mesh. + * The new mesh keep the initial ``nodes`` array (shared with the parent), so + an assembly built on it stays in the parent DOF space and can be summed + with full-mesh assemblies. To also remove the nodes, a simple solution is + to use the method "remove_isolated_nodes" with the new mesh. + * The result carries a ``parent_node_indices`` attribute listing the rows of + ``self.nodes`` actually referenced by the extracted elements. Consumers + that need the active subset (e.g. ``RigidBody`` slaving the right DOFs) + read it directly without recomputing ``np.unique(elements.ravel())``. """ if isinstance(element_set, str): element_set = self.element_sets[element_set] @@ -588,6 +641,11 @@ def extract_elements(self, element_set: str | list, name: str = "") -> "Mesh": self.local_frame, name=name, ) + # ``parent_node_indices`` flags the nodes actually referenced by + # the extracted elements. Consumers that need the active subset + # (e.g. ``RigidBody`` slaving the right DOFs) can read it + # directly without recomputing ``np.unique(elements.ravel())``. + sub_mesh.parent_node_indices = np.unique(sub_mesh.elements.ravel()) return sub_mesh def nearest_node(self, X: np.ndarray[float]) -> int: diff --git a/fedoo/problem/linear.py b/fedoo/problem/linear.py index 205258ad..129769a5 100644 --- a/fedoo/problem/linear.py +++ b/fedoo/problem/linear.py @@ -11,6 +11,7 @@ def __init__(self, assembly: Assembly, name: str = "MainProblem"): super().__init__(mesh=assembly.mesh, name=name) self.nlgeom = False + self._register_rigid_ties(assembly) assembly.initialize(self) self.time = self.dtime = 0 # self.set_A(assembly.get_global_matrix()) @@ -134,6 +135,25 @@ def GetNodalElasticEnergy(self): def assembly(self): return self.__assembly + def _register_rigid_ties(self, assembly): + """Add any RigidBodyAssembly's tie to ``self.bc`` before assembly init. + + Mirrors ``NonLinear._register_rigid_ties``. Without this the rigid + tie's ``var_cd`` is never populated, and ``RigidBodyAssembly.initialize`` + crashes reading it. + """ + from collections import deque + from fedoo.constraint.rigid_body import RigidBodyAssembly + + queue = deque([assembly]) + while queue: + a = queue.popleft() + if isinstance(a, RigidBodyAssembly) and a.rigid_tie not in self.bc: + self.bc.add(a.rigid_tie) + children = getattr(a, "_list_assembly", None) + if children: + queue.extend(children) + class Linear(_LinearBase, Problem): """Class that defines linear problems. diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index aed131a0..93671a1c 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -61,6 +61,12 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.__assembly = assembly super().__init__(A, B, D, assembly.mesh, name, assembly.space) self.nlgeom = nlgeom + + # Auto-register kinematic ties from any RigidBodyAssembly found in + # the assembly sum. Without this the user has to remember + # ``body.add_to_problem(pb)`` for each rigid body even though the + # body's assembly is already wired in through ``Assembly.sum``. + self._register_rigid_ties(assembly) self.t0 = 0 self.tmax = 1 self.time = 0 @@ -73,6 +79,27 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.exec_callback_at_each_iter = False self.err_num = 1e-8 # numerical error + def _register_rigid_ties(self, assembly): + """Walk the assembly tree and add any RigidBodyAssembly's tie to ``self.bc``. + + FIFO traversal so the registration order matches the order in + which bodies appear inside ``Assembly.sum``. The slot a body + gets in ``RigidTie.node_cd`` is determined by this order, so + switching to LIFO would silently rewire the user's BCs. + """ + # Local import — avoid a hard cycle with fedoo.constraint at module load. + from collections import deque + from fedoo.constraint.rigid_body import RigidBodyAssembly + + queue = deque([assembly]) + while queue: + a = queue.popleft() + if isinstance(a, RigidBodyAssembly) and a.rigid_tie not in self.bc: + self.bc.add(a.rigid_tie) + children = getattr(a, "_list_assembly", None) + if children: + queue.extend(children) + @property def n_iter(self): """Return the number of iterations made to solve the problem.""" @@ -685,18 +712,23 @@ def elastic_prediction(self): dX = self.get_X() alpha = self._step_size_callback(self, dX) if alpha < 1.0: - # Scale only free DOFs; preserve prescribed Dirichlet values - # self.set_X(dX * alpha + self._Xbc * (1 - alpha)) - dX *= alpha + # Scale only the free-DOF part of dX; keep the prescribed + # Dirichlet values unscaled. Otherwise the Dirichlet + # increment is only partially applied to _dU, and the + # leftover (Xbc * (1-alpha)) gets wiped at the next + # update_boundary_conditions (where t_fact - t_fact_old + # == 0), so the BC target is never reached. + self.set_X(dX * alpha + self._Xbc * (1 - alpha)) self._alpha = alpha if self._alpha == 1: self._boundary_is_0 = True - if self._boundary_is_0: - # set the increment Dirichlet boundray conditions to 0 (i.e. will not change during the NR interations) - self._Xbc *= 0 - else: - self._Xbc *= 1 - alpha + # Whether alpha < 1 or alpha == 1, the prescribed Dirichlet + # values have now been fully written into _dU (via the + # compensation term self._Xbc * (1 - alpha) above), so Xbc can + # be zeroed for all subsequent NR iterations of this increment. + self._Xbc *= 0 + self._boundary_is_0 = True # update displacement increment self._dU += self.get_X() @@ -1055,7 +1087,7 @@ def nlsolve( if self.print_info > 0: print( "Iter {} - Time: {:.5f} - dt {:.5f} - NR iter: {} - Err: {:.5f}".format( - self.__iter, self.time, dt, nb_nr_iter, error + self.__iter, self.time, self.dtime, nb_nr_iter, error ) ) diff --git a/tests/test_ipc_rigid_tie_shape.py b/tests/test_ipc_rigid_tie_shape.py new file mode 100644 index 00000000..30300d45 --- /dev/null +++ b/tests/test_ipc_rigid_tie_shape.py @@ -0,0 +1,184 @@ +"""Regression test: IPCContact must not double-count global DOFs. + +Before this fix, ``_compute_ipc_contributions`` padded ``global_matrix`` +and ``global_vector`` by ``self._n_global_dof`` whenever there were extra +global DOFs and no rigid bodies registered via ``add_rigid_body``. The +scatter matrix already maps to the full ``n_dof = nvar*n_nodes + +n_global_dof``, so that padding inflated the result to ``n_dof + +n_global_dof`` rows, mismatching the FEM ``Assembly`` sibling and +raising ``ValueError: inconsistent shapes`` inside ``assembly_sum``. + +The tests below exercise: + * RigidTie (12 global DOFs) + IPCContact across a deformable bottom + + two rigid-tied blocks. + * PeriodicBC + IPCContact on a single periodic box. + +For each, we run ``nlsolve`` for one increment and assert no +``ValueError`` and that the IPC and elastic blocks have the same +matrix/vector shape. +""" + +import os + +os.environ.setdefault("KMP_DUPLICATE_LIB_OK", "TRUE") + +import numpy as np +import pytest + +import fedoo as fd + +ipctk = pytest.importorskip("ipctk") + + +def _reset_space(): + """Ensure each test starts with a fresh ModelingSpace.""" + fd.Assembly.delete_memory() + + +@pytest.fixture +def fresh_3d_space(): + _reset_space() + fd.ModelingSpace("3D") + yield + _reset_space() + + +def test_rigid_tie_plus_ipc_assembles_with_consistent_shape(fresh_3d_space): + """Two RigidTie + IPCContact: matrix shapes must match across all + assemblies in the sum.""" + # Deformable bottom plate + mesh_bot = fd.mesh.box_mesh( + nx=4, + ny=4, + nz=3, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=0.0, + z_max=0.5, + ) + # Two rigid blocks above with an initial gap + mesh_A = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.05, + x_max=0.40, + y_min=0.30, + y_max=0.70, + z_min=0.70, + z_max=0.85, + ) + mesh_B = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.60, + x_max=0.95, + y_min=0.30, + y_max=0.70, + z_min=0.70, + z_max=0.85, + ) + + mesh = fd.Mesh.stack(fd.Mesh.stack(mesh_bot, mesh_A), mesh_B) + n_bot = mesh_bot.n_nodes + nodes_A = np.arange(n_bot, n_bot + mesh_A.n_nodes) + nodes_B = np.arange(n_bot + mesh_A.n_nodes, mesh.n_nodes) + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=True) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.03, + dhat_is_relative=False, + use_ccd=False, + ) + assembly = fd.Assembly.sum(solid, ipc) + + pb = fd.problem.NonLinear(assembly) + # Two RigidTie constraints contribute 12 global DOFs total + pb.bc.add(fd.constraint.RigidTie(nodes_A, name="tieA")) + pb.bc.add(fd.constraint.RigidTie(nodes_B, name="tieB")) + + pb.bc.add("Dirichlet", mesh.find_nodes("Z", 0.0), "Disp", 0) + # Push rigid blocks down — initial gap 0.20, dhat=0.03, push by 0.19 so + # we cross into the barrier zone and exercise the padded-matrix code path. + pb.bc.add("Dirichlet", "RigidDispZ", -0.19) + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=15) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof == 12 + + # Drive nlsolve through one increment. Before the fix this raised + # ``ValueError: inconsistent shapes`` from ``assembly_sum`` the moment + # IPC detected its first collision. + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + assert solid.global_vector.shape == ipc.global_vector.shape + + +def test_periodic_bc_plus_ipc_assembles_with_consistent_shape(fresh_3d_space): + """PeriodicBC + IPCContact: even with no contact, the global-DOF + padding path must produce matrices of size ``n_dof``, not + ``n_dof + n_global_dof``.""" + mesh = fd.mesh.box_mesh( + nx=4, + ny=4, + nz=4, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=0, + z_max=1, + ) + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=False) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + # No initial contact; we only need IPC's assemble path to be exercised + # alongside the PeriodicBC-driven global DOFs. + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.02, + dhat_is_relative=False, + use_ccd=False, + ) + assembly = fd.Assembly.sum(solid, ipc) + + pb = fd.problem.NonLinear(assembly) + pb.bc.add(fd.constraint.PeriodicBC(periodicity_type="small_strain", dim=3)) + # Pin one corner against rigid body translation + corner = mesh.nearest_node([0.0, 0.0, 0.0]) + pb.bc.add("Dirichlet", corner, "Disp", 0) + # Drive a small mean strain — small_strain BC exposes E_xx as a global DOF + pb.bc.add("Dirichlet", "E_xx", 0.005) + for v in ("E_yy", "E_zz", "E_xy", "E_xz", "E_yz"): + pb.bc.add("Dirichlet", v, 0.0) + + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=10) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof > 0 + + pb.nlsolve(dt=1.0, tmax=1.0, update_dt=True, print_info=0) + + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_rigid_body_ipc_deformable.py b/tests/test_rigid_body_ipc_deformable.py new file mode 100644 index 00000000..093a060d --- /dev/null +++ b/tests/test_rigid_body_ipc_deformable.py @@ -0,0 +1,239 @@ +"""Regression test for `RigidBody + IPCContact.add_rigid_body` coupling. + +A coarse version of `examples/contact/ipc/rigid_deformable_punch.py`. The +test asserts: + +1. Matrix/vector shapes are consistent across the assembly sum (regression + guard for the global-DOF padding bug fixed earlier on this branch). +2. The deformable plate actually compresses under the rigid piston — + the bug this entire feature exists to prevent. +3. Newton's third law: the IPC barrier gradient sums to zero between rigid + and deformable surface DOFs. +4. The RigidTie kinematic relation is satisfied exactly on the rigid + surface (max abs error = 0 across X/Y/Z). +5. ``set_static_obstacle`` and ``add_rigid_body`` are mutually exclusive + on a single body. +""" + +import os + +os.environ.setdefault("KMP_DUPLICATE_LIB_OK", "TRUE") + +import numpy as np +import pytest + +import fedoo as fd + +ipctk = pytest.importorskip("ipctk") + + +@pytest.fixture +def fresh_3d_space(): + fd.Assembly.delete_memory() + fd.ModelingSpace("3D") + yield + fd.Assembly.delete_memory() + + +def _build_punch_problem(): + """Disc + one rigid piston above. Returns (pb, ipc, solid, body, mesh, + n_disc, nodes_top).""" + disc = fd.mesh.box_mesh( + nx=6, + ny=6, + nz=3, + x_min=0.0, + x_max=1.0, + y_min=0.0, + y_max=1.0, + z_min=0.0, + z_max=0.10, + ) + top = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.30, + x_max=0.70, + y_min=0.30, + y_max=0.70, + z_min=0.15, + z_max=0.30, + ) + disc.add_element_set(np.arange(disc.n_elements), "disc") + top.add_element_set(np.arange(top.n_elements), "top") + mesh = fd.Mesh.stack(disc, top) + n_disc = disc.n_nodes + top_part = mesh.extract_elements("top") + nodes_top = top_part.parent_node_indices + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=True) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.02, + dhat_is_relative=False, + use_ccd=False, + barrier_stiffness=1e6, + ) + + body = fd.constraint.RigidBody( + top_part, + mass=1.0, + inertia_tensor=0.01 * np.eye(3), + name="piston", + ) + ipc.add_rigid_body(body) + + assembly = fd.Assembly.sum(solid, body.assembly, ipc) + pb = fd.problem.NonLinear(assembly, nlgeom=True) + + # Clamp the disc's four side faces against rigid-body motion. + for axis, value in (("X", 0.0), ("X", 1.0), ("Y", 0.0), ("Y", 1.0)): + pb.bc.add("Dirichlet", mesh.find_nodes(axis, value), "Disp", 0) + + nc = body.constraint.node_cd + pb.bc.add("Dirichlet", nc[0], "RigidDispX", 0.0) + pb.bc.add("Dirichlet", nc[1], "RigidDispY", 0.0) + pb.bc.add("Dirichlet", nc[2], "RigidDispZ", -0.06) + pb.bc.add("Dirichlet", nc[3], "RigidRotX", 0.0) + pb.bc.add("Dirichlet", nc[4], "RigidRotY", 0.0) + pb.bc.add("Dirichlet", nc[5], "RigidRotZ", 0.0) + + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=20) + return pb, ipc, solid, body, mesh, n_disc, nodes_top + + +def test_shapes_consistent_across_assembly_sum(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof == 6 + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + + +def test_deformable_plate_actually_compresses(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + disp = pb.get_disp() + plate_top_nodes = np.where( + (mesh.nodes[:n_disc, 2] > 0.099) + & (mesh.nodes[:n_disc, 0] > 0.25) + & (mesh.nodes[:n_disc, 0] < 0.75) + & (mesh.nodes[:n_disc, 1] > 0.25) + & (mesh.nodes[:n_disc, 1] < 0.75) + )[0] + assert len(plate_top_nodes) > 0 + # The piston bottom (z=0.15) moves to z=0.09; with dhat=0.02 the plate + # top must compress to at least z = 0.09 - 0.02 + small slack. + assert disp[2, plate_top_nodes].min() < -1e-3, ( + f"Plate top did not compress under the punch " + f"(min dz = {disp[2, plate_top_nodes].min():+.4e}). " + "IPC is not coupling the rigid body to the deformable disc." + ) + + +def test_newton_third_law_on_ipc_gradient(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + # IPC barrier gradient summed onto each side via P. + P = ipc._scatter_matrix + grad_surf = ipc._barrier_potential.gradient( + ipc._collisions, ipc._collision_mesh, ipc._get_current_vertices(pb) + ) + F = -ipc._kappa * (P @ grad_surf) + + # Rigid side: 6-DOF block at body's global DOFs (forces only — first 3). + rigid_dofs = body.assembly._dof_indices[:3] + F_rigid = F[rigid_dofs] + + # Deformable side: sum the IPC Z-force at disc surface nodes. + surface_idx = ipc._surface_node_indices + disc_surf = surface_idx[np.isin(surface_idx, np.arange(n_disc))] + nvar = pb.space.nvar + F_disc = np.array([F[d * mesh.n_nodes + disc_surf].sum() for d in range(3)]) + + # Newton 3 (in the Z direction, which is where load lives): + assert abs(F_rigid[2] + F_disc[2]) < 1e-6 * max(abs(F_rigid[2]), 1.0), ( + f"Newton's 3rd law violated: F_rigid_z={F_rigid[2]:+.4e} " + f"F_disc_z={F_disc[2]:+.4e}" + ) + + +def test_rigid_tie_kinematic_error_is_zero(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + disp = pb.get_disp() # shape (3, n_nodes) + body_disp = disp[:, nodes_top].T # (n_body, 3) + + q = pb.get_dof_solution()[body.assembly._dof_indices] + R, *_ = body.constraint._compute_rotation(q[3:]) + r_ref = mesh.nodes[nodes_top] - body.center_of_mass + expected = r_ref @ R.T + body.center_of_mass + q[:3] - mesh.nodes[nodes_top] + assert np.allclose(body_disp, expected, atol=1e-9), ( + f"RigidTie kinematic relation violated: " + f"max abs error = {np.abs(body_disp - expected).max():.4e}" + ) + + +def test_static_obstacle_and_add_rigid_body_are_mutually_exclusive(fresh_3d_space): + block = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.3, + x_max=0.7, + y_min=0.3, + y_max=0.7, + z_min=0.2, + z_max=0.3, + ) + block_surf = fd.mesh.extract_surface(block, quad2tri=True) + obstacle_surf = fd.mesh.extract_surface( + fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=-0.1, + z_max=0.0, + ), + quad2tri=True, + ) + body = fd.constraint.RigidBody( + block_surf, + mass=1.0, + inertia_tensor=0.01 * np.eye(3), + center_of_mass=block_surf.bounding_box.center, + name="b", + ) + body.set_static_obstacle(obstacle_surf, dhat=0.01, kappa=1e6) + + # Now try to plug the same body into a shared IPCContact — must refuse. + ipc = fd.constraint.IPCContact( + block, + surface_mesh=block_surf, + dhat=0.01, + dhat_is_relative=False, + use_ccd=False, + barrier_stiffness=1e6, + ) + with pytest.raises(RuntimeError, match=r"cannot use set_static_obstacle"): + ipc.add_rigid_body(body) + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) From d9bce6bf6fcf88190698b6f71aba5f474dec47ba Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Wed, 3 Jun 2026 00:37:09 +0200 Subject: [PATCH 11/21] Switch to rotation-vector DOFs and no rollback Clarify and change rotation handling: RigidTie now interprets RigidRotX/Y/Z as rotation-vector (exponential-map) components rather than intrinsic Euler XYZ increments. When use_quaternion=True the DOFs are small rotation-vector increments composed multiplicatively into a quaternion base; when False the DOFs represent the total rotation vector directly. Remove the _Q_base backup/restore semantics and make _Q_base advance only on converged increments (set_start). to_start_bc is now a no-op because a failed increment never advances the quaternion base, avoiding accidental loss of the last-converged rotation during dt reductions or failed increments. Tests updated to reflect the correct lifecycle and to assert that converged rotations are preserved and composed multiplicatively. --- fedoo/constraint/rigid_body.py | 4 +- fedoo/constraint/rigid_tie.py | 150 +++++++++++++++++++------------ tests/test_rigid_tie_rollback.py | 78 ++++++++++++---- 3 files changed, 156 insertions(+), 76 deletions(-) diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 1cbe9312..cdd6bebd 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -13,8 +13,8 @@ multiplicative quaternion update (using ``simcoon.Rotation``): - The total rotation is stored as a quaternion ``Q_base`` in the RigidTie. -- The rotation DOFs ``[rx, ry, rz]`` represent a small Euler increment - from the current base state. +- The rotation DOFs ``[rx, ry, rz]`` are rotation-vector components + representing a small increment from the current base state. - At each converged time step, the increment is composed into the quaternion: ``Q_base = R_inc * Q_base`` (quaternion multiplication). - This avoids gimbal lock and supports arbitrarily large rotations. diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 271b269a..259c99bd 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -40,38 +40,49 @@ class RigidTie(BCBase): use_quaternion: bool, optional If True (default), use a multiplicative quaternion update to avoid gimbal lock for large rotations. The rotation DOFs (RigidRotX/Y/Z) - are interpreted as incremental Euler angles relative to a quaternion - base state that is updated at each converged increment. - If False, use the original Euler angle formulation. + are interpreted as the **rotation-vector** components of a small + increment relative to a quaternion base state that is updated at each + converged increment. + If False, there is no base state and the DOFs are the components of + the total rotation vector (a single exponential map). name : str, optional Name of the created boundary condition. The default is "Rigid Tie". 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 after rotation around X) - and finally the rotation around Z" (Z" being 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. + The rotation DOFs ``RigidRotX/Y/Z`` are the components of a **rotation + vector** (exponential map): ``rotvec = theta * axis``, with ``theta`` the + rotation angle and ``axis`` the unit rotation axis, so that + ``R = expm(skew(rotvec))`` (Rodrigues' formula). This is convention-free + — there is no XYZ ordering — and is consistent with the rotation-vector + DOFs used by Fedoo's beam elements. + + .. note:: + This differs from earlier versions of ``RigidTie``, which interpreted + ``RigidRotX/Y/Z`` as intrinsic Euler **XYZ** angles + (``Rotation.from_euler("XYZ", ...)``). For a single-axis rotation the + two parameterizations are identical; they differ only for finite, + simultaneous multi-axis prescribed rotations. Small-strain / + linearized results (rotations near zero) are unchanged. When ``use_quaternion=True`` (default), the total rotation is stored as a quaternion (via ``simcoon.Rotation``) using a multiplicative update: - - The Euler angle DOFs represent a **small incremental** rotation from - the current quaternion base state ``Q_base``. + - The rotation-vector DOFs represent a **small incremental** rotation + ``delta`` from the current quaternion base state ``Q_base``. - At each converged increment, the increment is composed: ``Q_base = Rotation.from_rotvec(delta) * Q_base`` - The total rotation is exact for arbitrarily large angles — no gimbal lock, no small-angle approximation. - - The rotation derivatives ``dR/d(angle)`` used in the MPC linearization + - The rotation derivatives ``dR/d(rotvec)`` used in the MPC linearization are evaluated at the small increment (well-conditioned), then composed with ``R_base`` via the chain rule for exact consistency. + When ``use_quaternion=False``, there is no base state: the DOFs are the + components of the total rotation vector and ``Rotation.from_rotvec`` is + used directly. + Notes ----- @@ -159,10 +170,12 @@ def initialize(self, problem): + self.list_nodes[:, None] ) - # Quaternion base state for multiplicative rotation update + # Quaternion base state for multiplicative rotation update. + # ``_Q_base`` advances only on a converged increment (``set_start``) + # and is never reverted: a failed increment never advances it, so the + # rollback hook ``to_start_bc`` is a no-op. See ``to_start_bc``. if self.use_quaternion: self._Q_base = Rotation.identity() - self._Q_base_backup = Rotation.identity() self._angles_at_base = np.zeros(3) def _get_dof_ref(self, problem): @@ -272,32 +285,35 @@ def set_start(self, problem): """Absorb the converged incremental rotation into the quaternion base. Called by the solver after a converged increment, before _dU is reset. + This is the only place ``_Q_base`` advances; it is never reverted + (see ``to_start_bc``). """ if not self.use_quaternion or not hasattr(self, "_Q_base"): return dof_ref, _ = self._get_dof_ref(problem) angles = dof_ref[3:] if np.any(np.isnan(angles)) or np.any(np.isinf(angles)): - self._Q_base_backup = self._Q_base return delta = angles - self._angles_at_base if not np.allclose(delta, 0, atol=1e-15): R_inc = Rotation.from_rotvec(delta) - self._Q_base_backup = self._Q_base self._Q_base = R_inc * self._Q_base self._angles_at_base = angles.copy() - else: - self._Q_base_backup = self._Q_base def to_start_bc(self, problem): - """Revert the quaternion base after a failed increment. + """No-op rollback hook for a failed increment. - Called by the solver when an increment does not converge and - the state must be rolled back. + The solver advances ``_Q_base`` only via ``set_start``, and only for + *converged* increments (``set_start`` runs at the top of the following + increment). A failed increment therefore never touches ``_Q_base`` or + ``_angles_at_base`` — they already hold the last-converged state — so + rolling back requires no action here. + + The previous implementation reverted to a ``_Q_base_backup`` captured + *before* the last converged advance, which silently discarded the last + converged rotation on any ``dt`` reduction during a rotating solve. """ - if not self.use_quaternion: - return - self._Q_base = self._Q_base_backup + return @property def Q_total(self): @@ -442,68 +458,88 @@ def initialize(self, problem): np.c_[rank * n_nodes, (rank + 1) * n_nodes] + self.list_nodes[:, None] ) - def generate(self, problem, t_fact=1, t_fact_old=None): - mesh = problem.mesh - var_cd = self.var_cd - node_cd = self.node_cd - list_nodes = self.list_nodes - + def _get_dof_ref(self, problem): + """Read current values of the 3 rigid DOFs [dx, dy, rotZ].""" dof_cd = [ problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) + + problem._global_dof.indice_start(self.var_cd[i]) + + self.node_cd[i] + for i in range(len(self.var_cd)) ] - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) else: dof_ref = np.array( [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] ) + return dof_ref, dof_cd - disp_ref = dof_ref[:2] # reference displacement - angles = dof_ref[2] # rotation Z angle - - sin = np.sin(angles) - cos = np.cos(angles) + def _compute_rotation(self, angle): + """2D rotation matrix and its derivative w.r.t. the Z angle. - # Correct displacement of slave nodes to be consistent with the master nodes + A single in-plane angle has no gimbal lock, so the closed-form + ``[[cos, -sin], [sin, cos]]`` is exact for arbitrarily large rotation + — no quaternion base state is needed (unlike 3D ``RigidTie``). + """ + sin = np.sin(angle) + cos = np.cos(angle) R = np.array([[cos, -sin], [sin, cos]]) + dR_drz = np.array([[-sin, -cos], [cos, -sin]]) + return R, dR_drz + def _compute_slave_disp(self, problem, disp_ref, R): + """Compute and write 2D slave node displacements into _dU.""" + mesh = problem.mesh + list_nodes = self.list_nodes new_disp = ( (mesh.nodes[list_nodes] - self.center) @ R.T + self.center + disp_ref - mesh.nodes[list_nodes] ) - - if not (np.array_equal(problem._dU, 0)): + if not np.array_equal(problem._dU, 0): if np.array_equal(problem._U, 0): problem._dU[self._disp_indices] = new_disp else: problem._dU[self._disp_indices] = ( new_disp - problem._U[self._disp_indices] ) + return new_disp - # approche incrémentale: - dR_drz = np.array([[-sin, -cos], [cos, -sin]]) + def pre_update(self, problem): + """Refresh slave node positions in _dU before assembly update. - crd = mesh.nodes[list_nodes, :2] - self.center + Mirrors :meth:`RigidTie.pre_update` for the 2D case so other + assemblies (e.g. IPCContact) see the correct geometry of the rigid + surface. Without this hook a ``RigidTie2D`` combined with IPC contact + would expose stale slave positions for one iteration. + """ + dof_ref, _ = self._get_dof_ref(problem) + disp_ref = dof_ref[:2] + R, _ = self._compute_rotation(dof_ref[2]) + self._compute_slave_disp(problem, disp_ref, R) - du_drz = ( - crd @ dR_drz.T - ) # shape = (nnodes, nvar) with nvar = 3 in 3d (ux, uy, uz) + def generate(self, problem, t_fact=1, t_fact_old=None): + var_cd = self.var_cd + node_cd = self.node_cd + list_nodes = self.list_nodes + + dof_ref, dof_cd = self._get_dof_ref(problem) + disp_ref = dof_ref[:2] # reference displacement + R, dR_drz = self._compute_rotation(dof_ref[2]) # rotation Z angle + + # Correct displacement of slave nodes to be consistent with the masters + self._compute_slave_disp(problem, disp_ref, R) + + # MPC linearization + crd = problem.mesh.nodes[list_nodes, :2] - self.center + du_drz = crd @ dR_drz.T # shape = (nnodes, 2) #### MPC #### - # dU - dU_ref - du_drx*drx_ref - du_dry*dry_ref - du_drz*drz_ref = 0 - # with shapes: dU, du_drx, ... -> (nnodes, nvar) - dU_ref -> (nvar), drx_ref, ... -> scalar + # dUx - dUx_ref - du_drz[:,0]*drz_ref = 0 + # dUy - dUy_ref - du_drz[:,1]*drz_ref = 0 # dU are associated to eliminated dof and should be different than ref dof - # or - # dUx - dUx_ref - du_drx[:,0]*drx_ref - du_dry[:,0]*dry_ref - du_drz[:,0]*drz_ref = 0 - # dUy - dUy_ref - du_drx[1]*drx_ref - du_dry[1]*dry_ref - du_drz[1]*drz_ref = 0 - # dUz - dUz_ref - du_drx[2]*drx_ref - du_dry[2]*dry_ref - du_drz[2]*drz_ref = 0 res = ListBC() res.append( MPC( diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py index b8812580..f0d577e6 100644 --- a/tests/test_rigid_tie_rollback.py +++ b/tests/test_rigid_tie_rollback.py @@ -1,4 +1,18 @@ -"""Quaternion rollback: to_start_bc must exactly restore the pre-set_start state.""" +"""Quaternion base lifecycle: set_start advances, to_start_bc never reverts. + +These tests model the *real* solver sequence (see ``nlsolve``): + +* ``set_start`` is called only for a **converged** increment, at the top of + the following increment — this is the only place ``_Q_base`` advances. +* ``to_start_bc`` is called only for a **failed** increment, which never ran + ``set_start`` — so ``_Q_base`` is already at the last-converged state and + rollback must be a no-op. + +The earlier version of this constraint reverted ``_Q_base`` to a backup +captured *before* the last converged advance, which discarded the last +converged rotation on any ``dt`` reduction. The tests below pin the correct +behaviour. +""" import numpy as np import pytest @@ -12,11 +26,10 @@ def _make_tie_with_fake_problem(angles_seq): - """Build a RigidTie wired to a fake problem that returns `angles_seq[i]` - on the i-th call to `_get_dof_ref`.""" + """Build a RigidTie wired to a fake problem that returns ``angles_seq[i]`` + on the i-th call to ``_get_dof_ref``.""" rt = RigidTie([0, 1], use_quaternion=True) rt._Q_base = Rotation.identity() - rt._Q_base_backup = Rotation.identity() rt._angles_at_base = np.zeros(3) calls = {"i": 0} @@ -37,13 +50,35 @@ def _quat_equal(a, b, atol=1e-14): return np.allclose(qa, qb, atol=atol) or np.allclose(qa, -qb, atol=atol) -def test_to_start_bc_restores_identity_after_failed_increment(): - rt = _make_tie_with_fake_problem([[0.3, -0.4, 0.5]]) +def test_to_start_bc_is_a_noop_on_the_base(): + """Rollback must leave the quaternion base untouched.""" + rt = _make_tie_with_fake_problem([[0.2, 0.1, -0.3]]) + rt.set_start(problem=None) # accept a converged increment + q = rt._Q_base + assert not _quat_equal(q, Rotation.identity()) + + rt.to_start_bc(problem=None) + rt.to_start_bc(problem=None) # idempotent + assert _quat_equal(rt._Q_base, q) + + +def test_failed_increment_does_not_discard_converged_rotation(): + """The bug this fixes: converge A (set_start), then B fails (to_start_bc). + + In the solver, A's ``set_start`` runs at the top of increment B. When B + fails, the rollback must keep the base at A's rotation — *not* revert it + further (which previously sent it all the way back to identity). + """ + a_converged = [0.3, -0.4, 0.5] + rt = _make_tie_with_fake_problem([a_converged]) # one set_start: accept A rt.set_start(problem=None) - assert not _quat_equal(rt._Q_base, Rotation.identity()) + q_after_A = rt._Q_base + assert not _quat_equal(q_after_A, Rotation.identity()) + # Increment B now runs and FAILS; its set_start is never called. The + # solver rolls back via to_start_bc — the base must remain at A. rt.to_start_bc(problem=None) - assert _quat_equal(rt._Q_base, Rotation.identity()) + assert _quat_equal(rt._Q_base, q_after_A) def test_two_successful_increments_compose_multiplicatively(): @@ -60,18 +95,27 @@ def test_two_successful_increments_compose_multiplicatively(): assert np.array_equal(rt._angles_at_base, a2) -def test_rollback_after_converged_increment_reverts_to_prior_base(): - a1 = np.array([0.3, 0.0, 0.0]) - a2 = np.array([0.3, 0.4, 0.0]) - rt = _make_tie_with_fake_problem([a1, a2]) +def test_converge_fail_retry_keeps_first_increment(): + """Full sequence: A converges, B fails and rolls back, B retried converges. - rt.set_start(problem=None) - q_after_first = rt._Q_base + After the retry, the base must equal R(a_B - a_A) * R(a_A) — i.e. the + converged A rotation is preserved through the failed attempt. + """ + a_A = np.array([0.3, 0.0, 0.0]) + a_B = np.array([0.3, 0.4, 0.0]) + # set_start consumes one angle per call: accept A, then accept B (retry). + rt = _make_tie_with_fake_problem([a_A, a_B]) - rt.set_start(problem=None) - rt.to_start_bc(problem=None) + rt.set_start(problem=None) # accept converged A + q_after_A = rt._Q_base + + rt.to_start_bc(problem=None) # increment B fails -> rollback (no-op) + assert _quat_equal(rt._Q_base, q_after_A) - assert _quat_equal(rt._Q_base, q_after_first) + rt.set_start(problem=None) # accept converged B (retried) + expected = Rotation.from_rotvec(a_B - a_A) * Rotation.from_rotvec(a_A) + assert _quat_equal(rt._Q_base, expected) + assert np.array_equal(rt._angles_at_base, a_B) def test_nan_angles_do_not_mutate_base(): From 3f37c691836272a39a0a39b4d51833cb8b296ba1 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Wed, 3 Jun 2026 01:01:23 +0200 Subject: [PATCH 12/21] Fix DOF ref handling and rotation copies RigidTie2D: ensure center is an ndarray when a non-scalar is passed and harden _get_dof_ref against uninitialized solver state. _get_dof_ref now mirrors the guards used elsewhere to handle scalar 0 for get_dof_solution() and _Xbc, returning zeros or the appropriate combinations instead of indexing into scalars. Call sites ignore the unused dof_cd where applicable. Tests: copy Rotation objects by value (Rotation.from_quat(...)) before comparisons so assertions detect reassignments rather than in-place mutations. These changes prevent errors when pre_update reads DOFs before BCs are populated and make tests robust to quaternion mutations. --- fedoo/constraint/rigid_tie.py | 28 +++++++++++++++++++--------- tests/test_rigid_tie_rollback.py | 6 ++++-- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 259c99bd..515464f0 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -439,6 +439,8 @@ def initialize(self, problem): elif np.isscalar(self.center): # initialize the center at a position of a node self.center = problem.mesh.nodes[self.center] + else: + self.center = np.asarray(self.center) dof_indice_disp = problem.add_global_dof( ["RigidDispX", "RigidDispY"], 1, "RidigDisp" ) @@ -459,20 +461,28 @@ def initialize(self, problem): ) def _get_dof_ref(self, problem): - """Read current values of the 3 rigid DOFs [dx, dy, rotZ].""" + """Read current values of the 3 rigid DOFs [dx, dy, rotZ]. + + Mirrors :meth:`RigidTie._get_dof_ref`, including the guards for an + uninitialized state (``get_dof_solution()`` or ``_Xbc`` still the + scalar ``0``) — needed because ``pre_update`` may read the DOFs + before boundary conditions populate ``_Xbc``. + """ dof_cd = [ problem.n_node_dof + problem._global_dof.indice_start(self.var_cd[i]) + self.node_cd[i] for i in range(len(self.var_cd)) ] - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) - else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) - return dof_ref, dof_cd + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(3), dof_cd + return np.array([xbc[dof] for dof in dof_cd]), dof_cd + if np.isscalar(xbc) and xbc == 0: + return np.array([dof_sol[dof] for dof in dof_cd]), dof_cd + return np.array([dof_sol[dof] + xbc[dof] for dof in dof_cd]), dof_cd def _compute_rotation(self, angle): """2D rotation matrix and its derivative w.r.t. the Z angle. @@ -524,7 +534,7 @@ def generate(self, problem, t_fact=1, t_fact_old=None): node_cd = self.node_cd list_nodes = self.list_nodes - dof_ref, dof_cd = self._get_dof_ref(problem) + dof_ref, _ = self._get_dof_ref(problem) disp_ref = dof_ref[:2] # reference displacement R, dR_drz = self._compute_rotation(dof_ref[2]) # rotation Z angle diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py index f0d577e6..4c757dc9 100644 --- a/tests/test_rigid_tie_rollback.py +++ b/tests/test_rigid_tie_rollback.py @@ -54,7 +54,9 @@ def test_to_start_bc_is_a_noop_on_the_base(): """Rollback must leave the quaternion base untouched.""" rt = _make_tie_with_fake_problem([[0.2, 0.1, -0.3]]) rt.set_start(problem=None) # accept a converged increment - q = rt._Q_base + # Copy by value (not a reference) so the assertion catches a regression + # that reassigns _Q_base, not just an in-place mutation. + q = Rotation.from_quat(rt._Q_base.as_quat()) assert not _quat_equal(q, Rotation.identity()) rt.to_start_bc(problem=None) @@ -120,7 +122,7 @@ def test_converge_fail_retry_keeps_first_increment(): def test_nan_angles_do_not_mutate_base(): rt = _make_tie_with_fake_problem([[np.nan, 0.0, 0.0]]) - q_before = rt._Q_base + q_before = Rotation.from_quat(rt._Q_base.as_quat()) # copy by value rt.set_start(problem=None) assert _quat_equal(rt._Q_base, q_before) From 9a9796120526f4c103b05cd5c93b75cd67981ad9 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Thu, 4 Jun 2026 11:50:58 +0200 Subject: [PATCH 13/21] Fix damping, mesh merging, and rigid-body APIs Multiple fixes and improvements across the codebase: - Examples/tests: always import simcoon.Rotation (remove scipy fallback) and update test imports. - Mesh: make merge_coincident_nodes repeat greedy passes until no pairs remain and return total merged; expand docstring to explain clustering behavior. - IPCContact: prevent registering a RigidBody on a different IPCContact mesh (raise RuntimeError) and track registration via _ipc_registered_mesh. - RigidBody docs/example: clarify solve() now returns the NonLinear problem and show how to read rigid DOFs from assembly._dof_indices. - ElasticAnisotropic: clarify comment about early-return when strain is zero to avoid leaving stale tangent. - NonLinear Newmark (nl_newmark): reorganize damping handling in updateA/updateD to compute damping matrix from Rayleigh or explicit assembly; add assert and notes where damping behavior needs verification. - Tests: add regression tests for Rayleigh damping (test_implicit_dynamic_damping.py) and for merging coincident nodes (test_merge_coincident_nodes.py). These changes tighten API semantics, improve robustness for node merging and IPC registration, and add tests to guard damping and merging behavior. --- examples/rigid_body_bunny_bounce.py | 5 +- examples/rigid_body_cow_bounce.py | 5 +- fedoo/constitutivelaw/elastic_anisotropic.py | 6 +- fedoo/constraint/ipc_contact.py | 12 ++- fedoo/constraint/rigid_body.py | 5 +- fedoo/core/mesh.py | 57 +++++++------ fedoo/problem/nl_newmark.py | 79 +++++++++-------- tests/test_implicit_dynamic_damping.py | 89 ++++++++++++++++++++ tests/test_merge_coincident_nodes.py | 64 ++++++++++++++ tests/test_rigid_body_kinematics.py | 5 +- tests/test_rigid_tie_rollback.py | 5 +- 11 files changed, 254 insertions(+), 78 deletions(-) create mode 100644 tests/test_implicit_dynamic_damping.py create mode 100644 tests/test_merge_coincident_nodes.py diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index ad29cc43..5b74d64d 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -11,10 +11,7 @@ import fedoo as fd -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation +from simcoon import Rotation # required dependency (fedoo imports it at load) g = 9.81 dt = 5e-4 diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 35888f69..16eaaa6f 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -11,10 +11,7 @@ import fedoo as fd -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation +from simcoon import Rotation # required dependency (fedoo imports it at load) g = 9.81 dt = 5e-4 diff --git a/fedoo/constitutivelaw/elastic_anisotropic.py b/fedoo/constitutivelaw/elastic_anisotropic.py index 7bc8d892..a2fb0a68 100644 --- a/fedoo/constitutivelaw/elastic_anisotropic.py +++ b/fedoo/constitutivelaw/elastic_anisotropic.py @@ -47,7 +47,11 @@ def update(self, assembly, pb): else: total_strain = assembly.sv["Strain"] - # Handle uninitialized strain (e.g. first Newmark step with zero displacement) + # Handle uninitialized strain (e.g. first Newmark step with zero + # displacement). The tangent ``H`` has already been stored in + # ``assembly.sv["TangentMatrix"]`` above, so the early return only + # skips the stress evaluation (correctly zero for zero strain) — no + # stale tangent is left behind. if np.isscalar(total_strain) and total_strain == 0: assembly.sv["Stress"] = 0 return diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index 34bcd427..4b7361ca 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -284,9 +284,19 @@ def add_rigid_body(self, rigid_body, surface_node_indices=None): else: surface_node_indices = np.asarray(surface_node_indices, dtype=int) # Point the body's RigidBodyAssembly at the shared problem mesh so - # ``Assembly.sum`` accepts (mass / inertia / Newmark stay 6x6 and + # ``Assembly.sum`` accepts it (mass / inertia / Newmark stay 6x6 and # are scattered at the 6 global DOFs regardless of mesh size). + # This rebinds ``rigid_body.assembly.mesh`` as a side effect; guard + # against silently re-pointing a body already registered on a + # *different* IPCContact mesh (re-registering on the same mesh is OK). + prev_mesh = getattr(rigid_body, "_ipc_registered_mesh", None) + if prev_mesh is not None and prev_mesh is not self.mesh: + raise RuntimeError( + f"{rigid_body.name}: already registered on another IPCContact " + "mesh; create a fresh RigidBody per shared-IPC problem." + ) rigid_body.assembly.mesh = self.mesh + rigid_body._ipc_registered_mesh = self.mesh self._rigid_bodies.append((surface_node_indices, rigid_body)) def _create_broad_phase(self): diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index cdd6bebd..0a22dc9a 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -444,7 +444,10 @@ class RigidBody: body.set_rayleigh_damping(1.0) body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) - q, v, a = body.solve(dt=5e-4, tmax=2.0) + # solve() returns the underlying NonLinear problem; read the rigid + # DOFs from it via the assembly's _dof_indices. + pb = body.solve(dt=5e-4, tmax=2.0) + q = pb.get_dof_solution()[body.assembly._dof_indices] """ def __init__( diff --git a/fedoo/core/mesh.py b/fedoo/core/mesh.py index 009ffffd..f12c55d7 100644 --- a/fedoo/core/mesh.py +++ b/fedoo/core/mesh.py @@ -452,9 +452,15 @@ def merge_coincident_nodes(self, tol: float) -> int: and any constraint or RigidTie applied via the surviving index propagates kinematically across the interface. - The pairing is greedy by distance: closest pair first, each node - merged at most once. This guarantees no element collapse (no two - corners of the same tet end up merged to the same survivor). + The pairing is greedy by distance (closest pair first, each node + merged at most once per pass), and the whole pass is repeated until no + coincident pair remains. A single greedy pass merges only one pair of + any cluster, so a point shared by three or more parts (e.g. an edge + where 3+ volumes meet) would be left partially merged; repeating the + pass collapses the whole cluster to a single survivor. Within a pass + the each-node-once rule still prevents two corners of one element + collapsing to the same survivor (only degenerate sub-``tol`` elements + could). Parameters ---------- @@ -464,30 +470,33 @@ def merge_coincident_nodes(self, tol: float) -> int: Returns ------- int - Number of node pairs merged. + Total number of node pairs merged across all passes. """ from scipy.spatial import cKDTree - tree = cKDTree(self.nodes) - pairs = tree.query_pairs(r=tol, output_type="ndarray") - if len(pairs) == 0: - return 0 - dists = np.linalg.norm( - self.nodes[pairs[:, 0]] - self.nodes[pairs[:, 1]], axis=1 - ) - order = np.argsort(dists) - used = np.zeros(self.n_nodes, dtype=bool) - accepted = [] - for k in order: - i, j = int(pairs[k, 0]), int(pairs[k, 1]) - if used[i] or used[j]: - continue - used[i] = used[j] = True - accepted.append((i, j)) - if not accepted: - return 0 - self.merge_nodes(np.asarray(accepted, dtype=int)) - return len(accepted) + total_merged = 0 + while True: + tree = cKDTree(self.nodes) + pairs = tree.query_pairs(r=tol, output_type="ndarray") + if len(pairs) == 0: + break + dists = np.linalg.norm( + self.nodes[pairs[:, 0]] - self.nodes[pairs[:, 1]], axis=1 + ) + order = np.argsort(dists) + used = np.zeros(self.n_nodes, dtype=bool) + accepted = [] + for k in order: + i, j = int(pairs[k, 0]), int(pairs[k, 1]) + if used[i] or used[j]: + continue + used[i] = used[j] = True + accepted.append((i, j)) + if not accepted: + break + self.merge_nodes(np.asarray(accepted, dtype=int)) + total_merged += len(accepted) + return total_merged def merge_nodes(self, node_couples: np.ndarray[int]) -> None: """ diff --git a/fedoo/problem/nl_newmark.py b/fedoo/problem/nl_newmark.py index e4a5743d..28354c4d 100644 --- a/fedoo/problem/nl_newmark.py +++ b/fedoo/problem/nl_newmark.py @@ -52,35 +52,28 @@ def __init__( self.__Velocity = 0 self.__Acceleration = 0 - def _get_damping_matrix(self): - """Compute the damping matrix C from Rayleigh model or explicit assembly. - - Returns None if no damping is configured. - Rayleigh damping: C = alpha * M + beta * K - """ - if self.__RayleighDamping is not None: - return ( - self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() - + self.__RayleighDamping[1] - * self.__StiffnessAssembly.get_global_matrix() - ) - elif self.__DampingAssembly is not None: - return self.__DampingAssembly.get_global_matrix() - return None - def updateA(self): # internal function to be used when modifying M, K or C dt = self.dtime - K = self.__StiffnessAssembly.get_global_matrix() - M = self.__MassAssembly.get_global_matrix() - C = self._get_damping_matrix() - - if C is None: - self.set_A(K + 1 / (self.__Beta * (dt**2)) * M) + if self.__DampingAssembly is None: + self.set_A( + self.__StiffnessAssembly.get_global_matrix() + + 1 / (self.__Beta * (dt**2)) * self.__MassAssembly.get_global_matrix() + ) else: + if self.__RayleighDamping is not None: + # In this case, self.__RayleighDamping = [alpha, beta] + DampMatrix = ( + self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() + + self.__RayleighDamping[1] + * self.__StiffnessAssembly.get_global_matrix() + ) + else: + DampMatrix = self.__DampingAssembly.get_global_matrix() + self.set_A( - K - + 1 / (self.__Beta * (dt**2)) * M - + self.__Gamma / (self.__Beta * dt) * C + self.__StiffnessAssembly.get_global_matrix() + + 1 / (self.__Beta * (dt**2)) * self.__MassAssembly.get_global_matrix() + + self.__Gamma / (self.__Beta * dt) * DampMatrix ) def updateD(self, start=False): @@ -94,6 +87,7 @@ def updateD(self, start=False): self.set_D(0) return else: + # DeltaDisp = self._NonLinear__TotalDisplacementOld - self._NonLinear__TotalDisplacementStart DeltaDisp = self._dU D = ( @@ -105,17 +99,32 @@ def updateD(self, start=False): ) + self.__StiffnessAssembly.get_global_vector() ) - - # Damping contribution: D += -C * v_new - # where v_new = (1 - γ/β)*V + (γ/(β*dt))*ΔU + dt*(1 - γ/(2β))*A - C = self._get_damping_matrix() - if C is not None: - new_velocity = ( - (1 - self.__Gamma / self.__Beta) * self.__Velocity - + (self.__Gamma / (self.__Beta * dt)) * DeltaDisp - + dt * (1 - self.__Gamma / (2 * self.__Beta)) * self.__Acceleration + if self.__DampingAssembly is not None: + if self.__RayleighDamping is not None: + # In this case, self.__RayleighDamping = [alpha, beta] + DampMatrix = ( + self.__RayleighDamping[0] * self.__MassAssembly.get_global_matrix() + + self.__RayleighDamping[1] + * self.__StiffnessAssembly.get_global_matrix() + ) + else: + DampMatrix = self.__DampingAssembly.get_global_matrix() + + assert 0, "Non linear Dynamic problem with damping needs to be checked" + # need to be cheched + + # new_velocity = dt * 0.5*(2 - self.gamma/self.beta)*acceleration +(ou -) + # self.gamma/(self.beta*dt)) * delta_disp + # (1 - self.gamma/(self.beta))*velocity + + # D += DampMatrix * (-new_velocity) + # check if same as below + + D += DampMatrix * ( + (self.__Gamma / (self.__Beta * dt)) * DisplacementStart + + (self.__Gamma / self.__Beta - 1) * self.__Velocity + + (0.5 * dt * (self.__Gamma / self.__Beta - 2)) * self.__Acceleration ) - D += C @ (-new_velocity) self.set_D(D) diff --git a/tests/test_implicit_dynamic_damping.py b/tests/test_implicit_dynamic_damping.py new file mode 100644 index 00000000..1139307f --- /dev/null +++ b/tests/test_implicit_dynamic_damping.py @@ -0,0 +1,89 @@ +"""Rayleigh damping validation for the ImplicitDynamic weak form. + +``ImplicitDynamic`` (a Newmark weak form summed onto a standard ``NonLinear`` +problem) is fedoo's current implicit-dynamics path — the same +dynamics-as-assembly architecture as this PR's ``RigidBodyAssembly`` (and +unlike the legacy ``NonLinearNewmark`` problem class). The existing +``test_2DDynamicPlasticBending_v2`` sets ``rayleigh_damping`` but asserts +nothing about it; these tests pin the actual behaviour. +""" + +import numpy as np +import pytest + +import fedoo as fd + + +def _build_axial_bar(rayleigh=None): + """Axial bar (nu=0), fixed at x=0, constant tip load in X, ImplicitDynamic.""" + fd.Assembly.delete_memory() + fd.ModelingSpace("2Dplane") + E, nu, rho = 1.0, 0.0, 1.0 + fd.mesh.rectangle_mesh( + nx=9, + ny=3, + x_min=0, + x_max=1.0, + y_min=0, + y_max=0.2, + elm_type="quad4", + name="Domain", + ) + mesh = fd.Mesh["Domain"] + fd.constitutivelaw.ElasticIsotrop(E, nu, name="law") + wf = fd.weakform.ImplicitDynamic("law", rho, 0.25, 0.5) + if rayleigh is not None: + wf.rayleigh_damping = rayleigh # [alpha (mass), beta (stiffness)] + fd.Assembly.create(wf, "Domain", "quad4", name="asm") + pb = fd.problem.NonLinear("asm") + pb.bc.add("Dirichlet", mesh.find_nodes("X", 0.0), "Disp", 0) + pb.bc.add("Neumann", mesh.find_nodes("X", 1.0), "DispX", 0.05) + return pb, mesh + + +def test_rayleigh_damping_property_roundtrip(): + """``rayleigh_damping = [a, b]`` must read back as ``[a, b]`` (mass, stiff).""" + fd.Assembly.delete_memory() + fd.ModelingSpace("2Dplane") + fd.constitutivelaw.ElasticIsotrop(1.0, 0.0, name="law") + wf = fd.weakform.ImplicitDynamic("law", 1.0, 0.25, 0.5) + assert wf.rayleigh_damping is None + wf.rayleigh_damping = [0.7, 0.2] + assert wf.rayleigh_damping == [0.7, 0.2] + + +def _max_speed_over_run(rayleigh): + pb, mesh = _build_axial_bar(rayleigh=rayleigh) + speeds = [] + + def cb(p): + v = p.get_results("asm", ["Velocity"], "Node").node_data["Velocity"] + speeds.append(float(np.abs(v).max())) + + pb.nlsolve( + dt=0.05, + tmax=8.0, + update_dt=False, + print_info=0, + callback=cb, + exec_callback_at_each_iter=True, + ) + return max(speeds) if speeds else 0.0 + + +def test_implicit_dynamic_rayleigh_damping_reduces_peak_velocity(): + """Heavy mass-proportional Rayleigh damping must remove kinetic energy. + + A wrong-sign damping term would amplify the response, so this also pins + the sign of the damping contribution. + """ + v_undamped = _max_speed_over_run(None) + v_damped = _max_speed_over_run([20.0, 0.0]) + assert v_undamped > 0.0 + assert ( + v_damped < 0.5 * v_undamped + ), f"damped peak speed {v_damped:.3e} not < 0.5 * undamped {v_undamped:.3e}" + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_merge_coincident_nodes.py b/tests/test_merge_coincident_nodes.py new file mode 100644 index 00000000..a1222a1d --- /dev/null +++ b/tests/test_merge_coincident_nodes.py @@ -0,0 +1,64 @@ +"""Regression tests for Mesh.merge_coincident_nodes. + +In particular, a point shared by three or more parts must collapse to a single +survivor — a single greedy pass would merge only one pair of the cluster and +leave the rest split. +""" + +import numpy as np +import pytest + +import fedoo as fd + + +def test_merge_pair(): + nodes = np.array([[0, 0, 0], [1e-9, 0, 0], [1, 0, 0], [1, 1, 0]], dtype=float) + elements = np.array([[0, 2, 3], [1, 3, 2]]) + m = fd.Mesh(nodes, elements, "tri3") + merged = m.merge_coincident_nodes(tol=1e-6) + assert merged == 1 + assert m.n_nodes == 3 + # The two coincident corners now reference the same surviving node. + assert m.elements[0, 0] == m.elements[1, 0] + + +def test_merge_triple_point_collapses_to_single_node(): + """Three parts meeting at one point must all share a single node.""" + eps = 1e-9 + nodes = np.array( + [ + [0, 0, 0], + [eps, 0, 0], + [0, eps, 0], # three near-coincident corners + [1, 0, 0], + [0, 1, 0], + [2, 0, 0], + [2, 1, 0], + [-1, 0, 0], + [-1, 1, 0], + ], + dtype=float, + ) + elements = np.array([[0, 3, 4], [1, 5, 6], [2, 7, 8]]) + m = fd.Mesh(nodes, elements, "tri3") + + merged = m.merge_coincident_nodes(tol=1e-6) + + assert merged == 2 # two pairs collapse the 3-node cluster to 1 + assert m.n_nodes == 7 # 9 - 2 + shared = m.elements[:, 0] + assert ( + len(set(shared.tolist())) == 1 + ), f"triple point not fully merged: corner ids {shared}" + + +def test_merge_noop_when_no_coincident_nodes(): + nodes = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=float) + elements = np.array([[0, 1, 2]]) + m = fd.Mesh(nodes, elements, "tri3") + assert m.merge_coincident_nodes(tol=1e-6) == 0 + assert m.n_nodes == 3 + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py index 874b2a39..ace94616 100644 --- a/tests/test_rigid_body_kinematics.py +++ b/tests/test_rigid_body_kinematics.py @@ -10,10 +10,7 @@ import fedoo as fd from fedoo.constraint.rigid_body import RigidBodyAssembly -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation +from simcoon import Rotation # required dependency (fedoo imports it at load) @pytest.fixture diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py index 4c757dc9..2d84b023 100644 --- a/tests/test_rigid_tie_rollback.py +++ b/tests/test_rigid_tie_rollback.py @@ -19,10 +19,7 @@ from fedoo.constraint.rigid_tie import RigidTie -try: - from simcoon import Rotation -except ImportError: - from scipy.spatial.transform import Rotation +from simcoon import Rotation # required dependency (fedoo imports it at load) def _make_tie_with_fake_problem(angles_seq): From 73618eb89a06602b2a93da4ddb26e13326cc68f6 Mon Sep 17 00:00:00 2001 From: pruliere Date: Thu, 16 Jul 2026 10:14:35 +0200 Subject: [PATCH 14/21] Use embedded time integrators for rigid bodies --- examples/rigid_body_bounce_ipc.py | 1 + examples/rigid_body_bunny_bounce.py | 1 + examples/rigid_body_cow_bounce.py | 1 + fedoo/constraint/rigid_body.py | 327 ++++++++++++++-------------- fedoo/constraint/rigid_tie.py | 235 ++++++++++++++------ fedoo/problem/linear.py | 30 +-- fedoo/problem/non_linear.py | 60 ++++- fedoo/time/base.py | 5 + fedoo/time/generalized_alpha.py | 208 ++++++++++++++++++ tests/test_rigid_body_dynamics.py | 147 +++++++++++++ tests/test_rigid_body_freefall.py | 4 +- tests/test_rigid_body_kinematics.py | 88 +++++++- tests/test_rigid_tie_2d.py | 49 +++++ tests/test_rigid_tie_derivatives.py | 34 ++- tests/test_rigid_tie_rollback.py | 149 ++++--------- 15 files changed, 972 insertions(+), 367 deletions(-) create mode 100644 tests/test_rigid_body_dynamics.py create mode 100644 tests/test_rigid_tie_2d.py diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index 7cc3a8fc..60f622a4 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -55,6 +55,7 @@ # Solve using NonLinear via manual time stepping for trajectory collection pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) pb.initialize() idx = body.assembly._dof_indices diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py index 5b74d64d..0841c328 100644 --- a/examples/rigid_body_bunny_bounce.py +++ b/examples/rigid_body_bunny_bounce.py @@ -73,6 +73,7 @@ # Solve with manual loop for trajectory pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) pb.initialize() idx = body.assembly._dof_indices diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py index 16eaaa6f..afd27107 100644 --- a/examples/rigid_body_cow_bounce.py +++ b/examples/rigid_body_cow_bounce.py @@ -73,6 +73,7 @@ # Solve with NonLinear (manual stepping for trajectory) pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) pb.initialize() idx = body.assembly._dof_indices diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 0a22dc9a..71ab1f6b 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -9,15 +9,10 @@ Large rotations --------------- -Rotations are handled exactly (no small-angle approximation) via a -multiplicative quaternion update (using ``simcoon.Rotation``): - -- The total rotation is stored as a quaternion ``Q_base`` in the RigidTie. -- The rotation DOFs ``[rx, ry, rz]`` are rotation-vector components - representing a small increment from the current base state. -- At each converged time step, the increment is composed into the - quaternion: ``Q_base = R_inc * Q_base`` (quaternion multiplication). -- This avoids gimbal lock and supports arbitrarily large rotations. +By default, each step uses an incremental rotation vector composed +multiplicatively with the last converged quaternion orientation. ``RigidTie`` +evaluates the exact rotation matrix and its derivatives, so the kinematics do +not rely on a small-angle approximation. The contact Jacobian ``J = [I_3 | dR/d(angle) @ r_ref]`` uses the exact rotation derivatives from ``RigidTie._compute_rotation()``, not the @@ -26,8 +21,11 @@ import numpy as np from scipy import sparse + from fedoo.core.base import AssemblyBase +from fedoo.core.time_evolution import SECOND_ORDER from fedoo.constraint.rigid_tie import RigidTie +from fedoo.time.common import RayleighDamping class RigidBodyAssembly(AssemblyBase): @@ -41,8 +39,8 @@ class RigidBodyAssembly(AssemblyBase): M = [[m*I_3, 0 ], [ 0, J_global]] - where ``J_global = Q.apply_tensor(J_body)`` is the inertia tensor rotated - to the current configuration via the RigidTie's quaternion. + where ``J_global = R @ J_body @ R.T`` is the inertia tensor rotated + to the current trial configuration. Parameters ---------- @@ -67,8 +65,6 @@ def __init__( rigid_tie, mesh=None, space=None, - beta=0.25, - gamma=0.5, dynamic=True, name="RigidBodyAssembly", ): @@ -82,27 +78,23 @@ def __init__( self.rigid_tie = rigid_tie self.force = np.zeros(6) self.mesh = mesh - self.beta = beta - self.gamma = gamma - self.rayleigh_alpha = 0.0 - # Static vs dynamic. In static mode, no Newmark inertia, no - # damping, no v/a state; only external force + contact tangent - # contribute to the 6 rigid DOFs. A tiny diagonal regularisation - # is added to K to keep the linear solve well-posed when the body - # is not yet in contact (otherwise the unconstrained free rigid - # DOFs would produce a singular K row). + self.time_evolution = SECOND_ORDER if dynamic else None + self.storage = self if dynamic else None + self.dissipation = None + self.constraints = (rigid_tie,) + self._time_integrator = None + self._fedoo_time_integrated = False self.dynamic = bool(dynamic) + # A compiled time integrator supplies a mass tangent. Without one, + # the assembly is static and this tiny diagonal keeps free rigid DOFs + # well posed before contact. self.static_regularisation = 1e-9 self._dof_indices = None self._pb_ref = None - self.sv = { - "Velocity": np.zeros(6), - "Acceleration": np.zeros(6), - "_DeltaDisp": np.zeros(6), - } - self.sv_start = {k: v.copy() for k, v in self.sv.items()} + self.sv = {} + self.sv_start = {} self._ipc_collision_mesh = None self._ipc_collisions = None @@ -131,10 +123,11 @@ def initialize(self, pb): if self.mesh is None: self.mesh = pb.mesh - if self.dynamic and not np.any(self.sv["Acceleration"]): - M = self._get_mass_matrix() - self.sv["Acceleration"] = np.linalg.solve(M, self.force) - self.sv_start["Acceleration"] = self.sv["Acceleration"].copy() + if SECOND_ORDER not in getattr(pb, "time_integrators", {}): + self._time_integrator = None + self._fedoo_time_integrated = False + elif self._time_integrator is not None: + self._time_integrator.initialize(self, pb) @property def dof_indices(self): @@ -242,16 +235,100 @@ def compute_contact(self, q, rt, compute="all"): return self._contact_force, self._contact_stiffness - def _get_mass_matrix(self): - """6x6 mass matrix in global frame.""" - M = np.zeros((6, 6)) - M[0, 0] = M[1, 1] = M[2, 2] = self.mass - Q = self.rigid_tie.Q_total - M[3:, 3:] = ( - Q.apply_tensor(self.inertia_body) if Q is not None else self.inertia_body + @property + def time_dof_indices(self): + return self._dof_indices + + def _get_rotation_inertia(self, pb=None): + """Return trial orientation, world inertia, and its DOF derivatives.""" + pb = pb or self._pb_ref + rotvec = np.zeros(3) + if pb is not None and self._dof_indices is not None: + dof_ref, _ = self.rigid_tie._get_dof_ref(pb) + rotvec = np.asarray(dof_ref[3:], dtype=float) + + R, *dR = self.rigid_tie._compute_rotation(rotvec) + inertia = R @ self.inertia_body @ R.T + derivatives = tuple( + derivative @ self.inertia_body @ R.T + + R @ self.inertia_body @ derivative.T + for derivative in dR ) + return R, inertia, derivatives + + def _get_mass_matrix(self, pb=None): + """Return the rigid-body mass matrix in the current global frame.""" + M = np.zeros((6, 6)) + M[:3, :3] = self.mass * np.eye(3) + _, M[3:, 3:], _ = self._get_rotation_inertia(pb) return M + def get_time_inertia_force(self, pb, acceleration, velocity): + """Return translational inertia and Euler's rotational inertia force.""" + acceleration = np.asarray(acceleration, dtype=float) + velocity = np.asarray(velocity, dtype=float) + force = np.zeros(6) + force[:3] = self.mass * acceleration[:3] + + _, inertia, _ = self._get_rotation_inertia(pb) + angular_acceleration = acceleration[3:] + angular_velocity = velocity[3:] + force[3:] = inertia @ angular_acceleration + if self.rigid_tie.use_quaternion: + force[3:] += np.cross( + angular_velocity, inertia @ angular_velocity + ) + return force + + def get_time_inertia_tangent( + self, + pb, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ): + """Return the effective tangent of the rigid-body inertia force.""" + acceleration = np.asarray(acceleration, dtype=float) + velocity = np.asarray(velocity, dtype=float) + tangent = np.zeros((6, 6)) + tangent[:3, :3] = acceleration_factor * self.mass * np.eye(3) + + _, inertia, inertia_derivatives = self._get_rotation_inertia(pb) + if not self.rigid_tie.use_quaternion: + tangent[3:, 3:] = acceleration_factor * inertia + return tangent + + angular_acceleration = acceleration[3:] + angular_velocity = velocity[3:] + angular_momentum = inertia @ angular_velocity + basis = np.eye(3) + for axis, inertia_derivative in enumerate(inertia_derivatives): + d_acceleration = acceleration_factor * basis[axis] + d_velocity = velocity_factor * basis[axis] + tangent[3:, 3 + axis] = ( + inertia_derivative @ angular_acceleration + + inertia @ d_acceleration + + np.cross(d_velocity, angular_momentum) + + np.cross( + angular_velocity, + inertia_derivative @ angular_velocity + + inertia @ d_velocity, + ) + ) + return tangent + + def get_storage_matrix(self, pb=None): + """Assembly-level storage provider used by fedoo.time.""" + return self._get_mass_matrix(pb) + + def get_time_initial_force(self, pb=None): + return self.force + self._contact_force + + def get_time_stiffness_matrix(self, pb=None): + """Return the static/contact tangent before time integration.""" + return self._contact_stiffness + def _get_n_dof(self): if self._pb_ref is not None: return self._pb_ref.n_dof @@ -261,133 +338,53 @@ def _get_n_dof(self): ) def assemble_global_mat(self, compute="all"): - """Assemble the 6×6 stiffness and residual at the rigid global DOFs. - - Dynamic mode (Newmark): - K_eff = K_contact + (a0 + α·c0)·M - D = F_ext + F_contact − M·a_pred − C·v_pred - where a0 = 1/(β·dt²), c0 = γ/(β·dt). - - Static mode: - K_eff = K_contact + ε·I_6 (ε tiny, only to break - singularity of fully-free - rigid DOFs without contact) - D = F_ext + F_contact - """ + """Assemble static/contact terms, then apply the attached integrator.""" if self._dof_indices is None: return n = self._get_n_dof() idx = self._dof_indices - - if not self.dynamic: - # Pure static — no Newmark, no v/a state. - if compute in ("all", "matrix"): - K_eff_6 = self._contact_stiffness + ( - self.static_regularisation * np.eye(6) - ) - self.global_matrix = sparse.csr_matrix( - (K_eff_6.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), - shape=(n, n), - ) - if compute in ("all", "vector"): - vec = np.zeros(n) - vec[idx] = self.force + self._contact_force - self.global_vector = vec - return - - # Dynamic mode: guard against dt=0 (init-time probes) by emitting - # zeros instead of dividing. This replaces the previous class-name - # filter band-aid in IPCContact._get_elastic_gradient_on_surface. - dt = self._pb_ref.dtime if self._pb_ref is not None else 0.0 - if dt == 0: - if compute in ("all", "matrix"): - self.global_matrix = sparse.csr_matrix((n, n)) - if compute in ("all", "vector"): - self.global_vector = np.zeros(n) - return - - M = self._get_mass_matrix() - a0 = 1.0 / (self.beta * dt**2) - c0 = self.gamma / (self.beta * dt) - alpha = self.rayleigh_alpha + regularisation = ( + self.static_regularisation if self._time_integrator is None else 0.0 + ) if compute in ("all", "matrix"): - K_eff_6 = self._contact_stiffness + (a0 + alpha * c0) * M + stiffness = self._contact_stiffness + regularisation * np.eye(6) self.global_matrix = sparse.csr_matrix( - (K_eff_6.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), + (stiffness.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), shape=(n, n), ) - if compute in ("all", "vector"): - v_n = self.sv["Velocity"] - a_n = self.sv["Acceleration"] - delta_u = self.sv["_DeltaDisp"] - - a_pred = a0 * (delta_u - dt * v_n) + (1 - 0.5 / self.beta) * a_n - v_pred = ( - (1 - self.gamma / self.beta) * v_n - + (c0 * delta_u) - + dt * (1 - self.gamma / (2 * self.beta)) * a_n - ) - C = alpha * M + self.global_vector = np.zeros(n) + self.global_vector[idx] = self.force + self._contact_force - D_6 = self.force + self._contact_force - M @ a_pred - C @ v_pred - vec = np.zeros(n) - vec[idx] = D_6 - self.global_vector = vec + if self._time_integrator is not None: + self._time_integrator.integrate(self, self._pb_ref, compute) def update(self, pb, compute="all"): - """Update state from current displacement increment.""" self._pb_ref = pb - dof_sol = pb.get_dof_solution() - if not (np.isscalar(dof_sol) and dof_sol == 0): - if np.isscalar(pb._dU) and pb._dU == 0: - self.sv["_DeltaDisp"] = np.zeros(6) - else: - self.sv["_DeltaDisp"] = pb._dU[self._dof_indices] + if self._time_integrator is not None: + self._time_integrator.update(self, pb) + dof_solution = pb.get_dof_solution() if self._ipc_collision_mesh is not None: - q = dof_sol[self._dof_indices] if not np.isscalar(dof_sol) else np.zeros(6) + q = ( + np.asarray(dof_solution)[self._dof_indices] + if not np.isscalar(dof_solution) + else np.zeros(6) + ) self.compute_contact(q, self.rigid_tie, compute="all") self.assemble_global_mat(compute) def set_start(self, pb): - """Accept converged increment: update velocity and acceleration.""" self._pb_ref = pb - delta_u = self.sv["_DeltaDisp"] - - self.sv_start = { - k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv.items() - } - - # Static mode: no v/a state to update; just reset the increment. - if not self.dynamic: - self.sv["_DeltaDisp"] = np.zeros(6) - return - - dt = pb.dtime - v_n = self.sv["Velocity"] - a_n = self.sv["Acceleration"] - - if not np.any(delta_u): - return - - new_a = ( - (1 / (self.beta * dt**2)) * delta_u - - (1 / (self.beta * dt)) * v_n - - (0.5 / self.beta - 1) * a_n - ) - self.sv["Velocity"] = v_n + dt * ((1 - self.gamma) * a_n + self.gamma * new_a) - self.sv["Acceleration"] = new_a - self.sv["_DeltaDisp"] = np.zeros(6) + if self._time_integrator is not None: + self._time_integrator.set_start(self, pb) def to_start(self, pb): - """Revert to start of failed increment.""" - self.sv = { - k: v.copy() if hasattr(v, "copy") else v for k, v in self.sv_start.items() - } + if self._time_integrator is not None: + self._time_integrator.to_start(self, pb) class RigidBody: @@ -397,14 +394,15 @@ class RigidBody: of surface nodes to 6 rigid DOFs) with a :class:`RigidBodyAssembly` (dynamics: 6x6 mass matrix and generalized force vector). - Supports arbitrarily large translations and rotations via quaternion- - based multiplicative update (no gimbal lock, no small-angle - approximation). IPC barrier contact is handled via direct Jacobian + Supports large translations and multiplicative incremental rotations. + IPC barrier contact is handled via direct Jacobian projection ``J^T @ F`` in the 6-DOF space. Solve via ``body.solve(dt, tmax)`` (wraps ``NonLinear``) or include ``body.assembly`` in an ``Assembly.sum`` — the kinematic tie is then - registered automatically when the problem is constructed. + registered automatically when the problem is constructed. Attach a + second-order integrator to manually constructed dynamic problems with + ``pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark())``. Parameters ---------- @@ -424,7 +422,10 @@ class RigidBody: center_of_mass : array_like, shape (3,), optional Center of mass. Default: mean of mesh nodes. use_quaternion : bool, optional - Quaternion-based rotation in RigidTie (default True). + If ``True`` (default), compose incremental rotation vectors into a + quaternion orientation and include Euler's gyroscopic inertia term. + If ``False``, use one total rotation vector; this mode is primarily + retained for kinematic comparisons. name : str, optional Name of the rigid body. @@ -474,8 +475,8 @@ def __init__( center_of_mass = np.asarray(center_of_mass, dtype=float) if self.dynamic: - # Mass and inertia are only used by the Newmark integrator. - # Static mode allows callers to omit them entirely. + # Mass and inertia are consumed by the problem's second-order + # time integrator. Static mode allows callers to omit them. if density is not None: vol = mesh.get_volume() if mass is None: @@ -500,6 +501,11 @@ def __init__( self.mass = float(mass) self.center_of_mass = center_of_mass self.inertia_tensor = np.asarray(inertia_tensor, dtype=float) + if self.inertia_tensor.shape != (3, 3): + raise ValueError("inertia_tensor must have shape (3, 3).") + if not np.allclose(self.inertia_tensor, self.inertia_tensor.T): + raise ValueError("inertia_tensor must be symmetric.") + self.use_quaternion = bool(use_quaternion) # When ``mesh`` came from ``Mesh.extract_elements`` it carries # the parent-mesh active-node list used to slave the right DOFs @@ -512,7 +518,7 @@ def __init__( self.constraint = RigidTie( tie_node_indices, center=center_of_mass, - use_quaternion=use_quaternion, + use_quaternion=self.use_quaternion, name=f"{name}_tie", ) self.assembly = RigidBodyAssembly( @@ -538,7 +544,7 @@ def set_generalized_force(self, f): def set_rayleigh_damping(self, alpha): """Set mass-proportional damping: C = alpha * M.""" - self.assembly.rayleigh_alpha = float(alpha) + self.assembly.dissipation = RayleighDamping(alpha=float(alpha)) def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): """Enable IPC barrier contact with a STATIC obstacle surface. @@ -614,17 +620,13 @@ def add_to_problem(self, pb): if self.constraint not in pb.bc: pb.bc.add(self.constraint) - @property - def Q_total(self): - """Current total rotation as a Rotation object.""" - return self.constraint.Q_total - - def solve(self, dt, tmax, t0=0, print_info=1): + def solve(self, dt, tmax, t0=0, print_info=1, solver=None): """Solve rigid body dynamics using Fedoo's NonLinear solver. - Creates an internal ``NonLinear`` problem and calls ``nlsolve``. - The ``RigidBodyAssembly`` handles Newmark time integration, - IPC contact, and Rayleigh damping internally. + Creates an internal ``NonLinear`` problem, attaches + :class:`fedoo.time.Newmark` for a dynamic body, and calls ``nlsolve``. + IPC contact and Rayleigh damping remain assembly-level providers and + are integrated by the problem's time integrator. Parameters ---------- @@ -636,6 +638,8 @@ def solve(self, dt, tmax, t0=0, print_info=1): Start time. print_info : int Verbosity (0=silent, 1=iterations). + solver : str or callable, optional + Linear solver forwarded to :meth:`Problem.set_solver`. Returns ------- @@ -645,6 +649,11 @@ def solve(self, dt, tmax, t0=0, print_info=1): from fedoo.problem.non_linear import NonLinear pb = NonLinear(self.assembly) + if self.dynamic: + from fedoo.time import Newmark + pb.set_time_integrator(SECOND_ORDER, Newmark()) + if solver is not None: + pb.set_solver(solver) pb.nlsolve(dt=dt, tmax=tmax, t0=t0, print_info=print_info, update_dt=False) return pb diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 14658604..f9691eaa 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -39,6 +39,12 @@ class RigidTie(BCBase): of the corresponding node in the mesh. If center is an array (or list, ...) it contains the initial coordinates of the rotation center. By default, the center is set at the midle of the rigid sets. + use_quaternion : bool, optional + If ``True`` (default), the rotational DOFs describe an incremental + rotation vector relative to the last converged orientation. The + increment is composed multiplicatively and the converged orientation + is stored as a quaternion. If ``False``, the DOFs are interpreted as + one total rotation vector. name : str, optional Name of the created boundary condition. The default is "Rigid Tie". @@ -47,7 +53,10 @@ class RigidTie(BCBase): ----------------------- ``RigidRotX``, ``RigidRotY`` and ``RigidRotZ`` use a rotation-vector convention. The vector direction defines the rotation axis and its norm is - the rotation angle. + the rotation angle. With ``use_quaternion=True``, this rotation vector is + the current increment and is composed with the last converged orientation; + the quaternion is an orientation storage format, not a different DOF + convention. Notes @@ -76,9 +85,12 @@ class RigidTie(BCBase): rigid_tie = fd.constraint.RigidTie(right_face) """ - def __init__(self, list_nodes, center=None, name="Rigid Tie"): + def __init__( + self, list_nodes, center=None, use_quaternion=True, name="Rigid Tie" + ): self.list_nodes = list_nodes self.center = center + self.use_quaternion = bool(use_quaternion) self.bc_type = "RigidTie" BCBase.__init__(self, name) self._keep_at_end = True @@ -93,16 +105,6 @@ def __repr__(self): return "\n".join(list_str) def initialize(self, problem): - if problem.space.is_axisymmetric: - warnings.warn( - "RigidTie under the '2Daxi' ModelingSpace: the constraint " - "is applied as pure-kinematics MPCs and will function, but " - "only rigid motions consistent with axisymmetry (axial " - "translation along the symmetry axis) preserve the " - "axisymmetric assumption. Radial translation and any " - "rotation around the in-plane axes break it.", - stacklevel=2, - ) if self.center is None: # initialize the rotation center at center of rigid nodes bounding box nodes_crd = problem.mesh.nodes[self.list_nodes] @@ -145,32 +147,55 @@ def initialize(self, problem): + self.list_nodes[:, None] ) - def generate(self, problem, t_fact=1, t_fact_old=None): - mesh = problem.mesh - var_cd = self.var_cd - node_cd = self.node_cd - list_nodes = self.list_nodes + if self.use_quaternion: + self._Q_base = Rotation.identity() + self._angles_at_base = np.zeros(3) + def _get_dof_ref(self, problem): + """Return the current six rigid-body DOFs and their global indices.""" dof_cd = [ problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) + + problem._global_dof.indice_start(self.var_cd[i]) + + self.node_cd[i] + for i in range(6) ] - - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(6), dof_cd + return np.asarray(xbc)[dof_cd], dof_cd + if np.isscalar(xbc) and xbc == 0: + return np.asarray(dof_sol)[dof_cd], dof_cd + return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + + def _compute_rotation(self, rotvec): + """Return the orientation and exact derivatives for the active mode.""" + rotvec = np.asarray(rotvec, dtype=float) + if self.use_quaternion and hasattr(self, "_Q_base"): + increment = rotvec - self._angles_at_base + if not np.all(np.isfinite(increment)): + increment = np.zeros(3) + R_base = self._Q_base.as_matrix() else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) - - disp_ref = dof_ref[:3] # reference displacement - rotvec = dof_ref[3:] - - R = Rotation.from_rotvec(rotvec).as_matrix() + increment = rotvec + R_base = np.eye(3) + + R_increment = Rotation.from_rotvec(increment).as_matrix() + derivatives = dR_drotvec(increment) + R = R_increment @ R_base + return ( + R, + derivatives[:, :, 0] @ R_base, + derivatives[:, :, 1] @ R_base, + derivatives[:, :, 2] @ R_base, + ) - # Correct displacement of slave nodes to be consistent with the master nodes + def _compute_slave_disp(self, problem, disp_ref, R): + """Update slave-node displacements for a given rigid motion.""" + list_nodes = self.list_nodes + mesh = problem.mesh new_disp = ( (mesh.nodes[list_nodes] - self.center) @ R.T + self.center @@ -178,20 +203,65 @@ def generate(self, problem, t_fact=1, t_fact_old=None): - mesh.nodes[list_nodes] ) - if not (np.array_equal(problem._dU, 0)): + if not np.array_equal(problem._dU, 0): if np.array_equal(problem._U, 0): problem._dU[self._disp_indices] = new_disp else: problem._dU[self._disp_indices] = ( new_disp - problem._U[self._disp_indices] ) + return new_disp + + def pre_update(self, problem): + """Refresh rigid slave positions before contact assemblies update.""" + dof_ref, _ = self._get_dof_ref(problem) + R, _, _, _ = self._compute_rotation(dof_ref[3:]) + self._compute_slave_disp(problem, dof_ref[:3], R) + + def set_start(self, problem): + """Commit a converged incremental rotation to the quaternion state.""" + if not self.use_quaternion or not hasattr(self, "_Q_base"): + return + dof_ref, _ = self._get_dof_ref(problem) + angles = np.asarray(dof_ref[3:], dtype=float) + if not np.all(np.isfinite(angles)): + return + increment = angles - self._angles_at_base + if not np.allclose(increment, 0.0, atol=1e-15): + self._Q_base = Rotation.from_rotvec(increment) * self._Q_base + self._angles_at_base = angles.copy() + + def to_start(self, problem): + """Leave the last converged quaternion unchanged after a failed step.""" + return + + @property + def Q_total(self): + """Last converged quaternion orientation, or ``None`` in total mode.""" + if self.use_quaternion and hasattr(self, "_Q_base"): + return self._Q_base + return None + + def generate(self, problem, t_fact=1, t_fact_old=None): + mesh = problem.mesh + var_cd = self.var_cd + node_cd = self.node_cd + list_nodes = self.list_nodes + + dof_ref, dof_cd = self._get_dof_ref(problem) + + disp_ref = dof_ref[:3] # reference displacement + rotvec = dof_ref[3:] + + R, dR_drx, dR_dry, dR_drz = self._compute_rotation(rotvec) + + self._compute_slave_disp(problem, disp_ref, R) # approche incrémentale: - dR = dR_drotvec(rotvec) crd = mesh.nodes[list_nodes] - self.center - du_drx = crd @ dR[:, :, 0].T - du_dry = crd @ dR[:, :, 1].T - du_drz = crd @ dR[:, :, 2].T + du_drx = crd @ dR_drx.T + du_dry = crd @ dR_dry.T + du_drz = crd @ dR_drz.T #### MPC #### @@ -304,6 +374,9 @@ def initialize(self, problem): elif np.isscalar(self.center): # initialize the center at a position of a node self.center = problem.mesh.nodes[self.center] + else: + self.center = np.asarray(self.center) + dof_indice_disp = problem.add_global_dof( ["RigidDispX", "RigidDispY"], 1, "RidigDisp" ) @@ -323,58 +396,76 @@ def initialize(self, problem): np.c_[rank * n_nodes, (rank + 1) * n_nodes] + self.list_nodes[:, None] ) - def generate(self, problem, t_fact=1, t_fact_old=None): - mesh = problem.mesh - var_cd = self.var_cd - node_cd = self.node_cd - list_nodes = self.list_nodes - + def _get_dof_ref(self, problem): + """Return [dx, dy, rotZ] and their global DOF indices.""" dof_cd = [ problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) + + problem._global_dof.indice_start(self.var_cd[i]) + + self.node_cd[i] + for i in range(3) ] - - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) - else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) - - disp_ref = dof_ref[:2] # reference displacement - angles = dof_ref[2] # rotation Z angle - - sin = np.sin(angles) - cos = np.cos(angles) - - # Correct displacement of slave nodes to be consistent with the master nodes - R = np.array([[cos, -sin], [sin, cos]]) - + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(3), dof_cd + return np.asarray(xbc)[dof_cd], dof_cd + if np.isscalar(xbc) and xbc == 0: + return np.asarray(dof_sol)[dof_cd], dof_cd + return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + + @staticmethod + def _compute_rotation(angle): + """Return the 2D rotation matrix and its angle derivative.""" + sin = np.sin(angle) + cos = np.cos(angle) + rotation = np.array([[cos, -sin], [sin, cos]]) + derivative = np.array([[-sin, -cos], [cos, -sin]]) + return rotation, derivative + + def _compute_slave_disp(self, problem, disp_ref, rotation): + """Update 2D slave-node displacements for a rigid motion.""" + nodes = problem.mesh.nodes[self.list_nodes] new_disp = ( - (mesh.nodes[list_nodes] - self.center) @ R.T + (nodes - self.center) @ rotation.T + self.center + disp_ref - - mesh.nodes[list_nodes] + - nodes ) - - if not (np.array_equal(problem._dU, 0)): + if not np.array_equal(problem._dU, 0): if np.array_equal(problem._U, 0): problem._dU[self._disp_indices] = new_disp else: problem._dU[self._disp_indices] = ( new_disp - problem._U[self._disp_indices] ) + return new_disp + def pre_update(self, problem): + """Refresh rigid slave positions before contact assemblies update.""" + dof_ref, _ = self._get_dof_ref(problem) + rotation, _ = self._compute_rotation(dof_ref[2]) + self._compute_slave_disp(problem, dof_ref[:2], rotation) + + def generate(self, problem, t_fact=1, t_fact_old=None): + + var_cd = self.var_cd + node_cd = self.node_cd + list_nodes = self.list_nodes + + dof_ref, _ = self._get_dof_ref(problem) + + disp_ref = dof_ref[:2] # reference displacement + rotation, dR_drz = self._compute_rotation(dof_ref[2]) + # Correct displacement of slave nodes to be consistent with the masters + self._compute_slave_disp(problem, disp_ref, rotation) # approche incrémentale: - dR_drz = np.array([[-sin, -cos], [cos, -sin]]) - crd = mesh.nodes[list_nodes, :2] - self.center + # MPC linearization + crd = problem.mesh.nodes[list_nodes, :2] - self.center - du_drz = ( - crd @ dR_drz.T - ) # shape = (nnodes, nvar) with nvar = 3 in 3d (ux, uy, uz) + du_drz = crd @ dR_drz.T # shape = (nnodes, 2) #### MPC #### diff --git a/fedoo/problem/linear.py b/fedoo/problem/linear.py index 129769a5..7d8cf509 100644 --- a/fedoo/problem/linear.py +++ b/fedoo/problem/linear.py @@ -11,7 +11,7 @@ def __init__(self, assembly: Assembly, name: str = "MainProblem"): super().__init__(mesh=assembly.mesh, name=name) self.nlgeom = False - self._register_rigid_ties(assembly) + self._register_assembly_constraints(assembly) assembly.initialize(self) self.time = self.dtime = 0 # self.set_A(assembly.get_global_matrix()) @@ -135,24 +135,16 @@ def GetNodalElasticEnergy(self): def assembly(self): return self.__assembly - def _register_rigid_ties(self, assembly): - """Add any RigidBodyAssembly's tie to ``self.bc`` before assembly init. - - Mirrors ``NonLinear._register_rigid_ties``. Without this the rigid - tie's ``var_cd`` is never populated, and ``RigidBodyAssembly.initialize`` - crashes reading it. - """ - from collections import deque - from fedoo.constraint.rigid_body import RigidBodyAssembly - - queue = deque([assembly]) - while queue: - a = queue.popleft() - if isinstance(a, RigidBodyAssembly) and a.rigid_tie not in self.bc: - self.bc.add(a.rigid_tie) - children = getattr(a, "_list_assembly", None) - if children: - queue.extend(children) + def _register_assembly_constraints(self, assembly): + """Register constraints exposed by an assembly tree.""" + children = getattr(assembly, "_list_assembly", None) + if children: + for child in children: + self._register_assembly_constraints(child) + return + for constraint in getattr(assembly, "constraints", ()): + if constraint not in self.bc: + self.bc.add(constraint) class Linear(_LinearBase, Problem): diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index fe3e554f..9e643b1a 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -62,6 +62,7 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.__assembly = assembly super().__init__(A, B, D, assembly.mesh, name, assembly.space) + self._register_assembly_constraints(assembly) self.nlgeom = nlgeom self.time_integrators = {} self._time_integrators_compiled = False @@ -78,6 +79,22 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.exec_callback_at_each_iter = False self.err_num = 1e-8 # numerical error + def _register_assembly_constraints(self, assembly): + """Register constraints exposed by an assembly tree.""" + if isinstance(assembly, AssemblySum): + for child in assembly.list_assembly: + self._register_assembly_constraints(child) + return + for constraint in getattr(assembly, "constraints", ()): + if constraint not in self.bc: + self.bc.add(constraint) + + def _run_constraint_hook(self, name): + for bc in self.bc: + hook = getattr(bc, name, None) + if hook is not None: + hook(self) + @property def n_iter(self): """Return the number of iterations made to solve the problem.""" @@ -182,6 +199,8 @@ def set_time_integrator(self, evolution, integrator): return integrator def _has_time_integrated_weakform(self, assembly): + if getattr(assembly, "_fedoo_time_integrated", False): + return True if isinstance(assembly, AssemblySum): return any( self._has_time_integrated_weakform(child) @@ -214,12 +233,23 @@ def _iter_leaf_weakforms(self, assembly): for wf in getattr(weakform, "list_weakform", [weakform]): yield wf + def _iter_assembly_time_providers(self, assembly): + if isinstance(assembly, AssemblySum): + for child in assembly.list_assembly: + yield from self._iter_assembly_time_providers(child) + return + if getattr(assembly, "weakform", None) is None and ( + getattr(assembly, "storage", None) is not None + or getattr(assembly, "dissipation", None) is not None + ): + yield assembly + def _warn_ignored_storage(self): """Warn when declared storage/dissipation terms are silently ignored. - Weakforms may declare transient metadata (e.g. HeatEquation always - declares its heat capacity storage); without a matching problem-level - integrator the analysis is steady/static and these terms are dropped. + Weakforms and assembly-level providers may declare transient metadata; + without a matching problem-level integrator the analysis is steady/static + and these terms are dropped. This is legitimate for a deliberately steady analysis, but silent for a user migrating from the former transient-by-default weakforms, so a warning is emitted once at compile time. @@ -248,6 +278,21 @@ def _warn_ignored_storage(self): stacklevel=3, ) + for assembly in self._iter_assembly_time_providers(self.__assembly): + evolution = getattr(assembly, "time_evolution", None) + if evolution is None or evolution in self.time_integrators: + continue + warnings.warn( + f"Assembly provider '{assembly.name}' declares storage or " + f"dissipation terms for the '{evolution.kind}' time evolution, " + "but no matching time integrator is attached to the problem: " + "these terms are ignored and the analysis is treated as " + "steady/static. Attach a matching integrator with " + "pb.set_time_integrator.", + UserWarning, + stacklevel=3, + ) + def initialize(self): self._compile_time_integrators() self.__assembly.initialize(self) @@ -261,6 +306,7 @@ def set_start(self, save_results=False, callback=None): self._U += self._dU self._dU = 0 self.__assembly.set_start(self) + self._run_constraint_hook("set_start") # Save results if save_results: @@ -272,6 +318,7 @@ def set_start(self, save_results=False, callback=None): callback(self) else: self.__assembly.set_start(self) + self._run_constraint_hook("set_start") def to_start(self): self._dU = 0 @@ -279,6 +326,7 @@ def to_start(self): self._t_fact_inc = None self._err0 = self.nr_parameters["err0"] # initial error for NR error estimation self.__assembly.to_start(self) + self._run_constraint_hook("to_start") def update(self, compute="all", updateWeakForm=True): """ @@ -289,6 +337,12 @@ def update(self, compute="all", updateWeakForm=True): - Change in constitutive law (internal variable) Don't Update the problem with the new assembled global matrix and global vector -> use UpdateA and UpdateD method for this purpose """ + if self.bc._update_during_inc: + for bc in self.bc: + pre_update = getattr(bc, "pre_update", None) + if pre_update is not None: + pre_update(self) + if updateWeakForm == True: self.__assembly.update(self, compute) else: diff --git a/fedoo/time/base.py b/fedoo/time/base.py index 5cd1c56b..34602f2e 100644 --- a/fedoo/time/base.py +++ b/fedoo/time/base.py @@ -67,8 +67,13 @@ def _integrate_leaf(self, weakform): raise NotImplementedError def _compile_assembly_level_provider(self, assembly): + if getattr(assembly, "time_evolution", None) != self.evolution: + return assembly + if getattr(assembly, "_fedoo_time_integrated", False): + return assembly if any(hasattr(assembly, attr) for attr in ("storage", "dissipation")): raise NotImplementedError( f"Assembly-level time providers are part of the architecture but " f"do not have a concrete {type(self).__name__} adapter yet." ) + return assembly diff --git a/fedoo/time/generalized_alpha.py b/fedoo/time/generalized_alpha.py index e97a5c49..6d84e1ef 100644 --- a/fedoo/time/generalized_alpha.py +++ b/fedoo/time/generalized_alpha.py @@ -1,6 +1,7 @@ import copy import numpy as np +from scipy import sparse from fedoo.core.time_evolution import SECOND_ORDER from fedoo.core.weakform import WeakFormBase, WeakFormSum @@ -25,6 +26,199 @@ def _newmark_state(term, assembly, dt): ) +class GeneralizedAlphaAssemblyAdapter: + """Apply generalized-alpha to an assembly-level matrix provider. + + Assembly providers keep ownership of their static tangent and force. The + adapter owns the temporal state and augments those contributions on the + provider's declared time DOF indices. + """ + + def __init__(self, integrator): + self.alpha_m = integrator.alpha_m + self.alpha_f = integrator.alpha_f + self.beta = integrator.beta + self.gamma = integrator.gamma + self.force_start = None + self.current_force = None + + @staticmethod + def _copy_state(state): + return { + key: value.copy() if hasattr(value, "copy") else value + for key, value in state.items() + } + + def initialize(self, assembly, pb): + n_dof = len(assembly.time_dof_indices) + assembly.sv["Velocity"] = np.zeros(n_dof) + assembly.sv["Acceleration"] = np.zeros(n_dof) + assembly.sv["_DeltaDisp"] = np.zeros(n_dof) + + initial_force = getattr(assembly, "get_time_initial_force", None) + if initial_force is not None: + force = np.asarray(initial_force(pb), dtype=float) + self.force_start = force.copy() + mass = np.asarray(assembly.get_storage_matrix(pb), dtype=float) + try: + assembly.sv["Acceleration"] = np.linalg.solve(mass, force) + except np.linalg.LinAlgError: + pass + else: + self.force_start = np.zeros(n_dof) + + assembly.sv_start = self._copy_state(assembly.sv) + + def update(self, assembly, pb): + if np.isscalar(pb._dU) and pb._dU == 0: + delta_disp = np.zeros(len(assembly.time_dof_indices)) + else: + delta_disp = np.asarray(pb._dU)[assembly.time_dof_indices] + assembly.sv["_DeltaDisp"] = delta_disp.copy() + + def _local_static_matrix(self, assembly): + provider = getattr(assembly, "get_time_stiffness_matrix", None) + if provider is not None: + return np.asarray(provider(), dtype=float) + + idx = assembly.time_dof_indices + matrix = assembly.global_matrix + if sparse.issparse(matrix): + return matrix[idx][:, idx].toarray() + return np.asarray(matrix)[np.ix_(idx, idx)] + + def _damping_matrix(self, assembly, mass, stiffness): + damping = getattr(assembly, "dissipation", None) + if damping is None: + return np.zeros_like(mass) + if not isinstance(damping, RayleighDamping): + raise NotImplementedError( + "Assembly-level generalized-alpha currently supports only " + "RayleighDamping." + ) + return damping.alpha * mass + damping.beta * stiffness + + @staticmethod + def _inertia_force(assembly, pb, mass, acceleration, velocity): + provider = getattr(assembly, "get_time_inertia_force", None) + if provider is not None: + return np.asarray(provider(pb, acceleration, velocity), dtype=float) + return mass @ acceleration + + @staticmethod + def _inertia_tangent( + assembly, + pb, + mass, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ): + provider = getattr(assembly, "get_time_inertia_tangent", None) + if provider is not None: + return np.asarray( + provider( + pb, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ), + dtype=float, + ) + return acceleration_factor * mass + + def integrate(self, assembly, pb, compute="all"): + dt = pb.dtime + if dt == 0: + return + + idx = np.asarray(assembly.time_dof_indices, dtype=int) + mass = np.asarray(assembly.get_storage_matrix(pb), dtype=float) + stiffness = self._local_static_matrix(assembly) + damping = self._damping_matrix(assembly, mass, stiffness) + acc_np1, vel_np1 = newmark_acceleration_velocity( + self.beta, + self.gamma, + dt, + assembly.sv["_DeltaDisp"], + assembly.sv["Velocity"], + assembly.sv["Acceleration"], + ) + acc_alpha = (1.0 - self.alpha_m) * acc_np1 + self.alpha_m * assembly.sv[ + "Acceleration" + ] + vel_alpha = (1.0 - self.alpha_f) * vel_np1 + self.alpha_f * assembly.sv[ + "Velocity" + ] + + a0 = 1.0 / (self.beta * dt**2) + c0 = self.gamma / (self.beta * dt) + + if compute != "vector": + if self.alpha_f != 0.0: + assembly.global_matrix = (1.0 - self.alpha_f) * assembly.global_matrix + tangent = self._inertia_tangent( + assembly, + pb, + mass, + acc_alpha, + vel_alpha, + (1.0 - self.alpha_m) * a0, + (1.0 - self.alpha_f) * c0, + ) + tangent += (1.0 - self.alpha_f) * c0 * damping + rows = np.repeat(idx, len(idx)) + cols = np.tile(idx, len(idx)) + dynamic_matrix = sparse.csr_matrix( + (tangent.ravel(), (rows, cols)), + shape=assembly.global_matrix.shape, + ) + assembly.global_matrix = assembly.global_matrix + dynamic_matrix + + if compute != "matrix": + static_force = np.asarray(assembly.global_vector)[idx].copy() + self.current_force = static_force + force_start = ( + self.force_start if self.force_start is not None else static_force + ) + force_alpha = ( + (1.0 - self.alpha_f) * static_force + self.alpha_f * force_start + ) + inertia_force = self._inertia_force( + assembly, pb, mass, acc_alpha, vel_alpha + ) + assembly.global_vector[idx] = ( + force_alpha - inertia_force - damping @ vel_alpha + ) + + def set_start(self, assembly, pb): + dof_solution = pb.get_dof_solution() + if not (np.isscalar(dof_solution) and dof_solution == 0): + dt = getattr(pb, "_dtime_prev", None) or pb.dtime + if dt: + acc, vel = newmark_acceleration_velocity( + self.beta, + self.gamma, + dt, + assembly.sv["_DeltaDisp"], + assembly.sv["Velocity"], + assembly.sv["Acceleration"], + ) + assembly.sv["Acceleration"] = acc + assembly.sv["Velocity"] = vel + assembly.sv["_DeltaDisp"] = np.zeros_like( + assembly.sv["_DeltaDisp"] + ) + if self.current_force is not None: + self.force_start = self.current_force.copy() + assembly.sv_start = self._copy_state(assembly.sv) + + def to_start(self, assembly, pb): + assembly.sv = self._copy_state(assembly.sv_start) + + class GeneralizedAlpha(TimeIntegratorBase): """Generalized-alpha time integrator for second-order evolutions. @@ -64,6 +258,20 @@ def __init__(self, alpha_m=0.0, alpha_f=0.0, beta=None, gamma=None): if self.gamma <= 0.0: raise ValueError("gamma must be strictly positive.") + def _compile_assembly_level_provider(self, assembly): + if getattr(assembly, "time_evolution", None) != self.evolution: + return assembly + if not hasattr(assembly, "get_storage_matrix") or not hasattr( + assembly, "time_dof_indices" + ): + raise NotImplementedError( + "Second-order assembly providers must expose " + "get_storage_matrix(pb) and time_dof_indices." + ) + assembly._time_integrator = GeneralizedAlphaAssemblyAdapter(self) + assembly._fedoo_time_integrated = True + return assembly + def _integrate_leaf(self, weakform): storage = self._resolve_storage(weakform) if storage is None: diff --git a/tests/test_rigid_body_dynamics.py b/tests/test_rigid_body_dynamics.py new file mode 100644 index 00000000..0a449685 --- /dev/null +++ b/tests/test_rigid_body_dynamics.py @@ -0,0 +1,147 @@ +"""Rotational inertia tests for dynamic rigid bodies.""" + +import numpy as np + +import fedoo as fd +from fedoo.constraint.rigid_body import RigidBodyAssembly +from simcoon import Rotation + + +def _make_assembly(inertia, rotvec): + space = fd.ModelingSpace("3D") + tie = fd.constraint.RigidTie( + np.array([0]), center=np.zeros(3), use_quaternion=True + ) + tie._Q_base = Rotation.identity() + tie._angles_at_base = np.zeros(3) + state = {"rotvec": np.asarray(rotvec, dtype=float).copy()} + + def get_dof_ref(_problem): + return np.concatenate([np.zeros(3), state["rotvec"]]), None + + tie._get_dof_ref = get_dof_ref + assembly = RigidBodyAssembly( + mass=2.0, + inertia_tensor=np.asarray(inertia, dtype=float), + rigid_tie=tie, + space=space, + ) + assembly._dof_indices = np.arange(6) + return assembly, state + + +def test_non_spherical_inertia_adds_gyroscopic_force(): + inertia = np.diag([2.0, 3.0, 5.0]) + assembly, _ = _make_assembly(inertia, np.zeros(3)) + acceleration = np.array([0.1, -0.2, 0.3, 0.4, -0.5, 0.6]) + velocity = np.array([1.0, 2.0, -1.0, 0.7, -0.4, 1.2]) + + force = assembly.get_time_inertia_force(object(), acceleration, velocity) + + angular_velocity = velocity[3:] + expected_moment = inertia @ acceleration[3:] + np.cross( + angular_velocity, inertia @ angular_velocity + ) + np.testing.assert_allclose(force[:3], 2.0 * acceleration[:3]) + np.testing.assert_allclose(force[3:], expected_moment) + assert not np.allclose( + np.cross(angular_velocity, inertia @ angular_velocity), 0.0 + ) + + +def test_spherical_inertia_has_no_gyroscopic_force(): + inertia = 3.0 * np.eye(3) + assembly, _ = _make_assembly(inertia, np.zeros(3)) + acceleration = np.array([0.0, 0.0, 0.0, 0.4, -0.5, 0.6]) + velocity = np.array([0.0, 0.0, 0.0, 0.7, -0.4, 1.2]) + + force = assembly.get_time_inertia_force(object(), acceleration, velocity) + + np.testing.assert_allclose(force[3:], inertia @ acceleration[3:]) + + +def test_gyroscopic_inertia_tangent_matches_finite_difference(): + inertia = np.diag([2.0, 3.0, 5.0]) + rotvec = np.array([0.2, -0.15, 0.1]) + assembly, state = _make_assembly(inertia, rotvec) + acceleration = np.array([0.1, -0.2, 0.3, 0.4, -0.5, 0.6]) + velocity = np.array([1.0, 2.0, -1.0, 0.7, -0.4, 1.2]) + acceleration_factor = 8.0 + velocity_factor = 2.5 + + tangent = assembly.get_time_inertia_tangent( + object(), + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ) + + h = 1e-7 + numerical = np.zeros((3, 3)) + reference = state["rotvec"].copy() + for axis in range(3): + perturbation = np.zeros(3) + perturbation[axis] = h + + state["rotvec"] = reference + perturbation + acceleration_plus = acceleration.copy() + velocity_plus = velocity.copy() + acceleration_plus[3 + axis] += acceleration_factor * h + velocity_plus[3 + axis] += velocity_factor * h + force_plus = assembly.get_time_inertia_force( + object(), acceleration_plus, velocity_plus + )[3:] + + state["rotvec"] = reference - perturbation + acceleration_minus = acceleration.copy() + velocity_minus = velocity.copy() + acceleration_minus[3 + axis] -= acceleration_factor * h + velocity_minus[3 + axis] -= velocity_factor * h + force_minus = assembly.get_time_inertia_force( + object(), acceleration_minus, velocity_minus + )[3:] + numerical[:, axis] = (force_plus - force_minus) / (2.0 * h) + + state["rotvec"] = reference + np.testing.assert_allclose(tangent[3:, 3:], numerical, rtol=1e-6, atol=1e-6) + + +def test_principal_axis_torque_matches_constant_angular_acceleration(): + 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.5, -0.5, 0.0], + [0.5, -0.5, 0.0], + [0.5, 0.5, 0.0], + [-0.5, 0.5, 0.0], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + inertia = np.diag([2.0, 3.0, 5.0]) + body = fd.constraint.RigidBody( + mesh, + mass=1.0, + inertia_tensor=inertia, + center_of_mass=np.zeros(3), + ) + torque = 4.0 + body.set_torque([torque, 0.0, 0.0]) + + end_time = 0.02 + body.solve( + dt=1e-3, + tmax=end_time, + print_info=0, + solver="direct_scipy", + ) + + expected_angle = 0.5 * torque / inertia[0, 0] * end_time**2 + rotation_vector = body.constraint.Q_total.as_rotvec() + np.testing.assert_allclose( + rotation_vector, [expected_angle, 0.0, 0.0], atol=1e-10 + ) diff --git a/tests/test_rigid_body_freefall.py b/tests/test_rigid_body_freefall.py index 214a1c86..d1734962 100644 --- a/tests/test_rigid_body_freefall.py +++ b/tests/test_rigid_body_freefall.py @@ -34,7 +34,7 @@ def _run_freefall(dt, t_end, mass=1.0, z0=1.0, radius=0.1): ) body.set_force([0.0, 0.0, -mass * G]) - pb = body.solve(dt=dt, tmax=t_end, print_info=0) + pb = body.solve(dt=dt, tmax=t_end, print_info=0, solver="direct_scipy") dof = pb.get_dof_solution() idx = body.assembly._dof_indices return z0 + dof[idx[2]] @@ -74,7 +74,7 @@ def test_freefall_no_horizontal_drift(): ) body.set_force([0.0, 0.0, -G]) - pb = body.solve(dt=1e-3, tmax=t_end, print_info=0) + pb = body.solve(dt=1e-3, tmax=t_end, print_info=0, solver="direct_scipy") dof = pb.get_dof_solution() idx = body.assembly._dof_indices dx, dy = dof[idx[0]], dof[idx[1]] diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py index ace94616..ce33a6f3 100644 --- a/tests/test_rigid_body_kinematics.py +++ b/tests/test_rigid_body_kinematics.py @@ -15,12 +15,17 @@ @pytest.fixture def space(): - return fd.ModelingSpace("3D") + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + return space def _make_assembly(rest_positions, obstacle_positions, center): rt = fd.constraint.RigidTie( - np.arange(len(rest_positions)), center=center, use_quaternion=True + np.arange(len(rest_positions)), center=center ) rt.center = np.asarray(center, dtype=float) asm = RigidBodyAssembly(mass=1.0, inertia_tensor=np.eye(3), rigid_tie=rt) @@ -112,5 +117,84 @@ def vertex_positions(q): assert np.allclose(J[:, k], du_dq, atol=1e-6), f"column {k} mismatch" +def _make_rigid_body(dynamic=True): + nodes = np.array( + [ + [-0.5, -0.5, 0.0], + [0.5, -0.5, 0.0], + [0.5, 0.5, 0.0], + [-0.5, 0.5, 0.0], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + kwargs = {} + if dynamic: + kwargs = {"mass": 1.0, "inertia_tensor": np.eye(3)} + return fd.constraint.RigidBody(mesh, dynamic=dynamic, **kwargs) + + +def test_static_body_is_not_compiled_by_second_order_integrator(space): + body = _make_rigid_body(dynamic=False) + pb = fd.problem.NonLinear(body.assembly) + pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) + + pb.initialize() + + assert body.assembly.time_evolution is None + assert body.assembly.storage is None + assert body.assembly._time_integrator is None + assert not body.assembly._fedoo_time_integrated + + +def test_rigid_body_integrator_is_recompiled_for_each_problem(space): + body = _make_rigid_body() + + first = fd.problem.NonLinear(body.assembly) + first.set_time_integrator( + fd.time.SECOND_ORDER, fd.time.Newmark(beta=0.2, gamma=0.55) + ) + first.initialize() + first_adapter = body.assembly._time_integrator + assert first_adapter.beta == pytest.approx(0.2) + + second = fd.problem.NonLinear(body.assembly) + second.set_time_integrator( + fd.time.SECOND_ORDER, fd.time.Newmark(beta=0.3, gamma=0.6) + ) + second.initialize() + assert body.assembly._time_integrator is not first_adapter + assert body.assembly._time_integrator.beta == pytest.approx(0.3) + + static = fd.problem.NonLinear(body.assembly) + with pytest.warns(UserWarning, match="Assembly provider"): + static.initialize() + assert body.assembly._time_integrator is None + assert not body.assembly._fedoo_time_integrated + + +def test_problem_set_start_commits_quaternion_increment(space): + body = _make_rigid_body(dynamic=False) + pb = fd.problem.NonLinear(body.assembly) + pb.initialize() + rotation_indices = body.assembly.dof_indices[3:] + + first = np.array([0.2, 0.0, 0.0]) + pb._dU = np.zeros(pb.n_dof) + pb._dU[rotation_indices] = first + pb.set_start() + + second = np.array([0.0, -0.3, 0.1]) + pb._dU = np.zeros(pb.n_dof) + pb._dU[rotation_indices] = second + pb.set_start() + + expected = Rotation.from_rotvec(second) * Rotation.from_rotvec(first) + quaternion = body.constraint.Q_total.as_quat() + expected_quaternion = expected.as_quat() + assert np.allclose(quaternion, expected_quaternion) or np.allclose( + quaternion, -expected_quaternion + ) + + if __name__ == "__main__": pytest.main([__file__]) diff --git a/tests/test_rigid_tie_2d.py b/tests/test_rigid_tie_2d.py new file mode 100644 index 00000000..3ad9a096 --- /dev/null +++ b/tests/test_rigid_tie_2d.py @@ -0,0 +1,49 @@ +import numpy as np +from scipy import sparse + +import fedoo as fd + + +def test_rigid_tie_2d_helpers_and_array_like_center(): + space = fd.ModelingSpace("2D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_vector("Disp", ("DispX", "DispY")) + + nodes = np.array( + [ + [-1.0, -0.5], + [1.0, -0.5], + [1.0, 0.5], + [-1.0, 0.5], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + problem = fd.Problem( + A=sparse.eye(mesh.n_nodes * space.nvar), mesh=mesh, space=space + ) + + center = [0.2, -0.1] + tied_nodes = np.arange(mesh.n_nodes) + control = fd.constraint.RigidTie2D(tied_nodes, center=center) + problem.bc.add(control) + problem.bc.add("Dirichlet", "RigidDispX", 0.3) + problem.bc.add("Dirichlet", "RigidDispY", -0.2) + problem.bc.add("Dirichlet", "RigidRotZ", 0.4) + problem._U = np.zeros(problem.n_dof) + problem._dU = np.zeros(problem.n_dof) + + problem.apply_boundary_conditions() + + assert isinstance(control.center, np.ndarray) + rotation, _ = control._compute_rotation(0.4) + expected = ( + (nodes - control.center) @ rotation.T + + control.center + + np.array([0.3, -0.2]) + - nodes + ) + np.testing.assert_allclose(problem._dU[control._disp_indices], expected) + + problem._dU[:] = 0.0 + control.pre_update(problem) diff --git a/tests/test_rigid_tie_derivatives.py b/tests/test_rigid_tie_derivatives.py index fa26e957..74f64404 100644 --- a/tests/test_rigid_tie_derivatives.py +++ b/tests/test_rigid_tie_derivatives.py @@ -4,17 +4,19 @@ import pytest from fedoo.constraint.rigid_tie import RigidTie +from simcoon import Rotation def _R(omega): - """Rotation matrix via the tie's own forward pass (non-quaternion path).""" + """Rotation matrix via the tie's total-rotation-vector mode.""" rt = RigidTie([0, 1], use_quaternion=False) R, _, _, _ = rt._compute_rotation(np.asarray(omega, dtype=float)) return R def _dR_analytic(omega): - dR = RigidTie._dR_drotvec(np.asarray(omega, dtype=float)) + rt = RigidTie([0, 1], use_quaternion=False) + _, *dR = rt._compute_rotation(np.asarray(omega, dtype=float)) return np.stack(dR, axis=0) @@ -48,7 +50,8 @@ def test_dR_drotvec_matches_finite_difference(omega): def test_dR_drotvec_zero_limit_is_skew_basis(): - dR = RigidTie._dR_drotvec(np.zeros(3)) + rt = RigidTie([0, 1], use_quaternion=False) + _, *dR = rt._compute_rotation(np.zeros(3)) expected = ( np.array([[0, 0, 0], [0, 0, -1], [0, 1, 0]]), np.array([[0, 0, 1], [0, 0, 0], [-1, 0, 0]]), @@ -59,13 +62,13 @@ def test_dR_drotvec_zero_limit_is_skew_basis(): def test_compute_rotation_identity_at_zero(): - rt = RigidTie([0, 1], use_quaternion=False) + rt = RigidTie([0, 1]) R, _, _, _ = rt._compute_rotation(np.zeros(3)) assert np.allclose(R, np.eye(3)) def test_compute_rotation_is_orthogonal(): - rt = RigidTie([0, 1], use_quaternion=False) + rt = RigidTie([0, 1]) for omega in [ np.array([0.3, -0.4, 0.5]), np.array([1.2, 0.0, 0.0]), @@ -76,5 +79,26 @@ def test_compute_rotation_is_orthogonal(): assert np.isclose(np.linalg.det(R), 1.0, atol=1e-12) +def test_incremental_derivatives_match_finite_difference_after_base_rotation(): + tie = RigidTie([0, 1], use_quaternion=True) + tie._Q_base = Rotation.from_rotvec([0.3, -0.2, 0.1]) + tie._angles_at_base = np.array([0.4, 0.1, -0.2]) + angles = tie._angles_at_base + np.array([0.05, -0.04, 0.03]) + _, *derivatives = tie._compute_rotation(angles) + + h = 1e-7 + numerical = [] + for axis in range(3): + perturbation = np.zeros(3) + perturbation[axis] = h + rotation_plus = tie._compute_rotation(angles + perturbation)[0] + rotation_minus = tie._compute_rotation(angles - perturbation)[0] + numerical.append((rotation_plus - rotation_minus) / (2.0 * h)) + + np.testing.assert_allclose( + np.stack(derivatives), np.stack(numerical), rtol=1e-7, atol=1e-7 + ) + + if __name__ == "__main__": pytest.main([__file__]) diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py index 2d84b023..605eaf93 100644 --- a/tests/test_rigid_tie_rollback.py +++ b/tests/test_rigid_tie_rollback.py @@ -1,128 +1,67 @@ -"""Quaternion base lifecycle: set_start advances, to_start_bc never reverts. - -These tests model the *real* solver sequence (see ``nlsolve``): - -* ``set_start`` is called only for a **converged** increment, at the top of - the following increment — this is the only place ``_Q_base`` advances. -* ``to_start_bc`` is called only for a **failed** increment, which never ran - ``set_start`` — so ``_Q_base`` is already at the last-converged state and - rollback must be a no-op. - -The earlier version of this constraint reverted ``_Q_base`` to a backup -captured *before* the last converged advance, which discarded the last -converged rotation on any ``dt`` reduction. The tests below pin the correct -behaviour. -""" +"""Lifecycle tests for multiplicative quaternion rotation updates.""" import numpy as np -import pytest from fedoo.constraint.rigid_tie import RigidTie +from simcoon import Rotation -from simcoon import Rotation # required dependency (fedoo imports it at load) - - -def _make_tie_with_fake_problem(angles_seq): - """Build a RigidTie wired to a fake problem that returns ``angles_seq[i]`` - on the i-th call to ``_get_dof_ref``.""" - rt = RigidTie([0, 1], use_quaternion=True) - rt._Q_base = Rotation.identity() - rt._angles_at_base = np.zeros(3) - - calls = {"i": 0} - - def fake_get_dof_ref(_problem): - i = calls["i"] - calls["i"] += 1 - angles = np.asarray(angles_seq[i], dtype=float) - return np.concatenate([np.zeros(3), angles]), None - - rt._get_dof_ref = fake_get_dof_ref - return rt - - -def _quat_equal(a, b, atol=1e-14): - qa = a.as_quat() - qb = b.as_quat() - return np.allclose(qa, qb, atol=atol) or np.allclose(qa, -qb, atol=atol) - - -def test_to_start_bc_is_a_noop_on_the_base(): - """Rollback must leave the quaternion base untouched.""" - rt = _make_tie_with_fake_problem([[0.2, 0.1, -0.3]]) - rt.set_start(problem=None) # accept a converged increment - # Copy by value (not a reference) so the assertion catches a regression - # that reassigns _Q_base, not just an in-place mutation. - q = Rotation.from_quat(rt._Q_base.as_quat()) - assert not _quat_equal(q, Rotation.identity()) - - rt.to_start_bc(problem=None) - rt.to_start_bc(problem=None) # idempotent - assert _quat_equal(rt._Q_base, q) +def _make_tie(angles): + tie = RigidTie(np.array([0, 1]), use_quaternion=True) + tie._Q_base = Rotation.identity() + tie._angles_at_base = np.zeros(3) + calls = iter(np.asarray(value, dtype=float) for value in angles) -def test_failed_increment_does_not_discard_converged_rotation(): - """The bug this fixes: converge A (set_start), then B fails (to_start_bc). + def get_dof_ref(_problem): + return np.concatenate([np.zeros(3), next(calls)]), None - In the solver, A's ``set_start`` runs at the top of increment B. When B - fails, the rollback must keep the base at A's rotation — *not* revert it - further (which previously sent it all the way back to identity). - """ - a_converged = [0.3, -0.4, 0.5] - rt = _make_tie_with_fake_problem([a_converged]) # one set_start: accept A - rt.set_start(problem=None) - q_after_A = rt._Q_base - assert not _quat_equal(q_after_A, Rotation.identity()) + tie._get_dof_ref = get_dof_ref + return tie - # Increment B now runs and FAILS; its set_start is never called. The - # solver rolls back via to_start_bc — the base must remain at A. - rt.to_start_bc(problem=None) - assert _quat_equal(rt._Q_base, q_after_A) +def _same_rotation(left, right, atol=1e-14): + left_quaternion = left.as_quat() + right_quaternion = right.as_quat() + return np.allclose(left_quaternion, right_quaternion, atol=atol) or np.allclose( + left_quaternion, -right_quaternion, atol=atol + ) -def test_two_successful_increments_compose_multiplicatively(): - a1 = np.array([0.1, 0.0, 0.0]) - a2 = np.array([0.1, 0.2, 0.0]) - rt = _make_tie_with_fake_problem([a1, a2]) - rt.set_start(problem=None) - rt.set_start(problem=None) +def test_successive_increments_compose_multiplicatively(): + first = np.array([0.1, 0.0, 0.0]) + second = np.array([0.1, 0.2, 0.0]) + tie = _make_tie([first, second]) - # Q_total should be R(a2 - a1) * R(a1) = composed rotations - expected = Rotation.from_rotvec(a2 - a1) * Rotation.from_rotvec(a1) - assert _quat_equal(rt._Q_base, expected) - assert np.array_equal(rt._angles_at_base, a2) + tie.set_start(None) + tie.set_start(None) + expected = Rotation.from_rotvec(second - first) * Rotation.from_rotvec(first) + assert _same_rotation(tie.Q_total, expected) + np.testing.assert_array_equal(tie._angles_at_base, second) -def test_converge_fail_retry_keeps_first_increment(): - """Full sequence: A converges, B fails and rolls back, B retried converges. - After the retry, the base must equal R(a_B - a_A) * R(a_A) — i.e. the - converged A rotation is preserved through the failed attempt. - """ - a_A = np.array([0.3, 0.0, 0.0]) - a_B = np.array([0.3, 0.4, 0.0]) - # set_start consumes one angle per call: accept A, then accept B (retry). - rt = _make_tie_with_fake_problem([a_A, a_B]) +def test_failed_increment_keeps_last_converged_orientation(): + converged = np.array([0.3, -0.4, 0.5]) + tie = _make_tie([converged]) + tie.set_start(None) + orientation = Rotation.from_quat(tie.Q_total.as_quat()) - rt.set_start(problem=None) # accept converged A - q_after_A = rt._Q_base + tie.to_start(None) + tie.to_start(None) - rt.to_start_bc(problem=None) # increment B fails -> rollback (no-op) - assert _quat_equal(rt._Q_base, q_after_A) + assert _same_rotation(tie.Q_total, orientation) - rt.set_start(problem=None) # accept converged B (retried) - expected = Rotation.from_rotvec(a_B - a_A) * Rotation.from_rotvec(a_A) - assert _quat_equal(rt._Q_base, expected) - assert np.array_equal(rt._angles_at_base, a_B) +def test_total_rotvec_mode_does_not_create_quaternion_state(): + tie = RigidTie(np.array([0, 1]), use_quaternion=False) + rotation, _, _, _ = tie._compute_rotation(np.array([0.2, -0.1, 0.4])) -def test_nan_angles_do_not_mutate_base(): - rt = _make_tie_with_fake_problem([[np.nan, 0.0, 0.0]]) - q_before = Rotation.from_quat(rt._Q_base.as_quat()) # copy by value - rt.set_start(problem=None) - assert _quat_equal(rt._Q_base, q_before) + expected = Rotation.from_rotvec([0.2, -0.1, 0.4]).as_matrix() + np.testing.assert_allclose(rotation, expected) + assert tie.Q_total is None -if __name__ == "__main__": - pytest.main([__file__]) +def test_non_finite_increment_does_not_mutate_orientation(): + tie = _make_tie([[np.nan, 0.0, 0.0]]) + tie.set_start(None) + assert _same_rotation(tie.Q_total, Rotation.identity()) From 58a9c396bb148b0b63604fa52363d344b46a8f20 Mon Sep 17 00:00:00 2001 From: pruliere Date: Thu, 16 Jul 2026 10:21:52 +0200 Subject: [PATCH 15/21] ruff format --- fedoo/constraint/rigid_body.py | 11 ++++------- fedoo/constraint/rigid_tie.py | 12 ++---------- fedoo/time/generalized_alpha.py | 8 +++----- tests/test_rigid_body_dynamics.py | 12 +++--------- tests/test_rigid_body_kinematics.py | 4 +--- 5 files changed, 13 insertions(+), 34 deletions(-) diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 71ab1f6b..1c2616b0 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -250,8 +250,7 @@ def _get_rotation_inertia(self, pb=None): R, *dR = self.rigid_tie._compute_rotation(rotvec) inertia = R @ self.inertia_body @ R.T derivatives = tuple( - derivative @ self.inertia_body @ R.T - + R @ self.inertia_body @ derivative.T + derivative @ self.inertia_body @ R.T + R @ self.inertia_body @ derivative.T for derivative in dR ) return R, inertia, derivatives @@ -275,9 +274,7 @@ def get_time_inertia_force(self, pb, acceleration, velocity): angular_velocity = velocity[3:] force[3:] = inertia @ angular_acceleration if self.rigid_tie.use_quaternion: - force[3:] += np.cross( - angular_velocity, inertia @ angular_velocity - ) + force[3:] += np.cross(angular_velocity, inertia @ angular_velocity) return force def get_time_inertia_tangent( @@ -312,8 +309,7 @@ def get_time_inertia_tangent( + np.cross(d_velocity, angular_momentum) + np.cross( angular_velocity, - inertia_derivative @ angular_velocity - + inertia @ d_velocity, + inertia_derivative @ angular_velocity + inertia @ d_velocity, ) ) return tangent @@ -651,6 +647,7 @@ def solve(self, dt, tmax, t0=0, print_info=1, solver=None): pb = NonLinear(self.assembly) if self.dynamic: from fedoo.time import Newmark + pb.set_time_integrator(SECOND_ORDER, Newmark()) if solver is not None: pb.set_solver(solver) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index f9691eaa..cf143a0a 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -85,9 +85,7 @@ class RigidTie(BCBase): rigid_tie = fd.constraint.RigidTie(right_face) """ - def __init__( - self, list_nodes, center=None, use_quaternion=True, name="Rigid Tie" - ): + def __init__(self, list_nodes, center=None, use_quaternion=True, name="Rigid Tie"): self.list_nodes = list_nodes self.center = center self.use_quaternion = bool(use_quaternion) @@ -427,12 +425,7 @@ def _compute_rotation(angle): def _compute_slave_disp(self, problem, disp_ref, rotation): """Update 2D slave-node displacements for a rigid motion.""" nodes = problem.mesh.nodes[self.list_nodes] - new_disp = ( - (nodes - self.center) @ rotation.T - + self.center - + disp_ref - - nodes - ) + new_disp = (nodes - self.center) @ rotation.T + self.center + disp_ref - nodes if not np.array_equal(problem._dU, 0): if np.array_equal(problem._U, 0): problem._dU[self._disp_indices] = new_disp @@ -449,7 +442,6 @@ def pre_update(self, problem): self._compute_slave_disp(problem, dof_ref[:2], rotation) def generate(self, problem, t_fact=1, t_fact_old=None): - var_cd = self.var_cd node_cd = self.node_cd list_nodes = self.list_nodes diff --git a/fedoo/time/generalized_alpha.py b/fedoo/time/generalized_alpha.py index 6d84e1ef..9b0115a6 100644 --- a/fedoo/time/generalized_alpha.py +++ b/fedoo/time/generalized_alpha.py @@ -184,8 +184,8 @@ def integrate(self, assembly, pb, compute="all"): self.force_start if self.force_start is not None else static_force ) force_alpha = ( - (1.0 - self.alpha_f) * static_force + self.alpha_f * force_start - ) + 1.0 - self.alpha_f + ) * static_force + self.alpha_f * force_start inertia_force = self._inertia_force( assembly, pb, mass, acc_alpha, vel_alpha ) @@ -208,9 +208,7 @@ def set_start(self, assembly, pb): ) assembly.sv["Acceleration"] = acc assembly.sv["Velocity"] = vel - assembly.sv["_DeltaDisp"] = np.zeros_like( - assembly.sv["_DeltaDisp"] - ) + assembly.sv["_DeltaDisp"] = np.zeros_like(assembly.sv["_DeltaDisp"]) if self.current_force is not None: self.force_start = self.current_force.copy() assembly.sv_start = self._copy_state(assembly.sv) diff --git a/tests/test_rigid_body_dynamics.py b/tests/test_rigid_body_dynamics.py index 0a449685..8c9453ac 100644 --- a/tests/test_rigid_body_dynamics.py +++ b/tests/test_rigid_body_dynamics.py @@ -9,9 +9,7 @@ def _make_assembly(inertia, rotvec): space = fd.ModelingSpace("3D") - tie = fd.constraint.RigidTie( - np.array([0]), center=np.zeros(3), use_quaternion=True - ) + tie = fd.constraint.RigidTie(np.array([0]), center=np.zeros(3), use_quaternion=True) tie._Q_base = Rotation.identity() tie._angles_at_base = np.zeros(3) state = {"rotvec": np.asarray(rotvec, dtype=float).copy()} @@ -44,9 +42,7 @@ def test_non_spherical_inertia_adds_gyroscopic_force(): ) np.testing.assert_allclose(force[:3], 2.0 * acceleration[:3]) np.testing.assert_allclose(force[3:], expected_moment) - assert not np.allclose( - np.cross(angular_velocity, inertia @ angular_velocity), 0.0 - ) + assert not np.allclose(np.cross(angular_velocity, inertia @ angular_velocity), 0.0) def test_spherical_inertia_has_no_gyroscopic_force(): @@ -142,6 +138,4 @@ def test_principal_axis_torque_matches_constant_angular_acceleration(): expected_angle = 0.5 * torque / inertia[0, 0] * end_time**2 rotation_vector = body.constraint.Q_total.as_rotvec() - np.testing.assert_allclose( - rotation_vector, [expected_angle, 0.0, 0.0], atol=1e-10 - ) + np.testing.assert_allclose(rotation_vector, [expected_angle, 0.0, 0.0], atol=1e-10) diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py index ce33a6f3..2144e1ff 100644 --- a/tests/test_rigid_body_kinematics.py +++ b/tests/test_rigid_body_kinematics.py @@ -24,9 +24,7 @@ def space(): def _make_assembly(rest_positions, obstacle_positions, center): - rt = fd.constraint.RigidTie( - np.arange(len(rest_positions)), center=center - ) + rt = fd.constraint.RigidTie(np.arange(len(rest_positions)), center=center) rt.center = np.asarray(center, dtype=float) asm = RigidBodyAssembly(mass=1.0, inertia_tensor=np.eye(3), rigid_tie=rt) asm._ipc_rest_positions = np.asarray(rest_positions, dtype=float) From 3ce3755bbc6c54cdffb5ead56a457c298156ac51 Mon Sep 17 00:00:00 2001 From: pruliere Date: Thu, 16 Jul 2026 16:45:35 +0200 Subject: [PATCH 16/21] Refactor/Cleaning of the automatic constraint problem registration --- fedoo/constraint/ipc_contact.py | 20 +++++++++---- fedoo/constraint/rigid_body.py | 32 ++++++++++++++------ fedoo/problem/linear.py | 16 ---------- fedoo/problem/non_linear.py | 14 --------- pyproject.toml | 2 +- tests/test_rigid_body_ipc_deformable.py | 39 +++++++++++++++++++++++-- tests/test_rigid_body_kinematics.py | 14 +++++++++ 7 files changed, 89 insertions(+), 48 deletions(-) diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index 8012d819..6707dc89 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -268,13 +268,22 @@ def __init__( def add_rigid_body(self, rigid_body, surface_node_indices=None): """Register a :class:`RigidBody` with this IPCContact. - Use this for rigid-vs-deformable (or rigid-vs-rigid) contact where - a single ``IPCContact`` carries the full collision mesh. The body's - surface nodes are routed onto the 6 rigid-body global DOFs by the - scatter matrix via the exact rigid Jacobian + This optional convenience enables direct reduced-coordinate projection + for rigid-vs-deformable (or rigid-vs-rigid) contact where a single + ``IPCContact`` carries the full collision mesh. The body's surface + nodes are routed onto the 6 rigid-body global DOFs by the scatter + matrix via the exact rigid Jacobian ``J = [I_3 | dR/d(omega) @ r_ref]``, instead of placing forces at the per-node mesh DOFs. + Registration is not required for correctness when the rigid surface + nodes already belong to the contact mesh and a :class:`RigidTie` is + attached to the problem. In that generic path, IPC assembles nodal + contact contributions and the problem's MPC condensation projects + them onto the rigid DOFs. Registration additionally associates the + body with the shared mesh and lets automatic barrier-stiffness + estimation account for generalized rigid-body forces directly. + For rigid-vs-static contact (rigid body bouncing on a fixed floor), use :meth:`RigidBody.set_static_obstacle` instead — that path uses a private 6x6 fast route with no shared mesh. @@ -1057,7 +1066,8 @@ def initialize(self, pb): self._actual_dhat = self._dhat # Create barrier potential - self._barrier_potential = ipctk.BarrierPotential(self._actual_dhat) + # Fedoo applies the adaptive barrier stiffness separately. + self._barrier_potential = ipctk.BarrierPotential(self._actual_dhat, 1.0) # Create friction potential if needed if self.friction_coefficient > 0: diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 1c2616b0..b46fde82 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -81,7 +81,6 @@ def __init__( self.time_evolution = SECOND_ORDER if dynamic else None self.storage = self if dynamic else None self.dissipation = None - self.constraints = (rigid_tie,) self._time_integrator = None self._fedoo_time_integrated = False self.dynamic = bool(dynamic) @@ -108,6 +107,11 @@ def __init__( self._contact_force = np.zeros(6) self._contact_stiffness = np.zeros((6, 6)) + def _register_global_dofs(self, pb): + """Register the rigid kinematic constraint and its global DOFs.""" + if self.rigid_tie not in pb.bc: + pb.bc.add(self.rigid_tie) + def initialize(self, pb): """Extract global DOF indices from the problem.""" rt = self.rigid_tie @@ -538,9 +542,17 @@ def set_generalized_force(self, f): """Set full generalized force [Fx, Fy, Fz, Mx, My, Mz].""" self.assembly.force[:] = f - def set_rayleigh_damping(self, alpha): - """Set mass-proportional damping: C = alpha * M.""" - self.assembly.dissipation = RayleighDamping(alpha=float(alpha)) + def set_rayleigh_damping(self, alpha=0.0, beta=0.0): + """Set Rayleigh damping: ``C = alpha*M + beta*K``. + + A rigid body has no elastic stiffness. With the private contact made + by :meth:`set_static_obstacle`, ``K`` is that contact tangent. With a + shared :class:`IPCContact`, the contact tangent belongs to the IPC + assembly instead and the rigid-body ``beta`` contribution is zero. + """ + self.assembly.dissipation = RayleighDamping( + alpha=float(alpha), beta=float(beta) + ) def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): """Enable IPC barrier contact with a STATIC obstacle surface. @@ -552,10 +564,11 @@ def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): For rigid-vs-deformable contact (e.g., a punch crushing an elastic disc), build a shared :class:`IPCContact` over the union of all - surfaces and call ``ipc.add_rigid_body(body)`` instead. In that - path the deformable obstacle's vertex positions are tracked at - every NR iteration and the IPC reaction is scattered onto the - obstacle's mesh DOFs (Newton's 3rd law honoured). + surfaces instead. Calling ``ipc.add_rigid_body(body)`` optionally + projects contact directly onto the rigid DOFs; otherwise the same + projection is performed by ``RigidTie`` MPC condensation. In both + cases the deformable obstacle's vertex positions are tracked at every + NR iteration and its reaction is assembled on the mesh DOFs. Parameters ---------- @@ -594,7 +607,8 @@ def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): asm = self.assembly asm._ipc_collision_mesh = cm asm._ipc_collisions = ipctk.NormalCollisions() - asm._ipc_barrier = ipctk.BarrierPotential(dhat) + # Fedoo applies the contact stiffness separately through _ipc_kappa. + asm._ipc_barrier = ipctk.BarrierPotential(dhat, 1.0) if kappa is None: kappa = 1e9 asm._ipc_kappa = kappa diff --git a/fedoo/problem/linear.py b/fedoo/problem/linear.py index 82430577..7bef5918 100644 --- a/fedoo/problem/linear.py +++ b/fedoo/problem/linear.py @@ -1,6 +1,5 @@ import numpy as np from fedoo.core.assembly import Assembly -from fedoo.core.lagrange_multiplier import LagrangeMultiplierAssembly from fedoo.core.problem import Problem @@ -12,7 +11,6 @@ def __init__(self, assembly: Assembly, name: str = "MainProblem"): super().__init__(mesh=assembly.mesh, name=name) self.nlgeom = False - self._register_assembly_constraints(assembly) assembly.register_global_dofs(self) assembly.initialize(self) self.time = self.dtime = 0 @@ -137,20 +135,6 @@ def GetNodalElasticEnergy(self): def assembly(self): return self.__assembly - def _register_assembly_constraints(self, assembly): - """Register constraints exposed by an assembly tree.""" - if isinstance(assembly, LagrangeMultiplierAssembly): - return - children = getattr(assembly, "_list_assembly", None) - if children: - for child in children: - self._register_assembly_constraints(child) - return - for constraint in getattr(assembly, "constraints", ()): - if constraint not in self.bc: - self.bc.add(constraint) - - class Linear(_LinearBase, Problem): """Class that defines linear problems. diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index 743124fa..c1dfccfa 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -2,7 +2,6 @@ import numpy as np from fedoo.core.assembly import Assembly from fedoo.core.assembly_sum import AssemblySum -from fedoo.core.lagrange_multiplier import LagrangeMultiplierAssembly from fedoo.core.problem import Problem from fedoo.core.time_evolution import normalize_time_evolution from fedoo.problem.line_search import line_search, _line_search_manager @@ -63,7 +62,6 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.__assembly = assembly super().__init__(A, B, D, assembly.mesh, name, assembly.space) - self._register_assembly_constraints(assembly) self.nlgeom = nlgeom self.__assembly.register_global_dofs(self) self.time_integrators = {} @@ -81,18 +79,6 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.exec_callback_at_each_iter = False self.err_num = 1e-8 # numerical error - def _register_assembly_constraints(self, assembly): - """Register constraints exposed by an assembly tree.""" - if isinstance(assembly, LagrangeMultiplierAssembly): - return - if isinstance(assembly, AssemblySum): - for child in assembly.list_assembly: - self._register_assembly_constraints(child) - return - for constraint in getattr(assembly, "constraints", ()): - if constraint not in self.bc: - self.bc.add(constraint) - def _run_constraint_hook(self, name): for bc in self.bc: hook = getattr(bc, name, None) diff --git a/pyproject.toml b/pyproject.toml index b53b7081..a18621d1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -33,7 +33,7 @@ dependencies = [ [project.optional-dependencies] all = ['fedoo[solver,plot,test,ipc]'] -ipc = ['ipctk>=1.5.0'] +ipc = ['ipctk>=1.6.0'] solver = [ 'pypardiso ; platform_machine!="arm64" and platform_machine!="aarch64"', 'python-mumps ; platform_machine=="arm64" or platform_machine=="aarch64"', diff --git a/tests/test_rigid_body_ipc_deformable.py b/tests/test_rigid_body_ipc_deformable.py index 093a060d..d3af6ff5 100644 --- a/tests/test_rigid_body_ipc_deformable.py +++ b/tests/test_rigid_body_ipc_deformable.py @@ -1,4 +1,4 @@ -"""Regression test for `RigidBody + IPCContact.add_rigid_body` coupling. +"""Regression tests for rigid-body coupling with a shared ``IPCContact``. A coarse version of `examples/contact/ipc/rigid_deformable_punch.py`. The test asserts: @@ -13,6 +13,8 @@ surface (max abs error = 0 across X/Y/Z). 5. ``set_static_obstacle`` and ``add_rigid_body`` are mutually exclusive on a single body. +6. Generic nodal IPC followed by ``RigidTie`` MPC condensation works without + explicitly registering the body on ``IPCContact``. """ import os @@ -35,7 +37,7 @@ def fresh_3d_space(): fd.Assembly.delete_memory() -def _build_punch_problem(): +def _build_punch_problem(register_rigid_body=True): """Disc + one rigid piston above. Returns (pb, ipc, solid, body, mesh, n_disc, nodes_top).""" disc = fd.mesh.box_mesh( @@ -87,7 +89,8 @@ def _build_punch_problem(): inertia_tensor=0.01 * np.eye(3), name="piston", ) - ipc.add_rigid_body(body) + if register_rigid_body: + ipc.add_rigid_body(body) assembly = fd.Assembly.sum(solid, body.assembly, ipc) pb = fd.problem.NonLinear(assembly, nlgeom=True) @@ -108,6 +111,36 @@ def _build_punch_problem(): return pb, ipc, solid, body, mesh, n_disc, nodes_top +def test_generic_ipc_uses_rigid_tie_mpc_condensation(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem( + register_rigid_body=False + ) + + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + assert not ipc._rigid_bodies + disp = pb.get_disp() + plate_top_nodes = np.where( + (mesh.nodes[:n_disc, 2] > 0.099) + & (mesh.nodes[:n_disc, 0] > 0.25) + & (mesh.nodes[:n_disc, 0] < 0.75) + & (mesh.nodes[:n_disc, 1] > 0.25) + & (mesh.nodes[:n_disc, 1] < 0.75) + )[0] + assert disp[2, plate_top_nodes].min() < -1e-3 + + q = pb.get_dof_solution()[body.assembly._dof_indices] + rotation, *_ = body.constraint._compute_rotation(q[3:]) + relative_position = mesh.nodes[nodes_top] - body.center_of_mass + expected = ( + relative_position @ rotation.T + + body.center_of_mass + + q[:3] + - mesh.nodes[nodes_top] + ) + np.testing.assert_allclose(disp[:, nodes_top].T, expected, atol=1e-9) + + def test_shapes_consistent_across_assembly_sum(fresh_3d_space): pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py index 2144e1ff..f1699da7 100644 --- a/tests/test_rigid_body_kinematics.py +++ b/tests/test_rigid_body_kinematics.py @@ -131,9 +131,23 @@ def _make_rigid_body(dynamic=True): return fd.constraint.RigidBody(mesh, dynamic=dynamic, **kwargs) +def test_rigid_body_rayleigh_damping_accepts_mass_and_stiffness_terms(space): + body = _make_rigid_body() + + body.set_rayleigh_damping(alpha=0.7, beta=0.2) + + assert body.assembly.dissipation == fd.time.RayleighDamping( + alpha=0.7, beta=0.2 + ) + + def test_static_body_is_not_compiled_by_second_order_integrator(space): body = _make_rigid_body(dynamic=False) pb = fd.problem.NonLinear(body.assembly) + + assert body.constraint in pb.bc + assert pb.n_global_dof == 6 + pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) pb.initialize() From fbd242b57b6fb1a859750ae1284cc5e0acdd5f42 Mon Sep 17 00:00:00 2001 From: pruliere Date: Thu, 16 Jul 2026 16:46:10 +0200 Subject: [PATCH 17/21] ruff format --- fedoo/problem/linear.py | 1 + tests/test_rigid_body_kinematics.py | 4 +--- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/fedoo/problem/linear.py b/fedoo/problem/linear.py index 7bef5918..d1db1016 100644 --- a/fedoo/problem/linear.py +++ b/fedoo/problem/linear.py @@ -135,6 +135,7 @@ def GetNodalElasticEnergy(self): def assembly(self): return self.__assembly + class Linear(_LinearBase, Problem): """Class that defines linear problems. diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py index f1699da7..9bff9942 100644 --- a/tests/test_rigid_body_kinematics.py +++ b/tests/test_rigid_body_kinematics.py @@ -136,9 +136,7 @@ def test_rigid_body_rayleigh_damping_accepts_mass_and_stiffness_terms(space): body.set_rayleigh_damping(alpha=0.7, beta=0.2) - assert body.assembly.dissipation == fd.time.RayleighDamping( - alpha=0.7, beta=0.2 - ) + assert body.assembly.dissipation == fd.time.RayleighDamping(alpha=0.7, beta=0.2) def test_static_body_is_not_compiled_by_second_order_integrator(space): From bc0e7e8433d57247e4b1272c05354dd65a765f59 Mon Sep 17 00:00:00 2001 From: pruliere Date: Thu, 16 Jul 2026 16:59:20 +0200 Subject: [PATCH 18/21] add dt_max arg to NonLinear problems --- fedoo/problem/non_linear.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index c1dfccfa..8dfbe998 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -1048,6 +1048,7 @@ def nlsolve( interval_output: int | float | None = None, callback: Callable[[Problem, ...], None] | None = None, exec_callback_at_each_iter: bool | None = None, + dt_max: float | None = None, ) -> None: """Solve the non linear problem using the newton-raphson algorithm. @@ -1070,6 +1071,10 @@ def nlsolve( else, the attribute t0 is modified. dt_min: float, default = 1e-6 Minimal time increment + dt_max: float, optional + Maximum time increment. The initial ``dt`` and subsequent + adaptive increases are capped to this value. If omitted, the time + increment has no upper bound other than output and end times. max_subiter: int, optional Maximal number of newton raphson iteration allowed for each time increment, after the initial linear guess. @@ -1123,6 +1128,12 @@ def nlsolve( self.tmax = tmax if t0 is not None: self.t0 = t0 # time at the start of the time step + if dt_max is not None: + if dt_max <= 0: + raise ValueError("dt_max should be strictly positive.") + if dt_max < dt_min: + raise ValueError("dt_max should be greater than or equal to dt_min.") + dt = min(dt, dt_max) if max_subiter is None: max_subiter = self.nr_parameters["max_subiter"] if dt_increase_niter is None: @@ -1224,6 +1235,8 @@ def nlsolve( # Check if dt can be increased if update_dt and nb_nr_iter <= dt_increase_niter and dt == self.dtime: dt *= 1.25 + if dt_max is not None: + dt = min(dt, dt_max) else: if self.print_info > 0: From 2253387f6020e29fcb3a8e7d4799cb46da52af98 Mon Sep 17 00:00:00 2001 From: pruliere Date: Fri, 17 Jul 2026 09:48:58 +0200 Subject: [PATCH 19/21] post-treatment and example update --- examples/rigid_body_bounce_ipc.py | 47 ++++++------ fedoo/constraint/ipc_contact.py | 17 ++++- fedoo/constraint/rigid_body.py | 22 +++++- fedoo/core/dataset.py | 57 +++++++++++++-- fedoo/core/output.py | 115 +++++++++++++++++++++++++++++- fedoo/core/problem.py | 9 +++ fedoo/util/viewer.py | 71 +++++++++++++++--- 7 files changed, 295 insertions(+), 43 deletions(-) diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py index 60f622a4..d787c095 100644 --- a/examples/rigid_body_bounce_ipc.py +++ b/examples/rigid_body_bounce_ipc.py @@ -40,7 +40,8 @@ j_size=1.5, i_resolution=6, j_resolution=6, - ).triangulate() + ).triangulate(), + name="Floor", ) body = fd.constraint.RigidBody( @@ -53,39 +54,45 @@ body.set_rayleigh_damping(1.0) body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) -# Solve using NonLinear via manual time stepping for trajectory collection +# Solve with NonLinear and save the trajectory through Fedoo's output system. pb = fd.problem.NonLinear(body.assembly) pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) -pb.initialize() -idx = body.assembly._dof_indices -z_hist = [z0] -t_hist = [0.0] +results = pb.add_output( + "rigid_body_bounce", + ["Disp", "RigidDisp", "RigidRot"], + include_static_obstacles=True, # include the plane_mesh in the results field +) t0 = time.time() -n_steps = int(round(t_end / dt)) -for step in range(n_steps): - pb.dtime = dt - pb.solve_time_increment() - pb.set_start() - dof = pb.get_dof_solution() - z_hist.append(z0 + dof[idx[2]]) - t_hist.append((step + 1) * dt) +pb.nlsolve( + dt=dt, + dt_max=dt, + tmax=t_end, + update_dt=True, + print_info=1, + interval_output=dt * 80, +) + +# Results can be visualized with ``fd.viewer(results)``. elapsed = time.time() - t0 -t_hist = np.array(t_hist) -z_hist = np.array(z_hist) -print( - f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" +# The initial state is prepended because output frames start after the first +# requested output interval. +history = results.get_history( + ["RigidDisp", "Time"], + component=["Z", 0], ) +t_hist = np.r_[0.0, history["Time"]] +z_hist = z0 + np.r_[0.0, np.asarray(history["RigidDisp"]).reshape(-1)] +print(f"\n {results.n_iter} saved frames in {elapsed:.1f}s") print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") # Animation _here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() gif_path = os.path.join(_here, "rigid_body_bounce_ipc.gif") fps = 25 -frame_skip = max(1, int(1.0 / (fps * dt))) -frame_indices = np.arange(0, len(t_hist), frame_skip) +frame_indices = np.arange(len(t_hist)) sphere = pv.Sphere( radius=radius, center=(0, 0, z0), theta_resolution=20, phi_resolution=20 diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index 6707dc89..9ae7b1d9 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -10,8 +10,6 @@ def _import_ipctk(): """Lazy import of ipctk with a clear error message.""" try: import ipctk - - return ipctk except ImportError: raise ImportError( "The 'ipctk' package is required for IPC contact. " @@ -19,6 +17,21 @@ def _import_ipctk(): "Or install fedoo with IPC support: pip install fedoo[ipc]" ) + version = getattr(ipctk, "__version__", "0") + try: + major_minor = tuple(int(part) for part in version.split(".")[:2]) + except (AttributeError, ValueError): + major_minor = (0, 0) + if major_minor < (1, 6): + raise ImportError( + f"Fedoo IPC requires ipctk >= 1.6, but the loaded runtime is " + f"ipctk {version}. Restart Python after upgrading ipctk; mixing " + "Fedoo's 1.6 API calls with an already-loaded ipctk 1.5 changes " + "BarrierPotential argument semantics and produces invalid contact " + "forces." + ) + return ipctk + class IPCContact(AssemblyBase): r"""Contact Assembly based on the IPC (Incremental Potential Contact) method. diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index b46fde82..0b4d13bd 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -23,6 +23,7 @@ from scipy import sparse from fedoo.core.base import AssemblyBase +from fedoo.core.mesh import Mesh from fedoo.core.time_evolution import SECOND_ORDER from fedoo.constraint.rigid_tie import RigidTie from fedoo.time.common import RayleighDamping @@ -104,6 +105,8 @@ def __init__( self._ipc_rest_positions = None self._ipc_n_body = 0 self._ipc_obstacle_nodes = None + self._ipc_obstacle_mesh = None + self._ipc_obstacle_source_id = None self._contact_force = np.zeros(6) self._contact_stiffness = np.zeros((6, 6)) @@ -599,10 +602,10 @@ def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): edges = ipctk.edges(all_elms) cm = ipctk.CollisionMesh(all_nodes, edges, all_elms) - # Only body↔obstacle collisions (no self-contact within rigid body) + # Only body-obstacle collisions (no self-contact within either mesh). patches = np.zeros(len(all_nodes), dtype=np.int32) patches[n_body:] = 1 - cm.can_collide = ipctk.VertexPatchesCanCollide(patches) + cm.can_collide = ipctk.make_vertex_patches_filter(patches) asm = self.assembly asm._ipc_collision_mesh = cm @@ -613,10 +616,23 @@ def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): kappa = 1e9 asm._ipc_kappa = kappa asm._ipc_dhat = dhat - asm._ipc_broad_phase = ipctk.HashGrid() + asm._ipc_broad_phase = ipctk.LBVH() asm._ipc_rest_positions = body_nodes.copy() asm._ipc_n_body = n_body asm._ipc_obstacle_nodes = obst_nodes.copy() + # Keep the same frozen geometry for optional result export. The source + # identity lets shared obstacles be deduplicated across rigid bodies. + asm._ipc_obstacle_mesh = Mesh( + obst_nodes.copy(), + obstacle_mesh.elements.copy(), + obstacle_mesh.elm_type, + node_sets=obstacle_mesh.node_sets, + element_sets=obstacle_mesh.element_sets, + ndim=obstacle_mesh.ndim, + name=obstacle_mesh.name, + register_name=False, + ) + asm._ipc_obstacle_source_id = id(obstacle_mesh) def add_to_problem(self, pb): """Register the rigid body's kinematic tie with a Fedoo problem. diff --git a/fedoo/core/dataset.py b/fedoo/core/dataset.py index 921f424c..4c9b5f3e 100644 --- a/fedoo/core/dataset.py +++ b/fedoo/core/dataset.py @@ -549,6 +549,11 @@ def plot( Only used with MultiMesh objects. Restrict the plot to the selected submesh ids, submesh names or element types. + missing_field_color : color, optional + Only used with MultiMesh objects. Render submeshes on which the + selected element or Gauss-point field is absent with this solid + color. By default, submeshes missing the field are not plotted. + clip_args : dict, optional Dictionary of arguments to pass to the pyvista clip filter in order to clip the current plot. @@ -999,6 +1004,29 @@ def _submesh_dataset(self, submesh_id: int) -> "DataSet": } return dataset + def _submesh_has_field(self, field, submesh_id: int) -> bool: + """Return whether a field has data on one MultiMesh submesh.""" + if field in self.node_data: + used_nodes = np.unique(self.mesh[submesh_id].elements) + values = np.asarray(self.node_data[field])[..., used_nodes] + try: + return not np.all(np.isnan(values)) + except TypeError: + return True + for field_dict in (self.element_data, self.gausspoint_data): + if field in field_dict: + submesh = self.mesh[submesh_id] + return ( + self._multimesh_block_for_submesh( + field_dict[field], + submesh_id, + submesh.n_elements, + fill_missing=False, + ) + is not None + ) + return False + def _submesh_element_set( self, element_set, @@ -1066,6 +1094,8 @@ def _plot_multimesh( ): """Plot a MultiMesh by reusing the single-mesh plotting path.""" global_element_set = kargs.pop("global_element_set", False) + missing_field_color = kargs.pop("missing_field_color", None) + default_solid_color = kargs.pop("color", "lightgray") submesh_indices = self._resolve_submesh_indices(selected_submeshes) if ( element_set is not None @@ -1092,6 +1122,7 @@ def _plot_multimesh( base_name = kargs.pop("name", None) scalar_bar_added = False plotted = False + scalar_plotted = False for submesh_id in submesh_indices: local_element_set, should_plot = self._submesh_element_set( @@ -1105,15 +1136,28 @@ def _plot_multimesh( continue subdataset = self._submesh_dataset(submesh_id) - if field is not None: + field_available = field is None or self._submesh_has_field( + field, submesh_id + ) + if field is not None and not field_available: + if missing_field_color is None: + continue + submesh_field = None + submesh_color = missing_field_color + else: + submesh_field = field + submesh_color = default_solid_color + if submesh_field is not None: try: - subdataset.get_data(field, component, data_type) + subdataset.get_data(submesh_field, component, data_type) except NameError: continue sub_kargs = dict(kargs) + if submesh_field is None: + sub_kargs["color"] = submesh_color if base_name is not None: sub_kargs["name"] = f"{base_name}_{submesh_id}" - if field is not None and scalar_bar_added: + if submesh_field is not None and scalar_bar_added: sub_kargs["show_scalar_bar"] = False sub_kargs["_extra_cell_data"] = { "_fedoo_global_cell_ids": ( @@ -1132,7 +1176,7 @@ def _plot_multimesh( sub_clip_args = None pl = subdataset.plot( - field=field, + field=submesh_field, component=component, data_type=data_type, scale=scale, @@ -1159,14 +1203,15 @@ def _plot_multimesh( **sub_kargs, ) plotted = True - if field is not None: + if submesh_field is not None: scalar_bar_added = True + scalar_plotted = True if pl is None: pl = pv.Plotter(window_size=window_size) backgroundplotter = False - if field is not None and not plotted: + if field is not None and not scalar_plotted: raise NameError(f"Field data {field!r} not found on any selected submesh.") if title is None: diff --git a/fedoo/core/output.py b/fedoo/core/output.py index c28d94e3..b38de28e 100644 --- a/fedoo/core/output.py +++ b/fedoo/core/output.py @@ -137,6 +137,99 @@ def _output_mesh_for_assembly(assemb, element_set=None): return assemb.mesh.extract_elements(element_set) +def _mesh_with_static_obstacles(assemb, element_set=None): + """Build an output MultiMesh containing results and static geometry.""" + result_mesh = _output_mesh_for_assembly(assemb, element_set) + + def leaf_assemblies(assembly): + children = getattr(assembly, "list_assembly", None) + if children is None: + yield assembly + else: + for child in children: + yield from leaf_assemblies(child) + + obstacles = [] + seen = set() + for sub_assemb in leaf_assemblies(assemb): + obstacle = getattr(sub_assemb, "_ipc_obstacle_mesh", None) + if obstacle is None: + continue + obstacle_id = getattr(sub_assemb, "_ipc_obstacle_source_id", None) + if obstacle_id is None: + obstacle_id = id(obstacle) + if obstacle_id not in seen: + seen.add(obstacle_id) + obstacles.append(obstacle) + + if not obstacles: + return result_mesh + ndim = max(mesh.ndim for mesh in (result_mesh, *obstacles)) + + node_blocks = [] + submesh_entries = {} + node_offset = 0 + + def append_mesh(mesh, default_name): + nonlocal node_offset + mesh = mesh.as_ndim(ndim) + node_blocks.append(mesh.nodes) + submeshes = mesh.submeshes if isinstance(mesh, MultiMesh) else (mesh,) + for submesh in submeshes: + base_name = submesh.name or default_name + name = base_name + suffix = 2 + while name in submesh_entries: + name = f"{base_name}_{suffix}" + suffix += 1 + submesh_entries[name] = ( + submesh.elm_type, + submesh.elements + node_offset, + submesh.element_sets, + ) + node_offset += mesh.n_nodes + + append_mesh(result_mesh, "Results") + for i, obstacle in enumerate(obstacles, start=1): + append_mesh(obstacle, f"Static obstacle {i}") + + return MultiMesh( + np.vstack(node_blocks), + submesh_entries, + ndim=ndim, + name=getattr(result_mesh, "name", ""), + register_name=False, + ) + + +def _add_static_geometry_to_results(result, result_mesh, output_mesh): + """Extend result fields to an output mesh containing static nodes.""" + if output_mesh is result_mesh or output_mesh.n_nodes == result_mesh.n_nodes: + return result + + n_extra = output_mesh.n_nodes - result_mesh.n_nodes + for field, value in tuple(result.node_data.items()): + value = np.asarray(value) + if value.ndim == 0 or value.shape[-1] != result_mesh.n_nodes: + continue + dtype = np.result_type(value.dtype, float) + value = value.astype(dtype, copy=False) + fill_value = 0.0 if field == "Disp" else np.nan + padding = np.full(value.shape[:-1] + (n_extra,), fill_value, dtype=dtype) + result.node_data[field] = np.concatenate((value, padding), axis=-1) + + # A regular result mesh becomes submesh 0 of the augmented MultiMesh. + # Element and Gauss-point data therefore remain absent on obstacle blocks. + if not isinstance(result_mesh, MultiMesh): + result.element_data = { + field: {0: value} for field, value in result.element_data.items() + } + result.gausspoint_data = { + field: {0: value} for field, value in result.gausspoint_data.items() + } + return result + + def _find_constitutivelaw_with_method(weakform, method_name): """Return a constitutive law from a possibly nested weakform.""" law = getattr(weakform, "constitutivelaw", None) @@ -549,6 +642,7 @@ def add_output( position=1, element_set=None, save_mesh=True, + include_static_obstacles=False, ): dirname = os.path.dirname(filename) # filename = os.path.basename(filename) @@ -566,6 +660,11 @@ def add_output( 0 ] # remove extension for the base name + if include_static_obstacles and file_format != "fdh5": + raise ValueError( + "include_static_obstacles=True requires the 'fdh5' file format." + ) + if file_format not in _available_format: print( "WARNING: '", @@ -590,7 +689,10 @@ def add_output( if isinstance(assemb, str): assemb = AssemblyBase.get_all()[assemb] - mesh = _output_mesh_for_assembly(assemb, element_set) + if include_static_obstacles: + mesh = _mesh_with_static_obstacles(assemb, element_set) + else: + mesh = _output_mesh_for_assembly(assemb, element_set) if not (os.path.isdir(dirname)) and dirname != "": os.mkdir(dirname) @@ -604,6 +706,7 @@ def add_output( "position": position, "element_set": element_set, "compressed": compressed, + "include_static_obstacles": include_static_obstacles, } self.__list_output.append(new_output) @@ -642,9 +745,15 @@ def save_results(self, pb, comp_output=None): position = output["position"] element_set = output["element_set"] compressed = output["compressed"] + include_static_obstacles = output["include_static_obstacles"] assemb = output["assembly"] # material = assemb.weakform.GetConstitutiveLaw() + result_mesh = _output_mesh_for_assembly(assemb, element_set) + if include_static_obstacles: + output_mesh = _mesh_with_static_obstacles(assemb, element_set) + else: + output_mesh = result_mesh if file_format in _available_format: # if not ignored if (comp_output is None) or (file_format in ["fdz", "fdh5"]): @@ -663,7 +772,7 @@ def save_results(self, pb, comp_output=None): list_file_format.append(file_format) list_compressed.append(compressed) - out = DataSet(_output_mesh_for_assembly(assemb, element_set)) + out = DataSet(output_mesh) list_data.append(out) else: # else, the same file is used @@ -679,6 +788,8 @@ def save_results(self, pb, comp_output=None): element_set, False, ) + if include_static_obstacles: + _add_static_geometry_to_results(res, result_mesh, output_mesh) out.add_data(res) for i, out in enumerate(list_data): diff --git a/fedoo/core/problem.py b/fedoo/core/problem.py index eee4d2b3..abe3ad17 100644 --- a/fedoo/core/problem.py +++ b/fedoo/core/problem.py @@ -135,6 +135,7 @@ def add_output( position=1, element_set=None, save_mesh=True, + include_static_obstacles=False, ): """Add output requirement for automatic saving during nlsolve. @@ -179,6 +180,13 @@ def add_output( save_mesh : bool, default = True If True the mesh is saved. + + include_static_obstacles : bool, default = False + If True, static meshes registered by + :meth:`fedoo.constraint.RigidBody.set_static_obstacle` are added + to the output mesh. The same obstacle mesh shared by several + rigid bodies is included only once. This option requires the + ``fdh5`` file format, which preserves the MultiMesh structure. """ if output_list is None and hasattr(self, "assembly"): output_list = assembly @@ -194,6 +202,7 @@ def add_output( position, element_set, save_mesh, + include_static_obstacles, ) def save_results(self, iterOutput=None): diff --git a/fedoo/util/viewer.py b/fedoo/util/viewer.py index 40ee5b2d..b4b1d54d 100644 --- a/fedoo/util/viewer.py +++ b/fedoo/util/viewer.py @@ -31,6 +31,7 @@ from fedoo.core.mesh import MultiMesh USE_PYVISTA_QT = True +SOLID_COLOR_FIELD = "Solid color" def _is_multimesh(mesh): @@ -236,6 +237,8 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): ##################################################################### if opts: self.opts = opts + self.opts.setdefault("color", "lightgray") + self.opts.setdefault("color_missing_fields", True) else: self.opts = { # Scalar bar options @@ -259,6 +262,8 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): "metallic": 1.0, "roughness": 0.5, "diffuse": 1.0, + "color": "lightgray", + "color_missing_fields": True, # Clip plane parameters "clip_args": None, "clip_origin": None, @@ -281,7 +286,7 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): self.current_field = "Stress" self.current_comp = "vm" elif "Disp" in field_names: - self.current_field = "disp" + self.current_field = "Disp" self.current_comp = "X" else: try: @@ -402,10 +407,17 @@ def update_plot(self, val=None, iteration=None, lock_view=True, plotter=None): if is_multimesh: multimesh_kargs["global_element_set"] = True multimesh_kargs["name"] = "data" + if self.current_field is not None and self.opts.get( + "color_missing_fields", True + ): + multimesh_kargs["missing_field_color"] = self.opts["color"] with _defer_rendering(plotter, is_multimesh): # plotter.clear() # not compatible with pbr ??? plotter.renderer.clear_actors() + plot_kargs = {} + if self.current_field is None: + plot_kargs["color"] = self.opts["color"] self.data.plot( field=self.current_field, component=self.current_comp, @@ -431,6 +443,7 @@ def update_plot(self, val=None, iteration=None, lock_view=True, plotter=None): # element_set_invert = self.opts["element_set_invert"], cmap=self.opts["cmap"], **multimesh_kargs, + **plot_kargs, ) if self.parent()._plane_widget_enabled: @@ -1218,14 +1231,18 @@ def _set_active(self, dock): # old_state = self.field_combo.blockSignals(True) # update_field_combo + dock = self.active_dock + current_field = dock.current_field + field_blocker = QSignalBlocker(self.field_combo) self.field_combo.clear() + self.field_combo.addItem(SOLID_COLOR_FIELD) if self.data is not None: self.field_combo.addItems(self.data.field_names()) - dock = self.active_dock if dock.current_field is not None: self.field_combo.setCurrentText(dock.current_field) else: - self.field_combo.setCurrentIndex(0) + self.field_combo.setCurrentText(SOLID_COLOR_FIELD) + del field_blocker # self.field_combo.blockSignals(old_state) # update data_type value @@ -1233,7 +1250,7 @@ def _set_active(self, dock): self.avg_combo.setCurrentText(dock.current_data_type) # update component combo - self.update_components(dock.current_field) + self.update_components(current_field) # self.avg_combo.blockSignals(old_state) if self._plot_dialog: # if plot dialog exist @@ -1372,6 +1389,7 @@ def iteration(self) -> int: return self.iter_slider.value() def on_field_changed(self, field): + field = None if field == SOLID_COLOR_FIELD else field self.update_components(field) if self.active_dock: self.update_plot_with_clim(lock_view=True) @@ -1380,8 +1398,10 @@ def get_components(self, field): return self.active_dock.get_components(field) def update_components(self, field): - if not field or not self.active_dock: + if not field or field == SOLID_COLOR_FIELD or not self.active_dock: comps = [""] + if self.active_dock: + self.active_dock.current_field = None else: comps = self.get_components(field) self.active_dock.current_field = field @@ -1397,7 +1417,7 @@ def update_components(self, field): @property def current_field(self): field = self.field_combo.currentText() - if field == "": + if field in ("", SOLID_COLOR_FIELD): return None else: return self.field_combo.currentText() @@ -1672,6 +1692,10 @@ def _apply_renderer_options(self): list_docks = [self.active_dock] for dock in list_docks: dock.opts["opacity"] = float(self._renderer_dialog.opacity_spin.value()) + dock.opts["color"] = self._renderer_dialog.color + dock.opts["color_missing_fields"] = ( + self._renderer_dialog.missing_fields_cb.isChecked() + ) dock.opts["pbr"] = self._renderer_dialog.pbr_cb.isChecked() dock.opts["metallic"] = float(self._renderer_dialog.metallic_spin.value()) dock.opts["roughness"] = float(self._renderer_dialog.roughness_spin.value()) @@ -1684,7 +1708,9 @@ def update_plot_with_clim( if dock is None: if self._sync_field: for d in self.all_docks: - if self.current_field in d.data.field_names(): + if self.current_field is None: + d.current_field = None + elif self.current_field in d.data.field_names(): d.current_field = self.current_field # Only set component if field exists in this dock comps = d.get_components(self.current_field) @@ -1701,7 +1727,7 @@ def update_plot_with_clim( dock.current_data_type = self.current_data_type - if dock.opts["clim_mode"] == "all": + if dock.current_field is not None and dock.opts["clim_mode"] == "all": if hasattr(dock.data, "get_all_frame_lim"): dock.opts["clim"] = dock.data.get_all_frame_lim( field=dock.current_field, @@ -2712,9 +2738,9 @@ def _filter_cmaps(self, text): self.cmap_preview.update_preview(filtered[0], self.cmap_reverse.isChecked()) def _update_clim_values(self): - if self.rb_manual.isChecked(): - return parent = self.parent() + if self.rb_manual.isChecked() or parent.current_field is None: + return if self.rb_current.isChecked(): data = parent.get_current_data() data = _global_data_array(data) @@ -2828,6 +2854,15 @@ def __init__(self, parent=None): "Overall opacity (0.0 = transparent, 1.0 = opaque)." ) + self.color_button = QtWidgets.QPushButton() + self.color_button.setToolTip("Color used for geometry without field data.") + self.color_button.clicked.connect(self._choose_color) + self.color = "lightgray" + + self.missing_fields_cb = QtWidgets.QCheckBox( + "Apply to objects missing selected field" + ) + self.pbr_cb = QtWidgets.QCheckBox("Activate Physically Based Rendering (PBR)") self.pbr_cb.setToolTip("Enable VTK's PBR shading model for the actor/material.") @@ -2862,6 +2897,8 @@ def __init__(self, parent=None): form = QtWidgets.QFormLayout() form.addRow("Opacity:", self.opacity_spin) + form.addRow("Solid color:", self.color_button) + form.addRow(self.missing_fields_cb) main.addLayout(form) # PBR layout @@ -2895,11 +2932,25 @@ def __init__(self, parent=None): def update_values(self): opts = self.parent().opts self.opacity_spin.setValue(float(opts["opacity"])) + self._set_color(opts.get("color", "lightgray")) + self.missing_fields_cb.setChecked(opts.get("color_missing_fields", True)) self.pbr_cb.setChecked(opts["pbr"]) self.metallic_spin.setValue(float(opts["metallic"])) self.roughness_spin.setValue(float(opts["roughness"])) self.diffuse_spin.setValue(float(opts["diffuse"])) + def _set_color(self, color): + qcolor = QtGui.QColor(color) + if not qcolor.isValid(): + qcolor = QtGui.QColor("lightgray") + self.color = qcolor.name() + self.color_button.setStyleSheet(f"background-color: {self.color};") + + def _choose_color(self): + color = QtWidgets.QColorDialog.getColor(QtGui.QColor(self.color), self) + if color.isValid(): + self._set_color(color) + def _toggle_pbr_group(self, checked: bool): self.pbr_group.setEnabled(checked) From 4d45c09842587f5566314cc8b995e84d38486ab0 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Fri, 17 Jul 2026 11:20:54 +0200 Subject: [PATCH 20/21] Refactor rigid-body IPC and add scatter utility Consolidate IPC barrier and rigid-body helpers, improve robustness, and add a dense->sparse scatter utility. - Add _barrier_hessian_psd in ipc_contact to centralize PSD projection with a degenerate (NONE) fallback; both IPCContact and RigidBodyAssembly now use this policy. - Centralize rigid-DOF handling in rigid_tie: _resolve_rigid_dofs and _apply_slave_disp reduce duplicated DOF/disp logic, and rotation_jacobian provides a shared rotation + per-point derivative API used by IPC and RigidBodyAssembly. - RigidBodyAssembly: enforce 3D-only, introduce _N_RIGID_DOF constant, scale static_regularisation by mass/inertia magnitude, reuse RigidTie DOF indices, use rotation_jacobian, and compute contact stiffness via the shared PSD helper. Use scatter_dense_block to place dense stiffness blocks into the global sparse matrix. - IPCContact: factor scatter-matrix refresh into _refresh_rigid_scatter and call it from update/assemble/initialize; validate/pad global gradient lengths and raise if inconsistent; use _barrier_hessian_psd for Hessian assembly. - Add scatter_dense_block helper to time.common to build sparse csr matrices from dense blocks and indices; update generalized_alpha to use it for dynamic matrix assembly. - Minor docs and comment improvements in Mesh.merge_coincident_nodes and other small cleanups. These changes reduce duplication, improve handling of degenerate IPC Hessians, make dense block scattering consistent, and tighten rigid-body bookkeeping. --- fedoo/constraint/ipc_contact.py | 110 +++++++++++++++----------- fedoo/constraint/rigid_body.py | 91 ++++++++++------------ fedoo/constraint/rigid_tie.py | 134 +++++++++++++++++--------------- fedoo/core/mesh.py | 9 +++ fedoo/time/common.py | 18 +++++ fedoo/time/generalized_alpha.py | 13 ++-- 6 files changed, 211 insertions(+), 164 deletions(-) diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index 9ae7b1d9..19dbc962 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -33,6 +33,30 @@ def _import_ipctk(): return ipctk +def _barrier_hessian_psd(barrier, collisions, collision_mesh, vertices): + """ipctk barrier hessian with PSD projection and a degenerate fallback. + + CLAMP/ABS PSD projection can raise on degenerate (zero-distance) contact + pairs; retry without projection so assembly still proceeds. Shared by + :class:`IPCContact` and ``RigidBodyAssembly`` so both use the same policy. + """ + ipctk = _import_ipctk() + try: + return barrier.hessian( + collisions, + collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, + ) + except RuntimeError: + return barrier.hessian( + collisions, + collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, + ) + + class IPCContact(AssemblyBase): r"""Contact Assembly based on the IPC (Incremental Potential Contact) method. @@ -433,7 +457,7 @@ def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim, pb=None): n_rb = len(local_indices) - # Get exact rotation derivatives from RigidTie. The Jacobian + # Exact rotation derivatives from RigidTie. The Jacobian # J = [I_3 | dR/d(omega) @ r_ref] uses only r_ref (reference # positions) — current vertex positions and translation are # not needed. @@ -443,22 +467,17 @@ def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim, pb=None): else: angles = np.zeros(3) - _, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) - center = rt.center # initial center position - # Translation part: J_i[:3, :3] = I_3 for d in range(ndim): all_rows.append(np.full(n_rb, dof_indices[d])) all_cols.append(local_indices * ndim + d) all_data.append(np.ones(n_rb)) - # Rotation part: exact derivatives from RigidTie - # J_i[d, 3+k] = (dR_drk @ r_ref_i)[d] - # where r_ref = vertex_ref - center (in reference config) - r_ref = self._rest_positions[local_indices] - center # (n_rb, 3) - du_drx = r_ref @ dR_drx.T # (n_rb, 3) - du_dry = r_ref @ dR_dry.T - du_drz = r_ref @ dR_drz.T + # Rotation part: J_i[d, 3+k] = (dR_drk @ r_ref_i)[d], + # r_ref = vertex_ref - center. + _, du_drx, du_dry, du_drz = rt.rotation_jacobian( + angles, self._rest_positions[local_indices] + ) for d in range(ndim): # RotX contribution @@ -551,8 +570,17 @@ def _get_elastic_gradient_on_surface(self): n_mesh_dof = self.space.nvar * self.mesh.n_nodes grad_energy_full = grad_energy_full[:n_mesh_dof] - # Ensure dimension matches P rows + # Assembly vectors that don't span the extra global DOFs (PeriodicBC, + # RigidTie, RigidBody) are shorter than P's row count; zero-padding the + # global-DOF tail is correct since they contribute nothing there. A + # vector LONGER than P's rows means the DOF bookkeeping is inconsistent + # — fail loudly rather than silently truncate. n_P_rows = self._scatter_matrix.shape[0] + if len(grad_energy_full) > n_P_rows: + raise ValueError( + f"Global vector length {len(grad_energy_full)} exceeds scatter " + f"matrix rows {n_P_rows}; contact DOF bookkeeping is inconsistent." + ) if len(grad_energy_full) < n_P_rows: grad_energy_full = np.pad( grad_energy_full, (0, n_P_rows - len(grad_energy_full)) @@ -650,22 +678,12 @@ def _compute_ipc_contributions(self, vertices, compute="all"): self.global_vector = -self._kappa * (P @ grad_surf) if compute != "vector": - try: - hess_surf = self._barrier_potential.hessian( - self._collisions, - self._collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, - ) - except RuntimeError: - # CLAMP/ABS projection can fail on degenerate contact - # pairs (zero-distance); skip projection entirely. - hess_surf = self._barrier_potential.hessian( - self._collisions, - self._collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, - ) + hess_surf = _barrier_hessian_psd( + self._barrier_potential, + self._collisions, + self._collision_mesh, + vertices, + ) if axi_D is not None: # Axisymmetric IPC uses a single 2*pi*r weight on the residual. # The physically closer single-weight tangent would be: @@ -940,6 +958,22 @@ def _ogc_step_filter_callback(self, pb, dX, is_elastic_prediction=False): else: self._ogc_filter_step(pb, dX) + def _refresh_rigid_scatter(self, pb): + """Rebuild the rotation-dependent scatter matrix for rigid bodies. + + The rigid columns of the scatter matrix depend on the current rigid + rotation, so the matrix is refreshed on every NR iteration (from both + :meth:`update` and :meth:`assemble_global_mat`) whenever rigid bodies + are registered. No-op otherwise. + """ + if self._rigid_bodies and pb is not None: + self._scatter_matrix = self._build_scatter_matrix( + self._surface_node_indices, + self.mesh.n_nodes, + self.space.ndim, + pb, + ) + def assemble_global_mat(self, compute="all"): """Recompute barrier contributions from cached vertex positions. @@ -954,13 +988,7 @@ def assemble_global_mat(self, compute="all"): consistent with the kinematic constraint. """ if self._last_vertices is not None: - if self._rigid_bodies and self._pb is not None: - self._scatter_matrix = self._build_scatter_matrix( - self._surface_node_indices, - self.mesh.n_nodes, - self.space.ndim, - self._pb, - ) + self._refresh_rigid_scatter(self._pb) self._compute_ipc_contributions(self._last_vertices, compute) def initialize(self, pb): @@ -1247,15 +1275,9 @@ def update(self, pb, compute="all"): vertices = self._get_current_vertices(pb) self._last_vertices = vertices - # Rebuild scatter matrix for rigid bodies (Jacobian uses r_ref but - # depends on the current rotation, so refresh after every update) - if self._rigid_bodies: - self._scatter_matrix = self._build_scatter_matrix( - self._surface_node_indices, - self.mesh.n_nodes, - self.space.ndim, - pb, - ) + # The rigid Jacobian depends on the current rotation, so refresh the + # scatter matrix after every update. + self._refresh_rigid_scatter(pb) # Rebuild collision set self._collisions.build( diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index 0b4d13bd..cec31b93 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -20,13 +20,15 @@ """ import numpy as np -from scipy import sparse from fedoo.core.base import AssemblyBase from fedoo.core.mesh import Mesh from fedoo.core.time_evolution import SECOND_ORDER from fedoo.constraint.rigid_tie import RigidTie -from fedoo.time.common import RayleighDamping +from fedoo.time.common import RayleighDamping, scatter_dense_block + +# Rigid-body dynamics is formulated in 3D: 3 translational + 3 rotational DOFs. +_N_RIGID_DOF = 6 class RigidBodyAssembly(AssemblyBase): @@ -74,10 +76,16 @@ def __init__( space = ModelingSpace.get_active() AssemblyBase.__init__(self, name, space) + if self.space.ndim != 3: + raise NotImplementedError( + "RigidBodyAssembly supports 3D modeling spaces only " + f"(got ndim={self.space.ndim}). RigidTie2D provides 2D rigid " + "kinematics, but 2D rigid-body dynamics is not yet implemented." + ) self.mass = float(mass) self.inertia_body = np.asarray(inertia_tensor, dtype=float) self.rigid_tie = rigid_tie - self.force = np.zeros(6) + self.force = np.zeros(_N_RIGID_DOF) self.mesh = mesh self.time_evolution = SECOND_ORDER if dynamic else None self.storage = self if dynamic else None @@ -85,10 +93,12 @@ def __init__( self._time_integrator = None self._fedoo_time_integrated = False self.dynamic = bool(dynamic) - # A compiled time integrator supplies a mass tangent. Without one, - # the assembly is static and this tiny diagonal keeps free rigid DOFs - # well posed before contact. - self.static_regularisation = 1e-9 + # A compiled time integrator supplies a mass tangent. Without one, the + # assembly is static and this tiny diagonal keeps the free rigid DOFs + # well posed before contact. It is scaled to the body's mass/inertia + # magnitude so it stays negligible yet non-zero under any unit system. + mass_scale = max(self.mass, float(np.trace(self.inertia_body)), 1.0) + self.static_regularisation = 1e-9 * mass_scale self._dof_indices = None self._pb_ref = None @@ -107,8 +117,8 @@ def __init__( self._ipc_obstacle_nodes = None self._ipc_obstacle_mesh = None self._ipc_obstacle_source_id = None - self._contact_force = np.zeros(6) - self._contact_stiffness = np.zeros((6, 6)) + self._contact_force = np.zeros(_N_RIGID_DOF) + self._contact_stiffness = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) def _register_global_dofs(self, pb): """Register the rigid kinematic constraint and its global DOFs.""" @@ -118,14 +128,9 @@ def _register_global_dofs(self, pb): def initialize(self, pb): """Extract global DOF indices from the problem.""" rt = self.rigid_tie - self._dof_indices = np.array( - [ - pb.n_node_dof - + pb._global_dof.indice_start(rt.var_cd[i]) - + rt.node_cd[i] - for i in range(6) - ] - ) + # Same global-DOF layout the RigidTie constraint resolves; reuse it + # so the two never drift apart. + self._dof_indices = np.array(rt._get_dof_ref(pb)[1]) self._pb_ref = pb if self.mesh is None: self.mesh = pb.mesh @@ -151,15 +156,13 @@ def _build_ipc_jacobian(self, rt, angles): J : ndarray, shape (n_all*3, 6) Body vertices use exact derivatives; obstacle rows are zero. """ - R, dR_drx, dR_dry, dR_drz = rt._compute_rotation(angles) - r_ref = self._ipc_rest_positions - rt.center - du_drx = r_ref @ dR_drx.T - du_dry = r_ref @ dR_dry.T - du_drz = r_ref @ dR_drz.T + _, du_drx, du_dry, du_drz = rt.rotation_jacobian( + angles, self._ipc_rest_positions + ) n_body = self._ipc_n_body n_all = n_body + len(self._ipc_obstacle_nodes) - J = np.zeros((n_all * 3, 6)) + J = np.zeros((n_all * 3, _N_RIGID_DOF)) for d in range(3): J[d : n_body * 3 : 3, d] = 1.0 J[d : n_body * 3 : 3, 3] = du_drx[:, d] @@ -210,9 +213,7 @@ def compute_contact(self, q, rt, compute="all"): self._contact_stiffness[:] = 0 return self._contact_force, self._contact_stiffness - from fedoo.constraint.ipc_contact import _import_ipctk - - ipctk = _import_ipctk() + from fedoo.constraint.ipc_contact import _barrier_hessian_psd J = self._build_ipc_jacobian(rt, q[3:]) @@ -222,20 +223,12 @@ def compute_contact(self, q, rt, compute="all"): self._contact_force = -self._ipc_kappa * (J.T @ grad) if compute in ("all", "stiffness"): - try: - hess = self._ipc_barrier.hessian( - self._ipc_collisions, - self._ipc_collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, - ) - except RuntimeError: - hess = self._ipc_barrier.hessian( - self._ipc_collisions, - self._ipc_collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, - ) + hess = _barrier_hessian_psd( + self._ipc_barrier, + self._ipc_collisions, + self._ipc_collision_mesh, + vertices, + ) self._contact_stiffness = self._ipc_kappa * (J.T @ (hess @ J)) else: self._contact_stiffness[:] = 0 @@ -264,7 +257,7 @@ def _get_rotation_inertia(self, pb=None): def _get_mass_matrix(self, pb=None): """Return the rigid-body mass matrix in the current global frame.""" - M = np.zeros((6, 6)) + M = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) M[:3, :3] = self.mass * np.eye(3) _, M[3:, 3:], _ = self._get_rotation_inertia(pb) return M @@ -273,7 +266,7 @@ def get_time_inertia_force(self, pb, acceleration, velocity): """Return translational inertia and Euler's rotational inertia force.""" acceleration = np.asarray(acceleration, dtype=float) velocity = np.asarray(velocity, dtype=float) - force = np.zeros(6) + force = np.zeros(_N_RIGID_DOF) force[:3] = self.mass * acceleration[:3] _, inertia, _ = self._get_rotation_inertia(pb) @@ -295,7 +288,7 @@ def get_time_inertia_tangent( """Return the effective tangent of the rigid-body inertia force.""" acceleration = np.asarray(acceleration, dtype=float) velocity = np.asarray(velocity, dtype=float) - tangent = np.zeros((6, 6)) + tangent = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) tangent[:3, :3] = acceleration_factor * self.mass * np.eye(3) _, inertia, inertia_derivatives = self._get_rotation_inertia(pb) @@ -352,11 +345,8 @@ def assemble_global_mat(self, compute="all"): ) if compute in ("all", "matrix"): - stiffness = self._contact_stiffness + regularisation * np.eye(6) - self.global_matrix = sparse.csr_matrix( - (stiffness.ravel(), (np.repeat(idx, 6), np.tile(idx, 6))), - shape=(n, n), - ) + stiffness = self._contact_stiffness + regularisation * np.eye(_N_RIGID_DOF) + self.global_matrix = scatter_dense_block(stiffness, idx, (n, n)) if compute in ("all", "vector"): self.global_vector = np.zeros(n) self.global_vector[idx] = self.force + self._contact_force @@ -374,7 +364,7 @@ def update(self, pb, compute="all"): q = ( np.asarray(dof_solution)[self._dof_indices] if not np.isscalar(dof_solution) - else np.zeros(6) + else np.zeros(_N_RIGID_DOF) ) self.compute_contact(q, self.rigid_tie, compute="all") @@ -602,7 +592,8 @@ def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): edges = ipctk.edges(all_elms) cm = ipctk.CollisionMesh(all_nodes, edges, all_elms) - # Only body-obstacle collisions (no self-contact within either mesh). + # Only body-obstacle collisions (no self-contact within either mesh): + # the filter blocks same-patch pairs, so body (0) and obstacle (1) only. patches = np.zeros(len(all_nodes), dtype=np.int32) patches[n_body:] = 1 cm.can_collide = ipctk.make_vertex_patches_filter(patches) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 1cb0bdbd..2683ba97 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -8,6 +8,45 @@ from simcoon import dR_drotvec +def _resolve_rigid_dofs(problem, var_cd, node_cd, n_dof): + """Return the ``n_dof`` rigid DOF values and their global indices. + + Shared by :class:`RigidTie` (6 DOF) and :class:`RigidTie2D` (3 DOF). The + values fall back to zeros before the problem holds a solution. + """ + dof_cd = [ + problem.n_node_dof + problem._global_dof.indice_start(var_cd[i]) + node_cd[i] + for i in range(n_dof) + ] + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(n_dof), dof_cd + return np.asarray(xbc)[dof_cd], dof_cd + if np.isscalar(xbc) and xbc == 0: + return np.asarray(dof_sol)[dof_cd], dof_cd + return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + + +def _apply_slave_disp(problem, list_nodes, center, disp_ref, rotation, disp_indices): + """Write rigid slave-node displacements into ``problem._dU``. + + Shared by :class:`RigidTie` (3D rotation matrix) and :class:`RigidTie2D` + (2D rotation matrix); ``rotation`` and ``center`` carry the dimension. + Returns the computed slave displacement. + """ + nodes = problem.mesh.nodes[list_nodes] + new_disp = (nodes - center) @ rotation.T + center + disp_ref - nodes + if not np.array_equal(problem._dU, 0): + if np.array_equal(problem._U, 0): + problem._dU[disp_indices] = new_disp + else: + problem._dU[disp_indices] = new_disp - problem._U[disp_indices] + return new_disp + + class RigidTie(BCBase): """Constraint that eliminate dof assuming a rigid body tie between nodes. @@ -152,22 +191,7 @@ def initialize(self, problem): def _get_dof_ref(self, problem): """Return the current six rigid-body DOFs and their global indices.""" - dof_cd = [ - problem.n_node_dof - + problem._global_dof.indice_start(self.var_cd[i]) - + self.node_cd[i] - for i in range(6) - ] - dof_sol = problem.get_dof_solution() - xbc = problem._Xbc - - if np.isscalar(dof_sol) and dof_sol == 0: - if np.isscalar(xbc) and xbc == 0: - return np.zeros(6), dof_cd - return np.asarray(xbc)[dof_cd], dof_cd - if np.isscalar(xbc) and xbc == 0: - return np.asarray(dof_sol)[dof_cd], dof_cd - return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + return _resolve_rigid_dofs(problem, self.var_cd, self.node_cd, 6) def _compute_rotation(self, rotvec): """Return the orientation and exact derivatives for the active mode.""" @@ -193,23 +217,25 @@ def _compute_rotation(self, rotvec): def _compute_slave_disp(self, problem, disp_ref, R): """Update slave-node displacements for a given rigid motion.""" - list_nodes = self.list_nodes - mesh = problem.mesh - new_disp = ( - (mesh.nodes[list_nodes] - self.center) @ R.T - + self.center - + disp_ref - - mesh.nodes[list_nodes] + return _apply_slave_disp( + problem, self.list_nodes, self.center, disp_ref, R, self._disp_indices ) - if not np.array_equal(problem._dU, 0): - if np.array_equal(problem._U, 0): - problem._dU[self._disp_indices] = new_disp - else: - problem._dU[self._disp_indices] = ( - new_disp - problem._U[self._disp_indices] - ) - return new_disp + def rotation_jacobian(self, rotvec, points): + """Rotation matrix and per-point displacement/rotation derivatives. + + For each point, ``du_dr{k} = (point - center) @ (dR/dr{k}).T`` is the + sensitivity of the slaved displacement to rigid rotation DOF ``k``. + Shared by the MPC linearization (:meth:`generate`) and the IPC contact + Jacobians (``RigidBodyAssembly`` and ``IPCContact``) so the three stay + consistent. + + Returns ``(R, du_drx, du_dry, du_drz)`` where each derivative array has + the same shape as ``points``. + """ + R, dR_drx, dR_dry, dR_drz = self._compute_rotation(rotvec) + rel = np.asarray(points, dtype=float) - self.center + return R, rel @ dR_drx.T, rel @ dR_dry.T, rel @ dR_drz.T def pre_update(self, problem): """Refresh rigid slave positions before contact assemblies update.""" @@ -252,16 +278,13 @@ def generate(self, problem, t_fact=1, t_fact_old=None): disp_ref = dof_ref[:3] # reference displacement rotvec = dof_ref[3:] - R, dR_drx, dR_dry, dR_drz = self._compute_rotation(rotvec) + # approche incrémentale + R, du_drx, du_dry, du_drz = self.rotation_jacobian( + rotvec, mesh.nodes[list_nodes] + ) self._compute_slave_disp(problem, disp_ref, R) - # approche incrémentale: - 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 - #### MPC #### # dU - dU_ref - du_drx*drx_ref - du_dry*dry_ref - du_drz*drz_ref = 0 @@ -393,22 +416,7 @@ def initialize(self, problem): def _get_dof_ref(self, problem): """Return [dx, dy, rotZ] and their global DOF indices.""" - dof_cd = [ - problem.n_node_dof - + problem._global_dof.indice_start(self.var_cd[i]) - + self.node_cd[i] - for i in range(3) - ] - dof_sol = problem.get_dof_solution() - xbc = problem._Xbc - - if np.isscalar(dof_sol) and dof_sol == 0: - if np.isscalar(xbc) and xbc == 0: - return np.zeros(3), dof_cd - return np.asarray(xbc)[dof_cd], dof_cd - if np.isscalar(xbc) and xbc == 0: - return np.asarray(dof_sol)[dof_cd], dof_cd - return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + return _resolve_rigid_dofs(problem, self.var_cd, self.node_cd, 3) @staticmethod def _compute_rotation(angle): @@ -421,16 +429,14 @@ def _compute_rotation(angle): def _compute_slave_disp(self, problem, disp_ref, rotation): """Update 2D slave-node displacements for a rigid motion.""" - nodes = problem.mesh.nodes[self.list_nodes] - new_disp = (nodes - self.center) @ rotation.T + self.center + disp_ref - nodes - if not np.array_equal(problem._dU, 0): - if np.array_equal(problem._U, 0): - problem._dU[self._disp_indices] = new_disp - else: - problem._dU[self._disp_indices] = ( - new_disp - problem._U[self._disp_indices] - ) - return new_disp + return _apply_slave_disp( + problem, + self.list_nodes, + self.center, + disp_ref, + rotation, + self._disp_indices, + ) def pre_update(self, problem): """Refresh rigid slave positions before contact assemblies update.""" diff --git a/fedoo/core/mesh.py b/fedoo/core/mesh.py index 52dd2384..4782a7c2 100644 --- a/fedoo/core/mesh.py +++ b/fedoo/core/mesh.py @@ -485,6 +485,10 @@ def find_coincident_nodes(self, tol: float = 1e-8) -> np.ndarray[int]: meshObject.merge_nodes(meshObject.find_coincident_nodes()) where meshObject is the Mesh object containing merged coincidentNodes. + + Detection here uses a round-to-tolerance lexsort. For a distance-based + (KDTree) detector that also collapses clusters of 3+ coincident nodes + in one call, see :py:meth:`merge_coincident_nodes`. """ decimal_round = int(-np.log10(tol) - 1) crd = self.nodes.round(decimal_round) # round coordinates to match tolerance @@ -528,6 +532,11 @@ def merge_coincident_nodes(self, tol: float) -> int: ------- int Total number of node pairs merged across all passes. + + See Also + -------- + find_coincident_nodes : lighter round-to-tolerance detector returning + pairs for a single-pass ``merge_nodes`` call. """ from scipy.spatial import cKDTree diff --git a/fedoo/time/common.py b/fedoo/time/common.py index 947ec163..a18ebe99 100644 --- a/fedoo/time/common.py +++ b/fedoo/time/common.py @@ -1,6 +1,24 @@ from dataclasses import dataclass import numpy as np +from scipy import sparse + + +def scatter_dense_block(block, dof_indices, shape): + """Scatter a dense ``(k, k)`` block onto a global sparse matrix. + + ``dof_indices`` (length ``k``) gives the global row/column position of each + local DOF: entry ``block[i, j]`` lands at + ``(dof_indices[i], dof_indices[j])``. Returns a + :class:`scipy.sparse.csr_matrix` of the requested ``shape``. + """ + idx = np.asarray(dof_indices, dtype=int) + k = len(idx) + rows = np.repeat(idx, k) + cols = np.tile(idx, k) + return sparse.csr_matrix( + (np.asarray(block, dtype=float).ravel(), (rows, cols)), shape=shape + ) @dataclass(frozen=True) diff --git a/fedoo/time/generalized_alpha.py b/fedoo/time/generalized_alpha.py index 9b0115a6..6f986e48 100644 --- a/fedoo/time/generalized_alpha.py +++ b/fedoo/time/generalized_alpha.py @@ -6,7 +6,11 @@ from fedoo.core.time_evolution import SECOND_ORDER from fedoo.core.weakform import WeakFormBase, WeakFormSum from fedoo.time.base import TimeIntegratorBase -from fedoo.time.common import RayleighDamping, newmark_acceleration_velocity +from fedoo.time.common import ( + RayleighDamping, + newmark_acceleration_velocity, + scatter_dense_block, +) from fedoo.weakform.inertia import Inertia @@ -169,11 +173,8 @@ def integrate(self, assembly, pb, compute="all"): (1.0 - self.alpha_f) * c0, ) tangent += (1.0 - self.alpha_f) * c0 * damping - rows = np.repeat(idx, len(idx)) - cols = np.tile(idx, len(idx)) - dynamic_matrix = sparse.csr_matrix( - (tangent.ravel(), (rows, cols)), - shape=assembly.global_matrix.shape, + dynamic_matrix = scatter_dense_block( + tangent, idx, assembly.global_matrix.shape ) assembly.global_matrix = assembly.global_matrix + dynamic_matrix From 40818d1f4170477f325e26222664afcbcdcf2ee3 Mon Sep 17 00:00:00 2001 From: Yves Chemisky Date: Fri, 17 Jul 2026 22:03:46 +0200 Subject: [PATCH 21/21] Move scatter_dense_block; tidy imports & ipc fix Move the scatter_dense_block utility from fedoo/time/common.py to fedoo/core/_sparsematrix.py and update consumers to import it from the core module (generalized_alpha.py, rigid_body.py). Remove the duplicate implementation and the scipy.sparse import from time/common. Also: import ipctk directly in ipc_contact.py to avoid re-running the version check on the contact hot path, update a doc comment in rigid_body.py to reference RigidTie.rotation_jacobian(), and simplify the static_regularisation initialization by removing the mass scaling calculation. --- fedoo/constraint/ipc_contact.py | 5 ++++- fedoo/constraint/rigid_body.py | 11 +++++------ fedoo/core/_sparsematrix.py | 17 +++++++++++++++++ fedoo/time/common.py | 18 ------------------ fedoo/time/generalized_alpha.py | 7 ++----- 5 files changed, 28 insertions(+), 30 deletions(-) diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index 19dbc962..caabe07a 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -40,7 +40,10 @@ def _barrier_hessian_psd(barrier, collisions, collision_mesh, vertices): pairs; retry without projection so assembly still proceeds. Shared by :class:`IPCContact` and ``RigidBodyAssembly`` so both use the same policy. """ - ipctk = _import_ipctk() + # ipctk is already validated/imported by the callers; a plain cached import + # avoids re-running the version check on the contact hot path. + import ipctk + try: return barrier.hessian( collisions, diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py index cec31b93..2d0520e9 100644 --- a/fedoo/constraint/rigid_body.py +++ b/fedoo/constraint/rigid_body.py @@ -15,7 +15,7 @@ not rely on a small-angle approximation. The contact Jacobian ``J = [I_3 | dR/d(angle) @ r_ref]`` uses the exact -rotation derivatives from ``RigidTie._compute_rotation()``, not the +rotation derivatives from ``RigidTie.rotation_jacobian()``, not the infinitesimal skew-symmetric approximation. """ @@ -23,9 +23,10 @@ from fedoo.core.base import AssemblyBase from fedoo.core.mesh import Mesh +from fedoo.core._sparsematrix import scatter_dense_block from fedoo.core.time_evolution import SECOND_ORDER from fedoo.constraint.rigid_tie import RigidTie -from fedoo.time.common import RayleighDamping, scatter_dense_block +from fedoo.time.common import RayleighDamping # Rigid-body dynamics is formulated in 3D: 3 translational + 3 rotational DOFs. _N_RIGID_DOF = 6 @@ -95,10 +96,8 @@ def __init__( self.dynamic = bool(dynamic) # A compiled time integrator supplies a mass tangent. Without one, the # assembly is static and this tiny diagonal keeps the free rigid DOFs - # well posed before contact. It is scaled to the body's mass/inertia - # magnitude so it stays negligible yet non-zero under any unit system. - mass_scale = max(self.mass, float(np.trace(self.inertia_body)), 1.0) - self.static_regularisation = 1e-9 * mass_scale + # well posed before contact. + self.static_regularisation = 1e-9 self._dof_indices = None self._pb_ref = None diff --git a/fedoo/core/_sparsematrix.py b/fedoo/core/_sparsematrix.py index ea4a4b30..86cc0138 100644 --- a/fedoo/core/_sparsematrix.py +++ b/fedoo/core/_sparsematrix.py @@ -541,3 +541,20 @@ def RowBlocMatrix(listBloc, nb_bloc, position, coef, n_cols=None): # return sum([bloc_matrix(listBloc[ii], (1,nb_bloc), (0,position[ii])) if coef[ii] == 1 \ # else coef[ii]*bloc_matrix(listBloc[ii], (1,nb_bloc), (0,position[ii])) for ii in range(len(listBloc))]) # + + +def scatter_dense_block(block, dof_indices, shape): + """Scatter a dense ``(k, k)`` block onto a global sparse matrix. + + ``dof_indices`` (length ``k``) gives the global row/column position of each + local DOF: entry ``block[i, j]`` lands at + ``(dof_indices[i], dof_indices[j])``. Returns a + :class:`scipy.sparse.csr_matrix` of the requested ``shape``. + """ + idx = np.asarray(dof_indices, dtype=int) + k = len(idx) + rows = np.repeat(idx, k) + cols = np.tile(idx, k) + return sparse.csr_matrix( + (np.asarray(block, dtype=float).ravel(), (rows, cols)), shape=shape + ) diff --git a/fedoo/time/common.py b/fedoo/time/common.py index a18ebe99..947ec163 100644 --- a/fedoo/time/common.py +++ b/fedoo/time/common.py @@ -1,24 +1,6 @@ from dataclasses import dataclass import numpy as np -from scipy import sparse - - -def scatter_dense_block(block, dof_indices, shape): - """Scatter a dense ``(k, k)`` block onto a global sparse matrix. - - ``dof_indices`` (length ``k``) gives the global row/column position of each - local DOF: entry ``block[i, j]`` lands at - ``(dof_indices[i], dof_indices[j])``. Returns a - :class:`scipy.sparse.csr_matrix` of the requested ``shape``. - """ - idx = np.asarray(dof_indices, dtype=int) - k = len(idx) - rows = np.repeat(idx, k) - cols = np.tile(idx, k) - return sparse.csr_matrix( - (np.asarray(block, dtype=float).ravel(), (rows, cols)), shape=shape - ) @dataclass(frozen=True) diff --git a/fedoo/time/generalized_alpha.py b/fedoo/time/generalized_alpha.py index 6f986e48..3a2a1d47 100644 --- a/fedoo/time/generalized_alpha.py +++ b/fedoo/time/generalized_alpha.py @@ -3,14 +3,11 @@ import numpy as np from scipy import sparse +from fedoo.core._sparsematrix import scatter_dense_block from fedoo.core.time_evolution import SECOND_ORDER from fedoo.core.weakform import WeakFormBase, WeakFormSum from fedoo.time.base import TimeIntegratorBase -from fedoo.time.common import ( - RayleighDamping, - newmark_acceleration_velocity, - scatter_dense_block, -) +from fedoo.time.common import RayleighDamping, newmark_acceleration_velocity from fedoo.weakform.inertia import Inertia