Re: [ompl-users] StateCost: Penalize Z space

[abhilesh b. <abh...@gm...>]

Hello Mark,

Thank you for getting back to me. I tried out deriving a class from
optimizationObjective but it was forcing me to define motionCost
function(Pure virtual function) . So I derived a class from
StateCostIntegralObjective  which in turn is derived from
optimizationObjective. Here is my attempt but the planner still does not
work as intended.

class Penz :  public ob::StateCostIntegralObjective
{
public:
    Penz(const ob::SpaceInformationPtr& si) :
         ob::StateCostIntegralObjective(si , true)
    {
    }
    ob::Cost stateCost(const ob::State* s) const
    {
         const ob::RealVectorStateSpace::StateType* state3D =
            s->as<ob::RealVectorStateSpace::StateType>();
        double z = state3D->values[2];
        return ob::Cost(10000*z);
    }
};

/*
class Penz1 :  public ob::OptimizationObjective
{
public:
    Penz1(const ob::SpaceInformationPtr& si) :
          ob::OptimizationObjective(si )
    {
    }
    ob::Cost motionCost(const ob::State *s1, const ob::State *s2) const
override;  // causes undefined vtable for class Penz1 error

    ob::Cost stateCost(const ob::State* s) const
    {
         const ob::RealVectorStateSpace::StateType* state3D =
            s->as<ob::RealVectorStateSpace::StateType>();
        double z = state3D->values[2];
        return ob::Cost(10000*z);
    }
};*/


ob::OptimizationObjectivePtr getcost(const ob::SpaceInformationPtr& si)
{

    return std::make_shared<Penz>(si);

}

.........
pdef->setOptimizationObjective(getcost(si));

ob::PlannerPtr planner(new og::RRTConnect(si));

planner->setProblemDefinition(pdef);

Could you please point out where am I going wrong ?


Thanks & Regards

On Mon, Jan 28, 2019 at 9:20 PM Mark Moll <mm...@ri...> wrote:

> Hi Abhilesh,
>
> The call to stateCost in getcost does nothing; the return value is ignore.
> Please follow the directions in this tutorial:
> http://ompl.kavrakilab.org/optimizationObjectivesTutorial.html
>
> Basically, define a new class that derives from OptimizationObjective and
> implement its stateCost method s.t. it returns a cost that depends on the z
> component. Then, set the optimization objective to be a shared pointer to
> an instance of this class. (BTW, I don’t understand why you include x and y
> in your cost.)
>
> Best,
>
> Mark
>
>
> > On Jan 28, 2019, at 11:44 AM, abhilesh borode <abh...@gm...>
> wrote:
> >
> > Hello Everyone,
> >
> > I am working on a RRT planner in an octomap based environment. My aim is
> to perform the same task by penalizing the Z space which forces the RRT
> planner to generate trjectories more along x and y space in comparison to
> the z space. Here is my unsuccessful attempt in trying to do so :
> >
> > double penaliseZ(const ob::State* state)
> >     {
> >
> >         const ob::RealVectorStateSpace::StateType* state3D =
> >             state->as<ob::RealVectorStateSpace::StateType>();
> >
> >         double x = state3D->values[0];
> >         double y = state3D->values[1];
> >         double z = state3D->values[2];
> >
> >         return sqrt((x*x + y*y + 1000*z*z));
> >     }
> >
> > ob::Cost ob::OptimizationObjective::stateCost(const State* s)const
> > {
> >
> >     Cost c (penaliseZ(s));
> >     return c;
> > }
> >
> > ob::OptimizationObjectivePtr getcost(const ob::SpaceInformationPtr& si)
> > {
> >     ob::OptimizationObjectivePtr obj(new
> ob::PathLengthOptimizationObjective(si));
> >     const ob::State* s;
> >     obj->stateCost(s);
> >     return obj;
> > }
> > void plan(void)
> > {
> >     ob::StateSpacePtr space(new ob::SE3StateSpace());
> >
> >   . . . .
> >
> >     ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
> >
> >     si->setStateValidityChecker(std::bind(&isStateValid,
> std::placeholders::_1));
> >
> >     ob::ScopedState<ob::SE3StateSpace> start(space);
> >
> >     start->setXYZ(-5,-2,1);
> >     goal->setXYZ(6.5,7.5,1);
> >
> >     // create a problem instance
> >     ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
> >
> >     // set the start and goal states
> >     pdef->setStartAndGoalStates(start, goal);
> >
> >     pdef->setOptimizationObjective(getcost(si));
> >
> >     ob::PlannerPtr planner(new og::RRTConnect(si));
> >
> >     planner->setProblemDefinition(pdef);
> >
> >
> >     // perform setup steps for the planner
> >     planner->setup();
> > }
> >
> > The planner is still generating trajectories along the Z space.
> > Could anyone please tell me where am I going wrong ?
> >
> >
> > Thanks & Regards
> >
> >
> > _______________________________________________
> > ompl-users mailing list
> > omp...@li...
> > https://lists.sourceforge.net/lists/listinfo/ompl-users
>
>