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_;
        };
 
    }
}