Final trajectory has collision with the obstacle
VincentHFZ opened this issue · 8 comments
We test your code and compare it with your paper, [Gradient-Based Online Safe Trajectory for Quadrotor Flight in Complex Environments]. We have some questions about your project.
First, in your paper V-C, you mentioned that you first optimize the collision cost only. All the free variables will be pushed to the basin of the distance field from the neighborhood of obstacles. Then re-allocate the time and re-parameterize the trajectory to time according to current waypoints' postion.
Although, we found in your code. The first-setp optimize the trajectory's smooth and obstacle.The second-step optimize obstacles, trajectory's smooth, velocity and acceleration, which conflict with your paper work. We guess it is why sometime the final trajectory would have collision with the obstacles.
Second, sometimes the demo random would terminate like the image show below. There is some bug maybe?
Ahhhhh, it's a very gooood question! I have the same Problem when I use this Package. Unfortunately, I have no idea either...
Sorry for the problem and thanks for reporting that. Shortly speaking, that's because some bugs and we have fixed them, the code would be updated soon tonight.
Actually, the functionality should be consistent with the paper, as we did for all onboard flight test and simulation shown in the paper. Originally all optimization code (the solver) is written by my hand and we planned to use off-the-shelf optimization solver (such as NLopt) to achieve better numerical performance, so we re-implemented the algorithm. As you can see, now the package is not in a plug-and-use stage, and we will keep updating and improving this package.
So, the reason your project code has some conflict with the paper is the re-implementation using NLopt, am I right? I wander why you changed the optimization process instead of just replacing the optimization solver. Would the changes of process get better effect?
Furthermore, did the phenomenon, the final trajectory having collision with obstacles, occur when you test on your machine?
Yes, that's right. And the most important reason is that this package is re-implemented by Boyu Zhou, not me, based on my code. Of course different people have different understanding even to one method. We have discussed about this yesterday and would fix it soon.
Actually we didn't change the entire optimization process and the problem occurred here is because the package is still in developing and there exist bugs. Using NLopt, the optimization procedure "at most time" runs faster and can get a lower objective cost. But sometimes it may diverge or converge very slowly, and we are still testing it.
For the last question, the "collision with obstacles" phenomenon should not occur. In our previous onboard flight tests for the IROS paper, it didn't occur; or the drone should have crashed.
Can all these above answer your question?
We are testing the package and will update the code as soon as possible (in 1 or 2 days, as you know, Chrismas is approaching ^_^ )
Thanks for your answer. Marry Christmas. ^__^
@VincentHFZ Thanks for your questions. This project is implemented by me, and I should clarify that the code does not conflict with the paper, but has some slight changes. The most important change is that only collision cost is optimized firstly, as mentioned in the paper, while in this project both collision cost and smoothness cost are optimized in the first step. Both the formulation of cost function and the optimization process are actually the same. I made this change since I found optimizing both collision and smoothness cost firstly results in a smoother trajectory. And I will test it further.
As for the collision with obstacles, it should never happen and I will fix this bug.
After updating, the bug that the program terminate is less. We only see it occur once. Just as it shows below:
Still have the situation that collision with obstacles, occur less though:
In your updates, you removed the first initial step for optimization, which make the success rate become lower maybe? (sometimes just optimize one, sometimes the try to optimize all failed)
none optimization:
optimize once only:
