BundleSpace.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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: Andreas Orthey */
 
#include <ompl/multilevel/datastructures/BundleSpace.h>
#include <ompl/multilevel/datastructures/Projection.h>
 
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/control/SpaceInformation.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/base/spaces/SO3StateSpace.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/TimeStateSpace.h>
#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
 
#include <ompl/util/Exception.h>
#include <cmath>  //to use isnan(d)
 
using namespace ompl::base;
using namespace ompl::multilevel;
 
unsigned int BundleSpace::counter_ = 0;
 
BundleSpace::BundleSpace(const SpaceInformationPtr &si, BundleSpace *child)
  : Planner(si, "BundleSpace"), childBundleSpace_(child), totalSpace_(si)
{
    id_ = counter_++;
    if (child)
    {
        baseSpace_ = childBundleSpace_->getBundle();
        childBundleSpace_->setParent(this);
        xBaseTmp_ = getBase()->allocState();
    }
 
    std::stringstream ss;
    ss << (*this);
    OMPL_DEBUG(ss.str().c_str());
 
    if (!Bundle_valid_sampler_)
    {
        Bundle_valid_sampler_ = getBundle()->allocValidStateSampler();
    }
    if (!Bundle_sampler_)
    {
        Bundle_sampler_ = getBundle()->allocStateSampler();
    }
    xBundleTmp_ = getBundle()->allocState();
}
 
BundleSpace::~BundleSpace()
{
    if (hasBaseSpace())
    {
        if (xBaseTmp_)
        {
            getBase()->freeState(xBaseTmp_);
        }
    }
    if (xBundleTmp_)
    {
        getBundle()->freeState(xBundleTmp_);
    }
}
 
bool BundleSpace::makeProjection()
{
    ProjectionFactory projectionFactory;
 
    projection_ = projectionFactory.makeProjection(getBundle(), getBase());
 
    if (!projection_)
        return false;
 
    sanityChecks();
    return true;
}
 
bool BundleSpace::hasBaseSpace() const
{
    return !(baseSpace_ == nullptr);
}
 
bool BundleSpace::findSection()
{
    return false;
}
 
bool BundleSpace::hasParent() const
{
    return !(parentBundleSpace_ == nullptr);
}
 
bool BundleSpace::isDynamic() const
{
    return isDynamic_;
}
 
void BundleSpace::setup()
{
    BaseT::setup();
 
    hasSolution_ = false;
    firstRun_ = true;
 
    if (pdef_)
    {
        if (!pdef_->hasOptimizationObjective())
        {
            OptimizationObjectivePtr lengthObj = std::make_shared<base::PathLengthOptimizationObjective>(getBundle());
 
            lengthObj->setCostThreshold(base::Cost(std::numeric_limits<double>::infinity()));
            pdef_->setOptimizationObjective(lengthObj);
        }
    }
    else
    {
        OMPL_ERROR("Called without ProblemDefinitionPtr");
        throw "NoProblemDef";
    }
}
 
GoalSampleableRegion *BundleSpace::getGoalPtr() const
{
    base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
    return goal;
}
 
void BundleSpace::clear()
{
    BaseT::clear();
 
    hasSolution_ = false;
    firstRun_ = true;
 
    pdef_->clearSolutionPaths();
}
 
void BundleSpace::sanityChecks() const
{
    const StateSpacePtr Bundle_space = getBundle()->getStateSpace();
    checkBundleSpaceMeasure("Bundle", Bundle_space);
 
    if (hasBaseSpace())
    {
        const StateSpacePtr Base_space = getBase()->getStateSpace();
        checkBundleSpaceMeasure("Base", Base_space);
        if (getProjection()->getDimension() != getBundleDimension())
        {
            throw Exception("BundleSpace Dimensions are wrong.");
        }
    }
}
 
void BundleSpace::checkBundleSpaceMeasure(std::string name, const StateSpacePtr space) const
{
    OMPL_DEVMSG1("%s dimension: %d measure: %f", name.c_str(), space->getDimension(), space->getMeasure());
 
    if ((space->getMeasure() >= std::numeric_limits<double>::infinity()))
    {
        throw Exception("Space infinite measure.");
    }
}
 
PlannerStatus BundleSpace::solve(const PlannerTerminationCondition &)
{
    throw Exception("A Bundle-Space cannot be solved alone. \
        Use class BundleSpaceSequence to solve Bundle-Spaces.");
}
 
void BundleSpace::setProblemDefinition(const ProblemDefinitionPtr &pdef)
{
    BaseT::setProblemDefinition(pdef);
}
 
void BundleSpace::resetCounter()
{
    BundleSpace::counter_ = 0;
}
 
void BundleSpace::setProjection(ProjectionPtr projection)
{
    projection_ = projection;
    if (getProjection() == nullptr)
    {
        OMPL_ERROR("Projection is nullptr.");
        throw "Projection is nullptr.";
    }
}
 
ProjectionPtr BundleSpace::getProjection() const
{
    return projection_;
}
 
void BundleSpace::allocIdentityState(State *s, StateSpacePtr space) const
{
    if (space->isCompound())
    {
        CompoundStateSpace *cspace = space->as<CompoundStateSpace>();
        const std::vector<StateSpacePtr> compounds = cspace->getSubspaces();
        for (unsigned int k = 0; k < compounds.size(); k++)
        {
            StateSpacePtr spacek = compounds.at(k);
            State *xk = s->as<CompoundState>()->as<State>(k);
            allocIdentityState(xk, spacek);
        }
    }
    else
    {
        int stype = space->getType();
        switch (stype)
        {
            case STATE_SPACE_SO3:
            {
                static_cast<SO3StateSpace::StateType *>(s)->setIdentity();
                break;
            }
            case STATE_SPACE_SO2:
            {
                static_cast<SO2StateSpace::StateType *>(s)->setIdentity();
                break;
            }
            case STATE_SPACE_TIME:
            {
                static_cast<TimeStateSpace::StateType *>(s)->position = 0;
                break;
            }
            case STATE_SPACE_DISCRETE:
            {
                DiscreteStateSpace *space_Discrete = space->as<DiscreteStateSpace>();
                int lb = space_Discrete->getLowerBound();
                static_cast<DiscreteStateSpace::StateType *>(s)->value = lb;
                break;
            }
            case STATE_SPACE_REAL_VECTOR:
            {
                RealVectorStateSpace::StateType *sRN = s->as<RealVectorStateSpace::StateType>();
                RealVectorStateSpace *RN = space->as<RealVectorStateSpace>();
                const std::vector<double> &bl = RN->getBounds().low;
                const std::vector<double> &bh = RN->getBounds().high;
                for (unsigned int k = 0; k < space->getDimension(); k++)
                {
                    double &v = sRN->values[k];
                    v = 0.0;
 
                    // if zero is not valid, use mid point as identity
                    if (v < bl.at(k) || v > bh.at(k))
                    {
                        v = bl.at(k) + 0.5 * (bh.at(k) - bl.at(k));
                    }
                }
                break;
            }
            default:
            {
                OMPL_ERROR("Type: %d not recognized.", stype);
                throw Exception("Type not recognized.");
            }
        }
    }
}
 
State *BundleSpace::allocIdentityState(StateSpacePtr space) const
{
    if (space != nullptr)
    {
        State *s = space->allocState();
        allocIdentityState(s, space);
        return s;
    }
    else
    {
        return nullptr;
    }
}
 
State *BundleSpace::allocIdentityStateBundle() const
{
    return allocIdentityState(getBundle()->getStateSpace());
}
 
State *BundleSpace::allocIdentityStateBase() const
{
    return allocIdentityState(getBase()->getStateSpace());
}
 
const SpaceInformationPtr &BundleSpace::getBundle() const
{
    return totalSpace_;
}
 
const SpaceInformationPtr &BundleSpace::getBase() const
{
    return baseSpace_;
}
 
unsigned int BundleSpace::getBaseDimension() const
{
    if (getBase())
        return getBase()->getStateDimension();
    else
        return 0;
}
 
unsigned int BundleSpace::getBundleDimension() const
{
    return getBundle()->getStateDimension();
}
 
unsigned int BundleSpace::getCoDimension() const
{
    return getBundleDimension() - getBaseDimension();
}
 
const StateSamplerPtr &BundleSpace::getBaseSamplerPtr() const
{
    if (hasBaseSpace())
    {
        return getChild()->getBundleSamplerPtr();
    }
    else
    {
        OMPL_ERROR("Cannot get Base Sampler without Base Space.");
        throw Exception("Tried Calling Non-existing base space sampler");
    }
}
 
const StateSamplerPtr &BundleSpace::getBundleSamplerPtr() const
{
    return Bundle_sampler_;
}
 
bool BundleSpace::isInfeasible()
{
    return false;
}
 
bool BundleSpace::hasConverged()
{
    return false;
}
 
bool BundleSpace::hasSolution()
{
    if (!hasSolution_)
    {
        PathPtr path;
        hasSolution_ = getSolution(path);
    }
    return hasSolution_;
}
 
BundleSpace *BundleSpace::getChild() const
{
    return childBundleSpace_;
}
 
void BundleSpace::setChild(BundleSpace *child)
{
    childBundleSpace_ = child;
}
 
BundleSpace *BundleSpace::getParent() const
{
    return parentBundleSpace_;
}
 
void BundleSpace::setParent(BundleSpace *parent)
{
    parentBundleSpace_ = parent;
}
 
unsigned int BundleSpace::getLevel() const
{
    return level_;
}
 
void BundleSpace::setLevel(unsigned int level)
{
    level_ = level;
}
 
OptimizationObjectivePtr BundleSpace::getOptimizationObjectivePtr() const
{
    return pdef_->getOptimizationObjective();
}
 
bool BundleSpace::sampleBundleValid(State *xRandom)
{
    bool found = false;
 
    unsigned int attempts = 0;
    do
    {
        sampleBundle(xRandom);
        found = getBundle()->getStateValidityChecker()->isValid(xRandom);
        attempts++;
    } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
    return found;
}
 
void BundleSpace::sampleBundle(State *xRandom)
{
    if (!hasBaseSpace())
    {
        Bundle_sampler_->sampleUniform(xRandom);
    }
    else
    {
        if (getProjection()->getCoDimension() > 0)
        {
            // Adjusted sampling function: Sampling in G0 x Fiber
            getChild()->sampleFromDatastructure(xBaseTmp_);
            getProjection()->lift(xBaseTmp_, xRandom);
        }
        else
        {
            getChild()->sampleFromDatastructure(xRandom);
        }
    }
}
 
void BundleSpace::lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const
{
    projection_->lift(xBase, xBundle);
}
 
void BundleSpace::project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
{
    projection_->project(xBundle, xBase);
}
 
void BundleSpace::print(std::ostream &out) const
{
    out << getProjection();
}
 
namespace ompl
{
    namespace multilevel
    {
        std::ostream &operator<<(std::ostream &out, const BundleSpace &bundleSpace)
        {
            bundleSpace.print(out);
            return out;
        }
    }
}