maspack.spatialmotion

## Class SpatialInertia

• All Implemented Interfaces:
java.io.Serializable, java.lang.Cloneable, DenseMatrix, LinearTransformNd, Matrix, MatrixBlock, VectorObject<Matrix6d>, Clonable, Scannable

```public class SpatialInertia
extends Matrix6dBlock
implements java.io.Serializable```
Spatial inertia used to compute the dynamics of a rigid body. The spatial inertia M is a 6 x 6 matrix which relates the spatial acceleration of a rigid body to its applied spatial force, according to
``` [ f ]   [  m I     - m [c]    ] [ dv ]
[   ] = [                     ] [    ]
[ t ]   [ m [c]  J - m [c][c] ] [ dw ]
```
The left-hand vector is a wrench describing the spatial force, composed of a translational force f and a moment m. The right-hand vector is a twist describing the spatial acceleration, composed of a translational acceleration dv and an angular acceleration dw. The components of the inertia matrix itself are the body mass m, the center of mass position c, and the rotational inertia J with respect to the center of mass. [c] denotes the 3 x 3 cross-product matrix formed from the components of c and I is the identity matrix.

An inertia is easy to invert given the inverse of J:

```
[               -1            -1  ]
-1   [  1/m I - [c] J  [c]    [c] J    ]
M   = [                                 ]
[         -1                 -1   ]
[       -J  [c]             J     ]
```
If the inertia's coordinate frame is located at the center of mass, then c = 0 and the M and it's inverse are both block diagonal.

This class will typically be used to compute the acceleration of a rigid body in response to applied forces. In doing this, it is generally necessary to consider the coriolis forces which arise as a result of the body's rotation. Coriolis forces can be computed using either `coriolisForce` or `bodyCoriolisForce`, depending on whether the body's velocity is stored in a fixed frame or in one which moves with the body.

### Fixed-frame usage example

In this situation, the body's velocity is stored in some fixed "world" frame, which we transform into body coordinates for purposes of computing the acceleration. We use this acceleration to update the velocity and acceleration using a simple symplectic Euler integration step.
``` SpatialInertia M; // spatial inertia for the body
RigidTransform Xbw; // transform from body to world coordinates
Twist vw; // body velocity in world coordinates
Twist vb; // body velocity in body coordinates
Wrench fx; // external force (world coordinates)
Wrench fc; // coriolis force (body coordinates)
Wrench fb; // total applied force in body coordinates
Twist acc; // spatial accelertion (body coodinates)
double h; // time step for integrator

vb.inverseTransform (Xbw.R, vw); // rotate vw to body coordinates
fb.inverseTransform (Xbw.R, fx); // rotate fx to body coordinates
M.coriolisForce (fc, vb); // compute fixed-frame coriolis force
M.mulInverse (acc, fb); // solve fb = M acc
// integrate using symplectic Euler step
vw.transform (Xbw.R, vb); // rotate vb back to world coordinates
vb.extrapolateTranform (Xbw, h); // use vb to update Xbw
```

### Body-frame usage example

In this situation, the body's velocity is stored with respect to the body frame, and hence is assumed to be moving with it. This means that coriolis forces are computed differently:
``` SpatialInertia M; // spatial inertia for the body
RigidTransform Xbw; // transform from body to world coordinates
Twist vb; // body velocity in body coordinates
Wrench fx; // external force (world coordinates)
Wrench fc; // coriolis force (body coordinates)
Wrench fb; // total applied force in body coordinates
Twist acc; // spatial accelertion (body coodinates)
double h; // time step for integrator

fb.inverseTransform (Xbw.R, fx); // rotate fx to body coordinates
M.bodyCoriolisForce (fc, vb); // compute body coriolis force
M.mulInverse (acc, fb); // solve fb = M acc
// integrate using symplectic Euler step
vb.extrapolateTranform (Xbw, h); // use vb to update Xbw
```

The difference between this example and the previous one is quite subtle. In the first example, vb is first computed by rotating vw into body coordinates using the inverse of Xbw.R. It is then updated using the body acceleration, and then rotated back into vw using Xbw.R. After that happens, Xbw is then itself updated using vb. Now, at the beginning of the next step, when vb is recomputed from vw, we will be using a different (updated) value of Xbw.R, and so vb will be different from the value of vb that was computed at the end of the previous step. In otherwords, vb depends on Xbw.R, and so changes in Xbw.R cause vb to vary, even if vw is constant. In the body-frame example, we don't transform between vb and vw, but the variation still exists, and is instead accounted for varying the coriolis forces.

Serialized Form

• ### Nested classes/interfaces inherited from interface maspack.matrix.Matrix

`Matrix.Partition, Matrix.WriteFormat`
• ### Field Summary

Fields
Modifier and Type Field and Description
`static int` `MASS_INERTIA_STRING`
Specifies a string representation of this spatial inertia consisting of a mass, a center of mass point, and a 3 x 3 rotational inertia matrix with respect to the center of mass.
`static int` `MATRIX_STRING`
Specifies a string representation of this spatial inertia as a 6 x 6 matrix.
`static SpatialInertia` `UNIT`
`static SpatialInertia` `ZERO`
• ### Fields inherited from class maspack.matrix.Matrix6dBase

`m00, m01, m02, m03, m04, m05, m10, m11, m12, m13, m14, m15, m20, m21, m22, m23, m24, m25, m30, m31, m32, m33, m34, m35, m40, m41, m42, m43, m44, m45, m50, m51, m52, m53, m54, m55`
• ### Fields inherited from interface maspack.matrix.Matrix

