Robot Dynamics Library: RobotDynamics::Math Namespace Reference

Math types such as vectors and matrices and utility functions. More...

Classes
class	ForceVector
	A ForceVector is a SpatialVector containing 3 moments and 3 linear forces. More...

class	FrameOrientation
	A Frame object that represents an orientation(quaternion) relative to a reference frame. More...

class	FramePoint
	A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a FramePoint is expressed in, you may call the inhereted FrameObject::changeFrame method and supply it a pointer to the ReferenceFrame you wish to have the FramePoint expressed in. This class and its implementation are an adaptation of FramePoint.java by Jerry Pratt and the IHMC Robotics Group. More...

class	FrameVector
	A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other frame objects will perform runtime checks that objects are expressed in the same frames. This class and its implementation are an adaptation of FrameVector.java by Jerry Pratt and the IHMC Robotics Group. More...

class	FrameVectorPair
	A FrameVector is a pair of 3D vector with a ReferenceFrame. More...

class	Momentum
	Momentum is mass/inertia multiplied by velocity. More...

class	MotionVector

class	Point3d

class	TransformableGeometricObject
	The TransformableGeometricObject class is an essential interface because it forces all geometric objects to implement a method that tells how to transform them. This makes in possible for frame transformations of any TransformableGeometricObject can be done via the FrameObject::changeFrame method. More...

class	Vector3d

class	Matrix3d

class	Vector4d

class	SpatialVector

class	Matrix4d

class	SpatialMatrix

struct	SpatialTransform
	Compact representation of spatial transformations. More...

class	Quaternion
	Quaternion that are used for singularity free joints. More...

class	RigidBodyInertia
	This class stores a bodies mass, center of mass, and inertia information. The inertia elements are stored individually since the inertia matrix is a 3x3 symmetric matrix. The bodies inertia matrix, expressed about its center of mass, can be reconstructed as. More...

class	SpatialAcceleration
	SpatialAcceleration. For clarity, the ReferenceFrames are stated as follows. A spatial acceleration is the acceleration of the SpatialAcceleration::bodyFrame with respect to the SpatialAcceleration::baseFrame expressed in the SpatialAcceleration::expressedInFrame. More...

class	SpatialForce
	A SpatialForce is a spatial vector with the angular part being three moments and the linear part being 3 linear forces. More...

class	SpatialMomentum
	A SpatialMomentum is a Momentum expressed in a RobotDynamics::ReferenceFrame. The angular portion of the vector is referred to as and the linear portion as . More...

class	SpatialMotion
	A SpatialMotion vector is a MotionVector with a RobotDynamics::ReferenceFrame it is expressed in. This allows for runtime checks that frame rules are obeyed and makes it easy to change the frame the metion vector is expressed in. As with a SpatialAcceleration, a SpatialMotion vector is the spatial velocity of a SpatialMotion::bodyFrame relative to a SpatialMotion::baseFrame and is expressed in RobotDynamics::FrameObject::referenceFrame. More...

class	SpatialInertia
	A Math::SpatialInertia is a RigidBodyInertia explicitly expressed in a RobotDynamics::ReferenceFrame. The frame a Math::SpatialInertia is expressed in can be changed by calling RobotDynamics::FrameObject::changeFrame. More...

Typedefs
typedef std::vector< ForceVector, Eigen::aligned_allocator< ForceVector > >	ForceVectorV

typedef std::vector< FramePoint, Eigen::aligned_allocator< FramePoint > >	FramePointV

typedef std::vector< FrameVector, Eigen::aligned_allocator< FrameVector > >	FrameVectorV

typedef std::vector< MotionVector, Eigen::aligned_allocator< MotionVector > >	MotionVectorV

typedef std::vector< Point3d, Eigen::aligned_allocator< Point3d > >	Point3V

typedef Eigen::Matrix< double, 6, 3 >	Matrix63

typedef Eigen::VectorXd	VectorNd

typedef Eigen::MatrixXd	MatrixNd

typedef Eigen::AngleAxisd	AxisAngle

typedef std::vector< RigidBodyInertia, Eigen::aligned_allocator< RigidBodyInertia > >	RigidBodyInertiaV

typedef std::vector< SpatialAcceleration, Eigen::aligned_allocator< SpatialAcceleration > >	SpatialAccelerationV

typedef std::vector< SpatialForce, Eigen::aligned_allocator< SpatialForce > >	SpatialForceV

typedef std::vector< SpatialMomentum, Eigen::aligned_allocator< SpatialMomentum > >	SpatialMomentumV

typedef std::vector< SpatialMotion, Eigen::aligned_allocator< SpatialMotion > >	SpatialMotionV

typedef std::vector< SpatialInertia, Eigen::aligned_allocator< SpatialInertia > >	SpatialInertiaV

Enumerations
enum	LinearSolver { LinearSolverUnknown = 0 , LinearSolverPartialPivLU , LinearSolverColPivHouseholderQR , LinearSolverHouseholderQR , LinearSolverLLT , LinearSolverLast }
	Available solver methods for the linear systems. More...

Functions
bool	linSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x)
	Solves a linear system using gaussian elimination with pivoting. More...

Matrix3d	parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com)
	Translates the inertia matrix to a new center. More...

void	SparseFactorizeLTL (Model &model, Math::MatrixNd &H)

void	SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x)

void	SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x)

ForceVector	operator* (const SpatialTransform &X, ForceVector f)
	Operator for transforming a ForceVector. Calls the ForceVector::transform method. More...

FramePoint	operator+ (FramePoint p, const FrameVector &v)

FramePoint	operator- (FramePoint p, const FrameVector &v)

template<typename T >
FrameVector	operator* (const T scale, FramePoint p)

template<typename T >
FrameVector	operator* (FramePoint p, const T scale)

bool	operator== (const FramePoint &lhs, const FramePoint &rhs)
	Check if two FramePoints are equal. More...

FrameVector	operator- (FramePoint p1, const FramePoint &p2)
	Subtract two FramePoints and return result in newly created FramePoint. More...

bool	operator!= (const FramePoint &lhs, const FramePoint &rhs)
	Check if two FramePoints are not equal. More...

std::ostream &	operator<< (std::ostream &output, const FramePoint &framePoint)

template<typename T >
FrameVector	operator* (FrameVector v1, const T scale)

template<typename T >
FrameVector	operator* (const T scale, FrameVector v1)

FrameVector	operator+ (FrameVector v1, const Vector3d v2)

FrameVector	operator- (FrameVector v1, const Vector3d v2)

FrameVector	operator+ (FrameVector v1, const FrameVector v2)
	Add two FrameVectors together. More...

FrameVector	operator- (FrameVector v1, const FrameVector v2)
	Add two FrameVectors together. More...

template<typename T >
FrameVectorPair	operator* (FrameVectorPair pair, const T scalar)

template<typename T >
FrameVectorPair	operator* (const T scalar, FrameVectorPair pair)

FrameVectorPair	operator+ (FrameVectorPair v1, const FrameVectorPair v2)

FrameVectorPair	operator- (FrameVectorPair v1, const FrameVectorPair v2)

static Momentum	operator* (const RigidBodyInertia &I, const MotionVector &v)
	Multiplying a Math::RigidBodyInertia by a Math::MotionVector returns a Momentum. More...

static Momentum	operator- (Momentum m1, const Momentum &m2)
	Add two momentums. More...

MotionVector	operator* (const SpatialTransform &X, MotionVector v)
	Operator for transforming a MotionVector. More...

MotionVector	operator% (MotionVector v, const MotionVector &v2)

ForceVector	operator% (MotionVector v, const ForceVector &v2)

Point3d	operator+ (Point3d p, const Vector3d &v)

Point3d	operator- (Point3d p, const Vector3d &v)

template<typename T >
Point3d	operator* (Point3d p, const T scale)

template<typename T >
Point3d	operator* (const T scale, Point3d p)

Vector3d	operator- (Point3d p1, const Point3d &p2)

std::ostream &	operator<< (std::ostream &os, const Point3d &point)

static Matrix3d	toTildeForm (const Point3d &p)

static Quaternion	intrinsicZYXAnglesToQuaternion (double yaw, double pitch, double roll)
	Convert YPR angles to quaternion. More...

static Quaternion	intrinsicZYXAnglesToQuaternion (const Vector3d &zyx_angles)
	Convert YPR angles to quaternion. More...

static Quaternion	intrinsicYXZAnglesToQuaternion (double pitch, double roll, double yaw)
	Convert PRY angles to quaternion. More...

static Quaternion	intrinsicYXZAnglesToQuaternion (const Vector3d &yxz_angles)
	Convert PRY angles to quaternion. More...

static Quaternion	intrinsicXYZAnglesToQuaternion (double roll, double pitch, double yaw)
	Convert RPY angles to quaternion. More...

static Quaternion	intrinsicXYZAnglesToQuaternion (const Vector3d &xyz_angles)
	Convert RPY angles to quaternion. More...

static Matrix3d	toTildeForm (const double x, const double y, const double z)

std::ostream &	operator<< (std::ostream &output, const SpatialTransform &X)

SpatialTransform	Xrot (double angle_rad, const Vector3d &axis)
	Get spatial transform from angle and axis. More...

SpatialTransform	Xrotx (const double &xrot)
	Get transform with zero translation and pure rotation about x axis. More...

SpatialTransform	Xroty (const double &yrot)
	Get transform with zero translation and pure rotation about y axis. More...

SpatialTransform	Xrotz (const double &zrot)
	Get transform with zero translation and pure rotation about z axis. More...

SpatialTransform	XeulerZYX (double yaw, double pitch, double roll)
	Get transform with zero translation and intrinsic euler z/y/x rotation. More...

SpatialTransform	XeulerZYX (const Vector3d &ypr)
	Get transform defined by intrinsic YPR(yaw->pitch->roll) euler angles. More...

SpatialTransform	XeulerXYZ (double roll, double pitch, double yaw)
	Get transform with zero translation and intrinsic euler x/y/z rotation. More...

SpatialTransform	XeulerXYZ (const Vector3d &xyz_angles)
	Get transform with zero translation and euler x/y/z rotation. More...

SpatialTransform	XrotQuat (double x, double y, double z, double w)

SpatialTransform	XrotQuat (const Quaternion &orientation)

SpatialTransform	Xtrans (const Vector3d &r)
	Get pure translation transform. More...

SpatialTransform	Xtrans (double x, double y, double z)

VectorNd	vectorFromPtr (unsigned int n, double *ptr)

MatrixNd	matrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major=true)

Vector3d	angular_velocity_from_zyx_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates)
	convert zyx euler angles and angle rates to angular velocity More...

Vector3d	angular_velocity_from_xyz_angle_rates (const Vector3d &xyz_angles, const Vector3d &xyz_angle_rates)
	convert xyz euler angles and angle rates to angular velocity More...

Matrix3d	angular_velocity_to_zyx_angle_rates_jacobian (const Vector3d &zyx_angles, double singularity_threshold=1.e-10)
	compute the 3x3 jacobian matrix that when multiplied by the angular velocity, ordered (wx, wy, wz) returns the angular velocity vector, note that the ordering of the returned angle rates is (yaw_dot, pitch_Dot, roll_dot) More...

