Port FSU speed limit handling from MotoROS1

[iydv]

As discussed at here I have implemented motoros2 version of FSU speed limit handling. Additionally, I have introduced following changes to resolve issues in the implementation:

1. prevPulsePosData is defined as global variable in CtrlGroup. This is required, since this variable is constantly changing in Ros_MotionControl_IncMoveLoopStart function. The prevPulsePos variable cannot be used instead, because it will break the logic of Ros_MotionControl_AddToIncQueueProcess.

This change is required to allow initialization of prevPulsePosData when trajectory mode is disabled/enabled. Otherwise, the robot will always tried to return to its last position every time the trajectory mode is enabled.

2. feedback_FollowJointTrajectory.feedback.desired.positions is estimated differently now. Previously, it was accumulating all moveData increments in every iteration of Ros_MotionControl_IncMoveLoopStart function. However, the robot cannot reach desired increments due to the speed limit. Which means that Ros_ActionServer_FJT_UpdateProgressTracker function is called multiple times with increments that are never actually executed by the robot.
   I have changed feedback_FollowJointTrajectory.feedback.desired.positions to represent a current desired increment of actual positions, which better reflects the behavior of Ros_MotionControl_IncMoveLoopStart with current changes.

[gavanderhoorn]

Hi, thanks for the PR 👍

Could I be difficult and ask you to split it into multiple commits?

Ideally a first commit showing just the 'import' of @EricMarcil's work here in MotoROS2, then further commits with the changes you describe you had to make.

That would facilitate review as it would very clearly show what changed, where and when.

thanks.

[iydv]

Hi, thanks for the PR 👍

Could I be difficult and ask you to split it into multiple commits?

Ideally a first commit showing just the 'import' of @EricMarcil's work here in MotoROS2, then further commits with the changes you describe you had to make.

That would facilitate review as it would very clearly show what changed, where and when.

thanks.

Hi, I have split it into 3 commits

[ted-miller]

I will try to do some testing next week. I don't currently have a system with FSU configured.

[EricMarcil]

@iydv
I probably should have looked at you solution before working on the MotoRos1 version on my PR.
FSU Speed Limit Handling
Please review the way I have done it. I believe that in your correction, there will still be a small motion. During testing, I also noticed a small motion that was caused by the "toProcessPulses" not being re-initialized.
See commit

[iydv]

@EricMarcil thank you for the update. I have checked your solution and it is better than mine. I will implement and test it in MotoROS2 (which I can do only next week).
Unfortunately, I am no longer using ROS1 so I cannot test the original MotoROS1 implementation.

[iydv]

I have integrated changes from the latest MotoROS1 commit to reset prevPulsePosData and
toProcessPulses.

@EricMarcil unfortunately, I encountered another issue with this solution: when the robot is moved away and enabled again the following error is triggered:
mpExRcsIncrementMove returned UNREADY: -1 (Could be PFL Active)

I believe the issue is this condition. The variable hasUnprocessedData is never reset inside the main loop, which means it is always TRUE and when the robot is enabled it always enters the queue processing loop. However, by original logic, it should enter the loop only when the queue has data. I added a reset of hasUnprocessedData here. And I think it should also reset during normal operation similar to isMissingPulse

[ted-miller]

I have tested this on a few trajectories and it seems to work well. Admittedly, my trajectories are rather primitive and hard-coded. But the behavior with speed-limiting enabled always matches the behavior with speed-limiting disabled. I can even switch back and forth mid trajectory.

The only unfortunate part is that this makes the most-complicated section of code even more complicated. But I guess that's inevitable.

