HySST.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2025, University of Santa Cruz Hybrid Systems Laboratory
 *  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 University of Santa Cruz 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.
 *********************************************************************/
 
/* Authors: Beverly Xu */
/* Adapted from: ompl/geometric/planners/src/SST.cpp by  Zakary Littlefield of Rutgers the State University of New
 * Jersey, New Brunswick */
 
#ifndef OMPL_CONTROL_PLANNERS_SST_HySST_
#define OMPL_CONTROL_PLANNERS_SST_HySST_
 
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/control/Control.h"
#include "ompl/control/spaces/RealVectorControlSpace.h"
#include "ompl/base/spaces/HybridStateSpace.h"
 
namespace ompl
{
    namespace control
    {
        class HySST : public base::Planner
        {
        public:
            HySST(const control::SpaceInformationPtr &si);

            ~HySST() override;

            void setup() override;

            class Motion
            {
            public:
                Motion() = default;

                Motion(const control::SpaceInformation *si) : state(si->allocState()), control(si->allocControl())
                {
                }

                virtual ~Motion() = default;

                virtual base::State *getState() const
                {
                    return state;
                }

                virtual Motion *getParent() const
                {
                    return parent;
                }

                base::Cost accCost_{0.};

                base::State *state{nullptr};

                Motion *parent{nullptr};

                unsigned numChildren_{0};

                bool inactive_{false};

                std::vector<base::State *> *solutionPair{nullptr};

                control::Control *control{nullptr};
            };

            base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;

            void getPlannerData(base::PlannerData &data) const override;

            void clear() override;

            void setSelectionRadius(double selectionRadius)
            {
                if (selectionRadius < 0)
                    throw Exception("Selection radius must be positive");
                selectionRadius_ = selectionRadius;
            }

            double getSelectionRadius() const
            {
                return selectionRadius_;
            }

            void setPruningRadius(double pruningRadius)
            {
                if (pruningRadius < 0)
                    throw ompl::Exception("Pruning radius must be non-negative");
                pruningRadius_ = pruningRadius;
            }

            double getPruningRadius() const
            {
                return pruningRadius_;
            }

            template <template <typename T> class NN>
            void setNearestNeighbors()
            {
                if (nn_ && nn_->size() != 0)
                    OMPL_WARN("Calling setNearestNeighbors will clear all states.");
                clear();
                nn_ = std::make_shared<NN<Motion *>>();
                witnesses_ = std::make_shared<NN<Motion *>>();
                setup();
            }

            void setTm(double tM)
            {
                if (tM <= 0)
                    throw Exception("Maximum flow time per propagation step must be greater than 0");
                if (!flowStepDuration_)
                {
                    if (tM < flowStepDuration_)
                        throw Exception(
                            "Maximum flow time per propagation step must be greater than or equal to the length of time for each flow "
                            "integration step (flowStepDuration_)");
                }
                tM_ = tM;
            }

            void setFlowStepDuration(double duration)
            {
                if (duration <= 0)
                    throw Exception("Flow step length must be greater than 0");
                if (!tM_)
                {
                    if (tM_ < duration)
                        throw Exception(
                            "Flow step length must be less than or equal to the maximum flow time per propagation step (Tm)");
                }
                flowStepDuration_ = duration;
            }

            void setJumpSet(std::function<bool(Motion *)> jumpSet)
            {
                jumpSet_ = jumpSet;
            }

            void setFlowSet(std::function<bool(Motion *)> flowSet)
            {
                flowSet_ = flowSet;
            }

            void setUnsafeSet(std::function<bool(Motion *)> unsafeSet)
            {
                unsafeSet_ = unsafeSet;
            }

            void setDistanceFunction(std::function<double(base::State *, base::State *)> function)
            {
                distanceFunc_ = function;
            }

            void setDiscreteSimulator(
                std::function<base::State *(base::State *curState, const control::Control *u, base::State *newState)>
                    function)
            {
                discreteSimulator_ = function;
            }

            void setContinuousSimulator(std::function<base::State *(const control::Control *u, base::State *curState,
                                                                    double tFlowMax, base::State *newState)>
                                            function)
            {
                continuousSimulator_ = function;
            }

            std::function<ompl::base::State *(const control::Control *control, ompl::base::State *x_cur, double tFlow,
                                              ompl::base::State *new_state)>
                continuousSimulator =
                    [this](const control::Control *control, base::State *x_cur, double tFlow, base::State *new_state)
            {
                siC_->getStatePropagator()->propagate(x_cur, control, tFlow, new_state);
                return new_state;
            };

