ChainIdSolver_RNE constructor get wrong nj and ns

[DimitriEckert137]

@smits I have a KDL chain and want to construct a ChainIdSolver_RNE for it. I do it the following way (MELO_LOG is just to print out what the program is doing, I changed nj and ns from private to public in order to be able to access it):

MELO_INFO("chain.getNrOfJoints(): %u, chain.getNrOfSegments(): %u", chain.getNrOfJoints(), chain.getNrOfSegments());
KDL::ChainIdSolver_RNE TestSolver(chain, g);
MELO_INFO("nj: %u, ns: %u", TestSolver.nj, TestSolver.ns);
TestSolver.updateInternalDataStructures();
MELO_INFO("nj: %u, ns: %u", TestSolver.nj, TestSolver.ns);
MELO_INFO("chain.getNrOfJoints(): %u, chain.getNrOfSegments(): %u", chain.getNrOfJoints(), chain.getNrOfSegments());
MELO_INFO("TestSolver.chain.getNrOfJoints(): %u", TestSolver.chain.getNrOfJoints());

The output of this part of the code is:

[ INFO] [1592818814.802771651]: chain.getNrOfJoints(): 6, chain.getNrOfSegments(): 10
[ INFO] [1592818814.802839467]: nj: 0, ns: 4294954208
[ INFO] [1592818814.802874958]: nj: 0, ns: 4294954208
[ INFO] [1592818814.802902799]: chain.getNrOfJoints(): 6, chain.getNrOfSegments(): 10

Obiously this leads to an E_NOT_UP_TO_DATE = -3 when CartToJnt{} is called. This doen't really make sense to me since according to the constructor nj and ns get initialized with getNrOfJoints and getNrOfSegments. How can this happen?

EDIT: I work with the system version (1.4.0). The parsed robot is a UR3e.

[MatthijsBurgh]

