maspack.spatialmotion
Class SpatialInertia

java.lang.Object
  maspack.matrix.MatrixObject
      maspack.spatialmotion.SpatialInertia

All Implemented Interfaces:

java.io.Serializable

public class SpatialInertia

extends MatrixObject

implements java.io.Serializable

See Also:

Serialized Form

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.

Constructor Summary

SpatialInertia()
Creates a new spatial inertia with a value of zero.

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.
int	colSize() Returns the number of columns in this matrix (which is always 6).
void	coriolisForce(Wrench wr, Twist tw) Computes the coriolis forces induced by a given velocity acting on this spatial inertia.
double	get(int i, int j) Gets a single element of this matrix.
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.
void	getRotationalInertia(SymmetricMatrix3d J) Gets the rotational interia for this spatial inertia.
void	inverseTransform(RigidTransform3d X) Transforms this inertia into a new coordinate frame, given an inverse spatial transformation matrix.
void	mul(Wrench wrr, Twist tw1) Multiplies a twist by this spatial inertia and places the result in a wrench.
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(Wrench wrr, Wrench wr1) Multiplies a wrench by the left Cholesky factor of this spatial inertia.
void	mulLeftFactorInverse(Wrench wrr, Wrench wr1) Multiplies a wrench by the inverse of the left Cholesky factor of this spatial inertia.
void	mulRightFactor(Twist twr, Twist tw1) Multiplies a twist by the right Cholesky factor of this spatial inertia.
void	mulRightFactorInverse(Twist twr, Twist tw1) Multiplies a twist by the inverse of the right Cholesky factor of this spatial inertia.
int	rowSize() Returns the number of rows in this matrix (which is always 6).
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	scan(java.io.StreamTokenizer stok) Reads the contents of this spatial inertia from a StreamTokenizer.
void	set(double m, double Jx, double Jy, double Jz) 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(SpatialInertia M) Sets this spatial inertia to be identical to be identical to another one.
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	setMass(double m) Sets the mass for this spatial inertia.
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	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 6 x 6 matrix.
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 6 x 6 matrix, 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.

Methods inherited from class maspack.matrix.MatrixObject

determinant, epsilonEquals, equals, equals, frobeniusNorm, get, get, getColumn, getColumn, getRow, getRow, infinityNorm, isFixedSize, oneNorm, set, set, set, setColumn, setColumn, setRow, setRow, setSize, toString

Methods inherited from class java.lang.Object

getClass, hashCode, notify, notifyAll, wait, wait, wait

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

Method Detail

colSize

public int colSize()

Returns the number of columns in this matrix (which is always 6).

Specified by:

colSize in class MatrixObject

Returns:

6

rowSize

public int rowSize()

Returns the number of rows in this matrix (which is always 6).

Specified by:

rowSize in class MatrixObject

Returns:

6

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 class MatrixObject

Parameters:

i - element row index

j - element column index

val - element value

get

public double get(int i,
                  int j)

Gets a single element of this matrix.

Specified by:

get in class MatrixObject

Parameters:

i - element row index

j - element column index

Returns:

element value

scan

public void scan(java.io.StreamTokenizer stok)
          throws java.io.IOException

Reads the contents of this spatial inertia from a StreamTokenizer. 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.

Note:This method disables numeric parsing in the StreamTokenizer. This is because the method does its own numeric parsing, in order to handle exponents.

Overrides:

scan in class MatrixObject

Parameters:

stok - StreamTokenizer from which to read the inertia

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 6 x 6 matrix.

Overrides:

toString in class MatrixObject

Returns:

String representation of this matrix

toString

public java.lang.String toString(java.lang.String numberFmtStr)

Returns a string representation of this spatial inertia as a 6 x 6 matrix, with each number formatted according to a supplied numeric format.

Overrides:

toString in class MatrixObject

Parameters:

numberFmtStr - numeric format string (see NumberFormat)

Returns:

String representation of this matrix

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 interia for this spatial inertia.

Parameters:

J - returns the rotational inertia (with respect to the center of mass)

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(Point3d com)

Gets the center of mass for this spatial inertia.

Parameters:

com - returns the center of mass

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(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

set

public void set(double m,
                double Jx,
                double Jy,
                double Jz)

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

Jx - rotational inertia about x

Jy - rotational inertia about x

Jz - rotational inertia about x

setZero

public void setZero()

Sets this spatial inertia to zero.

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.

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.

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.

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

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.

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

mul

public void mul(Wrench wrr,
                Twist tw1)

Multiplies a twist by this spatial inertia and places the result in a wrench.

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.

Parameters:

twr - result twist

wr1 - wrench to multiply

mulRightFactor

public void mulRightFactor(Twist twr,
                           Twist tw1)

Multiplies a twist 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 twr = G' tw1.

Parameters:

twr - result twist

tw1 - twist to multiply

mulRightFactorInverse

public void mulRightFactorInverse(Twist twr,
                                  Twist tw1)

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 twr = inv (G') tw1.

Parameters:

twr - result twist

tw1 - twist to multiply

mulLeftFactor

public void mulLeftFactor(Wrench wrr,
                          Wrench wr1)

Multiplies a wrench 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 wrr = G wr1.

Parameters:

wrr - result wrench

wr1 - wrench to multiply

mulLeftFactorInverse

public void mulLeftFactorInverse(Wrench wrr,
                                 Wrench wr1)

Multiplies a wrench 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 wrr = inv (G) wr1.

Parameters:

wrr - result wrench

wr1 - wrench to multiply

coriolisForce

public void coriolisForce(Wrench wr,
                          Twist tw)

Computes the coriolis forces induced by a given velocity acting on this spatial inertia. Both the force and velocity are given in the coordinate frame of the inertia.

Parameters:

wr - returns the coriolis forces

tw - velocity inducing the corioilis force.

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