Pose of a single rigid body. More...

#include <RigidBodyPose.h>

Public Types
enum	STORAGE_TYPE { QUAT = 0, EULER = 1 }
	the orientation can either be stored as EulerAngles or as quaternion. More...

using	HomogeneousTransform = kindr::HomogeneousTransformationPosition3RotationQuaternion< SCALAR >

using	Matrix4Tpl = Eigen::Matrix< SCALAR, 4, 4 >

using	Matrix3Tpl = Eigen::Matrix< SCALAR, 3, 3 >

using	Position3Tpl = kindr::Position< SCALAR, 3 >

using	Vector3Tpl = Eigen::Matrix< SCALAR, 3, 1 >

Public Member Functions
	RigidBodyPose (STORAGE_TYPE storage=EULER)
	default construct a RigidBodyPose More...

	RigidBodyPose (const kindr::EulerAnglesXyz< SCALAR > &orientationEulerXyz, const Position3Tpl &position, STORAGE_TYPE storage=EULER)
	construct a RigidBodyPose from Euler Angles and a position vector More...

	RigidBodyPose (const kindr::RotationQuaternion< SCALAR > &orientationQuat, const Position3Tpl &position, STORAGE_TYPE storage=EULER)
	construct a RigidBodyPose from a rotation quaternion and a position vector More...

	RigidBodyPose (const Eigen::Quaternion< SCALAR > &orientationQuat, const Eigen::Matrix< SCALAR, 3, 1 > &position, STORAGE_TYPE storage=EULER)
	construct a RigidBodyPose from a rotation quaternion and a position vector More...

	RigidBodyPose (const Matrix4Tpl &homTransform, STORAGE_TYPE storage=EULER)
	construct a RigidBodyPose from a homogeneous transformation matrix More...

	RigidBodyPose (const RigidBodyPose< SCALAR > &arg)
	copy-constructor for a RigidBodyPose More...

	~RigidBodyPose ()=default
	destructor for a rigid body pose More...

bool	isNear (const RigidBodyPose &rhs, const double &tol=1e-10) const

kindr::EulerAnglesXyz< SCALAR >	getEulerAnglesXyz () const
	This method returns the Euler angles rotation (X,Y,Z / roll,pitch,yaw). More...

kindr::RotationQuaternion< SCALAR >	getRotationQuaternion () const
	This method returns the quaternion rotation. More...

kindr::RotationMatrix< SCALAR >	getRotationMatrix () const
	This method returns the rotation matrix from base to inertia frame. This method returns which is the following rotation matrix $_iv = {_iR_b} {_bv}$ . More...

void	setFromEulerAnglesXyz (const kindr::EulerAnglesXyz< SCALAR > &eulerAngles)
	This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type. More...

void	setFromEulerAnglesXyz (const Vector3Tpl &eulerAngles)
	This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type. More...

void	setFromRotationQuaternion (const kindr::RotationQuaternion< SCALAR > &quat)
	This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr quaternion type. More...

void	setFromRotationQuaternion (const Eigen::Quaterniond &quat)
	This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from an Eigen quaternion. More...

void	setFromRotationMatrix (const kindr::RotationMatrix< SCALAR > &rotMat)
	This method sets the quaternion angles rotation from a kindr rotation matrix. More...

const Position3Tpl &	position () const
	This method returns the position of the Base frame in the inertia frame. More...

Position3Tpl &	position ()
	This method returns the position of the Base frame in the inertia frame. More...

RigidBodyPose< SCALAR >	inReferenceFrame (const RigidBodyPose< SCALAR > &ref_frame) const
	This method returns the Base frame in a custom Frame specified in the Inertia Frame. More...

template<class Vector3s >
Vector3s	rotateBaseToInertia (const Vector3s &vector) const
	This methods rotates a 3D vector expressed in Base frame to Inertia Frame. More...

Matrix3Tpl	rotateBaseToInertiaMat (const Matrix3Tpl &mat) const
	This methods rotates a 3D matrix expressed in Base frame to Inertia Frame. More...

