Skip to content

Instantly share code, notes, and snippets.

Last active February 16, 2018 16:53
What would you like to do?
How to get a transformation frame from KDL Tree ROS
#include <geometry_msgs/Pose.h>
#include <kdl/tree.hpp>
bool getTransformFromKDL(KDL::Tree tree, std::string root, std::string child, geometry_msgs::Pose& pose)
KDL::Chain chain;
tree.getChain(root, child, chain);
KDL::Frame frame = chain.getSegment(0).getFrameToTip();
pose.position.x = frame.p[0];
pose.position.y = frame.p[1];
pose.position.z = frame.p[2];
frame.M.GetQuaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
return true;
catch(std::exception& e)
return false;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment