Error "Validation failed: Trajectory doesn't start at current position." when executing high speed back to back moves

[MarqRazz]

Summary

I get following error when I run this python test_script.zip with the robot at max speed. Validation failed: Trajectory doesn't start at current position.

Every once in a while I also get the error: Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

Environment

OS : Ubuntu 18.04 ROS Version : Melodic Robot : MH5L (I have a forked repo here if you want to test on this robot) Controller : FS100

Robot configuration details

Because my joint accelerations don't match the robots capabilities exactly I have increased my allowed_execution_duration_scaling to 1.4 (I want to to always go as fast as possible) <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.4"/>

Below is a link to a copy of my joint_limits.yaml file along with a python script I have been hacking together to run the robot through a benchmark test to compare their planning/execution speed. test_script.zip

Details

I have added a line into utils.cpp in the industrial_core lib to print out which joint is causing the problem and how much error I am seeing. ROS_ERROR_STREAM(__FUNCTION__ << " Joint " << keys[i] << " has lhs position of: " << lhs.at(keys[i]) << " and rhs has position of: " << rhs.at(keys[i]) << " Total error: " << fabs(lhs.at(keys[i]) - rhs.at(keys[i])));

In my python script if I set the dwell_time_between_moves (line 34) to 0.5 second the robot performs as expected and I do not get the "Validation falied: ..." error. Here is a printout of each joints position error after executing move_to_joint_value_target() to give you an idea of how well the controller is doing (it looks like is it doing really good!). ['joint_s', 'joint_l', 'joint_u', 'joint_r', 'joint_b', 'joint_t'] Error is in radians [-0.00007, -0.00006, -0.00007, 0.00018, -0.00007, 0.00035] [-0.00004, 0.00002, -0.00004, 0.00018, -0.00013, 0.00044] [-0.00011, -0.00006, -0.00011, 0.00024, -0.00013, 0.00047] [0.00004, 0.00005, -0.00011, -0.00008, 0.00006, -0.00049] [0.00002, -0.00005, -0.00007, 0.00012, 0.00011, 0.00052]

If I lower the dwell_time_between_moves to anything below 0.3 I start getting "Validation falied: ..." errors and below is the printout I get from utils.cpp: isWithinRange Joint joint_b has lhs position of: -0.952353 and rhs has position of: -0.952468 Total error: 0.000115037 isWithinRange Joint joint_t has lhs position of: 0.708883 and rhs has position of: 0.709037 Total error: 0.000153422 isWithinRange Joint joint_r has lhs position of: -0.000997088 and rhs has position of: -0.00107379 Total error: 7.6699e-05 isWithinRange Joint joint_s has lhs position of: -0.708659 and rhs has position of: -0.70873 Total error: 7.01547e-05 isWithinRange Joint joint_r has lhs position of: -0.000824515 and rhs has position of: -0.000920388 Total error: 9.58738e-05 isWithinRange Joint joint_u has lhs position of: -0.699196 and rhs has position of: -0.699141 Total error: 5.51939e-05

As noted above also sometimes I also get the error Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011) and do not receive a printout from utils.cpp which I assume means the validation passed on the ROS side driver.

Lastly I am also getting the error: Validation failed: Missing velocity data for trajectory pt 0 when running with dwell_time_between_moves set below 0.3 but that is a MoveIt error and I am working with that source code to fix the problem.

Use Case

My use case that I am targeting is high speed pick and place. The program I use plans the entire pick and place of the item up front to ensure success and then breaks the execution up into three sections:

1. move from current pose to the pick point
2. move to the place pose and release the item
3. move back to a specified pose.

This methodology builds each section such that all of the trajectories are continuous but is now assuming that the robot controller has the capability to handle any stopping error or joint_state sync error that happens between moves. I am considering adding functionality to the driver if needed that will append start points if the start_pose does not match the current_pose but is within a specified amount of error. One question I have is do you think that this is something other users would like or should I keep this in my own fork? My second question is do you have any suggestions of where I should put this functionality (inside motoman_driver or MotoROS)? Do you have other suggestions on how I can get around this error?

[marip8]

