Implements integrators for the kinematics, in terms or rotations and translations. More...

#include <Eigen/SVD>
#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>
#include <state-observation/tools/miscellaneous-algorithms.hpp>
#include <state-observation/tools/probability-law-simulation.hpp>
#include <state-observation/tools/rigid-body-kinematics.hxx>

Include dependency graph for rigid-body-kinematics.hpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	stateObservation::kine::indexes< rotationType >
struct	stateObservation::kine::indexes< rotationVector >
struct	stateObservation::kine::indexes< quaternion >
class	stateObservation::kine::Orientation
class	stateObservation::kine::internal::KinematicsInternal< T >
struct	stateObservation::kine::internal::KinematicsInternal< T >::Flags
struct	stateObservation::kine::Kinematics
	Class facilitating the manipulation of the kinematics of a frame within another and the associated operations. More...
struct	stateObservation::kine::LocalKinematics
	Class facilitating the manipulation of the local kinematics of a frame within another and the associated operations. More...

Namespaces
namespace	stateObservation
	Filtering of divergent component of motion (DCM) and estimation of a bias betweeen the DCM and the corresponding zero moment point for a linearized inverted pendulum model.

Enumerations
enum	rotationType { matrix = 0 , rotationVector = 1 , quaternion = 2 , angleaxis = 3 }

Functions
void	stateObservation::kine::integrateKinematics (Vector3 &position, const Vector3 &velocity, double dt)
void	stateObservation::kine::integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, double dt)
void	stateObservation::kine::integrateKinematics (Matrix3 &orientation, const Vector3 &rotationVelocity, double dt)
void	stateObservation::kine::integrateKinematics (Matrix3 &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	stateObservation::kine::integrateKinematics (Quaternion &orientation, const Vector3 &rotationVelocity, double dt)
void	stateObservation::kine::integrateKinematics (Quaternion &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	stateObservation::kine::integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, Matrix3 &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	stateObservation::kine::integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, Quaternion &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	stateObservation::kine::integrateKinematics (Vector3 &position, const Vector3 &velocity, Matrix3 &orientation, const Vector3 &rotationVelocity, double dt)
	integrates the postition/orientation given the velocities
void	stateObservation::kine::integrateKinematics (Vector3 &position, const Vector3 &velocity, Quaternion &orientation, const Vector3 &rotationVelocity, double dt)
	integrates the postition/orientation given the velocities
Vector	stateObservation::kine::regulateRotationVector (const Vector3 &v)
AngleAxis	stateObservation::kine::rotationVectorToAngleAxis (const Vector3 &v)
	Transforms the rotation vector into angle axis.
Matrix3	stateObservation::kine::rotationVectorToRotationMatrix (const Vector3 &v)
	Transforms the rotation vector into rotation matrix.
Quaternion	stateObservation::kine::rotationVectorToQuaternion (const Vector3 &v)
	Transforms the rotation vector into quaternion.
Vector3	stateObservation::kine::rotationMatrixToRotationVector (const Matrix3 &R)
	Transforms the rotation matrix into rotation vector.
Vector3	stateObservation::kine::quaternionToRotationVector (const Quaternion &q)
	Tranbsform a quaternion into rotation vector.
Vector3	stateObservation::kine::quaternionToRotationVector (const Vector4 &v)
	Transforms a quaternion into rotation vector.
double	stateObservation::kine::scalarComponent (const Quaternion &q)
	scalar component of a quaternion
Vector3	stateObservation::kine::vectorComponent (const Quaternion &q)
	vector part of the quaternion
Vector3	stateObservation::kine::rotationMatrixToRollPitchYaw (const Matrix3 &R, Vector3 &v)
Vector3	stateObservation::kine::rotationMatrixToRollPitchYaw (const Matrix3 &R)
Matrix3	stateObservation::kine::rollPitchYawToRotationMatrix (double roll, double pitch, double yaw)
Matrix3	stateObservation::kine::rollPitchYawToRotationMatrix (const Vector3 &rpy)
Quaternion	stateObservation::kine::rollPitchYawToQuaternion (double roll, double pitch, double yaw)
Quaternion	stateObservation::kine::rollPitchYawToQuaternion (const Vector3 &rpy)
Matrix3	stateObservation::kine::orthogonalizeRotationMatrix (const Matrix3 &M)
	Projects the Matrix to so(3).
Matrix3	stateObservation::kine::skewSymmetric (const Vector3 &v, Matrix3 &R)
	transform a 3d vector into a skew symmetric 3x3 matrix
Matrix3	stateObservation::kine::skewSymmetric (const Vector3 &v)
	transform a 3d vector into a skew symmetric 3x3 matrix
Matrix3	stateObservation::kine::skewSymmetric2 (const Vector3 &v, Matrix3 &R)
	transform a 3d vector into a squared skew symmetric 3x3 matrix
Matrix3	stateObservation::kine::skewSymmetric2 (const Vector3 &v)
	transform a 3d vector into a squared skew symmetric 3x3 matrix
Vector6	stateObservation::kine::homogeneousMatrixToVector6 (const Matrix4 &M)
	transforms a homogeneous matrix into 6d vector (position theta mu)
Matrix4	stateObservation::kine::vector6ToHomogeneousMatrix (const Vector6 &v)
	transforms a 6d vector (position theta mu) into a homogeneous matrix
Matrix3	stateObservation::kine::twoVectorsToRotationMatrix (const Vector3 &v1, const Vector3 Rv1)
	Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1 This is based on Rodrigues formula.
bool	stateObservation::kine::isPureYaw (const Matrix3 &R)
	checks if this matrix is a pure yaw matrix or not
Vector3	stateObservation::kine::getInvariantHorizontalVector (const Matrix3 &R)
	Gets a vector that remains horizontal with this rotation. This vector is NOT normalized.
Vector3	stateObservation::kine::getInvariantOrthogonalVector (const Matrix3 &Rhat, const Vector3 &Rtez)
	Gets a vector \(v\) that is orthogonal to \(e_z\) and such that \(\hat{R}^T e_z\) is orthogonal to the tilt \(R^T e_z\). This vector is NOT normalized.
Matrix3	stateObservation::kine::mergeTiltWithYaw (const Vector3 &Rtez, const Matrix3 &R2, const Vector3 &v=Vector3::UnitX()) noexcept(false)
	Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector).
Matrix3	stateObservation::kine::mergeRoll1Pitch1WithYaw2 (const Matrix3 &R1, const Matrix3 &R2, const Vector3 &v=Vector3::UnitX())
	Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector).
