rigid-body-kinematics.hpp

Go to the documentation of this file.

 
#ifndef StATEOBSERVATIONRIGIDBODYKINEMATICS_H
#define StATEOBSERVATIONRIGIDBODYKINEMATICS_H
 
#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>
 
namespace stateObservation
{
namespace kine
{
inline void integrateKinematics(Vector3 & position, const Vector3 & velocity, double dt);
 
inline void integrateKinematics(Vector3 & position, Vector3 & velocity, const Vector3 & acceleration, double dt);
 
inline void integrateKinematics(Matrix3 & orientation, const Vector3 & rotationVelocity, double dt);
 
inline void integrateKinematics(Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);
 
inline void integrateKinematics(Quaternion & orientation, const Vector3 & rotationVelocity, double dt);
 
inline void integrateKinematics(Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Matrix3 & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Quaternion & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

inline Vector regulateRotationVector(const Vector3 & v);

inline AngleAxis rotationVectorToAngleAxis(const Vector3 & v);

inline Matrix3 rotationVectorToRotationMatrix(const Vector3 & v);

inline Quaternion rotationVectorToQuaternion(const Vector3 & v);

inline Vector3 rotationMatrixToRotationVector(const Matrix3 & R);

inline Vector3 quaternionToRotationVector(const Quaternion & q);

inline Vector3 quaternionToRotationVector(const Vector4 & v);

inline double scalarComponent(const Quaternion & q);

inline Vector3 vectorComponent(const Quaternion & q);

inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R, Vector3 & v);
 
inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R);

inline Matrix3 rollPitchYawToRotationMatrix(double roll, double pitch, double yaw);
 
inline Matrix3 rollPitchYawToRotationMatrix(const Vector3 & rpy);

inline Quaternion rollPitchYawToQuaternion(double roll, double pitch, double yaw);
 
inline Quaternion rollPitchYawToQuaternion(const Vector3 & rpy);

inline Matrix3 orthogonalizeRotationMatrix(const Matrix3 & M);

inline Matrix3 skewSymmetric(const Vector3 & v, Matrix3 & R);

inline Matrix3 skewSymmetric(const Vector3 & v);

inline Matrix3 skewSymmetric2(const Vector3 & v, Matrix3 & R);

inline Matrix3 skewSymmetric2(const Vector3 & v);

inline Vector6 homogeneousMatrixToVector6(const Matrix4 & M);

inline Matrix4 vector6ToHomogeneousMatrix(const Vector6 & v);

inline Matrix3 twoVectorsToRotationMatrix(const Vector3 & v1, const Vector3 Rv1);

inline bool isPureYaw(const Matrix3 & R);

inline Vector3 getInvariantHorizontalVector(const Matrix3 & R);

inline Vector3 getInvariantOrthogonalVector(const Matrix3 & Rhat, const Vector3 & Rtez);

inline Matrix3 mergeTiltWithYaw(const Vector3 & Rtez,
                                const Matrix3 & R2,
                                const Vector3 & v = Vector3::UnitX()) noexcept(false);

inline Matrix3 mergeRoll1Pitch1WithYaw2(const Matrix3 & R1, const Matrix3 & R2, const Vector3 & v = Vector3::UnitX());

inline Matrix3 mergeTiltWithYawAxisAgnostic(const Vector3 & Rtez, const Matrix3 & R2);

inline Matrix3 mergeRoll1Pitch1WithYaw2AxisAgnostic(const Matrix3 & R1, const Matrix3 & R2);

inline double rotationMatrixToAngle(const Matrix3 & rotation, const Vector3 & axis, const Vector3 & v);

inline double rotationMatrixToYaw(const Matrix3 & rotation, const Vector2 & v);

inline double rotationMatrixToYaw(const Matrix3 & rotation);

inline double rotationMatrixToYawAxisAgnostic(const Matrix3 & rotation);

inline Quaternion zeroRotationQuaternion();

inline Quaternion randomRotationQuaternion();

inline double randomAngle();

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

inline void fixedPointRotationToTranslation(const Matrix3 & R,
                                            const Vector3 & rotationVelocity,
                                            const Vector3 & rotationAcceleration,
                                            const Vector3 & fixedPoint,
                                            Vector3 & outputTranslation,
                                            Vector3 & outputLinearVelocity,
                                            Vector3 & outputLinearAcceleration);

inline Vector3 derivateRotationFD(const Quaternion & q1, const Quaternion & q2, double dt);

inline Vector3 derivateRotationFD(const Vector3 & o1, const Vector3 & o2, double dt);
 
inline Vector6 derivateHomogeneousMatrixFD(const Matrix4 & m1, const Matrix4 & m2, double dt);
 
inline Vector6 derivatePoseThetaUFD(const Vector6 & v1, const Vector6 & v2, double dt);

inline void derivateRotationMultiplicative(const Vector3 & deltaR, Matrix3 & dRdR, Matrix3 & dRddeltaR);

inline Matrix3 derivateRtvMultiplicative(const Matrix3 & R, const Vector3 & v);

inline IndexedVectorArray reconstructStateTrajectory(const IndexedVectorArray & positionOrientation, double dt);
 
inline Vector invertState(const Vector & state);
 
inline Matrix4 invertHomoMatrix(const Matrix4 & m);
 
enum rotationType
{
  matrix = 0,
  rotationVector = 1,
  quaternion = 2,
  angleaxis = 3
};
 
template<rotationType = rotationVector>
struct indexes
{
};
 
template<>
struct indexes<rotationVector>
{
  static const Index pos = 0;
  static const Index ori = 3;
  static const Index linVel = 6;
  static const Index angVel = 9;
  static const Index linAcc = 12;
  static const Index angAcc = 15;
  static const Index size = 18;
};
 
template<>
struct indexes<quaternion>
{
  static const Index pos = 0;
  static const Index ori = 3;
  static const Index linVel = 7;
  static const Index angVel = 10;
  static const Index linAcc = 13;
  static const Index angAcc = 16;
  static const Index size = 19;
};

constexpr double quatNormTol = 1e-6;
 
class Orientation
{
public:
  explicit Orientation(bool initialize = true);