I ran into a similar issue on a project where I was using a rail-mounted ES200RN robot. We weren't doing any sort of high speed operation, but one of the axes (S, if I remember correctly) would consistently be outside the 30 pulse-count threshold. We ended up writing a function in our application that compared the current state of the robot to the first state of the planned trajectory and replacing it with the current state if it was within our own defined threshold. This solution was really simple and worked well for us. I'm not sure if adding this functionality to the driver would be advisable due to the safety/accuracy concerns described in #312, but it should be easy enough to implement on your application side

[MarqRazz]

@marip8 do you have a branch of code with your fix implemented? How did you go about inflating the time_from_start and velocities of the initial start_point?

This functionality seems like an easy enough feature to add a configuration variable for end users to enable/disable if there are safety concerns.

[marip8]

All our trajectories started from a zero-velocity state. I'm not entirely sure what you mean by inflating the time_from_start field of the initial point. Are you wanting to insert the current state of the robot into the trajectory and keep all existing points? In our case, we simply replaced the first point of the trajectory with the current state of the robot, so its time_from_start was 0. If this is relevant for you, I can share my code that does this.

This functionality seems like an easy enough feature to add a configuration variable for end users to enable/disable if there are safety concerns.

True. There would probably have to be an upper limit on the allowable tolerance specified by the user above which replacing the start state of the trajectory with the current state would not be allowed. We could probably come up with a reasonable number from the testing that's been done around this issue. We would also need to be transparent to the user about this functionality. Maybe log warnings that the start state is being replaced would be enough, but it's something to think about

[MarqRazz]

Sorry @marip8 I was thinking you were appending the current_state to the trajectory not just replacing it. I tried this too and it fixes some of the execution requests but still fails frequently.

[tdl-ua]

We've got a MA2010 manipulator for welding applications with a DX200 controller. I am using the same method as @marip8 describes. This works excellent at least for trajectories where the welding torch is not activated. We are using a Fronius Robacta torch for welding and occasionally, the force of the welding wire pushing down onto the substrate plate just before arc ignition can cause enough joint displacement in some of the more distal joints (B and T) for the error to be thrown--I can actually observe the displacement of the torch. I don't know your particular setup @MarqRazz but maybe in your case, your gripping tool could have the same effect. I've also noticed that certain torch orientations add to this problem because of varying torques imposed on each joint by the advancing wire. Note that in my use case, this displacement can occur just after the current joint angles are obtained because the arc ignition and motion start need to be very synchronous.

For now, in these cases where the torch causes displacement I simply detect that the manipulator has not started moving when it should have (in_motion flag in RobotStatus) and try to abort and re-send the trajectory with updated initial joint angles. This causes a maximum delay of around 300 ms in my case if the manipulator fails to start moving and no delay if it does. If the error is not thrown, the manipulator usually starts to move within ~50-100 ms of publishing the trajectory.

I would agree that this kind of feature of updating the initial trajectory point if the given one deviates too much would add to the robustness of the driver and would also agree with @marip8 that there must be an upper tolerance limit at which updating the start point is not allowed to avoid unintended behavior.

My second question is do you have any suggestions of where I should put this functionality (inside motoman_driver or MotoROS)?

I think it should go into motoman_driver, not MotoROS .

[MarqRazz]

I did some measurement tests to see how much the joint angles settle after moveit has returned from executing the trajectory and here are the results.

