Robotics Library

Application Programming Interface

The complete class hierarchy and API is available as a Doxygen documentation. In this page, we give a rather high-level description of the component architecture and leave out the details defined in Doxygen. For each component of RL, a few example function calls are given.

Robotics Library Components
The Robotics Library is structured as a hierarchical set of components. Interfaces for motion planning are built on top of basic mathematics functions, kinematics and dynamics calculation, scene graph abstraction, hardware abstraction, XML parsing, and utility functions.

rl::math

Basic mathematic functions are provided by this part of RL. Vector and matrix algebra uses Eigen 3 with convenient type definitions for the specified real representation (with double specified as default in Real.h).

typedef double rl::math::Real;
typedef Eigen::Matrix< rl::math::Real, 3, 1 > rl::math::Vector3;
typedef Eigen::Matrix< rl::math::Real, 3, 3 > rl::math::Matrix33;
typedef Eigen::Matrix< rl::math::Real, Eigen::Dynamic, 1 > rl::math::Vector;
typedef Eigen::Matrix< rl::math::Real, Eigen::Dynamic, Eigen::Dynamic > rl::math::Matrix;

Mathematical expressions are therefore specified identical to Eigen and can be used to easily describe various vector and matrix operations.

rl::math::Vector a(3); // dynamic size vector
a << 1.0f, 2.0f, 3.0f;
rl::math::Real b = 2.0f;
rl::math::Vector3 c = a * b; // fixed size vector

Operations involving 3D rotations, quaternions, transformations are supported through Eigen as well.

rl::math::Matrix33 r(
	rl::math::AngleAxis(90.0f * rl::math::DEG2RAD, rl::math::Vector3::UnitZ()) *
	rl::math::AngleAxis(0.0f * rl::math::DEG2RAD, rl::math::Vector3::UnitY()) *
	rl::math::AngleAxis(90.0f * rl::math::DEG2RAD, rl::math::Vector3::UnitX())
);
rl::math::Transform t(r);
rl::math::Vector3 xyz = t.rotation().eulerAngles(2, 1, 0).reverse();
rl::math::Quaternion q(r);
r = q.toRotationMatrix();

Additional types for spatial vector algebra are provided as well. Spatial vector algebra combines angular and linear components and enables the writing of efficient formulas, e.g., for dynamics algorithms in rl::mdl. Provided are classes for spatial transformations, motion and force vectors, as well as rigid and articulated body inertia. More examples can be found in rlSpatialDemo.

rl::math::PlueckerTransform x;
x.rotation().setIdentity();
x.translation() << 1, 2, 3;
rl::math::MotionVector v1;
v1.angular() << 1, 2, 3;
v1.linear() << 1, 2, 3;
rl::math::MotionVector v2 = x * v2;

SI units are used throughout the library and constants are available to conveniently translate between different units and scalings.

rl::math::Real radians = 90.0f * rl::math::DEG2RAD; // degrees to radians
rl::math::Real nanoseconds = 1.0f * rl::math::UNIT2NANO; // seconds to nanoseconds
rl::math::Unit distance = rl::math::UNIT_METER;

Interpolation between robot configurations and poses can be calculated via various interpolation algorithms. The following code lines perform an interpolation with a third order polynomial from a start position of 30° to a goal position of 45° within 2 seconds.

rl::math::Cubic< rl::math::Real > interpolator;
interpolator.te = 2.0f; // duration
interpolator.x0 = 30.0f * rl::math::DEG2RAD; // start position
interpolator.v0 = 0.0f * rl::math::DEG2RAD; // start velocity
interpolator.xe = 45.0f * rl::math::DEG2RAD;; // goal position
interpolator.ve = 0.0f * rl::math::DEG2RAD; // goal velocity
interpolator.interpolate();

The interpolated positions and velocities can be calculated in a loop at the required timesteps. For further examples see rlInterpolatorDemo and rlTrapezoidalVelocityDemo.

