robowflex/robowflex_dart/scripts/fetch_ik.cpp at 75df8340c1dbf43cfcc29a6e20a5a60608c93f33 · KavrakiLab/robowflex · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
/* Author: Zachary Kingston */

#include <chrono>
#include <thread>

#include <robowflex_library/log.h>

#include <robowflex_dart/gui.h>
#include <robowflex_dart/io.h>
#include <robowflex_dart/planning.h>
#include <robowflex_dart/robot.h>
#include <robowflex_dart/space.h>
#include <robowflex_dart/tsr.h>
#include <robowflex_dart/world.h>

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

using namespace robowflex;

int main(int /*argc*/, char ** /*argv*/)
{
    auto world = std::make_shared<darts::World>();

    auto fetch1 = darts::loadMoveItRobot("fetch1",                                         //
                                         "package://fetch_description/robots/fetch.urdf",  //
                                         "package://fetch_moveit_config/config/fetch.srdf");
    auto fetch2 = fetch1->cloneRobot("fetch2");
    fetch2->setDof(4, 1);

    world->addRobot(fetch1);
    world->addRobot(fetch2);

    darts::Window window(world);

    darts::PlanBuilder builder(world);
    builder.addGroup("fetch1", "arm_with_torso");
    builder.addGroup("fetch2", "arm_with_torso");

    builder.setStartConfiguration({
        0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0,  //
        0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0,  //
    });

    builder.initialize();

    darts::TSR::Specification goal1_spec;
    goal1_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
    goal1_spec.setPose(0.4, 0.3, 0.92,  //
                                        // 0.5, -0.5, 0.5, 0.5);
                       0.707, 0, 0, 0.707);

    darts::TSR::Specification goal2_spec;
    goal2_spec.setFrame("fetch2", "wrist_roll_link", "base_link");
    goal2_spec.setPose(0.4, -0.3, 0.92,  //
                       0.707, 0, 0, -0.707);

    auto goal1_tsr = std::make_shared<darts::TSR>(world, goal1_spec);
    auto goal2_tsr = std::make_shared<darts::TSR>(world, goal2_spec);

    auto goal = builder.getGoalTSR({goal1_tsr, goal2_tsr});
    builder.setGoal(goal);

    auto rrt = std::make_shared<ompl::geometric::RRTConnect>(builder.info, true);
    rrt->setRange(1.);
    builder.ss->setPlanner(rrt);

    builder.setup();
    builder.ss->print();

    window.run(
        [&]()
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
            while (true)
            {
                goal->startSampling();
                ompl::base::PlannerStatus solved = builder.ss->solve(30.0);
                goal->stopSampling();

                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                if (solved)
                {
                    RBX_INFO("Found solution!");
                    window.animatePath(builder, builder.getSolutionPath());
                }
                else
                    RBX_WARN("No solution found");

                builder.ss->clear();
            }
        });

    return 0;
}