rosout: Made it to the approach place pose with the following joint angle error:
rosout: ['joint_s', 'joint_l', 'joint_u', 'joint_r', 'joint_b', 'joint_t']
rosout: [-0.00019, 0.00018, 0.00002, 0.00044, -0.00000, 0.00110]
rosout: Delta time from robot stop: 0.002165
rosout: [-0.00019, 0.00016, 0.00002, 0.00044, -0.00000, 0.00107]
rosout: Delta time from robot stop: 0.027140
rosout: [-0.00019, 0.00015, 0.00000, 0.00042, -0.00000, 0.00104]
rosout: Delta time from robot stop: 0.052622
rosout: [-0.00018, 0.00014, -0.00001, 0.00040, -0.00002, 0.00101]
rosout: Delta time from robot stop: 0.077456
rosout: [-0.00016, 0.00013, -0.00001, 0.00036, -0.00002, 0.00086]
rosout: Delta time from robot stop: 0.103405
rosout: [-0.00015, 0.00012, -0.00003, 0.00035, -0.00002, 0.00067]
rosout: Delta time from robot stop: 0.130729
rosout: [-0.00014, 0.00012, -0.00004, 0.00031, -0.00002, 0.00052]
rosout: Delta time from robot stop: 0.155889
rosout: [-0.00013, 0.00011, -0.00005, 0.00027, -0.00002, 0.00049]
rosout: Delta time from robot stop: 0.177884
rosout: [-0.00013, 0.00009, -0.00007, 0.00023, -0.00002, 0.00046]
rosout: Delta time from robot stop: 0.203401
rosout: [-0.00012, 0.00008, -0.00007, 0.00019, -0.00004, 0.00040]
rosout: Delta time from robot stop: 0.227734
rosout: [-0.00011, 0.00008, -0.00007, 0.00017, -0.00004, 0.00034]
rosout: Delta time from robot stop: 0.253232
rosout: [-0.00011, 0.00008, -0.00008, 0.00013, -0.00004, 0.00031]
rosout: Delta time from robot stop: 0.281928
rosout: [-0.00009, 0.00007, -0.00008, 0.00012, -0.00004, 0.00025]
rosout: Delta time from robot stop: 0.306626
rosout: [-0.00009, 0.00007, -0.00008, 0.00010, -0.00006, 0.00021]
rosout: Delta time from robot stop: 0.328860
rosout: [-0.00008, 0.00007, -0.00008, 0.00008, -0.00006, 0.00015]
rosout: Delta time from robot stop: 0.354071
rosout: [-0.00008, 0.00007, -0.00009, 0.00008, -0.00006, 0.00012]
rosout: Delta time from robot stop: 0.382369
rosout: [-0.00008, 0.00007, -0.00009, 0.00008, -0.00006, 0.00009]
rosout: Delta time from robot stop: 0.406016
rosout: [-0.00008, 0.00007, -0.00009, 0.00006, -0.00008, 0.00009]
rosout: Delta time from robot stop: 0.427689
rosout: [-0.00008, 0.00007, -0.00009, 0.00006, -0.00008, 0.00006]
rosout: Delta time from robot stop: 0.456632
rosout: [-0.00008, 0.00007, -0.00009, 0.00006, -0.00008, 0.00006]
rosout: Delta time from robot stop: 0.481408
rosout: [-0.00007, 0.00007, -0.00009, 0.00006, -0.00008, 0.00003]
rosout: Delta time from robot stop: 0.504423
rosout: [-0.00007, 0.00007, -0.00009, 0.00006, -0.00008, 0.00003]
rosout: Delta time from robot stop: 0.527073
rosout: [-0.00007, 0.00007, -0.00009, 0.00004, -0.00008, 0.00003]
rosout: Delta time from robot stop: 0.552112
rosout: [-0.00007, 0.00007, -0.00009, 0.00004, -0.00010, 0.00003]
rosout: Delta time from robot stop: 0.578670
rosout: [-0.00007, 0.00007, -0.00009, 0.00004, -0.00010, 0.00003]
rosout: Delta time from robot stop: 0.605785

I have made adjustments to the driver so that when the first point in the trajectory does not pass the is_valid test I then check to see if it is withing a safety tolerance.

if (!is_valid(*traj)){
    namespace IRC_utils = industrial_robot_client::utils;
    if (IRC_utils::isWithinRange(cur_joint_pos_.name, cur_joint_pos_.position,
                                  traj->joint_names, traj->points[0].positions,
                                  0.02)){
      replace_start_state = true;
      ROS_ERROR("Traject is close enough, replacing first point");
    }
    else{
      return false;
    }
  }

After that point when the driver is converting the joint trajectory msg to a robot msg I replace the first point with it's current known position cur_joint_pos_ and print out the amount that I am changing the first point. Below are some results of running the modified driver...

Here is a successful replacement:

