From a348db7dc9cb87cdc826197aeef52b57ec77bfdf Mon Sep 17 00:00:00 2001
From: probablydavid <devnull@localhost>
Date: Wed, 3 Nov 2010 20:05:26 +0000
Subject: [PATCH] Bug Fix 3101648

---
 arm_kinematics/arm_kinematics.cpp | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/arm_kinematics/arm_kinematics.cpp b/arm_kinematics/arm_kinematics.cpp
index d854f47..375f99e 100644
--- a/arm_kinematics/arm_kinematics.cpp
+++ b/arm_kinematics/arm_kinematics.cpp
@@ -244,6 +244,7 @@ bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
 
     geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
     tf::Stamped<tf::Pose> transform;
+    tf::Stamped<tf::Pose> transform_root;
     tf::poseStampedMsgToTF( pose_msg_in, transform );
 
     //Do the IK
@@ -259,9 +260,17 @@ bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
         }
     }
 
-    // populate F_dest from tf::Transform parameter
+    //Convert F to our root_frame
+    try {
+        tf_listener.transformPose(root_name, transform, transform_root);
+    } catch (...) {
+        ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
+        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
+       return false;
+    }
+
     KDL::Frame F_dest;
-    tf::TransformTFToKDL(transform, F_dest);
+    tf::TransformTFToKDL(transform_root, F_dest);
 
     int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);