import numpy as np
import rigeo.util as util
from rigeo.random import random_psd_matrix
[docs]def H2I(H):
"""Convert second moment matrix to inertia matrix."""
assert H.shape == (3, 3)
return np.trace(H) * np.eye(3) - H
[docs]def I2H(I):
"""Convert inertia matrix to second moment matrix."""
assert I.shape == (3, 3)
return 0.5 * np.trace(I) * np.eye(3) - I
[docs]class InertialParameters:
"""Inertial parameters of a rigid body.
Parameters
----------
mass : float
Mass of the body.
h : np.ndarry, shape (3,)
First moment of mass.
H : np.ndarray, shape (3, 3)
Second moment matrix.
"""
def __init__(self, mass, h=None, com=None, H=None, I=None, translate_from_com=False):
assert mass >= 0, f"Mass must be non-negative but is {mass}."
self.mass = mass
if h is not None and com is not None:
raise ValueError("Cannot specify both h and com.")
if com is None:
if h is None:
com = np.zeros(3)
else:
com = h / mass
self.com = com
if H is not None and I is not None:
raise ValueError("Cannot specify both H and I.")
if H is None:
if I is None:
if translate_from_com:
H = np.zeros((3, 3))
else:
H = mass * np.outer(com, com)
else:
H = I2H(I)
# translate the second moment matrix from CoM reference point to the
# origin
if translate_from_com:
H = H + mass * np.outer(com, com)
self.H = H
def __repr__(self):
return f"InertialParameters(mass={self.mass}, com={self.com}, H={self.H})"
[docs] @classmethod
def zero(cls):
return cls(mass=0, com=np.zeros(3), H=np.zeros((3, 3)))
@property
def h(self):
return self.mass * self.com
@property
def vec(self):
"""Inertial parameter vector."""
return np.concatenate([[self.mass], self.h, util.vech(self.I)])
@property
def I(self):
"""Inertia matrix."""
return H2I(self.H)
@property
def Hc(self):
"""H matrix about the CoM."""
return self.H - np.outer(self.h, self.h) / self.mass
@property
def Ic(self):
"""Inertia matrix about the CoM."""
return H2I(self.Hc)
@property
def J(self):
"""Pseudo-inertia matrix."""
J = np.zeros((4, 4))
J[:3, :3] = self.H
J[:3, 3] = self.h
J[3, :3] = self.h
J[3, 3] = self.mass
return J
@property
def M(self):
"""Spatial mass matrix."""
S = util.skew3(self.h)
return np.block([[self.mass * np.eye(3), -S], [S, self.I]])
[docs] @classmethod
def from_vec(cls, vec):
"""Construct from a parameter vector."""
assert vec.shape == (10,)
mass = vec[0]
h = vec[1:4]
# fmt: off
I = np.array([
[vec[4], vec[5], vec[6]],
[vec[5], vec[7], vec[8]],
[vec[6], vec[8], vec[9]]
])
# fmt: on
return cls(mass=mass, h=h, I=I)
[docs] @classmethod
def from_pim(cls, J):
"""Construct from a pseudo-inertia matrix.
The pseudo-inertia matrix is
.. math::
\\boldsymbol{J} = \\begin{bmatrix}
\\boldsymbol{H} & m\\boldsymbol{c} \\\\
m\\boldsymbol{c}^T & m
\\end{bmatrix}
Parameters
----------
J : np.ndarray (4, 4)
The pseudo-inertia matrix.
"""
assert J.shape == (4, 4)
H = J[:3, :3]
h = J[3, :3]
mass = J[3, 3]
return cls(mass=mass, h=h, H=H)
[docs] @classmethod
def from_point_masses(cls, masses, points):
"""Construct from a system of point masses.
Parameters
----------
masses : np.ndarray, shape (n,)
The masses at each point.
points : np.ndarray, shape (n, 3)
The locations of each mass.
"""
masses = np.array(masses)
points = np.array(points)
assert masses.shape[0] == points.shape[0]
# homogeneous representation
P = np.hstack((points, np.ones((points.shape[0], 1))))
J = sum([m * np.outer(p, p) for m, p in zip(masses, P)])
return cls.from_pim(J)
[docs] @classmethod
def random(cls):
"""Generate a random set of physically consistent inertial parameters.
Useful for testing purposes.
"""
mass = 0.1 + np.random.random() * 0.9
com = np.random.random(3) - 0.5
H = random_psd_matrix((3, 3)) + mass * np.outer(com, com)
return cls(mass=mass, com=com, H=H)
[docs] def is_same(self, other):
"""Check if this set of inertial parameters is the same as another.
Parameters
----------
other : InertialParameters
The other set of inertial parameters to check.
Returns
-------
: bool
``True`` if they are the same, ``False`` otherwise.
"""
return np.allclose(self.J, other.J)
[docs] def consistent(self, tol=0):
"""Check if the inertial parameters are fully physically consistent.
This means that there exists a physical rigid body that can have these
parameters; a necessary and sufficient condition is that the
pseudo-inertia matrix is positive semidefinite.
Parameters
----------
tol : float, non-negative
The parameters will be considered consistent if all of the
eigenvalues of the pseudo-inertia matrix are greater than or equal
to ``-tol``.
Returns
-------
: bool
``True`` if the parameters are consistent, ``False`` otherwise.
"""
assert tol >= 0, "Numerical tolerance cannot be negative."
return np.min(np.linalg.eigvals(self.J)) >= -tol
def __add__(self, other):
return InertialParameters(
mass=self.mass + other.mass, h=self.h + other.h, H=self.H + other.H
)
def __radd__(self, other):
if other == 0:
return self
return self.__add__(other)
[docs] def body_wrench(self, V, A):
"""Compute the body-frame wrench about the reference point."""
M = self.M
return M @ A + util.skew6(V) @ M @ V