Tutorial
This section drives you through the different steps to create and implement a new package to control a robot that was not integrated in RKCL before. As explained here, this requires to provide 3 distinct elements to RKCL:
- A kinematic description of the robot using either a YAML or URDF description file.
- A driver implementation that matches RKCL format, to communicate with the hardware components.
- A dedicated application along with a configuration file which allows to parametrized almost everything.
This tutorial will take as an example the existing package rkcl-ur-robot to explain how one can achieve the integration and utilization of a new robot in RKCL. This package intends to provide resources for Universal Robots manipulator arms. So far, only the UR10 robot has been integrated but other models can be added in the future. The rkcl-ur-robot package also includes an application example that uses the provided model to perform a simple scenario.
In this application example, we use the existing V-REP driver to run the scenario on simulation. Implementing a new driver to communicate with a real robot is outside the scope of this tutorial. However, do not hesitate to contact us for any information on this purpose.
1. Deployment and overview of the package
Before getting to the heart of the matter, be sure to have your workspace ready to use. If not, please follow the install section before continuing. You should also download and launch the V-REP robot simulator (recently renamed CoppeliaSim) to run the scenario on a virtual environment. You can get the latest version of the software here
First, let’s start by deploying the rkcl-ur-robot package. As shown in the install section, this can be achieved by performing the following commands:
cd pid-workspace
pid deploy package=rkcl-ur-robotThis will download and install the rkcl-ur-robot package and all its dependencies. Now you can explore the content of the rkcl-ur-robot package which is stored in the pid-workspace/package folder.
The package is structured as any PID package. The CMakeLists.txt file at the root of the package declares rkcl-ur-robot as a PID package and list its dependencies. Thanks to this, the deploying process that we just performed was able to get and link the rkcl-ur-robot package with the proper versions of the dependencies.
What interests us in this package is located in the share and apps folders. Inside the resources sub-folder, the former contains the robot model, a V-REP scene and configuration files used to parametrize the application scenario. The latter contains C++ application files used to create executable files.
2. Robot description
The robot model is described in the file ur_models/ur10.yaml using the YAML language. The description is composed of two elements: first, the links of the robot are listed with user-selectable names. Then, the rest of the file is dedicated to the definition of the joints. They express how the previously enumerated links are connected together with different types of joints and indicate the spatial transforms between the links. It is also possible to define the limits of the robot that will be taken into account in the control process.
3. Declaring a new application
Now let’s see how to use this new robot in an application example. The C++ application file that is used to generate the executable can be found at apps/ur_simple_simu_example/main.cpp. To take it into account while building the package, the application has to be declared as a new PID component within the CMakeLists.txt file of the apps folder:
declare_PID_Component(
EXAMPLE_APPLICATION
NAME ur-simple-simu-example
DIRECTORY ur_simple_simu_example
RUNTIME_RESOURCES ur_models ur_config ur_logs
DEPEND
rkcl-ur-robot/rkcl-ur-robot
rkcl-driver-vrep/rkcl-driver-vrep
rkcl-otg-reflexxes/rkcl-otg-reflexxes
pid-os-utilities/pid-signal-manager
rkcl-app-utility/rkcl-app-utility
)
The declared component is of type EXAMPLE_APPLICATION, we assign a NAME, specify its containing DIRECTORY and list the RUNTIME_RESOURCES that are needed. Here, we specify the folders ur_models, ur_config and ur_logs containing respectively the robot description file, the application configuration file and an empty repository used to store the log files. Then we declare the dependencies for this application, which is formulated as <name of the package>/<name of the library>. If you look at the src folder of the package, a PID component has been declared in the CMakeLists.txt file. This component is a HEADER_LIB which defines a component rkcl-ur-robot used to include all the RKCL libraries always used for the control of UR robots. This way, when declaring the EXAMPLE_APPLICATION above, we can directly set the rkcl-ur-robot header library as a dependency to the application, and it automatically exports all the dependencies defined in rkcl-ur-robot. Other than that, the application also requires some specific packages:
rkcl-driver-vrepis used to make the communication between the controller and the simulator.rkcl-otg-reflexxesis used to generate trajectories for the tasks.pid-signal-managerallows to handle signals.rkcl-app-utilityis a library used to ease the development of applications, as explained below.
Note that a component of type EXAMPLE_APPLICATION is used to provide an example of what has been implemented in the package (in our case the robot model) but this is not the main purpose of the package. Hence, the compilation of an EXAMPLE_APPLICATION is made optional in CMake and is not enabled by default. To change this, go to the build folder of the package (rkcl-ur-robot/build) and execute the following commands:
cmake ..
ccmake ..This will open the CMake curses interface which allows to set the various CMake options for this project. Change the value of BUILD_EXAMPLES to ON.
4. Implementing the application
Once the application is declared, the next step is to implement a C++ file containing the main() function. The file should be placed inside the DIRECTORY specified in the component declaration. Let’s open the file apps/ur_simple_simu_example/main.cpp to see how the application is implemented.
4.1 Including the headers
The file starts with some header inclusions:
#include <rkcl/robots/ur.h>
#include <rkcl/processors/app_utility.h>
#include <rkcl/processors/task_space_otg.h>
#include <rkcl/drivers/vrep_driver.h>
#include <rkcl/processors/vrep_visualization.h>
#include <pid/signal_manager.h>This allows to use the various RKCL libraries to define the controller for the UR10 robot. The last inclusion specific to PID is required to be able to catch the interrupt signal (ctrl+c) to properly stop the application.
4.2 Objects instantiation and initialization
Now, let’s go inside the main function. The first step consists in the instantiation and initialization of RKCL objects:
rkcl::DriverFactory::add<rkcl::VREPMainDriver>("vrep_main");
rkcl::DriverFactory::add<rkcl::VREPJointDriver>("vrep_joint");
rkcl::QPSolverFactory::add<rkcl::OSQPSolver>("osqp");
auto conf = YAML::LoadFile(PID_PATH("ur_config/ur10_simu.yaml"));
auto app = rkcl::AppUtility::create<rkcl::ForwardKinematicsRBDyn>(conf);
rkcl::TaskSpaceOTGReflexxes task_space_otg(app.getRobot(), app.getTaskSpaceController().getControlTimeStep());
if (not app.init())
{
throw std::runtime_error("Cannot initialize the application");
}
app.addDefaultLogging();
bool stop = false;
bool done = false;
pid::SignalManager::registerCallback(pid::SignalManager::Interrupt, "stop",
[&](int) { stop = true; });Let’s break down the operations:
rkcl::DriverFactory::add<rkcl::VREPMainDriver>("vrep_main");
rkcl::DriverFactory::add<rkcl::VREPJointDriver>("vrep_joint");
rkcl::QPSolverFactory::add<rkcl::OSQPSolver>("osqp");To make RKCL as generic as possible, drivers and quadratic programming (QP) solvers, that are used to solve the inverse kinematics, all inherit from base classes defined in the rkcl-core library. Each time a new driver or solver is created, we add it to a dedicated factory which gathers all the derived components. At the beginning of the main program, it is necessary to declare the components from the factories that will be used in the application, otherwise RKCL would not find it. V-REP has a particular driver which is made opf two elements: a main driver VREPMainDriver which manages the simulation execution and one or several VREPJointDriver used to communicate (read/send data) with a specific joint group.
auto conf = YAML::LoadFile(PID_PATH("ur_config/ur10_simu.yaml"));
auto app = rkcl::AppUtility::create<rkcl::ForwardKinematicsRBDyn>(conf);
rkcl::TaskSpaceOTGReflexxes task_space_otg(app.getRobot(), app.getTaskSpaceController().getControlTimeStep());We load and store in the variable conf the content of the configuration file. We can then create an AppUtility object using the configuration file as parameter. This object contains the main RKCL processor (forward kinematics, inverse kinematics, …) called during the control loop execution. All these processors are also configured with the configuration file. The AppUtility object is constructed with a template class for the forward kinematics processor. Here, the forward kinematics processor based on RBDyn is used.
A trajectory generator for generating task space trajectories based on Reflexxes is instantiated at the end of the block. This is the only processor called in the control loop which is not included in the AppUtility (because it is optional).
if (not app.init())
{
throw std::runtime_error("Cannot initialize the application");
}
app.addDefaultLogging();We initialize the AppUtility object. It takes care of initializing every driver and processor that it manages. Then, we activate the default logging in the application object, which means that all relevant data is automatically logged. We will explain later in this tutorial how to see the results of log files.
bool stop = false;
bool done = false;
pid::SignalManager::registerCallback(pid::SignalManager::Interrupt, "stop",
[&](int) { stop = true; });We define two boolean variables: stop is a flag that becomes true whenever the user wants to interrupt the application (ctrl+c). To do so, we link this variable to the interrupt signal using the PID SignalManager class. The other boolean done remains false until all tasks are completed or something wrong happens in the control process.
4.3 The control loop
Once everything has been properly declared and initialized, the robot can start operating. To do so, the program iteratively load the next task to execute specified in the configuration file. Each joint group can be either controlled in joint space or task space. In the first case, the goal is to move the robot from an initial to a desired joint configuration while in the second case the objective is defined in the Cartesian space, through the use of control points. For advanced users, different types of task space control are available (e.g force control, damping control).
std::cout << "Starting control loop \n";
app.configureTask(0);
task_space_otg.reset();
while (not stop and not done)
{
bool ok = app.runControlLoop(
[&] {
if (app.isTaskSpaceControlEnabled())
return task_space_otg();
else
return true;
});We load the first task specified in the configuration file. Then, we run the control loop using the AppUtility object. It automatically detects whether the operation has to be performed in joint or task space, and manages the successive calls to the drivers ans processors accordingly. If the task space control is enabled, we add a call to the task space trajectory generator, which is not yet managed by the AppUtility class, before calling the other processes.
if (ok)
{
done = true;
if (app.isTaskSpaceControlEnabled())
{
done &= (app.getTaskSpaceController().getControlPointsPoseErrorGoalNormPosition() < 0.01);
}
if (app.isJointSpaceControlEnabled())
{
auto joint_group_error_pos_goal = app.getRobot().getJointGroup(0)->selection_matrix * (app.getRobot().getJointGroup(0)->goal.position - app.getRobot().getJointGroup(0)->state.position);
done &= (joint_group_error_pos_goal.norm() < 0.001);
}
}
else
{
throw std::runtime_error("Something wrong happened in the control loop, aborting");
}If everything went well during the control iteration (i.e. ok = true ), we check if the current task is completed. The corresponding condition variable done is evaluated depending on the control space currently enabled. In both cases, a task is considered achieved if the current position/pose is sufficiently close to the goal.
if (done)
{
done = false;
std::cout << "Task completed, moving to the next one" << std::endl;
done = not app.nextTask();
task_space_otg.reset();
}If the operation is in progress (done = false), we keep looping until the user send the interrupt signal or the task is achieved.
When a task is completed (i.e. done = true ), we should load the next one regarding the sequence defined in the configuration file. We reset done at the beginning of a new task and reset the trajectory generator so that it is aware of the current state of the environment.
If no more tasks are available, app.nextTask() return false which set done = true again. This makes the program exit the main loop.
4.4 Ending the program
When all the tasks have been achieved, we exit the control loop and end the application:
pid::SignalManager::unregisterCallback(pid::SignalManager::Interrupt, "stop");
app.end();We unregister the interrupt signal from the PID signal manager and call the end() function from the AppUtility object in order to stop the different processes and drivers properly
5. Configuring the application
As we have just seen in the previous section, the C++ application file does not contain any information concerning the operations executed by the robot and how it behaves with respect to the environment. In fact, the same C++ application file could be used for a large majority of scenarios, and the goal of RKCL is to make the maximum parameters tunable without having to go inside the C++ code.
Now, let’s see what the configuration file is made of: open the YAML configuration file located at share/resources/ur_config/ur10_simu.yaml.
Note for those who are not familiar with YAML, this language allows a concise and clear description where whitespace indentation is used for denoting structure. It is thus crucial to take care of how the different blocks are vertically aligned to respect the desired structure.
We will detail in what follows the role of each block (node) :
app:
task_config_files:
- name: ur_config/tasks/joint_init_pos.yaml
- name: ur_config/tasks/task_init_pos.yaml
- name: ur_config/tasks/task_trans_z+.yamlThe app node configure what concerns the scenario of the application. In particular, it allows to store the set of tasks respecting the order of the list. For each task, a specific auxiliary YAML file should be created. For the current application, 3 tasks have been implemented and put in the share/resources/ur_config/tasks folder. A task either modify joint groups or control points data depending on the selected control mode (joint/task control).
robot:
control_points:
- name: tcp
body_name: tcp
priority: 1
limits:
max_velocity: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
max_acceleration: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
gains:
position_control:
proportional: [10, 10, 10, 10, 10, 10]
joint_groups:
- name: arm
is_controlled: true
priority: 1
joints: [joint1, joint2, joint3, joint4, joint5, joint6]
control_time_step: 0.008
limits:
max_velocity: [1, 1, 1, 1, 1, 1]
max_acceleration: [1, 1, 1, 1, 1, 1]The robot node is used to create and configure the control points and joint groups. In this simple application, we define only one of each but you can specify as many as you want.
For control_points, you should give a name, and specify a body_name which is the name of the body it is attached to. This name should match a body from the robot model. We can assigned to each control point a priority. In case of multiple control points, the robot’s capabilities will be exploited in a way that maximum efforts will be dedicated to solve the highest priority task while lower priority task will be treated with remaining possibilities.
The velocity and acceleration limits for the control point will be used to configure the trajectory generator in order to always satisfy these constraints. Finally, the gains used to compute the task space command for the control point are specified.
The joint_groups are also referred to using a name. We can decide to enable/disable the use of a joint group each time a new task is loaded. To do so, we set the parameter is_controlled to the desired boolean value. A priority can also be given for joint groups: if more than one group exist, the robot will try to solve all tasks using the highest priority joint group. If for one or more tasks there is some residual error, the next joint group regarding the priority will be used to compensate for the remaining error on tasks, and so on. Each joint group is composed of a set of joints among those defined in the robot model, they should be specified using the joints option. Considering that a robot can be made of different hardware components, the control_time_step for the joint group has to be set. Finally, joint limits can be specified for position/velocity/acceleration. If stated, these values overwrite existing joint constraints provided in the robot model.
joint_space_otgs:
- joint_group: arm
input_data: PreviousOutput #CurrentStateThe joint_space_otgs node allows to define the joint space trajectory generators. One instance should be defined for each joint group, otherwise the joint group would not be able to perform joint space operations. One trajectory generator should refer to one joint_group using its name. The parameters of the trajectory generator (maximum velocity/acceleration) are set using the joint group properties. This allows to generate a trajectory between the current and goal joint configuration which stays within the admissible range of motion.
A trajectory generator can be used in open or closed loop, depending of the value of input_data. If PreviousOutput is chosen, the process is open loop as it reuses the last generated output to set the new input. If CurrentState is selected, the trajectory generation takes into account the actual configuration of the robot as the new input, making the process closed-loop.
task_space_controller:
wrench_measure_enabled: false
control_time_step: 0.008As its name suggests, the task_space_controller node configure the task space controller. As explained in the RKCL main concepts page, to deal with synchronization issues in multi-robot systems, we define one main control loop which communicates with the different parts of the robot. This task space control loop has its own control rate which is defined by the control_time_step parameter. To be optimal, this value should be the same as the lowest joint group control time step. Other than that, we can can indicate if wrenches (force/torque) have to be considered in the control process.
ik_controller:
IK_type: StandardQP
QP_solver:
type: osqp
verbose: false
check_termination: 10The ik_controller node allows to configure the inverse kinematics controller. Different redundancy resolution strategies can be adopted. You can select one using the IK_type option. If you don’t know, just keep the StandardQP approach. Several library implementing QP solvers are wrapped in RKCL. You can choose which solver you want to use and parametrize specific options through the QP_solver node. It is recommended to use the osqp solver.
model:
path: ur_models/ur10.yaml
world_pose: [0, 0, 0, 0, 0, 0]The model node is used to indicate the path to the configuration file (relative to share/resources). You can use either a YAML or a URDF file format. The world_pose parameter indicates the pose of the first robot link with respect to the world frame (in this case, the pose of the base link).
joint_group_drivers:
- type: vrep_main
cycle_time: 0.008
- type: vrep_joint
joint_group: arm
control_mode: Velocity
joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]The joint_group_drivers node lists all the drivers in charge of the communication between the joint groups and the hardware components. Here is a particular case because we run the application on simulation using V-REP. This way, we should define a main driver and one joint driver for each group (there is only one here). We should specify the cycle_time for the main driver, which should correspond to the simulation time step used in V-REP. For each joint driver, we should set the control_mode (position or velocity) and indicate the name of the joints used in the V-REP in interface so that the joint group data car be read/sent properly.
logger:
log_folder: ur_logsThe logger node allows to configure the logging process. Just indicate the path to the log_folder (relative to share/resources) in which the log files should be stored.
6. Launching the application
Ok now everything is ready to use, we can build and execute the application !
Go to the build folder of the package and make sure that the BUILD_EXAMPLES option is activated using ccmake, as explained before at the beginning of this page. Then, in a terminal, type:
cmake ..
pid buildIf everything went well, the package is now built and installed in pid-workspace/install/<platform>/rkcl-ur-robot/<version>/. From now, we call this folder <install> for simplicity.
Before starting the program execution, let’s load the scene in CoppeliaSim to visualize the robot: in the CoppeliaSim interface, go to File -> Open scene… and select the scene share/resources/vrep_scenes/ur10.ttt in the package folder.
Move to the <install>/bin folder where the executable file has been generated and execute it:
./ur-simple-simu-exampleYou should see the robot moving in V-REP ! Just let it complete all the operations.
After running an the application, you can see the generated log files in <install>/share/resources/ur_logs. You can plot the data using Gnuplot, you just have to load the gnuplot files that have been generated automatically.