Matrix3d	angular_velocity_to_xyz_angle_rates_jacobian (const Vector3d &xyz_angles, double singularity_threshold=1.e-10)
	compute the 3x3 jacobian matrix that when multiplied by the angular velocity, ordered (wx, wy, wz) returns the angular velocity vector, note that the ordering of the returned angle rates is (roll_dot, pitch_dot, yaw_dot) More...

Vector3d	angular_velocity_to_zyx_angle_rates (const Vector3d &zyx_angles, const Vector3d &angular_velocity, double singularity_threshold=1.e-10)
	convert angular velocity to zyx angle rates More...

Vector3d	angular_velocity_to_xyz_angle_rates (const Vector3d &xyz_angles, const Vector3d &angular_velocity, double singularity_threshold=1.e-10)
	convert angular velocity to xyz angle rates More...

Vector3d	global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates)

Vector3d	angular_acceleration_from_zyx_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot)
	calculate angular acceleration from zyx angles, angle rates, and angle accelerations More...

Vector3d	angular_acceleration_from_xyz_angle_rates (const Vector3d &xyz_angles, const Vector3d &xyz_angle_rates, const Vector3d &xyz_angle_rates_dot)
	calculate angular acceleration from zyx angles, angle rates, and angle accelerations More...

void	SparseMultiplyHx (Model &model, Math::MatrixNd &L)

void	SparseMultiplyLx (Model &model, Math::MatrixNd &L)

void	SparseMultiplyLTx (Model &model, Math::MatrixNd &L)

RigidBodyInertia	operator+ (RigidBodyInertia rbi_in, const RigidBodyInertia &rbi)
	Add two Math::RigidBodyInertia objects together. More...

SpatialVector	operator* (const RigidBodyInertia &I, const SpatialVector &v)
	Operator for multiplying a Math::RigidBodyInertia by a Math::SpatialVector. More...

Matrix63	operator* (const RigidBodyInertia &I, const Matrix63 &m)
	Operator for multiplying a Math::RigidBodyInertia by a Math::Matrix63. More...

static RigidBodyInertia	createFromMassComInertiaC (double mass, const Vector3d &com, const Matrix3d &inertia_C)

SpatialAcceleration	operator+ (SpatialAcceleration v1, const SpatialAcceleration &v2)
	Add two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by. More...

SpatialAcceleration	operator- (SpatialAcceleration v1, const SpatialAcceleration &v2)
	Subtract two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by. More...

SpatialForce	operator+ (SpatialForce f1, const SpatialForce &f2)
	Overloaded + operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame checks are performed. More...

SpatialForce	operator- (SpatialForce f1, const SpatialForce &f2)
	Overloaded - operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame checks are performed. More...

SpatialForce	operator* (SpatialForce f1, double scale)
	Overloaded * operator to scale a spatial force by a scalar value. More...

SpatialMomentum	operator* (const SpatialInertia &inertia, const SpatialMotion &vector)

EIGEN_STRONG_INLINE SpatialMotion	operator+ (SpatialMotion v1, const SpatialMotion &v2)

EIGEN_STRONG_INLINE SpatialMotion	operator- (SpatialMotion v1, const SpatialMotion &v2)

EIGEN_STRONG_INLINE SpatialMotion	operator% (SpatialMotion v1, const SpatialMotion &v2)

SpatialInertia	operator+ (SpatialInertia inertia_a, const SpatialInertia &inertia_b)
	Adds two Math::SpatialInertia together. Performs frame checks. More...

Variables
Vector3d	Vector3dZero (0., 0., 0.)

Matrix3d	Matrix3dIdentity (1., 0., 0., 0., 1., 0., 0., 0., 1)

Matrix3d	Matrix3dZero (0., 0., 0., 0., 0., 0., 0., 0., 0.)

Quaternion	QuaternionIdentity (0., 0., 0., 1)

SpatialVector	SpatialVectorZero (0., 0., 0., 0., 0., 0.)

SpatialMatrix	SpatialMatrixIdentity (1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1.)

SpatialMatrix	SpatialMatrixZero (0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)

Detailed Description

Math types such as vectors and matrices and utility functions.

Typedef Documentation

◆ AxisAngle

typedef Eigen::AngleAxisd RobotDynamics::Math::AxisAngle

◆ ForceVectorV

typedef std::vector<ForceVector, Eigen::aligned_allocator<ForceVector> > RobotDynamics::Math::ForceVectorV

◆ FramePointV

typedef std::vector<FramePoint, Eigen::aligned_allocator<FramePoint> > RobotDynamics::Math::FramePointV

◆ FrameVectorV

typedef std::vector<FrameVector, Eigen::aligned_allocator<FrameVector> > RobotDynamics::Math::FrameVectorV

◆ Matrix63

typedef Eigen::Matrix<double, 6, 3> RobotDynamics::Math::Matrix63

◆ MatrixNd

typedef Eigen::MatrixXd RobotDynamics::Math::MatrixNd

◆ MotionVectorV

typedef std::vector<MotionVector, Eigen::aligned_allocator<MotionVector> > RobotDynamics::Math::MotionVectorV

◆ Point3V

typedef std::vector<Point3d, Eigen::aligned_allocator<Point3d> > RobotDynamics::Math::Point3V

◆ RigidBodyInertiaV

typedef std::vector<RigidBodyInertia, Eigen::aligned_allocator<RigidBodyInertia> > RobotDynamics::Math::RigidBodyInertiaV

◆ SpatialAccelerationV

typedef std::vector<SpatialAcceleration, Eigen::aligned_allocator<SpatialAcceleration> > RobotDynamics::Math::SpatialAccelerationV

◆ SpatialForceV