Matrix3	stateObservation::kine::mergeTiltWithYawAxisAgnostic (const Vector3 &Rtez, const Matrix3 &R2)
	Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector).
Matrix3	stateObservation::kine::mergeRoll1Pitch1WithYaw2AxisAgnostic (const Matrix3 &R1, const Matrix3 &R2)
	Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector.
double	stateObservation::kine::rotationMatrixToAngle (const Matrix3 &rotation, const Vector3 &axis, const Vector3 &v)
	take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this rotation
double	stateObservation::kine::rotationMatrixToYaw (const Matrix3 &rotation, const Vector2 &v)
	take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical axis with this rotation
double	stateObservation::kine::rotationMatrixToYaw (const Matrix3 &rotation)
	take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation
double	stateObservation::kine::rotationMatrixToYawAxisAgnostic (const Matrix3 &rotation)
	take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
Quaternion	stateObservation::kine::zeroRotationQuaternion ()
	Get the Identity Quaternion.
Quaternion	stateObservation::kine::randomRotationQuaternion ()
	Get a uniformly random Quaternion.
double	stateObservation::kine::randomAngle ()
	get a randomAngle between -pi and pu
bool	stateObservation::kine::isRotationMatrix (const Matrix3 &, double precision=2 *cst::epsilon1)
	Checks if it is a rotation matrix (right-hand orthonormal) or not.
void	stateObservation::kine::fixedPointRotationToTranslation (const Matrix3 &R, const Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, const Vector3 &fixedPoint, Vector3 &outputTranslation, Vector3 &outputLinearVelocity, Vector3 &outputLinearAcceleration)
	transforms a rotation into translation given a constraint of a fixed point
Vector3	stateObservation::kine::derivateRotationFD (const Quaternion &q1, const Quaternion &q2, double dt)
	derivates a quaternion using finite difference to get a angular velocity vector
