Robot status motion possible flag incorrect when robot enable called during PFL stop

[CameronDevine]

I am controlling a Yaskawa HC10 with the power force limiting features enabled from ROS. I have found that if the robot is in PFL stop, and the /robot_enable service is called, the motion_possible flag in the /robot_state messages is set to true, but the robot cannot be moved.

I am using my own branch of motoman_driver (with the master branch recently merged in) available here, https://github.com/CameronDevine/motoman/tree/point-streaming-with-io

I believe I am using MotoROS v3.6.0, but I can check early next week. I am also planning to test this with the current master branch.

[gavanderhoorn]

I believe I am using MotoROS v3.6.0

that would be interesting, as we're only at v1.9.1 with MotoROS.

I am using my own branch of motoman_driver (with the master branch recently merged in) available here, https://github.com/CameronDevine/motoman/tree/point-streaming-with-io

Diff here.

I see no changes to MotoROS here, only to the motoman_driver side.

I am controlling a Yaskawa HC10 with the power force limiting features enabled from ROS.

What do you mean exactly by "enabled from ROS"? There is no explicit support for any PFL features in motoman_driver, so I'm not sure I understand what you write here.

I have found that if the robot is in PFL stop, and the /robot_enable service is called, the motion_possible flag in the /robot_state messages is set to true, but the robot cannot be moved.

This sounds a bit like #287.

@ted-miller @EricMarcil: could it be that PFL status is not taken into account when calculating the values for those fields?

[CameronDevine]

I'm sorry, when I wrote this up I was a little hurried.

I believe I am running MotoROS v1.9.0. Is there any way to check which version is installed on the controller?

I am using the HC10 robot with the PFL feature turned on and sending motion commands to the robot from ROS.

[gavanderhoorn]

Is there any way to check which version is installed on the controller?

I don't believe so (not until we get #193 going).

