Huge tracking error / delay (off-line planned trajectories, JointTrajectoryController)

[aatb-ch]

I have used successfully the driver with our IRB4600 over EGM by using a position_controllers/JointTrajectoryController, and a velocity_controllers/JointTrajectoryController. I am now trying to optimize the configuration to very closely follow highly-dynamic trajectories and so have been reading around, for example this issue ros-industrial/abb_libegm#111. The config.base.use_velocity_outputs = true allows to send velocity data, and I noticed that it is enabled by default in the driver and writes velocity commands if passed by the controller, like with a velocity_controller.

Unless I've misunderstood Position controllers, they will not forward anything else besides position commands, correct? How should I go about including the velocity data as well without having a PID? Is there any existing method, or trajectory controller available for that? Am I supposed to write my own controller? I have seen the PosVel interface is already implemented in the driver, but I just can not find any information on existing pos_vel_controllers implementation, I've naively tried to use a pos_vel_controllers/JointTrajectoryController but it wildy overshoots and seems like a PID is running on top of it, I just want to simply forward the splined trajectory as-is.

[aatb-ch]

Following up on my previous question, I had a previous homemade implementation of the EGM driver that was simply streaming position setpoints at 4ms, from pregenerated trajectories in a 3d animation software. The robot in RobotStudio perfectly tracks this trajectory, while it completely drifts behind and lags behind target when importing the same trajectory to the joint_trajectory_controller. I have tried importing only pos, pos+vel and pos+vel+acc, no difference. I have tried position_controllers, velocity_controllers with PID, changed tuning parameters, with/without velocity_ff, pos_vel_controllers and neither of them is able to follow the trajectory even close to accurately. Keep in mind I'm specifically discussing highly-dynamic movements, with short accelerations and reaching as close as maximum rated joint speeds. The robot is perfectly capable of doing that in RAPID, but it should be also able to do that over EGM at 250Hz control.

I am starting to wonder if this could come from abb_libegm ? Seems like the joints are somehow not accelerating as fast as they could, it ramps linearly as if artificially limited, it takes almost 0.4s to go from 0 to 3rad/s, which seems.. slow? I am using no filtering (level 0 RAW), position gain 20 and correction gain 1, which should be the maximum possible.

[gavanderhoorn]

The driver attempts to enforce joint limits in a few places, one of which is here for velocity interfaces:

abb_robot_driver/abb_egm_hardware_interface/src/egm_hardware_interface.cpp

Lines 265 to 269 in 72c71f7

	//--------------------------------------------------------
	// Enforce joint limits on the desired commands
	//--------------------------------------------------------
	joint_position_soft_limits_interface_.enforceLimits(period);
	joint_velocity_soft_limits_interface_.enforceLimits(period);

You don't give any information on how you've configured things on the ROS side, so I cannot say this is affecting you or not.

