TreeFkSolverPos_recursive, segmentation fault

[mganglb]

Hello, i wonder if someone else has that problem as well.

PyKDL: modified SIP version KDL: ros2_eloquent version

calling the function TreeFkSolverPos_recursive results in an segmentation fault for me. The error codes work for me, but if the else gets entered my process dies dies.

Additional Information: The Tree consist out of fixed joints only.

regards mgangl

Subset of my changes:

class TreeFkSolverPos 
{
%TypeHeaderCode
#include <kdl/treefksolver.hpp>
using namespace KDL;
%End
    virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)=0;
};

class TreeFkSolverPos_recursive : TreeFkSolverPos
{
%TypeHeaderCode
#include <kdl/treefksolverpos_recursive.hpp>
using namespace KDL;
%End

public:
    TreeFkSolverPos_recursive(const Tree &tree);
    virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName);
};

http://docs.ros.org/hydro/api/orocos_kdl/html/treefksolverpos__recursive_8cpp_source.html#l00033

00038         if(q_in.rows() != tree.getNrOfJoints())
00039                 return -1;
00040         else if(it == tree.getSegments().end()) //if the segment name is not found
00041                 return -2;
00042         else{
00043                         p_out = recursiveFk(q_in, it);
00044                 return 0;               
00045         }

[MatthijsBurgh]

@mganglb It is a little bit unclear to me. Yes you get a segfault. Can you confirm it is happening in? https://github.com/orocos/orocos_kinematics_dynamics/blob/6393a0f9c1667378160744578864a11d8e684c02/orocos_kdl/src/treefksolverpos_recursive.cpp#L43

In case that is correct, can you pin point which line in recursiveFk?

Could you convert your python code to cpp? So we can check if this is an issue related to the bindings or the cpp library.

[mganglb]

@MatthijsBurgh since i get the correct error codes (-1, -2) if my args violate the conditions, i am quite confident that the error occurs in 'p_out = recursiveFk(q_in, it)'.

maybe i find time to write an c++ testcase on the we. Ty for your fast response.

Pseudo Code:

tree = Tree('root') (All joints fixed)
tree_solver = kdl.TreeFkSolverPos_recursive(tree)
seg = 'wc_world' (valid Segment Name / not root)
kdl_frame = kdl.Frame()
jnt_array = kdl.JntArray() (with len 0 since getNrOfJoints() == 0 )
err = tree_solver.JntToCart(jnt_array , kdl_frame , seg)

[MatthijsBurgh]

@mganglb Your provided pseudo code, returns -2. As you request segment wc_world, which isn't in the tree. But in case you request root. I do get the segfault in JntArray::operator() in q_in(0). https://github.com/orocos/orocos_kinematics_dynamics/blob/6393a0f9c1667378160744578864a11d8e684c02/orocos_kdl/src/treefksolverpos_recursive.cpp#L52 As it finds the root segment, but jnt_array and thus jnt_array.data has size (0,1). So requesting any member in jnt_array will cause a segfault. But the root segment is initialized with q_nr(0). Which is incorrect as index zero means first element. So this a bug we should fix IMO.

My test code:

#include "tree.hpp"
#include "treefksolverpos_recursive.hpp"
#include "jntarray.hpp"
#include "frames.hpp"

#include <iostream>

using namespace KDL;

int main(int, char**)
{
   Tree tree("root");
   TreeFkSolverPos_recursive tree_solver(tree);
   Frame frame;
   JntArray jnt_array(0);
   int err = tree_solver.JntToCart(jnt_array, frame, "root");
   std::cout << err << std::endl;
}

[MatthijsBurgh]

@smits @meyerj This is a nasty bug. I am not completely sure where to fix this bug. IMO, initializing the root TreeElement with q_nr(0) is incorrect as there are no joints in a tree with only a root segment. So the best is to fix that. That could lead a lot of required changes. So unsure if we want that, though it is the cleanest solution. Another solution is to add some more checks to TreeFkSolverPos_recursive. Which is more a hack IMO.

[mganglb]

Ty for your quick response, I just want to point out, that this problem exists with every tree what is build out of exclusively fixed joints.

I also tested the TreeJntToJacSolver what appears to have obv. The same issue.

   std::cout << "started";
   Tree tree("root");

   Joint rx = Joint(Joint::RotX);//Rotational Joint about X
   Joint fixed1 = Joint(Joint::None);//Rigid Connection
   std::string name("seg1");
   Segment segment1 = Segment(name, fixed1,
                Frame(Rotation::RPY(0.0,M_PI/4,0.0),
                          Vector(0.1,0.2,0.3) )
                    );
   std::cout << "started 1" << std::endl;;
   bool exit_value;
   int err;
   exit_value = tree.addSegment(segment1,"root");
   std::cout << exit_value;
   TreeFkSolverPos_recursive tree_solver(tree);
   Frame frame;
   JntArray jnt_array(0);
   err = tree_solver.JntToCart(jnt_array, frame, "seg1");