rigid_body_motions.hpp

Go to the documentation of this file.

#ifndef MODERN_ROBOTICS__RIGID_BODY_MOTIONS_HPP___
#define MODERN_ROBOTICS__RIGID_BODY_MOTIONS_HPP___
 
#include <armadillo>
#include "modern_robotics/utils.hpp"
 
namespace mr
{
 
const arma::mat33 RotInv(const arma::mat33 & R);
 
const arma::mat33 VecToso3(const arma::vec3 & omg);
 
const arma::vec3 so3ToVec(const arma::mat33 & so3mat);
 
const std::tuple<const arma::vec3, double> AxisAng3(const arma::vec3 & expc3);
 
const arma::mat33 MatrixExp3(const arma::mat33 & so3mat);
 
const arma::mat33 MatrixLog3(const arma::mat33 & R);
 
const arma::mat44 RpToTrans(const arma::mat33 & R, const arma::vec3 & p);
 
const std::tuple<const arma::mat33, const arma::vec3> TransToRp(const arma::mat44 & T);
 
const arma::mat44 TransInv(const arma::mat44 & T);
 
const arma::mat44 VecTose3(const arma::vec6 & V);
 
const arma::vec6 se3ToVec(const arma::mat44 & se3mat);
 
const arma::mat66 Adjoint(const arma::mat44 & T);
 
const arma::vec6 ScrewToAxis(const arma::vec3 & q, const arma::vec3 & s, const double h);
 
const std::tuple<const arma::vec6, double> AxisAng6(const arma::vec6 & expc6);
 
const arma::mat44 MatrixExp6(const arma::mat44 & se3mat);
 
const arma::mat44 MatrixLog6(const arma::mat44 & T);
 
const arma::mat33 ProjectToSO3(const arma::mat33 & mat);
 
const arma::mat44 ProjectToSE3(const arma::mat44 & mat);
 
double DistanceToSO3(const arma::mat33 & mat);
 
double DistanceToSE3(const arma::mat44 & mat);
 
inline bool TestIfSO3(const arma::mat33 & mat)
{
  return std::fabs(DistanceToSO3(mat)) < tolerance;
}
 
inline bool TestIfSE3(const arma::mat44 & mat)
{
  return std::fabs(DistanceToSE3(mat)) < tolerance;
}
} 
 
#endif