orocos / orocos_kinematics_dynamics

Orocos Kinematics and Dynamics C++ library
710 stars 409 forks source link

Why does ChainFkSolverPos_recursive break if you modify the Chain used to construct it? #477

Open rsandzimier-machina opened 1 month ago

rsandzimier-machina commented 1 month ago

I am trying to use ChainFkSolverPos_recursive and I noticed that after constructing the solver, if you modify the chain (the one used to construct the solver) it affects the solver. I went through the source code and for the life of me I can't figure out why the solver should share any memory with the Chain used to construct it. From the source code it seems like it makes a copy of the Chain and also makes a copy of the Segments that make up the chain. So I can't figure out where it still shares memory with the original Chain. Could someone point out to me why this is happening?

Here's an example showing what I am seeing. I create a solver using a chain with 1 joint. I can use the solver without issue. But then if I modify the chain (default constructed so there are 0 joints), I get a size mismatch error code. So it seems that the solver shares memory with the original chain, but I can't figure out where/why.

#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <iostream>

int main() {
    KDL::Chain chain;
    chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame()));

    KDL::ChainFkSolverPos_recursive fk_solver(chain);

    KDL::JntArray joints;
    joints.resize(1);
    joints.data(0) = 0.0;

    KDL::Frame frame;
    int result = fk_solver.JntToCart(joints, frame); // Returns 0 (E_NOERROR)
    std::cout << "FK solver result: " << result << std::endl;

    chain = KDL::Chain(); // Modifying the chain affects the solver

    int result2 = fk_solver.JntToCart(joints, frame); // Returns -4 (E_SIZE_MISMATCH)
    std::cout << "FK solver result 2: " << result2 << std::endl;
}

The output when running the above code is:

FK solver result: 0
FK solver result 2: -4
MatthijsBurgh commented 1 month ago

The solver keeps a reference to the chain. So this fully explains the behaviour.

I don't have the time to do the research whether changing it to a copy of the chain, would affect any 'chaining' of solvers or other common ways to use the library.