37 for (
unsigned int i = 0; i < 3; ++i)
39 for (
unsigned int i = 0; i < 9; ++i)
65 for (
unsigned int i = 0; i < 3; ++i)
67 for (
unsigned int i = 0; i < 9; ++i)
95 geometry_msgs::Pose
addDelta(
const geometry_msgs::Pose &pose,
const geometry_msgs::Twist &twist,
const double &t)
97 geometry_msgs::Pose result;
void vectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t)
Converts a tf Vector3 into a KDL Vector.
geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t) __attribute__((deprecated))
Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B...
void transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
Converts a KDL Frame into a tf Transform.
static Rotation Quaternion(double x, double y, double z, double w)
void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
Converts a tf Quaternion into a KDL Rotation.
void GetQuaternion(double &x, double &y, double &z, double &w) const
void poseTFToKDL(const tf::Pose &t, KDL::Frame &k)
Converts a tf Pose into a KDL Frame.
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
Converts a KDL Frame into a tf Pose.
void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
Converts a tf Transform into a KDL Frame.
void vectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k)
Converts a tf Vector3 into a KDL Vector.
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void quaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k)
Converts a tf Quaternion into a KDL Rotation.
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
IMETHOD doubleVel addDelta(const doubleVel &a, const doubleVel &da, double dt=1.0)
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)