ros.industrial_robot_client: isWithinRange Joint joint_b has lhs position of: -0.81997 and rhs has position of: -0.820085 Total error: 0.000115097
ros.motoman_driver: Validation failed: Trajectory doesn't start at current position.
ros.motoman_driver: Traject is close enough, replacing first point
ros.motoman_driver: Changing first trajectory point from: 0.708694 to 0.708694 for joint: joint_s
ros.motoman_driver: Changing first trajectory point from: 0.123804 to 0.123815 for joint: joint_l
ros.motoman_driver: Changing first trajectory point from: -0.626453 to -0.626467 for joint: joint_u
ros.motoman_driver: Changing first trajectory point from: 0.000499 to 0.000499 for joint: joint_r
ros.motoman_driver: Changing first trajectory point from: -0.820085 to -0.819970 for joint: joint_b
ros.motoman_driver: Changing first trajectory point from: -0.709067 to -0.709067 for joint: joint_t
ros.moveit_simple_controller_manager.SimpleControllerManager: Controller arm_controller successfully finished
ros.moveit_ros_planning.trajectory_execution_manager: Completed trajectory execution with status SUCCEEDED ...

The modified driver has made my script run much better at executing but I still get frequent failures: on the MotoROS side: Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

ros.industrial_robot_client: isWithinRange Joint joint_s has lhs position of: -0.708157 and rhs has position of: -0.707993 Total error: 0.000163734
ros.motoman_driver: Validation failed: Trajectory doesn't start at current position.
ros.motoman_driver: Traject is close enough, replacing first point
ros.motoman_driver: Changing first trajectory point from: -0.707993 to -0.708157 for joint: joint_s
ros.motoman_driver: Changing first trajectory point from: 0.124321 to 0.124264 for joint: joint_l
ros.motoman_driver: Changing first trajectory point from: -0.625128 to -0.625445 for joint: joint_u
ros.motoman_driver: Changing first trajectory point from: 0.000594 to 0.000575 for joint: joint_r
ros.motoman_driver: Changing first trajectory point from: -0.820871 to -0.820756 for joint: joint_b
ros.motoman_driver: Changing first trajectory point from: 0.706889 to 0.707012 for joint: joint_t
ros.motoman_driver: Aborting Trajectory.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)
ros.moveit_ros_planning.trajectory_execution_manager: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.328374 seconds). Stopping trajectory.

Looking through the code more the joint_trajectory_interface class that I have been modifying subscribes to the joint_states topic to update the cur_joint_pos_ variable. This means that we are only getting updates to the variable at 40 Hz and it has gone through multiple relays meaning the information is slightly delayed.

I also have my entire robot setup running in Gazebo and have been doing some tests. I have found that if I reduce my joint_state_controller rate (this is the rate the joint_states are published) from 50 Hz to 40 Hz (to match the Yaskawa hardware) I can get the trajectory execution manager to complain about start state not matching.

ros.moveit_ros_planning.trajectory_execution_manager: Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'joint_l': expected: 0.104871, current: 0.123557

My question is can I easily increase the joint_state update rate on the motoman_driver side? I have done some digging but cant find the location to update the polling rate.

Any other suggestions on things I can do to fix this error @EricMarcil or @ted-miller? Is there any chance I can get Yaskawa to also start investigating a fix?

[MarqRazz]

I found the update rate, its in the MotoPlus application in StateServer.h.

I have a request out to Yaskawa for access to the MotoPlus SDK. I'll let you know when I get a chance to run the joint states at 50 Hz.

[ted-miller]

To update the joint-state rate on the MotoROS side, you just need to modify the define in StateServer.h. #define STATE_UPDATE_MIN_PERIOD 25 // Time (ms) delay between each state update

But, I think that you should be looking at the "in_motion" flag to determine when the robot has reached the destination of the previous trajectory. If the robot is still decelerating when you fetch the current position, then it will have moved by the time you issue the next trajectory. This will lead to the invalid-trajectory error again.