typedef std::vector<SpatialForce, Eigen::aligned_allocator<SpatialForce> > RobotDynamics::Math::SpatialForceV

◆ SpatialInertiaV

typedef std::vector<SpatialInertia, Eigen::aligned_allocator<SpatialInertia> > RobotDynamics::Math::SpatialInertiaV

◆ SpatialMomentumV

typedef std::vector<SpatialMomentum, Eigen::aligned_allocator<SpatialMomentum> > RobotDynamics::Math::SpatialMomentumV

◆ SpatialMotionV

typedef std::vector<SpatialMotion, Eigen::aligned_allocator<SpatialMotion> > RobotDynamics::Math::SpatialMotionV

◆ VectorNd

typedef Eigen::VectorXd RobotDynamics::Math::VectorNd

Enumeration Type Documentation

◆ LinearSolver

enum RobotDynamics::Math::LinearSolver

Available solver methods for the linear systems.

Enumerator
LinearSolverUnknown
LinearSolverPartialPivLU
LinearSolverColPivHouseholderQR
LinearSolverHouseholderQR
LinearSolverLLT
LinearSolverLast

Function Documentation

◆ angular_acceleration_from_xyz_angle_rates()

Vector3d RobotDynamics::Math::angular_acceleration_from_xyz_angle_rates	(	const Vector3d &	xyz_angles,
		const Vector3d &	xyz_angle_rates,
		const Vector3d &	xyz_angle_rates_dot
	)

inline

calculate angular acceleration from zyx angles, angle rates, and angle accelerations

Parameters

xyz_angles	(roll, pitch, yaw) euler angles
xyz_angle_rates	(roll_dot, pitch_dot, yaw_dot) angle rates
xyz_angle_accelerations	(roll_ddot, pitch_ddot, yaw_ddot) angle accelerations

Returns: angular accelerations (wx_dot, wy_dot, wz_dot)

◆ angular_acceleration_from_zyx_angle_rates()

Vector3d RobotDynamics::Math::angular_acceleration_from_zyx_angle_rates	(	const Vector3d &	zyx_angles,
		const Vector3d &	zyx_angle_rates,
		const Vector3d &	zyx_angle_rates_dot
	)

inline

calculate angular acceleration from zyx angles, angle rates, and angle accelerations

Parameters

zyx_angles	(yaw, pitch, roll) euler angles
zyx_angle_rates	(yaw_dot, pitch_dot, roll_dot) angle rates
zyx_angle_accelerations	(yaw_ddot, pitch_ddot, roll_ddot) angle accelerations

Returns: angular accelerations (wx_dot, wy_dot, wz_dot)

◆ angular_velocity_from_xyz_angle_rates()

Vector3d RobotDynamics::Math::angular_velocity_from_xyz_angle_rates	(	const Vector3d &	xyz_angles,
		const Vector3d &	xyz_angle_rates
	)

inline

convert xyz euler angles and angle rates to angular velocity

Parameters

xyz_angles	(roll, pitch, yaw) euler angles
xyz_angle_rates	(roll_dot, pitch_dot, yaw_dot)

Returns: angular velocities (wx, wy, wz)

◆ angular_velocity_from_zyx_angle_rates()

Vector3d RobotDynamics::Math::angular_velocity_from_zyx_angle_rates	(	const Vector3d &	zyx_angles,
		const Vector3d &	zyx_angle_rates
	)

inline

convert zyx euler angles and angle rates to angular velocity

Parameters

xyz_angles	(yaw, pitch, roll) euler angles
xyz_angle_rates	(yaw_dot, pitch_dot, roll_dot)

Returns: angular velocities (wx, wy, wz)

◆ angular_velocity_to_xyz_angle_rates()

Vector3d RobotDynamics::Math::angular_velocity_to_xyz_angle_rates	(	const Vector3d &	xyz_angles,
		const Vector3d &	angular_velocity,
		double	singularity_threshold = `1.e-10`
	)

inline

convert angular velocity to xyz angle rates

Parameters

xyz_angles	yaw/pitch/roll euler angles, ordered (roll, pitch, yaw)
angular_velocity	angular velocity (x, y, z)
singularity	threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a vector of zeros and set errno to EDOM, signifying a domain error

Returns: given xyz angles and angular velocity, a vector of angle rates orderd (roll_dot, pitch_dot, yaw_dot)

◆ angular_velocity_to_xyz_angle_rates_jacobian()

Matrix3d RobotDynamics::Math::angular_velocity_to_xyz_angle_rates_jacobian	(	const Vector3d &	xyz_angles,
		double	singularity_threshold = `1.e-10`
	)

inline

compute the 3x3 jacobian matrix that when multiplied by the angular velocity, ordered (wx, wy, wz) returns the angular velocity vector, note that the ordering of the returned angle rates is (roll_dot, pitch_dot, yaw_dot)

Parameters

xyz_angles	roll/pitch/yaw euler angles, ordered (roll, pitch, yaw)
singularity	threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and set errno to EDOM, signifying a domain error

Returns: jacobian matrix, S, that when multiplied by angular velocity, v as in S*v returns a vector of angle rates

◆ angular_velocity_to_zyx_angle_rates()

Vector3d RobotDynamics::Math::angular_velocity_to_zyx_angle_rates	(	const Vector3d &	zyx_angles,
		const Vector3d &	angular_velocity,
		double	singularity_threshold = `1.e-10`
	)

inline

convert angular velocity to zyx angle rates

Parameters

zyx_angles	yaw/pitch/roll euler angles, ordered (yaw, pitch, roll)
angular_velocity	angular velocity (x, y, z)
singularity	threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and set errno to EDOM, signifying a domain error

