collision.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <se2ez/core.h>

using namespace se2ez;

int main()
{
    std::vector<GeometryPtr> geo1 = {
        std::make_shared<Geometry>(Geometry::BOX, Eigen::Vector2d{0.5, 0.5}, Eigen::Vector3d{0, 0, 0})  //
    };

    std::vector<GeometryPtr> geo2 = {
        std::make_shared<Geometry>(Geometry::BOX, Eigen::Vector2d{0.5, 0.5}, Eigen::Vector3d{0.1, 0, 0}),   //
        std::make_shared<Geometry>(Geometry::BOX, Eigen::Vector2d{0.5, 0.5}, Eigen::Vector3d{-0.1, 0, 0}),  //
    };

    auto f_1 = std::make_shared<Frame>("l1",                               // name
                                       Eigen::Vector3d{0, 0, 0},           // tip offset
                                       Joint::FLOAT,                       // type
                                       Eigen::Vector3d{-1, -1, math::pi},  // lower limits
                                       Eigen::Vector3d{1, 1, -math::pi},   // upper limits
                                       geo1);                              // geometry

    auto f_2 = std::make_shared<Frame>("l2",                               // name
                                       Eigen::Vector3d{0, 0, 0},           // tip offset
                                       Joint::FLOAT,                       // type
                                       Eigen::Vector3d{-1, -1, math::pi},  // lower limits
                                       Eigen::Vector3d{1, 1, -math::pi},   // upper limits
                                       geo2);                              // geometry

    auto robot = std::make_shared<Robot>();
    robot->attachFrame(f_1);
    robot->attachFrame(f_2);
    robot->compileTree();

    Box2DCollisionManager cm(robot);
    // cm.getACM()->disable("l2", "l1");

    auto state = std::make_shared<State>(robot);
    for (double s = 0; s <= 3; s += 0.01)
    {
        state->setFloatJoint(robot, "l1", Eigen::Vector3d{0, -1.5 + s, 0});
        state->setFloatJoint(robot, "l2", Eigen::Vector3d{0, 0, 0});
        state->fk(robot);

        CollisionManager::SignedDistanceMap distances;
        auto d = cm.distance(state, distances);

        std::cout << (cm.collide(state) ? "In Collision" : "Collision Free") << std::endl;
        std::cout << "Distance: " << d.distance << std::endl;
        std::cout << robot->printTree([&](const Robot::FrameDataPtr &frame) -> std::string {
                auto f = distances.find(frame->frame->getName());
                if (f == distances.end())
                    return "ERROR";

                const CollisionManager::SignedDistance &sd = f->second;

                std::stringstream ss;
                ss << log::format("%1% [%2%][%3%]", sd.distance, tf::printVector(sd.point), tf::printVector(sd.normal));
                return ss.str();
        });
        std::cout << std::endl;

    }

    return 0;
}