38 #include <Eigen/Dense> 45 static const double Infinity = std::numeric_limits<double>::infinity();
52 template<
typename T>
bool equal(
const T& a,
const T& b)
54 return std::abs<T>(a - b) < std::numeric_limits<T>::epsilon();
64 inline Eigen::Vector3d
diffRot(
const Eigen::Matrix3d& R_a_b1,
const Eigen::Matrix3d& R_a_b2)
66 Eigen::Matrix3d R_b1_b2 = R_a_b1.transpose() * R_a_b2;
67 Eigen::AngleAxisd aa(Eigen::Map<const Eigen::Matrix3d>(R_b1_b2.data()));
68 return R_a_b1 * aa.angle() * aa.axis();
71 inline Eigen::Matrix<double,6,1>
diffTransform(
const Eigen::Matrix4d& t_a_b1,
const Eigen::Matrix4d& t_a_b2)
73 Eigen::Matrix<double,6,1> d_t1_t2;
75 d_t1_t2.head<3>() = t_a_b2.block<3,1>(0,3) - t_a_b1.block<3,1>(0,3);
76 d_t1_t2.tail<3>() =
diffRot(t_a_b1.topLeftCorner<3,3>(),t_a_b2.topLeftCorner<3,3>());
80 inline Eigen::Quaterniond
quatFromRPY(
double roll,
double pitch,
double yaw )
82 return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
83 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
84 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
89 return Eigen::AngleAxisd(A, Eigen::Vector3d::UnitZ())
90 * Eigen::AngleAxisd(B, Eigen::Vector3d::UnitY())
91 * Eigen::AngleAxisd(C, Eigen::Vector3d::UnitX());
96 template <
typename Derived>
Size(
const Eigen::MatrixBase<Derived>& mat)
109 return (s.
rows() == rows_) && (s.
cols() == cols_);
113 return !(*
this == s);
115 int rows()
const {
return rows_;}
116 int cols()
const {
return cols_;}
Size(int rows=0, int cols=0)
Definition: Utils.h:100
int cols() const
Definition: Utils.h:116
Eigen::Vector3d diffRot(const Eigen::Matrix3d &R_a_b1, const Eigen::Matrix3d &R_a_b2)
Definition: Utils.h:64
void setToLowest(Eigen::VectorXd &v)
Definition: Utils.h:57
int rows() const
Definition: Utils.h:115
Eigen::Matrix< double, 6, 1 > diffTransform(const Eigen::Matrix4d &t_a_b1, const Eigen::Matrix4d &t_a_b2)
Definition: Utils.h:71
bool operator!=(const Size &s) const
Definition: Utils.h:111
bool equal(const T &a, const T &b)
Definition: Utils.h:52
bool operator==(const Size &s) const
Definition: Utils.h:107
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: Utils.h:62
Eigen::Quaterniond quatFromKukaConvention(double A, double B, double C)
Definition: Utils.h:87
Definition: CartesianAccelerationPID.h:44
static const double Infinity
Definition: Utils.h:45
Size(const Size &s)
Definition: Utils.h:104
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:80
void setToHighest(Eigen::VectorXd &v)
Definition: Utils.h:47
Size(const Eigen::MatrixBase< Derived > &mat)
Definition: Utils.h:96