TangentBundleStateSpace.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Rice University
*  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 Rice University 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: Zachary Kingston */

#include "ompl/base/spaces/constraint/TangentBundleStateSpace.h"
#include "ompl/base/spaces/constraint/AtlasChart.h"

#include "ompl/util/Exception.h"

#include <Eigen/Core>

#include <cmath>

ompl::base::TangentBundleStateSpace::TangentBundleStateSpace(const StateSpacePtr &ambientSpace,
                                                             const ConstraintPtr &constraint)
  : AtlasStateSpace(ambientSpace, constraint, false)
{
    setName("TangentBundle" + space_->getName());
    setBiasFunction([&](AtlasChart *c) -> double {
        double d = 0;
        for (auto anchor : anchors_)
            d = std::max(d, distance(anchor, c->getOrigin()));

        return d;
    });
}

void ompl::base::TangentBundleStateSpace::sanityChecks() const
{
    constrainedSanityChecks(CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY | CONSTRAINED_STATESPACE_SAMPLERS);

    double zero = std::numeric_limits<double>::epsilon();
    double eps = std::numeric_limits<double>::epsilon();
    unsigned int flags = STATESPACE_DISTANCE_DIFFERENT_STATES | STATESPACE_DISTANCE_SYMMETRIC |
                         STATESPACE_DISTANCE_BOUND | STATESPACE_RESPECT_BOUNDS | STATESPACE_ENFORCE_BOUNDS_NO_OP;

    StateSpace::sanityChecks(zero, eps, flags);
}

bool ompl::base::TangentBundleStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
                                                           std::vector<State *> *geodesic) const
{
    // We can't traverse the manifold if we don't start on it.
    if (!constraint_->isSatisfied(from))
        return false;

    auto afrom = from->as<StateType>();
    auto ato = to->as<StateType>();

    // Try to get starting chart from `from` state.
    AtlasChart *c = getChart(afrom);
    if (c == nullptr)
        return false;

    // Save a copy of the from state.
    if (geodesic != nullptr)
    {
        geodesic->clear();
        geodesic->push_back(cloneState(from));
    }

    auto &&svc = si_->getStateValidityChecker();

    // No need to traverse the manifold if we are already there
    const double tolerance = delta_;
    const double distTo = distance(from, to);
    if (distTo <= tolerance)
        return true;

    // Traversal stops if the ball of radius distMax centered at from is left
    const double distMax = lambda_ * distTo;

    // Create a scratch state to use for movement.
    auto scratch = cloneState(from)->as<StateType>();
    auto temp = cloneState(from)->as<StateType>();

    // Project from and to points onto the chart
    Eigen::VectorXd u_j(k_), u_b(k_);
    c->psiInverse(*scratch, u_j);
    c->psiInverse(*ato, u_b);

    bool done = false;
    std::size_t chartsCreated = 0;
    double dist = 0;

    const double sqDelta = delta_ * delta_;
    do
    {
        // Take a step towards the final state
        u_j += delta_ * (u_b - u_j).normalized();
        c->phi(u_j, *temp);

        const double step = distance(temp, scratch);

        if (step < std::numeric_limits<double>::epsilon())
            break;

        dist += step;

        if (!(interpolate || svc->isValid(scratch))                    // valid
            || distance(temp, from) > distMax || !std::isfinite(dist)  // exceed max dist
            || dist > distMax                                          // exceed wandering
            || chartsCreated > maxChartsPerExtension_)                 // exceed chart limit
            break;

        done = (u_b - u_j).squaredNorm() <= sqDelta;
        // Find or make a new chart if new state is off of current chart
        if (done || !c->inPolytope(u_j)                  // outside polytope
            || constraint_->distance(*temp) > epsilon_)  // to far from manifold
        {
            const bool onManifold = c->psi(u_j, *temp);
            if (!onManifold)
                break;

            copyState(scratch, temp);
            scratch->setChart(c);

            bool created = false;
            if ((c = getChart(scratch, true, &created)) == nullptr)
            {
                OMPL_ERROR("Treating singularity as an obstacle.");
                break;
            }
            chartsCreated += created;

            // Re-project onto the next chart.
            c->psiInverse(*scratch, u_j);
            c->psiInverse(*ato, u_b);

            done = (u_b - u_j).squaredNorm() <= sqDelta;
        }

        copyState(scratch, temp);

        // Keep the state in a list, if requested.
        if (geodesic != nullptr)
            geodesic->push_back(cloneState(scratch));

    } while (!done);

    const bool ret = distance(to, scratch) <= delta_;
    freeState(scratch);
    freeState(temp);

    return ret;
}

ompl::base::State *ompl::base::TangentBundleStateSpace::geodesicInterpolate(const std::vector<State *> &geodesic,
                                                                            const double t) const
{
    auto state = ConstrainedStateSpace::geodesicInterpolate(geodesic, t)->as<StateType>();
    if (!project(state))
        return geodesic[0];

    return state;
}

bool ompl::base::TangentBundleStateSpace::project(State *state) const
{
    auto astate = state->as<StateType>();
    auto &&svc = si_->getStateValidityChecker();

    Eigen::VectorXd u(k_);
    AtlasChart *chart = getChart(astate, true);
    chart->psiInverse(*astate, u);

    if (chart->psi(u, *astate)   // On manifold
        && svc->isValid(state))  // Valid state
        return true;

    return false;
}