Ask Your Question
1

MoveIt problem. error: Trajectory message contains waypoints that are not strictly increasing in time

asked 2017-01-25 12:36:13 -0600

Tristan gravatar image

Hello,

I'm trying to use MoveIt's group interface to execute a Cartesian path. When I try to execute the plan with the trajectory, I get the error: Trajectory message contains waypoints that are not strictly increasing in time. I've even tried to fix it based on some code that someone else had luck with, but it doesn't work for me. Does anyone have any suggestions? Thanks! Here is my code:

    // Compute the trajectory of my waypoints 
moveit_msgs::RobotTrajectory trajectory;
double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory);

// All of the JointTrajectoryPoints in the trajectory have time_from_start = 0.00, which is the reason why the error gets thrown. The below steps are my attempt to fix the error, but they do nothing for me.
// First create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(move_group.getCurrentState()->getRobotModel(), "arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*move_group.getCurrentState(), trajectory);
// Thrid create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory);

plan.trajectory_ = trajectory;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);

if (success) {
    move_group.execute(plan);
    return true;
    }else{
    ROS_WARN("no success");
    return false;
}
edit retag flag offensive close merge delete

Comments

Hello, did you found a solution?

HannesIII gravatar imageHannesIII ( 2017-02-09 06:41:35 -0600 )edit

To some extent, I found a solution. The solution for me was trying my code on several test cases and finding that this code actually works perfectly on some of them. I think that MoveIt has trouble finding a cartesian path most of the time.

Tristan gravatar imageTristan ( 2017-02-09 08:17:01 -0600 )edit

I'm a little surprised that in some cases MoveIt doesn't have a more loud way to say that it has failed to find a cartesian path than just quietly returning a path with incorrect times.

Tristan gravatar imageTristan ( 2017-02-09 08:18:43 -0600 )edit

Update: see below for possible answer.

Tristan gravatar imageTristan ( 2017-02-09 13:19:09 -0600 )edit

5 Answers

Sort by ยป oldest newest most voted
1

answered 2017-02-09 08:14:18 -0600

Tristan gravatar image

It turns out that this error may be a problem with MoveIt itself. I tried my code on many different test cases and it worked properly (gave correct times between points and moved the arm) some of the time, after I tried quite a few paths. It seems that when MoveIt cannot find a proper path in some cases, the success boolean may still be true, but the waypoints still might all have time from start of 0, and the path can't be executed.

edit flag offensive delete link more

Comments

If this is really a bug in MoveIt, it would be greatly appreciated if you could report it on the issue tracker.

gvdhoorn gravatar imagegvdhoorn ( 2017-02-09 08:21:35 -0600 )edit
1

Okay thanks. Its impossible to know what is a bug, though, because MoveIt's code isn't documented. I'll throw it on the issue tracker soon, just in case it is a bug.

Tristan gravatar imageTristan ( 2017-02-09 09:29:03 -0600 )edit

isn't documented

that is a bit strong? What is it exactly that you feel is not provided here? A high-level overview of the semantics and intent of the code?

gvdhoorn gravatar imagegvdhoorn ( 2017-02-09 10:21:58 -0600 )edit

