Torque and wrench overlay demos do not work

[WZHwerk5]

Hi, I have tried demos in lbr_demos_fri_ros2_cpp, the JointSineOverlay works smoothly but wrench and torque not. After launch there is no error/warning message from both ROS2 side and FRI side, and I can even see the command topic with values in rqt, but on our iiwa7, nothing happens. Need your help!

Thanks in advance!

[WZHwerk5]

I've also tried the FRI-Client-SDK_Cpp, and wrench/torque overlay both worked.

[mhubii]

hi @WZHwerk5 and thank you for the feedback. May I ask what procedure you followed?

[WZHwerk5]

I followed the guidance herehttps://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/lbr_demos_fri_ros2_cpp/doc/lbr_demos_fri_ros2_cpp.html

[mhubii]

is a connection established to the robot? What can you see on the smart pad?

[WZHwerk5]

Yes, the FRI connection can be established. Both smart pad and my Ubuntu terminal have no error/warning message.

[mhubii]

okay, the issue is probably the QoS policy

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/c3766c61047382a1b39dd9dacd7e163f4b5ff66b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp#L25

May have gone out of date. I'll test and be back in half an hour

[WZHwerk5]

Actually I have manually set it as 1ms, but still does not work...

[mhubii]

are you on the humble branch?

so in LBRWrenchSineOverlay.java and LBRTorqueSineOverlay.java you'll find

friConfiguration.setSendPeriodMilliSec(5);

The lbr_fri_ros2 package therefore creates pub/sub with 5 ms QoS deadline.

To fix:

1. git pull changes (there was a missing set: https://github.com/lbr-stack/lbr_fri_ros2_stack/commit/0b0dede3cbadd2b888492e550e02ea0caabfaab8)
2. change deadlines to 5 ms:

pub: std::chrono::milliseconds(10) -> std::chrono::milliseconds(5) https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/c3766c61047382a1b39dd9dacd7e163f4b5ff66b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp#L25

sub: std::chrono::milliseconds(10) -> std::chrono::milliseconds(5) https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/c3766c61047382a1b39dd9dacd7e163f4b5ff66b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp#L32

3. re-compile:

   colcon build --packages-select lbr_demos_fri_ros2_cpp lbr_fri_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release

This is a bug and should be updated. Sorry for any inconvenience and thank you for reporting this issue!

[mhubii]

To be fixed in #116

[WZHwerk5]

Thank you so much!!

[mhubii]

Np. Note, when you push too hard against the robot you might see the connection drop. This has to do with the command guard and how velocity limits are checked. This needs fixing

[WZHwerk5]

Okay I see. By the way, is it possible to dynamically change the impedance values (stiffness and damping) of Cartesian impedance mode?

[mhubii]

no sorry, the FRI doesn't support that. You'd have to restart the FRI each time. That's on KUKA

[WZHwerk5]

I see, sorry but another question is: I want to control the robot joint position together with wrench, but when I launched the bringup in wrench command mode, the robot started to move suddenly with high velocity and was quickly stopped because it is not on the planned path, according to FRI message. Do you know why? Many thanks.

[mhubii]

Thank you for the feedback @WZHwerk5 . In wrench command mode the FRI takes target wrench and target position as input. Maybe you set the target position to zero (or did not set it at all in which case it is zero initialized) as opposed to the current joint configuration?

The wrench is then applied with respect to the target position (spring in cartesian space)

See for example this line https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/7d2717d7426092f55a6a6ef555184213a3be55b5/lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp#L41

IPO joint position is the position where you connect to the robot (it remains unchanged)

I think there is a flaw with this repository, that is there is one FRI client to handle all cases. There should rather be 3 command messages instead of one, this might be addressed in #116:

• Position command mode: Position input
• Wrench command mode: Position and wrench input
• Torque command mode: Position and torque input

Let me know if this helps

[WZHwerk5]

Thanks, it's very clear. But actually I was using the default bringup launch file bringup.launch.py with moveit:=true, so the target position should not be zero or not set, am I right?

[WZHwerk5]

Or do you think that in this case I must send eg. ipo_joint_position additionally as a target command?

[mhubii]

Ah okay I understand. Sorry for the late reply.

So on the smartPad you chose:

Control mode: Cartesian Impedance Command mode: Position?

I have to admit that doing this through the bringup isn't well supported for all the possible cases right now. The Cartesian Impedance controller is a bit prone to singularities. Do you require higher level libraries like MoveIt?

I would love to understand your goal a little better so I can try to help.

[WZHwerk5]

On the smartPad I chose:

Control mode: Cartesian Impedance Command mode: Wrench

What I want is to use the bringup with Moveit, then add a wrench overlay, so that my iiwa can execute surface processing task like polishing or sanding by planning defined paths. What do you think of it? Thanks!

[mhubii]

Sorry @WZHwerk5 , been caught up. Will investigate this now. Thank you for sharing this issue.

For anyone trying to solve the issue, this would be an approach:

• Turn the wrench command controller into a chained controller
  • current controller: https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp
  • ros2_control doc: https://control.ros.org/humble/doc/ros2_control/controller_manager/doc/controller_chaining.html
  • give it the ability to overlay a wrench at the end-effector
• load the joint trajectory controller as usual

[WZHwerk5]

Hi, I already turned the wrench command controller into a chained controller, now should I launch the bringup with wrench command controller in wrench command mode first, then load the joint trajectory controller from another launch file?

[mhubii]

wow sounds awesome @WZHwerk5! Would you want to open a pull request? I personally can't answer the question as I haven't investigated