`INDEFINITE, POSITIVE_DEFINITE, SPD, SYMMETRIC`
• ### Constructor Summary

Constructors
Constructor and Description
`SpatialInertia()`
Creates a new spatial inertia with a value of zero.
```SpatialInertia(double m, double Jxx, double Jyy, double Jzz)```
Creates a new spatial inertia with a specific mass and a diagonal rotational inertia with the indicated element values.
`SpatialInertia(SpatialInertia inertia)`
Creates a new spatial inertia with the same values as an existing one.
• ### Method Summary

All Methods
Modifier and Type Method and Description
`void` `add(SpatialInertia M1)`
Adds this spatial inertia to M1 and places the result in this spatial inertia.
`void` ```add(SpatialInertia M1, SpatialInertia M2)```
Adds spatial inertia M1 to M2 and places the result in this spatial inertia.
`void` ```addLineSegmentInertia(double m, Vector3d p0, Vector3d p1)```
Adds the inertia of a line segment to this inertia.
`void` ```addPointMass(double m, Vector3d pos)```
Adds a point mass at a specified point to this inertia.
`static void` ```addPointMass(Matrix M, double m, Vector3d pos)```
Adds a point mass at a specified point to a 6x6 matrix representing a spatial inertia.
`static void` ```addPointRotationalInertia(Matrix3dBase J, double m, Vector3d r)```
Add to a rotational inertia the effect of a point mass at location r.
`void` ```addTriangleInertia(double m, Vector3d p0, Vector3d p1, Vector3d p2)```
Adds the inertia of a triangle to this inertia.
`void` ```bodyCoriolisForce(Wrench wr, Twist tw)```
Computes the coriolis forces induced by a given body-coordinate velocity acting on this spatial inertia.
`void` ```coriolisForce(Wrench wr, Twist tw)```
Computes the coriolis forces induced by a given fixed-frame velocity acting on this spatial inertia.
`static SpatialInertia` ```createBoxInertia(double m, double wx, double wy, double wz)```
Creates a new spatial inertia corresponding to the center of an axis-aligned box of uniform density.
`static SpatialInertia` ```createCylinderInertia(double m, double r, double l)```
Creates a new spatial inertia corresponding to the center of a cylinder of uniform density aligned with the z axis.
`static SpatialInertia` ```createEllipsoidInertia(double m, double a, double b, double c)```
Creates a new spatial inertia corresponding to the center of an axis-aligned ellispoid of uniform density.
`static SpatialInertia` ```createSphereInertia(double m, double r)```
Creates a new spatial inertia corresponding to the center of a sphere of uniform density.
`double` `directedMass(Wrench wr)`
Computes the effective mass of this inertia along a specific twist direction.
`boolean` `equals(Matrix6dBase M1)`
Returns true if the elements of this matrix exactly equal those of matrix `M1`.
`boolean` `fullEquals(SpatialInertia M)`
For debugging: compares all the fields of M with the this inertia, including the component fields that depend on the matrix entries.
`double` ```get(int i, int j)```
Gets a single element of this matrix.
`Point3d` `getCenterOfMass()`
Gets the center of mass for this spatial inertia.
`void` `getCenterOfMass(Vector3d com)`
Gets the center of mass for this spatial inertia.
`void` `getInverse(MatrixNd MI)`
Computes the inverse of this spatial inertia matrix.
`double` `getMass()`
Gets the mass for this spatial inertia.
`SymmetricMatrix3d` `getOffsetRotationalInertia()`
Gets the rotational interia, offset by the center of mass, for this spatial inertia.
`void` ```getRotated(Matrix6d M, RotationMatrix3d R)```
Transforms this inertia into a different frame and returns the rotated value.
`SymmetricMatrix3d` `getRotationalInertia()`
Gets the rotational interia (with respect to the center of mass) for this spatial inertia.
`void` `getRotationalInertia(SymmetricMatrix3d J)`
Gets the rotational inertia (with respect to the center of mass) for this spatial inertia.
`void` `invalidateComponents()`
Invalidate the center of mass and inertia components of this spatial inertia.
`void` `inverseTransform(RigidTransform3d X)`
Transforms this inertia into a new coordinate frame, given an inverse spatial transformation matrix.
`void` `inverseTransform(RotationMatrix3d R)`
Rotates this inertia into a new coordinate frame, given by the inverse of a rotation matrix.
`static void` ```invert(Matrix6d MI, Matrix6d M)```
Computes the inverse of a spatial inertia, stored in M, and return the result in MI.
`void` ```mul(Wrench wrr, Twist tw1)```
Multiplies a twist by this spatial inertia and places the result in a wrench:
`void` ```mulInverse(SpatialVector svr, SpatialVector sv1)```
Multiplies a spatial vector by the inverse of this spatial inertia and places the result in another spatial vector.
`void` ```mulInverse(Twist twr, Wrench wr1)```
Multiplies a wrench by the inverse of this spatial inertia and places the result in a twist.
`void` ```mulLeftFactor(SpatialVector svr, SpatialVector sv1)```
Multiplies a spatial vector by the left Cholesky factor of this spatial inertia.
`void` ```mulLeftFactorInverse(SpatialVector svr, SpatialVector sv1)```
Multiplies a spatial vector by the inverse of the left Cholesky factor of this spatial inertia.
`void` ```mulRightFactor(SpatialVector svr, SpatialVector sv1)```
Multiplies a spatial vector by the right Cholesky factor of this spatial inertia.
`void` ```mulRightFactorInverse(SpatialVector svr, SpatialVector sv1)```
Multiplies a twist by the inverse of the right Cholesky factor of this spatial inertia.
`void` `scale(double s)`
Scales this spatial inertia by `s` in place.
`void` ```scale(double s, SpatialInertia M1)```
Scales the spatial inertia M1 by `s` and places the results in this spatial inertia.
`void` `scaleDistance(double s)`
Scale the distance units associated with this SpatialInertia.
`void` `scaleMass(double s)`
Scale the mass units associated with this SpatialInertia.
`void` `scan(ReaderTokenizer rtok)`
`void` `set(double[] vals)`
Sets the elements of this matrix from an array of doubles.
`void` ```set(double m, double Jxx, double Jyy, double Jzz)```
Sets this spatial inertia to have a specific mass and a diagonal rotational inertia with the indicated element values.
`void` ```set(double m, SymmetricMatrix3d J)```
Sets this spatial inertia to have a specific mass and rotational inertia.
`void` ```set(double m, SymmetricMatrix3d J, Point3d com)```
Sets this spatial inertia to have a specific mass, rotational inertia, and center of mass.
`void` ```set(int i, int j, double val)```
Sets a single element of this spatial inertia.
`void` `set(Matrix M)`
Sets the size and values of this matrix to those of another matrix.
`void` `set(Matrix6d M)`
Sets this spatial inertia to be identical to be identical to another one.
`void` `set(Matrix6dBase M)`
Sets this spatial inertia to be identical to be identical to another one.
`void` `set(SpatialInertia M)`
Sets this spatial inertia to be identical to be identical to another one.
`void` ```setBox(double m, double wx, double wy, double wz)```
Sets this spatial inertia to correspond to the center of an axis-aligned box of uniform density.
`void` ```setCenterOfMass(double x, double y, double z)```
Sets the center of mass for this spatial inertia.
`void` `setCenterOfMass(Point3d com)`
Sets the center of mass for this spatial inertia.
`void` ```setCylinder(double m, double r, double l)```
Sets this spatial inertia to correspond to the center of a cylinder of uniform density aligned with the z axis.
`void` ```setEllipsoid(double m, double a, double b, double c)```
Sets this spatial inertia to correspond to the center of an axis-aligned ellispoid of uniform density.
`void` `setMass(double m)`
Sets the mass for this spatial inertia.
`void` ```setPointMass(double m, Point3d com)```
Sets this spatial inertia to correspond to a point mass at a specific point.
`void` `setRandom()`
Sets the components of this spatial inertia to uniformly distributed random values in the range -0.5 (inclusive) to 0.5 (exclusive).
`void` ```setRandom(double lower, double upper)```
Sets the components of this spatial inertia to uniformly distributed random values in a specified range.
`void` ```setRandom(double lower, double upper, java.util.Random generator)```
Sets the components of this spatial inertia to uniformly distributed random values in a specified range, using a supplied random number generator.
`void` ```setRotationalInertia(double J00, double J11, double J22, double J01, double J02, double J12)```
Sets the rotational interia for this spatial inertia, given the diagonal and upper off-diagonal elements
`void` `setRotationalInertia(SymmetricMatrix3d J)`
Sets the rotational interia for this spatial inertia.
`void` ```setSphere(double m, double r)```
Sets this spatial inertia to correspond to the center of a sphere of uniform density.
`void` `setZero()`
Sets this spatial inertia to zero.
`void` `sub(SpatialInertia M1)`
Subtracts this spatial inertia from M1 and places the result in this spatial inertia.
`void` ```sub(SpatialInertia M1, SpatialInertia M2)```
Subtracts spatial inertia M1 from M2 and places the result in this spatial inertia.
`java.lang.String` `toString()`
Returns a string representation of this spatial inertia as a `MATRIX_STRING`.
`java.lang.String` ```toString(NumberFormat numberFmt, int outputCode)```
Returns a specified string representation of this spatial inertia, with each number formatted according to the a supplied numeric format.
`java.lang.String` `toString(java.lang.String numberFmtStr)`
Returns a string representation of this spatial inertia as a `MATRIX_STRING`, with each number formatted according to a supplied numeric format.
`java.lang.String` ```toString(java.lang.String numberFmtStr, int outputCode)```
Returns a specified string representation of this spatial inertia, with each number formatted according to the a supplied numeric format.
`java.lang.String` `toStringAll(java.lang.String fmt)`
For debugging: creates a string showing this inertia and all its fields
`void` `transform(RigidTransform3d X)`
Transforms this inertia into a new coordinate frame, given a spatial transformation matrix.
`void` `transform(RotationMatrix3d R)`
Rotates this inertia into a new coordinate frame given by a rotation matrix.
`void` `updateComponents()`
• ### Methods inherited from class maspack.matrix.Matrix6dBlock

`add, addNumNonZerosByCol, addNumNonZerosByRow, clone, createTranspose, down, getBlockCCSIndices, getBlockCCSValues, getBlockCol, getBlockCRSIndices, getBlockCRSValues, getBlockNumber, getBlockRow, mulAdd, mulTransposeAdd, next, scaledAdd, setBlockCol, setBlockNumber, setBlockRow, setDown, setNext, sub, valueIsNonZero`
• ### Methods inherited from class maspack.matrix.Matrix6d

`add, add, addObj, addSubMatrix00, addSubMatrix03, addSubMatrix30, addSubMatrix33, epsilonEquals, getSubMatrix00, getSubMatrix03, getSubMatrix30, getSubMatrix33, inverseTransform, invert, mul, mul, mulAdd, mulInverse, mulInverseBoth, mulInverseLeft, mulInverseRight, mulTranspose, mulTransposeBoth, mulTransposeLeft, mulTransposeLeftAdd, mulTransposeRight, mulTransposeRightAdd, negate, scale, scaledAddObj, scaleObj, setDiagonal, setDiagonal, setDiagonal, setLowerToUpper, setSubMatrix00, setSubMatrix03, setSubMatrix30, setSubMatrix33, sub, sub, transform, transpose`
• ### Methods inherited from class maspack.matrix.Matrix6dBase