Returns: given zyx angles and angular velocity, a vector of angle rates orderd (yaw_dot, pitch_dot, roll_dot)

◆ angular_velocity_to_zyx_angle_rates_jacobian()

Matrix3d RobotDynamics::Math::angular_velocity_to_zyx_angle_rates_jacobian	(	const Vector3d &	zyx_angles,
		double	singularity_threshold = `1.e-10`
	)

inline

Parameters

zyx_angles	yaw/pitch/roll euler angles, ordered (yaw, pitch, roll)
singularity	threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and set errno to EDOM, signifying a domain error

Returns: jacobian matrix, S, that when multiplied by angular velocity, v as in S*v returns a vector of angle rates

◆ createFromMassComInertiaC()

static RigidBodyInertia RobotDynamics::Math::createFromMassComInertiaC	(	double	mass,
		const Vector3d &	com,
		const Matrix3d &	inertia_C
	)

inlinestatic

Creates a RigidBodyInertia from a body's mass, center of mass location, and 3x3 inertia matrix

Parameters

mass	Body mass
com	Center of mass location in body frame
inertia_C	3x3 inertia tensor about from located at center of mass and aligned with body frame

Returns: RigidBodyInertia

◆ global_angular_velocity_from_rates()

Vector3d RobotDynamics::Math::global_angular_velocity_from_rates	(	const Vector3d &	zyx_angles,
		const Vector3d &	zyx_rates
	)

inline

◆ intrinsicXYZAnglesToQuaternion() [1/2]

static Quaternion RobotDynamics::Math::intrinsicXYZAnglesToQuaternion ( const Vector3d & xyz_angles )

inlinestatic

Convert RPY angles to quaternion.

◆ intrinsicXYZAnglesToQuaternion() [2/2]

static Quaternion RobotDynamics::Math::intrinsicXYZAnglesToQuaternion	(	double	roll,
		double	pitch,
		double	yaw
	)

inlinestatic

Convert RPY angles to quaternion.

◆ intrinsicYXZAnglesToQuaternion() [1/2]

static Quaternion RobotDynamics::Math::intrinsicYXZAnglesToQuaternion ( const Vector3d & yxz_angles )

inlinestatic

Convert PRY angles to quaternion.

◆ intrinsicYXZAnglesToQuaternion() [2/2]

static Quaternion RobotDynamics::Math::intrinsicYXZAnglesToQuaternion	(	double	pitch,
		double	roll,
		double	yaw
	)

inlinestatic

Convert PRY angles to quaternion.

◆ intrinsicZYXAnglesToQuaternion() [1/2]

static Quaternion RobotDynamics::Math::intrinsicZYXAnglesToQuaternion ( const Vector3d & zyx_angles )

inlinestatic

Convert YPR angles to quaternion.

◆ intrinsicZYXAnglesToQuaternion() [2/2]

static Quaternion RobotDynamics::Math::intrinsicZYXAnglesToQuaternion	(	double	yaw,
		double	pitch,
		double	roll
	)

inlinestatic

Convert YPR angles to quaternion.

◆ linSolveGaussElimPivot()

bool RobotDynamics::Math::linSolveGaussElimPivot	(	MatrixNd	A,
		VectorNd	b,
		VectorNd &	x
	)

Solves a linear system using gaussian elimination with pivoting.

◆ matrixFromPtr()

MatrixNd RobotDynamics::Math::matrixFromPtr	(	unsigned int	rows,
		unsigned int	cols,
		double *	ptr,
		bool	row_major = `true`
	)

inline

◆ operator!=()

bool RobotDynamics::Math::operator!=	(	const FramePoint &	lhs,
		const FramePoint &	rhs
	)

inline

Check if two FramePoints are not equal.

Parameters

lhs
rhs

Exceptions

ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame

Returns: bool false if equal, true if not equal

◆ operator%() [1/3]

ForceVector RobotDynamics::Math::operator%	(	MotionVector	v,
		const ForceVector &	v2
	)

inline

Operator for performing the spatial vector $\times*$ operator

Parameters

v
v2

Returns: Returns $ret = (v\times*)v2$

◆ operator%() [2/3]

MotionVector RobotDynamics::Math::operator%	(	MotionVector	v,
		const MotionVector &	v2
	)

inline

Operator for performing the spatial vector $\times$ operator.

Parameters

v
v2

Returns: Returns $ret = (v \times)v2$

◆ operator%() [3/3]

EIGEN_STRONG_INLINE SpatialMotion RobotDynamics::Math::operator%	(	SpatialMotion	v1,
		const SpatialMotion &	v2
	)

◆ operator*() [1/15]

Matrix63 RobotDynamics::Math::operator*	(	const RigidBodyInertia &	I,
		const Matrix63 &	m
	)

inline

Operator for multiplying a Math::RigidBodyInertia by a Math::Matrix63.

Parameters

I
m

Returns: The result of multiplying a Math::RigidBodyInertia by a Math::Matrix63

◆ operator*() [2/15]

static Momentum RobotDynamics::Math::operator*	(	const RigidBodyInertia &	I,
		const MotionVector &	v
	)

inlinestatic

Multiplying a Math::RigidBodyInertia by a Math::MotionVector returns a Momentum.

Parameters

I
v

Returns: I*V

◆ operator*() [3/15]

SpatialVector RobotDynamics::Math::operator*	(	const RigidBodyInertia &	I,
		const SpatialVector &	v
	)

inline

Operator for multiplying a Math::RigidBodyInertia by a Math::SpatialVector.

Parameters

I
v

Returns: Math::RigidBodyInertia times Math::SpatialVector

◆ operator*() [4/15]

SpatialMomentum RobotDynamics::Math::operator*	(	const SpatialInertia &	inertia,
		const SpatialMotion &	vector
	)

