motoman_driver: driver flags trajectory completion too soon

[jrgnicho]

The fs100 controller driver seems to return a completion result for a trajectory even though the robot is still moving towards the goal position. This issue was observed when I sent a new trajectory with the "execute" method of the moveit move_group interface. The driver error message complained about the start state differing from the current state and then the node running the move_group interface would quit. My application only uses locking motion methods (move() and execute() ), which should theoretically prevent situations where a new trajectory is sent while another trajectory is being consumed.

[jrgnicho]

A catkin package that can help reproduced this behaviour has been created in the following repo https://github.com/jrgnicho/motoman_driver_test . It also contains instructions for setup and execution.

[shaun-edwards]

@jrgnicho, @JeremyZoss, Do either of you have time to track this down?

[JeremyZoss]

Actually, I think this behavior may be caused by MoveIt's trajectory planner, rather than the FS100 controller. When I dig into the issue, it seems like the internal state MoveIt's trajectory planner uses for the trajectory start-state is different than MoveIt's own "Current Joint State" feedback. When motion plans are issued quickly back-to-back, the trajectory planner seems to use out-dated position data, and thus triggers the error described above.

I have been able to reproduce this error, both on a physical robot (Motoman FS100 controller) and using the robot-agnostic industrial_robot_simulator. Note that I have only tested in Groovy so far.

I have created a test-script here that demonstrates this behavior. Load up any MoveIt package, then run the script to issue a set of back-to-back motion commands. In my testing, for all planned moves other than the first (which is a special-case), the start position of a MoveIt-planned trajectory does not match the Current Joint Position also reported by MoveIt.

[JeremyZoss]

I have verified this behavior in hydro. On further testing, it seems this behavior is only seen when using the separate plan and execute(plan) calls. When using the all-in-one move() call, the robot behaves as expected. This explains why we haven't typically seen this before, since most of our code uses the single move() call.

I have logged a detailed MoveIt Bug Report (moveit/moveit_ros#442). See that bug for further discussions. We'll close this bug after verifying a MoveIt fix.

[gavanderhoorn]

According to the comments on moveit/moveit_ros#442 this is fixed.

I'm closing this as I believe this is no longer a problem - at least not the problem that @jrgnicho and @JeremyZoss observed and investigated.

There is / may still be a problem with the driver flagging completion of trajectories too quickly, but that is related to multi-group motion and is being addressed in #179.