Program Listing for File VampStateSpace.h

↰ Return to documentation for file (src/ompl/vamp/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