Vector3	stateObservation::kine::derivateRotationFD (const Vector3 &o1, const Vector3 &o2, double dt)
	derivates a rotation vector using finite difference to get a angular velocity vector
Vector6	stateObservation::kine::derivateHomogeneousMatrixFD (const Matrix4 &m1, const Matrix4 &m2, double dt)
Vector6	stateObservation::kine::derivatePoseThetaUFD (const Vector6 &v1, const Vector6 &v2, double dt)
void	stateObservation::kine::derivateRotationMultiplicative (const Vector3 &deltaR, Matrix3 &dRdR, Matrix3 &dRddeltaR)
Matrix3	stateObservation::kine::derivateRtvMultiplicative (const Matrix3 &R, const Vector3 &v)
IndexedVectorArray	stateObservation::kine::reconstructStateTrajectory (const IndexedVectorArray &positionOrientation, double dt)
Vector	stateObservation::kine::invertState (const Vector &state)
Matrix4	stateObservation::kine::invertHomoMatrix (const Matrix4 &m)
std::ostream &	operator<< (std::ostream &os, const stateObservation::kine::Kinematics &k)
std::ostream &	operator<< (std::ostream &os, const stateObservation::kine::LocalKinematics &k)

Variables
constexpr double	stateObservation::kine::quatNormTol = 1e-6
	relative tolereance to the square of quaternion norm.

Detailed Description

Implements integrators for the kinematics, in terms or rotations and translations.

Author: Mehdi Benallegue

Date: 2013

Definition in file rigid-body-kinematics.hpp.

Enumeration Type Documentation

◆ rotationType

enum stateObservation::kine::rotationType

Definition at line 310 of file rigid-body-kinematics.hpp.

Function Documentation

◆ integrateKinematics() [1/2]

void stateObservation::kine::integrateKinematics	(	Vector3 &	position,
		Vector3 &	velocity,
		const Vector3 &	acceleration,
		Matrix3 &	orientation,
		Vector3 &	rotationVelocity,
		const Vector3 &	rotationAcceleration,
		double	dt )

inline

integrates the position/orientation and their time derivatives, given the accelerations, and initial velocities and positions. The rotations are expressed by rotation matrix

◆ integrateKinematics() [2/2]

void stateObservation::kine::integrateKinematics	(	Vector3 &	position,
		Vector3 &	velocity,
		const Vector3 &	acceleration,
		Quaternion &	orientation,
		Vector3 &	rotationVelocity,
		const Vector3 &	rotationAcceleration,
		double	dt )

inline

integrates the position/orientation and their time derivatives, given the accelerations, and initial velocities and positions. The orientations are expressed by quaternions

◆ regulateRotationVector()

Vector stateObservation::kine::regulateRotationVector ( const Vector3 & v )

inline

Puts the orientation vector norm between 0 and Pi if it gets close to 2pi

◆ rotationMatrixToRollPitchYaw()

Vector3 stateObservation::kine::rotationMatrixToRollPitchYaw	(	const Matrix3 &	R,
		Vector3 &	v )

inline

Transforms the rotation matrix into roll pitch yaw (decompose R into Ry*Rp*Rr)

◆ rollPitchYawToRotationMatrix()

Matrix3 stateObservation::kine::rollPitchYawToRotationMatrix	(	double	roll,
		double	pitch,
		double	yaw )

inline

Transform the roll pitch yaw into rotation matrix ( R = Ry*Rp*Rr)

◆ rollPitchYawToQuaternion()

Quaternion stateObservation::kine::rollPitchYawToQuaternion	(	double	roll,
		double	pitch,
		double	yaw )

inline

Transform the roll pitch yaw into rotation matrix ( R = Ry*Rp*Rr)

◆ twoVectorsToRotationMatrix()

Matrix3 stateObservation::kine::twoVectorsToRotationMatrix	(	const Vector3 &	v1,
		const Vector3	Rv1 )

inline

Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1 This is based on Rodrigues formula.

Parameters

v1	the NORMALIZED vector
Rv1	the NORMALIZED image of this vector by the rotation matrix R

Returns: Matrix3 the rotation matrix R

◆ isPureYaw()

bool stateObservation::kine::isPureYaw ( const Matrix3 & R )

