Controller and industrial-robot-client out of sync for empty trajectories

[simonschmeisser]

If the robot is already at the goal position up until now we were sending a trajectory containing start=goal two times and an execution time of 0s. robot_state_node will therefore decide that we reached the goal immediately. The trajectory Uploader/Streamer will however just now start uploading the trajectory to the controller, do some overhead there and generally take some time. During this time our code thinks the robot is ready while in fact it is still executing a "pointless" trajectory.

This seems to have two implications for us, our mitsubishi driver fails to control IOs sometimes as the controller does not allow parallel access. The fanuc driver sometimes rejects trajectories as the end of the previous trajectory (empty or not) is not properly synchronized.

[gavanderhoorn]

The fanuc driver sometimes rejects trajectories as the end of the previous trajectory (empty or not) is not properly synchronized.

Can you elaborate on this a bit? I'm not sure I understand what you are trying to say.

[simonschmeisser]

So JointTrajectoryStreamer has an purely internal state variable TransferState state_;, this is set to this->state_ = TransferStates::STREAMING; in send_to_robot. A worker thread will then stream those trajectory points to the controller (taking up to 0.25s for the first point as the worker thread is "sleeping" (reduces it's rate when idle)).
Meanwhile RobotStateInterface checks position and duration and concludes we reached the goal already. We will therefore send the next trajectory and JointRelayHandler will accept it.
Now JointTrajectoryStreamer is still in STREAMING state and will give us
ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");

As we lack the feedback channel to JointRelayHandler, this will lead to
a) the current motion being canceled
b) the next trajectory being rejected
c) no feedback on trajectory execution interface until the timeout is reached

[gavanderhoorn]

That I understood :) (but thanks for the additional info)

I was more curious as to why you specifically mentioned fanuc_driver.

But I gather that you use it as a reference to a "regular IRC driver/client". Correct?

[simonschmeisser]

yes sorry, fanuc_driver is the first streaming "regular IRC driver" I actually use. Up until now I was always using self-made drivers based on joint_trajectory_downloaders interface

[shaun-edwards]

@simonschmeisser, thanks for reporting this issue. I definitely needed the additional info, it's been a while since I looked at that particular code.

As I understand it, this bug occurs when a trajectory with the start and goal states being identical (or nearly). This is a known issue in #131 (submitted by you) with the action interface node, but if I understand correctly, this occurs with the streaming node as well?

[simonschmeisser]

Well, there is two issues here:

c) no feedback on trajectory execution interface until the timeout is reached

is indeed #131

but this is more a symptom than a cause, ideally there should be something like EXECUTING flag in status message that allows the server/controller to decide when it's done. I think this was discussed in ros-industrial/rep#12 but never materialized

[gavanderhoorn]

@simonschmeisser wrote:

ideally there should be something like EXECUTING flag in status message that allows the server/controller to decide when it's done. I think this was discussed in ros-industrial/rep#12 but never materialized

That REP is specifically targeted at Cartesian motions. I would rather see such a flag added to STATUS, but as always, that would have to take bw-compatibility into account.

We have to be careful though: making IRC check those things should not make it impossible to implement something like trajectory replacement (or we should just migrate to ros_control wholesale).