@gavanderhoorn, I think this is a great addition and should in the 0.1.2 milestone. Any arguments against it?
(Just today I had a user contact me regarding this issue. So, it'd be good timing for a fix.)

[EricMarcil]

I've also been reviewing and testing the last recommendation from @iydv. I agree with his change but in the process, I also found a new condition where the robot can still move. If in the middle of a motion, the mode key is switch to TEACH mode, and then the robot is manually restarted by switching back to PLAY, turning on the servos and then switch back to REMOTE. The robot continues execution what's left in the motion buffer.
Note: I haven;t tested yet, but I don't believe this is specific to the change in this PR. It's probably an issue that has always existed and was never detected.

[ted-miller]

Are you testing with MotoROS2 or MotoROS1?
I'm not seeing this behavior on MR2.

[EricMarcil]

I'm testing on MotoRos1. I'm working on fixing it. I'll commit to the PR on MotoRos1 and then you can review to see if it applied on MotoRos2.

[ted-miller]

you can review to see if it applied on MotoRos2

It looks like this does not apply to MR2.

[gavanderhoorn]

Could we perhaps add some more comments? As you write @ted-miller, this is already some of the most -- if not the most -- complex code in MotoROS (1 and 2) and this PR makes it even more complex.

Seeing the discussion on ros-industrial/motoman#542, it's a non-trivial change which adds quite a bit of bookkeeping and handling for new corner-cases.

I'd like to see more comments explaining what's going on, perhaps also at a conceptual level. ros-industrial/motoman#542 has some of that in the PR description and the subsequent comments, but we should embed that and have it right next to the source.

I would perhaps also suggest setting an IO signal whenever the new code detects the FSU is 'active'. That would allow us to use that in other places without increasing coupling even more than we already do.

[gavanderhoorn]

Marking as request changes to prevent us from merging this accidentally.

[gavanderhoorn]

Note: @iydv, as this is essentially @EricMarcil's code, I'm not asking you add comments of course.

As maintainers we can add more commits to this PR, which I what I suggest we do @ted-miller and @EricMarcil.

[gavanderhoorn]

@EricMarcil / @ted-miller: friendly ping?

[ted-miller]

It's on the todo-list.

I'd be in favor of merging this and opening another issue for the next milestone regarding comments/documentation. We also need to update the EDS document to explain the overall architecture of the code.

[EricMarcil]

I added comments to code on the MotoRos1 branch ros-industrial/motoman#542
ros-industrial/motoman@9c6ff7d

[gavanderhoorn]

@iydv: I've just pushed a commit which cherry-picked @EricMarcil's comments from ros-industrial/motoman@9c6ff7d to your branch.

New changes have been added

[gavanderhoorn]

I don't have an FSU configured, so I can't test this myself right now.

@ted-miller: if you could verify again this works as intended -- also with different (ie: multi-group) systems?

I'll leave the decision to merge to you.

[gavanderhoorn]

It would also still be good to address my previous comment:

I would perhaps also suggest setting an IO signal whenever the new code detects the FSU is 'active'. That would allow us to use that in other places without increasing coupling even more than we already do.

but we can do that in a follow up PR.

[iydv]

@gavanderhoorn I will test it tomorrow on my robot, but I do not have a multi group setup.

[iydv]

I have tested all recent changes including this and everything still works fine.

[ted-miller]

Great. I'm setting up a multi-group system now to verify. If there are no issues, I'll merge.

[ted-miller]

@EricMarcil, I found an issue...

I've got an R1+R2+S1 system. I'm commanding S1 to remain stationary. (target pos = 0, target vel = 0) However, S1 decided to rotate up and crash into R1. This behavior does not occur when using the main branch.

I'm also getting some unexpected messages on the debug log:

[1697813531.82279396] [192.168.1.31:57384]: 2023-10-20 14:52:11.822169 Streaming point received
[1697813531.82306910] [192.168.1.31:57384]: 2023-10-20 14:52:11.822169 Reply (4): Busy processing the previous trajectory point. Please resend the next point shortly
[1697813531.82414675] [192.168.1.31:57384]: 2023-10-20 14:52:11.823369 Warning undefined speed: Axis 3 Defaulting Max Inc: 3647 (prevSpeed: 0 curSpeed 0)

[1697813531.82440352] [192.168.1.31:57384]: 2023-10-20 14:52:11.823569 Warning undefined speed: Axis 0 Defaulting Max Inc: 1092 (prevSpeed: 0 curSpeed 0)

[1697813531.83224535] [192.168.1.31:57384]: 2023-10-20 14:52:11.831569 Warning undefined speed: Axis 3 Defaulting Max Inc: 3647 (prevSpeed: 0 curSpeed 0)

[1697813531.83251739] [192.168.1.31:57384]: 2023-10-20 14:52:11.831569 Warning undefined speed: Axis 0 Defaulting Max Inc: 1092 (prevSpeed: 0 curSpeed 0)

[1697813531.84020162] [192.168.1.31:57384]: 2023-10-20 14:52:11.839569 Warning undefined speed: Axis 3 Defaulting Max Inc: 3647 (prevSpeed: 0 curSpeed 0)

[1697813531.84046841] [192.168.1.31:57384]: 2023-10-20 14:52:11.839769 Warning undefined speed: Axis 0 Defaulting Max Inc: 1092 (prevSpeed: 0 curSpeed 0)

[1697813531.84838128] [192.168.1.31:57384]: 2023-10-20 14:52:11.847569 Warning undefined speed: Axis 3 Defaulting Max Inc: 3647 (prevSpeed: 0 curSpeed 0)

[1697813531.84850216] [192.168.1.31:57384]: 2023-10-20 14:52:11.847569 Warning undefined speed: Axis 5 Defaulting Max Inc: 2546 (prevSpeed: 0 curSpeed 0)

[1697813531.84857678] [192.168.1.31:57384]: 2023-10-20 14:52:11.847769 Warning undefined speed: Axis 0 Defaulting Max Inc: 1092 (prevSpeed: 0 curSpeed 0)

[1697813531.84867573] [192.168.1.31:57384]: 2023-10-20 14:52:11.847769 Streaming point received
[1697813531.84878302] [192.168.1.31:57384]: 2023-10-20 14:52:11.847969 Reply (4): Busy processing the previous trajectory point. Please resend the next point shortly

I'm investigating now. But if you have some idea of what could cause this, please let me know.

[iydv]

@ted-miller can this be the issue? In here the current controller command position is retrieved. But if the command group is always 0, the positions for all groups are set to position of group 0?

[ted-miller]

@iydv, Good catch. That does look wrong.

Unfortunately, it's not the only problem. Now the robots come to immediate violent halt after they start moving.

I'm guessing R2 is getting commanded with some uninitialized value.

[iydv]

maybe initialization here ? The prevPulsePosData for all groups is getting initialized by the data from group 0. And then here the computation is wrong for any group other than 0.

[ted-miller]

Yep. There were a few instances.

It is working now (with only minimal physical damage done...)

Given that the main functionality has been thoroughly vetted at this point, and that we've now identified a fix for other control groups, I'd say this is good to merge.

As a side note, I was surprised to find that activating the speed limit for any group in the job will activate a speed limit for all groups in the job. This makes sense in hindsight, but I was surprised. This is good because it keeps everything in sync.

I also attempted to create a custom INIT_ROS job which pstarted individual jobs for each group to see how the speed limit would cause things to go out of sync. Turns out that you can't do that. The motion API returns:

Thank you @EricMarcil and @iydv for the implementation and debugging.

[ted-miller]

As a side note, I was surprised to find that activating the speed limit for any group in the job will activate a speed limit for all groups in the job. This makes sense in hindsight, but I was surprised. This is good because it keeps everything in sync.

Well... turns out this isn't entirely true. If the FSU board is not configured for a particular servo board, then group(s) on that board will not be limited.

I can't imagine a scenario where it would be intentionally configured like this. But its apparently possible.

[gavanderhoorn]

That looks like something we might want to detect and warn about?