Motion Planning

Motion planning is about planning a collision free path from one robot configuration to another, avoiding any obstacles in the workspace. In the core RobWork library (rw::pathplanning namespace), there is an interface for such planners called QToQPlanner. RobWork provides a pathplanning library with implementation of various planning algorithms. These are found under the namespace rwlibs::pathplanners and implements the QToQPlanner interface:

ARWPlanner	Adaptive Random Walk Planner	API
PRMPlanner	Probabilistic RoadMap Planner	API
RRTPlanner	Rapidly-exploring Random Tree Planner	API
SBLPlanner	Single-query Bi-directional Lazy collision checking Planner	API
Z3Planner	Z3 Method	API

See the API documentation for each of these classes for references to litterature and more information about possible variants of the algorithms.

Example Scene

We will use the SimplePA10Demo scene for demonstration of pathplanning. This scene has a Mitsubishi PA-10 robot mounted on a gantry. To move from one location in the scene to another requires pathplanning to avoid collisions.

SinglePA10Demo scene from the Device & Scene Collection (RobWorkData).

Metrics

rw::math::Metric<X> is the general interface for measuring a distance between a pair of values of type X. Path planning algorithms, for example, often require a metric for measuring the distance between configurations.

Metrics available in RobWork include:

• Manhattan metric (rw::math::MetricFactory::makeManhattan(), rw::math::MetricFactory::makeWeightedManhattan())

• Euclidean metric (rw::math::MetricFactory::makeEuclidean(), rw::math::makeWeightedEuclidean())

• Infinity metric (rw::math::MetricFactory::makeInfinity(), rw::math::MetricFactory::makeWeightedInfinity())

These build-in metrics can be instantiated for configuration types (rw::math::Q) and other vector types such as rw::math::Vector3D and std::vector<double>. This program shows instantiation and expected output for 3 different metrics:

#include <rw/math/Metric.hpp>
#include <rw/math/MetricFactory.hpp>

using rw::core::Ptr;
using namespace rw::math;

void metricExample()
{
    typedef Vector3D<> V;
    typedef Ptr<Metric<V> > VMetricPtr;

    VMetricPtr m1 = MetricFactory::makeManhattan<V>();
    VMetricPtr m2 = MetricFactory::makeEuclidean<V>();
    VMetricPtr mInf = MetricFactory::makeInfinity<V>();

    const V a(0, 0, 0);
    const V b(1, 1, 1);

    std::cout
        << m1->distance(a, b) << " is " << 3.0 << "\n"
        << m2->distance(a, b) << " is " << sqrt(3.0) << "\n"
        << mInf->distance(a, b) << " is " << 1 << "\n";
}

QIKSolver

For use in planning the IK sampler interface (rw::pathplanning::QIKSampler) hides details of selection of IK solver and start configurations for the solver. The program below tests the default iterative IK solver for a device. The program selects 10 random base to end transforms for a device using the forward kinematics for the device. Using the default IK sampler, the program then checks that an IK solution is found for all transforms. Only a small number of start configurations are used for each target transform, and therefore the IK sampler might not always find an IK solution. If the IK sampler is constrained by the requirement that the IK solutions must be collision free, then solutions for only a subset of the target transforms are found.

#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/pathplanning/QConstraint.hpp>
#include <rw/pathplanning/QIKSampler.hpp>
#include <rw/pathplanning/QSampler.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>

using rw::core::ownedPtr;
using rw::kinematics::State;
using rw::loaders::WorkCellLoader;
using namespace rw::math;
using namespace rw::models;
using namespace rw::pathplanning;
using rw::proximity::CollisionDetector;
using namespace rwlibs::proximitystrategies;

typedef std::vector<Transform3D<> > TransformPath;

TransformPath getRandomTargets(const Device& device, State state, int targetCnt)
{
    TransformPath result;
    QSampler::Ptr sampler = QSampler::makeUniform(device);
    for (int cnt = 0; cnt < targetCnt; cnt++) {
        device.setQ(sampler->sample(), state);
        result.push_back(device.baseTend(state));
    }
    return result;
}