`colSize, determinant, epsilonEquals, frobeniusNorm, get, get, getColumn, getColumn, getRow, getRow, infinityNorm, invert, mul, mul, mulInverse, mulInverse, mulInverseTranspose, mulInverseTranspose, mulTranspose, mulTranspose, negate, oneNorm, rowSize, scaledAdd, setColumn, setIdentity, setRow, transpose`
• ### Methods inherited from class maspack.matrix.DenseMatrixBase

`add, checkConsistency, set, set, set, setCCSValues, setColumn, setCRSValues, setRow, setSubMatrix`
• ### Methods inherited from class maspack.matrix.MatrixBase

`containsNaN, epsilonEquals, equals, frobeniusNormSquared, get, getCCSIndices, getCCSIndices, getCCSIndices, getCCSValues, getCCSValues, getCCSValues, getColumn, getCRSIndices, getCRSIndices, getCRSIndices, getCRSValues, getCRSValues, getCRSValues, getDefaultFormat, getRow, getSize, getSubMatrix, hasNaN, idString, isFixedSize, isSymmetric, isWritable, maxNorm, mul, mul, mulAdd, mulAdd, mulAdd, mulTranspose, mulTranspose, mulTransposeAdd, mulTransposeAdd, mulTransposeAdd, numNonZeroVals, numNonZeroVals, scan, setCRSValues, setDefaultFormat, setSize, toString, trace, write, write, write, write, write, write, write, writeToFile`
• ### Methods inherited from class java.lang.Object

`equals, getClass, hashCode, notify, notifyAll, wait, wait, wait`
• ### Methods inherited from interface maspack.matrix.MatrixBlock

`mulAdd, mulTransposeLeftAdd, mulTransposeRightAdd, negate`
• ### Methods inherited from interface maspack.matrix.DenseMatrix

`set, setColumn, setColumn, setRow, setRow, setSubMatrix`
• ### Methods inherited from interface maspack.matrix.Matrix

`checkConsistency, colSize, determinant, epsilonEquals, equals, frobeniusNorm, frobeniusNormSquared, get, getCCSIndices, getCCSIndices, getCCSIndices, getCCSValues, getCCSValues, getCCSValues, getColumn, getColumn, getColumn, getCRSIndices, getCRSIndices, getCRSIndices, getCRSValues, getCRSValues, getCRSValues, getRow, getRow, getRow, getSize, getSubMatrix, infinityNorm, isFixedSize, isSymmetric, maxNorm, mul, mul, mul, mulAdd, mulAdd, mulAdd, mulTranspose, mulTranspose, mulTranspose, mulTransposeAdd, mulTransposeAdd, mulTransposeAdd, numNonZeroVals, numNonZeroVals, oneNorm, rowSize, set, setCCSValues, setCRSValues, setSize, toString, trace, write, write, write`
• ### Methods inherited from interface maspack.util.Scannable

`isWritable, scan, write`
• ### Field Detail

• #### ZERO

`public static SpatialInertia ZERO`
• #### UNIT

`public static SpatialInertia UNIT`
• #### MASS_INERTIA_STRING

`public static final int MASS_INERTIA_STRING`
Specifies a string representation of this spatial inertia consisting of a mass, a center of mass point, and a 3 x 3 rotational inertia matrix with respect to the center of mass.
Constant Field Values
• #### MATRIX_STRING

`public static final int MATRIX_STRING`
Specifies a string representation of this spatial inertia as a 6 x 6 matrix.
Constant Field Values
• ### Constructor Detail

• #### SpatialInertia

`public SpatialInertia()`
Creates a new spatial inertia with a value of zero.
• #### SpatialInertia

`public SpatialInertia(SpatialInertia inertia)`
Creates a new spatial inertia with the same values as an existing one.
Parameters:
`inertia` - spatial inertia whose values are copied
• #### SpatialInertia

```public SpatialInertia(double m,
double Jxx,
double Jyy,
double Jzz)```
Creates a new spatial inertia with a specific mass and a diagonal rotational inertia with the indicated element values. The center of mass is set to zero.
Parameters:
`m` - mass
`Jxx` - rotational inertia about x
`Jyy` - rotational inertia about y
`Jzz` - rotational inertia about z
• ### Method Detail

• #### set

```public void set(int i,
int j,
double val)```
Sets a single element of this spatial inertia. The matrix structure is enforced, so that zero elements are unaffected and the necessary symmetries are preserved. The mass, center of mass, and rotational inertia elements are also updated.
Specified by:
`set` in interface `DenseMatrix`
Overrides:
`set` in class `Matrix6dBase`
Parameters:
`i` - element row index
`j` - element column index
`val` - element value
• #### set

