collision.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_COLLISION_
#define SE2EZ_CORE_COLLISION_

#include <set>
#include <map>

#include <se2ez/core/class_forward.h>
#include <se2ez/core/math.h>

namespace se2ez
{
    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(State)
    /** \endcond */

    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(ACM)
    SE2EZ_CLASS_FORWARD(CollisionManager)
    /** \endcond */

    /** \class se2ez::ACMPtr
        \brief A shared pointer wrapper for se2ez::ACM. */

    /** \class se2ez::ACMConstPtr
        \brief A const shared pointer wrapper for se2ez::ACM. */

    /** \class se2ez::CollisionManagerPtr
        \brief A shared pointer wrapper for se2ez::CollisionManager. */

    /** \class se2ez::CollisionManagerConstPtr
        \brief A const shared pointer wrapper for se2ez::CollisionManager. */

    /** \brief An allowed collision map / matrix for the robot. Stores what frames are allowed to collide with
     * each other.
     */
    class ACM
    {
    public:
        /** \brief Disable collision checking between frame \e a and \e b.
         *  \param[in] a One frame.
         *  \param[in] b Other frame.
         */
        void disable(const std::string &a, const std::string &b);

        /** \brief Enable collision checking between frame \e a and \e b.
         *  \param[in] a One frame.
         *  \param[in] b Other frame.
         */
        void enable(const std::string &a, const std::string &b);

        /** \brief Returns true if frame \e a and frame \e b can collide.
         *  \param[in] a One frame.
         *  \param[in] b Other frame.
         *  \return True if \e a and \e b can collide, false if they do not.
         */
        bool collide(const std::string &a, const std::string &b) const;

        /** \brief Clears all entries within the ACM.
         */
        void clear();

    private:
        std::set<std::pair<std::string, std::string>> disabled_;  ///< Set of all disabled frame pairs
    };

    /** \brief An abstract class that all collision managers must implement.
     */
    class CollisionManager
    {
    public:
        /** \brief A class that contains all information about the signed distance of a frame to another.
         */
        struct SignedDistance
        {
            SE2EZ_EIGEN_CLASS

            double distance{math::inf};  ///< Distance to collision.
            Eigen::Vector2d point;       ///< Point on fixture where normal begins.
            Eigen::Vector2d normal;      ///< Normal vector to collision.
            std::string frame;           ///< Frame closet to collision.
            std::string collider;        ///< Frame this frame is closest to collision with.

            /** \brief Flips the direction of the normal and swaps the endpoint.
             */
            void flip();
        };

        /** \brief A map of frame names to their signed distances.
         */
        using SignedDistanceMap = tf::EigenMap<std::string, SignedDistance>;

        /** \brief Constructor.
         */
        CollisionManager();

        /** \brief Check if a state is in collision.
         *  \param[in] state State to check for collisions using an allowed collision map.
         *  \return Returns true if state is in collision, false otherwise.
         */
        virtual bool collide(StatePtr &state) = 0;

        /** \brief Check if a state is in collision.
         *  \param[in] state State to check for collisions using an allowed collision map.
         *  \param[out] frames What frames are in collision.
         *  \return Returns true if state is in collision, false otherwise.
         */
        virtual bool collide(StatePtr &state, std::set<std::string> &frames) = 0;

        /** \brief Computes the signed distance of a scene to collision.
         *  \param[in] state State to computed signed distance for.
         *  \return Returns signed distance.
         */
        virtual SignedDistance distance(StatePtr &state) = 0;

        /** \brief Computes the signed distance of a scene to collision.
         *  \param[in] state State to computed signed distance for.
         *  \param[out] frames Distance of frame to collision.
         *  \return Returns signed distance.
         */
        virtual SignedDistance distance(StatePtr &state, SignedDistanceMap &frames) = 0;

        /** \brief Compiles internal data structures for collision checking. Needs to be called if the robot
         * is updated.
         */
        virtual void compile() = 0;

        /** \brief Sets a new ACM to use.
         *  \param[in] acm New ACM to use.
         */
        virtual void setACM(const ACMPtr &acm);

        /** \brief Gets the current ACM.
         *  \return The ACM.
         */
        virtual ACMPtr getACM();

    protected:
        ACMPtr acm_{nullptr};  ///< ACM for collision manager.
    };
}  // namespace se2ez

#endif