diff --git a/.DS_Store b/.DS_Store index d8c2fa63..81726f5d 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/gsoc/2022/posts/index.md b/gsoc/2022/posts/index.md index 30e5357b..0449503c 100644 --- a/gsoc/2022/posts/index.md +++ b/gsoc/2022/posts/index.md @@ -40,6 +40,11 @@ Project: Model Predictive Control for obstacle avoidance Mentors: Pablo Bustos, Pedro Núñez 1. [Introduction](/web/gsoc/2022/posts/kaustab_pal/1-introduction) +2. [Community bonding period](/web/gsoc/2022/posts/kaustab_pal/2-community_bonding_period) +3. [Phase 1](/web/gsoc/2022/posts/kaustab_pal/3-phase_1_coding_period) +4. [Phase 2](/web/gsoc/2022/posts/kaustab_pal/4-phase_2_coding_period) +5. [Conclusion](/web/gsoc/2022/posts/kaustab_pal/5-conclusion) + ## Swati Dantu diff --git a/gsoc/2022/posts/kaustab_pal/.DS_Store b/gsoc/2022/posts/kaustab_pal/.DS_Store deleted file mode 100644 index e88da98a..00000000 Binary files a/gsoc/2022/posts/kaustab_pal/.DS_Store and /dev/null differ diff --git a/gsoc/2022/posts/kaustab_pal/4-phase_2_coding_period.md b/gsoc/2022/posts/kaustab_pal/4-phase_2_coding_period.md new file mode 100644 index 00000000..071c40a2 --- /dev/null +++ b/gsoc/2022/posts/kaustab_pal/4-phase_2_coding_period.md @@ -0,0 +1,112 @@ +# GSoC'22 RoboComp project: Model Predictive Control for obstacle avoidance + +08 September 2022 + +## Phase 2 + +The phase 2 started from July 25th. The plans for phase 2 are: +1. Introduce a feature where if the target is given inside an obstacle, the + target will automatically be moved to the nearest unoccupied cell. +2. We discoreved a problem where the agent is unable to pass through narrow + corridors. This bug needs to be fixed. +3. Re-formulating the obstacle avoidance constraints as free-balls [ [Paper] ]( + https://arxiv.org/abs/1909.08267 ) and implement that as a constraint in the + MPC. + +## Contribution 1: Move target if it is in occupied cell + +During our testing in phase-1, I observed that if the target is given inside an +occupied cell, the MPC tries to move towards it as one of the cost function of +the MPC is to minimize the distance between the robot position and the goal +position. This often leads to the robot crashing as it reeached near the +obstacle. To fix this issue I first checked if the target and all it's +neighbouring cells are unoccupied. If even one of the neighbouring 16 cells of +the target is occupied, I consider the target to be inside a occupied cell and +move the target to the nearest free cell available. + +The following code snippet is taking care of this: + +```C++ +if(neighboors_16(target).size()<16){ + std::optional new_target = closest_free(target_); + target = pointToKey(new_target->x(), new_target->y()); + std::cout<<"OCCUPIED"<(v).free; }, fmap)) +{ + v.cost = 100; + v.tile->setBrush(occ_brush); + for (auto neighs = neighboors_8(k); auto &&[kk, vv]: neighs) + { + fmap.at(kk).cost = 100; + fmap.at(kk).tile->setBrush(occ_brush); + } +} +``` + +In the video [ [Passing through narrow corridors] ]( +https://youtu.be/1x6ngcrBRds ), you can see that the robot can now pass through +the narrow corridors with the euclidean distance obstacle avoidance constraints. +We can also observe that the obstacles are now represented in a much less +conservative way. +[ [Pull Rrequest #379] ]( https://github.com/robocomp/robocomp/pull/379 ) + +## Contribution 3: Reformulating the obstacle avoidance constraints as freeballs + +Suppose we have N obstacles and we are planning for T timesteps into the +future. Using the euclidean distance obstacle avoidance constraints, for each +timestep we will have a constraint for each of the N obstacles such that the +distance between the robot and the obstacle is greater than the sum of the +radius. Therefore for T timesteps, we will have N\*T constraints. This is a lot +of constraints and will slow down the computation time. To overcome this , we +use the freeballs algorithm as mentioned in the paper [ [An NMPC Approach using +Convex Inner Approximations for Online Motion Planning with Guaranteed Collision +Avoidance] ]( https://arxiv.org/abs/1909.08267 ) to avoid obstacles. In this +approach, we first take an initial guess for T centerpoints and and optimize for +them using a gradient based update rule such that the center points are furthest +away from the closest obstacle at that timestep. Now for each free ball we draw +a circle such that it's radius is the distance to the closest obstalce. If the +distance is above a certain threshold, we take a maximum circle radius defined +by the user. Once we have the center points and the radius, we add a constraint +to the MPC formualtion such that the robot always stays inside the circle. With +this approach, we only have T constraints and because of the decrease in the +number of constraints the computation time also decreases a lot. + +However this approach is very dependent on the value of the weight variables. +That is why to improve this, I added the feature that we will do a line search +for the weight variables and whichever weight will lead to a trajectory that is +furthest away from all the obstacles will be selected. In my experimentation, I +observed that we can only search for 3 weight variables after which the +computation time increases too much to operate safely in the environment. + +In the video [ [Free Balls obstacle avoidance with automatic weights tuning] ]( +https://youtu.be/BvM0eflDXGI ), the robot uses the freeballs algorith, with the +automatic weight search to navigate in the environment with narrow corridors. +[ [Pull Request #9] ]( https://github.com/robocomp/optimizer/pull/9 ) + diff --git a/gsoc/2022/posts/kaustab_pal/5-conclusion.md b/gsoc/2022/posts/kaustab_pal/5-conclusion.md new file mode 100644 index 00000000..39316536 --- /dev/null +++ b/gsoc/2022/posts/kaustab_pal/5-conclusion.md @@ -0,0 +1,21 @@ +# GSoC'22 RoboComp project: Model Predictive Control for obstacle avoidance + +08 September 2022 + +## Detailed report + +While the above contributions explain my GSoC'22 work, you can find a more +detailed explanation along with mathematical explanations here: [ [GSoC'22 +Project Report] ]( https://kaustabpal.github.io/gsoc_22_report ) + +## Future scope + +While the free balls algorithm works really well in complex environment, there +are much to be explored on how we can speed up the computation time to compute +the trajectories more efficiently. One approach would be to comne up with better +ways to initialize the optimizer. Another approach can be to make the trajectory +optimization formulation differentiable. Having a differentiable formulation +will enable us to integrate the trajectory computation with the perception +system in an end to end manner. + + diff --git a/gsoc/2022/posts/kaustab_pal/assets/narrow_corridor.png b/gsoc/2022/posts/kaustab_pal/assets/narrow_corridor.png new file mode 100644 index 00000000..2c897c7e Binary files /dev/null and b/gsoc/2022/posts/kaustab_pal/assets/narrow_corridor.png differ