maspack.spatialmotion
Class SpatialInertia

java.lang.Object
  extended by maspack.matrix.MatrixBase
      extended by maspack.matrix.DenseMatrixBase
          extended by maspack.matrix.Matrix6dBase
              extended by maspack.matrix.Matrix6d
                  extended by maspack.matrix.Matrix6dBlock
                      extended by maspack.spatialmotion.SpatialInertia
All Implemented Interfaces:
java.io.Serializable, java.lang.Cloneable, DenseMatrix, LinearTransformNd, Matrix, MatrixBlock, Clonable

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
 fb.add (fc); // add coriolis force to body force
 M.mulInverse (acc, fb); // solve fb = M acc
 // integrate using symplectic Euler step
 vb.scaledAdd (h, acc, vb);
 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
 fb.add (fc); // add coriolis force to body force
 M.mulInverse (acc, fb); // solve fb = M acc
 // integrate using symplectic Euler step
 vb.scaledAdd (h, acc, vb);
 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.

See Also:
Serialized Form

Nested Class Summary
 
Nested classes/interfaces inherited from interface maspack.matrix.Matrix
Matrix.Partition, Matrix.WriteFormat
 
Field Summary
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.
 
Fields inherited from class maspack.matrix.Matrix6d
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
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
 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 addPointMass(double m, Vector3d pos)
          Adds a point mass at a specified point to this inertia.
static void addPointMass(Matrix6d M, double m, Vector3d pos)
          Adds a point mass at a specified point to a 6x6 matrix representing a spatial 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.
 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(Point3d 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 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)
          Reads the contents of this spatial inertia from a ReaderTokenizer.
 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(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)
          Gets the center of mass vector for this spatial ine /** 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.
 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.
 
Methods inherited from class maspack.matrix.Matrix6dBlock
add, addNumNonZerosByCol, addNumNonZerosByRow, clone, 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, addSubMatrix00, addSubMatrix03, addSubMatrix30, addSubMatrix33, getSubMatrix00, getSubMatrix03, getSubMatrix30, getSubMatrix33, inverseTransform, invert, mul, mul, mulInverse, mulInverseBoth, mulInverseLeft, mulInverseRight, mulTranspose, mulTransposeBoth, mulTransposeLeft, mulTransposeRight, negate, scale, setDiagonal, setDiagonal, setLowerToUpper, setSubMatrix00, setSubMatrix03, setSubMatrix30, setSubMatrix33, sub, sub, transform, transpose
 
Methods inherited from class maspack.matrix.Matrix6dBase
colSize, determinant, epsilonEquals, equals, 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, get, getCCSIndices, getCCSIndices, getCCSValues, getCCSValues, getColumn, getCRSIndices, getCRSIndices, getCRSValues, getCRSValues, getDefaultFormat, getRow, getSize, getSubMatrix, hasNaN, idString, isFixedSize, isSymmetric, mul, mul, mulAdd, mulAdd, mulAdd, mulTranspose, mulTranspose, mulTransposeAdd, mulTransposeAdd, mulTransposeAdd, numNonZeroVals, numNonZeroVals, setCRSValues, setDefaultFormat, setSize, toString, trace, 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.DenseMatrix
set, setColumn, setColumn, setRow, setRow, setSubMatrix
 
Methods inherited from interface maspack.matrix.Matrix
checkConsistency, colSize, determinant, epsilonEquals, equals, frobeniusNorm, get, getCCSIndices, getCCSIndices, getCCSValues, getCCSValues, getColumn, getColumn, getColumn, getCRSIndices, getCRSIndices, getCRSValues, getCRSValues, getRow, getRow, getRow, getSize, getSubMatrix, infinityNorm, isFixedSize, isSymmetric, 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
 

Field Detail

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.

See Also:
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.

See Also:
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

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

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
See Also:
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.

Overrides:
toString in class MatrixBase
Parameters:
numberFmtStr - numeric format string (see NumberFormat)
Returns:
String representation of this vector

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)
Gets the center of mass vector for this spatial ine /** 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(Point3d 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(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
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

add

public void add(SpatialInertia M1)
Adds this spatial inertia to M1 and places the result in this spatial inertia.

Parameters:
M1 - right-hand inertia

add

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

addPointMass

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

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

addPointMass

public static void addPointMass(Matrix6d 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 - 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.
See Also:
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.
See Also:
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