void printReachableTargets(
    const TransformPath& targets,
    QIKSampler& ik)
{
    int i = 0;
    for(const Transform3D<>& target: targets) {
        const Q q = ik.sample(target);
        std::cout << i << " " << (q.empty() ? "False" : "True") << "\n";
        ++i;
    }
}

void invkinExample(
    Device& device, const State& state, QConstraint& constraint)
{
    QIKSampler::Ptr ik_any = QIKSampler::make(&device, state, NULL, NULL, 25);
    QIKSampler::Ptr ik_cfree = QIKSampler::makeConstrained(ik_any, &constraint, 25);

    const TransformPath targets = getRandomTargets(device, state, 10);

    std::cout << "IK solutions found for targets:\n";
    printReachableTargets(targets, *ik_any);

    std::cout << "Collision free IK solutions found for targets:\n";
    printReachableTargets(targets, *ik_cfree);
}

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " <workcell-file>\n";
        exit(1);
    }

    WorkCell::Ptr workcell = WorkCellLoader::Factory::load(argv[1]);
    Device::Ptr device = workcell->getDevices().front();
    const State state = workcell->getDefaultState();

    CollisionDetector::Ptr detector = ownedPtr( new CollisionDetector(
        workcell, ProximityStrategyYaobi::make()) );

    QConstraint::Ptr constraint = QConstraint::make(detector, device, state);

    invkinExample(*device, state, *constraint);
}

PathPlanning

rw::pathplanning::PathPlanner<From, To, Path> is the general interface for finding a path of type Path connecting a start location of type From and an goal location of type To.

Important variations of this interface includes:

• rw::pathplanning::QToQPlanner: Standard planning of a configuration space path that connects a start configuration to a goal configuration.

• rw::pathplanning::QToTPlanner: Planning of a configuration space path connecting a start configuration to any end configuration for which a spatial constraint represented a value of type rw::math::Transform3D<> is satisfied. Typically, planners of this type find paths for devices such that the tool of the device ends up at the given goal transformation (in other words, the planner of type rw::pathplanning::QToTPlanner implicitly solves an inverse kinematics problem).

• rw::pathplanning::QToQSamplerPlanner: Planning of a configuration space path from a start configuration to any end configuration returned by the sampler (rw::pathplanning::QSampler) representing the goal region.

These 3 planners all represent the resulting path by a sequence of configurations (rw::trajectory::QPath).

The path planners of RobWork are placed in the library rw_pathplanners. The example below instantiates a path planner for the first device of the workcell and plans a number of paths to random collision free configurations of the workcell. The full configuration space path mapped to the corresponding sequence of states (rw::kinematics::State) and written to a file that can be loaded into RobWorkStudio using the PlayBack plugin. The example makes use of configuration space sampling and path planning constraints described in these earlier sections:

• Configuration space sampling

• Workcell and configuration space constraints

#include <rw/models/WorkCell.hpp>
#include <rw/models/Models.hpp>
#include <rw/loaders/path/PathLoader.hpp>
#include <rw/pathplanning/QSampler.hpp>
#include <rw/pathplanning/QToQPlanner.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>
#include <rwlibs/pathplanners/sbl/SBLPlanner.hpp>

using rw::math::Q;
using namespace rw::models;
using rw::kinematics::State;
using namespace rw::pathplanning;
using rw::proximity::CollisionDetector;
using rw::loaders::PathLoader;
using rwlibs::proximitystrategies::ProximityStrategyYaobi;
using namespace rwlibs::pathplanners;