kindr::RotationQuaternion< SCALAR >	rotateBaseToInertiaQuaternion (const kindr::RotationQuaternion< SCALAR > &q) const
	This methods rotates a quaternion expressed in Base frame to Inertia Frame. More...

template<class Vector3s >
Vector3s	rotateInertiaToBase (const Vector3s &vector) const
	This methods rotates a 3D vector expressed in Inertia frame to Base Frame. More...

HomogeneousTransform	getHomogeneousTransform () const
	This methods returns the Homogeneous transform from the Base frame to the inertia frame. More...

Vector3Tpl	computeGravityB (const Vector3Tpl &gravityW=gravity3DW()) const
	This methods returns the 3D gravity vector expressed in the Base frame. More...

Eigen::Matrix< SCALAR, 6, 1 >	computeGravityB6D (const Vector3Tpl &gravityW=gravity3DW()) const
	This methods returns the 6D gravity vector expressed in the Base frame. More...

void	setIdentity ()
	Sets orientation to identity and position to (0, 0, 0) More...

void	setRandom ()

STORAGE_TYPE	getStorageType () const

Static Public Member Functions
static Vector3Tpl	gravity3DW (SCALAR g=SCALAR(-9.81))

Detailed Description

template<typename SCALAR = double>
class ct::rbd::tpl::RigidBodyPose< SCALAR >

Pose of a single rigid body.

This implements the pose of a single rigid body. Internally, the pose is stored as either a quaternion (default) or Euler angles. Hence, the user is encouraged to use rotateBaseToInertia() or rotateInertiaToBase() instead of directly working with either representation. This will prevent unnecessary conversions.

Member Typedef Documentation

◆ HomogeneousTransform

template<typename SCALAR = double>

using ct::rbd::tpl::RigidBodyPose< SCALAR >::HomogeneousTransform = kindr::HomogeneousTransformationPosition3RotationQuaternion<SCALAR>

◆ Matrix4Tpl

template<typename SCALAR = double>

using ct::rbd::tpl::RigidBodyPose< SCALAR >::Matrix4Tpl = Eigen::Matrix<SCALAR, 4, 4>

◆ Matrix3Tpl

template<typename SCALAR = double>

using ct::rbd::tpl::RigidBodyPose< SCALAR >::Matrix3Tpl = Eigen::Matrix<SCALAR, 3, 3>

◆ Position3Tpl

template<typename SCALAR = double>

using ct::rbd::tpl::RigidBodyPose< SCALAR >::Position3Tpl = kindr::Position<SCALAR, 3>

◆ Vector3Tpl

template<typename SCALAR = double>

using ct::rbd::tpl::RigidBodyPose< SCALAR >::Vector3Tpl = Eigen::Matrix<SCALAR, 3, 1>

Member Enumeration Documentation

◆ STORAGE_TYPE

template<typename SCALAR = double>

enum ct::rbd::tpl::RigidBodyPose::STORAGE_TYPE

the orientation can either be stored as EulerAngles or as quaternion.

Todo:: reduce to quaternion only

Enumerator
QUAT
EULER

Constructor & Destructor Documentation

