plan.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

#include <se2ez/core.h>
#include <se2ez/plan.h>

using namespace se2ez;

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

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

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

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

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

    auto f_b = std::make_shared<Frame>("box",                     // name
                                       Eigen::Vector3d{0, 0, 0},  // tip offset
                                       geo3);                     // geometry

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

    plan::EZPlans planner(robot);
    planner.initialize();
    planner.setPlanner<ompl::geometric::RRTConnect>();
    planner.setStartGoal(math::toVector({-1.5, 0, 0, 1.5, 0, 0}), math::toVector({1.5, 0, 0, -1.5, 0, 0}));

    ompl::base::PlannerStatus solved = planner.setup->solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        planner.setup->getSolutionPath().print(std::cout);

        std::cout << "Simplified solution:" << std::endl;
        planner.setup->simplifySolution();
        planner.setup->getSolutionPath().print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}