  explicit Orientation(const Vector3 & v);
 
  explicit Orientation(const Quaternion & q);
 
  explicit Orientation(const Matrix3 & m);
 
  explicit Orientation(const AngleAxis & aa);
 
  Orientation(const Quaternion & q, const Matrix3 & m);
 
  Orientation(const double & roll, const double & pitch, const double & yaw);
 
  Orientation(const Orientation & multiplier1, const Orientation & multiplier2);
 
  inline Orientation & operator=(const Vector3 & v);
 
  inline Orientation & operator=(const Quaternion & q);
 
  inline Orientation & operator=(const Matrix3 & m);
 
  inline Orientation & operator=(const AngleAxis & aa);
 
  inline Orientation & setValue(const Quaternion & q, const Matrix3 & m);
 
  inline Orientation & fromVector4(const Vector4 & v);
 
  inline Orientation & setRandom();
 
  template<typename t = Quaternion>
  inline Orientation & setZeroRotation();

  inline const Matrix3 & toMatrix3() const;
  inline const Quaternion & toQuaternion() const;
 
  inline operator const Matrix3 &() const;
  inline operator const Quaternion &() const;
 
  inline Vector4 toVector4() const;
 
  inline Vector3 toRotationVector() const;
  inline Vector3 toRollPitchYaw() const;
  inline AngleAxis toAngleAxis() const;

 
  inline Orientation operator*(const Orientation & R2) const;

  inline const Orientation & setToProductNoAlias(const Orientation & R1, const Orientation & R2);
 
  inline Orientation inverse() const;

  inline const Orientation & integrate(Vector3 dt_x_omega);

  inline const Orientation & integrateRightSide(Vector3 dt_x_omega);

  inline Vector3 differentiate(Orientation R_k1) const;

  inline Vector3 differentiateRightSide(Orientation R_k1) const;

  inline Vector3 operator*(const Vector3 & v) const;

  inline bool isSet() const;

  inline void reset();

  inline bool isMatrixSet() const;
  inline bool isQuaternionSet() const;

  inline void setMatrix(bool b = true);
  inline void setQuaternion(bool b = true);


  inline CheckedMatrix3 & getMatrixRefUnsafe();
  inline CheckedQuaternion & getQuaternionRefUnsafe();

  inline void synchronize();

  static inline Orientation zeroRotation();

  static inline Orientation randomRotation();
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  void check_() const;
 
  inline const Matrix3 & quaternionToMatrix_() const;
  inline const Quaternion & matrixToQuaternion_() const;
 
  mutable CheckedQuaternion q_;
  mutable CheckedMatrix3 m_;
};
 
namespace internal
{
 
template<class T>
class KinematicsInternal
{
public:
  KinematicsInternal() {}

  KinematicsInternal(const CheckedVector3 & position,
                     const CheckedVector3 & linVel,
                     const CheckedVector3 & linAcc,
                     const Orientation & orientation,
                     const CheckedVector3 & angVel,
                     const CheckedVector3 & angAcc);
  struct Flags
  {
    typedef unsigned char Byte;
 