Alternatively, it sounds like your application would be good for the point streaming interface. (#215) I believe this interface allows you to continuously stream in points without starting a "new trajectory". The new points would just appended to the end of what is currently executing. The joint tolerance check is only performed at the first point of a new trajectory. Please note there is a 4 hour limit for a single trajectory. After that, you must start a new trajectory, or you will get an error.

[MarqRazz]

But, I think that you should be looking at the "in_motion" flag to determine when the robot has reached the destination of the previous trajectory. If the robot is still decelerating when you fetch the current position, then it will have moved by the time you issue the next trajectory. This will lead to the invalid-trajectory error again.

If you look in joint_trajectory_action.cpp line 240 you will see that this is where the action server is checking to see if the arm has made it within the required goal constrains. Once the arm is considered stopped it sets the in_motion flag false and returns from the action. This is when my MoveIt call of move_group.execute(plan, wait=True) returns and I go on planning the next move. So in the end I am waiting until the in_motion flag is false.

Alternatively, it sounds like your application would be good for the point streaming interface. (#215) I believe this interface allows you to continuously stream in points without starting a "new trajectory". The new points would just appended to the end of what is currently executing. The joint tolerance check is only performed at the first point of a new trajectory. Please note there is a 4 hour limit for a single trajectory. After that, you must start a new trajectory, or you will get an error.

This sounds like a hack and would be a lot of extra work on my application to do something I think the robot driver should be capable of. If you look above at my test where I poll the joint states after executing a move you can see that the arm is still moving for more than a half second after it has said it finished. Do you think that all applications should have to wait that long before they send another move to the arm? I would like my code to work on any industrial arm and would prefer to not have to implement the streaming interface on all of them for this work around.

[gavanderhoorn]

@MarqRazz wrote:

If you look in joint_trajectory_action.cpp line 240 you will see that this is where the action server is checking to see if the arm has made it within the required goal constrains. Once the arm is considered stopped it sets the in_motion flag false and returns from the action.

I'm not sure I understand this: in_motion is not set by the action server. It's a field in the STATUS message (here) which is broadcast by MotoROS (here and here).

The action server uses it, but it certainly does it set it.

[EricMarcil]

There are basically 3 main tasks in the MotoRos driver: 1) The motion server receives the command, processes it to generate pulse increments for each interpolation cycle (4 ms) and puts them in a buffer.
2) There is another task that is synchronize with the controller interpolation cycle, that one transfers the motion increments from the buffer to the controller servo control loop. 3) The state server monitors the robot current feedback position and periodically report on the robot real position.

Like any control system, the feedback position tries to catch-up to the command position but will lag behind because of inertia, filtering, communication delays. So even if you have completed sending your command, the robot may still be in motion.

In theory, if you keep track of where you've finish you last position, you could just continue on from it. The robot will eventually reach that position and continue on afterwards.

The problem is when something interrupts the motion, like a hold, e-stop, alarm... and the robot never reached it last commanded position. How can we ensure that we are starting from robot real position and not the last final destination that was never reached.

Since we don't know how people are implemented their code on the ROS side, we were extra careful to keep the system as safe as possible and added the check to enforce that new trajectory always starts from the robot stopped feedback position. The drawback is that delay to make sure that the robot feedback comes to a complete stop.

We might be able to improve on that by storing the last commanded position and allow a new trajectory to restart from that position, but we would need to make sure that this position is invalidate in any situation where the motion is interrupted. We also need to make sure that there is not accumulation of small errors anywhere which might accumulate over time and cause the robot to gradually drift over time.

[MarqRazz]

@gavanderhoorn wrote: I'm not sure I understand this: in_motion is not set by the action server. The action server uses it, but it certainly does it set it.

Sorry I worded that incorrectly. It checks the in_motion status and when it is false it returns from the action which means that I am waiting for the in_motion flag to be false before planning another move. As you can see below it does not write the value it just reads it.

if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE)
 {
     ROS_INFO("Inside goal constraints, stopped moving, return success for action");
     active_goal_map_[robot_id].setSucceeded();
     has_active_goal_map_[robot_id] = false;
 }

@EricMarcil wrote: Since we don't know how people are implemented their code on the ROS side, we were extra careful to keep the system as safe as possible and added the check to enforce that new trajectory always starts from the robot stopped feedback position. The drawback is that delay to make sure that the robot feedback comes to a complete stop.

We might be able to improve on that by storing the last commanded position and allow a new trajectory to restart from that position, but we would need to make sure that this position is invalidate in any situation where the motion is interrupted. We also need to make sure that there is not accumulation of small errors anywhere which might accumulate over time and cause the robot to gradually drift over time.

