Obstacle collision avoidance using g2o

[lucarossini-iit]

Hello Everybody,

Thanks for the fantastic work you do with this repo!

I am working on an obstacle avoidance algorithm on joint space based on g2o using edges that computes an error relatively to the distance of each link with every obstacle. Additionally, an edge used to follow a nominal trajectory is added to every vertex to recover the initial trajectory when no obstacle can interfere with it.
In order to speed up the overall process, I decided to fix those vertices whose edges have an error greater then a user-defined threshold in order to avoid useless optimization on feasible vertices. The optimization is run every loop of my ros node.

However, when I remove all the obstacles from the scene, the nominal trajectory is restored except for some vertices which are not optimized even in the next optimizations (~20/100 vertices are still active in the next optimizations and the initial trajectory for those vertices is not restored wvwn if I keep running the optimziation node for an infinite time).

My question is: is there a minimum number of active vertices required to effectively solve an optimization problem?

Please let me know if further information can be useful to describe better the problem.
Best,
Luca Rossini

[github-actions]

This issue is stale because it has been open 60 days with no activity. Remove stale label or comment or this will be closed in 14 days.