rsi_hw_iface: "1073: Command gear torque An" error on any use

[gavanderhoorn]

Context:

• robot: KR 210
• controller: KR C4 (v8.3.22)
• RSI: 3.3
• afaict payload is configured correctly
• controller in T2

Set up kuka_rsi_hw_interface and the controller as I've done before (followed krl/readme.md as always).

State reporting works fine, RSI connection seems stable (quality: 100, total loss: +2 in 1 or 2 seconds, max contiguous loss: 2).

At the "run rqt_joint_trajectory_controller" section I do just that.

After enabling the rqt controller (push the big red button), robot moves slightly (robot is in 0, -90, 90, 0, 90, 0 pose and apparently rqt_joint_trajectory_controller rounds the values in the sliders to two decimals). Even without touching any of the sliders (so just enabling the rqt controller), the controller will go into a fault mode, always giving me Command gear torque An errors (with n any (or sometimes all) of the joints in the robot).

I've verified the in- and outgoing RSI XML documents and they seem reasonable enough. Maximum correction send right before the controller faults is similar to (A1 angle differs slightly each time):

<Sen Type="ImFree">
    <AK A1="1.544213" A2="0.000000" A3="0.000000" A4="0.000000" A5="0.000000" A6="0.000000"/>
    <IPOC>115859599</IPOC>
</Sen>

Running a simple test script (FollowJointTrajectory action client that sends A1 to +5 degrees in 10 seconds (two point JointTrajectory)) also results in the error, but after somewhat more time.

I must be missing something here, but don't see it (any more).

[gavanderhoorn]

@BrettHemes, @tingelst, @pgorczak, @destogl, @bj0rkedal: did you guys ever run into anything like this?

[SebNag]

Are you using an PREEMPTIVE kernel as recommend in the krl/readme.md ?
I had a similar issue. A real time patch of my OS made the connection stable.

[gavanderhoorn]

No, I'm currently running a -lowlatency kernel with kuka_hardware_interface_node at high priority. I'm aware of what the readme says, but didn't want to compile a kernel yet if a low latency one would do the job. And I'm also slightly confused as to why this is suddenly a problem for us.

Tbh I would've expected the RSI Diagnostics screen to show much worse stats if this is the problem, but I guess we'll have to try anyway.

[pgorczak]

FWIW I had similar stability issues before, that could be mitigated by modifying the RSI/ros-control interface to use relative position commands. In that case, the instability was clearly visible when you plotted the measured joint angles (sudden escalating oscillation).

[gavanderhoorn]

It's kind of hard to know for certain without additional tests -- as we committed the cardinal sin of changing more than a single 'variable' in our attempts to diagnose our issue, but it's very likely that incorrect tool load data was the cause of the excessive torque faults I reported in my OP.

Dropped / missing RSI packets will probably exacerbate this, as the controller will attempt an even larger correction than if everything is contiguous, but the controller is now no longer complaining, so we can move on to other things.

Thanks @SNBasti and @pgorczak for your input.

[rtonnaer]

@gavanderhoorn: I'd like to reopen this issue. In the mean while I have changed the configuration with respect to the first post by Gijs:
Configuration

• I am using a RT PREEMPT kernel: uname -r shows 4.14.3-rt5-custom running on:
• a different systemwith: Ubuntu 16.04.3 LTS with ROS Kinetic. Can this cause extra problems? I am using the indigo-devel branch....
• Using ps ax -o pid,ni,cmd |grep kuka_rsi I have identified this:
  4880 0 /home/rik/workspaces/robotmotion_ws/devel/lib/kuka_rsi_hw_interface/kuka_hardware_interface_node position_trajectory_controller/follow_joint_trajectory:=/joint_trajectory_action position_trajectory_controller/state:=/feedback_states position_trajectory_controller/command:=/joint_path_command __name:=kuka_hardware_interface __log:=/home/rik/.ros/log/9b0f1542-d9b3-11e7-88e7-507b9db1a247/kuka_hardware_interface-2.log
  to be the kuka_rsi_hw_interface process.
• Used sudo chrt -p 99 4880 to provide it with real time priority.
• And checked it with: chrt -p 4880 the result: pid 4880's current scheduling policy: SCHED_RR pid 4880's current scheduling priority: 99
• On the controller the original .src file is running (from this repository) and the robot is switched to auto @ 3 to 5 percent speed (I believe this value does not actually matter. Speed settings come directly from ROS).

Behavior
Using the RRTConnectkConfigDefault planner I am planning some very simple and short motions. When the default velocity scaling is used the robot sounds like driving an old car on a bumpy road. A kind of mechanical vibration. When the velocity and acceleration scaling are decreases (to 0.01) this is less, however the motion is off course very slow.

The RSI Diagnostic Monitor on the KR C4 shows (in my opinion) quite good connection results:
Communication cycles: 52167
Total Loss: 71
Connection quality: 71
Maximum contiguous loss: 2