It's also unclear to me how you've configured EGM on the controller exactly. Showing the output of the rws/get_egm_settings service (such as shown in #16 (comment)) could help spot something.

(one thing I've often seen fi with velocity is people not setting pos_corr_gain to 0)

[aatb-ch]

So i'm going down the rabbit hole, if I understand correctly, the trajectory interface for abb_libegm applies linear velocity transitions between goals?

https://github.com/ros-industrial/abb_libegm/blob/ab40aec4fd79e24e3d342d87b41f8375991a1f04/src/egm_trajectory_interface.cpp#L602

If someone with knowledge of the internals of the abb_libegm driver could confirm what I suspect, that would be extremely nice, and also how to bypass that as this totally breaks my use-case if this is the case.

[aatb-ch]

Yes I have already confirmed that the joint position and velocity limits are correct, and these are not exceeded. The issue seems related to the acceleration of the joints.

Here is the full configuration I send before starting the EGM session:

Click to expand

task: T_ROB1
  settings:
    allow_egm_motions: True
    use_presync: False
    setup_uc:
      use_filtering: False
      comm_timeout: 1.0
    activate:
      tool:
        robhold: True
        tframe:
          trans:
            x: 0.0
            y: 0.0
            z: 150.0
          rot:
            q1: 1.0
            q2: 0.0
            q3: 0.0
            q4: 0.0
        tload:
          mass: 1
          cog:
            x: 0.0
            y: 0.0
            z: 10.0
          aom:
            q1: 1.0
            q2: 0.0
            q3: 0.0
            q4: 0.0
          ix: 0.0
          iy: 0.0
          iz: 0.0
      wobj:
        robhold: False
        ufprog: True
        ufmec: ''
        uframe:
          trans:
            x: 0.0
            y: 0.0
            z: 0.0
          rot:
            q1: 1.0
            q2: 0.0
            q3: 0.0
            q4: 0.0
        oframe:
          trans:
            x: 0.0
            y: 0.0
            z: 0.0
          rot:
            q1: 1.0
            q2: 0.0
            q3: 0.0
            q4: 0.0
      correction_frame:
        trans:
          x: 0.0
          y: 0.0
          z: 0.0
        rot:
          q1: 1.0
          q2: 0.0
          q3: 0.0
          q4: 0.0
      sensor_frame:
        trans:
          x: 0.0
          y: 0.0
          z: 0.0
        rot:
          q1: 1.0
          q2: 0.0
          q3: 0.0
          q4: 0.0
      cond_min_max: 0.1
      lp_filter: 100.0
      sample_rate: 4
      max_speed_deviation: 1000.0
    run:
      cond_time: 36000.0
      ramp_in_time: 1.0
      offset:
        trans:
          x: 0.0
          y: 0.0
          z: 0.0
        rot:
          q1: 1.0
          q2: 0.0
          q3: 0.0
          q4: 0.0
      pos_corr_gain: 1.0
    stop:
      ramp_out_time: 1.0"

If I am on the right track, it seems that the joint acceleration is hardcoded in the abb_libegm driver here:

https://github.com/ros-industrial/abb_libegm/blob/ab40aec4fd79e24e3d342d87b41f8375991a1f04/src/egm_trajectory_interface.cpp#L583

And this acceleration value a_ is then used to interpolate velocity between goals

https://github.com/ros-industrial/abb_libegm/blob/ab40aec4fd79e24e3d342d87b41f8375991a1f04/src/egm_trajectory_interface.cpp#L602

And here

https://github.com/ros-industrial/abb_libegm/blob/ab40aec4fd79e24e3d342d87b41f8375991a1f04/src/egm_trajectory_interface.cpp#L664

Which would explain the linear ramp that the joint velocity follows in my previous rqt_plot screenshot, does that make any sense?

[aatb-ch]

And specifically for velocity_controllers I tried with both pos_corr_gain: 0.0 and 1.0. Weirdly when using the velocity_controllers this had a large impact, but I though that position_controllers and velocity_controllers would output either only position corrections (EgmPlanned message) or velocity (SpeedRef message) but it seems that the velocity_controller sends both position and velocity as it was working quite fine, albeit more smooth with pure velocity control.

[gavanderhoorn]

First: this part was written by @jontje, so I'm not able to give you an authoritative answer here.

From reading the code it does appear acceleration ramping is implemented in abb_libegm. However, I'm not sure the EGMTrajectoryInterface is used by EGMHardwareInterface.

Afaik (and IIRC), the driver uses an abstraction called EGMManager, which manages a set of Channels (and associated Configurations). Those are used directly to read/write state data/new references. I don't see any reference (directly or indirectly) to the EGMTrajectoryInterface. That's mainly for use by stand-alone programs.

Using the trajectory interface (from abb_libegm) would also seem like it would not work: ros_control controllers are already capable of interpolating between setpoints, and that's essentially what the EGMTrajectoryInterface does.

And specifically for velocity_controllers I tried with both pos_corr_gain: 0.0 and 1.0.

pure velocity controllers should not work with pos_corr_gain == 1.0. The ABB controller would 'fight' with the incoming velocity references and try to overlay / return to a position command.

Weirdly when using the velocity_controllers this had a large impact, but I though that position_controllers and velocity_controllers would output either only position corrections (EgmPlanned message) or velocity (SpeedRef message)

ros_control controllers don't know anything about EGM, so they don't "output either only position corrections (EgmPlanned message) or velocity (SpeedRef message)".

Controllers in the velocity_controllers package write velocity references to the underlying hw interface. Either as part of a position controller implemented over a velocity interface or as a forwarding controller, which just forwards incoming setpoints directly to the hw interface.

Controllers in the position_controllers package write position references to the underlying hw interface. There are only forwarding implementations available there, no closed-loops.

but it seems that the velocity_controller sends both position and velocity

no, it really doesn't.

Also: velocity_controllers is a package, not a controller. See my earlier comment on this.

as it was working quite fine, albeit more smooth with pure velocity control.

I'm confused by your statements.

It's been a while, but without setting pos_corr_gain to 0.0, the velocity_controllers/JointTrajectoryController (fi) will not work correctly.

---

Edit: the call chain is: EGMHardwareInterface::write(..) -> EGMManager::write(..) -> EGMManager::Channel::write(..) -> EGMManager::Channel::updateTCPRobotJointCommands(..). That last one essentially directly writes position / velocity references into the Protobuf message (after some checks and conversions).

[gavanderhoorn]

Yes I have already confirmed that the joint position and velocity limits are correct, and these are not exceeded.

Could you show what you've configured exactly?

Do you also set acceleration limits (they would get ignored, see here)?

[gavanderhoorn]

Btw, just to clarify: I'm not dismissing your observations.

I'm just confused by various statements you've made.

[aatb-ch]

I have set in the hardware_egm.yaml:

joint_limits:
  joint_1:
    has_velocity_limits: true
    max_velocity: 175
  joint_2:
    has_velocity_limits: true
    max_velocity: 175
  joint_3:
    has_velocity_limits: true
    max_velocity: 175
  joint_4:
    has_velocity_limits: true
    max_velocity: 360
  joint_5:
    has_velocity_limits: true
    max_velocity: 360
  joint_6:
    has_velocity_limits: true
    max_velocity: 500

The joint position limits are set in the urdf. Yes I noticed that any acceleration limits would be ignored, but is there some default one that might be applied?

I can definitely see that sending my non-interpolated list of setpoints (csv list of setpoints at 250Hz) the robot can track perfectly fine, but once the list has been sent as a trajectory and reinterpolated by the controller it does not follow the trajectory accurately (still seems to be correctly interpolated as the desired trajectory state channel is showing the correct track in rqt_plot, while the actual is lagging behind, as if the forwarded setpoint from the controller is limited by the hw interface)

Edit: copy-pasted from the github with the wrong default limits, can confirm the updated values above are loaded on the controller.

[gavanderhoorn]

Joint limits are in radians and radians/sec. The values you show don't make too much sense to me.

Yes I noticed that any acceleration limits would be ignored, but is there some default one that might be applied?

Not according to what I can find in joint_limits_interface.h, but it's easy to test: comment these lines and rebuild the driver:

abb_robot_driver/abb_egm_hardware_interface/src/egm_hardware_interface.cpp

Lines 265 to 269 in 72c71f7

	//--------------------------------------------------------
	// Enforce joint limits on the desired commands
	//--------------------------------------------------------
	joint_position_soft_limits_interface_.enforceLimits(period);
	joint_velocity_soft_limits_interface_.enforceLimits(period);

be sure to not command anything going over the limits though. I'm not responsible for any damage you cause to your hw.

[aatb-ch]

Btw, just to clarify: I'm not dismissing your observations.

I'm just confused by various statements you've made.

No problem. I just tested again and can confirm that the velocity_controllers/JointTrajectoryController behaves exactly the same if using pos_corr_gain 0 or 1.

velocity_joint_trajectory_controller:
  type: "velocity_controllers/JointTrajectoryController"
  joints:
   - joint_1
   - joint_2
   - joint_3
   - joint_4
   - joint_5
   - joint_6
  gains: # Required because we're controlling a velocity interface
    joint_1: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
    joint_2: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
    joint_3: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
    joint_4: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
    joint_5: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
    joint_6: {p: 4,  d: 0.1, i: 0, i_clamp: 0.1} # Smaller 'p' term, since ff term does most of the work
  velocity_ff:
    joint_1: 1
    joint_2: 1
    joint_3: 1
    joint_4: 1
    joint_5: 1
    joint_6: 1
  stop_trajectory_duration: 0.5
  state_publish_rate: 250
  action_monitor_rate: 20

Regarding limits, my bad. I read them here https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_bringup_examples/config/ex2_hardware_egm.yaml and assumed they were in deg/s as as I was reading the EGM docs stating MaxSpeedDeviation is 1deg/s, mixup on my side.

I'll try commenting out the limits to see, anyway I'm only working in RobotStudio right now, this is only to figure out where the different behavior is from, we'll not run any of this on the real robot until we're clear what's the issue.

[aatb-ch]

Mystery solved!

Turns out it has actually nothing to do with either ROS nor EGM.

Our brand new robot shipped with the default Motion Process Mode rob1_optimal_cycle_time, once we switched it to rob1_accuracy_mode the robot has been perfectly tracking very high accelerations and velocities, it's just zipping around and stopping in a blink. Absolutely nothing to do with the previous behavior that was overshooting and lagging crazy. I'd guess that it's a mode optimized to preserve the gearboxes and applies some acceleration limits on top of EGM which are impossible to bypass even in RAW.

[gavanderhoorn]

Good to hear you figured it out.

Also good to hear the infrastructure here wasn't causing it.

I've updated the title to better capture what you observed, which I believe will also make it easier to find by future users.

[aatb-ch]

I might have spoken too fast as this morning the problem was back even with the robot in accuracy mode, I am not sure if I must have not reloaded properly my machine after compiling with/without disabling limits, but we fixed it this morning by commenting out position and velocity joint limits. I'm on a tight project that should end tomorrow but will have time to really dig deeper afterwards, as I'm not sure anymore if it's still something to do with the driver.. or if I am really declaring the limits wrong somehow. I'll post when I have time to do more testing.

[gavanderhoorn]

Have you been able to find anything @aatb-ch ?

[aatb-ch]

I have run some comparison tests yesterday with/without position/velocity limits and there is a clear difference with/without limits compiled in, but I'm not yet sure what seems to be the issue.

At this point it looks like axis 2 velocity seems to be capped in positive direction to 1rad/s, but not in negative, which is very confusing to me so I am for now double-checking everything in the setup, URDF, yaml config files, etc to see if I might not have missed a very obvious mistake. I will run again more tests tonight to double check.

[gavanderhoorn]

@aatb-ch: friendly ping?

[aatb-ch]

Still on my radar! I just couldn't find the time to run more tests, I dont want to report back before having a reproducible isolated test case, but i should have time this week hopefully.

[gavanderhoorn]

Friendly ping again?

[aatb-ch]

Sorry i have not found time yet to go further and my RobotStudio license had expired.

I have now renewed the license to do further tests, as the real robot is on a remote location currently. Its really important for me to figure this one out so ill report as soon as i have more infos.

[gavanderhoorn]

friendly 🛎️

[aatb-ch]

Yes sorry I've been busy on other projects but must work again on this in the next couple of weeks, will report as soon as I find time to get to the end of it.

[dpiet]

@aatb-ch Were you able to track down this definitively? We may be experiencing similar behavior.

[aatb-ch]

Hey, i coulndt find more time to troubleshoot this yet.. definitely the behavior disappears when commenting out enforcing limits in the hardware interface so that clearly limits the problem, could not confirm if the robot mode "accuracy" or "optimal cycle time" was the definitive culprit or a combination of both, its still on my todo list. I also suspect my URDF could have wrong joint settings but need to reserve some time to look into it.

What bot are you using?

[dpiet]

We have seen this behavior on a IRB 4600. Changing to accuracy mode didn't appear to make a difference but we're continuing to investigate.

[aatb-ch]

Ok, could you try commenting out the limits in the hardware interface to check?

abb_robot_driver/abb_egm_hardware_interface/src/egm_hardware_interface.cpp

Line 266 in 32eb0f4

// Enforce joint limits on the desired commands

How does the phenomenon manifests to you?

On our end, with highly dynamic trajectories it looks like the whole thing is going through some kind of low-pass filter as if everything is smoothed out and overshoots any hard accelerations, mostly visible if trying to stop hard, it will overshoot and come back while oscillating around the setpoint a couple times, as if the whole robot is made out of bubble gum.. it looks completely wrong. Once we comment out the limits it behaves perfectly.. but we had also this issue previously when using an in-house developed EGM driver so I still need to dig in further..

[dpiet]

Sorry, this wasn't our issue after all. We're using a different wrapper around the libegm that doesn't have these soft limit enforcements. Our issue was a low MaxSpeedDeviation parameter in the EGMActJoint command of the RAPID setup program that was clamping acceleration lower than what our planner was assuming.

[aatb-ch]

hey, ah that makes sense yes, also have to check the correction gain and if you are in raw or filtered mode. This is something we've had that properly set and the robot can reach high joint velocities, just with these strange overshoots so seems like a different matter, seems like on our end the acceleration gets clamped, not the joint velocity but I cant see acceleration limits in the driver being enforced..

[gavanderhoorn]

It's been quite a while.

@aatb-ch: any updates?

[enricovillagrossi]

@aatb-ch we run into the same issues: huge tracking error and a slight delay (around 0.4-0.5 sec) before starting the trajectory execution. As @dpiet we fix the problem with the tracking error by changing DEFAULT_MAX_SPEED_DEVIATION in RAPID TRobEGM from 1deg/s (default) to and higher value (30deg/s in our case). The DEFAULT_MAX_SPEED_DEVIATION acts on \MaxSpeedDeviation and according to RAPID documentation:

that is the maximum joint velocity change allowed for each joint in deg/s, so If you are receiving from EGM a trajectory with a higher velocity change, it won't be applied; the \MaxSpeedDeviation should be set higher or equal to the limit of the trajectory received from EGM.
We didn't fix the problem related to the delay before starting the trajectory execution. Does anyone see the same problem?

[aatb-ch]

hi @enricovillagrossi, thanks, but in our case this is not it, we've changed this settings fine and this is not the source of the problem. I will finally get access back to the robot and have time to troubleshoot the issue, the problem was disappearing when recompiling the driver with limit enforcing commented out, i will get the bot down in the workshop to run more tests..