If we change to the 'load_existing_trajectory' in main.yaml to false, then we can generate a new trajectory based on the replaced gate positions instead of reading from a trajectory file that was generated from a different gate layout.
In order to realize that, I made several minor changes in the code and found a typo (bug) in the code:
in global_trajectory.cpp, GlobalTrajectory::generateGlobalTrajectory(),
if (!load_existingtrajectory):
...(some code)
saveGlobalTrajectory();
loadGlobalTrajectory();(added line)
in global_trajectory.cpp, GlobalTrajectory::saveGlobalTrajectory(),
delete this line:
outfile_trajectory.open(filename_trajectory, std::ios_base::app);
instead use this line:
outfile_trajectory.open(filename_trajectory, std::ofstream::out | std::ofstream::trunc);
(in order to clear the content saved in the previous file)
this is typo (bug) that exists in the code which would disable the global trajetory generation.
in rpg_quadrotor_control repository, minimum_snap_trajectories.cpp,
Eigen::VectorXd generateFVector(),
delete this line:
for (int k = 0; k < num_polynoms; k++)
instead use this line:
for (int k = 0; k < num_polynoms - 1; k++)
since within the for loop, there is a reference of way_points_1D(k + 1), and num_polynoms equals to the number of waypoints, which is 14. The previous usage would use way_points_1D(14) which is out of the dimension, however, this won't arise any error when compile which makes the bug hard to find.
this is because in PolynomialTrajectory generateMinimumSnapRingTrajectory(),
this line:
Eigen::VectorXd way_points_d = Eigen::VectorXd::Zero(num_waypoints);
defines the way_points_d as Eigen::VectorXd, which would later give to function
Eigen::VectorXd generateFVector().
when you refer an element that is out of the dimension, such as way_points_d(14), it won't arise error, instead, it will give you a random number which could be extremely large to eventually disable the global trajectory generation.
If we change to the 'load_existing_trajectory' in main.yaml to false, then we can generate a new trajectory based on the replaced gate positions instead of reading from a trajectory file that was generated from a different gate layout.
In order to realize that, I made several minor changes in the code and found a typo (bug) in the code:
in global_trajectory.cpp, GlobalTrajectory::generateGlobalTrajectory(), if (!load_existingtrajectory): ...(some code) saveGlobalTrajectory(); loadGlobalTrajectory();(added line)
in global_trajectory.cpp, GlobalTrajectory::saveGlobalTrajectory(), delete this line: outfile_trajectory.open(filename_trajectory, std::ios_base::app); instead use this line: outfile_trajectory.open(filename_trajectory, std::ofstream::out | std::ofstream::trunc); (in order to clear the content saved in the previous file)
this is typo (bug) that exists in the code which would disable the global trajetory generation. in rpg_quadrotor_control repository, minimum_snap_trajectories.cpp, Eigen::VectorXd generateFVector(), delete this line: for (int k = 0; k < num_polynoms; k++) instead use this line: for (int k = 0; k < num_polynoms - 1; k++)
since within the for loop, there is a reference of way_points_1D(k + 1), and num_polynoms equals to the number of waypoints, which is 14. The previous usage would use way_points_1D(14) which is out of the dimension, however, this won't arise any error when compile which makes the bug hard to find. this is because in PolynomialTrajectory generateMinimumSnapRingTrajectory(), this line: Eigen::VectorXd way_points_d = Eigen::VectorXd::Zero(num_waypoints); defines the way_points_d as Eigen::VectorXd, which would later give to function Eigen::VectorXd generateFVector(). when you refer an element that is out of the dimension, such as way_points_d(14), it won't arise error, instead, it will give you a random number which could be extremely large to eventually disable the global trajectory generation.