bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Bug Fix 3101648

This commit is contained in:
probablydavid 2010-11-03 20:05:26 +00:00
parent 0135a03811
commit a348db7dc9
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
arm_kinematics/arm_kinematics.cpp
View File

@ -244,6 +244,7 @@ bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
tf::Stamped<tf::Pose> transform; tf::Stamped<tf::Pose> transform;
tf::Stamped<tf::Pose> transform_root;
tf::poseStampedMsgToTF( pose_msg_in, transform ); tf::poseStampedMsgToTF( pose_msg_in, transform );
//Do the IK //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; 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); int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);