FSU Speed Limit Handling

[EricMarcil]

These modifications to the MotoRos driver checks if the incremental pulses sent on the previous iteration matches the change in the robot command position. If it doesn't it means that the FSU Speed Limit is actively limiting the speed and rejecting some of the command.
By keeping track of the previous iterations values, the amount of increment processed is calculated and the unprocessed part is resent. When there is unprecessed pulses, we limit the reading of the incremental queue to one and then skip further reading until the previous increment is completely processed. The speed associated with an increment is also track so that if the speed limit is removed, we don't send unprocessed pulses all at once and exceed the commanded speed.

This is still work in process but the basic functionality is working.

Need to test:

• Turning on/off speed limit in the middle of a trajectory.
• Move along a line. Does it still follow the line when speed limit is enabled or does it cause distortion of the motion
• Test with multiple control group.

To Do:

• DX100 support?
• Should we add some flag to notify that speed limit is on?
• Need some way of MotoROS notifying motoman_driver what is going on. The FSU slowing down things is going to cause a aborted trajectories of motion planners/executors like MoveIt that will notice a gradually increasing tracking error, and at some point will decide to cancel the motion.

[ted-miller]

I'm not sure I fully understand this comment:

By keeping track of the previous iterations values, the amount of increment processed is calculated and the unprocessed part is resent. When there is unprecessed pulses, we limit the reading of the incremental queue to one and then skip further reading until the previous increment is completely processed.

Is this accurate?

I'm curious how smooth the motion would be on the "unlimited" axes. Though maybe not a big deal, given that speed is already limited to a presumably "safe" level.

DX100 support

I think that your changes just involve queue-management. They don't have any direct changes to the motion API behavior, right? If so, then DX100 should be fine.

Should we add some flag to notify that speed limit is on?

Yes. Much-requested feature.

[EricMarcil]

Is this accurate?

No, it's more complicated than that to keep the smooth motion. You example would give something like:

In this example, you are slowing down. If you are speeding up, if the remain from previous cycle is too high, we actually skip reading from the IncQueue.

DX100 support

I think that your changes just involve queue-management. They don't have any direct changes to the motion API behavior, right? If so, then DX100 should be fine.

I need to move my code up because currently it is only applied in the case of the mpExRcsIncrementMove, not the mpMeiIncrementMove (DX100)

Should we add some flag to notify that speed limit is on?

Yes. Much-requested feature.

Add 'speed_limited' to the SmBodyRobotStatus?

[ted-miller]

No, it's more complicated than that to keep the smooth motion... we actually skip reading from the IncQueue.

When you "skip" reading from the queue, is it selective on a per axis basis? If so, then I feel like that would cause the axes to get out of sync, resulting in a different TCP path.

[EricMarcil]

No the "skip" is per control group. It keep tracks of data for two cycles at the time and will keep skipping as long as the "previous" cycle is not completely processed for all axes. Then it moves the "current" cycle to the "previous" and allows reading of the next cycle from the incQueue.

[gavanderhoorn]

@EricMarcil: it's been some time since you opened this.

Could you add a comment here clarifying the status of this PR?

Thanks

[EricMarcil]

The MotoRos driver is read except for adding a new flag to indicate that the FSU is slowing the robot (which is easy to add once we agree on the format).
I'm not very familiar with the motoman_driver side and haven't had time to look into it. I had ask if someone could help, but no one has come forward. The FSU slowing down things is going to cause a aborted trajectories of motion planners/executors like MoveIt that will notice a gradually increasing tracking error, and at some point will decide to cancel the motion. I'm not sure how that works. The new flag will notify the motoman_driver what is going on, but I'm not sure what should happen after...

[gavanderhoorn]

I'm not very familiar with the motoman_driver side and haven't had time to look into it. I had ask if someone could help, but no one has come forward.

yes, apologies. I was rather busy with other things.

The FSU slowing down things is going to cause a aborted trajectories of motion planners/executors like MoveIt that will notice a gradually increasing tracking error, and at some point will decide to cancel the motion. I'm not sure how that works. The new flag will notify the motoman_driver what is going on, but I'm not sure what should happen after...

Motoman is not the only OEM that does something like this. Some other OEMs with ROS drivers have implemented ways to notify the ROS side of events like this. We could see whether we could implement similar systems.

I'm not sure what should happen after...

that's the more difficult part.

MoveIt (or some other motion generating system) would have to be made aware of the changes to 'execution speed' and accommodate those. Conceptually, that's not that difficult. But someone will have to do the work.

[iydv]

Hi @EricMarcil . I have implemented your changes in the MotoROS2 driver and tested it on HC10 robot. I have tested the behavior when FSU speed limit is active and remains constant during the whole process. The solution is working fine when trajectory is executed and always reaches the goal for simple trajectories.

However, I have discovered a very dangerous behavior:

in here the latest pulsePosData is always assigned to prevPulsePosData, but only when robot is working and trajectory mode is enabled. Once the robot is stopped or switched to manual mode prevPulsePosData is no longer updated.

If the robot is manually moved and then switched back to remote mode, as soon as trajectory mode is enabled it starts moving to old position stored in prevPulsePosData. I think prevPulsePosData should be reinitialized every time the robot is stopped. Currently, it is initialized only once at the beginning of Ros_MotionServer_IncMoveLoopStart

[ted-miller]

Hi @iydv Could you open a Pull Request on MotoROS2 integrated with Eric's changes?

Also, I believe that the issue with prevPulsePosData doesn't exist in MR2 because it gets initialized when you activate one of the motion-modes. Do you agree?

[EricMarcil]

Hi @iydv Thanks for the feedback.
I was under the impression that after going to Manual, a new motion trajectory would need to be generated, and the Ros_MotionServer_IncMoveLoopStart always be called. I'll try to reproduce what you are discribing and get back to you (probably next week)

[iydv]

I have created an implementation of current solution for motoros2. I have introduced a way to fix described problem with prevPulsePosData

[EricMarcil]

I was able to reproduce the issue in two situations when the ROS commanded motion is restrained by the FSU Speed Limit:

1. User switches to manual mode, jogs the robot around. Switch back to Play, turn on Servo and then start playback. Switch to Remote and resume ROS operation.
2. While under ROS contorl, HC-robot is stop or deviated from its path by FPL function (some external force is applied externally), when ROS operation is resume (robot enabled) the robot may move.

I was under the impression that under those conditions that the MotionServer thread was being terminated and a new one would have to be started. But after verification, the motion command and motion buffer are cleared but the motion server thread keeps on running and moving the robot creates a difference between the current position and the previous position (introduce by this PR to track motion progress).

So there are two options to fix this: 1- Have the MotionServer thread constantly update the previous position when not moving under ROS. 2- Terminate the MotionServer thread whenever "ROS operation" conditions are not met. (instead of just clearing the motion queue)

[EricMarcil]

@iydv I've committed a fix that corresponds to option 1 described above.

Did you make changes on the Ros side for the MotoRos1 to manage the speed limit reduction, or only made changes for the MotoRos2?

[EricMarcil]

I've reset the hasUnprocessedData variable as per comment on https://github.com/Yaskawa-Global/motoros2/pull/157#issuecomment-1748859421 Tested for proper operation.
Found pre-existing bug causing unwanted robot motion when switching pendant mode key. It will open different PR to address that issue since it is not cause by this PR.

[gavanderhoorn]

@EricMarcil: it looks like 961183ae751a3d0b416a161b9a7d1d7d378de91c isn't the same as what Yaskawa-Global/motoros2@9d6655f (#157) changed?

[EricMarcil]

Sorry about that. I rushed the change on Friday night so I would forget to do it the following week but ended up making bad cut/paste operation. I fixed it and tested on a single robot system. @ted-miller Can you test on your multi-robot system for Ros1?

[ted-miller]

Can you test on your multi-robot system for Ros1?

Yes. It'll be a few days though. I just reconfigured my system for Smart Pendant (single group). I need to get a few tasks done first.

[ted-miller]

Yes, it appears to be working.

---

Regarding my comment about the speed limit potentially not being applied to all groups: https://github.com/Yaskawa-Global/motoros2/pull/157#issuecomment-1781857284

I went ahead and disabled the FSU board for R1. So, R1 isn't limited.

It turns out, your algorithm still "mostly" keeps the arms in sync. R1 appears to plow through it's increments at full speed, but it is prevented from getting new increments until R2 does.

(I'll keep the video available for a few days, then will probably delete from my library.) https://photos.app.goo.gl/y7WkpS6nDm1gkpEc7 Check out around 30 seconds into the video.

[EricMarcil]

@ted-miller Please review latest changes.