            void setCollisionChecker(std::function<bool(Motion *motion, std::function<bool(Motion *motion)> obstacleSet,
                                                        base::State *newState, double *collisionTime)>
                                         function)
            {
                collisionChecker_ = function;
            }

            void setBatchSize(int batchSize)
            {
                if (batchSize < 1)
                    throw Exception("Batch size must be greater than 0");
                batchSize_ = batchSize;
            }

            void checkMandatoryParametersSet(void) const
            {
                if (!discreteSimulator_)
                    throw Exception("Jump map not set");
                if (!continuousSimulator_)
                    throw Exception("Flow map not set");
                if (!flowSet_)
                    throw Exception("Flow set not set");
                if (!jumpSet_)
                    throw Exception("Jump set not set");
                if (!unsafeSet_)
                    throw Exception("Unsafe set not set");
                if (tM_ < 0.0)
                    throw Exception("Max flow propagation time (Tm) not set");
                if (pruningRadius_ < 0)
                    throw Exception("Pruning radius (pruningRadius_) not set");
                if (selectionRadius_ < 0)
                    throw Exception("Selection radius (selectionRadius_) not set");
            }
 
        protected:
            const static ompl::control::Control *getFlowControl(const ompl::control::Control *control)
            {
                return control->as<CompoundControl>()->as<ompl::control::Control>(0);
            }
 
            const static ompl::control::Control *getJumpControl(const ompl::control::Control *control)
            {
                return control->as<CompoundControl>()->as<ompl::control::Control>(1);
            }

            class Witness : public Motion
            {
            public:
                Witness() = default;

                Witness(const control::SpaceInformation *si) : Motion(si)
                {
                }

                base::State *getState() const override
                {
                    return rep_->state;
                }

                Motion *getParent() const override
                {
                    return rep_->parent;
                }

                void linkRep(Motion *lRep)
                {
                    rep_ = lRep;
                }

                Motion *rep_{nullptr};
            };

            Motion *lastGoalMotion_{nullptr};

            void initTree(void);

            void randomSample(Motion *randomMotion);

            std::shared_ptr<NearestNeighbors<Motion *>> nn_;


            std::function<bool(Motion *motion, std::function<bool(Motion *motion)> obstacleSet, base::State *newState,
                               double *collisionTime)>
                collisionChecker_ = [this](Motion *motion, std::function<bool(Motion *motion)> obstacleSet,
                                           base::State *newState, double *collisionTime) -> bool
            {
                if (obstacleSet(motion))
                {
                    si_->copyState(newState, motion->solutionPair->back());
                    *collisionTime =
                        ompl::base::HybridStateSpace::getStateTime(motion->solutionPair->back()) - flowStepDuration_;
                    motion->solutionPair->resize(motion->solutionPair->size() - 1);
                    return true;
                }
                return false;
            };

            control::DirectedControlSamplerPtr controlSampler_;

            control::SpaceInformation *siC_;

            std::function<double(base::State *state1, base::State *state2)> distanceFunc_ =
                [this](base::State *state1, base::State *state2) -> double { return si_->distance(state1, state2); };

            double tM_{-1.};

            double minStepLength = 1e-06;

            double flowStepDuration_;

            std::function<base::State *(base::State *curState, const control::Control *u, base::State *newState)>
                discreteSimulator_;

            std::function<bool(Motion *motion)> jumpSet_;

            std::function<bool(Motion *motion)> flowSet_;

            std::function<bool(Motion *motion)> unsafeSet_;

            base::OptimizationObjectivePtr opt_;

            std::function<base::Cost(Motion *motion)> costFunc_;

            std::function<base::State *(const control::Control *u, base::State *curState, double tFlowMax,
                                        base::State *newState)>
                continuousSimulator_;

            base::PlannerStatus constructSolution(Motion *lastMotion);

            Motion *selectNode(Motion *sample);

            Witness *findClosestWitness(Motion *node);

            std::vector<Motion *> extend(Motion *m);

            void freeMemory();

            base::StateSamplerPtr sampler_;

            std::shared_ptr<NearestNeighbors<Motion *>> witnesses_;

            double selectionRadius_{-1.};

            double pruningRadius_{-1.};

            RNG rng_;

            double dist_;

            std::vector<Motion *> prevSolution_;

            base::Cost prevSolutionCost_{std::numeric_limits<double>::quiet_NaN()};

            int batchSize_{1};
        };
    }  // namespace control
}  // namespace ompl
 
#endif