rl::math::Real xi = interpolator.x(ti); // position at time i
rl::math::Real vi = interpolator.v(ti); // velocity at time i

rl::xml

This component serves as a C++ abstraction layer for the XML library libxml2. It offers support for XPath (XML Path Language) and provides convenient parsing of XML files. Further information on this topic can be found in the XPath Tutorial.

In order to work on an XML document, a parser needs to be created. This parser can then be used to load a document given a filename or a string in memory.

rl::xml::DomParser parser;
rl::xml::Document doc = parser.readFile("foo.xml");

Validating a document against a given XML schema is also possible.

rl::xml::Schema schema("schema.xsd");
bool isValid = schema.validate(doc);

In order to use XPath queries on the document, a corresponding object needs to be created for the document.

rl::xml::Path path(doc);

The following queries refer to this example file.

<name>Foo</name>
<world>
	<rotation>
		<x unit="deg">0</x>
		<y unit="deg">90</y>
		<z unit="deg">0</z>
	</rotation>
	<translation>
		<x>0</x>
		<y>0</y>
		<z>2</z>
	</translation>
</world>

The access to the result value depends on the specific XPath expression. In order to read a string value of a node, the following syntax is used.

std::string name = path.eval("string(/name)").getStringval();

Existance of a specific node can be tested with this expression and results in a boolean value.

bool hasWorld = path.eval("count(/world) > 0").getBoolval();

A floating point value can be accessed in the following manner.

float x = path.eval("number(/world/rotation/x)").getFloatval();

A specific node can also be retrieved if desired. Node attributes can then be tested and accessed directly.

rl::xml::Node y = path.eval("/world/rotation/y").getNodeTab(0);
bool yHasUnit = y.hasAttribute("unit");
std::string yUnit = y.getAttribute("unit").getValue();

The XPath expression for reading the string value of an attribute on the other hand looks like this.

std::string zUnit = path.eval("string(/world/rotation/z/@unit)").getStringval();

rl::util

This part of RL provides operating system independent functions for accessing high-precision timer data, threads, and thread synchronization structures (mutexes and semaphores).

In order to access the current time in seconds, the following function call can be used. Depending on the underlying operating system, the given precision will be either in nanoseconds (Linux) or microseconds (Windows).

rl::math::Real timestamp = rl::util::Timer::now(); // current timestamp in seconds

The duration between two timestamps can also be calculated by using the following structure.

rl::util::Timer timer;
timer.start(); // initial timestamp
timer.stop(); // final timestamp
rl::math::Real duration = timer.elapsed(); // difference between timestamps in seconds

A thread can be specified by inheriting from the abstract class Thread and implementing the pure virtual function run().

class MyThread : public rl::util::Thread
{
public:
	void run()
	{
		std::cout << "MyThread::run()" << std::endl;
	}
}

The new thread can then be started and joined in the following manner. For further information see the rlThreadsDemo example.

MyThread thread;
thread.start();
thread.join();

Synchronization between threads can be achieved by using the Mutex structure, which can be locked and unlocked. A call to attempt locking the mutex without blocking is available as well.

rl::util::Mutex mutex;
mutex.lock(); // or mutex++
mutex.unlock; // or mutex--
bool isLocked = mutex.tryLock();

In order to automatically unlock the mutex upon leaving the current scope, the following structures are available.

rl::util::MutexLocker lock(&mutex);
rl::util::MutexTryLocker tryLock(&mutex);

In addition to mutexes, a semaphore structre is also provided and can be used in a similar fashion.

rl::util::Semaphore semaphore;
semaphore.acquire(); // or semaphore++
semaphore.release(); // or semaphore--
bool isAcquired = semaphore.tryAcquire();

rl::hal

The abstraction of different actuators and sensors within a device hierarchy is handled by the hardware abstraction layer. Devices are categorized in order to be able to exchange hardware components with similar properties, but different interfaces. Available categories include robot controllers, force/torque and range sensors, or cameras.

