Trajectory rejected on first use or after powersafe

[simonschmeisser]

We get a failing trajectory start position check after the robot was idle for some time. In this case we also observe that the actual and desired pulse/joint position do not match. In one case I was seeing a joint difference as big as 2.2e-3 on the wrist joint (this is a small size, GP7 robot where we usually rather expect precision < 1e-4) between the display on the teach pendant and what is sent out via simple message to ROS.

For reading the current position the driver uses mpGetFbPulse() while later for the check of the first trajectory point mpGetPulse() is used. The allowed difference between the trajectory start position and the theoretical robot position is 30 pulses. According to Yaskawa Germany internally 500 pulses are accepted before an error is shown to the user.

motoman/motoman_driver/MotoPlus/Controller.h

Line 76 in 7860ff5

#define START_MAX_PULSE_DEVIATION 30

motoman/motoman_driver/MotoPlus/MotionServer.c

Line 1225 in bd4bdbd

if(abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)

motoman/motoman_driver/MotoPlus/CtrlGroup.c

Line 242 in bd4bdbd

status = mpGetPulsePos (&sData,&pulse_data);

vs

motoman/motoman_driver/MotoPlus/CtrlGroup.c

Line 305 in bd4bdbd

status = mpGetFBPulsePos (&sData,&pulse_data);

My theory is that brakes are only released after this check and therefore the offset will never be corrected. Somehow it usually works on the second attempt however. Or maybe the joint position used in ROS is from before unbraking the robot? (We automatically send "enable robot" when it is in remote mode and motion_possible is false)

I tried increasing the limit but what I compiled with the MotoPlusSDK (it was not the one integrated in VS but the old (?) standalone one) from the partner download area did not match in size and failed to start. Also had to manually define the YRC1000 define or it would not compile the IOServer. We will get a MotoPlus SDK license on Wednesday but if anybody could provide a binary with increased tolerance or with checking against mpGetFbPulse we would be very grateful as the automatica fair is pretty close

[gavanderhoorn]

(for now I've converted this to a discussion. If it turns out this is something that needs to be fixed, we'll create a more detailed issue)

I'm not dismissing your experience, but the code you refer to is old, and has been used on hundreds (if not thousands) of robot+controller combinations over the years. I would have expected more reports about failed trajectory execution.

