incorrect ros control HardwareInterface implement?

[jacknlliu]

I use the joint trajectory controller to send current joint position with the iiwa_hw and ros control, but java client received the joint position is always from zero position, then the robot moves to zero position very quickly then halt. This is very dangerous for our robots.

[SalvoVirga]

I was checking yesterday your PR #210 and I believe this is caused by those changes.
Are you using that PR?
You should try to revert those changes (or use master) and check the behavior, the joint_trajectory_controller should work out of the box in master.

[jacknlliu]

@SalvoVirga I didn't use the PR, just the master branch here. And I can confirm that the joint_trajectory_controller can't work in master.

I use the following launch file for starting the ros control node:

<?xml version="1.0"?>
<launch>

<arg name="hardware_interface" default="PositionJointInterface"/>
<arg name="robot_name" default="iiwa"/>
<arg name="model" default="iiwa7"/>

<include file="$(find iiwa_description)/launch/iiwa7_upload.launch">
    <arg name="hardware_interface" value="PositionJointInterface"/> 
    <arg name="robot_name" value="iiwa"/>
    <arg name="origin_xyz" value="'0 0 0'"/> <!-- Note the syntax to pass a vector -->
    <arg name="origin_rpy" value="'0 0 0'"/>
</include>

    <!-- Robot interface -->
    <include file="$(find iiwa_hw)/launch/iiwa_hw.launch">
        <arg name="hardware_interface" value="$(arg hardware_interface)" />
    </include>

      <!-- start the ros controller: pos_joint_trajectory_controller -->
    <include file="$(find iiwa_control)/launch/iiwa_control.launch">
        <arg name="hardware_interface" value="$(arg hardware_interface)" />
        <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
        <arg name="robot_name" value="$(arg robot_name)" />
        <arg name="model" value="$(arg model)" />
    </include>

</launch>

[SalvoVirga]

I don't have an iiwa at hand at the moment, so it's a bit hard to test that launch file.
Did you try to just launch the MoveIt interface with

roslaunch iiwa_moveit moveit_planning_execution.launch sim:=false

That should do internally exactly what your launch file does (plus spawning the MoveIt interface), if that works you know somehow your launch file is not entirely correct and you could trace back what the moveit launch file calls to fix it.

[jacknlliu]

I change the number of threads of the spinner in hardware interface node

from

iiwa_stack/iiwa_hw/src/main.cpp

Line 45 in d921597

ros::AsyncSpinner spinner(1);

to

 ros::AsyncSpinner spinner(3); 

Then it works!

But this implement of hardware interface for ros control has some other issues, I have patched more things than here.

And the default control frequency 1kHz also needs a real-time kernel, or the discontinuous motion will occur. There is much work to do for a reliable ros control node for high-frequency control.

NOTE: I find that the spinner with one thread can't work because that the ros rate initialized not correctly, the ros rate period is actually zero! I have patched this and I will provide a PR for this.

[jacknlliu]

Just test the old version with commit id 489b355, the ros control also doesn't work correctly because of the initial joint state incorrect from java side(the initial joint state is always zero from LBR) about a few seconds. I think some problem on the Kuka sunrise.

The only possible working situation is that you always start the ros control when the robot is at zero position.

[jacknlliu]

I will file a PR for this. But the smart servo interface has large delay (up to 300ms) when sending commands to the RTOS (Sunrise 1.11 with the latest iiwa stack java application), so the trajectory will have a little discontinuity when using ros control.

[jacknlliu]

SmartServo interface is not a real-time interface, so many delay behavior exists. And using SmartServo to control the robot in a real-time way is not possible. But the FRI interface lacks many functions, so It's hard to choose.

[JimmyDaSilva]

Hello, I am facing the same problem. There is too much delay when using SmartServo and the ROSJava interface. Even when sending commands at 50Hz, I can clearly see a delay between the state and the command.
I tried:

• using TCP_NODELAY with the TransportHints of rosjava subscriber.
• installing a linux low-latency kernel
• reducing the control frequency to 20Hz
• publishing directly to '/iiwa/command/...' without the use of iiwa_ros

@jacknlliu, did you find any working solution?

I tested DirectServo with the Matlab implementation in KST. I didn't get any delay neither on Windows or Linux. It's promising but the Matlab interface is limiting.

@SalvoVirga, do you plan to create an interface the DirectServo functions as well?

Many thanks,
Jimmy

[jacknlliu]

@JimmyDaSilva I've studied this problem more deeply. The root cause of this problem is that there is a random delay in the SmartServo interface. I suggest you try the epfl-lasa/iiwa_ros.

[JimmyDaSilva]

The problem is that it uses the FRI interface, like ahundt/grl.
Thinking about the final product, I wanted to stay with the DirectServo and SmartServo modes.
Does using the FRI imply doing torque control on a real-time OS ? or not ?

Thanks a lot @jacknlliu !!

[jacknlliu]

@JimmyDaSilva the FRI interface also support position control interface, see here.

If you want to use the SmartServo mode, I think only a few hertz frequencies are allowed for real-time control.

[JimmyDaSilva]

@jacknlliu When sending position commands to the position controller of the FRI, does it pass to a trajectory validator as well? Do you get good (or better) tracking performances?

[jacknlliu]

@JimmyDaSilva To get dynamic tracking performance should be easy with high stiffness parameters.