void plannerExample(WorkCell& workcell)
{
    // The common state for which to plan the paths.
    const State state = workcell.getDefaultState();

    // The first device of the workcell.
    Device::Ptr device = workcell.getDevices().front();

    CollisionDetector coldect(&workcell, ProximityStrategyYaobi::make());

    // The q constraint is to avoid collisions.
    QConstraint::Ptr constraint = QConstraint::make(&coldect, device, state);

    // the edge constraint tests the constraint on edges, eg. edge between two configurations
    QEdgeConstraintIncremental::Ptr edgeconstraint = QEdgeConstraintIncremental::makeDefault(
        constraint, device);

    // An SBL based point-to-point path planner.
    QToQPlanner::Ptr planner = SBLPlanner::makeQToQPlanner(
        SBLSetup::make(constraint, edgeconstraint, device));

    // A sampler of collision free configurations for the device.
    QSampler::Ptr cfreeQ = QSampler::makeConstrained(
        QSampler::makeUniform(device), constraint);

    // The start configuration for the path.
    Q pos = device->getQ(state);

    // Plan 10 paths to sampled collision free configurations.
    rw::trajectory::Path<Q> path;
    for (int cnt = 0; cnt < 10; cnt++) {
        const Q next = cfreeQ->sample();
        const bool ok = planner->query(pos, next, path);
        if (!ok) {
            std::cout << "Path " << cnt << " not found.\n";
            return;
        } else {
            pos = next;
        }
    }

    // Map the configurations to a sequence of states.
    const std::vector<State> states = Models::getStatePath(*device, path, state);

    // Write the sequence of states to a file.
    PathLoader::storeVelocityTimedStatePath(
        workcell, states, "ex-path-planning.rwplay");
}

The path planner of the above example is based on the SBL algorithm. This example shows instantiation of some more of the available path planners:

#include <rw/pathplanning/PlannerConstraint.hpp>
#include <rwlibs/pathplanners/sbl/SBLPlanner.hpp>
#include <rwlibs/pathplanners/sbl/SBLSetup.hpp>
#include <rwlibs/pathplanners/rrt/RRTPlanner.hpp>
#include <rwlibs/pathplanners/arw/ARWPlanner.hpp>

using rw::models::Device;
using namespace rw::pathplanning;
using namespace rwlibs::pathplanners;

QToQPlanner::Ptr getQToQPlanner(
    Device::Ptr device,
    const PlannerConstraint constraint,
    const std::string& type)
{
    if (type == "SBL") {
    	QConstraint::Ptr qconstraint = constraint.getQConstraintPtr();
        return SBLPlanner::makeQToQPlanner(SBLSetup::make(qconstraint, QEdgeConstraintIncremental::makeDefault(qconstraint,device), device));
    } else if (type == "RRT") {
        return RRTPlanner::makeQToQPlanner(constraint, device);
    } else if (type == "ARW") {
        return ARWPlanner::makeQToQPlanner(constraint, device);
    } else {
        return NULL;
    }
}

Variations of these constructor functions have options for example for controlling the configuration space exploration of the planner.

Workcell and configuration space constraints

A collision detector (rw::proximity::CollisionDetector) is an example of a constraint on the states of a workcell. Collision checking is but one form of constraint, and applications may implement their constraints in terms of other classes than rw::proximity::CollisionDetector.

The general interface for a discrete constraint on states (rw::kinematics::State) is rw::pathplanning::StateConstraint. The method to call to check if a constraint is satisfied for a state is rw::pathplanning::StateConstraint::inCollision(). The naming of the method is only a convention. The constraint need not not be concerned with actual collisions of the workcell. A user may inherit from the interface and implement any kind of constraint they desire.

Path planners and other planners often operate on configurations (rw::math::Q) rather than workcell states (rw::kinematics::State). The interface for a discrete constraint on the configuration space is rw::pathplanning::QConstraint and the method to call to check if the constraint is satisfied is rw::pathplanning::QConstraint::inCollision().

rw::pathplanning::StateConstraint as well as rw::pathplanning::QConstraint provide constructor functions and functions for combining constraints.

A sampling based path planner typically calls a configuration constraint (rw::pathplanning::QConstraint) to verify individual configurations. The path planner connects individual configurations by edges, and verifies if the device can follow the path represented by the edge. The interface for verifying a configuration space path connecting a pair of configurations is called rw::pathplanning::QEdgeConstraint. The method on the interface to verify the edge is rw::pathplanning::QEdgeConstraint::inCollision().

