Skip to content

Instantly share code, notes, and snippets.

@lucascoelhof

lucascoelhof/getKDLTransform.cpp

Last active February 16, 2018 16:53
Download ZIP

Revisions

  1. lucascoelhof revised this gist Feb 16, 2018. 1 changed file with 16 additions and 8 deletions.
    24 changes: 16 additions & 8 deletions getKDLTransform.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -2,13 +2,21 @@
    #include <geometry_msgs/Pose.h>
    #include <kdl/tree.hpp>

    void getTransformFromKDL(KDL::Tree tree, std::string root, std::string child, geometry_msgs::Pose& pose)
    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);
    try
    {
    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;
    }
    }
  2. lucascoelhof created this gist Feb 16, 2018.
    14 changes: 14 additions & 0 deletions getKDLTransform.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,14 @@

    #include <geometry_msgs/Pose.h>
    #include <kdl/tree.hpp>

    void 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);
    }