Could FRI use servo control?

[CarlDegio]

I use this package(foxy) with moveit2, and I want to give robot dynamic desired pose. So I choose moveit_servo to achieve this goal.

However, when I set FRI mode as 5ms, position mode, position control and then use moveit_servo to publish command at the frequency of 1hz, the motion stuck every second then go. The higher frequency sounds more dangerous. But I think FRI should support realtime servo because the normal moveit plan and execute runs successfully, where the frequency of sending robot command is higher than 1hz.

Do you have any good suggestions?

[CarlDegio]

I believe I have found two key points:

1. Frequency matching may be important. I use demo in lbr_examples, but when the frequency of FRI mode is different from the timer in code, the manipulator will make noise. It seems that if code don't publish command(which means timer frequency is less), the manipulator will stop motion. And the fast timer and publisher will also lead to some issue.
2. Close loop control is affected by measurement noise. The moveit_servo use /joint_state to obtain current manipulator joints state, and then use controller to publish joint trajectory. But the shake or stuck occured at any frequency even the frequency has matched.

The point1 maybe related to the characteristics of FRI, and the point2 need some filters.

[mhubii]

Hi @CarlDegio, thanks for testing this out and sorry for the late reply. Personally haven't used the Moveit servo yet but as you mention it is certainly related to getting control rates right. But noise could also be an issue as you pointed out.

There are two main threads.

• one is spawned in 'lbr_app_node' in lbr_fri_ros2/src/lbr_app_node.cpp. This thread runs at the rate that is set on the smart pad
• the other thread is the 'ros2_control_node'. This thread runs at the rate specified in lbr_bringup/config/lbr_controllers.yml. Maybe try tuning the update_rate in this yaml file

Please let me know if this helps. Maybe also link your moveit servo here so I can give it a go !

[CarlDegio]

Thanks! I will update my code later (Almost the copy of moveit2 tutorial). And I have a new question, what is the difference between update_rate and state_publish_rate (and action_monitor_rate). The update_rate controls the publish frequency of /joint_state? And the latter control /position_trajectory_controller/state?

[CarlDegio]

Besides, if frequency need to be matched, whether the python code should use realtime tools at such as 200hz or 500hz?

[CarlDegio]

My lbr_servo package link here. And the key setting seems config/kuka_joint_config.yaml.

This file set the publish_period of trajectory_msgs/JointTrajectory, and low pass filter coeff low_pass_filter_coeff.

But now, even I set publish_period the same as FRI mode frequency, the manipulator also run with stuck. This may because wrong frequency setting in lbr_bringup/config/lbr_controller.yml. I haven't verify it.

[CarlDegio]

Is position_trajectory_controller is equal to LBRcommand? Or which is upper or lower. In here I think the trajectory_msgs/JointTrajectory is higher than LBRcommand, which means when we publish /position_trajectory_controller/joint_trajectory, there will be /lbr_command topic also. So the frequency of /position_trajectory_controller/joint_trajectory should be more higher than FRI mode to prevent data loss?
https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/blob/d6d5b4eaa547eba4756add527a37a77c6208fd7c/lbr_hardware_interface/src/lbr_hardware_interface.cpp#L224

[CarlDegio]

I think there are many bugs of moveit_servo because so many issues on moveit2 about it. I decide to calculate motion by kinpy

[mhubii]

Is position_trajectory_controller is equal to LBRcommand? Or which is upper or lower. In here I think the trajectory_msgs/JointTrajectory is higher than LBRcommand, which means when we publish /position_trajectory_controller/joint_trajectory, there will be /lbr_command topic also. So the frequency of /position_trajectory_controller/joint_trajectory should be more higher than FRI mode to prevent data loss?

https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/blob/d6d5b4eaa547eba4756add527a37a77c6208fd7c/lbr_hardware_interface/src/lbr_hardware_interface.cpp#L224

You are correct that it makes sense to run the ros2_control_mode quicker.

The architecture is as follows:

• LBRAppNode creates
  • Real-time safe subscriber to /lbr_command
  • Real-time safe publisher to /lbr_state

It can be used stand-alone to command the robot (see this launch file. Just launching this file will let you command the robot already). The hardware interface is implemented by publishing to /lbr_command and subscribing to /lbr_state. This is done in LBRHardwareInterface.

This architecture was chosen since the LBRs support some control modes that ros2_control doesn't support, so you can always just forward commands to the /lbr_command topic.

I think there are many bugs of moveit_servo because so many issues on moveit2 about it. I decide to calculate motion by kinpy

I think the moveit_servo probably has issues becuase the JointTrajectoryController over-shoots for small motions. This isn't documented anywhere and it seems to be a bug in ros2_controllers. You can use any kinematics library to control the robot.

[mhubii]

Might be re-investigated as part of #137 or in a general documentation PR with #133, #134, #131

[mhubii]

MoveIt servo is now added, see doc: https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/lbr_moveit/doc/lbr_moveit.html#moveit-servo

One issue, the PID here:

lbr_fri_ros2_stack/lbr_ros2_control/config/lbr_system_parameters.yaml

Line 10 in 95ffd41

pid_p: 0.1 # P gain for the joint position (useful for asynchronous control)

Makes the real system move rather slow at the moment. This remains to be sorted.