Additional info
We are using the KR210 with additional axes. It is positioned on a KL2000 linear track and has an external horizontal positioner (KP1). Don`t know if this has any relevance, but it does mean I only use RSI_MOVECORR() for A1 up to A6.

[gavanderhoorn]

Seeing as you are on Kinetic, your description makes me think of moveit/moveit#416.

moveit/moveit#160 could also be an influence here (as in: the fact that it hasn't been merged yet).

When the default velocity scaling is used the robot sounds like driving an old car on a bumpy road. A kind of mechanical vibration.

This should definitely not happen.

To see whether it's the time parameterisation you could try a TOTG planning adapter that would take the place of the IPTP.

To see whether the OMPL changes are of influence we could see whether Indigo causes the same issues. An alternative could be to construct a trajectory manually and then submitting it to the driver (you could still use the TOTG or IPTP in those cases).

[gavanderhoorn]

I re-opened the issue, but I just realised: are you still getting the 1073: Command gear torque An errors, or is this something else?

If the latter, we should probably open a new issue.

[rtonnaer]

Seeing as you are on Kinetic, your description makes me think of moveit/moveit#416.

The description there seems very similar to the behaviour I also see. I will perform some more tests this week and specifically look into this.

I think I did not get 1073: Command gear torque after it was running in realtime, but let me verify this before we close it again. I shall reproduce the tests we performed last time: So using the rqt_joint_trajectory_controller and not touching the sliders. Just wait until if and when the robot gives this error.

Side note: I just changed the drive configuration of the robot (removed external positioner) so might take some time before I have it reconfigured (and am able to do the tests).

[hartmanndennis]

#119 probably fixes this.

[BrettHemes]

@rtonnaer update?

[gavanderhoorn]

I've been in contact with @rtonnaer and according to him, the nr of occurences of the reported error(s) has dropped significantly. Unfortunately 'nothing' has changed.

I put 'nothing' between quotes as a lot has changed in the meantime, but nothing that could be necessarily pointed to as the cause. MoveIt happily plans paths and the robot executes them.

This is with HOLDON as suggested by @hartmanndennis in #119 (but we had that enabled for some time already).

[hartmanndennis]

@rtonnaer's problem as described here seems to be a completely different one. @gavanderhoorn are you still getting the original faults of this issue?

[gavanderhoorn]

@hartmanndennis: I'm not sure whether it is a different issue. What makes you conclude that?

[gavanderhoorn]

I do agree that @rtonnaer's description of "mechanical vibration" sounds more similar to what is described by the op in #116.

[hartmanndennis]

Your original issue as I understand it was that the controller faults and aborts the program after some time, whereas rtonnaer never described(at least in this issue) the controller to fault, but having motions with vibrations, which can have many reasons, like your suggestion of kinetic moveit's weird time parametrization.

[gavanderhoorn]

You're correct that as @rtonnaer describes it, this doesn't sound like the same issue. We (I work with him) probably mixed some things up, which led him to post this here.

@rtonnaer: if you're still having this issue, it might make sense to open a new one. Please include a reference to #116 (specifically #116 (comment)).

[Levaru]

We had the same issue or at least similar issue a couple years ago. The robot would execute a trajectory planned with MoveIt and randomly fail with the same error during execution. I noticed that the cumulative link mass in our URDF was much higher than the total weight of the robot. After adjusting those values we never had this problem again.

[gavanderhoorn]

I'm happy you were able to work around this, but I'm surprised by the solution you present.

MoveIt (or: OMPL) does not take dynamics into account. Or at least, it only takes joint velocity and acceleration limits into account after the kinematic path has been planned (and "takes into account" is a bit of a misnomer: those joint limits are fed into the time-parameterisation algorithms MoveIt uses, which don't change the path any more, but add timing information).

mass information in the URDF is not used at all AFAIK.

Perhaps @simonschmeisser can weigh in here as well. He probably has more up-to-date working knowledge of MoveIt.

[Levaru]

No, you're right. I'm sorry, maybe I remembered it wrong. I don't have the old commits anymore since we had to redo our repository but I'm sure it had something to do with the way the trajectory was generated. I believe that if the time between the points was too low, or the distance too high it would require massive torque to reach it and at that point the fail safe would kick in.

[gavanderhoorn]

I believe that if the time between the points was too low, or the distance too high it would require massive torque to reach it and at that point the fail safe would kick in.

this would of course make the KRC complain, yes.

Perhaps you weren't using OMPL, but some other planner which does take dynamics into account?

[Levaru]

No, it was definitely OMPL. Honestly now I don't anymore what fixed it. All this time I believed that MoveIt takes the dynamics into account and thought that it would be impossible to drive the robot at max. possible speed along a trajectory (without knowing the mass & inertia).

I did find an old commit and found this while looking through it. For some reason my planned trajectories were in the form of std::vector<moveit::core::RobotStatePtr> and I would convert them to a moveit_msgs::RobotTrajectory before sending them over move_group. This is the only time that I was modifying the trajectory besides some trajectory_processing time plugins (e.g. IterativeParabolicTimeParameterization).

void robotStatesToMsg(std::vector<moveit::core::RobotStatePtr> approach_traj_states, moveit_msgs::RobotTrajectory& trajectory_msg_out)
{
  // Copy the vector of RobotStates to a RobotTrajectory
  robot_trajectory::RobotTrajectoryPtr robot_trajectory(new robot_trajectory::RobotTrajectory(robot_model_, arm_jmg_->getName()));
  double duration_from_previous = 0;
  double speed = 0.01;
  for (std::size_t k = 0; k < approach_traj_states.size(); ++k)
  {
    duration_from_previous += speed;
    robot_trajectory->addSuffixWayPoint(approach_traj_states[k], duration_from_previous);
  }
 
  // Convert trajectory to a message
  robot_trajectory->getRobotTrajectoryMsg(trajectory_msg_out);
}

Maybe this modification of the duration_from_previous was the fix, idk. I don't have the RSI license/software anymore to test this.