SE2MultiRigidBodyPlanning.h

/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2012, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Ryan Luna */

#include "omplapp/config.h"
#include "omplapp/apps/AppBase.h"
#include <ompl/base/spaces/SE2StateSpace.h>

namespace ompl
{
    namespace app
    {

        class SE2MultiRigidBodyPlanning : public AppBase<AppType::GEOMETRIC>
        {
        public:
            SE2MultiRigidBodyPlanning(unsigned int n);

            ~SE2MultiRigidBodyPlanning() override = default;

            base::ScopedState<> getDefaultStartState() const override;

            void inferEnvironmentBounds() override;

            void inferProblemDefinitionBounds() override;

            bool isSelfCollisionEnabled() const override
            {
                // Make sure that self collision is enabled to avoid inter-rigid body collision
                return true;
            }

            base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override
            {
                return state;
            }

            virtual const base::StateSpacePtr& getGeometricComponentStateSpace(unsigned int index) const
            {
                return getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(index);
            }

            const base::StateSpacePtr& getGeometricComponentStateSpace() const override
            {
                // Return the zeroth component.  All components are the same.
                return getGeometricComponentStateSpace(0);
            }

            unsigned int getRobotCount() const override
            {
                return n_;
            }

        protected:
            const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int index) const override;

            unsigned int n_;
        };

    }
}