`public void set(double[] vals)`
Description copied from class: `Matrix6dBase`
Sets the elements of this matrix from an array of doubles. The elements in the array should be stored using row-major order, so that element `(i,j)` is stored at location `i*colSize()+j`.
Specified by:
`set` in interface `DenseMatrix`
Overrides:
`set` in class `Matrix6dBase`
Parameters:
`vals` - array from which values are copied
• #### set

`public void set(Matrix M)`
Description copied from class: `Matrix6dBase`
Sets the size and values of this matrix to those of another matrix.
Specified by:
`set` in interface `Matrix`
Overrides:
`set` in class `Matrix6dBase`
Parameters:
`M` - matrix whose size and values are copied
• #### invalidateComponents

`public void invalidateComponents()`
Invalidate the center of mass and inertia components of this spatial inertia. Should be called after the entry fields (e.g., m00, m01, etc) are externally modified.
• #### get

```public double get(int i,
int j)```
Gets a single element of this matrix.
Specified by:
`get` in interface `Matrix`
Overrides:
`get` in class `Matrix6dBase`
Parameters:
`i` - element row index
`j` - element column index
Returns:
element value
• #### updateComponents

`public void updateComponents()`
• #### scan

```public void scan(ReaderTokenizer rtok)
throws java.io.IOException```
Reads the contents of this spatial inertia from a ReaderTokenizer. There are two allowed formats, each of which is delimited by square brackets.

The first format is a set of 13 numbers giving, in order, the mass, center of mass, and rotational inertia (in row-major order).

The second format format is a set of 36 numbers giving all elements of the matrix. In interpreting these numbers, matrix structure is preserved, so that zero elements remain zero and the necessary symmetries are maintained.

Specified by:
`scan` in interface `Matrix`
Overrides:
`scan` in class `MatrixBase`
Parameters:
`rtok` - ReaderTokenizer from which to read the inertia. Number parsing should be enabled.
Throws:
`java.io.IOException` - if an I/O error occured or if the inertia description is not consistent with one of the above formats.
• #### toString

`public java.lang.String toString()`
Returns a string representation of this spatial inertia as a `MATRIX_STRING`.
Overrides:
`toString` in class `MatrixBase`
Returns:
String representation of this matrix
`MatrixBase.toString(String)`
• #### toString

`public java.lang.String toString(java.lang.String numberFmtStr)`
Returns a string representation of this spatial inertia as a `MATRIX_STRING`, with each number formatted according to a supplied numeric format.
Specified by:
`toString` in interface `Matrix`
Overrides:
`toString` in class `MatrixBase`
Parameters:
`numberFmtStr` - numeric format string (see `NumberFormat`)
Returns:
String representation of this vector
`NumberFormat`
• #### toString

```public java.lang.String toString(java.lang.String numberFmtStr,
int outputCode)```
Returns a specified string representation of this spatial inertia, with each number formatted according to the a supplied numeric format.
Parameters:
`numberFmtStr` - numeric format string (see `NumberFormat`)
`outputCode` - desired representation, which should be either `MASS_INERTIA_STRING` or `MATRIX_STRING`
• #### toString

```public java.lang.String toString(NumberFormat numberFmt,
int outputCode)```
Returns a specified string representation of this spatial inertia, with each number formatted according to the a supplied numeric format.
Parameters:
`numberFmt` - numeric format
`outputCode` - desired representation, which should be either `MASS_INERTIA_STRING` or `MATRIX_STRING`
• #### getRotationalInertia

`public void getRotationalInertia(SymmetricMatrix3d J)`
Gets the rotational inertia (with respect to the center of mass) for this spatial inertia.
Parameters:
`J` - returns the rotational inertia
• #### getRotationalInertia

`public SymmetricMatrix3d getRotationalInertia()`
Gets the rotational interia (with respect to the center of mass) for this spatial inertia. The returned is an internal reference and should not be modified. Also, its value may not be updated immediately following subsequent modifications to this spatial inertia.
Returns:
the rotational inertia
• #### getOffsetRotationalInertia

`public SymmetricMatrix3d getOffsetRotationalInertia()`
Gets the rotational interia, offset by the center of mass, for this spatial inertia. The returned is an internal reference and should not be modified.
Returns:
the offset rotational inertia
• #### setRotationalInertia

`public void setRotationalInertia(SymmetricMatrix3d J)`
Sets the rotational interia for this spatial inertia.
Parameters:
`J` - rotational inertia (with respect to the center of mass)
• #### setRotationalInertia

```public void setRotationalInertia(double J00,
double J11,
double J22,
double J01,
double J02,
double J12)```
Sets the rotational interia for this spatial inertia, given the diagonal and upper off-diagonal elements
Parameters:
`J00` - element (0,0)
`J11` - element (1,1)
`J22` - element (2,2)
`J01` - element (0,1)
`J02` - element (0,2)
`J12` - element (1,2)
• #### getCenterOfMass

`public void getCenterOfMass(Vector3d com)`
Gets the center of mass for this spatial inertia.
Parameters:
`com` - returns the center of mass
• #### getCenterOfMass

`public Point3d getCenterOfMass()`
Gets the center of mass for this spatial inertia. The returned value is an internal reference and should not be modified. Also, its value may not be updated immediately following subsequent modifications to this spatial inertia.
Returns:
center of mass
• #### getRotated

```public void getRotated(Matrix6d M,
RotationMatrix3d R)```
Transforms this inertia into a different frame and returns the rotated value. The rotation is specified by a rotation matrix that transforms from the current frame into the target frame.
Parameters:
`M` - returns the rotated value
`R` - rotation matrix describing the rotation
• #### setCenterOfMass

