Tragectory generated is not smooth

[frontw]

Hi!

I continue testing this amazing repo. Really like its performance, but become not sure about smoothness of resulting trajectory. Could I ask question about the resulting trajectory it generates. I think, it is not so smooth, as it should be.

In picture there is orange trajectory by this repo and blue by another realization of original Minimum snap trajectory algorithm. Could you give me idea, what is my mistake?

Trajectory key points are

• {0, 0, 0}
• {0.9, 1, 0}
• {0.7, 2, 0}
• {3, 3, 0}
• {4, 4, 0}

Here is code mostly the same as in readme:

    mav_trajectory_generation::Vertex::Vector vertices;
    const int dimension = 3;
    const int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP;
    mav_trajectory_generation::Vertex start(dimension), middle1(dimension), middle2(dimension), middle3(dimension), end(dimension);

    //Add constraints to the vertices.
    start.makeStartOrEnd(Eigen::Vector3d(0,0,0), derivative_to_optimize);
    vertices.push_back(start);

    middle1.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(0.9,1,0));
    vertices.push_back(middle1);

    middle2.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(0.7,2,0));
    vertices.push_back(middle2);

    middle3.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(3,3,0));
    vertices.push_back(middle3);

    end.makeStartOrEnd(Eigen::Vector3d(4,4,0), derivative_to_optimize);
    vertices.push_back(end);

    //Compute the segment times.
    std::vector<double> segment_times;
    const double v_max = 2.0;
    const double a_max = 2.0;
    const double magic_fabian_constant = 6.5; // A tuning parameter.
    segment_times = estimateSegmentTimes(vertices, v_max, a_max, magic_fabian_constant);

    //Create an optimizer object and solve.
    //The template parameter (N) denotes the number of coefficients
    //of the underlying polynomial, which has to be even.
    //If we want the trajectories to be snap-continuous, N needs to be at least 10.
    const int N = 10;
    mav_trajectory_generation::PolynomialOptimization<N> opt(dimension);
    opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
    opt.solveLinear();

    //Obtain the polynomial segments.
    mav_trajectory_generation::Segment::Vector segments;
    opt.getSegments(&segments);

[frontw]

Do I need to add another constraints for middle points?

[helenol]

Hey @frontw! Thanks for the analysis. :) As a quick question, is the N and and derivative to optimize the same between the two test trajectories (our repo and the other)? Because then the difference you're seeing is due to the time allocation, which is coming from estimateSegmentTimes. This is just a heuristic written by a student ages ago, that tries to very conservatively respect the velocity and acceleration constraints. If you compare the resulting segment times between the implementations, I suppose they will be quite different. Since the underlying optimization math remains the same no matter how you implement it ( ;) ), this is probably the only difference between the two. Could you adjust the segment times (divide by 2, multiply by 2, whatever) and qualitatvely look at the results? And could you compare the segment times you get from our magic fabian estimation vs the other implementation?

Thanks a lot, Helen

[frontw]

Hi, @helenol! Thank you for quick answer and suggestions! I tried to set up the same timing for 2 variants of trajectory generation. I get times, that mav_tragectory_generation gives me segment_times 3.62292 3.41056 3.83548 3.64904 and setup times for compute_minsnap_mellinger11 from https://github.com/dennisss/tansa/

    ConstrainedPoint cp1({p1});
    x.push_back(cp1);
    t.push_back(0);
    ConstrainedPoint cp2({p2});
    x.push_back(cp2);
    t.push_back(0 + segment_times[0]);
    ConstrainedPoint cp3({p3});
    x.push_back(cp3);
    t.push_back(0 + segment_times[0] + segment_times[1]);
    ConstrainedPoint cp4({p4});
    x.push_back(cp4);
    t.push_back(0 + segment_times[0] + segment_times[1] + segment_times[2]);
    ConstrainedPoint cp5({p5});
    x.push_back(cp5);
    t.push_back(0 + segment_times[0] + segment_times[1] + segment_times[2] + segment_times[3]);

    compute_minsnap_mellinger11(x,
                                t,
                                {},
                                &tr, NULL);

The result is as follows (not the same): Multiplying segment times by 0.5, 2, 5 and so on doesn't change trajectory, but speed and acceleration.

[helenol]

Thanks for the link to the code, very interesting! Haven't seen this implementation before. Looks like they actually use the constrained formulation rather than the unconstrained... But this anyway shouldn't matter, I believe they're equivalent. Then the only possible differences I can see are the constraints on the waypoints. For our implementation, start and goal (makeStartOrEnd) constrains all derivatives up to snap to 0 for the initial and last points, and I'm not sure this is the case in their implementation? ( direct link if anyone else wants to compare: https://github.com/dennisss/tansa/blob/7c7405aa6f98caa1f43d73906fc1e0a7831ad41b/src/trajectory/minsnap.cpp#L33 ) Could you either switch mav_traj_gen to not use makeStartOrEnd (and just leave the ends free) and see if you get the same result then, or alternatively swap the tansa implementation to constrain the start and end to 0 derivatives in all dimensions and all derivatives?

If it's not that, I'm really stumped about what the difference could be!

Edit: Just to make it clear, I mean don't constrain the derivatives below position for the start and end; so rather than makeStartOrEnd, call addConstraint with just the position. Not leave them completely free ;)

[frontw]

Yes, that's it! Thanks!

[helenol]

Woohoo, glad that's solved!