E_NOT_UP_TO_DATE error when using ChainIdSolver_RNE::CartToJnt for a real-time controller

[wych1005]

Hi @smits @MatthijsBurgh

I have been trying to run the model-based real-time controller from https://github.com/ufrgs-ece/ufrgs_wam on ROS Noetic. I am using Ubuntu 20.04.3 LTS on a Lenovo Yoga C930, and am using the 5.15.0-43-generic kernel (as opposed to the PREEMPT_RT-patched one), as the CPU load is not too high. I have configured my user limits as follows:

username soft cpu   unlimited
username -  rtprio 99
username -  nice -20
username -  memlock unlimited

As far as I can see, I have not encountered any issues with setting the scheduler to SCHED_FIFO, or memory locking when I launch the computed torque controller with roslaunch wam_controllers computed_torque.launch. However, when I press the play button in Gazebo, I run into the said E_NOT_UP_TO_DATE error.

I have tried to build this version of orocos_kdl in my catkin workspace (as described in #266), but got a controller spawning error, similar to this. Thus, I have been using the kdl package installed via apt-get liborocos-kdl-dev.

The following modifications were made to computed_torque_controller.cpp for debugging purposes:

Similar to what was done in #266, I made nj and ns public in /usr/include/kdl/chainidsolver_recursive_newton_euler.hpp and modified the following lines in the update() function:

if(idsolver->CartToJnt(q,dq,v,fext,torque) < 0)
{
    ROS_INFO("nj = %u, ns = %u", idsolver->nj, idsolver->ns);
    // ROS_ERROR("KDL inverse dynamics solver failed.");
    // ROS_INFO("CartToJnt: %d", idsolver->CartToJnt(q,dq,v,fext,torque));
}

and got the following results : nj = 0, ns = 3279928368. I also checked the values of chain.getNrOfJoints() and chain.getNrOfSegments() after the creation of the chain object in the init() function, and got chain.getNrOfJoints() = 7 and chain.getNrOfSegments() = 9, as expected.

[MatthijsBurgh]

Please make an example as simple as possible, which reproduces the issue.

Then after each line we can print nj and ns. So we can detect what corrupts the data.

[wych1005]

Apologies for the late reply. IMHO due to its dependence on things such as the JointStateController, the simplest/most practical example to attach is this version of the package, which works in a clean ROS Noetic catkin workspace (with the settings mentioned in my previous message) - in case you're interested in testing it out on your machine (with the real-time configuration, etc. that you see fit).

The previously-mentioned roslaunch command still applies, and the error may be triggered by pressing the play button in Gazebo. It is also possible to send a reference JointTrajectoryPoint with the move_home.sh script in wam_controllers/scripts. In the meantime, I will continue trying to identify the source of this error, and will post an update if I discover anything noteworthy.