Re: [ompl-users] OMPL-1.4.0 Validity check is not enforced with certain planners

[Zak K. <za...@ri...>]

Steven,

When working with the constrained planning framework in OMPL, your 
collision checker needs to access state values through the constrained 
state space, not the underlying state space.
For example, if you view the tutorial on constrained planning [1], 
you'll see that in the state validity checker accesses the underlying 
state values as follows:

     // The constrained state space state type is derived from 
Eigen::Map<Eigen::VectorXd>
     const Eigen::Map<Eigen::VectorXd> &x = 
*state->as<ob::ConstrainedStateSpace::StateType>();

rather than casting the state to a RealVectorStateSpace::StateType. You 
could also do something like the following:

double* values = state->as<ob::ConstrainedStateSpace::StateType>() // 
Get the constrained state space state
->getState()                                 // Access the state from 
the underlying state space
->as<ob::RealVectorStateSpace::StateType>()  // Cast appropriately
                           ->values;

I'd guess that this is the reason for why you are seeing junk data in 
state values.

Best,
Zak

[1] http://ompl.kavrakilab.org/constrainedPlanningTutorial.html


On 11/27/18 10:32 PM, Steven Morad wrote:
> Hello,
>
> It appears that my validity checker does not work with certain 
> planners, such as RRT*. I've tested this by creating a validity 
> checker that rules out my start state. Using RRT*, I still get a 
> solution, but using something like SBL I get an error along the lines 
> of "no valid start state".
>
> I use SimpleSetup like so:
>
>     auto constraint = std::make_shared<TetherConstraint>();
>     auto const_space =
>     std::make_shared<ob::ProjectedStateSpace>(space, constraint);
>     auto const_space_info =
>     std::make_shared<ob::ConstrainedSpaceInformation>(const_space);vv
>     auto state_space =
>     std::make_shared<ompl::geometric::SimpleSetup>(const_space_info);
>     state_space->setStateValidityChecker(check_state);
>     ...
>
>     state_space->setPlanner(std::make_shared<og::RRTStar>(const_space_info));
>
>     state_space->solve(10);
>
>
> Check state just measures distance to an obstacle:
>
>     bool check_state(const ob::State *state)
>     {
>         double* pos =
>     state->as<ob::RealVectorStateSpace::StateType>()->values;
>         Eigen::Vector2d bot0(pos[0], pos[1]);
>         Eigen::Vector2d bot1(pos[2], pos[3]);
>         Eigen::Vector2d bot2(pos[4], pos[5]);
>
>         // add obstacles
>         double clearance = 2;
>         std::vector<Eigen::Vector2d> obstacles = {
>             Eigen::Vector2d(5,5),
>             Eigen::Vector2d(5,6),
>             Eigen::Vector2d(4,6),
>             Eigen::Vector2d(2,2),
>         };
>
>         for (auto obstacle : obstacles) {
>             if ((bot0 - obstacle).norm() < clearance){
>                 return false;
>             }
>             if ((bot1 - obstacle).norm() < clearance){
>                 return false;
>             }
>             if ((bot2 - obstacle).norm() < clearance){
>                 return false;
>             }
>         }
>         return true;
>     }
>
>
> The planner appears to only pass states containing all zero (not 
> exact, but around 6.92615e-310) to the validity checker. The output 
> paths appear to go where I want them, but they ignore obstacles 
> defined in the validity check.
>
> Using LBKPIECE, there are nonzero states being passed to the check.
>
> Does anybody have any idea what might be causing this?
>
> Thanks,
> Steven
> -- 
> Steven Morad
> PhD Student
> Space and Terrestrial Robotic Exploration Laboratory
> University of Arizona
>
> 	
>
> www.dangersteve.com/home <http://www.dangersteve.com/home>
>
>
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users