Given a configuration constraint (rw::pathplanning::QConstraint), a constraint for an edge (rw::pathplanning::QEdgeConstraint) can be implemented by discretely checking the edge for collisions. When constructing such edge constraint (see rw::pathplanning::QEdgeConstraint::make()) you can specify the resolution and metric for the discrete verification of the edge, or a default metric and resolution can be used.

A configuration constraint together with an edge constraint is named a planner constraint (rw::pathplanning::PlannerConstraint). rw::pathplanning::PlannerConstraint::make() utility functions are provided to ease the construction of constraints for standard collision detection.

This program constructs a collision detector and corresponding default planner constraint for the first device of the workcell. The program calls the planner constraint to check if the edge from the lower to upper corner of the configuration space can be traversed:

#include <rw/models/Device.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/pathplanning/PlannerConstraint.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>

using rw::core::ownedPtr;
using rw::math::Q;
using namespace rw::models;
using rw::pathplanning::PlannerConstraint;
using rw::proximity::CollisionDetector;
using rwlibs::proximitystrategies::ProximityStrategyYaobi;

void constraintExample(WorkCell& workcell)
{
    Device::Ptr device = workcell.getDevices().front();

    const PlannerConstraint constraint = PlannerConstraint::make(
        ownedPtr( new CollisionDetector(
            &workcell, ProximityStrategyYaobi::make() ) ),
        device,
        workcell.getDefaultState());

    const Q start = device->getBounds().first;
    const Q end = device->getBounds().second;

    std::cout
        << "Start configuration is in collision: "
        << constraint.getQConstraint().inCollision(start)
        << "\n"
        << "End configuration is in collision: "
        << constraint.getQConstraint().inCollision(end)
        << "\n"
        << "Edge from start to end is in collision: "
        << constraint.getQEdgeConstraint().inCollision(start, end)
        << "\n";
}

Configuration space sampling

Configuration space sampling is a useful tool for path planners and various other planning algorithms.

The interface for a sampler of the configuration space is rw::pathplanning::QSampler. The rw::pathplanning::QSampler interface provides constructor functions, including:

• rw::pathplanning::QSampler::makeFinite(): Deterministic sampling from a finite sequence of configurations.

• rw::pathplanning::QSampler::makeUniform(): Configurations for a device sampled uniformly at random.

• rw::pathplanning::QSampler::makeConstrained(): A sampler filtered by a constraint.

This example shows the construction of a sampler of collision free configurations. The sampler calls a randomized sampler of the configuration space of the device, and filters these configurations by the constraint that the configurations should be collision free.

#include <rw/models/WorkCell.hpp>
#include <rw/models/Device.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rw/pathplanning/QConstraint.hpp>
#include <rw/pathplanning/QSampler.hpp>
#include <rw/math/Q.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>

using rw::core::ownedPtr;
using rw::math::Q;
using namespace rw::models;
using rw::proximity::CollisionDetector;
using namespace rw::pathplanning;
using rwlibs::proximitystrategies::ProximityStrategyYaobi;

void samplerExample(WorkCell& workcell)
{
    Device::Ptr device = workcell.getDevices().front();

    CollisionDetector::Ptr coldect = ownedPtr( new CollisionDetector(&workcell, ProximityStrategyYaobi::make()) );

    QConstraint::Ptr constraint = QConstraint::make(
        coldect,
        device,
        workcell.getDefaultState());

    QSampler::Ptr anyQ = QSampler::makeUniform(device);
    QSampler::Ptr cfreeQ = QSampler::makeConstrained(anyQ, constraint);

    for (int i = 0; i < 4; i++) {
        const Q q = cfreeQ->sample();
        std::cout
            << "Q(" << i << ") is in collision: "
            << constraint->inCollision(q) << "\n";
    }
}

RobWorkStudio Planning Plugin

Open the Planning , Jog and Log plugins.

C++

