Problems with 3-dimensional (x,y,z) motion with abb_libegm

[shobhitcito]

Task: I have a task that requires x-y tracking in a given plane and at a fixed-known time the robot executes a deterministic motion along the z-direction.

System Specification: I am working with ABB IRB-1200, RobotWare: 6.8, ROS, abb_libegm

The questions that I would like to explore through this forum are explained below. I have discussed these questions with @gavanderhoorn and we decided to put them here to get a wider perspective from the experience users/researchers here.

1. To execute the z-axis motion while robot is performing the tracking task in x-y using EGM. I used the abb_libegm as explained in the below code snippet:

   current_pose.CopyFrom(input.feedback().robot().cartesian().pose());
   gsm.rob_z = current_pose.position().z();
   
   if(gsm.rob_z>z_reference-185 && down==true)
   {
     output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_z(gsm.rob_z-20);
     output.mutable_robot()->mutable_cartesian()->mutable_velocity()->mutable_linear()->set_z(-4.0*(gsm.rob_z-315));
   }

   As it is evident, i give robot the set points both in position & velocity, if i give only one of them, the robot moves but does not execute the complete motion (the robot goes down from current z-position to 315mm in the robot frame). In other words, using only the velocity inputs to the robot the robot does not move till the desire position i.e. 315 mm in the above code.

2. Also, using both the position & velocity inputs I could move the robot. But results in simulation on RobotStudio varies drastically with the tests on the actual robot (ABB IRB1200). For instance, the scaling parameter in the code for velocity is 4.0 while in the actual tests it works only till 1.5 and robot ends up showing excessive dynamic load. Why so much deviation?

3. What is the latency that I can expect with the abb_libegm. Right now I am working with ROS1 and depending on the EGM tuning parameters (filter parameters, position correction gain, maxSpeed deviation etc) --> the latency that I obtain by plotting data from input.feedback() is 50ms to 100ms. I think that is too much.

4. The effect of using raw EGM vs filtered-EGM (Configuraiton-Motion --> External Motion Interface Data --> Level). It hardly leads to any impact on the EGM performance.

[jontje]

Hi @shobhitcito,

I have some follow-up questions for you.

1. As it is evident, i give robot the set points both in position & velocity, if i give only one of them, the robot moves but does not execute the complete motion (the robot goes down from current z-position to 315mm in the robot frame). In other words, using only the velocity inputs to the robot the robot does not move till the desire position i.e. 315 mm in the above code.

When testing to use only velocity inputs, did you set the EGMRunPose RAPID instruction's PosCorrGain argument to 0? It is required for pure velocity control.

2. Also, using both the position & velocity inputs I could move the robot. But results in simulation on RobotStudio varies drastically with the tests on the actual robot (ABB IRB1200). For instance, the scaling parameter in the code for velocity is 4.0 while in the actual tests it works only till 1.5 and robot ends up showing excessive dynamic load. Why so much deviation?

Have you specified a correct tooldata in the RAPID code? This might be one reason depending on the actual tool you use.

And what arguments do you use for the EGMActPose RAPID instruction? I know that EGM have had a bug (not sure if it has been fixed), which caused EGM to ignore the tool's load if it wasn't set explicitly. However, this is only allowed if the system configuration ModalPayLoadMode has been set to No (Configuration -> Controller -> General RAPID -> ModalPayLoadMode instance).

Then you can do something like this in your RAPID code:

IF GetModalPayloadMode() = 1 THEN
  EGMActPose ... \Tool:=<your tooldata> ... 
ELSE
  EGMActPose ... \Tool:=<your tooldata> \Load:=<your tooldata>.tload ... 
ENDIF

Also, if you have the possibility, then I would recommend that you upgrade to RobotWare 6.10 or newer. It added dynamic reduction of acceleration to EGM, which should mitigate the dynamic load error. It also added a utilization rate field to the EGM feedback messages, which lets the user know if they are sending to aggressive references (can for example be useful for tuning).

3. What is the latency that I can expect with the abb_libegm. Right now I am working with ROS1 and depending on the EGM tuning parameters (filter parameters, position correction gain, maxSpeed deviation etc) --> the latency that I obtain by plotting data from input.feedback() is 50ms to 100ms. I think that is too much.