This is why I have added a second check to see if the robot is close enough (0.02 radians) to its current "known" position (this should cover both of the cases you listed). The issue I am running into is that the motoman_driver does not have current enough information to pass the lower level check on the controller.

if (!is_valid(*traj)){
    namespace IRC_utils = industrial_robot_client::utils;
    if (IRC_utils::isWithinRange(cur_joint_pos_.name, cur_joint_pos_.position,
                                  traj->joint_names, traj->points[0].positions,
                                  0.02)){
      replace_start_state = true;
      ROS_ERROR("Traject is close enough, replacing first point");
    }
    else{
      return false;
    }
  }

Do you guys think this approach is acceptable and safe? In the end I think that this check belongs on the MotoROS side as its the only one with the true state of the robot.

I am saying as an engineer in the industrial sector that the current level of performance will not meet my customers need for cycle time. As you can see below MoveIt is more than capable of creating motion plans in the same order of time that the controller is running at and we will always have to deal with delayed joint_states. Planning time to move to retract pick took: 0.00326681137085 seconds This is why I think that it is the driver/controllers job to help correct the problem. I 100% think that we can achieve the level of performance I'm looking for and ensure safety because in the end I require both to release my system.

[EricMarcil]

I've been testing some changes to the MotoRos driver:

1) I’ve made modifications to allow a trajectory start from the end of the previous one as long as the motion is not interrupted (hold, e-stop…). But to take advantage of this, the ROS side probably need to be modified: a. Keep track of the end of last trajectory. b. If the InMotion is on, you can generate a new trajectory from the last trajectory. The start the new trajectory with time=0 sequence=0 and the coordinate of the last trajectory end. c. If the InMotion is off then use the current position to plan the new trajectory. I'll make a branch of this and make it available soon.

2) We can switch the current position from using feedback to command position. That should stabilize the reported current position and tell you sooner that you’ve reached the end of the trajectory. The drawback is that during motion the position you will be receiving is not the real position of the robot, the robot usually lags behind between 150 ms to 350 ms. So even if the position you are receiving is the end of the trajectory, the arm may still move a little bit afterward.

[EricMarcil]

The code for the change mention in 1) above that allow a trajectory to start from the end of the previous one is available on my personal branch AppendTrajectory Let me know if that help and if you thing this should eventually made part of the standard driver.

[MarqRazz]

So I have been working with Eric to test new firmware and we have the arm behaving much better but I am still running into issues with the ROS side driver when calling stop from MoveIt and then making a plan immediately afterwards to somewhere else. Below is a printout from my terminal where I have modified the jointTrajectoryCB to print out what is happening when it receives a message.

[1589925191.609793889] ros.industrial_robot_client.joint_trajectory_action: Received new goal
[1589925191.609963927] ros.industrial_robot_client.joint_trajectory_action: Publishing trajectory
[1589925191.610378818] ros.motoman_driver: Receiving joint trajectory message of length 11
[1589925191.610445939] ros.motoman_driver: Current state is: 0
[1589925191.610478691] ros.motoman_driver: converting trajectory_to_msgs
[1589925191.611467332] ros.motoman_driver: Sending to robot
[1589925191.628056196] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.647084399] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.666138282] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.684926813] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.704091417] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.723076208] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.741829370] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory
[1589925191.760686421] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory

[1589925192.490669244] ros.moveit_simple_controller_manager.ActionBasedController: Cancelling execution for arm_controller
[1589925192.490797334] ros.moveit_ros_planning.trajectory_execution_manager: Stopped trajectory execution.
[1589925192.491157204] ros.industrial_robot_client.joint_trajectory_action: Received action cancel request
[1589925192.491545641] ros.moveit_simple_controller_manager.SimpleControllerManager: Controller arm_controller successfully finished
[1589925192.491605338] ros.motoman_driver: Receiving joint trajectory message of length 0
[1589925192.491688121] ros.motoman_driver: Current state is: 0
[1589925192.491710002] ros.moveit_ros_planning.trajectory_execution_manager: Completed trajectory execution with status PREEMPTED ...
[1589925192.491723883] ros.motoman_driver: Empty trajectory received while in IDLE state, nothing is done 
[1589925192.492048552] ros.pick_place_action.pick_place_robot: Finished executing seek move

