Go to the documentation of this file.
13 #ifndef StATEOBSERVATIONRIGIDBODYKINEMATICS_H
14 #define StATEOBSERVATIONRIGIDBODYKINEMATICS_H
18 #include <state-observation/api.h>
35 const Vector3 & rotationAcceleration,
42 const Vector3 & rotationAcceleration,
53 const Vector3 & rotationAcceleration,
64 const Vector3 & rotationAcceleration,
71 const Vector3 & rotationVelocity,
78 const Vector3 & rotationVelocity,
192 const Vector3 & v = Vector3::UnitX()) noexcept(
false);
271 const Vector3 & rotationVelocity,
272 const Vector3 & rotationAcceleration,
275 Vector3 & outputLinearVelocity,
276 Vector3 & outputLinearAcceleration);
318 template<rotationType = rotationVector>
373 Orientation(
const double & roll,
const double & pitch,
const double & yaw);
391 template<
typename t = Quaternion>
398 inline operator const Matrix3 &()
const;
450 inline bool isSet()
const;
488 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
560 template<
typename t = Quaternion>
666 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
720 template<
typename t = Quaternion>
765 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
782 #include <state-observation/tools/rigid-body-kinematics.hxx>
784 #endif // StATEOBSERVATIONRIGIDBODYKINEMATICS_H
Eigen::VectorXd Vector
Dynamic sized scalar vector.
Definition: definitions.hpp:76
const LocalKinematics & integrate(double dt)
integrates the current local kinematics over the timestep dt.
Kinematics()
Definition: rigid-body-kinematics.hpp:587
@ angleaxis
Definition: rigid-body-kinematics.hpp:315
constexpr double quatNormTol
relative tolereance to the square of quaternion norm.
Definition: rigid-body-kinematics.hpp:352
Vector regulateRotationVector(const Vector3 &v)
Eigen::Matrix4d Matrix4
4x4 Scalar Matrix
Definition: definitions.hpp:115
Vector3 toRollPitchYaw() const
static const Byte linAcc
Definition: rigid-body-kinematics.hpp:530
Kinematics operator*(const Kinematics &) const
composition of transformation
static const Byte acc
Definition: rigid-body-kinematics.hpp:535
Matrix3 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...
Matrix3 rollPitchYawToRotationMatrix(double roll, double pitch, double yaw)
Quaternion randomRotationQuaternion()
Get a uniformly random Quaternion.
CheckedMatrix3 m_
Definition: rigid-body-kinematics.hpp:497
CheckedVector3 position
Definition: rigid-body-kinematics.hpp:539
static T zeroKinematics(typename Flags::Byte=Flags::all)
returns an object corresponding to zero kinematics on the desired variables.
Orientation inverse() const
const Orientation & setToProductNoAlias(const Orientation &R1, const Orientation &R2)
Noalias versions of the operator*.
Vector3 tempVec_5
Definition: rigid-body-kinematics.hpp:772
Orientation(bool initialize=true)
Eigen::Matrix< double, 2, 1 > Vector2
2d Vector
Definition: definitions.hpp:82
Vector4 toVector4() const
AngleAxis rotationVectorToAngleAxis(const Vector3 &v)
Transforms the rotation vector into angle axis.
Matrix3 rotationVectorToRotationMatrix(const Vector3 &v)
Transforms the rotation vector into rotation matrix.
Definition: rigid-body-kinematics.hpp:319
Vector3 tempVec_3
Definition: rigid-body-kinematics.hpp:770
Matrix3 skewSymmetric2(const Vector3 &v, Matrix3 &R)
transform a 3d vector into a squared skew symmetric 3x3 matrix
Matrix3 skewSymmetric(const Vector3 &v, Matrix3 &R)
transform a 3d vector into a skew symmetric 3x3 matrix
static const Byte vel
Definition: rigid-body-kinematics.hpp:534
Definition: rigid-body-kinematics.hpp:522
Matrix3 mergeRoll1Pitch1WithYaw2AxisAgnostic(const Matrix3 &R1, const Matrix3 &R2)
Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector.
Vector3 getInvariantHorizontalVector(const Matrix3 &R)
Gets a vector that remains horizontal with this rotation. This vector is NOT normalized.
LocalKinematics()
Definition: rigid-body-kinematics.hpp:679
const Orientation & integrateRightSide(Vector3 dt_x_omega)
static const Byte linVel
Definition: rigid-body-kinematics.hpp:528
T & fromVector(const Vector &v, typename Flags::Byte=Flags::all)
Orientation & operator=(const Vector3 &v)
bool isPureYaw(const Matrix3 &R)
checks if this matrix is a pure yaw matrix or not
CheckedVector3 linAcc
Definition: rigid-body-kinematics.hpp:545
IndexedVectorArray reconstructStateTrajectory(const IndexedVectorArray &positionOrientation, double dt)
Matrix3 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...
Vector3 toRotationVector() const
Class facilitating the manipulation of the kinematics of a frame within another and the associated op...
Definition: rigid-body-kinematics.hpp:584
const LocalKinematics & update(const LocalKinematics &newValue, double dt, Flags::Byte=Flags::all)
updates the current local kinematics (k) with the new ones (k+1).
LocalKinematics & setToDiffNoAliasAngPart(const LocalKinematics &multiplier1, const LocalKinematics &multiplier2)
Angular part of the setToDiffNoAlias(const LocalKinematics &, const LocalKinematics &) function.
void synchronize()
synchronizes the representations (quaternion and rotation matrix)
double rotationMatrixToYawAxisAgnostic(const Matrix3 &rotation)
take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
void 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 differentiateRightSide(Orientation R_k1) const
gives the log (rotation vector) of the "right-side" difference of orientation: log of (*this)....
const Matrix3 & toMatrix3() const
get a const reference on the matrix or the quaternion
static const Byte position
Definition: rigid-body-kinematics.hpp:526
Orientation & setRandom()
rotationType
Definition: rigid-body-kinematics.hpp:310
const Orientation & integrate(Vector3 dt_x_omega)
Kinematics & setToDiffNoAliasAngPart(const Kinematics &multiplier1, const Kinematics &multiplier2)
Angular part of the setToDiffNoAlias(const Kinematics &, const Kinematics &) function.
LocalKinematics operator*(const LocalKinematics &) const
composition of transformation
double rotationMatrixToYaw(const Matrix3 &rotation, const Vector2 &v)
take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vert...
Kinematics & setToProductNoAlias(const Kinematics &operand1, const Kinematics &operand2)
computes the composition of two Kinematics object.
void derivateRotationMultiplicative(const Vector3 &deltaR, Matrix3 &dRdR, Matrix3 &dRddeltaR)
Quaternion rotationVectorToQuaternion(const Vector3 &v)
Transforms the rotation vector into quaternion.
double 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 t...
Class facilitating the manipulation of the local kinematics of a frame within another and the associa...
Definition: rigid-body-kinematics.hpp:677
Vector6 derivateHomogeneousMatrixFD(const Matrix4 &m1, const Matrix4 &m2, double dt)
Matrix4 invertHomoMatrix(const Matrix4 &m)
const Quaternion & matrixToQuaternion_() const
double randomAngle()
get a randomAngle between -pi and pu
Matrix3 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 d...
Orientation & setValue(const Quaternion &q, const Matrix3 &m)
Orientation operator*(const Orientation &R2) const
Eigen::Matrix3d Matrix3
3x3 Scalar Matrix
Definition: definitions.hpp:109
static const Byte pose
Definition: rigid-body-kinematics.hpp:533
Vector3 tempVec_4
Definition: rigid-body-kinematics.hpp:771
LocalKinematics getInverse() const
returns the inverse of the current local kinematics.
CheckedQuaternion q_
Definition: rigid-body-kinematics.hpp:496
static const Byte all
Definition: rigid-body-kinematics.hpp:536
Kinematics & setToDiffNoAlias(const Kinematics &multiplier1, const Kinematics &multiplier2)
bool isSet() const
checks that the orientation has been assigned a value.
constexpr double m
mass of the robot
Definition: hrp2.hpp:36
Vector invertState(const Vector &state)
LocalKinematics & setZero(Flags::Byte=Flags::all)
@ rotationVector
Definition: rigid-body-kinematics.hpp:313
Vector3 derivateRotationFD(const Quaternion &q1, const Quaternion &q2, double dt)
derivates a quaternion using finite difference to get a angular velocity vector
bool isRotationMatrix(const Matrix3 &, double precision=2 *cst::epsilon1)
Checks if it is a rotation matrix (right-hand orthonormal) or not.
T & setZero(typename Flags::Byte=Flags::all)
Eigen::Matrix< double, 6, 1 > Vector6
6D vector
Definition: definitions.hpp:97
Eigen::Index Index
Definition: definitions.hpp:138
Vector3 getInvariantOrthogonalVector(const Matrix3 &Rhat, const Vector3 &Rtez)
Gets a vector that is orthogonal to and such that is orthogonal to the tilt . This vector is NOT n...
const Quaternion & toQuaternion() const
Definitions of types and some structures.
KinematicsInternal()
Definition: rigid-body-kinematics.hpp:507
Vector3 vectorComponent(const Quaternion &q)
vector part of the quaternion
CheckedQuaternion & getQuaternionRefUnsafe()
get a reference to the quaternion representation of the orientation without calling the check functio...
Definition: rigid-body-kinematics.hpp:504
Eigen::AngleAxis< double > AngleAxis
Euler Axis/Angle representation of orientation.
Definition: definitions.hpp:133
Vector3 rotationMatrixToRotationVector(const Matrix3 &R)
Transforms the rotation matrix into rotation vector.
void integrateKinematics(Vector3 &position, const Vector3 &velocity, double dt)
CheckedMatrix3 & getMatrixRefUnsafe()
no checks are performed for these functions, use with caution
Definition: rigid-body-kinematics.hpp:354
@ quaternion
Definition: rigid-body-kinematics.hpp:314
static const Byte orientation
Definition: rigid-body-kinematics.hpp:527
Vector3 rotationMatrixToRollPitchYaw(const Matrix3 &R, Vector3 &v)
constexpr double epsilon1
number considered zero when compared to 1
Definition: definitions.hpp:672
void reset()
resets the Orientation object.
bool isQuaternionSet() const
checks that the quaternion representation of the orientation has been assigned a value.
Gathers many kinds of algorithms.
bool isMatrixSet() const
checks that the matrix representation of the orientation has been assigned a value.
LocalKinematics & setToDiffNoAliasLinPart(const LocalKinematics &multiplier1, const LocalKinematics &multiplier2)
Linear part of the setToDiffNoAlias(const LocalKinematics &, const LocalKinematics &) function.
Kinematics & operator=(const LocalKinematics &locK)
fills the Kinematics object given its equivalent in the local frame.
Eigen::Vector3d Vector3
3D vector
Definition: definitions.hpp:85
Quaternion zeroRotationQuaternion()
Get the Identity Quaternion.
Matrix3 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 d...
CheckedVector3 angVel
Definition: rigid-body-kinematics.hpp:543
Eigen::Quaterniond Quaternion
Quaternion.
Definition: definitions.hpp:127
static Orientation randomRotation()
Returns a uniformly distributed random rotation.
Matrix3 orthogonalizeRotationMatrix(const Matrix3 &M)
Projects the Matrix to so(3)
Orientation orientation
Definition: rigid-body-kinematics.hpp:540
Vector6 derivatePoseThetaUFD(const Vector6 &v1, const Vector6 &v2, double dt)
CheckedVector3 angAcc
Definition: rigid-body-kinematics.hpp:546
void setMatrix(bool b=true)
Matrix4 vector6ToHomogeneousMatrix(const Vector6 &v)
transforms a 6d vector (position theta mu) into a homogeneous matrix
Kinematics & setToDiffNoAliasLinPart(const Kinematics &multiplier1, const Kinematics &multiplier2)
Linear part of the setToDiffNoAlias(const Kinematics &, const Kinematics &) function.
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
Kinematics getInverse() const
returns the inverse of the current kinematics.
AngleAxis toAngleAxis() const
std::ostream & operator<<(std::ostream &os, const stateObservation::kine::Kinematics &k)
LocalKinematics & setToProductNoAlias(const LocalKinematics &operand1, const LocalKinematics &operand2)
computes the composition of two LocalKinematics object.
Eigen::Vector4d Vector4
4D vector
Definition: definitions.hpp:91
LocalKinematics & operator=(const Kinematics &kine)
fills the LocalKinematics object given its equivalent in the global frame.
unsigned char Byte
Definition: rigid-body-kinematics.hpp:524
Vector3 differentiate(Orientation R_k1) const
gives the log (rotation vector) of the "left-side" difference of orientation: log of R_k1*(*this)....
CheckedVector3 linVel
Definition: rigid-body-kinematics.hpp:542
Orientation & fromVector4(const Vector4 &v)
@ matrix
Definition: rigid-body-kinematics.hpp:312
const Kinematics & integrate(double dt)
integrates the current kinematics over the timestep dt.
Vector3 quaternionToRotationVector(const Quaternion &q)
Tranbsform a quaternion into rotation vector.
static const Byte angVel
Definition: rigid-body-kinematics.hpp:529
Vector6 homogeneousMatrixToVector6(const Matrix4 &M)
transforms a homogeneous matrix into 6d vector (position theta mu)
const Kinematics & update(const Kinematics &newValue, double dt, Flags::Byte=Flags::all)
updates the current kinematics (k) with the new ones (k+1).
const Matrix3 & quaternionToMatrix_() const
Quaternion rollPitchYawToQuaternion(double roll, double pitch, double yaw)
Vector3 tempVec_
Definition: rigid-body-kinematics.hpp:768
Orientation & setZeroRotation()
static Orientation zeroRotation()
retruns a zero rotation
static const Byte angAcc
Definition: rigid-body-kinematics.hpp:531
LocalKinematics & setToDiffNoAlias(const LocalKinematics &multiplier1, const LocalKinematics &multiplier2)
Vector3 tempVec_2
Definition: rigid-body-kinematics.hpp:769
void setQuaternion(bool b=true)
double scalarComponent(const Quaternion &q)
scalar component of a quaternion
Vector3 tempVec_
Definition: rigid-body-kinematics.hpp:669
Matrix3 derivateRtvMultiplicative(const Matrix3 &R, const Vector3 &v)