#include <rw/core/Ptr.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/math/Q.hpp>
#include <rw/models/CompositeDevice.hpp>
#include <rw/models/Device.hpp>
#include <rw/pathplanning/PlannerConstraint.hpp>
#include <rw/pathplanning/QToQPlanner.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rw/proximity/CollisionStrategy.hpp>
#include <rw/proximity/ProximityData.hpp>
#include <rw/trajectory/Path.hpp>
#include <rwlibs/pathplanners/rrt/RRTPlanner.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyFactory.hpp>

using rw::core::ownedPtr;
using rw::kinematics::State;
using rw::loaders::WorkCellLoader;
using rw::math::Q;
using namespace rw::models;
using namespace rw::proximity;
using namespace rw::pathplanning;
using rw::trajectory::QPath;
using rwlibs::pathplanners::RRTPlanner;
using rwlibs::proximitystrategies::ProximityStrategyFactory;

#define WC_FILE "/scenes/SinglePA10Demo/SinglePA10DemoGantry.wc.xml"

int main (int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " <path/to/RobWorkData>" << std::endl;
        exit (1);
    }
    const std::string path = argv[1];

    const WorkCell::Ptr wc = WorkCellLoader::Factory::load (path + WC_FILE);
    if (wc.isNull ())
        RW_THROW ("WorkCell could not be loaded.");
    const Device::Ptr gantry = wc->findDevice ("Gantry");
    const Device::Ptr pa10   = wc->findDevice ("PA10");
    if (gantry.isNull ())
        RW_THROW ("Gantry device could not be found.");
    if (pa10.isNull ())
        RW_THROW ("PA10 device could not be found.");

    const State defState     = wc->getDefaultState ();
    const Device::Ptr device = ownedPtr (new CompositeDevice (
        gantry->getBase (), wc->getDevices (), pa10->getEnd (), "Composite", defState));

    const CollisionStrategy::Ptr cdstrategy =
        ProximityStrategyFactory::makeCollisionStrategy ("PQP");
    if (cdstrategy.isNull ())
        RW_THROW ("PQP Collision Strategy could not be found.");
    const CollisionDetector::Ptr collisionDetector =
        ownedPtr (new CollisionDetector (wc, cdstrategy));
    const PlannerConstraint con    = PlannerConstraint::make (collisionDetector, device, defState);
    const QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner (con, device);

    const Q beg (9, -0.67, -0.79, 2.8, -0.02, -1.01, -0.26, -0.77, -1.01, 0);
    const Q end (9, 0.57, 0.79, -1.23, 0.21, -0.63, -0.55, -0.07, -1.08, 0);

    ProximityData pdata;
    State state = defState;
    device->setQ (beg, state);
    if (collisionDetector->inCollision (state, pdata))
        RW_THROW ("Initial configuration in collision! can not plan a path.");
    device->setQ (end, state);
    if (collisionDetector->inCollision (state, pdata))
        RW_THROW ("Final configuration in collision! can not plan a path.");

    QPath result;
    if (planner->query (beg, end, result)) {
        std::cout << "Planned path with " << result.size ();
        std::cout << " configurations" << std::endl;
    }

    return 0;
}

Python

import sdurw_models
from sdurw import *
from sdurw_loaders import *
from sdurw_pathplanners import RRTPlanner
from sdurw_proximitystrategies import ProximityStrategyFactory
import sys

WC_FILE = "/scenes/SinglePA10Demo/SinglePA10DemoGantry.wc.xml"

