IFL-CAMP / iiwa_stack

ROS integration for the KUKA LBR IIWA R800/R820 (7/14 Kg).
Other
337 stars 250 forks source link

incorrect ros control HardwareInterface implement? #212

Open jacknlliu opened 5 years ago

jacknlliu commented 5 years ago

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 commented 5 years ago

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 commented 5 years ago

@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 commented 5 years ago

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 commented 5 years ago

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

from

https://github.com/IFL-CAMP/iiwa_stack/blob/d921597de8ead744f7f452a3a4c822e663334445/iiwa_hw/src/main.cpp#L45

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 commented 5 years ago

Just test the old version with commit id 489b355926daddb3d11c5553b997709d785defed, 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 commented 5 years ago

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 commented 4 years ago

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 commented 4 years ago

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:

@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 commented 4 years ago

@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 commented 4 years ago

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 commented 4 years ago

@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 commented 4 years ago

@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 commented 4 years ago

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