Help: trajectory generator always gives a straight line instead of curve

[ZongyuanShen]

Hi,

I am trying to integrate mav_trajectory_generation with RRT* in 3D space, i.e., use it as a local steering function to compute trajectory from point A to point B. However, the trajectory is always a straight line, insteaf of a curve. Please help!

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), end(dimension); start.makeStartOrEnd(Eigen::Vector3d(0,0,1), derivative_to_optimize); vertices.push_back(start); end.makeStartOrEnd(Eigen::Vector3d(2,1,5), derivative_to_optimize); vertices.push_back(end); std::vector segment_times; const double v_max = 2.0; const double a_max = 2.0; segment_times = estimateSegmentTimes(vertices, v_max, a_max); const int N = 10; mav_trajectory_generation::PolynomialOptimization opt(dimension); opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); opt.solveLinear(); mav_trajectory_generation::Trajectory trajectory; opt.getTrajectory(&trajectory); // Print out the sample points on the computed trajectory int derivative_order = mav_trajectory_generation::derivative_order::POSITION; double edgeTraversalTime = trajectory.getMaxTime(); int sampleNum = ceil(edgeTraversalTime / 0.1); for (int sample_id = 1; sample_id <= sampleNum; sample_id++) { double traversalTime = edgeTraversalTime * sample_id / sampleNum; if (traversalTime > edgeTraversalTime) traversalTime = edgeTraversalTime; Eigen::VectorXd sample = trajectory.evaluate(traversalTime, derivative_order); std::cout<<sample(0)<<", "<<sample(1)<<", "<<sample(2)<<";"<<endl; }