Hm... that sounds too much (should be around 20 ms, if I remember correctly), but I have no good idea at the moment.

4. The effect of using raw EGM vs filtered-EGM (Configuraiton-Motion --> External Motion Interface Data --> Level). It hardly leads to any impact on the EGM performance.

The filter configuration might be more useful for the IO-signal variant of EGM, compared to the UDP variant used in abb_libegm.

I hope that at least some of this can be helpful!

[shobhitcito]

Hi @jontje

Thank you very much for your answers. I couldn't access the facility to test your suggestions until now but i am planning them next week. I will put my feedback on them ASAP.

I have one additional question: Q. The robot exceeds its dynamic limits when its initial position is far (~100mm) from the data stream coming via abb_libegm (through the sensor). Is it possible to use the first data point for a normal MoveL instruction following which EGMRunPose takes over. Or if you have other suggestions for that.

Q. Also you mentioned here (https://github.com/ros-industrial/abb_libegm/issues/64) that it is possible to change position_correction_gain during runtime. Can you provide more information on this how.

--> do i need to use abb_librws (https://github.com/ros-industrial/abb_librws/issues/49) for both above as I can make changes to RAPID through that or is their any other way (i do not want to compromise the response of the robot as that is extremely important as I am dealing with moving targets) <--

--> or is it possible to use state_machine_interface directly with EGM to switch between pure RAPID module and EGM <--

When testing to use only velocity inputs, did you set the EGMRunPose RAPID instruction's PosCorrGain argument to 0? It is required for pure velocity control.

1. I am using position_correction_gain as high as possible (~0.95) as I perform teleoperation in x-y direction which is position based control. Only concern was that in simulation everything works fine but it deviates in actual test on the robot. I could post some test based trajectories next week.

the above image teleoperates robot in x-y from sensor inputs & executes a deterministic z-axis motion as in the below code (all with position based control via EGM & _position_correctiongain = 0.95

current_pose.CopyFrom(input.feedback().robot().cartesian().pose());
gsm.rob_z = current_pose.position().z();

if(gsm.rob_z>z_reference-185 && down==true)
{
  output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_z(gsm.rob_z-20);
}

using both position & velocity based control as in the original message

current_pose.CopyFrom(input.feedback().robot().cartesian().pose());
gsm.rob_z = current_pose.position().z();
if(gsm.rob_z>z_reference-185 && down==true)
{
output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_z(gsm.rob_z-20);
output.mutable_robot()->mutable_cartesian()->mutable_velocity()->mutable_linear()->set_z(-4.0*(gsm.rob_z-315));
}

the same code results in below response @ the first iteration (every time, after this if i stop the program by the stop button and then the response is as in the above figure (the desired response)):

Have you specified a correct tooldata in the RAPID code? This might be one reason depending on the actual tool you use.

2. For these tests there is no tool mounted on the robot (the gripper is being designed, and i will keep your suggestion in mind)

Hm... that sounds too much (should be around 20 ms, if I remember correctly), but I have no good idea at the moment.

3. I too think that the latency what i had was too much. Could it be that the ROS implementation that uses abb_libegm feedback (input.feedback().robot().cartesian().pose()) is not fast enough and the data being retrieved does not capture the actual delay on the robot motion.

Regards Shobhit

[jontje]

Are you using the RobotWare StateMachine Add-In?

Q. The robot exceeds its dynamic limits when its initial position is far (~100mm) from the data stream coming via abb_libegm (through the sensor). Is it possible to use the first data point for a normal MoveL instruction following which EGMRunPose takes over. Or if you have other suggestions for that.

RobotWare 6.10 should be able to mitigate that, but here are a few other suggestions as well:

• \RampInTime Argument Increase the ramp in time for the EGM motions with the \RampInTime argument (e.g. in the EGMRunPose instruction). This allows for a larger distance to the initial references, but only at the start of a new EGM communication session.

• abb_librws without StateMachine Add-In Add a global robtarget to you RAPID code, and then use RWSInterface to set it. Then you should be able to add a MoveL instruction, which uses the robtarget, before you start the EGM communication session. Rough C++ example:

  abb::rws::RWSInterface rws_interface("127.0.0.1");
  
  abb::rws::RobTarget current_robtarget;
  rws_interface.getMechanicalUnitRobTarget("ROB_1", &current_robtarget);
  current_robtarget.pos.x.value = 360.0;
  current_robtarget.pos.y.value = -140.0;
  current_robtarget.pos.z.value = 600.0;
  
  rws_interface.setRAPIDSymbolData("T_ROB1", "TRobEGM", "pre_egm_rt", current_robtarget);

• abb_librws with StateMachine Add-In Use the RWSStateMachineInterface to move into position before starting EGM. Rough C++ example:

  abb::rws::RWSStateMachineInterface rws_interface("127.0.0.1");
  
  auto waitForIdle = [&]()
  {
    do
    {
      std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    while (!rws_interface.isRAPIDRunning().isTrue() ||
           !rws_interface.services().main().isStateIdle("T_ROB1").isTrue());
  };
  
  waitForIdle();
  abb::rws::RobTarget current_robtarget;
  rws_interface.getMechanicalUnitRobTarget("ROB_1", &current_robtarget);
  current_robtarget.pos.x.value = 360.0;
  current_robtarget.pos.y.value = -140.0;
  current_robtarget.pos.z.value = 600.0;
  rws_interface.services().rapid().runMoveJ("T_ROB1", current_robtarget);
  
  waitForIdle();
  rws_interface.services().egm().signalEGMStartPose();

Q. Also you mentioned here (#64) that it is possible to change position_correction_gain during runtime. Can you provide more information on this how.

It is possible to update the position correction gain during runtime, but it doesn't affect currently running EGM motions. I.e. you need to stop EGM and then start it anew before the changes are applied.

• abb_librws without StateMachine Add-In You can add a global num variable to your RAPID code, which you then can set similar to the corresponding example above. This can then be used in the \PosCorrGain argument of the EGMRunPose instruction.

• abb_librws with StateMachine Add-In Continuing on the corresponding example above, then you can do something like this:

  rws_interface.services().egm().signalEGMStop();
  
  abb::rws::RWSStateMachineInterface::EGMSettings egm_settings;
  if (rws_interface.services().egm().getSettings("T_ROB1", &egm_settings))
  {
    egm_settings.run.pos_corr_gain.value = 0.5;
    rws_interface.services().egm().setSettings("T_ROB1", egm_settings);
  }
  
  waitForIdle();
  rws_interface.services().egm().signalEGMStartPose();

--> do i need to use abb_librws (ros-industrial/abb_librws#49) for both above as I can make changes to RAPID through that or is their any other way (i do not want to compromise the response of the robot as that is extremely important as I am dealing with moving targets) <--

There are other ways besides RWS, but this answer has already grown larger than I intended and RWS is the most straightforward solution in my opinion. If you go for RWS, then you should probably run the C++ code in a separate thread from the EGM related code.

--> or is it possible to use state_machine_interface directly with EGM to switch between pure RAPID module and EGM <--

See the "abb_librws with StateMachine Add-In" examples above.

[jontje]

the same code results in below response @ the first iteration (every time, after this if i stop the program by the stop button and then the response is as in the above figure (the desired response)):

Hm... that sounds a bit strange, and I don't have a good idea at the moment for what could cause this behaviour.

I too think that the latency what i had was too much. Could it be that the ROS implementation that uses abb_libegm feedback (input.feedback().robot().cartesian().pose()) is not fast enough and the data being retrieved does not capture the actual delay on the robot motion.

It might also be caused somewhere inside EGMControllerInterface, since it is far from optimal and contains a bit of redundant checks/copying.

Had you tried to comment out this section? It causes a sleep in the thread processing the EGM communication, which might be part of the problem.

It could be interesting to make measurements in the UDPServer's receive and send callbacks, to compare with the ROS level measurements.

[shobhitcito]

@jontje , @gavanderhoorn thanks for your help

We have been using velocity based motions entirely which work quite well and has resolved all issues that I mentioned in this post. Lot of errors wherein robot landed up exceeding its dynamic limit was due to a very crude detection algorithm that wasn't feeding very great references.

[jbeck28]

@shobhitcito You were able to reduce the latency to closer to 20ms?