What you could do is copy the .out file to a USB stick, generate the md5sum and compare that to the hashes shown here (you'd be essentially following the instructions there).

For v1.9.0 it would be:

0d539aa636a844518b73b9a090dfd5ef  MotoRosFS_190.out
e6ccd26991d85b8cee8638081cffe9ba  MotoRosYRC1u_190.out
a174f6d569badc699b121173d63cb3ef  MotoRosDX1_190.out
ceae6f706883695a5f4465deb5597bfd  MotoRosYRC1_190.out
514c4f13cbb96e68b07fbbda8018fffc  MotoRosDX2_190.out

[EricMarcil]

Yes, the Motion Ready signal was not reviewed for PFL when the HC-robot support was added.
I'll try to fix that in the next week.

[EricMarcil]

For the version, on DX200 and YRC1000, there is a MotoPlus Monitor button and you can list the MotoPlus application that are running. The version number is part of the application name.

[ted-miller]

For the version, on DX200 and YRC1000, there is a MotoPlus Monitor button and you can list the MotoPlus application that are running. The version number is part of the application name.

You can also see the version info directly on the regular pendant by touching [System] > [Version]. Then push the {PAGE} button a couple times. Also, when doing a system backup, this version info is exported to SYSTEM.SYS and PANELBOX.LOG.
@EricMarcil, where is this info located on the SP?

Of course, those version numbers are only valid if there have been no customization done to the MotoPlus code.

[EricMarcil]

MotoPlus support is being released with version 1.5 of the Smart Pendant. You can access the information under the Menu --> System Settings --> Controller Settings --> MotoPlus Management button.
For earlier versions, you have to use the PC based Software Pendant which is the equivalent of the standard pendant running on a PC.

[CameronDevine]

I was able to check today and I am running MotoROS v1.9.0. I also checked and the same issue is present when using the default branch of this Git repository.

[EricMarcil]

@CameronDevine It's an easy fix but I don't have an HC10 to confirm is my fix is working. I have tested that none HC10 robots are still working with the change.
Can you test with the attached version. In the zip folder take the version that corresponds to your controller (YRC1000 or YRC1000micro). Make sure you remove the MotoROS v1.9.0.
Edit: Removed files

Let me know if that fixed it and I can start a PR to update the official version.

[CameronDevine]

@EricMarcil I should be able to test this on Thursday.

[CameronDevine]

@EricMarcil I tested your attached version of the MotoROS Server. For my needs it works correctly. However, I did notice two things:

1. When the robot is stopped by the PFL function the in_motion flag remains at 1.
2. After the PFL resume button is pressed, the robot can immediately be moved again. Is this correct? I had assumed that /robot_enable would have to be called again before the robot could be moved.

[EricMarcil]

Interesting, the in_motion flag is actually meant to indicate if the feedback position has reached command position. Normally, if it hasn't that means that the robot is still moving toward its destination. I don't think we have ever considered the state of that signal when the robot is stopped. Probably, when an alarm or hold signal is raised, the command position is reset to the current stopped position, so the In_Motion turns off.
For the PFL, it probably works different because technically (but not with ROS) the robot can be push away from its trajectory and as the force is removed, it resumes on its path. So, I guest the final command position remains active and that's probably also why you don't need to call the /robot_enable.
I'm not sure what should be the correct behavior. I'll have to run some tests and think about it.
Input is welcome from you or anyone else in this matter.

[CameronDevine]

@EricMarcil

It sounds a simple solution could be to logically and the motion_possible and current in_motion flags. The resulting value would be true whenever the robot has not reached its command position, but if the robot is stopped due to the PFL feature, the in_motion flag would be false. The value would then automatically become true when motion is again possible.

As for the /robot_enable service, if a joint trajectory command is sent to the robot, then resumed after a PFL stop, it would be strange to have to call /robot_enable before any subsequent trajectories can be performed.

[CameronDevine]

Having tested this some more, I can confirm that when returning from a PFL stop the robot executes a portion of the remaining trajectory before stopping.

[CameronDevine]

@EricMarcil

My suggestion would be to change the line,

sendMsg->body.robotStatus.in_motion = (int)(Ros_Controller_IsInMotion(controller));

in the Ros_Controller_StatusToMsg function in Controller.c to,

sendMsg->body.robotStatus.in_motion = (int)(Ros_Controller_IsInMotion(controller) && Ros_Controller_IsMotionReady(controller));

[EricMarcil]

@CameronDevine Thanks for letting me know. I definitely need to investigate this further but I'm on vacation until Aug 10th.
Your suggestion make sense, do you have the MotoPlus SDK to test it? Otherwise, I'll try to get to it in the week of Aug 10th.

[CameronDevine]

@EricMarcil Unfortunately, I don't have the MotoPlus SDK.

[EricMarcil]

@CameronDevine I've started looking at the code. Your suggestion above might fix the status but it doesn't fix the fact that the robot can still move afterward. According to code, you shouldn't be able to move without re-enable the robot by sending a new trajectory.

	case IO_ROBOTSTATUS_PFL_STOP: // PFL Stop
	case IO_ROBOTSTATUS_PFL_ESCAPE: //  PFL Escaping
	{
		if (ioStatus[IO_ROBOTSTATUS_PFL_ESCAPE] == ON)  // signal turned ON
		{
			// Job execution stopped take action
			controller->bRobotJobReady = FALSE; //force job to be restarted with new ROS_CMD_START_TRAJ_MODE command
			Ros_MotionServer_ClearQ_All(controller);
		}
		break;
	}

I think the problem might be that the IO_ROBOTSTATUS_PFL_STOP is ON but the IO_ROBOTSTATUS_PFL_ESCAPE is OFF.
I've made the change in the attached file, if you want to try it. Otherwise, I'm having an HC10 transferred to my facility for testing. So I should be able to test it myself in the first week of September.
edit: removed file

[CameronDevine]

@EricMarcil this sounds correct based on what I have observed. If the robot stops due to PFL but does not attempt to escape a clamping situation, the trajectory will continue once the PFL resume button is pressed. However, if the robot attempts to escape a clamping situation, the trajectory will not continue after the PFL resume button is pressed.

[EricMarcil]

@CameronDevine is the inMotion signal correct when the robot attempts to escape?

[CameronDevine]

@EricMarcil I will have to check if that is the case. I'm not sure.

[CameronDevine]

@EricMarcil when the robot stops after attempting to escape the in_motion flag is 0.

[CameronDevine]

@EricMarcil I tried using the last file you sent; however, this caused a new issue to present itself. After a PFL stop the motion_possible and in_motion flags are both 0. After pressing the PFL Resume button the motion_possible flag is still at 0. Furthermore, calling the /robot_enable service returns the error message, Motoman robot was NOT enabled. Please re-examine and retry.. Strangely, if the E-stop button is pressed and released, then the robot can be enabled again.

[EricMarcil]

@CameronDevine Thanks for the feedback. This seems tricky to get all the correct signal state. I'll wait to get access to HC10 robot and do proper testing with it. Should be in the 1st week of September.

[EricMarcil]

@CameronDevine I believe that I've got everything working now. Please give this version a try.
Edit: removed file

[CameronDevine]

@EricMarcil I installed the version you sent yesterday; however, when I attempt to enable the robot I get this error:

[ERROR] [1599690123.910304836]: Failed to set TrajectoryMode: Not Ready (5) : Unknown (5011)
[ERROR] [1599690123.910333805]: Motoman robot was NOT enabled. Please re-examine and retry.

[EricMarcil]

That is strange. 5011 is basically saying that PFL was on. Which would mean that the Amber light button on the robot was on. The other possibility is that the input #4090 is on. That is something new that I added. It's from the Avoidance logic. This is normally not enabled but I figured I should check for it, in case someone wants to eventually use it.
It's basically allows to push the robot off it's path with a lower force. To use, it requires to add the interrupt enable function: EI file #1 in the INIT_ROS job. But I haven't thought about what happens when it is not enabled and the avoidance level is not set properly. Even is disabled, the request signal might still be turning on and that is causing the problem. I might need to check the enable signal along the request signal. I'm still writing up test cases, so I'll add it to the list and continue testing this week.

[EricMarcil]

OK, I've revised the code to use #4089 which indicates the job interruption by the external force instead of #4090 which is the external force exceeding the avoidance start. You system probably isn't configured for avoidance, so it was probably detecting an avoidance level from the robot's own weight.

Whenever the PFL (STOP, ESCAPE, AVOID) is detected, the robot trajectory mode will be stopped. So the INIT_ROS job will reach the END of the job and the trajectory mode will need to be restarted (I believe in ROS that corresponds to the Robot Enable.)
Attached is a test version:
output.zip

I'm still debating on whether that is a correct approach or not. Technically, someone having a modified INIT_ROS job with some motion would see that motion executing after the Amber button is pressed. I could probably just do an explicit STOP but I need to check that doesn't interfere with the Escape motion that is to release something being pinned by the robot.

Note: I did most of my testing using an internal none-ros application. I did some test with the basic Ros/MoveIt planning but that didn't seem to explicitly send the robot enable. So I had to manually start the robot job when testing with ROS. I'll try to program something be bit more elaborated for testing with ROS when I have a chance.

[CameronDevine]

@EricMarcil The most recent version is working much better. However, I do have one comment. After the robot has been stopped by the PFL function, the PFL Resume button must be pressed. At this point it appears the /robot_enable service must be called again to re-enable the robot. While this seems like a reasonable approach, it would be nice to have a method for knowing when the robot can be enabled again.

[gavanderhoorn]

Has there been any progress on this off-list (ie: something which hasn't been reported here yet)?

[EricMarcil]

Sorry about that. I have to clean-up the code and finalize testing so that I can submit a PR. I should be getting access to an HC10 next week and try to finalize things.

[EricMarcil]

@CameronDevine If you have achance please text and review the PR #382. I actually lost the code changes that I had made and recoded everything. So please check if it behaves as the previous version that I had made for you. I've added information about the expected behavior.
I've only modified the MotoRos side, did you have to make any changes to the Ros side related to these modifications?

[CameronDevine]

@EricMarcil thanks for letting me know. I'll try to test this out either this or next week.

[CameronDevine]

I believe this issue should be closed now because #382 was merged.

[EricMarcil]

@CameronDevine Thanks for the help on this.