Having written that: increasing START_MAX_PULSE_DEVIATION is not necessarily a good idea. All subsequent traj pts are converted to increments, meaning a large(r) deviation at the start will lead to a similar/increasing deviation during execution (it's essentially open-loop).

I'm willing to build a binary for you, but I'd really like input from @ted-miller and/or @EricMarcil on what we're potentially seeing here.

I guess you have a YRC1000?

We get a failing trajectory start position check after the robot was idle for some time.

could you please show the ROS-side log and a wireshark capture including the traffic when the failure occurs?

#492 is something you'd run into "after the robot was idle for some time". Is eco-mode enabling on your robot? How are you recovering from that? For a demo setup, you could work-around it by increasing the idle-timeout significantly (it's a configuration setting you can change on the TP -- I wouldn't know the German name for it though).

[simonschmeisser]

I'd be happy to find out we're doing something stupid and I agree with you that historical evidence speaks against us.

Having written that: increasing START_MAX_PULSE_DEVIATION is not necessarily a good idea. All subsequent traj pts are converted to increments, meaning a large(r) deviation at the start will lead to a similar/increasing deviation during execution (it's essentially open-loop).

Given that I personally had my first contact with a Motoman two days ago I feel slightly uneasy disagreeing with you here but my understanding is that this offset is a temporary thing in this case due to the axis not being powered on and thus not being able to control the current pulse to the desired pulse (or not recovering automatically, apparently there is also a setting for whether it should just stay at the displaced pulse or correct it as soon as possible). So possibly this check should be more sophisticated or check the offset to mpGetFBPulsePos instead.

This is the code by @cjue for enabling, I guess it also serves as recovery:

void MotomanIndustrialIoAdapter::robotStatusUpdate(const industrial_msgs::RobotStatus& stateMsg)
{
    // check drives_powered?
    // check int8 in_error
    // check int32 error_code

    bool isRemoteMode = stateMsg.mode.val == 2;              // 2==remote, RobotMode int8
    bool isEmergencyStopped = stateMsg.e_stopped.val > 0;    // 1==TRUE, int8 TriState
    bool motionPossible = stateMsg.motion_possible.val > 0;  // 1==TRUE, int8 TriState

    // call robot_enable if robot is ready to put MotoROS driver in StartTraj mode
    // TODO: if other INFORM jobs need to run on the controller we should call robot_disable and not call robot_enable while they run
    try {
        if (motionPossible) {
            return;  // nothing to do
        }

        if (isRemoteMode && !isEmergencyStopped) {
            ros::NodeHandle nodeHandle;
            ros::ServiceClient serviceClient;

            serviceClient = nodeHandle.serviceClient<std_srvs::Trigger>("robot_enable");
            if (!serviceClient.exists()) {
                ROS_ERROR("[IO Motoman adapter]: Service is not available: %s", serviceClient.getService().c_str());
                return;
            }

            std_srvs::Trigger service;
            if (!serviceClient.call(service)) {
                ROS_ERROR("[IO Motoman adapter]: Calling ROS service failed: %s", serviceClient.getService().c_str());
                return;
            }
            if (!service.response.success) {
                ROS_ERROR_STREAM("[IO Motoman adapter]: robot_enable failed, message " << service.response.message);
                return;
            }

            ROS_INFO("[IO Motoman adapter]: robot_enable was called");
        } else {
            ROS_DEBUG("[IO Motoman adapter]: not calling robot_enable, robot not ready");
        }
    } catch (std::exception& e) {
        ROS_ERROR_STREAM("[IO Motoman adapter]: caught exception in robotStatusUpdate: " << e.what());
    }
}

@cjue will be at the site with the YRC1000 on Tuesday and should be able to capture the wireshark logs. If it's not too much trouble for you it would be great if you could send me a build with a higher threshold (I admit I don't know what would be high enough, 2.5e-3 in radians?)

[ted-miller]

My theory is that brakes are only released after this check and therefore the offset will never be corrected.

The brakes will be released when you call the robot_enable service. This service must be invoked prior to submitting any trajectory.

The only way the brakes would re-engage (assuming there's no error/estop), is if the power-saving mode kicks on. In that event, you're going to have another issue (#492). I suspect you're seeing another symptom of that issue.

I would also like to see a wireshark capture. But, please do it with an active telnet connection to the controller. (Set robot parameter S2C1119 = 2, then reboot. The username and password for the telnet connection is MOTOMANrobot.)

it's not too much trouble for you it would be great if you could send me a build with a higher threshold (I admit I don't know what would be high enough, 2.5e-3 in radians?)

I don't think that this is going to solve the issue. But here you go: MotoRosYRC_SPECIAL.zip
The tolerance should be around 4.9e-3 rad. (I'm not 100% certain about pulse/rad ratio for the GP7.)

I tried increasing the limit but what I compiled with the MotoPlusSDK (it was not the one integrated in VS but the old (?) standalone one) from the partner download area did not match in size and failed to start.

The project file(s) for MotoPlusIDE (standalone environment) have not been maintained for MotoROS. But, it uses the same compiler as the Visual Studio version. So, it should work if you get all the source and library files added to the project. (And manually define -DYRC1000 in the build settings.)

[gavanderhoorn]

@simonschmeisser: have you been able to make any progress?

[cjue]

We are still getting reports from our customer about rejected trajectories after the robot was idle for 2-5 minutes, but before energy saving mode kicks in, so it's not related to #492.

This is with the MotoRosYRC_SPECIAL.zip MotoROS version b< @ted-miller that increases the tolerance.

As this issue did not come up while I was at the site, I'll try again to capture it on wireshark on Monday.

Thanks already @ted-miller and @gavanderhoorn for your input and the hint regarding the extra Telnet connection in addition to capturing the SimpleMessage traffic with Wireshark and filtering it using https://github.com/ros-industrial/packet-simplemessage .

[gavanderhoorn]

@cjue: have you been able to capture a failure?