FSU Speed Limit Handling #542

EricMarcil · 2023-04-25T17:43:41Z

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.

This is the basic handling of FSU Speed Limit. Works for a single control group. Need to generalize for multiple control groups.

Added multiple control group support and debug statements

Updated prevPulsePosData for all control groups.

Added MotoRosYRC1u_,out so people can test without needing to compile from source code.

ted-miller · 2023-04-25T19:54:25Z

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.

ted-miller · 2023-04-25T20:45:15Z

motoman_driver/MotoPlus/mpMain.c

@@ -57,6 +57,9 @@ void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int a
 #ifdef DX100
 	Ros_Sleep(10000);  // 10 sec. delay to enable DX100 system to complete initialization
 #endif
+#ifdef DEBUG
+	Ros_Debug_Init();


This is not actually needed. This function is called in Debug_BroadcastMsg

OK, removed

ted-miller · 2023-04-25T20:46:55Z

motoman_driver/MotoPlus/MotionServer.c

@@ -31,6 +31,10 @@

 #include "MotoROS.h"

+#ifdef DEBUG


I recommend removing this condition. The DEBUG preprocessor was to protect against costly printf commands. The udp-broadcast method is way more efficient. I think it's safe to allow these w/o also enabling the printf.

ted-miller · 2023-04-25T20:47:36Z

motoman_driver/MotoPlus/mpMain.c

@@ -41,8 +41,8 @@

 #include "MotoROS.h"

-
 #ifdef DEBUG


Same as previous comment:

I recommend removing this condition. The DEBUG preprocessor was to protect against costly printf commands. The udp-broadcast method is way more efficient. I think it's safe to allow these w/o also enabling the printf.

ted-miller · 2023-04-25T20:48:52Z

motoman_driver/MotoPlus/debug.c

+
+
+extern STATUS setsockopt(int s, int level, int optname, char* optval, int optlen);
+extern int mpNICData(USHORT if_no, ULONG* ip_addr, ULONG* subnet_mask, UCHAR* mac_addr, ULONG* default_gw);


This is YRC1000 (and micro) only. So you probably want a conditional flag around this whole file.

ted-miller · 2023-04-25T20:49:42Z

motoman_driver/MotoPlus/debug.c

+#define MP_USER_LAN2	2	/* general LAN interface2(only YRC1000) */
+
+
+extern STATUS setsockopt(int s, int level, int optname, char* optval, int optlen);


We've got some stuff in the MotoROS2 project to obfuscate externs like this. We think it's probably portable to MotoROS1.

EricMarcil · 2023-04-25T21:46:42Z

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 · 2023-04-26T12:23:52Z

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 · 2023-04-26T12:46:44Z

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.

Change to support DX100

gavanderhoorn · 2023-06-28T11:41:24Z

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

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

Thanks

EricMarcil · 2023-06-28T12:27:20Z

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 · 2023-06-28T13:26:16Z

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 · 2023-09-19T15:18:16Z

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 · 2023-09-19T21:03:13Z

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 · 2023-09-20T13:47:03Z

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 · 2023-09-20T16:33:34Z

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

EricMarcil · 2023-09-27T14:48:35Z

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

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.
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)

…n control When the ROS commanded motion is restrained by the FSU Speed Limit, fixed unwanted motion after robot is moved by non-ROS means and then ROS motion is restarted.

EricMarcil · 2023-09-27T16:09:43Z

@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?

Added code to reset hasUnprocessedData state when Ros operation is interrupted.

EricMarcil · 2023-10-06T20:39:32Z

I've reset the hasUnprocessedData variable as per comment on Yaskawa-Global/motoros2#157 (comment)
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.

Added comments to code to help developers understand what is being done.

Fix for multi-group support from correction made by Ted Miller on the MotoRos2 PR#157 Port FSU speed limit handling from MotoROS1

gavanderhoorn · 2023-10-21T07:17:58Z

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

EricMarcil · 2023-10-23T15:28:22Z

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 · 2023-10-23T17:17:36Z

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 · 2023-10-26T20:52:54Z

Yes, it appears to be working.

Regarding my comment about the speed limit potentially not being applied to all groups: Yaskawa-Global/motoros2#157 (comment)

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.

ted-miller · 2023-10-26T20:55:19Z

motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1_/MotoRosYRC1_.out

Need to update the binary names. Plus, make sure to update the APPLICATION_VERSION

Removed binary. Will be added by separate commit.

Mainly modifications to use of the debug broadcast which only works on YRC1000/YRC1000micro controller.

EricMarcil · 2023-12-20T20:20:52Z

@ted-miller Please review latest changes.

EricMarcil added 5 commits April 24, 2023 11:40

Added basic FSU Speed Limit Handling

544c41d

This is the basic handling of FSU Speed Limit. Works for a single control group. Need to generalize for multiple control groups.

Added multiple control group support

305136f

Added multiple control group support and debug statements

Updated prevPulsePosData for all control groups

6d69201

Updated prevPulsePosData for all control groups.

Added MotoRosYRC1u_,out

5c7870e