if __name__ == '__main__':
    if len(sys.argv) != 2:
        print("Usage: python3 " + sys.argv[0] + " <path/to/RobWorkData>")
        sys.exit(1)

    wc = WorkCellLoaderFactory.load(sys.argv[1] + WC_FILE)
    if wc.isNull():
        raise Exception("WorkCell could not be loaded")
    gantry = wc.findDevice("Gantry")
    pa10 = wc.findDevice("PA10")
    if gantry.isNull():
        raise Exception("Gantry device could not be found.")
    if pa10.isNull():
        raise Exception("PA10 device could not be found.")

    defState = wc.getDefaultState()
    device = sdurw_models.ownedPtr(CompositeDevice(gantry.getBase(), wc.getDevices(), pa10.getEnd(), "Composite", defState))

    cdstrategy = ProximityStrategyFactory.makeCollisionStrategy("PQP")
    if cdstrategy.isNull():
        raise Exception("PQP Collision Strategy could not be found.")
    print(device.cptr())
    collisionDetector = sdurw_proximity.ownedPtr(CollisionDetector(wc, cdstrategy))
    con = PlannerConstraint.make(collisionDetector, device, defState)
    planner = RRTPlanner.makeQToQPlanner(con, device.asDevicePtr())

    beg = Q(9, -0.67, -0.79, 2.8, -0.02, -1.01, -0.26, -0.77, -1.01, 0)
    end = Q(9, 0.57, 0.79, -1.23, 0.21, -0.63, -0.55, -0.07, -1.08, 0)

    pdata = ProximityData()
    state = defState
    device.setQ(beg, state)
    if collisionDetector.inCollision(state, pdata):
        raise Exception("Initial configuration in collision!")
    device.setQ(end, state)
    if collisionDetector.inCollision(state, pdata):
        raise Exception("Final configuration in collision!")

    result = []
    if planner.query(beg, end, result):
        print("Planned path with " + str(len(result)) + " configurations")

Java

import java.lang.String;
import org.robwork.LoaderRW;
import org.robwork.sdurw.*;
import org.robwork.sdurw_math.*;
import org.robwork.sdurw_models.*;
import org.robwork.sdurw_kinematics.*;
import org.robwork.sdurw_proximity.*;
import static org.robwork.sdurw.sdurw.ownedPtr;
import org.robwork.sdurw_pathplanners.RRTPlanner;
import org.robwork.sdurw_proximitystrategies.ProximityStrategyFactory;

public class ExMotionPlanning {
    public static final String WC_FILE =
            "/scenes/SinglePA10Demo/SinglePA10DemoGantry.wc.xml";

    public static void main(String[] args) throws Exception {
        LoaderRW.load("sdurw");
        LoaderRW.load("sdurw_math");
        LoaderRW.load("sdurw_kinematics");
        LoaderRW.load("sdurw_proximity");
        LoaderRW.load("sdurw_models");
        LoaderRW.load("sdurw_pathplanners");
        LoaderRW.load("sdurw_proximitystrategies");

        if (args.length != 1) {
            System.out.print("Usage: java " + ExMotionPlanning.class.getSimpleName());
            System.out.println(" <path/to/RobWorkData>");
            System.exit(1);
        }

        WorkCellPtr wc = WorkCellLoaderFactory.load(args[0] + WC_FILE);
        if (wc.isNull())
            throw new Exception("WorkCell could not be loaded.");
        DevicePtr gantry = wc.findDevice("Gantry");
        DevicePtr pa10 = wc.findDevice("PA10");
        if (gantry.isNull())
            throw new Exception("Gantry device could not be found.");
        if (pa10.isNull())
            throw new Exception("PA10 device could not be found.");

        State defState = wc.getDefaultState();
        CompositeDevicePtr device = sdurw_models.ownedPtr(new CompositeDevice(new FramePtr(gantry.getBase()),
                wc.getDevices(), new FramePtr(pa10.getEnd()), "Composite", defState));

        CollisionStrategyPtr cdstrategy =
                ProximityStrategyFactory.makeCollisionStrategy("PQP");
        if (cdstrategy.isNull())
            throw new Exception("PQP Collision Strategy could not be found.");
        CollisionDetectorPtr collisionDetector = sdurw_proximity.ownedPtr(
                new CollisionDetector(wc, cdstrategy));
        PlannerConstraint con = PlannerConstraint.make(collisionDetector,
                device.asDeviceCPtr(), defState);
        QToQPlannerPtr planner = RRTPlanner.makeQToQPlanner(con, device.asDevicePtr());
        State state = wc.getDefaultState();

        final Q beg = new Q(9, -0.67, -0.79, 2.8, -0.02, -1.01, -0.26, -0.77, -1.01, 0);
        final Q end = new Q(9, 0.57, 0.79, -1.23, 0.21, -0.63, -0.55, -0.07, -1.08, 0);

        ProximityData pdata = new ProximityData();
        device.setQ(beg, state);
        if (collisionDetector.inCollision(state, pdata))
            throw new Exception("Initial configuration in collision!");
        device.setQ(end, state);
        if (collisionDetector.inCollision(state, pdata))
            throw new Exception("Final configuration in collision!");

        PathQ result = new PathQ();
        if (planner.query(beg, end, result)) {
            System.out.print("Planned path with " + result.size());
            System.out.println(" configurations");
        }
    }
}

