Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
rsandzimier-machina opened this issue Oct 4, 2024 · 1 comment

Comments

@rsandzimier-machina
Copy link

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
Copy link
Collaborator

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants