Command gear torque A3/A2 error when using kuka_rsi_hw_interface

[RhysMcK]

KUKA System:

Controller- KRC4 compact
Robot- Kr3-540 Agilus

Environment

ROS Distro: Kinetic
Moveit

Problem

Hi all,
I have been struggling with this issue for a while now. While controlling the kuka robot through the RSI hardware interface , every now and again i will receive a command gear torque error, typically for axis A3 or A2. This is an issue which has been reported before (#89) however none of the comments have solved the problem for me personally. I don't believe this to be a communication issue, as this happens when i have not lost any communication packets ( according to the kuka RSI monitor). I have played with the HOLD_ON parameter and that does not seem to make any difference.

A possible suggestion which was bought up in #89 was the tool load data not being set correctly. It is possible that this could be the cause, as i must admit i have not set the payload 100% accurately ( due to not having any cad program currently available to me). I have however entered the mass and estimated the location of CoM and moment of inertia's. However, the reason i don't believe this to be at fault is that i have played with big and small values for the mass/ inertias and have seen no difference in the behavior. I would have thought that setting values significantly far away from the true values would cause this error to happen instantly or more frequently. To me it doesn't seem as if this data has any effect. But i could be very wrong.

A more likely possibility in my opinion is that the motion plans from moveit are not abiding by the acceleration limits, as previously mention in moveit/moveit#416. I will try and get some trajectory plots in the next day or so but in the mean time i just wanted to get this discussion going again and see if people had any other insights into this issue.

Cheers!

[RhysMcK]

Update: This issue persists after i have physically removed the tool from the robot arm. Which surely must indicate that this is not an issue with inaccurate tool load data.

[RhysMcK]

I have had a chance to do some further testing on this issue. It seems the problem is most likely to do with the trajectories that are being sent to the KUKA robot. I recorded all of the position commands that were being set from within the KukaHardwareInterface::write function. From this data i was then able to extrapolate the commanding velocities by taking the difference between the last two commanding positions and assuming cycle time of 12ms. From the figures below, it can be seen that just before i receive this error on A2 there is a noticeable velocity spike.
Trial 1:

Trial 2:

Trial 3:

The Moveit! motion planners definitely seem to be generating non-ideal trajectories, which leads to this issue. There has been discussion previously about the OMPL library (which i am using), potentially causing this issue on Kinetic (moveit/moveit#416). However i'm not sure if this issue has been further discussed/fixed.

A simple, first attempt fix might be to just adjust the command position if the change in velocity ( acceleration) is too great, i.e set a threshold for the acceleration. A very basic moving average filter could be used to determine the command position if this limit is exceeded. I will try this and see how it goes. Any further ideas on this issue would be greatly appreciated it!

Note that adjusting your payload data on the kuka controller will effect when you will receive this error, as the kuka motion planner is looking at the commanded motion, relative to the current inertial conditions, and predicting that the commanded motion will exceed torque limits. $LOAD is taken into account when making these predictions [1]. However, this does not really solve the root of the problem.

Cheers!

[1] https://www.robot-forum.com/robotforum/kuka-robot-forum/command-gear-torque-a23-with-rsi-control/

[gavanderhoorn]

Hi. Thanks for the detailed description.

I believe your analysis could be true: the OMPL version used in Kinetic MoveIt is known to generate (sometimes) discontinous paths, leading to time parameterisation artefacts such as the one you've plotted.

Unfortunately there isn't much we can do about that, other than implementing something like the filter you mention. I'm not entirely sure I'd be willing to do that in the driver though: perhaps an intermediate node should do the filtering before the trajectory gets passed to the driver (the driver (actually: RSI) just requires smooth trajectories, it is the producer's responsibility to provide those).

moveit/moveit#416 has not been resolved, and in fact, the linked issue on the OMPL bitbucket repository (ompl/ompl#309) is also still open. Perhaps commenting on that might help get this some more attention.

In the meantime MoveIt has also seen some changes to the time parameterisation components. moveit/moveit#441 got merged into Kinetic. The interplay with the OMPL issue is not clear to me, but perhaps one is related to the other.

An interesting PR that has not been merged yet could be moveit/moveit#809: this adds a parameterisation plugin based on TOTG. In my experience this produces rather impressively smooth trajectories, but I'm again not sure how the OMPL issue influences this.

From this data i was then able to extrapolate the commanding velocities

You're probably aware, but the driver only uses position control. Any velocities are only the distance travelled in one interpolation period.

[gavanderhoorn]

@rtonnaer

[gavanderhoorn]

Unfortunately there isn't much we can do about that, other than implementing something like the filter you mention. I'm not entirely sure I'd be willing to do that in the driver though: perhaps an intermediate node should do the filtering before the trajectory gets passed to the driver (the driver (actually: RSI) just requires smooth trajectories, it is the producer's responsibility to provide those).

I think I'll change my opinion on this: I would like to add this to the driver, but not to filter (ie: change) the trajectory, but to check it before accepting it for execution.

That way, we can at least avoid executing trajectories that will (probably) lead to problems some time later after we've started executing them.

A complicating factor is that we only provide a ros_control hardware_interface, and do not control any of the infrastructure that accepts and processes the trajectories. That would be joint_trajectory_controller.

Note: the JTC actually does the interpolation of the received trajectory. MoveIt et al influence the initial parameterisation, but the JTC is ultimately the one that interpolates the trajectories and sends the values to the hardware_interface.

[RhysMcK]

Possibly a more robust solution could be to also add a RSI FILTER object on the Kuka side. I have not experimented with this before and the documentation is a bit scarce, but it is possible to implement a bunch of standard filters (butterworth, low-pass, high-pass, etc.). A simple low-pass filter should help omit any random jerks and inconsistency outputted from the JTC.

[cschindlbeck]

So apparently this issue has been solved, see moveit/moveit#416 moveit/moveit#869

Can you confirm that this solves your problem?

[gavanderhoorn]

@cschindlbeck: I'm not sure it has been really solved. The MoveIt changes are related, but do not address the cause afaict. Changes in OMPL are needed as well, and until those are released into Kinetic the situation as-is remains.

See also the bitbucket issue.

[gavanderhoorn]

See moveit/moveit#416 (comment) if you can't wait :)

[RhysMcK]

Thanks @gavanderhoorn, i've been keeping a close eye on that thread!.... I'll try this out and see if it makes any improvements. Although i now believe the weird velocity spikes seen the plots above is more likely due to the current default method of time parameterization used in moveit. I'm eager to try either TOPP and/or TOTG when its ready for kinetic :) moveit/moveit#809

[gavanderhoorn]

Getting moveit/moveit#809 to work on Kinetic should not too difficult. It's fairly stand-alone. You would just need to add the plugin and update your MoveIt config to load that parameterisation adapter instead of the default one (it's configured in the OMPL planning adapter section).

[gavanderhoorn]

@RhysMcK: have you had any opportunity to test the fixes in OMPL+MoveIt?

[RhysMcK]

Haven't had a chance yet @gavanderhoorn. This is top of my todo list though, hopefully i get around to it in the next couple days.