VampStateSpace.h

#pragma once
 
#include <ompl/base/spaces/RealVectorStateSpace.h>
 
namespace ompl::vamp
{
    namespace ob = ompl::base;

    template <typename Robot>
    inline auto getRobotBounds() -> ob::RealVectorBounds
    {
        using Configuration = typename Robot::Configuration;
 
        std::array<float, Robot::dimension> zeros{};
        std::array<float, Robot::dimension> ones{};
        std::fill(ones.begin(), ones.end(), 1.0f);
 
        auto zero_v = Configuration(zeros);
        auto one_v = Configuration(ones);
 
        Robot::scale_configuration(zero_v);
        Robot::scale_configuration(one_v);
 
        auto zero_arr = zero_v.to_array();
        auto one_arr = one_v.to_array();
 
        ob::RealVectorBounds bounds(Robot::dimension);
        for (auto i = 0U; i < Robot::dimension; ++i)
        {
            bounds.setLow(i, zero_arr[i]);
            bounds.setHigh(i, one_arr[i]);
        }
 
        return bounds;
    }
 
    //==========================================================================
    // VAMP State Space for OMPL
    //==========================================================================
 
    template <typename Robot>
    class VampStateSpace : public ob::RealVectorStateSpace
    {
    public:
        VampStateSpace() : ob::RealVectorStateSpace(Robot::dimension)
        {
            this->setBounds(getRobotBounds<Robot>());
        }
    };
 
}  // namespace ompl::vamp