inline

checks if this matrix is a pure yaw matrix or not

Parameters

R	the rotation matrix

Returns: true is pure yaw; false is not pure yaw

◆ getInvariantHorizontalVector()

Vector3 stateObservation::kine::getInvariantHorizontalVector ( const Matrix3 & R )

inline

Gets a vector that remains horizontal with this rotation. This vector is NOT normalized.

There is a general version in getInvariantOrthogonalVector(). This can be used to extract yaw angle from a rotation matrix without needing to specify an order in the tils (e.g. roll then pich).

Parameters

R	the input rotation

Returns: Vector3 the output horizontal vector

◆ getInvariantOrthogonalVector()

Vector3 stateObservation::kine::getInvariantOrthogonalVector	(	const Matrix3 &	Rhat,
		const Vector3 &	Rtez )

inline

Gets a vector \(v\) that is orthogonal to \(e_z\) and such that \(\hat{R}^T e_z\) is orthogonal to the tilt \(R^T e_z\). This vector is NOT normalized.

This is a generalization of getInvariantHorizontalVector() which corresponds to no tilt \(\hat{R}^T e_z=e_z\). This function is useful to merge the yaw from the rotation matrix with the tilt.

Parameters

Rhat	the input rotation matrix \(\hat{R}^T\)
Rtez	the input tilt \(\hat{R}^T e_z\)

Returns: Vector3 the output horizontal vector

◆ mergeTiltWithYaw()

Matrix3 stateObservation::kine::mergeTiltWithYaw	(	const Vector3 &	Rtez,
		const Matrix3 &	R2,
		const Vector3 &	v = Vector3::UnitX() )

inline

Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector).

throws exception when the orientation is singlular (likely gimbal lock) to avoid these issues, we recommend to use mergeTiltWithYawAxisAgnostic()

Parameters

Rtez	the tilt \(R_1^T e_z\) (the local image of \(e_z\) unit vector)
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference it must be horizontal and normalized (for a traditional yaw v is by deftault \(e_x\))

Returns: Matrix3 the merged rotation matrix

◆ mergeRoll1Pitch1WithYaw2()

Matrix3 stateObservation::kine::mergeRoll1Pitch1WithYaw2	(	const Matrix3 &	R1,
		const Matrix3 &	R2,
		const Vector3 &	v = Vector3::UnitX() )

inline

Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector).

Parameters

R1	is the first rotation to get the roll and pitch
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns: Matrix3 the merged rotation matrix

◆ mergeTiltWithYawAxisAgnostic()

Matrix3 stateObservation::kine::mergeTiltWithYawAxisAgnostic	(	const Vector3 &	Rtez,
		const Matrix3 &	R2 )

inline

Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector).

Parameters

Rtez	the tilt \(R_1^T e_z\) (the local image of \(e_z\) unit vector)
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns: Matrix3 the merged rotation matrix

◆ mergeRoll1Pitch1WithYaw2AxisAgnostic()

Matrix3 stateObservation::kine::mergeRoll1Pitch1WithYaw2AxisAgnostic	(	const Matrix3 &	R1,
		const Matrix3 &	R2 )

inline

Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector.

Parameters

R1	is the first rotation to get the roll and pitch
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns: Matrix3 the merged rotation matrix

◆ rotationMatrixToAngle()

double stateObservation::kine::rotationMatrixToAngle	(	const Matrix3 &	rotation,
		const Vector3 &	axis,
		const Vector3 &	v )

inline

take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this rotation

Parameters

rotation	The 3x3 rotation matrix
axis	the axis of rotation (must be normalized)
v	the vector that is rotated with the rotation (must be orthogonal to axis and normalized)

Returns: double the angle

◆ rotationMatrixToYaw() [1/2]

double stateObservation::kine::rotationMatrixToYaw	(	const Matrix3 &	rotation,
		const Vector2 &	v )

inline

take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical axis with this rotation

this is a generalization of yaw extraction (yaw is equivalent to v = Matrix3::UnitX(), but it is more efficiant to calll the dedicated rotationMatrixToYaw() without vector parameter).

Parameters

rotation	The 3x3 rotation matrix
v	the rotated vector (expressed in the horizontal plane, must be normalized)

