38 #if defined(_WIN32) && !defined(_ENABLE_EXTENDED_ALIGNED_STORAGE) 39 #define _ENABLE_EXTENDED_ALIGNED_STORAGE 41 #include <Eigen/Dense> 48 static const double Infinity = std::numeric_limits<double>::infinity();
55 template<
typename T>
bool equal(
const T& a,
const T& b)
57 return std::abs<T>(a - b) < std::numeric_limits<T>::epsilon();
67 inline Eigen::Vector3d
diffRot(
const Eigen::Matrix3d& R_a_b1,
const Eigen::Matrix3d& R_a_b2)
69 Eigen::Matrix3d R_b1_b2 = R_a_b1.transpose() * R_a_b2;
70 Eigen::AngleAxisd aa(Eigen::Map<const Eigen::Matrix3d>(R_b1_b2.data()));
71 return R_a_b1 * aa.angle() * aa.axis();
74 inline Eigen::Matrix<double,6,1>
diffTransform(
const Eigen::Matrix4d& t_a_b1,
const Eigen::Matrix4d& t_a_b2)
76 Eigen::Matrix<double,6,1> d_t1_t2;
78 d_t1_t2.head<3>() = t_a_b2.block<3,1>(0,3) - t_a_b1.block<3,1>(0,3);
79 d_t1_t2.tail<3>() =
diffRot(t_a_b1.topLeftCorner<3,3>(),t_a_b2.topLeftCorner<3,3>());
83 inline Eigen::Quaterniond
quatFromRPY(
double roll,
double pitch,
double yaw )
85 return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
86 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
87 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
92 return Eigen::AngleAxisd(A, Eigen::Vector3d::UnitZ())
93 * Eigen::AngleAxisd(B, Eigen::Vector3d::UnitY())
94 * Eigen::AngleAxisd(C, Eigen::Vector3d::UnitX());
97 inline Eigen::Matrix<double,6,1>&
transportWrench(
const Eigen::Matrix<double,6,1>& wrench_in_a,
const Eigen::Matrix<double,3,1>& b_to_a, Eigen::Matrix<double,6,1>& wrench_in_b_out)
100 wrench_in_b_out.head<3>() = wrench_in_a.head<3>();
101 wrench_in_b_out.tail<3>() = wrench_in_a.tail<3>() + b_to_a.cross(wrench_in_a.head<3>());
102 return wrench_in_b_out;
107 template <
typename Derived>
Size(
const Eigen::MatrixBase<Derived>& mat)
120 return (s.
rows() == rows_) && (s.
cols() == cols_);
124 return !(*
this == s);
126 int rows()
const {
return rows_;}
127 int cols()
const {
return cols_;}
bool operator==(const Size &s) const
Definition: Utils.h:118
Size(int rows=0, int cols=0)
Definition: Utils.h:111
Eigen::Vector3d diffRot(const Eigen::Matrix3d &R_a_b1, const Eigen::Matrix3d &R_a_b2)
Definition: Utils.h:67
void setToLowest(Eigen::VectorXd &v)
Definition: Utils.h:60
Eigen::Matrix< double, 6, 1 > diffTransform(const Eigen::Matrix4d &t_a_b1, const Eigen::Matrix4d &t_a_b2)
Definition: Utils.h:74
bool operator!=(const Size &s) const
Definition: Utils.h:122
bool equal(const T &a, const T &b)
Definition: Utils.h:55
int cols() const
Definition: Utils.h:127
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: Utils.h:65
Eigen::Quaterniond quatFromKukaConvention(double A, double B, double C)
Definition: Utils.h:90
int rows() const
Definition: Utils.h:126
Definition: CartesianAccelerationPID.h:44
static const double Infinity
Definition: Utils.h:48
Size(const Size &s)
Definition: Utils.h:115
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:83
void setToHighest(Eigen::VectorXd &v)
Definition: Utils.h:50
Size(const Eigen::MatrixBase< Derived > &mat)
Definition: Utils.h:107
Eigen::Matrix< double, 6, 1 > & transportWrench(const Eigen::Matrix< double, 6, 1 > &wrench_in_a, const Eigen::Matrix< double, 3, 1 > &b_to_a, Eigen::Matrix< double, 6, 1 > &wrench_in_b_out)
Definition: Utils.h:97