LUA

This example shows how pathplanning can be done in a LUA script. You can run this script directly in the RobWorkStudio LUA plugin. If you want to run the script without RobWorkStudio, see the Standalone LUA section.

require("sdurw_core")
require("sdurw_models")
require("sdurw_proximity")
require("sdurw_loaders")
require("sdurw_trajectory")
require("sdurw_pathplanning")
require("sdurw")
require("sdurw_pathplanners")
require("sdurw_proximitystrategies")
using("sdurw_models")
using("sdurw_proximity")
using("sdurw_loaders")
using("sdurw_trajectory")
using("sdurw")
using("sdurw_pathplanning")
using("sdurw_pathplanners")
using("sdurw_proximitystrategies")

require("sdurw_math")
using("sdurw_math")

if #arg ~= 1 then
    print("Usage: lua ex-motionplanning.lua <path/to/RobWorkData>")
    return 1
end

local WC_FILE = "/scenes/SinglePA10Demo/SinglePA10DemoGantry.wc.xml"

local wc = WorkCellLoaderFactory.load(arg[1] .. WC_FILE)
if wc:isNull() then
    error("WorkCell could not be loaded")
end
local gantry = wc:findDevice("Gantry")
local pa10 = wc:findDevice("PA10")
if gantry:isNull() then
    error("Gantry device could not be found.")
end
if pa10:isNull() then
    error("PA10 device could not be found.")
end

local state = wc:getDefaultState()
local device = sdurw_models.ownedPtr(CompositeDevice(gantry:getBase(), wc:getDevices(),
                        pa10:getEnd(), "Composite", state))

local cdstrategy = sdurw_proximitystrategies.ProximityStrategyFactory.makeCollisionStrategy("PQP")
if cdstrategy:isNull() then
    error("PQP Collision Strategy could not be found.")
end
local collisionDetector = sdurw_proximity.ownedPtr(CollisionDetector(wc, cdstrategy))
local con = PlannerConstraint.make(collisionDetector, device:asDeviceCPtr(), state)
local planner = RRTPlanner.makeQToQPlanner(con, device:asDevicePtr())

local beg = Q(9, -0.67, -0.79, 2.8, -0.02, -1.01, -0.26, -0.77, -1.01, 0)
local fin = Q(9, 0.57, 0.79, -1.23, 0.21, -0.63, -0.55, -0.07, -1.08, 0)

local pdata = ProximityData()
device:setQ(beg, state)
if collisionDetector:inCollision(state, pdata) then
    error("Initial configuration in collision!")
end
device:setQ(fin, state)
if collisionDetector:inCollision(state, pdata) then
    error("Final configuration in collision!")
end

local result = PathQ()
if planner:query(beg, fin, result) then
    print("Planned path with " .. result:size() .. " configurations")
end

In lines 8-26 the WorkCell is loaded, and the two devices ‘Gantry’ and ‘PA10’ are extracted. Rember to check is the returned smart pointers are null, as this would indicate that something went wrong. If you continue without checking, you will likely end up with segmentation errors.

In lines 28 and 29, we first get the default state of the workcell. This is used to construct a new CompositeDevice. The CompositeDevice will make the two devices act as one device to the pathplanning algorithms. As the PA10 device is placed at the end of the Gantry device, we specify the base frame to be the PA10 base frame, and the end frame to be the PA10 end frame. The name of the device will be “Composite” and we construct the CompositeDevice from all the devices in the WorkCell (assuming there is only the two). If you only have one device to plan for, there is obviously no need for constructing a CompositeDevice. Instead, use the device in the WorkCell directly.

