Plan a path

Table of Contents

• Intro
• Perform motion planning
• Control the task
• Wait until the task is done
• Retrieving the results

Intro

Note

All code can be found in the Tutorials package under MotionPlanningTutorials/source/MotionPlanningTutorials/components/SimpleMotionPlanningExample

Code using A* can be found at MotionPlanningTutorials/source/MotionPlanningTutorials/components/SimpleAStarPathSearchExample

The execution of these two examples can be done by simply running the executable (no scenario is required).

Code using both algorithms in a kitchen scenario can be found at MotionPlanningTutorials/source/MotionPlanningTutorials/components/MotionPlanningKitchenExample. This example can be started with the scenario MotionPlanningKitchenExample.

Perform motion planning

To perform planning you need to have a proxy for a armar::MotionPlanningServer.

MotionPlanningServerInterfacePrx pServerProxy;

Next you will need to select the algorithm to use. In this tutorial the RRTConnect algorithm is used.

Depending on the algorithm you will need some parameters.

All algorithms will require a CSpace (e.g. armarx::SimoxCSpace [Setting up a SimoxCSpace]). This CSpace defines the configuration space and is able to check whether a configuration is valid.

Next you will need the start and goal configuration.

VectorXf start;
start.assign(7, 0);
 
 
VectorXf goal=start;
goal.at(2)=-1;

Now you can create and enqueue the task:

MotionPlanningTaskBasePtr task = new RRTConnectTask{cspace, start, goal};
RRTConnectTaskHandle taskHandle = pServerProxy->enqueueTask(task);

Note

The RRTConnect algorithm has several optional parameters.

Warning

QtCreator fails to display the functions of a task handle. (see here)

After you enqueued the task the server will execute it (the queue is fifo).

Control the task

At any time you can abort the task

taskHandle->abortTask()

or get the task's status:

taskHandle->getTaskStatus()
taskHandle->isPlanning()
taskHandle->finishedPlanning()

Some algorithms may offer special control interfaces (e.g. armarx::ADDIRRTStarTask). Simply cast the proxy if you require the special features.

Wait until the task is done

You may want to block until the task finished planning:

taskHandle->waitForFinishedRunning();

Retrieving the results

After the task finished you can get the found path (if any path was found).

Path p = taskHandle->getPath();

Use the following code for converting the Path-instance into a Trajectory (which is a Variant-type that can be used for statechart parameters):

TrajectoryPtr trajectory = cspace->pathToTrajectory(p);