/**
* @function RRTPlan
*/
void PlannerTab::RRTPlan() {
double stepSize = 0.02;
PathPlanner *planner = new PathPlanner( *mWorld, mCollision, false, stepSize );
int maxNodes = 5000;
bool result = planner->planPath( gRobotId,
gLinks,
gStartConf,
gTargetConf,
true, // bidirectional
true, // connect
true, // greedy
false, // smooth
maxNodes );
RRTExecute( planner->path );
}
/// Visualizes a 2D RRT
void drawTwoLink () {
// Create a robot and a world
const double l1 = 1.5, l2 = 1.0;
Skeleton* r = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL);
World w;
w.addSkeleton(r);
// Create a path planner and feed it an unfeasible goal to grow the RRT freely
PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0);
vector <int> links;
links.push_back(0);
links.push_back(1);
list <VectorXd> path;
planner.planPath(r, links, Vector2d(-DART_PI+0.1, -DART_PI+0.1), Vector2d(DART_PI-0.1, DART_PI-0.1), path);
// Print the nodes
RRT* rrt = planner.start_rrt;
draw(rrt, NULL);
}