[ompl-users] Path following while avoiding obstacles.

[Adrien B. <ab...@ro...>]

Hello,

I am trying to use OMPL to perform path following while avoiding obstacles.

I guessed that using TRRT algorithm with an optimization function which
prune solution far of the path I am following would do the job.

So I create the following optimization function :

class FollowLineObjective : public ob::StateCostIntegralObjective

{

public:

    FollowLineObjective(const ob::SpaceInformationPtr& si) :

        ob::StateCostIntegralObjective(si, true)

    {

    }

    ob::Cost stateCost(const ob::State* s) const

    {

        Eigen::VectorXd pt = vectorField(s);

        return ob::Cost(hypot(pt[0], pt[1]));

    }

};


Where vectorField return the vector from the state to the line I am following.


I give this optimization objective to my simple setup, and solutions are found.


The solution is shown here : https://ibb.co/cL5QYF


As we can see the obstacle (in black) is avoided, and the path I am
trying to follow (in red) is followed by the solutuion (in blue).


But the solution is not realy optimal because when there is no
obstacle, the solution is not a "straight line".


I tryied to "simplify" the solution. But when I simplify it, the
optimization seems to be ignored.


So how can I make the solution better "follow" my red line ?


Regards,


Adrien BARRAL.