Returns: double the angle

◆ rotationMatrixToYaw() [2/2]

double stateObservation::kine::rotationMatrixToYaw ( const Matrix3 & rotation )

inline

take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation

Parameters

rotation The 3x3 rotation matrix

Returns: double the angle

◆ rotationMatrixToYawAxisAgnostic()

double stateObservation::kine::rotationMatrixToYawAxisAgnostic ( const Matrix3 & rotation )

inline

take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis

This is similar to yaw angle but here we identify a horizontal vector that stays horizontal after rotation. this can be called axis agnostic yaw extraction. and get the angle between them

Parameters

rotation The 3x3 rotation matrix

Returns: double the angle

◆ zeroRotationQuaternion()

Quaternion stateObservation::kine::zeroRotationQuaternion ( )

inline

Get the Identity Quaternion.

Returns: Quaternion

◆ randomRotationQuaternion()

Quaternion stateObservation::kine::randomRotationQuaternion ( )

inline

Get a uniformly random Quaternion.

Returns: Quaternion

◆ randomAngle()

double stateObservation::kine::randomAngle ( )

inline

get a randomAngle between -pi and pu

Returns: double the random angle

◆ isRotationMatrix()

bool stateObservation::kine::isRotationMatrix	(	const Matrix3 &	,
		double	precision = 2 *cst::epsilon1 )

inline

Checks if it is a rotation matrix (right-hand orthonormal) or not.

Parameters

precision the absolute precision of the test

Returns: true when it is a rotation matrix; false when not

◆ derivateRotationMultiplicative()

void stateObservation::kine::derivateRotationMultiplicative	(	const Vector3 &	deltaR,
		Matrix3 &	dRdR,
		Matrix3 &	dRddeltaR )

inline

Computes the "multiplicative Jacobian" for Kalman filtering for example orientation is the current orientation dR is the rotation delta between the current orientation and the orientation at the next step. dRdR is the "multiplicative" Jacobian with regard to variations of orientation dRddeltaR is the "multiplicative" Jacobian with regard to variations of deltaR

◆ derivateRtvMultiplicative()

Matrix3 stateObservation::kine::derivateRtvMultiplicative	(	const Matrix3 &	R,
		const Vector3 &	v )

inline

Computes the "multiplicative Jacobian" for a function R^T.v giving a vector v expressed in a local frame with regard to Rotations of this local frame

◆ reconstructStateTrajectory()

IndexedVectorArray stateObservation::kine::reconstructStateTrajectory	(	const IndexedVectorArray &	positionOrientation,
		double	dt )

inline

uses the derivation to reconstruct the velocities and accelerations given trajectories in positions and orientations only

Variable Documentation

◆ quatNormTol

double stateObservation::kine::quatNormTol = 1e-6

constexpr

relative tolereance to the square of quaternion norm.

Definition at line 352 of file rigid-body-kinematics.hpp.

Classes

Namespaces

Enumerations

Functions

Variables

Detailed Description

Enumeration Type Documentation

◆ rotationType

Function Documentation

◆ integrateKinematics() [1/2]

◆ integrateKinematics() [2/2]

◆ regulateRotationVector()

◆ rotationMatrixToRollPitchYaw()

◆ rollPitchYawToRotationMatrix()

◆ rollPitchYawToQuaternion()

◆ twoVectorsToRotationMatrix()

◆ isPureYaw()

◆ getInvariantHorizontalVector()

◆ getInvariantOrthogonalVector()

◆ mergeTiltWithYaw()

◆ mergeRoll1Pitch1WithYaw2()

◆ mergeTiltWithYawAxisAgnostic()

◆ mergeRoll1Pitch1WithYaw2AxisAgnostic()

◆ rotationMatrixToAngle()

◆ rotationMatrixToYaw() [1/2]

◆ rotationMatrixToYaw() [2/2]

◆ rotationMatrixToYawAxisAgnostic()

◆ zeroRotationQuaternion()

◆ randomRotationQuaternion()

◆ randomAngle()

◆ isRotationMatrix()

◆ derivateRotationMultiplicative()

◆ derivateRtvMultiplicative()

◆ reconstructStateTrajectory()

Variable Documentation

◆ quatNormTol