inline

Overloaded * operator for computing a SpatialMomentum from a SpatialInertia and SpatialMotion. Frame checks will be performed.

Parameters

inertia
vector

Returns: The resulting SpatialMomentum

◆ operator*() [5/15]

ForceVector RobotDynamics::Math::operator*	(	const SpatialTransform &	X,
		ForceVector	f
	)

inline

Operator for transforming a ForceVector. Calls the ForceVector::transform method.

Parameters

X	SpatialTransform
f	ForceVector to be transformed

Returns: Transformed ForceVector

◆ operator*() [6/15]

MotionVector RobotDynamics::Math::operator*	(	const SpatialTransform &	X,
		MotionVector	v
	)

inline

Operator for transforming a MotionVector.

Parameters

X	SpatialTransform to the desired frame
v

Returns: Transformed MotionVector

◆ operator*() [7/15]

template<typename T >

FrameVectorPair RobotDynamics::Math::operator*	(	const T	scalar,
		FrameVectorPair	pair
	)

inline

◆ operator*() [8/15]

template<typename T >

FrameVector RobotDynamics::Math::operator*	(	const T	scale,
		FramePoint	p
	)

inline

◆ operator*() [9/15]

template<typename T >

FrameVector RobotDynamics::Math::operator*	(	const T	scale,
		FrameVector	v1
	)

inline

◆ operator*() [10/15]

template<typename T >

Point3d RobotDynamics::Math::operator*	(	const T	scale,
		Point3d	p
	)

inline

◆ operator*() [11/15]

template<typename T >

FrameVector RobotDynamics::Math::operator*	(	FramePoint	p,
		const T	scale
	)

inline

◆ operator*() [12/15]

template<typename T >

FrameVector RobotDynamics::Math::operator*	(	FrameVector	v1,
		const T	scale
	)

inline

◆ operator*() [13/15]

template<typename T >

FrameVectorPair RobotDynamics::Math::operator*	(	FrameVectorPair	pair,
		const T	scalar
	)

inline

◆ operator*() [14/15]

template<typename T >

Point3d RobotDynamics::Math::operator*	(	Point3d	p,
		const T	scale
	)

inline

◆ operator*() [15/15]

SpatialForce RobotDynamics::Math::operator*	(	SpatialForce	f1,
		double	scale
	)

inline

Overloaded * operator to scale a spatial force by a scalar value.

Parameters

f1
scale

Returns

◆ operator+() [1/10]

FramePoint RobotDynamics::Math::operator+	(	FramePoint	p,
		const FrameVector &	v
	)

inline

◆ operator+() [2/10]

FrameVector RobotDynamics::Math::operator+	(	FrameVector	v1,
		const FrameVector	v2
	)

inline

Add two FrameVectors together.

Parameters

v1
v2

Returns: THe result of

◆ operator+() [3/10]

FrameVector RobotDynamics::Math::operator+	(	FrameVector	v1,
		const Vector3d	v2
	)

inline

◆ operator+() [4/10]

FrameVectorPair RobotDynamics::Math::operator+	(	FrameVectorPair	v1,
		const FrameVectorPair	v2
	)

inline

◆ operator+() [5/10]

Point3d RobotDynamics::Math::operator+	(	Point3d	p,
		const Vector3d &	v
	)

inline

◆ operator+() [6/10]

RigidBodyInertia RobotDynamics::Math::operator+	(	RigidBodyInertia	rbi_in,
		const RigidBodyInertia &	rbi
	)

inline

Add two Math::RigidBodyInertia objects together.

Parameters

rbi_in	A copy that is modified and returned.
rbi

Returns: A Math::RigidBodyInertia that is the addition of the two arguments

◆ operator+() [7/10]

SpatialAcceleration RobotDynamics::Math::operator+	(	SpatialAcceleration	v1,
		const SpatialAcceleration &	v2
	)

inline

Add two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by.

Parameters

v1
v2

Returns: A new SpatialAcceleration that is the addition of the two arguments.

◆ operator+() [8/10]

SpatialForce RobotDynamics::Math::operator+	(	SpatialForce	f1,
		const SpatialForce &	f2
	)

inline

Overloaded + operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame checks are performed.

Parameters

f1
f2

Returns

◆ operator+() [9/10]

SpatialInertia RobotDynamics::Math::operator+	(	SpatialInertia	inertia_a,
		const SpatialInertia &	inertia_b
	)

inline

Adds two Math::SpatialInertia together. Performs frame checks.

Parameters

inertia_a
inertia_b

Returns: The addition of the two arguments

◆ operator+() [10/10]

EIGEN_STRONG_INLINE SpatialMotion RobotDynamics::Math::operator+	(	SpatialMotion	v1,
		const SpatialMotion &	v2
	)

◆ operator-() [1/11]

FramePoint RobotDynamics::Math::operator-	(	FramePoint	p,
		const FrameVector &	v
	)

inline

◆ operator-() [2/11]

FrameVector RobotDynamics::Math::operator-	(	FramePoint	p1,
		const FramePoint &	p2
	)

inline

Subtract two FramePoints and return result in newly created FramePoint.

Parameters

p1
p2

Exceptions

ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame

Returns: A FramePoint that is the difference of the two argument FramePoints, i.e. p1-=p2

◆ operator-() [3/11]

FrameVector RobotDynamics::Math::operator-	(	FrameVector	v1,
		const FrameVector	v2
	)

inline

Add two FrameVectors together.

Parameters

v1
v2

Returns: THe result of

◆ operator-() [4/11]

FrameVector RobotDynamics::Math::operator-	(	FrameVector	v1,
		const Vector3d	v2
	)

inline

◆ operator-() [5/11]

FrameVectorPair RobotDynamics::Math::operator-	(	FrameVectorPair	v1,
		const FrameVectorPair	v2
	)

inline

◆ operator-() [6/11]

static Momentum RobotDynamics::Math::operator-	(	Momentum	m1,
		const Momentum &	m2
	)

inlinestatic

Add two momentums.

Parameters

m1
m2

Returns: m = m1-m2 /f$

◆ operator-() [7/11]

Point3d RobotDynamics::Math::operator-	(	Point3d	p,
		const Vector3d &	v
	)

inline

◆ operator-() [8/11]

Vector3d RobotDynamics::Math::operator-	(	Point3d	p1,
		const Point3d &	p2
	)

inline

◆ operator-() [9/11]

SpatialAcceleration RobotDynamics::Math::operator-	(	SpatialAcceleration	v1,
		const SpatialAcceleration &	v2
	)

inline

Subtract two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by.

Parameters

v1
v2

Returns: A new SpatialAcceleration that is the subtraction of the second argument from the first.

◆ operator-() [10/11]

SpatialForce RobotDynamics::Math::operator-	(	SpatialForce	f1,
		const SpatialForce &	f2
	)

inline

Overloaded - operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame checks are performed.

Parameters

f1
f2

Returns

◆ operator-() [11/11]

EIGEN_STRONG_INLINE SpatialMotion RobotDynamics::Math::operator-	(	SpatialMotion	v1,
		const SpatialMotion &	v2
	)

◆ operator<<() [1/3]

std::ostream& RobotDynamics::Math::operator<<	(	std::ostream &	os,
		const Point3d &	point
	)

inline

◆ operator<<() [2/3]

std::ostream& RobotDynamics::Math::operator<<	(	std::ostream &	output,
		const FramePoint &	framePoint
	)

inline

◆ operator<<() [3/3]

std::ostream& RobotDynamics::Math::operator<<	(	std::ostream &	output,
		const SpatialTransform &	X
	)

inline

◆ operator==()

bool RobotDynamics::Math::operator==	(	const FramePoint &	lhs,
		const FramePoint &	rhs
	)

inline

Check if two FramePoints are equal.

Parameters

lhs
rhs

Exceptions

ReferenceFrameException If the two FramePoint arguments are not expressed in the same ReferenceFrame

Returns: bool true if equal, false if not equal

◆ parallel_axis()

Matrix3d RobotDynamics::Math::parallel_axis	(	const Matrix3d &	inertia,
		double	mass,
		const Vector3d &	com
	)

Translates the inertia matrix to a new center.

◆ SparseFactorizeLTL()

void RobotDynamics::Math::SparseFactorizeLTL	(	Model &	model,
		Math::MatrixNd &	H
	)

◆ SparseMultiplyHx()

void RobotDynamics::Math::SparseMultiplyHx	(	Model &	model,
		Math::MatrixNd &	L
	)

◆ SparseMultiplyLTx()

void RobotDynamics::Math::SparseMultiplyLTx	(	Model &	model,
		Math::MatrixNd &	L
	)

◆ SparseMultiplyLx()

void RobotDynamics::Math::SparseMultiplyLx	(	Model &	model,
		Math::MatrixNd &	L
	)

◆ SparseSolveLTx()

void RobotDynamics::Math::SparseSolveLTx	(	Model &	model,
		Math::MatrixNd &	L,
		Math::VectorNd &	x
	)

◆ SparseSolveLx()

void RobotDynamics::Math::SparseSolveLx	(	Model &	model,
		Math::MatrixNd &	L,
		Math::VectorNd &	x
	)

◆ toTildeForm() [1/2]

static Matrix3d RobotDynamics::Math::toTildeForm	(	const double	x,
		const double	y,
		const double	z
	)

inlinestatic

◆ toTildeForm() [2/2]

static Matrix3d RobotDynamics::Math::toTildeForm ( const Point3d & p )

inlinestatic

◆ vectorFromPtr()

VectorNd RobotDynamics::Math::vectorFromPtr	(	unsigned int	n,
		double *	ptr
	)

inline

◆ XeulerXYZ() [1/2]

SpatialTransform RobotDynamics::Math::XeulerXYZ ( const Vector3d & xyz_angles )

inline

Get transform with zero translation and euler x/y/z rotation.

Parameters

xyz_angles xyz_angles xyz angles where element 0 is roll, 1 is pitch, and 2 is yaw

Returns: Transform with zero translation and x/y/z rotation

◆ XeulerXYZ() [2/2]

SpatialTransform RobotDynamics::Math::XeulerXYZ	(	double	roll,
		double	pitch,
		double	yaw
	)

inline

Get transform with zero translation and intrinsic euler x/y/z rotation.

Parameters

roll
pitch
yaw

Returns: Transform with zero translation and x/y/z rotation

◆ XeulerZYX() [1/2]

SpatialTransform RobotDynamics::Math::XeulerZYX ( const Vector3d & ypr )

inline

Get transform defined by intrinsic YPR(yaw->pitch->roll) euler angles.

Parameters

ypr	Vector of euler angles where ypr[0] is yaw, ypr[1] is pitch, and ypr[2] is roll

Returns: spatial transform where rotation component is defined by the YPR rotations

◆ XeulerZYX() [2/2]

SpatialTransform RobotDynamics::Math::XeulerZYX	(	double	yaw,
		double	pitch,
		double	roll
	)

inline

Get transform with zero translation and intrinsic euler z/y/x rotation.

Parameters

yaw
pitch
roll

Returns: Transform with zero translation and z/y/x rotation

◆ Xrot()

