Trajectory start doesn't match Current Position, when using plan/execute

[JeremyZoss]

When sending back-to-back motion commands to MoveIt (using blocking calls), the trajectory start position should match (approximately) the goal position of the previous move. This works when using the all-in-one move() call, but not when using separate plan() and execute(plan) calls. In that case, it seems like the start position is using an outdated representation of the robot state, which lags behind the current robot state.

The expected behavior can be coerced using a few workarounds:

• add a delay (~1 sec) between motion requests
• explicitly force the MotionPlanRequest current state to the current joint position reported by MoveIt
• use the combined move() method instead.

I have tested this behavior using both a physical robot (Motoman FS100/SIA-20D) and a "virtual robot" (the ROS-Industrial Industrial_Robot_Simulator), under both groovy and hydro releases.

Steps to Reproduce

1. Launch any MoveIt package. For example:

   apt-get install ros-hydro-motoman ros-hydro-industrial-robot-simulator
   roslaunch motoman_sia20d_moveit_config moveit_planning_execution.launch
   

2. Disable any MoveIt RViz planning displays, so that only the current robot state is displayed.

3. Download the test script from here

4. Run the test script, using its default configuration

   ./moveit_trajStart.py
   

5. Observe the odd RViz motion and reported error messages. See below for a more detailed explanation.

Test Script Explanation

The test script creates a sequence of joint positions, using fixed-offsets from the current robot position. It's easiest to observe what's happening if the robot starts in a "clean" position (all-zeros, or similar). The script then executes a move to each of the positions, using blocking plan and execute(plan) calls. The planned trajectory is compared against both the current_joint_values (as reported by MoveIt) and the target goal position, to verify the start/end positions. If the values differ, an error message is reported.

Here is a sample of the reported planning error:

    curPos  : [0.200006, 0.199941, 0.200077, 0.200019, 0.200077, 0.200019, 0.200097]
    traj[0] : [0.015490, 0.015486, 0.015490, 0.015488, 0.015494, 0.015491, 0.015492]
    delta   : [-0.184516, -0.184455, -0.184587, -0.184531, -0.184583, -0.184528, -0.184604]

The correct behavior is for the script to execute cleanly, with no reported trajectory validation errors. The observed behavior is that the start position does not match the current position reported by MoveIt.

Experimentation

To further experiment with this issue, you can try the following things:

• pass a numeric argument to the script to adjust the delay between motion commands. A delay of 1 sec will allow clean execution: ./moveit_trajStart.py 1.0
• uncomment the script line set_start_state, to explicitly force the start state in the MotionPlanRequest to match the reported current state.
• comment the plan(), validate_plan(), and execute() lines, and uncomment the go() line.

[JimmyDaSilva]

I had the same problem with a UR10 in both simulation and real robot.
For me the problem was mainly caused by calling setStartStateToCurrentState(). This function was sometimes setting the start state to the previous start state. I just don't use this function anymore.

[simonschmeisser]

I sometimes observe a similar behaviour, but with regards to an object being attached or not. As JimmyDaSilva noted, I can increase the likelyhood of this to happen by calling setStartStateToCurrentState()

So I call attachObject(...); sleep(5); and then setPoseTarget and plan. Now the planing will happen without considering the attached object (including no collision checking of course). It is also executed without considering the attached object (it does get moved, but there is no collision checking for it)

[simonschmeisser]

In C++ I use the following workaround:

const std::string PLANNING_SCENE_SERVICE = "get_planning_scene";
robot_state::RobotState getCurrentRobotState(planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_)
{
        // each time the current state is needed
        planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
        planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
        ps->getCurrentStateNonConst().update();
        robot_state::RobotState current_state = ps->getCurrentState();
        return current_state;
}

planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ =
    boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"/*, tf_listener_*/);
group.setStartState(getCurrentRobotState(planning_scene_monitor_));

I'm on Indigo btw

[JimmyDaSilva]

Hi all,

I am coming back to using moveit and especially on this issue. It seems that it's been discussed a lot and some modifications have been merged: #407

Unfortunately I am still stuck with this same exact problem: If two successive plans are too close (less than 1sec) it seems that the state update is not made properly and so I see "jumps" at the beginning of the trajectory.

I tried:
planning_scene_monitor_->getStateMonitor()->waitForCurrentState(1.0);
or

robot_state::RobotState current_state = planning_scene_->getCurrentStateNonConst();
  group_->setStartState(current_state);

or
group_->setStartStateToCurrentState();

Problem goes away if I add a 1 second sleep after the end of the execution;

@davetcoleman @isucan @sachinchitta Did you ever face this issue ?

Thank you for your help !

PS: Running on Ubuntu 14.04 and Indigo

[andreaskoepf]

We ran into this issue too. I created PR #671 which contains a hot-fix for this problem which ensures that MoveIt! does not start planning on the basis of an outdated RobotState.

[JimmyDaSilva]

@andreaskoepf Thank you so much. Works great !

[130s]

Fixed in #716 et al.