collision_checking.cpp

Go to the documentation of this file.

#include "collision_checking.h"
 
#include <algorithm>
#include <cstddef>
#include <limits>
#include <string>
#include <vector>
 
#include <boost/geometry/algorithms/buffer.hpp>
#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/simplify.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/io/wkt/write.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/pending/disjoint_sets.hpp>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/util/geometry.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/all.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::algorithms::orientation_aware
{
    bool
    Robot3D::isInitialized() const
    {
        ARMARX_TRACE;
        return robot != nullptr && not collisionModels.empty();
    }
 
    Robot3D
    Robot3D::fromSimoxRobot(const VirtualRobot::RobotPtr& robot, Robot3D::Params params)
    {
        Robot3D distanceRobot;
        {
            ARMARX_DEBUG << "Copying robot";
            ARMARX_CHECK_NOT_NULL(robot);
            distanceRobot.robot =
                robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
 
 
            // replace the standard collision model by our custom one
            if (not params.primitiveApproximationIDs.empty())
            {
                ARMARX_INFO << "Setting primitive approximation IDs: "
                            << std::vector(params.primitiveApproximationIDs.begin(),
                                           params.primitiveApproximationIDs.end());
                distanceRobot.robot->setPrimitiveApproximationModel(
                    std::vector(params.primitiveApproximationIDs.begin(),
                                params.primitiveApproximationIDs.end()),
                    false);
            }
 
            distanceRobot.robot->setUpdateVisualization(true); // FIXME false
            ARMARX_DEBUG << "Copying done";
        }
 
        ARMARX_CHECK_NOT_NULL(distanceRobot.robot);
 
        ARMARX_CHECK(distanceRobot.robot);
 
        if (not params.collisionModelName.empty())
        {
            ARMARX_INFO << "Using collision model " << QUOTED(params.collisionModelName);
            ARMARX_CHECK(distanceRobot.robot->hasRobotNode(params.collisionModelName));
 
            const auto collisionRobotNode =
                distanceRobot.robot->getRobotNode(params.collisionModelName);
            ARMARX_CHECK_NOT_NULL(collisionRobotNode);
 
            distanceRobot.collisionModels = {collisionRobotNode->getCollisionModel()};
            ARMARX_CHECK(distanceRobot.collisionModels.front())
                << params.collisionModelName << " does not provide a collision model!";
        }
        else
        {
            ARMARX_INFO << "Using reduced robot (merging all collision models) ...";
 
            std::vector<std::string> otherNodeNames{distanceRobot.robot->getRootNode()->getName()};
            if (not params.rootNode.empty())
            {
                otherNodeNames.push_back(params.rootNode);
            }
 
            // create reduced robot
            // distanceRobot.robot = VirtualRobot::RobotFactory::createReducedModel(
            //     *distanceRobot.robot, {}, otherNodeNames);
 
            // ARMARX_CHECK_EQUAL(distanceRobot.robot->getCollisionModels().size(), 1);
            ARMARX_IMPORTANT << distanceRobot.robot->getCollisionModels().size();
 
            // distanceRobot.collisionModel = distanceRobot.robot->getCollisionModels().front();
            distanceRobot.collisionModels = distanceRobot.robot->getCollisionModels();
        }
 
        ARMARX_CHECK(distanceRobot.collisionModels.empty() == false)
            << "Collision model not available. "
               "Make sure that you load the robot correctly!";
 
 
        // scale collision model
        for (auto& colModel : distanceRobot.robot->getCollisionModels())
        {
            colModel->scale(params.scaleFactor);
        }
 
        return distanceRobot;
    }
 
    DistanceCalculator::DistanceCalculator(SceneRepresentation& scene) : scene(scene)
    {
    }
 
    bool
    DistanceCalculator::isRobotInitialized() const
    {
        ARMARX_TRACE;
        return robot.robot != nullptr && not robot.collisionModels.empty();
    }
 
    void
    DistanceCalculator::setRobot(Robot3D& robot)
    {
        ARMARX_TRACE;
        this->robot = robot;
    }
 
    void
    DistanceCalculator::initializeRobot(const VirtualRobot::RobotPtr& robot, Robot3D::Params params)
    {
        this->robot = Robot3D::fromSimoxRobot(robot, params);
    }
 
    float
    DistanceCalculator::distanceToObstacles(const core::Pose2D& pose2d) const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot.robot);
        ARMARX_CHECK_NOT_EMPTY(robot.collisionModels);
        ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
        ARMARX_CHECK_NOT_NULL(scene.staticObjects);
 
        const core::Pose globalPose = conv::to3D(pose2d);
        robot.robot->setGlobalPose(globalPose.matrix());
 
        ARMARX_CHECK_EQUAL(robot.collisionModels.size(), 1);
 
        // distance to non-articulated objects
        float distanceNonArticulated = std::numeric_limits<float>::max();
        if (scene.staticObjects->getSize() > 0)
        {
            distanceNonArticulated =
                VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                    robot.collisionModels.front(),
                    scene.staticObjects);
        }
 
        // distance to articulated objects
        float distanceArticulatedMin = std::numeric_limits<float>::max();
        for (const auto& articulatedObject : scene.articulatedObjects)
        {
            for (const auto& colModel : articulatedObject->getCollisionModels())
            {
                const float distanceArticulated =
                    VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                        robot.collisionModels.front(),
                        colModel);
                distanceArticulatedMin = std::min(distanceArticulated, distanceArticulatedMin);
            }
        }
 
        return std::min(distanceNonArticulated, distanceArticulatedMin);
    }
 
    namespace
    {
 
        void
        collPolyFromVertices(const std::vector<Eigen::Vector3f>& vertices,
                             util::geometry::polygon_type& out)
        {
            const auto verticesConverted =
                vertices |
                ranges::views::transform([](const Eigen::Vector3f& vec)
                                         { return util::geometry::point_type{vec.x(), vec.y()}; }) |
                ranges::to_vector;
            boost::geometry::model::multi_point<util::geometry::point_type> mp(
                verticesConverted.begin(),
                verticesConverted.end());
            boost::geometry::convex_hull(mp, out);
        }
 
        template <typename Polygon>
        boost::geometry::model::multi_polygon<Polygon>
        buffer_polygon_simple(const Polygon& poly, double distance)
        {
            namespace bg = boost::geometry;
            using Multi = bg::model::multi_polygon<Polygon>;
 
            using namespace bg::strategy::buffer;
            distance_symmetric<double> distance_strategy(distance);
            side_straight side_strategy;
            join_miter join_strategy;
            end_flat end_strategy;
            point_circle circle_strategy(8);
 
            Multi result;
            bg::buffer(poly,
                       result,
                       distance_strategy,
                       side_strategy,
                       join_strategy,
                       end_strategy,
                       circle_strategy);
            return result;
        }
 
        std::vector<util::geometry::polygon_type>
        collPolyFrom3D(const std::vector<Eigen::Vector3f>& vertices,
                       const std::vector<VirtualRobot::MathTools::TriangleFace>& faces)
        {
            /*for (const auto& f : faces)
        {
            ARMARX_DEBUG << "Face: " << f.id1 << ", " << f.id2 << ", " << f.id3;
            ARMARX_DEBUG << "  v1: " << vertices[f.id1].transpose();
            ARMARX_DEBUG << "  v2: " << vertices[f.id2].transpose();
            ARMARX_DEBUG << "  v3: " << vertices[f.id3].transpose();
        }*/
            using point2d = boost::geometry::model::d2::point_xy<double>;
            using polygon2d = boost::geometry::model::polygon<point2d>;
            using multipolygon2d = boost::geometry::model::multi_polygon<polygon2d>;
 
            multipolygon2d projected;
            for (const auto& f : faces)
            {
                polygon2d p;
                boost::geometry::append(p, point2d(vertices[f.id1].x(), vertices[f.id1].y()));
                boost::geometry::append(p, point2d(vertices[f.id2].x(), vertices[f.id2].y()));
                boost::geometry::append(p, point2d(vertices[f.id3].x(), vertices[f.id3].y()));
                boost::geometry::append(p, point2d(vertices[f.id1].x(), vertices[f.id1].y()));
                projected.push_back(p);
            }
 
            // --- Merge them ---
            multipolygon2d unified = {projected.front()}; // start with first
 
            for (size_t i = 1; i < projected.size(); ++i)
            {
                multipolygon2d tmp;
                polygon2d next = projected[i];
                boost::geometry::correct(next);
                //multipolygon2d next_buffered;
                //boost::geometry::buffer(next, next_buffered, 0.01);
                multipolygon2d next_buffered = buffer_polygon_simple(next, 0.01);
                if (not boost::geometry::is_valid(next_buffered))
                {
                    ARMARX_WARNING << "Buffered polygon is not valid: "
                                   << boost::geometry::wkt(next);
                    continue;
                }
                boost::geometry::union_(unified, next_buffered, tmp);
                unified = std::move(tmp);
            }
 
            /*multipolygon2d unified;
        boost::geometry::union_(projected, projected, unified);*/
            //boost::geometry::simplify(unified, unified, 10);
 
            std::vector<util::geometry::polygon_type> out;
            for (const auto& p : unified)
            {
                util::geometry::polygon_type poly;
                for (const auto& point : p.outer())
                {
                    boost::geometry::append(
                        poly,
                        util::geometry::point_type{static_cast<float>(point.x()),
                                                   static_cast<float>(point.y())});
                }
                out.push_back(poly);
                ARMARX_INFO << "  Polygon: " << boost::geometry::wkt(poly);
            }
            return out;
        }
 
    } // namespace
 
    Robot2D::Robot2D(VirtualRobot::RobotPtr& collisionRobot,
                     const VirtualRobot::CollisionModelPtr& robotCollisionModel)
    {
        // Reset robot to standard pose -> so that we can simply rotate the polygon
        const core::Pose globalPose = conv::to3D(core::Pose2D::Identity());
        collisionRobot->setUpdateCollisionModel(true);
        collisionRobot->setUpdateVisualization(true);
 
        collisionRobot->setGlobalPose(globalPose.matrix());
 
        // create robot polygon
        // vertices are relative to the collision node
        const std::vector<Eigen::Vector3f> vertices = robotCollisionModel->getModelVeticesGlobal();
 
        collPolyFromVertices(vertices, robot_polygon);
        ARMARX_INFO << "Robot: " << boost::geometry::wkt(robot_polygon);
    }
 
    Robot2D::Robot2D(VirtualRobot::RobotPtr& collisionRobot,
                     const std::vector<VirtualRobot::CollisionModelPtr>& robotCollisionModels)
    {
        // Reset robot to standard pose -> so that we can simply rotate the polygon
        const core::Pose globalPose = conv::to3D(core::Pose2D::Identity());
        collisionRobot->setUpdateCollisionModel(true);
        collisionRobot->setUpdateVisualization(true);
 
        collisionRobot->setGlobalPose(globalPose.matrix());
 
        // create robot polygon
        // vertices are relative to the collision node
        std::vector<Eigen::Vector3f> vertices;
        for (const auto& robotCollisionModel : robotCollisionModels)
        {
            const auto v = robotCollisionModel->getModelVeticesGlobal();
            vertices.insert(vertices.end(), v.begin(), v.end());
        }
 
        collPolyFromVertices(vertices, robot_polygon);
        ARMARX_INFO << "Robot: " << boost::geometry::wkt(robot_polygon);
    }
 
    util::geometry::polygon_type
    Robot2D::getOrientedRobot(float orientationRad) const
    {
        boost::geometry::strategy::transform::
            rotate_transformer<boost::geometry::radian, float, 2, 2>
                transform(orientationRad);
        util::geometry::polygon_type out;
        boost::geometry::transform(robot_polygon, out, transform);
        return out;
    }
 
    util::geometry::polygon_type
    Robot2D::getRobotAtPose(const core::Pose2D& pose) const
    {
        boost::geometry::strategy::transform::translate_transformer<float, 2, 2> translate(
            pose.translation().x(),
            pose.translation().y());
        const auto angle =
            -Eigen::Rotation2Df{pose.linear()}
                 .angle(); // negative because boost::geometry uses counter-clockwise rotation
        boost::geometry::strategy::transform::
            rotate_transformer<boost::geometry::radian, float, 2, 2>
                rotate(angle);
        util::geometry::polygon_type out;
        boost::geometry::transform(robot_polygon, out, rotate);
        util::geometry::polygon_type out2;
        boost::geometry::transform(out, out2, translate);
        return out2;
    }
 
    Scene2D::Scene2D(std::vector<VirtualRobot::CollisionModelPtr>& collisionModels,
                     float ignoreVerticesOver)
    {
        initializeFromCollisionModels(collisionModels, ignoreVerticesOver);
    }
 
    Scene2D::Scene2D(const SceneRepresentation& scene, float ignoreVerticesOver)
    {
        std::vector<VirtualRobot::CollisionModelPtr> collisionModels;
        const auto& sceneObjectColModels = scene.staticObjects->getCollisionModels();
        collisionModels.insert(collisionModels.end(),
                               sceneObjectColModels.begin(),
                               sceneObjectColModels.end());
        for (const auto& articulatedObject : scene.articulatedObjects)
        {
            const auto colModels = articulatedObject->getCollisionModels();
            collisionModels.insert(collisionModels.end(), colModels.begin(), colModels.end());
        }
 
        initializeFromCollisionModels(collisionModels, ignoreVerticesOver);
    }
 
    float
    Scene2D::distance(const CollisionPolygon& robot) const
    {
        double minDistance = std::numeric_limits<float>::max();
        for (const auto& obs : obstacles)
        {
            const auto distance = boost::geometry::distance(obs, robot);
            minDistance = std::min(distance, minDistance);
        }
        return static_cast<float>(minDistance);
    }
 
    std::vector<CollisionPolygon>&
    Scene2D::getCollisionPolygons()
    {
        return this->obstacles;
    }
 
    namespace
    {
 
        std::vector<std::vector<Eigen::Vector2f>>
        getCollModelClusters(VirtualRobot::CollisionModelPtr collModel, float ignoreVerticesOver)
        {
            std::vector<std::vector<Eigen::Vector2f>> clusters;
 
            auto allVertices = collModel->getModelVeticesGlobal();
            auto allFaces = collModel->getTriMeshModel()->faces;
 
            // filter out all faces where at least one vertex is too high in z-direction
            auto facesFiltered = allFaces |
                                 ranges::views::filter(
                                     [ignoreVerticesOver, &allVertices](const auto& f)
                                     {
                                         return allVertices[f.id1].z() < ignoreVerticesOver &&
                                                allVertices[f.id2].z() < ignoreVerticesOver &&
                                                allVertices[f.id3].z() < ignoreVerticesOver;
                                     }) |
                                 ranges::to_vector;
 
            // create clusters of connected faces
            // idea: use union-find data structure for vertices and then join vertices that share faces
            std::vector<int> rank;
            std::vector<int> parent;
            rank.reserve(allVertices.size());
            parent.reserve(allVertices.size());
            boost::disjoint_sets<int*, int*> ds(rank.data(), parent.data());
            // initialize
            for (std::size_t i = 0; i < allVertices.size(); i++)
            {
                rank.push_back(0);
                parent.push_back(static_cast<int>(i));
                ds.make_set(static_cast<int>(i));
            }
            for (const auto& face : facesFiltered)
            {
                ds.union_set(face.id1, face.id2);
                ds.union_set(face.id2, face.id3);
                ds.union_set(face.id1, face.id3);
            }
            // also merge vertices that are very close to one another
            for (std::size_t i = 0; i < allVertices.size(); i++)
            {
                for (std::size_t j = i + 1; j < allVertices.size(); j++)
                {
                    if (allVertices[i].z() >= ignoreVerticesOver ||
                        allVertices[j].z() >= ignoreVerticesOver)
                    {
                        continue;
                    }
                    const float dist = (allVertices[i] - allVertices[j]).squaredNorm();
                    if (dist < 100.F) // threshold for merging vertices
                    {
                        ds.union_set(static_cast<int>(i), static_cast<int>(j));
                    }
                }
            }
            // collect clusters
            std::map<int, std::vector<Eigen::Vector2f>> clustersMap;
            for (std::size_t i = 0; i < allVertices.size(); i++)
            {
                if (allVertices[i].z() < ignoreVerticesOver)
                {
                    const int setId = ds.find_set(static_cast<int>(i));
                    clustersMap[setId].emplace_back(allVertices[i].x(), allVertices[i].y());
                }
            }
            for (const auto& [setId, vertices] : clustersMap)
            {
                clusters.push_back(vertices);
            }
            return clusters;
        }
 
        std::vector<util::geometry::polygon_type>
        mergeOverlappingPolygons(std::vector<util::geometry::polygon_type>& obstacles)
        {
            std::vector<util::geometry::polygon_type> mergedObstacles;
            std::vector<bool> merged(obstacles.size(), false);
            for (std::size_t i = 0; i < obstacles.size(); i++)
            {
                if (merged[i])
                {
                    continue;
                }
                util::geometry::polygon_type currentPoly = obstacles[i];
                merged[i] = true;
                bool mergedSomething = true;
                while (mergedSomething)
                {
                    mergedSomething = false;
                    for (std::size_t j = 0; j < obstacles.size(); j++)
                    {
                        if (i == j || merged[j])
                        {
                            continue;
                        }
                        if (boost::geometry::intersects(currentPoly, obstacles[j]) ||
                            boost::geometry::touches(currentPoly, obstacles[j]) ||
                            boost::geometry::within(currentPoly, obstacles[j]) ||
                            boost::geometry::within(obstacles[j], currentPoly))
                        {
                            std::vector<util::geometry::polygon_type> unionPoly;
                            boost::geometry::union_(currentPoly, obstacles[j], unionPoly);
                            if (unionPoly.size() > 1)
                            {
                                ARMARX_WARNING
                                    << "Merging two polygons resulted in multiple polygons. "
                                       "This should not happen. Keeping the first polygon.";
                            }
                            else if (unionPoly.empty())
                            {
                                ARMARX_WARNING << "Merging two polygons resulted in an empty "
                                                  "polygon. This should "
                                                  "not happen. Keeping the original polygon.";
                                continue;
                            }
                            currentPoly = unionPoly[0];
                            merged[j] = true;
                            mergedSomething = true;
                        }
                    }
                }
                mergedObstacles.push_back(currentPoly);
            }
            return mergedObstacles;
        }
    } // namespace
 
    void
    Scene2D::initializeFromCollisionModels(
        std::vector<VirtualRobot::CollisionModelPtr>& collisionModels,
        float ignoreVerticesOver)
    {
        obstacles.clear();
        bool useFaces = true;
        for (const auto& collModel : collisionModels)
        {
            if (ignoreVerticesOver >= 0)
            {
                const auto clusters = getCollModelClusters(collModel, ignoreVerticesOver);
                for (const auto& cluster : clusters)
                {
                    if (cluster.size() >= 3) // need at least 3 points to form a polygon
                    {
                        useFaces = false; // currenty not supported
                        if (useFaces)
                        {
                            ARMARX_INFO << "";
                        }
                        else
                        {
                            ARMARX_INFO << "Using only vertices for collision model";
                            auto& collPoly = obstacles.emplace_back();
                            collPolyFromVertices(
                                cluster |
                                    ranges::views::transform(
                                        [](const Eigen::Vector2f& v)
                                        { return Eigen::Vector3f{v.x(), v.y(), 0.0f}; }) |
                                    ranges::to_vector,
                                collPoly);
                        }
                    }
                    else
                    {
                        ARMARX_INFO << "Ignoring cluster with less than 3 vertices";
                    }
                }
            }
            else
            {
                const auto vertices = collModel->getModelVeticesGlobal();
                if (useFaces)
                {
                    ARMARX_INFO << "Using faces for collision model";
                    const auto triMeshModel = collModel->getTriMeshModel();
                    const auto faces = triMeshModel->faces;
                    const auto polygons = collPolyFrom3D(vertices, faces);
                    ARMARX_INFO << faces.size() << " faces in collision model";
                    ARMARX_INFO << "Found " << polygons.size() << " polygons in collision model";
                    obstacles.insert(obstacles.end(), polygons.begin(), polygons.end());
                }
                else
                {
                    ARMARX_INFO << "Using only vertices for collision model";
                    auto& collPoly = obstacles.emplace_back();
                    collPolyFromVertices(vertices, collPoly);
                }
            }
 
 
            // merge overlapping polygons
            auto mergedObstacles = mergeOverlappingPolygons(obstacles);
            ARMARX_INFO << "Reduced " << obstacles.size() << " obstacles to "
                        << mergedObstacles.size() << " after merging.";
            obstacles = std::move(mergedObstacles);
        }
 
        for (const auto& obs : obstacles)
        {
            ARMARX_INFO << "Obstacle: " << boost::geometry::wkt(obs);
        }
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware