Index: include/tf_conversions/tf_kdl.h
===================================================================
--- include/tf_conversions/tf_kdl.h (revision 31694)
+++ include/tf_conversions/tf_kdl.h (working copy)
@@ -34,6 +34,7 @@
#include "kdl/frames.hpp"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Wrench.h"
namespace tf
{
@@ -67,8 +68,13 @@
/// Converts a KDL Frame into a Pose message
void PoseKDLToMsg(const KDL::Frame &t, geometry_msgs::Pose &p);
+/// Converts a KDL Wrench into a Wrench message
+void WrenchKDLToMsg(const KDL::Wrench &w, geometry_msgs::Wrench &m);
+/// Converts a Wrench message into a KDL Wrench
+void WrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &w);
+
/* DEPRECATED FUNCTIONS */
/// Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B, during a time t.
geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t) __attribute__((deprecated));
Index: src/tf_kdl.cpp
===================================================================
--- src/tf_kdl.cpp (revision 31694)
+++ src/tf_kdl.cpp (working copy)
@@ -108,6 +108,26 @@
tf::poseTFToMsg(pose_tf,p);
}
+ void WrenchKDLToMsg(const KDL::Wrench &w, geometry_msgs::Wrench &m)
+ {
+ m.force.x = w.force[0];
+ m.force.y = w.force[1];
+ m.force.z = w.force[2];
+ m.torque.x = w.torque[0];
+ m.torque.y = w.torque[1];
+ m.torque.z = w.torque[2];
+ }
+
+ void WrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &w)
+ {
+ w.force[0] = m.force.x;
+ w.force[1] = m.force.y;
+ w.force[2] = m.force.z;
+ w.torque[0] = m.torque.x;
+ w.torque[1] = m.torque.y;
+ w.torque[2] = m.torque.z;
+ }
+
geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t)
{
geometry_msgs::Pose result;