matrix.h

Go to the documentation of this file.

 
#ifndef RB_MATRIX_H_
#define RB_MATRIX_H_
#include "unit.h"
 
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
 
namespace rb    
{
namespace math  
{
typedef Eigen::MatrixXd MatrixX;        
typedef Eigen::Matrix3d Matrix3;        
typedef Eigen::Matrix4d Matrix4;        
 
typedef Eigen::VectorXd VectorX;        
typedef Eigen::Vector3d Vector3;        
typedef Eigen::Vector4d Vector4;        
 
typedef Eigen::AngleAxisd AngleAxis;    
 
template <typename T, size_t R, size_t C>
  using Array = Eigen::Array<T, R, C>;
 
typedef Eigen::Array<double, 6, 1> Array6;
 
inline Matrix4 homoTrans(const double& A, const double& alpha, const double& D, const double theta)
{
  double ct = cos(DEG2RAD * theta);
  double st = sin(DEG2RAD * theta);
  double ca = cos(DEG2RAD * alpha);
  double sa = sin(DEG2RAD * alpha);
 
  Matrix4 T;
  T << ct,   -st,   0,     A,
    st*ca, ct*ca, -sa, -sa*D,
    st*sa, ct*sa,  ca,  ca*D,
        0,     0,   0,     1;
  return T;
}
 
inline Matrix4 rotateX(const double& rad)
{
  Matrix3 m33;
  m33 = AngleAxis(rad, Vector3::UnitX());
  Matrix4 m = Matrix4::Identity();
  m.topLeftCorner(3, 3) << m33;
  return m;
}
 
inline Matrix4 rotateY(const double& rad)
{
  Matrix3 m33;
  m33 = AngleAxis(rad, Vector3::UnitY());
  Matrix4 m = Matrix4::Identity();
  m.topLeftCorner(3, 3) << m33;
  return m;
}
 
inline Matrix4 rotateZ(const double& rad)
{
  Matrix3 m33;
  m33 = AngleAxis(rad, Vector3::UnitZ());
  Matrix4 m = Matrix4::Identity();
  m.topLeftCorner(3, 3) << m33;
  return m;
}
 
inline void tr2rpy(const Matrix4& m, double& roll_z, double& pitch_y, double& yaw_x)
{
  if (fabs(m(0,0)) < EPSILON && fabs(m(1,0)) < EPSILON)
  {
    roll_z  = 0;
    pitch_y = atan2(-m(2,0), m(0,0));
    yaw_x   = atan2(-m(1,2), m(1,1));
  }
  else
  {
    roll_z  = atan2(m(1,0), m(0,0));
    double sr = sin(roll_z);
    double cr = cos(roll_z);
    pitch_y = atan2(-m(2,0), cr * m(0,0) + sr * m(1,0));
    yaw_x   = atan2(sr * m(0,2) - cr * m(1,2), cr * m(1,1) - sr * m(0,1));
  }
}
 
inline void rpy2tr(const double& roll_z, const double& pitch_y, const double& yaw_x, Matrix4& out)
{
  out = rotateZ(roll_z) * rotateY(pitch_y) * rotateX(yaw_x);
}
 
}   // namespace math
}   // namespace rb
 
#endif // RB_MATRIX_H_