I tested it on my own machine with the use of kdl_parser to get a tree from the urdf file from ur_description. After which I used tree.getChain("world", "ee_link" (Results in 6 Joints, 8 segments). But I don't get such a problem. Not on master, not on 1.4.0 tag.

This is also tested in CI: https://github.com/orocos/orocos_kinematics_dynamics/blob/de3e8ea92c2ffc07b000bbbf3eaee322916f2149/orocos_kdl/tests/solvertest.cpp#L184-L329

So please test with clean build, etc. So please run make check to see if the test succeeds on your machine. (Please run it multiple times, as it sometimes fails on precision, which is going to be fixed soon.)

As I am not able to locate the issue.

[DimitriEckert137]

Thank you very much for your reply! Stupid question: where do i do make check? In the catkin workspace? When i do it there it says: make: *** No rule to make target 'check'. Stop.

(Probably because i installed via apt, so there is no package of kdl in the catkin workspace?)

[DimitriEckert137]

Ok I tried with cleaning the whole workspace and rebuilding but got the same result, also after several attempts. When I try to access TestSolver.chain.getNrOfJoints() directly I get a segfault (I changed the chain from private to public). When I manually set the nj by hand like this: TestSolver.nj = ur_chain.getNrOfJoints(); Then the value is (obviously) correct at 6. But it even remains at 6 when I afterwards do: TestSolver.updateInternalDataStructures(); which according to the code should set: nj = chain.getNrOfJoints();

[MatthijsBurgh]

Thank you very much for your reply! Stupid question: where do i do make check? In the catkin workspace? When i do it there it says: make: *** No rule to make target 'check'. Stop.

(Probably because i installed via apt, so there is no package of kdl in the catkin workspace?)

It needs a little bit more work.

catkin build orocos_kdl --force-cmake -DENABLE_TESTS=ON # in catkin workspace; --force-cmake is just to be sure cmake is run
cd catkin_ws/build/orocos_kdl
make check

[MatthijsBurgh]

Ok I tried with cleaning the whole workspace and rebuilding but got the same result, also after several attempts. When I try to access TestSolver.chain.getNrOfJoints() directly I get a segfault (I changed the chain from private to public). When I manually set the nj by hand like this: TestSolver.nj = ur_chain.getNrOfJoints(); Then the value is (obviously) correct at 6. But it even remains at 6 when I afterwards do: TestSolver.updateInternalDataStructures(); which according to the code should set: nj = chain.getNrOfJoints();

Could you please provide some information about your system. As all the test we run are on ubuntu with gcc/clang.

[DimitriEckert137]

It needs a little bit more work.

catkin build orocos_kdl --force-cmake -DENABLE_TESTS=ON # in catkin workspace; --force-cmake is just to be sure cmake is run cd catkin_ws/build/orocos_kdl make check

When i run the first command i get: [build] Given package 'orocos_kdl' is not in the workspace

I might be completely mistaken, but i guess this is because i installed it not into the workspace by cloning orocos_kdl from git but installing it via sudo apt install ros-melodic-orocos-kdl?

[DimitriEckert137]

Could you please provide some information about your system. As all the test we run are on ubuntu with gcc/clang. I am using Ubuntu 18.04.4 LTS on a Lenovo Thinkpad L380. My ros version is melodic. Any other information that you need?

[MatthijsBurgh]

How can you change if from private to public if you are using the system version?

In case you are using the system version, you can't run the tests.

It might just be that the release version has a bug, not the code itself.

[DimitriEckert137]

How can you change if from private to public if you are using the system version?

I changed the chainidsolver_recursive_newton_euler.hpp file in /opt/ros/melodic/include/kdl. The .cpp on the other hand is not there.

In case you are using the system version, you can't run the tests.

It might just be that the release version has a bug, not the code itself.

Ok so do you think I should clone the github version?

[MatthijsBurgh]

Yes, and checkout the v1.4.0 tag. In case that works, the released library is broken.

EDIT: try reinstalling the library, sudo apt-get install --reinstall ros-melodic-orocos-kdl

[DimitriEckert137]

Ok reinstalling the library seems to fix it halfway.
At least at initialization it works but somehow the "chain" member of the solver gets lost along the way.

I have a class that approximately looks like this:

class UrKdl {
  UrKdl(){
    // parse tree from urdf file
    // get chain from tree
    invDynSolver = new KDL::ChainIdSolver_RNE(ur_chain, g);
    MELO_INFO("nj: %u, ns: %u", invDynSolver->nj, invDynSolver->ns);
    MELO_INFO("invDynSolver->chain.getNrOfJoints(): %u", invDynSolver->chain.getNrOfJoints());
    MELO_INFO("invDynSolver->chain.getNrOfSegments(): %u", invDynSolver->chain.getNrOfSegments());
    invDynSolver->updateInternalDataStructures();
    MELO_INFO("nj: %u, ns: %u", invDynSolver->nj, invDynSolver->ns);
    MELO_INFO("invDynSolver->chain.getNrOfJoints(): %u", invDynSolver->chain.getNrOfJoints());
    MELO_INFO("invDynSolver->chain.getNrOfSegments(): %u", invDynSolver->chain.getNrOfSegments());
  }

sensor_msgs::JointState calculateTorques(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const KDL::Wrenches& f_ext, KDL::JntArray &tau){
    int error = 0;
    MELO_INFO("nj: %u, ns: %u", invDynSolver->nj, invDynSolver->ns);
    MELO_INFO("invDynSolver->chain.getNrOfJoints(): %u", invDynSolver->chain.getNrOfJoints());
    MELO_INFO("invDynSolver->chain.getNrOfSegments(): %u", invDynSolver->chain.getNrOfSegments());
    error = invDynSolver->CartToJnt(q, q_dot, q_dotdot, f_ext, tau);
    MELO_INFO("calculateTorques, %i", error);
    sensor_msgs::JointState torques;
    std::vector<double> effort;
    for(int i = 0; i < tau.data.rows(); i++){
      effort.push_back(tau.data[i]);
      MELO_INFO("%i   %f", i, tau.data[i]);
    }
    torques.effort = effort;
    return torques;
  } 

  KDL::ChainIdSolver_RNE* invDynSolver;
};

Now during the constructor the numbers seem to be correct:

[ INFO] [1593424952.716213959]: nj: 6, ns: 8
[ INFO] [1593424952.716245162]: invDynSolver->chain.getNrOfJoints(): 6
[ INFO] [1593424952.716282291]: invDynSolver->chain.getNrOfSegments(): 8
[ INFO] [1593424952.716396584]: nj: 6, ns: 8
[ INFO] [1593424952.716319075]: invDynSolver->chain.getNrOfJoints(): 6
[ INFO] [1593424952.716360761]: invDynSolver->chain.getNrOfSegments(): 8

But wihin the "calculateTorques" function not:

[ INFO] [1593424953.740712754, 8.393000000]: nj: 6, ns: 8
[ INFO] [1593424953.740786068, 8.393000000]: invDynSolver->chain.getNrOfJoints(): 0
[ INFO] [1593424953.740897040, 8.393000000]: invDynSolver->chain.getNrOfSegments(): 0
[ INFO] [1593424953.741103253, 8.394000000]: calculateTorques, -3
[ INFO] [1593424953.741240792, 8.394000000]: 0   0.000000
[ INFO] [1593424953.741340976, 8.394000000]: 1   0.000000
[ INFO] [1593424953.741404192, 8.394000000]: 2   0.000000
[ INFO] [1593424953.741529383, 8.394000000]: 3   0.000000
[ INFO] [1593424953.741614483, 8.394000000]: 4   0.000000
[ INFO] [1593424953.741667670, 8.394000000]: 5   0.000000

Which can also be seen from the error -3 ("E_NOT_UP_TO_DATE") that is thrown. I dont actively change the chain within the solver anywhere. Is there a reason it gets set back to 0 somewhere?

[DimitriEckert137]

And when I clone the github version with the tag v1.4.0 I get the issue that rviz dies everytime (same as here). And the behavior described above is also the same except that here after the second time it passes the calculateTorques function the numbers look like this:

[ INFO] [1593431867.625808842, 6.725000000]: invDynSolver->chain.getNrOfJoints(): 1431727408
[ INFO] [1593431867.625890621, 6.725000000]: invDynSolver->chain.getNrOfSegments(): 21845

[MatthijsBurgh]

As it does work on construction, but not later in your program. I can't think of anything else, that it is your program that corrupts the data. As also in the CI tests it succeeds. Please make an as simple test as possible. Prevent the usage of pointers and references.

[MatthijsBurgh]

Stale issue, closing