[1589925192.521729483] ros.industrial_robot_client.joint_trajectory_action: Received new goal
[1589925192.521918117] ros.industrial_robot_client.joint_trajectory_action: Publishing trajectory
[1589925192.522409668] ros.motoman_driver: Receiving joint trajectory message of length 10
[1589925192.522490935] ros.motoman_driver: Current state is: 0
[1589925192.522537263] ros.motoman_driver: converting trajectory_to_msgs
[1589925192.523551861] ros.motoman_driver: Sending to robot
[1589925192.530826511] ros.motoman_driver: Aborting Trajectory.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

If you look at jointTrajectoryCB() you will see that the function checks if the TransferState != IDLE and if that is the case it should send the robot the trajectoryStop() command but if it is equal to IDLE then it does nothing and just prints a message. If you look at my printout from above you can see that as the arm is executing my first move when MoveIt cancels the trajectory and the state == IDLE so the robot is not sent the trajectoryStop() command. Here is an issue on the industrial_core side with the same problem. Looks like the Fanuc robot is also having an issue (that will probably be my next robot arm).

I changed this behavior so the trajectoryStop() command is sent but I have found another issue. Below is a Wireshark capture of the above event.

Inspecting the industrial_robot_client/joint_trajectory_action cancelCB you can see that the method publishes an empty trajectory message and then returns before the robot has actually stopped. @gavanderhoorn do you think that the trajectory action server should be required to wait until the robot stops before it returns from a stop command? I can open an issued over there but wanted to see what you think the best course of action should be here.

[EricMarcil]

Is the IDLE state corresponding to my In_Motion state? Because as part of the modification of the MotoRos driver, we are now using the command position instead of the feedback and the In_Motion now becomes false as soon as the internal motion queue is empty. This was to allow a quicker restart of a new trajectory.

So toward the end of the motion, there is probably a 300 to 400 ms where the robot feedback position is still catching up to the command position, but the In_Motion will be false.

But then you don't actually need the stop motion at that point, since the command motion is complete. At that time, the reported current position should be the command position which has reached the end of the trajectory and you should be able to replan from there...

So I guess that is not what is happening. But still, I thought I should share the reflection and make everyone aware that this is a modified version where the current position and the in_motion logic is slightly different. Maybe someone can see something we are missing?

[EricMarcil]

The code for the change mention in 2) above that allow a new trajectory to be planned and started as soon as the previous trajectory has been completed transferred to the controller or aborted (MotionStop cmd). This is available on my personal branch Command-position Let me know if that helps and if you thing this should eventually made part of the standard driver.

[Pranav186]

Hello, I have been facing the same problem for my Motoman MHJ robot. From the discussions, I assume that there is some change to be made in the source code to overcome this issue.

I am unable to find this code that you have mentioned?

The code for the change mention in 1) above that allow a trajectory to start from the end of the previous one is available on my personal branch AppendTrajectory Let me know if that help and if you thing this should eventually made part of the standard driver.

[gavanderhoorn]

@Pranav186: unless you either have a multi-group system or are trying to run trajectories at almost max robot performance with no stops in between them I'm not sure you are running into the issue described here.

Please post a new issue and describe the problem you are seeing.

If it turns out it's the same as the one discussed here, we'll redirect.

[cjue]

The code for the change mention in 1) above that allow a trajectory to start from the end of the previous one is available on my personal branch AppendTrajectory Let me know if that help and if you thing this should eventually made part of the standard driver.

@EricMarcil the AppendTrajectory changes look very promising for our current issues with very fast back-to-back trajectories.

Could you please rebase that branch to MotoROS 1.9.11? There have been some safety/error-handling commits in between and I'm wary of doing that rebase myself without lots of care and testing.

[EricMarcil]

@cjue I'm working on a tight deadline for the end of Nov. I probably won't have time to work on this until early December.

[cjue]

We noticed another possible cause for mismatch between command and feedback position: In one of our applications we do a handover of the gripped work-piece to another gripper. As soon as the second gripper closes it can exert some force on the robot, pulling it away from its resting position. In our case this deviation was sometimes large enough to fail the check for the next trajectory starting position (3011).

Conditionally replacing the first trajectory point as described by @marip8 above helped mitigating this problem.