MoveIt doesn't provide unambiguous parameter and return specifications for many of their methods (sometimes theres no return documentation at all). You can always take MIT's 6.005 ( http://web.mit.edu/6.005/www/fa16/ ) via OpenCourseWare if you want to learn more about documentation standards.

Tristan gravatar imageTristan ( 2017-02-09 12:52:09 -0600 )edit

I don't think I have time to reply much more. I'll likely report this as a potential bug and be done.

Tristan gravatar imageTristan ( 2017-02-09 12:52:49 -0600 )edit
7

answered 2017-03-21 03:05:48 -0600

spooner-dev gravatar image

updated 2017-03-24 01:18:44 -0600

Not adding start point into waypoints solved this problem for me. In the Move Group Interface Tutorial they've added start point for visualisation:

You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through. Note that we are starting from the new start state above. The initial pose (start state) does not need to be added to the waypoint list but adding it can help with visualizations

Seem's to me that something like: "Initial pose must not be added to the waypoint list" would be better. Another note: even if fraction is less than 100% controller will still execute as much trajectory as was planned.

edit flag offensive delete link more

Comments

1

I can confirm that this bug still exists in the current version, and this fixed it for us. Our real robot did not complain about the trajectory, but gazebo did.

fvd gravatar imagefvd ( 2018-09-26 07:04:23 -0600 )edit

Fixed it for me two. When printing all the trajectory points' time from start, I noticed that the two firsts were 0. Either manually changing the second or not adding start point to waypoints solved it.

francoisdtf gravatar imagefrancoisdtf ( 2018-10-08 09:19:40 -0600 )edit
3

answered 2017-04-15 00:49:59 -0600

SimonB gravatar image

+1 for not adding start points. This solves the "Trajectory message contains waypoints that are not strictly increasing in time" issue for me, using the Python MoveIt interface (which is definitely undocumented!).

edit flag offensive delete link more
1

answered 2018-11-09 13:06:16 -0600

DanK gravatar image

You can see which waypoints are not increasing in time by looping through the computed RobotTrajectory and checking the 'time_from_start' on each point. I often find that it is only the first two points or the last two points in the trajectory that have the same value for 'time_from_start'. If you remove one of the points with equal 'time_from_start' you should be able to execute the trajectory.

You should also be able to prevent this issue altogether by increasing the value of the 'eff_step' argument in computeCartesianPath. As you lower the value of 'eff_step' you will get more points with equal 'time_from_start'. As an experiment, I tried entering some ridiculously low values for 'eff_step'. At an 'eff_step' of 0.00001, almost every other point was not increasing in time.

edit flag offensive delete link more
0

answered 2017-02-09 10:24:35 -0600

gvdhoorn gravatar image

updated 2017-02-09 10:26:28 -0600

I just noticed it in your code, but you never check the value of fraction: that is actually what encodes the 'success' of computeCartesianPath(..) here. If fraction < 1.0, something was not right.

bool success here contains the return value from the IPTP, which is a later step.

And the semantics of fraction are documented here.

edit flag offensive delete link more

Comments

Yes I know; thanks. before I even posted my question, I used fraction the way you instructed, but it still didn't prevent the time problem.

Tristan gravatar imageTristan ( 2017-02-09 12:56:43 -0600 )edit

Ok. Well if you don't include it in the code you show in your question this is what happens.

gvdhoorn gravatar imagegvdhoorn ( 2017-02-09 13:09:13 -0600 )edit

Actually you could very well be correct, because I can't completely remember if the time error ever happened at 100% (I think it may have but can't say for sure because I don't have access to the hardware and software anymore as this was for a winter internship that I finished).

Tristan gravatar imageTristan ( 2017-02-09 13:13:39 -0600 )edit

I ended up leaving fraction up to the user of my method to specify. I (as well as everyone else I was working with) was under the impression that fraction didn't have to be 100%, but that the plan would still be executed imperfectly(just say better than 90% if you asked for better than 90%).

Tristan gravatar imageTristan ( 2017-02-09 13:15:49 -0600 )edit

I'll mark your answer as correct because it seems plausible (but I still can't confirm it anymore). Thanks for your help, and I apologize if you felt inconvenienced since I didn't list everything that I did beforehand

Tristan gravatar imageTristan ( 2017-02-09 13:18:05 -0600 )edit

Thanks for the apology, but no need. I'm actually not entirely sure myself (was just editing my answer). I 'unaccepted' my answer, as it needs checking.

You might still want to report this on the tracker, but without a way to reproduce it, I'm not sure what will happen with the issue.

gvdhoorn gravatar imagegvdhoorn ( 2017-02-09 13:26:29 -0600 )edit

The interface should not yield broken timestamps for generated trajectories (even if fraction < 1.0. If you had a reproducible test-case (or bugfix), that would be highly appreciated. A bug report too if you fail to find the specific error.

v4hn gravatar imagev4hn ( 2017-02-09 13:32:53 -0600 )edit

Actually, as another moveit maintainer, I pretty much agree with you on the state of the code base: MoveIt is a huge heap of code that does not adhere to many design standards and does not give guarantees. But still, it is unusually useful and stands where it does for good reasons.

v4hn gravatar imagev4hn ( 2017-02-09 13:34:10 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2017-01-25 12:36:13 -0600

Seen: 1,925 times

Last updated: Apr 15 '17