KDL::ChainFkSolverPos_recursive incorrectly returning error

[khoshrav]

Hi,
I updated from ROS Indigo running ros-indigo-orocos-kdl 1.3.1 to ROS Melodic running ros-melodic-orocos-kdl 1.4.0.
Something that works fine (KDL::ChainFkSolverPos_recursive.JntToCart ) in Indigo now returns an error
The size of the input does not match the internal state
Based on the documentation this occurs when
q_in.rows()!=chain.getNrOfJoints()

However, when I print the rows of the input and the number of joints of the chain they are the same. Even an == operator on them yields true.
This issue was repeated on multiple machines with the same ROS/KDL setup and does not occur in Indigo.

[MatthijsBurgh]

Could you provide an example?

[khoshrav]

Sure, I have an example I have run that produces the same error.
KDL example.zip
It always returns a -4 for the error despite both sizes being 3.
In indigo the code shows no error

[MatthijsBurgh]

@khoshrav This uses a robot_description. Without it, I can't test it. So could you please provide the robot_description or build the chain in the example.

[MatthijsBurgh]

I have checked solvertest.cpp on the master branch. Which has no diff to the release 1.4.0 regarding the functions used. I stepped through the tests with GDB, it doesn't show any weird things and the tests have been succeeding for a long time in CI. So for me it is hard to pin point a cause for the issue you are experiencing.

[khoshrav]

@MatthijsBurgh Sorry about that.
I made a new catkin package (that I built in it's own workspace away from any of my other dependencies).
This zip file should have absolutely everything you need to run the code in isolation.
kdl_test.zip
You can just run
roslaunch kdl_test test.launch and that should launch the xacro with the robot description along with the test code.
There is also an Output folder with the output on Indigo and Melodic.

Yeah, I am also very surprised at the error being thrown, especially since the values of the variables are exactly identical.

[MatthijsBurgh]

Thanks for the package.

When I run your rest unmodified, I also get a size mismatch, but I get zero joints in the chain.

[ INFO][/kdl_test_node][1612651499.274538549]: Number of rows in joint_pos_ in ClassA::compute(): 3
[ INFO][/kdl_test_node][1612651499.274577229]: Number of Joints in Chain in ClassA::compute(): 0

I checked with GDB and indeed the chain is empty. I changed every chain variable to a std::shared_ptr<KDL::Chain>. All arguments containing a chain are changed to std::shared_ptr<KDL::Chain>&. After these changes, all prints show 3 and the error code is 0.

So I think your issue is caused by bad management of variables etc.
I have included the adapted package.
kdl_test2.zip

[khoshrav]

Oh wow, thanks!
I'm not sure why that would be, but your fix at least lets us move forward.
Do you have any thoughts about why passing the variables as a shared pointer works but not as a reference?

In either case, I am willing to close this issue (although I am still unsure about why it is occuring)

[MatthijsBurgh]

I does work with references too, but in your example, you created a new copy in classA. Both the argument of the constructor was just a plain class as the member variable.

Right now I do get the same error as you, both of size 3, but with the size mismatch error. In GDB in QtCreator, the expression evaluation of chain_.getNrOfJoints() doesn't return a value, while chain_.nrOfJoints does return 3. So this is an indication that stuff isn't working correct.
Update: This keeps happening after changing everything to references, so this isn't related to the issue.

When only changing the argument of the constructor of classA to a reference, it already works correctly.

[MatthijsBurgh]

No activity

[ghost]

I have a similar issue using the python bindings after upgrading ros:foxy to ros:humble.

        frame = kdl.Frame()
        joint_array = list2jntArray(joint_state)
        status = kdl.ChainFkSolverPos_recursive(self.get_chain()).JntToCart(joint_array, frame)
        if status == -4:
            raise RuntimeError("Forward kinematics failed with size issue "
                               f"where joint state array size is {joint_array.rows()} "
                               f"and joint chain size is {self.get_chain().getNrOfJoints()}.")

The output:

RuntimeError: Forward kinematics failed with size issue where joint state array size is 1 and joint chain size is 1.

The relevant KDL C++ code:

int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr)    {
        unsigned int segmentNr;
        if(seg_nr<0)
            segmentNr=chain.getNrOfSegments();
        else
            segmentNr = seg_nr;

        p_out = Frame::Identity();

        if(q_in.rows()!=chain.getNrOfJoints())
            return (error = E_SIZE_MISMATCH);  // -4
        else if(segmentNr>chain.getNrOfSegments())
            return (error = E_OUT_OF_RANGE);
        else{
            int j=0;
            for(unsigned int i=0;i<segmentNr;i++){
                if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                    p_out = p_out*chain.getSegment(i).pose(q_in(j));
                    j++;
                }else{
                    p_out = p_out*chain.getSegment(i).pose(0.0);
                }
            }
            return (error = E_NOERROR);
        }
    }

@MatthijsBurgh any ideas what might it be?

[MatthijsBurgh]

@bbk-riact I suggest you start with a very simple cpp example. Run it in debug mode and step through it. So you can check the variables etc.

It is also giving me a hint of memory issues. Which results in reading the wrong values.

[aaron-riact]

I think what has happened in both the examples here is the because the class ChainFkSolverPos_recursive (and also at least ChainJntToJacSolver but perhaps all of the others) holds the private Chain member only as a reference, then the referred-to object has gone out of scope before chain is used in methods like JntToCart.

Perhaps this issue can be closed with a PR to the docs on the constructors of these classes reminding the user that its their responsibility to ensure the lifetime of Chain is valid?

[MatthijsBurgh]

When @aaron-riact would be correct, the problem would be solved if you save the chain before forwarding it to the solver.

So @bbk-riact please test with assigning the chain to a separate variable first, which you pass on as argument to the solver.