`public void setCenterOfMass(Point3d com)`
Sets the center of mass for this spatial inertia.
Parameters:
`com` - center of mass
• #### setCenterOfMass

```public void setCenterOfMass(double x,
double y,
double z)```
Sets the center of mass for this spatial inertia.
Parameters:
`x` - center of mass x coordinate
`y` - center of mass y coordinate
`z` - center of mass z coordinate
• #### getMass

`public double getMass()`
Gets the mass for this spatial inertia.
Returns:
mass
• #### setMass

`public void setMass(double m)`
Sets the mass for this spatial inertia.
Parameters:
`m` - mass
• #### set

`public void set(Matrix6d M)`
Sets this spatial inertia to be identical to be identical to another one.
Specified by:
`set` in interface `VectorObject<Matrix6d>`
Overrides:
`set` in class `Matrix6d`
Parameters:
`M` - spatial inertia to be copied
• #### set

`public void set(Matrix6dBase M)`
Sets this spatial inertia to be identical to be identical to another one.
Overrides:
`set` in class `Matrix6dBase`
Parameters:
`M` - spatial inertia to be copied
• #### set

`public void set(SpatialInertia M)`
Sets this spatial inertia to be identical to be identical to another one.
Parameters:
`M` - spatial inertia to be copied
• #### set

```public void set(double m,
SymmetricMatrix3d J)```
Sets this spatial inertia to have a specific mass and rotational inertia. The center of mass is set to zero.
Parameters:
`m` - mass
`J` - rotational inertia (with respect to the center of mass)
• #### set

```public void set(double m,
SymmetricMatrix3d J,
Point3d com)```
Sets this spatial inertia to have a specific mass, rotational inertia, and center of mass.
Parameters:
`m` - mass
`J` - rotational inertia (with respect to the center of mass)
`com` - center of mass
• #### setPointMass

```public void setPointMass(double m,
Point3d com)```
Sets this spatial inertia to correspond to a point mass at a specific point. The resulting inertia will not be invertible.
Parameters:
`m` - mass
`com` - center of mass
• #### set

```public void set(double m,
double Jxx,
double Jyy,
double Jzz)```
Sets this spatial inertia to have a specific mass and a diagonal rotational inertia with the indicated element values. The center of mass is set to zero.
Parameters:
`m` - mass
`Jxx` - rotational inertia about x
`Jyy` - rotational inertia about y
`Jzz` - rotational inertia about z
• #### setSphere

```public void setSphere(double m,
double r)```
Sets this spatial inertia to correspond to the center of a sphere of uniform density.
Parameters:
`m` - mass
`r` - sphere radius
• #### setEllipsoid

```public void setEllipsoid(double m,
double a,
double b,
double c)```
Sets this spatial inertia to correspond to the center of an axis-aligned ellispoid of uniform density.
Parameters:
`m` - mass
`a` - semi-axis length in the x direction
`b` - semi-axis length in the y direction
`c` - semi-axis length in the z direction
• #### setBox

```public void setBox(double m,
double wx,
double wy,
double wz)```
Sets this spatial inertia to correspond to the center of an axis-aligned box of uniform density.
Parameters:
`m` - mass
`wx` - width along the x direction
`wy` - width along the y direction
`wz` - width along the z direction
• #### setCylinder

```public void setCylinder(double m,
double r,
double l)```
Sets this spatial inertia to correspond to the center of a cylinder of uniform density aligned with the z axis.
Parameters:
`m` - mass
`r` - radius
`l` - length
• #### setZero

`public void setZero()`
Sets this spatial inertia to zero.
Specified by:
`setZero` in interface `MatrixBlock`
Specified by:
`setZero` in interface `VectorObject<Matrix6d>`
Overrides:
`setZero` in class `Matrix6d`
• #### setRandom

`public void setRandom()`
Sets the components of this spatial inertia to uniformly distributed random values in the range -0.5 (inclusive) to 0.5 (exclusive). The components include the mass, center of mass, and rotational inertia. The results are adjusted to ensure that the mass is positive and the rotational inertia matrix is positive definite.
Overrides:
`setRandom` in class `Matrix6d`
• #### setRandom

```public void setRandom(double lower,
double upper)```
Sets the components of this spatial inertia to uniformly distributed random values in a specified range. The components include the mass, center of mass, and rotational inertia. The results are adjusted to ensure that the mass is positive and the rotational inertia matrix is positive definite.
Overrides:
`setRandom` in class `Matrix6d`
Parameters:
`lower` - lower random value (inclusive)
`upper` - upper random value (exclusive)
• #### setRandom

```public void setRandom(double lower,
double upper,
java.util.Random generator)```
Sets the components of this spatial inertia to uniformly distributed random values in a specified range, using a supplied random number generator. The components include the mass, center of mass, and rotational inertia. The results are adjusted to ensure that the mass is positive and the rotational inertia matrix is positive definite.
Overrides:
`setRandom` in class `Matrix6d`
Parameters:
`lower` - lower random value (inclusive)
`upper` - upper random value (exclusive)
`generator` - random number generator

`public void add(SpatialInertia M1)`
Adds this spatial inertia to M1 and places the result in this spatial inertia.
Parameters:
`M1` - right-hand inertia

```public void add(SpatialInertia M1,
SpatialInertia M2)```
Adds spatial inertia M1 to M2 and places the result in this spatial inertia.
Parameters:
`M1` - left-hand spatial inertia
`M2` - right-hand spatial inertia

```public void addPointMass(double m,
Vector3d pos)```
Adds a point mass at a specified point to this inertia. Note that subtracting a point mass can be achieved by specifying a negative mass.
Parameters:
`m` - Mass of the point being added
`pos` - Point position relative to this inertia's coordinate frame

```public void addLineSegmentInertia(double m,
Vector3d p0,
Vector3d p1)```
Adds the inertia of a line segment to this inertia. The line segment is assumed to have uniform density and is defined by two points relative to this inertia's coordinate frame.
Parameters:
`m` - line segment mass
`p0` - first line segment point
`p1` - second line segment point

```public void addTriangleInertia(double m,
Vector3d p0,
Vector3d p1,
Vector3d p2)```
Adds the inertia of a triangle to this inertia. The triangle is assumed to have uniform density and is defined by two points relative to this inertia's coordinate frame.
Parameters:
`m` - triangle mass
`p0` - first triangle point
`p1` - second triangle point
`p2` - third triangle point
• #### sub

`public void sub(SpatialInertia M1)`
Subtracts this spatial inertia from M1 and places the result in this spatial inertia.
Parameters:
`M1` - right-hand inertia
Throws:
`java.lang.IllegalArgumentException` - if the resulting mass is negative or the rotational inertia is not positive definite.
• #### sub

```public void sub(SpatialInertia M1,
SpatialInertia M2)```
Subtracts spatial inertia M1 from M2 and places the result in this spatial inertia.
Parameters:
`M1` - left-hand spatial inertia
`M2` - right-hand spatial inertia
Throws:
`java.lang.IllegalArgumentException` - if the resulting mass is negative or the rotational inertia is not positive definite.
• #### scale

```public void scale(double s,
SpatialInertia M1)```
Scales the spatial inertia M1 by `s` and places the results in this spatial inertia.
Parameters:
`s` - scaling factor
`M1` - spatial inertia to be scaled
Throws:
`java.lang.IllegalArgumentException` - if s is negative
• #### scale

`public void scale(double s)`
Scales this spatial inertia by `s` in place.
Specified by:
`scale` in interface `MatrixBlock`
Overrides:
`scale` in class `Matrix6d`
Parameters:
`s` - scaling factor
Throws:
`java.lang.IllegalArgumentException` - if s is negative
• #### getInverse

```public void getInverse(MatrixNd MI)
throws ImproperSizeException```
Computes the inverse of this spatial inertia matrix. The result matrix is resized if necessary.
Parameters:
`MI` - returns the inverse matrix
Throws:
`ImproperSizeException` - if M1 is not 6 x 6 and has a fixed size

```public static void addPointMass(Matrix M,
double m,
Vector3d pos)```
Adds a point mass at a specified point to a 6x6 matrix representing a spatial inertia. Note that subtracting a point mass can be achieved by specifying a negative mass.
Parameters:
`M` - Matrix containing the inertia. Must be a Matrix6d or a MatrixNd with size at least 6x6.
`m` - Mass of the point being added
`pos` - Point position relative to this inertia's coordinate frame
• #### invert

```public static void invert(Matrix6d MI,
Matrix6d M)```
Computes the inverse of a spatial inertia, stored in M, and return the result in MI.
Parameters:
`MI` - Matrix returning the spatial inertia
`M` - Inertia to invert
• #### mul

```public void mul(Wrench wrr,
Twist tw1)```
Multiplies a twist by this spatial inertia and places the result in a wrench:
```   wrr = M tw1
```
Parameters:
`wrr` - result wrench
`tw1` - twist to multiply
• #### mulInverse

```public void mulInverse(Twist twr,
Wrench wr1)```
Multiplies a wrench by the inverse of this spatial inertia and places the result in a twist.
```         -1
twr = M   wr1
```
Parameters:
`twr` - result twist
`wr1` - wrench to multiply
• #### mulInverse

```public void mulInverse(SpatialVector svr,
SpatialVector sv1)```
Multiplies a spatial vector by the inverse of this spatial inertia and places the result in another spatial vector.
```           -1
svr  =  M   sv1
```
Parameters:
`svr` - result
`sv1` - spatial vector to multiply
• #### mulRightFactor

```public void mulRightFactor(SpatialVector svr,
SpatialVector sv1)```
Multiplies a spatial vector by the right Cholesky factor of this spatial inertia. In other words, if M = G G' is the Cholesky factorization of this spatial inertia, then this routine computes svr = G' sv1.
Parameters:
`svr` - result
`sv1` - spatial vector to multiply
• #### mulRightFactorInverse

```public void mulRightFactorInverse(SpatialVector svr,
SpatialVector sv1)```
Multiplies a twist by the inverse of the right Cholesky factor of this spatial inertia. In other words, if M = G G' is the Cholesky factorization of this spatial inertia, then this routine computes svr = inv (G') sv1.
Parameters:
`svr` - result twist
`sv1` - twist to multiply
• #### mulLeftFactor

```public void mulLeftFactor(SpatialVector svr,
SpatialVector sv1)```
Multiplies a spatial vector by the left Cholesky factor of this spatial inertia. In other words, if M = G G' is the Cholesky factorization of this spatial inertia, then this routine computes svr = G sv1.
Parameters:
`svr` - result
`sv1` - spatial vector to multiply
• #### mulLeftFactorInverse