A device is accessed by opening the connection (e.g., serial or socket), starting the device (e.g., opening brakes), continuosly synchronizing the device state for the duration of execution (while reading and writing program-specific data), and finally stopping the device and closing the communication channel. The following example reads distance data from a range sensor once in this fashion.

rl::hal::SickLms200 laser;
laser.open(); // connect to device
laser.start(); // activate device
laser.step(); // synchronize with device
rl::math::Vector data(laser.getDistancesCount());
laser.getDistances(data); // read current data
laser.stop(); // deactivate device
laser.close(); // disconnect from device

Abstraction for common communication protocols such as TCP/UDP sockets or serial communication is available as well.

rl::hal::TcpSocket tcp;
tcp.open();
char buffer[256];
tcp.write(buffer, 256);
tcp.read(buffer, 256);
tcp.close();

Signed and unsigned types of specific sizes are defined identical across operating systems. In order to conveniently translate between different Endian representations (host, little, big), a number of corresponding function calls can be used to construct (double) words and access low and high bytes.

uint8_t highByte = 0x00;
uint8_t lowByte = 0xFF;
uint16_t word = hostEndianWord(highByte, lowByte);
hostEndianToBigEndian(word);
highByte = highByteFromBigEndian(word);
lowByte = lowByteFromBigEndian(word);

rl::mdl

This component provides kinematics and dynamics calculations based on spatial vector algebra with support for branches. Supported are various joint types (e.g., revolute, prismatic, cylindrical, helical) and algorithms (e.g., forward kinematics, inverse dynamics via recurisve Newton-Euler, and forward dynamics via articulated-body).

Models can be constructed and modified during runtime. A matching XML format is available in order to conventiently load existing models. A few examples can be found in the examples/rlmdl folder.

rl::mdl::XmlFactory factory;
rl::mdl::Model* model = factory.create("model.xml");
delete model;

A model stores the current state (position, velocity, acceleration, torque) of the robot. These variables can be retrieved and modified via matching get and set functions.

rl::math::Vector q(model->getDof());
model->getPosition(q);
rl::math::Vector qd(model->getDof());
model->getVelocity(qd);
rl::math::Vector qdd(model->getDof());
model->getAcceleration(qdd);
rl::math::Vector tau(model->getDof());
model->getTorque(tau);

Additional function calls for kinematics and dynamics algorithms are available in the corresponding subclasses.

rl::mdl::Kinematic* kinematic = dynamic_cast< rl::mdl::Kinematic* >(model);
rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model);

Executing an algorithm modifies the current state of the model. The results can be accessed using the appropriate get and set functions. A calculation of forward position kinematics requires the desired joint configuration as input. The corresponding operational frames can be accessed after calling the function.

kinematic->setPosition(q); // input value
kinematic->forwardPosition(); // modify model
const rl::math::Transform& t = kinematic->getOperationalPosition(0); // output value

The calculation of operational velocity values given joint positions and velocities can be performed in a similar fashion.

kinematic->setPosition(q); // input value
kinematic->setVelocity(qd); // input value
kinematic->forwardVelocity(); // modify model
const rl::math::MotionVector& v = kinematic->getOperationalVelocity(0); // output value

If required, the full Jacobian for a given joint position can be calculated as well. Have a look at rlInversePositionDemo in order to see how to perform an inverse kinematics calculation.

kinematic->setPosition(q); // input value
kinematic->calculateJacobian(); // modify model
const rl::math::Matrix& j = kinematic->getJacobian(); // output value

In order to translate a trajectory of joint positions, velocities, and acceleration into corresponding torque values, the recursive Newton-Euler algorithm is available with the following steps. A full example is given in rlDynamics1Demo, together with a forward dynamics calculation via the articulated-body algorithm.

