Joint Positions Not Updating in Real Time

[anselmo-parnada]

Hi everyone,

For my application, I need to use the real-time joint positions of the robot to feed it into my custom controller.

Upon trying all the sine overlay demos whilst echoing the LBRState in the ROS2 CLI, I found that that measured joint positions do not update in real time. The joint positions printed remain the same even though the robot is performing an oscillating motion. The joint positions will only update when I release the enable key on the SmartPad to halt the LBRServer program that was running on the controller.

Ideally, I would like to have these update with the motion. Oddly enough, the joint efforts are updated in real time, so it does not seem to be a connection issue.

Can somebody please help me with issue? Thank you in advance!

Some hardware/sofware specifications, for reference:

• Robot: KUKA IIWA 14
• FRI SDK Version: 1.11
• ROS2 Distro: Humble

[mhubii]

hi @anselmo-parnada. Thank you for reporting this issue. This is quite odd indeed. Maybe this is specific to 1.11? Will try to have a look. Maybe just to double check, could you

ros2 topic echo /lbr/state --field measured_joint_position

The FRI provides another output, called ipo_joint_position. The ipo_joint_position stays constant and represents the joint position where you first established the connection.

One more thing you can do is to bring up the visualization and check if it updates accordingly

ros2 launch lbr_bringup rviz.launch.py rviz_cfg:=config/hardware.rviz

Thanks for checking!

[anselmo-parnada]

Hi @mhubii,

I looked into it yesterday and have found a workaround for the issue.

In line 63 in state.cpp, the measured joint position is copied to memory like so within StateInterface::set_state_open_loop():

std::memcpy(state_.measured_joint_position.data(), joint_position.data(),
              (double) * fri_state_t::NUMBER_OF_JOINTS);

But in line 23 of state.cpp , the measured joint position is copied to memory in a different way within StateInterface::set_state():

std::memcpy(state_.measured_joint_position.data(), state.getMeasuredJointPosition(),
              sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);

I'm no expert on your codebase, but it seems StateInterface::set_state() is the function that is called at the beginning and end of the FRI connection and StateInterface::set_state_open_loop() is the function called every cycle during the FRI connection. I came to this conclusion as the line in StateInterface::set_state() is the way you would obtain joint position data from the robot using the FRI Client SDK from KUKA, which I confirmed worked on my machine with KUKA's demo FRI clients. The line in StateInterface::set_state_open_loop() was not something I'd seen used in the FRI client SDK, so I was suspicious it was the cause of the issue.

Based on this assumption, I changed line 63 in state.cpp to match line 23 of state.cpp and it seemed to allow the joint positions to be updated in real time!

Not sure if this because of my version of FRI or ROS2, but it seemed to work. I think the joint_position argument going into StateInterface::set_state_open_loop() is not storing up-to-date data, which is why this issue occurs for me.

Might be worth looking into. Hope this helps!

[mhubii]

Thank you for sharing your findings. The rational is open vs closed-loop. There is a configuration

lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_system_config.yaml

Line 23 in c5bf23f

open_loop: true # KUKA works the best in open_loop control mode

When open_loop: false, the actual measurement is fed back. When open_loop: true, the robot is just assumed to be in the commanded position (default).

[mhubii]

This can be an issue in impedance / cartesian impedance control. So maybe for verification @anselmo-parnada, did you run the robot in impedance control mode? Thank you very much for this

[anselmo-parnada]

From my experience, I do not think the issue is tied to the control mode. This issue occured regardless of the control mode chosen, as I experienced the issue on all the sine overlay demos within the lbr_demos_py package. I ensured the the client's yaml files and the LBRServer application were configured according to the documentation, too.

[anselmo-parnada]

For reference, I did first notice this issue when I was playing around with the wrench_sine_overlay.py demo. I'm not sure if that helps narrow things down.

[mhubii]

hi @anselmo-parnada. This issue is now fixed on the humble and rolling branches, so should the drifting you observed.

I do want to apologize for the bug and hope it did not cause any damage. I do my best to support all different cases but am doing this mostly in my spare time. Do keep in mind to always execute in T1 mode first. Thank you for reporting this issue. I'll try to document the load data calibration #235 ASAP.