ConditionalStateSampler.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, Technische Universität Berlin (TU Berlin)
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the TU Berlin nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Francesco Grothe */
 
#include "ompl/base/samplers/ConditionalStateSampler.h"
 
ompl::base::ConditionalStateSampler::ConditionalStateSampler(const ompl::base::SpaceInformation *si,
                                                             Motion *&startMotion, std::vector<Motion *> &goalMotions,
                                                             std::vector<Motion *> &newBatchGoalMotions,
                                                             bool &sampleOldBatch)
  : ValidStateSampler(si)
  , startMotion_(startMotion)
  , goalMotions_(goalMotions)
  , newBatchGoalMotions_(newBatchGoalMotions)
  , sampleOldBatch_(sampleOldBatch)
{
    name_ = "ConditionalSampler";
}
 
bool ompl::base::ConditionalStateSampler::sample(ompl::base::State *state)
{
    for (int i = 0; i < maxTries_; ++i)
    {
        internalSampler_->sampleUniform(state);
        double leftBound, rightBound;
        // get minimum time, when the state can be reached from the start
        double startBound =
            startMotion_->state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position +
            si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(state, startMotion_->state);
        // sample old batch
        if (sampleOldBatch_)
        {
            leftBound = startBound;
            // get maximum time, at which any goal can be reached from the state
            rightBound = std::numeric_limits<double>::min();
            for (auto goal : goalMotions_)
            {
                double t =
                    goal->state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position -
                    si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(goal->state, state);
                if (t > rightBound)
                {
                    rightBound = t;
                }
            }
        }
        // sample new batch
        else
        {
            // get maximum time, at which any goal from the new batch can be reached from the state
            rightBound = std::numeric_limits<double>::min();
            for (auto goal : newBatchGoalMotions_)
            {
                double t =
                    goal->state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position -
                    si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(goal->state, state);
                if (t > rightBound)
                {
                    rightBound = t;
                }
            }
            // get maximum time, at which any goal from the old batch can be reached from the state
            // only allow the left bound to be smaller than the right bound
            leftBound = std::numeric_limits<double>::min();
            for (auto goal : goalMotions_)
            {
                double t =
                    goal->state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position -
                    si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(goal->state, state);
                if (t > leftBound && t < rightBound)
                {
                    leftBound = t;
                }
            }
            leftBound = std::max(leftBound, startBound);
        }
 
        if (leftBound <= rightBound)
        {
            double time = rng_.uniformReal(leftBound, rightBound);
            state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position = time;
            return true;
        }
    }
    return false;
}
 
bool ompl::base::ConditionalStateSampler::sampleNear(ompl::base::State *, const ompl::base::State *, double)
{
    throw ompl::Exception("ConditionalSampler::sampleNear", "not implemented");
}