    static const Byte position = BOOST_BINARY(000001);
    static const Byte orientation = BOOST_BINARY(000010);
    static const Byte linVel = BOOST_BINARY(000100);
    static const Byte angVel = BOOST_BINARY(001000);
    static const Byte linAcc = BOOST_BINARY(010000);
    static const Byte angAcc = BOOST_BINARY(100000);
 
    static const Byte pose = position | orientation;
    static const Byte vel = linVel | angVel;
    static const Byte acc = linAcc | angAcc;
    static const Byte all = pose | vel | acc;
  };
 
  CheckedVector3 position;
  Orientation orientation;
 
  CheckedVector3 linVel;
  CheckedVector3 angVel;
 
  CheckedVector3 linAcc;
  CheckedVector3 angAcc;
 
  inline void reset();

  T & fromVector(const Vector & v, typename Flags::Byte = Flags::all);

  template<typename t = Quaternion>
  T & setZero(typename Flags::Byte = Flags::all);

  static inline T zeroKinematics(typename Flags::Byte = Flags::all);

  inline Vector toVector(typename Flags::Byte) const;
  inline Vector toVector() const;
};
} // namespace internal
 
struct LocalKinematics;

struct Kinematics : public internal::KinematicsInternal<Kinematics>
{
 
  Kinematics() {}

  Kinematics(const Vector & v, Flags::Byte = Flags::all);

  Kinematics(const Kinematics & multiplier1, const Kinematics & multiplier2);

  Kinematics(const CheckedVector3 & position,
             const CheckedVector3 & linVel,
             const CheckedVector3 & linAcc,
             const Orientation & orientation,
             const CheckedVector3 & angVel,
             const CheckedVector3 & angAcc);

  explicit inline Kinematics(const LocalKinematics & locK);

  inline Kinematics & operator=(const LocalKinematics & locK);

  inline const Kinematics & integrate(double dt);

  inline const Kinematics & update(const Kinematics & newValue, double dt, Flags::Byte = Flags::all);

  inline Kinematics getInverse() const;

  inline Kinematics operator*(const Kinematics &) const;

  inline Kinematics & setToProductNoAlias(const Kinematics & operand1, const Kinematics & operand2);

  inline Kinematics & setToDiffNoAlias(const Kinematics & multiplier1, const Kinematics & multiplier2);

  inline Kinematics & setToDiffNoAliasLinPart(const Kinematics & multiplier1, const Kinematics & multiplier2);

  inline Kinematics & setToDiffNoAliasAngPart(const Kinematics & multiplier1, const Kinematics & multiplier2);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  Vector3 tempVec_;
};

struct LocalKinematics : public internal::KinematicsInternal<LocalKinematics>
{
  LocalKinematics() {}

  inline LocalKinematics(const Vector & v, Flags::Byte flags);

  inline LocalKinematics(const LocalKinematics & multiplier1, const LocalKinematics & multiplier2);

  LocalKinematics(const CheckedVector3 & position,
                  const CheckedVector3 & linVel,
                  const CheckedVector3 & linAcc,
                  const Orientation & orientation,
                  const CheckedVector3 & angVel,
                  const CheckedVector3 & angAcc);

  explicit inline LocalKinematics(const Kinematics & kin);

  inline LocalKinematics & operator=(const Kinematics & kine);

  template<typename t = Quaternion>
  LocalKinematics & setZero(Flags::Byte = Flags::all);

  inline const LocalKinematics & integrate(double dt);

  inline const LocalKinematics & update(const LocalKinematics & newValue, double dt, Flags::Byte = Flags::all);

  inline LocalKinematics getInverse() const;

  inline LocalKinematics operator*(const LocalKinematics &) const;

  inline LocalKinematics & setToProductNoAlias(const LocalKinematics & operand1, const LocalKinematics & operand2);

  inline LocalKinematics & setToDiffNoAlias(const LocalKinematics & multiplier1, const LocalKinematics & multiplier2);

  inline LocalKinematics & setToDiffNoAliasLinPart(const LocalKinematics & multiplier1,
                                                   const LocalKinematics & multiplier2);

  inline LocalKinematics & setToDiffNoAliasAngPart(const LocalKinematics & multiplier1,
                                                   const LocalKinematics & multiplier2);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  Vector3 tempVec_;
  Vector3 tempVec_2;
  Vector3 tempVec_3;
  Vector3 tempVec_4;
  Vector3 tempVec_5;
};
 
} // namespace kine
} // namespace stateObservation
 
inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k);
 
inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::LocalKinematics & k);
 
#include <state-observation/tools/rigid-body-kinematics.hxx>
 
#endif // StATEOBSERVATIONRIGIDBODYKINEMATICS_H