Help: the code can not generate a curve

[ZongyuanShen]

Hi,

Thanks for the amazing project. Based on this, I code up a mini-example to compute 4D trajectory from (0,0,0,0) to (3,3,3,3). However, when I output the sample points along this trajectory, it is a straight line. If I understand correctly, the trajectory should be a curve.

I attached my code and the sample points below. Please point out the my mistake. Thank you very much.

Eigen::VectorXd start_pos(4); start_pos << 0, 0, 0, 0; Eigen::VectorXd end_pos(4); end_pos << 3, 3, 3, 3; const int dimension = start_pos.size(); mav_trajectory_generation::Vertex::Vector vertices; const int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP; mav_trajectory_generation::Vertex start(dimension), end(dimension); start.makeStartOrEnd(start_pos, derivative_to_optimize); vertices.push_back(start); end.makeStartOrEnd(end_pos, derivative_to_optimize); vertices.push_back(end); const double v_max = 2.0; const double a_max = 2.0; std::vector segment_times; segment_times = estimateSegmentTimes(vertices, v_max, a_max); mav_trajectory_generation::NonlinearOptimizationParameters parameters; parameters.max_iterations = 1000; parameters.f_rel = 0.05; parameters.x_rel = 0.1; parameters.time_penalty = 500.0; parameters.initial_stepsize_rel = 0.1; parameters.inequality_constraint_tolerance = 0.1; const int N = 10; mav_trajectory_generation::PolynomialOptimizationNonLinear opt(dimension, parameters); opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max); opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max); opt.optimize(); mav_trajectory_generation::Trajectory trajectory; opt.getTrajectory(&trajectory); trajectory.scaleSegmentTimesToMeetConstraints(v_max, a_max); int derivative_order = mav_trajectory_generation::derivative_order::POSITION; double edgeTraversalTime = trajectory.getMaxTime(); int sampleNum = ceil(edgeTraversalTime / 0.5); if (sampleNum == 0) sampleNum = 1; for (int sample_id = 0; 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(x,y,z,yaw): "<<sample(0)<<", "<<sample(1)<<", "<<sample(2)<<", "<<sample(3)<<endl; }

The sample points are shown below: sample(x,y,z,yaw): 0, 0, 0, 0 sample(x,y,z,yaw): 0.000396279, 0.000396279, 0.000396279, 0.000396279 sample(x,y,z,yaw): 0.00997142, 0.00997142, 0.00997142, 0.00997142 sample(x,y,z,yaw): 0.0587443, 0.0587443, 0.0587443, 0.0587443 sample(x,y,z,yaw): 0.189248, 0.189248, 0.189248, 0.189248 sample(x,y,z,yaw): 0.434537, 0.434537, 0.434537, 0.434537 sample(x,y,z,yaw): 0.799703, 0.799703, 0.799703, 0.799703 sample(x,y,z,yaw): 1.25536, 1.25536, 1.25536, 1.25536 sample(x,y,z,yaw): 1.74464, 1.74464, 1.74464, 1.74464 sample(x,y,z,yaw): 2.2003, 2.2003, 2.2003, 2.2003 sample(x,y,z,yaw): 2.56546, 2.56546, 2.56546, 2.56546 sample(x,y,z,yaw): 2.81075, 2.81075, 2.81075, 2.81075 sample(x,y,z,yaw): 2.94126, 2.94126, 2.94126, 2.94126 sample(x,y,z,yaw): 2.99003, 2.99003, 2.99003, 2.99003 sample(x,y,z,yaw): 2.9996, 2.9996, 2.9996, 2.9996 sample(x,y,z,yaw): 3, 3, 3, 3

[klaxalk]

Hey, why do you think that you should get a curve? The simplest trajectory connecting two waypoints (without specifying. other constraints like higher derivatives at the waypoints) lies on a line.

[ZongyuanShen]

Hey, why do you think that you should get a curve? The simplest trajectory connecting two waypoints (without specifying. other constraints like higher derivatives at the waypoints) lies on a line.

Hi, thanks for the reply. I understand now. I have several questions: In the sample points of the 6D trajectory from (2,0,0,0,0,0) to (4,0,0,0,0,0), the pitch is always zero.

Q1. Based on my understanding, if the quadcopter moves from x=2 to x=4, its front-end should point downward a little bit (pitch angle is negative) and then input thruster force to move forward. Is it because this libary does not simulate the actual orientation of the quadcopter when tracking the trajectory? Q2. To get the actual orientation, I need 1) trajectory tracker (e.g., MPC) takes the derired 6D trajectory and outputs velocity command, and 2) quadcopter simulation takes velocity command and computes force and position of each thruster. Is that right?