```public void mulLeftFactorInverse(SpatialVector svr,
SpatialVector sv1)```
Multiplies a spatial vector by the inverse of the left Cholesky factor of this spatial inertia. In other words, if M = G G' is the Cholesky factorization of this spatial inertia, then this routine computes svr = inv (G) sv1.
Parameters:
`svr` - result
`sv1` - spatial vector to multiply
• #### coriolisForce

```public void coriolisForce(Wrench wr,
Twist tw)```
Computes the coriolis forces induced by a given fixed-frame velocity acting on this spatial inertia. Both the force and velocity are given in the same coordinate frame as this inertia. The velocity is assumed to be fixed with respect to this frame, as opposed to moving with the body. For the latter case, one should use `bodyCoriolisForce` instead.
Parameters:
`wr` - returns the coriolis forces
`tw` - velocity inducing the corioilis force.
`bodyCoriolisForce(maspack.spatialmotion.Wrench, maspack.spatialmotion.Twist)`
• #### bodyCoriolisForce

```public void bodyCoriolisForce(Wrench wr,
Twist tw)```
Computes the coriolis forces induced by a given body-coordinate velocity acting on this spatial inertia. Both the force and velocity are given in the same coordinate frame as this inertia. The velocity is assumed to be moving with respect to this frame, as opposed to being fixed. For the latter case, one should use `coriolisForce` instead.
Parameters:
`wr` - returns the coriolis forces
`tw` - velocity inducing the corioilis force.
`coriolisForce(maspack.spatialmotion.Wrench, maspack.spatialmotion.Twist)`
• #### transform

`public void transform(RigidTransform3d X)`
Transforms this inertia into a new coordinate frame, given a spatial transformation matrix.
Parameters:
`X` - spatial transform from the current frame into the new frame
• #### inverseTransform

`public void inverseTransform(RigidTransform3d X)`
Transforms this inertia into a new coordinate frame, given an inverse spatial transformation matrix.
Parameters:
`X` - spatial transform from the new frame into the current frame
• #### transform

`public void transform(RotationMatrix3d R)`
Rotates this inertia into a new coordinate frame given by a rotation matrix.
Overrides:
`transform` in class `Matrix6d`
Parameters:
`R` - rotation transform from the current frame into the new frame
• #### inverseTransform

`public void inverseTransform(RotationMatrix3d R)`
Rotates this inertia into a new coordinate frame, given by the inverse of a rotation matrix.
Overrides:
`inverseTransform` in class `Matrix6d`
Parameters:
`R` - rotation transform from the new frame into the current frame
• #### scaleDistance

`public void scaleDistance(double s)`
Scale the distance units associated with this SpatialInertia.
Parameters:
`s` - scaling factor
• #### scaleMass

`public void scaleMass(double s)`
Scale the mass units associated with this SpatialInertia. This will affect both the lump mass and the moment of inertia.
Parameters:
`s` - scaling factor
• #### directedMass

`public double directedMass(Wrench wr)`
Computes the effective mass of this inertia along a specific twist direction. This is done by the formula
```              -1   T  -1
m = ( tw M   tw  )
```
where M is this spatial inertia and tw is the twist direction, which is treated as a row vector.
Parameters:
`wr` - direction along which effective mass is to be computed
Returns:
directed mass
• #### createSphereInertia

```public static SpatialInertia createSphereInertia(double m,
double r)```
Creates a new spatial inertia corresponding to the center of a sphere of uniform density.
Parameters:
`m` - mass
`r` - sphere radius
• #### createBoxInertia

```public static SpatialInertia createBoxInertia(double m,
double wx,
double wy,
double wz)```
Creates a new spatial inertia corresponding to the center of an axis-aligned box of uniform density.
Parameters:
`m` - mass
`wx` - width along the x direction
`wy` - width along the y direction
`wz` - width along the z direction
• #### createCylinderInertia

```public static SpatialInertia createCylinderInertia(double m,
double r,
double l)```
Creates a new spatial inertia corresponding to the center of a cylinder of uniform density aligned with the z axis.
Parameters:
`m` - mass
`r` - radius
`l` - length
• #### createEllipsoidInertia

```public static SpatialInertia createEllipsoidInertia(double m,
double a,
double b,
double c)```
Creates a new spatial inertia corresponding to the center of an axis-aligned ellispoid of uniform density.
Parameters:
`m` - mass
`a` - semi-axis length in the x direction
`b` - semi-axis length in the y direction
`c` - semi-axis length in the z direction

```public static void addPointRotationalInertia(Matrix3dBase J,
double m,
Vector3d r)```
Add to a rotational inertia the effect of a point mass at location r. This is equivalent to computing J = J - m [c] [c]
• #### equals

`public boolean equals(Matrix6dBase M1)`
Description copied from class: `Matrix6dBase`
Returns true if the elements of this matrix exactly equal those of matrix `M1`.
Overrides:
`equals` in class `Matrix6dBase`
Parameters:
`M1` - matrix to compare with
Returns:
false if the matrices are not equal
• #### fullEquals

`public boolean fullEquals(SpatialInertia M)`
For debugging: compares all the fields of M with the this inertia, including the component fields that depend on the matrix entries.
Parameters:
`M` - inertia to compare with
Returns:
true if all fields of M
• #### toStringAll

`public java.lang.String toStringAll(java.lang.String fmt)`
For debugging: creates a string showing this inertia and all its fields
Parameters:
`fmt` - numeric format
Returns:
string representation of the inertia and all its components