38 #include <Eigen/Geometry> 39 #include <ignition/math/Vector3.hh> 40 #include <ignition/math/Quaternion.hh> 41 #include <ignition/math/Pose3.hh> 42 #include <gazebo/msgs/wrench_stamped.pb.h> 44 inline Eigen::Matrix<double,6,1>
gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w)
46 Eigen::Matrix<double,6,1> e;
47 e[0] = w.wrench().force().x();
48 e[1] = w.wrench().force().y();
49 e[2] = w.wrench().force().z();
50 e[3] = w.wrench().torque().x();
51 e[4] = w.wrench().torque().y();
52 e[5] = w.wrench().torque().z();
56 inline Eigen::Quaterniond
quatFromRPY(
double roll,
double pitch,
double yaw )
58 return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
59 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
60 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
64 inline Eigen::Vector3d
convVec3(
const ignition::math::Vector3d &_vec3)
66 return Eigen::Vector3d(_vec3.X(),_vec3.Y(),_vec3.Z());
69 inline ignition::math::Vector3d
convVec3(
const Eigen::Vector3d &_vec3)
71 return ignition::math::Vector3d(_vec3[0], _vec3[1], _vec3[2]);
74 inline Eigen::Quaterniond
convQuat(
const ignition::math::Quaterniond &_quat)
76 return Eigen::Quaterniond(_quat.W(), _quat.X(), _quat.Y(), _quat.Z());
79 inline ignition::math::Quaterniond
convQuat(
const Eigen::Quaterniond &_quat)
81 return ignition::math::Quaterniond(_quat.w(), _quat.x(), _quat.y(), _quat.z());
84 inline Eigen::Affine3d
convPose(
const ignition::math::Pose3d &_pose)
88 res.translation() =
convVec3(_pose.Pos());
89 res.linear() =
convQuat(_pose.Rot()).toRotationMatrix();
94 inline ignition::math::Pose3d
convPose(
const Eigen::Affine3d &_T)
96 ignition::math::Pose3d pose;
97 pose.Pos() =
convVec3(_T.translation());
98 pose.Rot() =
convQuat(Eigen::Quaterniond(_T.linear()));
107 FILE* pipe = popen(_cmd.c_str(),
"r");
113 std::string result =
"";
117 if (fgets(buffer, 128, pipe) != NULL)
Eigen::Matrix< double, 6, 1 > gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped &w)
Definition: Utils.h:44
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:56
std::string custom_exec(const std::string &_cmd)
Definition: Utils.h:105
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:64
Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
Definition: Utils.h:74
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:84