XBotInterface
The XBotInterface library constitutes the high-level API used inside XBot Core for the purposes of robot control at the joint level and kinematic/dynamic modeling. As a matter of fact, it provides a framework-independent abstraction layer for any robotic platform, thus making it suitable for use also outside the XBotCore framework.
Special attention was taken in order to make it real-time safe, so that it can used inside RT frameworks like OROCOS and XBotCore itself. Moreover, it is fully integrated with the state-of-the-art Eigen3 library for linear algebra, that provides good performance and tuning capabilities for use inside a real-time loop.
The XBotInterface library it can be used for
- sense a robot joint states in terms of position, velocity, torque, impedance, ...
- read sensors that are commonly used for robot control (IMU, force-torque sensors, ...)
- send commands to a robot in terms of desired position, velocity, torque, impedance
- compute a robot kinematics (FK, jacobians, ...) and dynamics (gravity compensation, inverse dynamics, inertia matrix, ...)
Some useful additional functionalities are also offered, like:
- a lightweight console logging utility, which is completely real-time safe on Xenomai systems
- a mat file logging utility, completely real-time safe as well
The XBotInterface library is designed so as to be as generic and configurable as possible:
- it can be used with any robot which is described in terms of a URDF file plus an SRDF file for semantic description
- it can be used inside any robotic framework/middleware. Out-of-the-box implementations are available for ROS, YARP, OROCOS, and of course the XBotCore RT layer. This means that code written using XBotInterface will be very easily portable among all these frameworks, and also new ones that may come in the future!
- it can be used with any kinematics/dynamics library as a back-end. Out-of-the-box implementation are available for RBDL and iDynTree.
Moreover, the actual implementation that is used for communication with the robot and kinematics/dynamics can be selected at runtime, i.e. without the need to recompile any code!
The most atomic component is XBot::Joint, which collects all joint states and commands, and provides the corresponding getters and setters. Joints are organized into kinematic chains, which are implemented by the XBot::KinematicChain class.
Each kinematic chain of the robot (as described by the SRDF file) is parsed into an object of this class. It stores internally the vector of joints belonging to the chain, and provides chain-wise getters and setters for the robot state, so that is easy for the user to command or read a single chain of the robot. Moreover, the KinematicChain provides the ability of getting sensors attached to it. The set of all kinematic chains is organized into the main object of the library, the XBot::XBotInterface.
XBotInterface provides a view of the robot as a set of kinematic chains. Internally, it stores the vector of all chains that make up the robot. It provides robot-wise getters and setters for the robot state, and for all sensors attached to it.
The XBot::XBotInterface library acts as a simple, organized container for the robot state. The XBot::RobotInterface inherits from such a class, and adds a few method that allow for communication from/to a robot:
- sense() acquires data from the robot sensors
- move() sends commands to the robot actuators.
Depending on the actual implementation of the RobotInterface being used, these methods will perform different operations, e.g.
- reading/publishing to suitable ROS ropics
- reading/writing to suitable YARP ports
- reading/writing to suitable OROCOS ports
- reading/writing directly from the robot hardware abstraction layer as in the case of the XBotRT implementation
XBot::ModelInterface adds to XBot::XBotInterface methods for computing kinematics/dynamics quantities corresponing to the robot state described by the underlying XBotInterface object. These include:
- forward kinematics (link pose, orientation, twists, jacobians, jdotqdot terms, centre-of-mass, centroidal momentum and centroidal momentum matrix, ...)
- inverse dynamics (gravity compensation torques, inertia matrix, ...)
- forward dynamics
- ....
Before computing any of these quantities, an update() method must be called. This has the effect of updating the underlying back-end with the joint states stored inside the XBot::ModelInterface*.
Moreover, since robot and model objects derive from a common base class, it is very simple to exchange information between them. More specifically:
- a model can be synchronized to the actual state of the robot by calling a syncFrom() method on the model, and providing the robot as argument
- a model state can be used as desired state for the robot, by calling a setReferenceFrom() method on the robot and providing a model as argument.
Both these methods work also in the case that the model only represents a subset of the robot kinematic tree (e.g. just the upperbody of a humanoid robot)
A simple program that uses the XBotInterface API to read the state of the robot is the following:
#include <XBotInterface/RobotInterface.h>
#include <XBotInterface/Utils.h>
int main()
{
std::string path_to_config_file = XBot::Utils::getXBotConfig();
auto robot = XBot::RobotInterface::getRobot(path_to_config_file); // returns a shared pointer to a robot object
robot->sense();
std::cout << *robot << std::endl;
return 0;
}In this simple code, first a robot object is obtained from the factory RobotInterface::getRobot(). that takes as an argument a path to a configuration file. Assuming that such a path was set globally in the system by using the bash script
$ set_xbot_config path_to_config_file,
such a path can then be obtained in the code with the util getXBotConfig(). Then, the actual joint state is retrieved by calling sense(), and printed to the standard output.
Something a bit more complicated now, let's command the robot to go smoothly to a homing position!
#include <XBotInterface/RobotInterface.h>
#include <XBotInterface/Utils.h>
int main()
{
std::string path_to_config_file = XBot::Utils::getXBotConfig();
auto robot = XBot::RobotInterface::getRobot(path_to_config_file); // returns a shared pointer to a robot object
robot->sense();
/* Get the current robot state as a vector */
Eigen::VectorXd q_start;
robot->getJointPosition(q_start);
/* Get some homing that does not violate joint limits */
Eigen::VectorXd q_min, q_max, q_home;
robot->getJointLimits(q_min, q_max);
q_home = 0.5 * (q_min + q_max);
/* Get some joint velocity that does not violate velocity limits */
Eigen::VectorXd qdot_max;
robot->getVelocityLimits(qdot_max);
const double max_qdot = qdot_max.minCoeff() * 0.2; // [rad/s]
/* Send a smooth trajectory to the robot */
double time = 0;
const double start_time = 0;
const double dt = 0.01; // 100 Hz loop
double duration = -1;
bool homing_complete = false;
Eigen::VectorXd q_ref, qdot_ref;
while(!homing_complete)
{
XBot::Utils::FifthOrderTrajectory(start_time,
q_start,
q_home,
max_qdot,
time,
q_ref,
qdot_ref,
duration);
robot->setPositionReference(q_ref);
robot->setVelocityReference(qdot_ref);
robot->move();
homing_complete = time >= (start_time + duration);
time += dt;
usleep(dt * 1e6);
}
return 0;
}[TBD]
This section presents an overview on the most important part of the XBotInterface API. For further information, the user should consult the doxygen-generated documentation available here.
Quick reference for the API of the XBotInterface class. Since both RobotInterface and ModelInterface derive from it, everything applies to them as well except where differently specified.
XBotInterface mainly acts as an organized container for the robot state, gathering together all functionalities that are shared between robot and model in order to decrease code duplication. As such, it is not particularly useful by itself. However, obtaining an object of such a class can be done by using the default constructor, and then calling an init() function that takes as an argument a path to configuration file:
std::string path_to_cfg_file = ...;
XBot::XBotInterface robot;
robot.init(path_to_cfg_file);The most atomic component of the library is the XBot::Joint. Inside the library, a joint is characterized by
- a unique literal name (as given by the robot URDF)
- a unique numerical ID (as given by the robot joint ID map). Note that this ID must be treated as a numerical name of the joint; as such, IDs are not required to be consecutive integers. Joints with negative ID represent virtual joints, useful to model a floating-base system.
Joints provide a getter to their URDF description as given by the urdfdom package. This enables the user to obtain information such as the name of the parent/child link of the joint, ...
According the XBotInterface library, the joint state is made up of two parts: a reception (RX) part containing all fields that are an output for the joint and an input for the robot control system, and a transmission (TX) part that, conversely, contains all fields that are in input for the joint and an output for the robot control system. A detailed description follows:
Joint RX fields
- link-side position
- motor-side position
- link-side velocity
- motor-side velocity
- link-side acceleration (only accessible from ModelInterface)
- torque (applied to the child link)
- stiffness gain (impedance control)
- damping gain (impedance control)
- temperature
Joint TX fields
- motor-side position reference
- motor-side velocity reference
- torque reference
- stiffness gain
- damping gain
Appropriate getters are available (see doxygen docs) for all fields.
NOTE: impedance gains appear as both Rx (current impedance state from the joint) and Tx (desired impedance set by the user) fields. However, currently, there is only one field inside Joint which serves both purposes. This may cause an undesired behavior, since a call to sense() may overwrite the desired impedance set by the user. As a workaround, the user should set a desired impedance level right before calling move().
Some methods for getting information about the robot are available, as listed below:
-
getUrdf()returns a reference to aurdf::Model(from the urdfdom package), from which it is possible to get programmatically all information contained inside the URDF file; an analogousgetSrdf()method exists as well. In addition, it is also possible to get the path to the URDF/SRDF files, and also astd::stringcontaining such files. -
getChainNames()returns astd::vector<std::string>listing the name of all defined chains -
getEnabledJointId()returns astd::vector<int>listing all joint IDs; in the same way,getEnabledJointNames()returns astd::vector<std::string>listing all joint names. - ...
Inside XBotInterface, a robot joint state (e.g. link-side positions) can be represented in three different ways:
- as a
XBot::JointNameMap, which is a typedef for astd::unordered_map<std::string, double> - as a
XBot::JointIdMap, which is a typedef for astd::unordered_map<int, double> - as an
Eigen::VectorXd
This means that all getters and setters are compatible with these representations. Moreover, conversion methods are available:
using namespace XBot;
XBotInterface xbi; // assume that this has also been initialized with a proper config file
Eigen::VectorXd pos_vector;
JointIdMap pos_id_map;
JointNameMap pos_name_map;
xbi.getJointPosition(pos_id_map); // fills pos_id_map with joint id - joint positions pairs
xbi.mapToEigen(pos_id_map, pos_vector); // same information, represented as Eigen::VectorXd
xbi.eigenToMap(pos_vector, pos_name_map); // converts the vector into a joint name - joint positions map(KinematicChains offer exactly the same API)
These formats have their own advantages and disadvantages, as discussed below:
-
XBot::JointNameMaphas the advantage of being completely unambiguous, since each entry is clearly identified with the joint name it refers to. This means that you can define a map that contains only a subset of the robot joints (e.g. when you want to control just a small number of joints), and pass it to any setter function (likesetPositionReference()). Similarly, it is also possible to pass a map for the entire robot to a single chain. You cannot do the same with anEigen::VectorXd, since the library would not have any way of knowing which subset of the robot joints the user is referring to. On the other hand, maps are not amenable to mathematical computations, and it is hard to use them inside control code. -
XBot::JointIdMapshares the same pros and cons asXBot::JointNameMap. However, IDs are usually a less intuitive representation than names for the human user. -
Eigen::VectorXdis probably the most useful joint state format, since it can be easily used to write efficient control algorithms leveraging the power of the Eigen3 library for linear algebra. However, it is an inherently ambiguous format, since the object itself does not contain any information about the joint serialization being used. For this reason, anEigen::VectorXdobtained from an XBotInterface should never be passed to one of another kind. More safe and efficient ways of passing information between instances of the XBotInterface class exist, as it will be discussed later on.
When using this representation, it can be useful to know which place in the vector corresponds to which joint, and vice-versa. This can be done as follows:
XBot::XBotInterface xbi; // assume that this has also been initialized with a proper config file
/* Given a joint name or id, get its index inside an Eigen vector */
int idx = xbi.getDofIndex(joint_name);
idx = xbi.getDofIndex(joint_id);
/* Given the index inside an Eigen vector, get the joint */
XBot::Joint::ConstPtr joint = xbi.getJointByDofIndex(idx);
joint->getJointName() == joint_name; // returns true
joint->getJointId() == joint_id; // returns trueXBotInterface offers three ways of getting available IMU sensors attached to the robot:
-
getImu(), returning a map between the sensor name and a shared pointer to the sensor object -
getImu(std::string parent_link_name), taking as argument the name of robot link where the IMU is mounted on, and returning a shared pointer to the sensor object -
getImu(int imu_id), taking as argument the IMU id (as given by the joint id map), and returning a shared pointer to the sensor object
NOTE: the sensor name is the name of the fixed link representing the IMU sensor inside the SRDF
Similarly to the case of IMU, XBotInterface offers three ways of getting available FT sensors attached to the robot:
-
getForceTorque(), returning a map between the sensor name and a shared pointer to the sensor object -
getForceTorque(std::string parent_link_name), taking as argument the name of robot link where the FT is mounted on, and returning a shared pointer to the sensor object -
getForceTorque(int ft_id), taking as argument the FT id (as given by the joint id map), and returning a shared pointer to the sensor object
NOTE: the sensor name is the name of the fixed link representing the FT sensor inside the SRDF
RobotInterface adds a few methods to its superclass XBotInterface, in order to enable communication to/from a robot through the framework/middleware chosen by the user on the configuration file.
- A
sense()method updates all RobotInterface Rx variables with fresh data from the middleware, so that subsequent calls to all getters will provide the user with the latest data from the robot. - A
move()method collects data from Tx variables of all joints, and sends them to the robot. - The robot object stores internally an object of the ModelInterface class, which is updated with joint states during each call to
sense(), so that the user can obtain kinematic/dynamic information about the robot current state (e.g. what is the current inertia matrix, or the cartesian pose of a link, ...) - Moreover, a
setReferenceFrom()method allows the user to safely take the state from a model and set it as reference (recall from previous discussion that doing so by exchanging `Eigen::VectorXd's is unsafe).
RobotInterface adds many methods to its superclass XBotInterface, in order to make it possible for the user to get kinematic/dynamic quantities corresponding to the model current state:
- An
update()function updates the back-end (i.e. the actual engine used for computations, e.g. RBDL) with the robot state. - A
syncFrom()method allows the user to safely take the current robot state and transfer it to the model. - A lot of functions for querying forward kinematics, dynamics, ... (refer to the doxygen documentation for a complete list). All methods support both Eigen3 and KDL as I/O formatting.
The header file XBotInterface/Logger.hpp contains a simple and lightweight console logging utility, providing differently-colored messages depending on semantics (info, warnings, errors or success messages), and also severity levels. Even though more complete logging libraries exist, to the best of the author's knowledge, none of these can be used inside a real-time thread.
A useful feature of a logging library is the ability to specify the severity of each messages. For example, some message should be only displayed when debugging code, while other (like error messages) should be displayed also in production code. The Logger library defines five severity levels:
- debug, which is meant for messages that you never want to show, except when debugging a specific section of code
- low, for messages that you do not want to show in a non-verbose mode
- medium, for messages that may interest the user, also in non-verbose mode
- high, for messages that the user will absolutely want to see
- fatal, for when a fatal error happens in the system
The Logger defines four types of messages, each of which is displayed with a different color and a different header
- info, displayed in white with a bold [info] header. By default, information messages have low severity.
- warning, displayed in yellow with a bold [warning] header. By default, warnings have medium severity.
- error, displayed in red with a bold [error] header. By default, error messages have high severity.
- success, displayed in green with a bold [success] header. By default, success messages have low severity.
The simple program below shows the basic usage of the package. Notice how the library supports both a printf-like API and an ostream-based API.
#include <XBotInterface/Logger.hpp>
#include <Eigen/Dense>
using XBot::Logger;
int main(int argc, char ** argv)
{
/* The default severity level is LOW, set it to HIGH */
Logger::SetVerbosityLevel(Logger::Severity::HIGH);
/* In verbose mode prints all messages with severity >= LOW */
if(argc > 1 && std::string(argv[1]) == "--verbose")
{
Logger::SetVerbosityLevel(Logger::Severity::LOW);
}
Logger::info("Starting main with %d args... \n", argc); // low severity info message
Logger::info(Logger::Severity::HIGH, "Important information, PI = %f! \n", 3.14); // high severity info message
Logger::warning() << "Besides the prinf-like API, also methods returning ostream& are available" << Logger::endl(); // medium severity warning
Eigen::MatrixXd matrix;
matrix.setRandom(3,6);
Logger::success(Logger::Severity::HIGH) << "This way, you can print, for instance, a matrix... \n" << matrix << Logger::endl(); // medium severity warning
}user@pc-name:$ ./logger_example
user@pc-name:$ ./logger_example --verbose
[TBD]
[TBD]