-
Notifications
You must be signed in to change notification settings - Fork 432
Description
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