SpatialTransform RobotDynamics::Math::Xrot	(	double	angle_rad,
		const Vector3d &	axis
	)

inline

Get spatial transform from angle and axis.

Parameters

angle_rad	angle magnitude
axis	normalized 3d vector

Returns: Spatial transform

◆ XrotQuat() [1/2]

SpatialTransform RobotDynamics::Math::XrotQuat ( const Quaternion & orientation )

inline

◆ XrotQuat() [2/2]

SpatialTransform RobotDynamics::Math::XrotQuat	(	double	x,
		double	y,
		double	z,
		double	w
	)

inline

◆ Xrotx()

SpatialTransform RobotDynamics::Math::Xrotx ( const double & xrot )

inline

Get transform with zero translation and pure rotation about x axis.

Parameters

xrot

Returns: Transform with zero translation and x-rotation

◆ Xroty()

SpatialTransform RobotDynamics::Math::Xroty ( const double & yrot )

inline

Get transform with zero translation and pure rotation about y axis.

Parameters

yrot

Returns: Transform with zero translation and y-rotation

◆ Xrotz()

SpatialTransform RobotDynamics::Math::Xrotz ( const double & zrot )

inline

Get transform with zero translation and pure rotation about z axis.

Parameters

zrot

Returns: Transform with zero translation and z-rotation

◆ Xtrans() [1/2]

SpatialTransform RobotDynamics::Math::Xtrans ( const Vector3d & r )

inline

Get pure translation transform.

Parameters

Returns: Transform with identity rotation and translation

◆ Xtrans() [2/2]

SpatialTransform RobotDynamics::Math::Xtrans	(	double	x,
		double	y,
		double	z
	)

inline

Variable Documentation

◆ Matrix3dIdentity

Matrix3d RobotDynamics::Math::Matrix3dIdentity	(	1.	,
		0.	,
		0.	,
		0.	,
		1.	,
		0.	,
		0.	,
		0.	,
		1
	)

◆ Matrix3dZero

Matrix3d RobotDynamics::Math::Matrix3dZero	(	0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.
	)

◆ QuaternionIdentity

Quaternion RobotDynamics::Math::QuaternionIdentity	(	0.	,
		0.	,
		0.	,
		1
	)

◆ SpatialMatrixIdentity

SpatialMatrix RobotDynamics::Math::SpatialMatrixIdentity	(	1.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		1.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		1.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		1.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		1.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		1.
	)

◆ SpatialMatrixZero

SpatialMatrix RobotDynamics::Math::SpatialMatrixZero	(	0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.
	)

◆ SpatialVectorZero

SpatialVector RobotDynamics::Math::SpatialVectorZero	(	0.	,
		0.	,
		0.	,
		0.	,
		0.	,
		0.
	)

◆ Vector3dZero

Vector3d RobotDynamics::Math::Vector3dZero	(	0.	,
		0.	,
		0.
	)

Classes

Typedefs

Enumerations

Functions

Variables

Detailed Description

Typedef Documentation

◆ AxisAngle

◆ ForceVectorV

◆ FramePointV

◆ FrameVectorV

◆ Matrix63

◆ MatrixNd

◆ MotionVectorV

◆ Point3V

◆ RigidBodyInertiaV

◆ SpatialAccelerationV

◆ SpatialForceV

◆ SpatialInertiaV

◆ SpatialMomentumV

◆ SpatialMotionV

◆ VectorNd

Enumeration Type Documentation

◆ LinearSolver

Function Documentation

◆ angular_acceleration_from_xyz_angle_rates()

◆ angular_acceleration_from_zyx_angle_rates()

◆ angular_velocity_from_xyz_angle_rates()

◆ angular_velocity_from_zyx_angle_rates()

◆ angular_velocity_to_xyz_angle_rates()

◆ angular_velocity_to_xyz_angle_rates_jacobian()

◆ angular_velocity_to_zyx_angle_rates()

◆ angular_velocity_to_zyx_angle_rates_jacobian()

◆ createFromMassComInertiaC()

◆ global_angular_velocity_from_rates()

◆ intrinsicXYZAnglesToQuaternion() [1/2]

◆ intrinsicXYZAnglesToQuaternion() [2/2]

◆ intrinsicYXZAnglesToQuaternion() [1/2]

◆ intrinsicYXZAnglesToQuaternion() [2/2]

◆ intrinsicZYXAnglesToQuaternion() [1/2]

◆ intrinsicZYXAnglesToQuaternion() [2/2]

◆ linSolveGaussElimPivot()

◆ matrixFromPtr()

◆ operator!=()

◆ operator%() [1/3]

◆ operator%() [2/3]

◆ operator%() [3/3]

◆ operator*() [1/15]

◆ operator*() [2/15]

◆ operator*() [3/15]

◆ operator*() [4/15]

◆ operator*() [5/15]

◆ operator*() [6/15]

◆ operator*() [7/15]

◆ operator*() [8/15]

◆ operator*() [9/15]

◆ operator*() [10/15]

◆ operator*() [11/15]

◆ operator*() [12/15]

◆ operator*() [13/15]

◆ operator*() [14/15]

◆ operator*() [15/15]

◆ operator+() [1/10]

◆ operator+() [2/10]

◆ operator+() [3/10]

◆ operator+() [4/10]

◆ operator+() [5/10]

◆ operator+() [6/10]

◆ operator+() [7/10]

◆ operator+() [8/10]

◆ operator+() [9/10]

◆ operator+() [10/10]

◆ operator-() [1/11]

◆ operator-() [2/11]

◆ operator-() [3/11]

◆ operator-() [4/11]

◆ operator-() [5/11]

◆ operator-() [6/11]

◆ operator-() [7/11]

◆ operator-() [8/11]