collision_checking.h

Go to the documentation of this file.

#pragma once
 
#include <string>
#include <vector>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/util/geometry.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
    struct SceneRepresentation
    {
        const VirtualRobot::SceneObjectSetPtr staticObjects;
        const std::vector<VirtualRobot::RobotPtr> articulatedObjects;
    };
 
    struct Robot3D
    {
        struct Params
        {
            std::string collisionModelName;
            Eigen::Vector3f scaleFactor = Eigen::Vector3f::Ones();
            std::set<std::string> primitiveApproximationIDs;
 
            std::string rootNode;
        };
 
        VirtualRobot::RobotPtr robot;
        std::vector<VirtualRobot::CollisionModelPtr> collisionModels;
 
        bool isInitialized() const;
        static Robot3D fromSimoxRobot(const VirtualRobot::RobotPtr& robot, Robot3D::Params params);
    };
 
    class DistanceCalculator
    {
    public:
        DistanceCalculator(SceneRepresentation& scene);
 
        bool isRobotInitialized() const;
        void setRobot(Robot3D& robot);
        void initializeRobot(const VirtualRobot::RobotPtr& robot, Robot3D::Params params);
 
        float distanceToObstacles(const core::Pose2D& pose2d) const;
 
    private:
        Robot3D robot;
        SceneRepresentation scene;
    };
 
    using CollisionPolygon = util::geometry::polygon_type;
 
    class Robot2D
    {
    private:
        // This robot_polygon is centered around the rotationPoint
        CollisionPolygon robot_polygon;
 
    public:
        Robot2D(VirtualRobot::RobotPtr& collisionRobot,
                const VirtualRobot::CollisionModelPtr& robotCollisionModel);
        Robot2D(VirtualRobot::RobotPtr& collisionRobot,
                const std::vector<VirtualRobot::CollisionModelPtr>& robotCollisionModels);
        Robot2D(const Robot2D&) = default;
        Robot2D() = delete;
        CollisionPolygon getOrientedRobot(float orientationRad) const;
        CollisionPolygon getRobotAtPose(const core::Pose2D& pose) const;
    };
 
    class Scene2D
    {
    private:
        std::vector<CollisionPolygon> obstacles;
 
    public:
        Scene2D(std::vector<VirtualRobot::CollisionModelPtr>& collisionModels,
                float ignoreVerticesOver);
        Scene2D(const SceneRepresentation& scene, float ignoreVerticesOver);
        float distance(const CollisionPolygon& robot) const;
        std::vector<CollisionPolygon>& getCollisionPolygons();
 
    private:
        void
        initializeFromCollisionModels(std::vector<VirtualRobot::CollisionModelPtr>& collisionModels,
                                      float ignoreVerticesOver);
    };
} // namespace armarx::navigation::algorithms::orientation_aware