  # 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 of and leave out the details defined in Doxygen. For each component of RL, a few example function calls are given. 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::Transform t(r);
rl::math::Vector3 xyz = t.rotation().eulerAngles(2, 1, 0).reverse();
rl::math::Quaternion q(r);
r = q.toRotationMatrix();``````

RL extends the API of Eigen with features such as logarithm, exponential, angular velocity/acceleration, slerp, and squad for quaternions.

``````rl::math::Quaternion log = q.log();
rl::math::Quaternion exp = q.exp();
rl::math::Quaternion qd = q.firstDerivative(omega);
omega = q.angularVelocity(qd);
rl::math::Quaternion qi = q0.slerp(0.5, q1);
qi = q0.squad(0.5, a, b, q1);
``````

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 function classes. The following code lines perform an interpolation with a third order polynomial from a start position of 30° to an end position of 45° within 2 seconds.

``````rl::math::Real y0 = 30.0f * rl::math::DEG2RAD; // start position y_0
rl::math::Real y1 = 45.0f * rl::math::DEG2RAD;; // end position y_1
rl::math::Real yd0 = 0.0f * rl::math::DEG2RAD; // start velocity yd_0
rl::math::Real yd1 = 0.0f * rl::math::DEG2RAD; // end velocity yd_1
rl::math::Real x1 = 2.0f; // end time x_1
rl::math::Polynomial<rl::math::Real> f = rl::math::Polynomial<rl::math::Real>::CubicFirst(y0, y1, yd0, yd1, x1);
rl::math::Polynomial<rl::math::Real> fd = f.derivative();``````

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

``````rl::math::Real yi = f(xi); // position at time x_i
rl::math::Real ydi = fd(xi); // velocity at time x_i``````

## rl::xml

This component serves as a C++ abstraction layer for the XML library libxml2 and libxslt. 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;

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

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

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

``rl::xml::Path path(document);``

The following queries refer to this example file.

``````<?xml version="1.0" encoding="UTF-8"?>
<model>
<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>
</model>``````

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(/model/name)").getValue<std::string>();``

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

``bool hasWorld = path.eval("count(/model/world) > 0").getValue<bool>();``

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

``rl::math::Real x = path.eval("number(/model/world/rotation/x)").getValue<rl::math::Real>();``

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

``````rl::xml::Node y = path.eval("/model/world/rotation/y").getValue<rl::xml::NodeSet>();
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(/model/world/rotation/z/@unit)").getValue<std::string>();``

In combination with XSLT (XSL Transformations), parameters can be defined and referenced throughout the XML document. Please have a look at the XSLT Introduction for more information on this topic.

``````<?xml version="1.0" encoding="UTF-8"?>
<xsl:stylesheet version="1.0" xmlns:xsl="http://www.w3.org/1999/XSL/Transform">
<xsl:param name="length" select="2.5"/>
<xsl:template match="/">
<model>
<world>
<translation>
<x><xsl:value-of select="0.5 * \$length"/></x>
<y>0</y>
<z><xsl:value-of select="\$length"/></z>
</translation>
</world>
</model>
</xsl:template>
</xsl:stylesheet>``````

Such a stylesheet can be loaded and applied to a document with the following commands.

``````rl::xml::Stylesheet stylesheet(document);
document = stylesheet.apply();``````

The value of an entry can then be accessed identical to the examples above.

``````rl::xml::Path path(document);
rl::math::Real x = path.eval("number(/model/world/translation/x)").getValue<rl::math::Real>();``````

## rl::util

This part of RL provides operating system independent functions for accessing high-precision timer data, threads, and thread synchronization. Instead of its own implementations for this functionality in previous versions, RL now uses C++11 structures and includes extensions for additional features (e.g., Xenomai and RTAI). Please have a look at a documentation of the Thread Support Library for more details.

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).

``````std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
std::chrono::duration<rl::math::Real>(now).count(); // current timestamp in seconds``````

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

``````std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); // initial timestamp
std::this_thread::sleep_for(std::chrono::duration<rl::math::Real>(2.6)); // sleep for 2.6 seconds
rl::math::Real duration = std::chrono::duration<rl::math::Real>(stop - start).count();``````

A function with code that should be run in a separate thread can be specified as follows and can include any number of parameters..

``````void run(const std::size_t& i, const std::size_t& s)
{
std::cout << "thread " << i << " sleeps for " << s << " seconds" << std::endl;
}``````

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

``````std::thread thread0(run, 0, 3);

In order to set the current process or thread to the highest available priority, the following commands are available.

``````rl::util::this_process::set_priority(rl::util::this_process::get_priority_max());

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.

``````std::mutex mutex;
mutex.lock(); // lock mutex, blocks if not available
mutex.unlock(); // unlock mutex
bool isLocked = mutex.try_lock(); // try to lock mutex, return if not available``````

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

``````std::lock_guard guard(mutex);
std::lock_guard guardTryLock(mutex, std::try_to_lock);``````

## 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.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::Socket::Address address = rl::hal::Socket::Address::Ipv4("localhost", 50000);
socket.open();
socket.connect();
char buffer;
socket.write(buffer, 256);
socket.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.

``````std::uint8_t highByte = 0x00;
std::uint8_t lowByte = 0xFF;
std::uint16_t word = rl::hal::Endian::hostEndianWord(highByte, lowByte);
rl::hal::Endian::hostEndianToBigEndian(word);
highByte = rl::hal::Endian::highByteFromBigEndian(word);
lowByte = rl::hal::Endian::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 recursive 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 conveniently load existing models. A few examples can be found in the `examples/rlmdl` folder.

``````rl::mdl::XmlFactory factory;
std::shared_ptr<rl::mdl::Model> model(factory.create("model.xml")); // 0.7 branch``````

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->getPosition();
rl::math::Vector qd = model->getVelocity();
rl::math::Vector qdd = model->getAcceleration();
rl::math::Vector tau = model->getTorque();``````

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

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

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& x = 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
rl::math::Vector tau = dynamic->getTorque(); // 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``````

In order to solve the inverse kinematics for a given goal frame, different IK algorithms are available in `rl::mdl::JacobianInverseKinematics` and `rl::mdl::NloptInverseKinematics`. A time limit can be specified for the query and a goal frame can be specified for each TCP in the kinematic model.

``````rl::mdl::NloptInverseKinematics ik(kinematic);
ik.duration = std::chrono::seconds(1);
ik.goals.push_back(::std::make_pair(x, 0)); // goal frame in world coordinates for first TCP
bool result = ik.solve();
rl::math::Vector solution = kinematic->getPosition();``````

## rl::kin

This part of RL features kinematics using Denavit-Hartenberg frames. It is deprecated and a transition to `rl::mdl` is recommended.

## 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;

Please note that the API on the master branch has been updated and requires using a `rl::sg::XmlFactory`.

``````rl::sg::XmlFactory factory;
rl::sg::so::Scene scene;

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 models
if (rlmdl) model.mdl = dynamic; // for rl::mdl models
model.model = simpleScene->getModel(0); // ID of robot rl::sg 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.model = &model;
planner.verifier = &verifier;``````

A nearest neighbors component is required as well. Here, a k-d tree variant is selected.

``````rl::plan::KdtreeNearestNeighbors nearestNeighbors(&model);
planner.setNearestNeighbors(&nearestNeighbors);``````

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);``````