Plan a path

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);
armarx::TrajectoryPtr
IceInternal::Handle< Trajectory > TrajectoryPtr
Definition: Trajectory.h:52
armarx::RRTConnectTaskHandle
RemoteHandle< MotionPlanningTaskControlInterfacePrx > RRTConnectTaskHandle
Definition: Task.h:125
armarx::RRTConnectTask
rrtconnect::Task RRTConnectTask
Definition: Task.h:123