Added MotoRosYRC1u_,out so people can test without needing to compile from source code.

Delete MotoRosYRC1u_1910.out

196b5c0

EricMarcil mentioned this pull request Apr 25, 2023

FSU may limit robot velocity and interfere with MotoROS #247

Closed

ted-miller reviewed Apr 25, 2023

View reviewed changes

gavanderhoorn mentioned this pull request Jun 2, 2023

Publish FSU/PFL speed/power limit activation status Yaskawa-Global/motoros2#50

Open

Change to support DX100

9e3babc

Change to support DX100

ted-miller mentioned this pull request Sep 19, 2023

Trajectory doesn't Complete with Speed Limit set on Robot FSU Yaskawa-Global/motoros2#155

Closed

iydv mentioned this pull request Sep 20, 2023

Port FSU speed limit handling from MotoROS1 Yaskawa-Global/motoros2#157

Merged

Reset Speed Limit related variables when robot is not under ROS motio…

0654dfa

…n control When the ROS commanded motion is restrained by the FSU Speed Limit, fixed unwanted motion after robot is moved by non-ROS means and then ROS motion is restarted.

Reset of hasUnprocessedData

0adc258

Added code to reset hasUnprocessedData state when Ros operation is interrupted.

EricMarcil added 2 commits October 17, 2023 08:38

Added comments

9c6ff7d

Added comments to code to help developers understand what is being done.

Update MotionServer.c

961183a

Fix for multi-group support from correction made by Ted Miller on the MotoRos2 PR#157 Port FSU speed limit handling from MotoROS1

Fix last commit error

860fa61

ted-miller reviewed Oct 26, 2023

View reviewed changes

ted-miller mentioned this pull request Oct 27, 2023

Fix pendant mode switch issue #568

Merged

EricMarcil added 2 commits December 20, 2023 15:16

Correction following Ted's review

3166e21

Mainly modifications to use of the debug broadcast which only works on YRC1000/YRC1000micro controller.

Removed binary output file

d4e4073

ted-miller approved these changes Dec 20, 2023

View reviewed changes

ted-miller merged commit 78471a3 into ros-industrial:kinetic-devel Dec 20, 2023
6 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

FSU Speed Limit Handling #542

FSU Speed Limit Handling #542

EricMarcil commented Apr 25, 2023

ted-miller commented Apr 25, 2023

ted-miller Apr 25, 2023

EricMarcil Dec 20, 2023

ted-miller Apr 25, 2023

ted-miller Apr 25, 2023

ted-miller Apr 25, 2023

ted-miller Apr 25, 2023

EricMarcil commented Apr 25, 2023

ted-miller commented Apr 26, 2023

EricMarcil commented Apr 26, 2023

gavanderhoorn commented Jun 28, 2023

EricMarcil commented Jun 28, 2023

gavanderhoorn commented Jun 28, 2023

iydv commented Sep 19, 2023

ted-miller commented Sep 19, 2023

EricMarcil commented Sep 20, 2023

iydv commented Sep 20, 2023 •

edited

Loading

EricMarcil commented Sep 27, 2023 •

edited

Loading

EricMarcil commented Sep 27, 2023

EricMarcil commented Oct 6, 2023

gavanderhoorn commented Oct 21, 2023

EricMarcil commented Oct 23, 2023

ted-miller commented Oct 23, 2023

ted-miller commented Oct 26, 2023

ted-miller Oct 26, 2023

EricMarcil Dec 20, 2023

EricMarcil commented Dec 20, 2023



		extern STATUS setsockopt(int s, int level, int optname, char* optval, int optlen);
		extern int mpNICData(USHORT if_no, ULONG* ip_addr, ULONG* subnet_mask, UCHAR* mac_addr, ULONG* default_gw);

		#define MP_USER_LAN2 2 /* general LAN interface2(only YRC1000) */


		extern STATUS setsockopt(int s, int level, int optname, char* optval, int optlen);

FSU Speed Limit Handling #542

FSU Speed Limit Handling #542

Conversation

EricMarcil commented Apr 25, 2023

ted-miller commented Apr 25, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

EricMarcil commented Apr 25, 2023

ted-miller commented Apr 26, 2023

EricMarcil commented Apr 26, 2023

gavanderhoorn commented Jun 28, 2023

EricMarcil commented Jun 28, 2023

gavanderhoorn commented Jun 28, 2023

iydv commented Sep 19, 2023

ted-miller commented Sep 19, 2023

EricMarcil commented Sep 20, 2023

iydv commented Sep 20, 2023 • edited Loading

EricMarcil commented Sep 27, 2023 • edited Loading

EricMarcil commented Sep 27, 2023

EricMarcil commented Oct 6, 2023

gavanderhoorn commented Oct 21, 2023

EricMarcil commented Oct 23, 2023

ted-miller commented Oct 23, 2023

ted-miller commented Oct 26, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

EricMarcil commented Dec 20, 2023

iydv commented Sep 20, 2023 •

edited

Loading

EricMarcil commented Sep 27, 2023 •

edited

Loading