◆ RigidBodyPose() [1/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose ( STORAGE_TYPE storage = EULER )

inline

default construct a RigidBodyPose

◆ RigidBodyPose() [2/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose	(	const kindr::EulerAnglesXyz< SCALAR > &	orientationEulerXyz,
		const Position3Tpl &	position,
		STORAGE_TYPE	storage = `EULER`
	)

inline

construct a RigidBodyPose from Euler Angles and a position vector

◆ RigidBodyPose() [3/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose	(	const kindr::RotationQuaternion< SCALAR > &	orientationQuat,
		const Position3Tpl &	position,
		STORAGE_TYPE	storage = `EULER`
	)

inline

construct a RigidBodyPose from a rotation quaternion and a position vector

◆ RigidBodyPose() [4/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose	(	const Eigen::Quaternion< SCALAR > &	orientationQuat,
		const Eigen::Matrix< SCALAR, 3, 1 > &	position,
		STORAGE_TYPE	storage = `EULER`
	)

inline

construct a RigidBodyPose from a rotation quaternion and a position vector

◆ RigidBodyPose() [5/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose	(	const Matrix4Tpl &	homTransform,
		STORAGE_TYPE	storage = `EULER`
	)

inline

construct a RigidBodyPose from a homogeneous transformation matrix

◆ RigidBodyPose() [6/6]

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::RigidBodyPose ( const RigidBodyPose< SCALAR > & arg )

inline

copy-constructor for a RigidBodyPose

◆ ~RigidBodyPose()

template<typename SCALAR = double>

ct::rbd::tpl::RigidBodyPose< SCALAR >::~RigidBodyPose ( )

default

destructor for a rigid body pose

Referenced by ct::rbd::tpl::RigidBodyPose< double >::RigidBodyPose().

Member Function Documentation

◆ isNear()

template<typename SCALAR = double>

bool ct::rbd::tpl::RigidBodyPose< SCALAR >::isNear	(	const RigidBodyPose< SCALAR > &	rhs,
		const double &	tol = `1e-10`
	)		const

inline

Referenced by testEqual(), and testNotEqual().

◆ getEulerAnglesXyz()

template<typename SCALAR = double>

kindr::EulerAnglesXyz<SCALAR> ct::rbd::tpl::RigidBodyPose< SCALAR >::getEulerAnglesXyz ( ) const

inline

This method returns the Euler angles rotation (X,Y,Z / roll,pitch,yaw).

Referenced by ct::rbd::tpl::RigidBodyPose< double >::inReferenceFrame(), TEST(), testEqual(), testFunctionRBDPose(), and testNotEqual().

◆ getRotationQuaternion()

template<typename SCALAR = double>

kindr::RotationQuaternion<SCALAR> ct::rbd::tpl::RigidBodyPose< SCALAR >::getRotationQuaternion ( ) const

inline

This method returns the quaternion rotation.

Referenced by ct::rbd::IDControllerFB< Dynamics >::computeDesiredAcceleration(), ct::rbd::Kinematics< RBD, NEE >::getEEPoseInWorld(), ct::rbd::tpl::RigidBodyPose< double >::inReferenceFrame(), ct::rbd::tpl::RigidBodyPose< double >::isNear(), ct::rbd::IKCostEvaluator< KINEMATICS, SCALAR >::setTargetPose(), TEST(), testEqual(), and testNotEqual().

◆ getRotationMatrix()

template<typename SCALAR = double>

kindr::RotationMatrix<SCALAR> ct::rbd::tpl::RigidBodyPose< SCALAR >::getRotationMatrix ( ) const

inline

This method returns the rotation matrix from base to inertia frame. This method returns $ _iR_b $ which is the following rotation matrix $_iv = {_iR_b} {_bv}$ .

Referenced by ct::rbd::tpl::RigidBodyPose< double >::rotateBaseToInertia(), ct::rbd::tpl::RigidBodyPose< double >::rotateBaseToInertiaMat(), TEST(), testEqual(), testNotEqual(), and ct::rbd::tpl::OperationalModelRBD< RBDContainer, NUM_CONTACTPOINTS, SCALAR >::update().

◆ setFromEulerAnglesXyz() [1/2]

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setFromEulerAnglesXyz ( const kindr::EulerAnglesXyz< SCALAR > & eulerAngles )

inline

This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type.

Referenced by ct::rbd::tpl::RigidBodyPose< double >::RigidBodyPose(), TEST(), and testFunctionRBDPose().

◆ setFromEulerAnglesXyz() [2/2]

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setFromEulerAnglesXyz ( const Vector3Tpl & eulerAngles )

inline

This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type.

◆ setFromRotationQuaternion() [1/2]

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setFromRotationQuaternion ( const kindr::RotationQuaternion< SCALAR > & quat )

inline

This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr quaternion type.

Referenced by ct::rbd::tpl::RigidBodyPose< double >::RigidBodyPose(), and TEST().

◆ setFromRotationQuaternion() [2/2]

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setFromRotationQuaternion ( const Eigen::Quaterniond & quat )

inline

This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from an Eigen quaternion.

◆ setFromRotationMatrix()

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setFromRotationMatrix ( const kindr::RotationMatrix< SCALAR > & rotMat )

inline

This method sets the quaternion angles rotation from a kindr rotation matrix.

Referenced by ct::rbd::tpl::RigidBodyPose< double >::RigidBodyPose().

◆ position() [1/2]

template<typename SCALAR = double>

const Position3Tpl& ct::rbd::tpl::RigidBodyPose< SCALAR >::position ( ) const

inline

This method returns the position of the Base frame in the inertia frame.

Referenced by ct::rbd::IDControllerFB< Dynamics >::computeDesiredAcceleration(), ct::rbd::Kinematics< RBD, NEE >::getEEPoseInWorld(), ct::rbd::Kinematics< RBD, NEE >::getEEPositionInWorld(), ct::rbd::tpl::RigidBodyPose< double >::inReferenceFrame(), ct::rbd::tpl::RigidBodyPose< double >::isNear(), ct::rbd::Kinematics< RBD, NEE >::mapForceFromEEToLink(), ct::rbd::tpl::RigidBodyPose< double >::setIdentity(), ct::rbd::tpl::RigidBodyPose< double >::setRandom(), ct::rbd::IKCostEvaluator< KINEMATICS, SCALAR >::setTargetPose(), TEST(), testEqual(), and testFunctionRBDPose().

◆ position() [2/2]

template<typename SCALAR = double>

Position3Tpl& ct::rbd::tpl::RigidBodyPose< SCALAR >::position ( )

inline

This method returns the position of the Base frame in the inertia frame.

◆ inReferenceFrame()

template<typename SCALAR = double>

RigidBodyPose<SCALAR> ct::rbd::tpl::RigidBodyPose< SCALAR >::inReferenceFrame ( const RigidBodyPose< SCALAR > & ref_frame ) const

inline

This method returns the Base frame in a custom Frame specified in the Inertia Frame.

◆ rotateBaseToInertia()

template<typename SCALAR = double>

template<class Vector3s >

Vector3s ct::rbd::tpl::RigidBodyPose< SCALAR >::rotateBaseToInertia ( const Vector3s & vector ) const

inline

This methods rotates a 3D vector expressed in Base frame to Inertia Frame.

Referenced by TEST().

◆ rotateBaseToInertiaMat()

template<typename SCALAR = double>

Matrix3Tpl ct::rbd::tpl::RigidBodyPose< SCALAR >::rotateBaseToInertiaMat ( const Matrix3Tpl & mat ) const

inline

This methods rotates a 3D matrix expressed in Base frame to Inertia Frame.

◆ rotateBaseToInertiaQuaternion()

template<typename SCALAR = double>

kindr::RotationQuaternion<SCALAR> ct::rbd::tpl::RigidBodyPose< SCALAR >::rotateBaseToInertiaQuaternion ( const kindr::RotationQuaternion< SCALAR > & q ) const

inline

This methods rotates a quaternion expressed in Base frame to Inertia Frame.

◆ rotateInertiaToBase()

template<typename SCALAR = double>

template<class Vector3s >

Vector3s ct::rbd::tpl::RigidBodyPose< SCALAR >::rotateInertiaToBase ( const Vector3s & vector ) const

inline

This methods rotates a 3D vector expressed in Inertia frame to Base Frame.

Referenced by ct::rbd::IDControllerFB< Dynamics >::computeDesiredAcceleration(), ct::rbd::tpl::RigidBodyPose< double >::computeGravityB(), ct::rbd::tpl::RigidBodyPose< double >::computeGravityB6D(), and TEST().

◆ getHomogeneousTransform()

template<typename SCALAR = double>

HomogeneousTransform ct::rbd::tpl::RigidBodyPose< SCALAR >::getHomogeneousTransform ( ) const

inline

This methods returns the Homogeneous transform from the Base frame to the inertia frame.

◆ gravity3DW()

template<typename SCALAR = double>

static Vector3Tpl ct::rbd::tpl::RigidBodyPose< SCALAR >::gravity3DW ( SCALAR g = SCALAR(-9.81) )

inlinestatic

Returns gravity vector in world (0.0, 0.0, -9.81)

Returns

◆ computeGravityB()

template<typename SCALAR = double>

Vector3Tpl ct::rbd::tpl::RigidBodyPose< SCALAR >::computeGravityB ( const Vector3Tpl & gravityW = gravity3DW() ) const

inline

This methods returns the 3D gravity vector expressed in the Base frame.

Referenced by TEST(), testEqual(), and testNotEqual().

◆ computeGravityB6D()

template<typename SCALAR = double>

Eigen::Matrix<SCALAR, 6, 1> ct::rbd::tpl::RigidBodyPose< SCALAR >::computeGravityB6D ( const Vector3Tpl & gravityW = gravity3DW() ) const

inline

This methods returns the 6D gravity vector expressed in the Base frame.

Referenced by ct::rbd::Dynamics< RBD, NEE >::FloatingBaseForwardDynamics(), ct::rbd::Dynamics< RBD, NEE >::FloatingBaseFullyActuatedID(), ct::rbd::Dynamics< RBD, NEE >::FloatingBaseID(), ct::rbd::ProjectedDynamics< RBD, NEE >::ProjectedDynamics(), TEST(), testEqual(), testNotEqual(), and ct::rbd::tpl::OperationalModelRBD< RBDContainer, NUM_CONTACTPOINTS, SCALAR >::update().

◆ setIdentity()

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setIdentity ( )

inline

Sets orientation to identity and position to (0, 0, 0)

Referenced by ct::rbd::InverseKinematicsBase< 6, SCALAR >::computeInverseKinematicsCloseTo(), and TEST().

◆ setRandom()

template<typename SCALAR = double>

void ct::rbd::tpl::RigidBodyPose< SCALAR >::setRandom ( )

inline

Referenced by TEST().

◆ getStorageType()

template<typename SCALAR = double>

STORAGE_TYPE ct::rbd::tpl::RigidBodyPose< SCALAR >::getStorageType ( ) const

inline

Referenced by TEST().

The documentation for this class was generated from the following file:

/home/gim2rng/ct_devel_ws/src/control-toolbox/ct_rbd/include/ct/rbd/state/RigidBodyPose.h

Public Types

Public Member Functions

Static Public Member Functions

Detailed Description

template<typename SCALAR = double> class ct::rbd::tpl::RigidBodyPose< SCALAR >

Member Typedef Documentation

◆ HomogeneousTransform

◆ Matrix4Tpl

◆ Matrix3Tpl

◆ Position3Tpl

◆ Vector3Tpl

Member Enumeration Documentation

◆ STORAGE_TYPE

Constructor & Destructor Documentation

◆ RigidBodyPose() [1/6]

◆ RigidBodyPose() [2/6]

◆ RigidBodyPose() [3/6]

◆ RigidBodyPose() [4/6]

◆ RigidBodyPose() [5/6]

◆ RigidBodyPose() [6/6]

◆ ~RigidBodyPose()

Member Function Documentation

◆ isNear()

◆ getEulerAnglesXyz()

◆ getRotationQuaternion()

◆ getRotationMatrix()

◆ setFromEulerAnglesXyz() [1/2]

◆ setFromEulerAnglesXyz() [2/2]

◆ setFromRotationQuaternion() [1/2]

◆ setFromRotationQuaternion() [2/2]

◆ setFromRotationMatrix()

◆ position() [1/2]

◆ position() [2/2]

◆ inReferenceFrame()

◆ rotateBaseToInertia()

◆ rotateBaseToInertiaMat()

◆ rotateBaseToInertiaQuaternion()

◆ rotateInertiaToBase()

◆ getHomogeneousTransform()

◆ gravity3DW()

◆ computeGravityB()

◆ computeGravityB6D()

◆ setIdentity()

◆ setRandom()

◆ getStorageType()

template<typename SCALAR = double>
class ct::rbd::tpl::RigidBodyPose< SCALAR >