Coverage for src/sdynpy/signal_processing/sdynpy_massprops.py: 4%
69 statements
« prev ^ index » next coverage.py v7.16.1, created at 2026-09-18 21:32 +0000
1# -*- coding: utf-8 -*-
2"""
3Functions for working with mass properties
4"""
5"""
6Copyright 2022 National Technology & Engineering Solutions of Sandia,
7LLC (NTESS). Under the terms of Contract DE-NA0003525 with NTESS, the U.S.
8Government retains certain rights in this software.
10This program is free software: you can redistribute it and/or modify
11it under the terms of the GNU General Public License as published by
12the Free Software Foundation, either version 3 of the License, or
13(at your option) any later version.
15This program is distributed in the hope that it will be useful,
16but WITHOUT ANY WARRANTY; without even the implied warranty of
17MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18GNU General Public License for more details.
20You should have received a copy of the GNU General Public License
21along with this program. If not, see <https://www.gnu.org/licenses/>.
22"""
24import numpy as np
27def rigid_body_shape_scaling(
28 rbm_modes,
29 trans_shapes,
30 rot_shapes,
31 center_of_rotation=None,
32 return_mass_properties=False,
33 rcond=None,
34 symmetrize=True,
35 check_rank=True,
36):
37 """
38 Recover scaled/mass-normalized rigid-body shapes and optionally mass properties
39 from a set of mass-normalized rigid-body modes and user-supplied pure rigid-body
40 translation/rotation shapes.
42 Parameters
43 ----------
44 rbm_modes : (n, 6) array_like
45 Mass-normalized rigid-body mode shapes. Columns span the 6 rigid-body modes.
46 Assumed to satisfy Phi.T @ M @ Phi = I in the underlying mass metric.
47 trans_shapes : (n, 3) array_like
48 Unscaled pure translation shapes corresponding to unit translations in x, y, z
49 (or equivalent unit physical translations in the chosen measurement basis).
50 rot_shapes : (n, 3) array_like
51 Unscaled pure rotation shapes corresponding to unit rotations about x, y, z
52 (1 radian), defined about some reference point.
53 center_of_rotation : (3,) array_like, optional
54 Reference point about which rot_shapes were constructed. If provided, the
55 center of mass is also returned in global coordinates. If omitted, COM is
56 returned only relative to the rotation reference point.
57 return_mass_properties : bool, optional
58 If True, also return recovered mass properties.
59 rcond : float, optional
60 Cutoff for pseudoinverse.
61 symmetrize : bool, optional
62 If True, symmetrize the recovered rigid-body mass matrix and inertia tensors.
63 check_rank : bool, optional
64 If True, check that the supplied rigid-body basis has rank 6.
66 Returns
67 -------
68 scaled_shapes : (n, 6) ndarray
69 Mass-normalized/scaled physical rigid-body shapes corresponding as closely
70 as possible to the supplied pure translation/rotation basis.
71 Columns correspond to [Tx, Ty, Tz, Rx, Ry, Rz].
72 mass_props : dict, optional
73 Returned only if return_mass_properties=True. Contains:
74 - 'A' : (6, 6) transformation matrix such that B ≈ Phi @ A
75 - 'G' : (6, 6) rigid-body generalized mass matrix in basis B
76 - 'mass' : scalar total mass
77 - 'com_relative' : (3,) center of mass relative to rotation reference point
78 - 'com_global' : (3,) center of mass in global coordinates (if center_of_rotation provided)
79 - 'inertia_about_reference' : (3, 3) inertia tensor about the rotation reference point
80 - 'inertia_about_com' : (3, 3) inertia tensor about center of mass
81 - 'principal_inertias' : (3,) eigenvalues of inertia_about_com
82 - 'principal_axes' : (3, 3) eigenvectors of inertia_about_com
83 - 'reconstruction_error' : relative Frobenius norm of B - Phi @ A
84 - 'G_tt', 'G_tr', 'G_rt', 'G_rr' : 3x3 blocks of G
86 Notes
87 -----
88 Let Phi = rbm_modes and B = [trans_shapes rot_shapes].
89 Since both span the rigid-body subspace, B = Phi A for some A.
90 Using mass normalization Phi.T M Phi = I, the rigid-body generalized mass matrix is
92 G = B.T M B = A.T A
94 The mass properties are extracted from:
96 G = [[ m I, -m [c]_x ],
97 [ m [c]_x, J_ref ]]
99 where c is COM relative to the reference point used to define the rotation shapes,
100 and J_ref is the inertia tensor about that same reference point.
102 Sign convention for c assumes the rotation shapes follow:
103 u = omega x (x - O)
104 """
105 Phi = np.asarray(rbm_modes, dtype=float)
106 T = np.asarray(trans_shapes, dtype=float)
107 R = np.asarray(rot_shapes, dtype=float)
109 if Phi.ndim != 2 or Phi.shape[1] != 6:
110 raise ValueError("rbm_modes must have shape (n, 6)")
111 if T.ndim != 2 or T.shape[1] != 3:
112 raise ValueError("trans_shapes must have shape (n, 3)")
113 if R.ndim != 2 or R.shape[1] != 3:
114 raise ValueError("rot_shapes must have shape (n, 3)")
115 if T.shape[0] != Phi.shape[0] or R.shape[0] != Phi.shape[0]:
116 raise ValueError("rbm_modes, trans_shapes, and rot_shapes must have the same number of rows")
118 B = np.hstack((T, R))
120 if check_rank:
121 rank_phi = np.linalg.matrix_rank(Phi)
122 rank_B = np.linalg.matrix_rank(B)
123 if rank_phi < 6:
124 raise ValueError(f"rbm_modes has rank {rank_phi}, expected 6")
125 if rank_B < 6:
126 raise ValueError(f"[trans_shapes rot_shapes] has rank {rank_B}, expected 6")
128 # Solve B ≈ Phi @ A
129 Phi_pinv = np.linalg.pinv(Phi, rcond=rcond)
130 A = Phi_pinv @ B
132 # "Scaled" physical rigid-body shapes in the mass-normalized basis:
133 # columns are the unit physical RB shapes normalized by their generalized masses.
134 #
135 # Since G = A.T @ A, one valid whitening transform is inv(cholesky(G)).
136 # Then B_scaled = B @ inv(cholesky(G)) gives shapes whose mass metric is identity.
137 G = A.T @ A
138 if symmetrize:
139 G = 0.5 * (G + G.T)
141 # Cholesky can fail if numerically near-singular; use eig fallback
142 try:
143 L = np.linalg.cholesky(G)
144 G_inv_sqrt = np.linalg.inv(L).T # because G = L L^T, so G^{-1/2} can be taken as L^{-T}
145 except np.linalg.LinAlgError:
146 eigvals, eigvecs = np.linalg.eigh(G)
147 tol = np.max(eigvals) * 1e-12 if np.max(eigvals) > 0 else 1e-12
148 if np.any(eigvals <= tol):
149 raise ValueError("Recovered generalized mass matrix is not positive definite enough to invert.")
150 G_inv_sqrt = eigvecs @ np.diag(1.0 / np.sqrt(eigvals)) @ eigvecs.T
152 scaled_shapes = B @ G_inv_sqrt
154 if not return_mass_properties:
155 return scaled_shapes
157 # Diagnostics
158 recon = Phi @ A
159 reconstruction_error = np.linalg.norm(B - recon, ord="fro") / max(np.linalg.norm(B, ord="fro"), 1e-16)
161 # Partition G
162 G_tt = G[:3, :3]
163 G_tr = G[:3, 3:]
164 G_rt = G[3:, :3]
165 G_rr = G[3:, 3:]
167 if symmetrize:
168 G_tt = 0.5 * (G_tt + G_tt.T)
169 G_rr = 0.5 * (G_rr + G_rr.T)
170 # Ideally G_rt = G_tr.T, but keep both in case diagnostics are useful
172 # Total mass
173 mass = np.trace(G_tt) / 3.0
175 # Recover COM relative to reference point from G_tr = -m [c]_x
176 # For [c]_x = [[0, -cz, cy], [cz, 0, -cx], [-cy, cx, 0]]
177 #
178 # Then:
179 # G_tr = -m[c]_x = [[0, m*cz, -m*cy],
180 # [-m*cz, 0, m*cx],
181 # [m*cy, -m*cx, 0]]
182 #
183 # Use antisymmetric average for robustness.
184 G_tr_skew = 0.5 * (G_tr - G_rt.T)
186 cx = G_tr_skew[1, 2] / mass
187 cy = G_tr_skew[2, 0] / mass
188 cz = G_tr_skew[0, 1] / mass
189 com_relative = np.array([cx, cy, cz])
191 inertia_about_reference = G_rr.copy()
192 if symmetrize:
193 inertia_about_reference = 0.5 * (inertia_about_reference + inertia_about_reference.T)
195 # Parallel-axis theorem: J_com = J_ref - m (||c||^2 I - c c^T)
196 c = com_relative
197 c2 = np.dot(c, c)
198 inertia_about_com = inertia_about_reference - mass * (c2 * np.eye(3) - np.outer(c, c))
199 if symmetrize:
200 inertia_about_com = 0.5 * (inertia_about_com + inertia_about_com.T)
202 principal_inertias, principal_axes = np.linalg.eigh(inertia_about_com)
204 mass_props = {
205 "A": A,
206 "G": G,
207 "mass": mass,
208 "com_relative": com_relative,
209 "inertia_about_reference": inertia_about_reference,
210 "inertia_about_com": inertia_about_com,
211 "principal_inertias": principal_inertias,
212 "principal_axes": principal_axes,
213 "reconstruction_error": reconstruction_error,
214 "G_tt": G_tt,
215 "G_tr": G_tr,
216 "G_rt": G_rt,
217 "G_rr": G_rr,
218 }
220 if center_of_rotation is not None:
221 O = np.asarray(center_of_rotation, dtype=float).reshape(3)
222 mass_props["center_of_rotation"] = O
223 mass_props["com_global"] = O + com_relative
225 return scaled_shapes, mass_props