dynamic->setPosition(q); // input value
dynamic->setVelocity(qd); // input value
dynamic->setAcceleration(qdd); // input value
dynamic->inverseDynamics(); // modify model
dynamic->getTorque(tau); // output value

The full mass matrix, centrifugal and Coriolis vector, gravity vector, and operational mass matrix can also be calculated. For a more detailed example see rlDynamics2Demo with a comparison between the two approaches.

dynamic->setPosition(q); // input value
dynamic->calculateMassMatrix(); // modify model
const rl::math::Matrix& m = dynamic->getMassMatrix(); // output value

rl::kin

rl::sg

This compoment provides a scene graph abstraction layer around frameworks such as Open Inventor or Bullet Physics. Such a scene consists of several models (e.g., a robot, an obstacle) with multiple bodies (e.g., a robot link). Each body can contain a number of shapes (e.g., box, sphere, or polygon) and is positioned via a transformation in world coordinates. A shape uses a local transformation to its parent body.

A scene can be constructed and modified during runtime. Geometry data is imported via VRML files and an XML format is available in order to define the hierarchy of models and bodies. Examples can be found in the examples/rlsg folder.

rl::sg::so::Scene scene;
scene.load("scene.xml");

The different members of a scene graph can be accessed via corresponding get functions.

rl::sg::Model* model = scene.getModel(0);
rl::sg::Body* body = model.getBody(0);
rl::sg::Shape* shape = body.getShape(0);

A framework with the capability to perform basic collision tests inherits from the abstract class SimpleScene and offers additional functionality. See rlCollisionDemo for additional information.

if (rl::sg::SimpleScene* simpleScene = dynamic_cast< rl::sg::SimpleScene* >(&scene))
{
	bool isColliding = simpleScene->isColliding(); // test all objects
	bool areColliding = simpleScene->areColliding(body1, body2);
}

In order to perform distance computation between bodies, the framework needs to inherit from the abstract class DistanceScene. Queries between a point in space and all object are supported as well.

if (rl::sg::DistanceScene* distanceScene = dynamic_cast< rl::sg::DistanceScene* >(&scene))
{
	rl::math::Vector3 point1; // witness point on first object
	rl::math::Vector3 point2; // witness point on second object
	rl::math::Real distance = distanceScene->distance(body1, body2, point1, point2); // in meters
}

rl::plan

A number of standard motion planning algorithms is provided by this part. Especially the common sampling-based Probabilistic Roadmap (PRM) and Rapidly-Exploring Random Trees (RRT) planners are included in a number of variations.

The standard PRM planner can be instantiated in the following way.

rl::plan::Prm planner;

This planner requires a kinematic description of the used robot model together with a geometric description in a scene graph that is capable of performing basic collision tests. The kinematic description can be provided by either the rl::kin or rl::mdl component.

rl::plan::SimpleModel model;
if (rlkin) model.kin = kinematics; // for rl::kin kinematics
if (rlmdl) model.mdl = kinematics; // for rl::mdl kinematics
model.model = simpleScene->getModel(0); // ID of robot model
model.scene = simpleScene;

A sampling-based planner requires a corresponding sampling function. For uniform random sampling, the following class can be used. Other variants are available as well.

rl::plan::UniformSampler sampler;
sampler.model = &model;
planner.sampler = &sampler;

The PRM planner also requires and edge verification component. In this case, a recursive version is used.

rl::plan::RecursiveVerifier verifier;
verifier.delta = 1.0f * rl::math::DEG2RAD;
verifier.model = &model;
planner.verifier = &verifier;

Initial and target configuration need to be specified by providing corresponding joint configurations.

rl::math::Vector start(model.getDof());
planner.start = &start;
rl::math::Vector goal(model.getDof());
planner.goal = &goal;

The algorithm is executed by calling the following function. A complete program can be found in rlPrmDemo.

bool isSolved = planner.solve();

After succesful execution of the planning process, the joint configurations of the solution path can be accessed in the following way

rl::plan::VectorList path;
planner.getPath(path);