In lines 32-35 a CollisionStrategy is created. We base this on the PQP strategy. Remember to check that the returned smart pointer is not null before continuing.

In lines 36-38 a CollisionDetector is created based on this collision strategy. The detector is then wrapped in a PlannerConstraint. Finally, a RRTPlanner is constructed based on the PlannerConstraint. The planner works for our CompositeDevice.

In lines 40-51 the initial and goal configurations are defined. Before planning, we first check that these are collision free. The CollisionDetector uses the ProximityData structure to speed up collision detection by performing caching inbetween calls to inCollision.

Finally, in line 43-56, we do the actual pathplanning query. The result will be stored in the PathQ object, and we print the size of the path.

LUA API References

There is currently no separate API documentation for the LUA interface. Instead, see the references for Python and Java in API References. These are very similar to the LUA interface.

Standalone LUA

The code above will work when executed from the RobWorkStudio Lua plugin. It is also possible to execute the LUA script in a standalone script without using RobWorkStudio. In the RobWork/bin directory there will be a executable called ‘lua’ which is the LUA interpreter that can be used to execute standalone scripts. The script to execute must be given as the first argument to this program.

package.cpath = package.cpath .. ";/path/to/RobWork/libs/relwithdebinfo/lib?_lua.so"
require("rw")
require("rw_pathplanners")
require("rw_proximitystrategies")

function openpackage (ns)
  for n,v in pairs(ns) do
    if _G[n] ~= nil then
      print("name clash: " .. n .. " is already defined")
    else
      _G[n] = v
    end
  end
end

openpackage(rw)
openpackage(rw_pathplanners)
openpackage(rw_proximitystrategies)

-- then all of the above

You must tell LUA where to find the native RobWork libraries. This is done in the first line by appending to the LUA cpath. In line 2-4 the necessary native libraries are loaded. The question mark in line 1 is automatically substituted by the names given to require.

Lines 6 to 18 is optional. By default, the imported package functions are refered to by scoped names, such as ‘rw_pathplanners.RRTPlanner’. By using the openpackage function, all these scoped names are moved to the global table. That means you can refer directly to the RobWork types, for instance with ‘RRTPlanner’ instead of the longer ‘rw_pathplanners.RRTPlanner’. To just import a single type to global namespace, you could instead specify

local RRTPlanner = rw_pathplanners.RRTPlanner

After this initial import of the native libraries, the script in the LUA section can be run.

API References

Below is a list of relevant API references to the types used in the example, in the order they are used. Use this if you want to know more about options and finetuning of the algorithms.

C++ API	Python API Reference	Javadoc
rw::loaders::WorkCellLoader::Factory	rw.WorkCellLoaderFactory	org.robwork.rw.WorkCellLoaderFactory
rw::models::Device::Ptr	rw.DevicePtr	org.robwork.rw.DevicePtr
rw::kinematics::State	rw.State	org.robwork.rw.State
rw::models::CompositeDevice	rw.CompositeDevice	org.robwork.rw.CompositeDevice
	rw.CompositeDevicePtr	org.robwork.rw.CompositeDevicePtr
rwlibs::proximitystrategies::ProximityStrategyFactory	rw_proximitystrategies.ProximityStrategyFactory	org.robwork.rw_proximitystrategies.ProximityStrategyFactory
rw::proximity::CollisionDetector	rw.CollisionDetector	org.robwork.rw.CollisionDetector
rw::pathplanning::PlannerConstraint	rw.PlannerConstraint	org.robwork.rw.PlannerConstraint
rwlibs::pathplanners::RRTPlanner	rw_pathplanners.RRTPlanner	org.robwork.rw_pathplanners.RRTPlanner
rw::math::Q	rw.Q	org.robwork.rw.Q
rw::proximity::ProximityData	rw.ProximityData	org.robwork.rw.ProximityData
rw::trajectory::QPath	rw.PathQ	org.robwork.rw.PathQ