Your first controller with mc_rtc

The goal of this tutorial is to show you how to create a new controller for mc_rtc. We will use the JVRC1 robot in our example and we will simply try to shake the head left and right

Creating and running the controller skeleton

Note: in mc_rtc, every controller derives from the MCController class. To write a simple controller you can also derive from it and write the required functionnality. This is what we will do in these tutorials. However, for more complex controllers we advise you to use the FSM facilities

We will use mc_rtc_new_controller provided by mc_rtc to setup a new controller project:

$ mc_rtc_new_controller --help
usage: mc_rtc_new_controller [-h]
                             [project directory] [controller class name]
                             [[controller name]]

Create a new mc_rtc Controller project

positional arguments:
  [project directory]   Path of the project
  [controller class name]
                        Name of the controller class
  [controller name]     Name of the controller, defaults to controller class
                        name

optional arguments:
  -h, --help            show this help message and exit

Note: this tool requires Git for Python which is available as python-git in Debian-like systems and GitPython in pip

In this tutorial we will create a tutorial controller named MyFirstController, so we can use the following command:

$ mc_rtc_new_controller my_first_controller MyFirstController

Going into the newly created my_first_controller folder we can see some files have been automatically generated:

CMakeLists.txt
A minimal CMake file to build your controller
etc/MyFirstController.yaml
Your controller configuration file see this tutorial for more details
src/CMakeLists.txt
Describe the source files required to build your controller
src/api.h
Declaration to make sure your controller will be loadable on all platforms
src/MyFirstController.h
Declare your controller class. It must inherits from mc_control::Controller and override at least the run function and the reset function
src/MyFirstController.cpp
Implement your controller. We will go over this in more details in the next sections

Building the controller

This is done using CMake and your usual tool to run CMake, build the code and install it. Typically, on Linux/MacOS:

$ mkdir -p build
$ cd build
# This build type provides good performance with debuggable code
$ cmake ../ -DCMAKE_BUILD_TYPE=RelWithDebInfo
$ make
$ sudo make install

Note: sudo is only required if mc_rtc is installed in a privileged directory

Running the controller

Modify your mc_rtc configuration file so that we use the JVRC1 robot and our newly installed controller:

{
  "MainRobot": "JVRC1",
  "Enabled": ["MyFirstController"]
}

Then run your controller as explained in the previous section. Congratulations, you just built and and ran your first controller!

We will start by creating a new Python package:

$ mkdir -p my_first_controller
$ touch my_first_controller/__init__.py

Note: this assumes you are using a posix shell

Then we will create a file my_first_controller.py in the my_first_controller folder:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

import mc_control
import mc_rbdyn
import mc_rtc

class MyFirstController(mc_control.MCPythonController):
    def __init__(self, rm, dt):
        self.qpsolver.addConstraintSet(self.kinematicsConstraint)
        self.qpsolver.addConstraintSet(self.contactConstraint)
        self.qpsolver.addTask(self.postureTask)
    def run_callback(self):
        return True
    def reset_callback(self, data):
        pass
    @staticmethod
    def create(robot, dt):
        env = mc_rbdyn.get_robot_module("env", mc_rtc.MC_ENV_DESCRIPTION_PATH, "ground")
        return MyFirstController([robot,env], dt)

We also need to edit __init__.py so that the controller is available in the module:

from .my_first_controller import MyFirstController

Running the controller

Modify your mc_rtc configuration file so that we use the JVRC1 robot and our new controller:

{
  "MainRobot": "JVRC1",
  "Enabled": ["Python#my_first_controller.MyFirstController"]
}

Note: if you have enforced Python 2 or Python 3 or if you have installed both then you should explicitly choose Python2 or Python3.

For this to work, we need to make sure the my_first_controller folder is on the Python path. In fact, given this configuration, mc_rtc will create your controller this way:

from my_first_controller import MyFirstController
controller = MyFirstController.create(rm, dt)
return controller

Typically, if you run mc_rtc_ticker and the my_first_controller folder is in $HOME/my_python_controllers, then you should run as follows:

$ PYTHONPATH=$HOME/my_python_controllers:$PYTHONPATH rosrun mc_rtc_ticker mc_rtc_ticker

Key difference with C++

A key difference between the C++ controller and the Python controller is that the C++ controller let you bypass the call to mc_control::MCController::run() and mc_control::MCController::reset() functions if you like. Whereas in Python, run_callback(self) and reset_callback(self, data) will always be called after its C++ counterpart.

Use the mc-rtc/new-controller template project. This is equivalent to using the mc_rtc_new_controller tool with extra goodies.

Understanding the skeleton code step-by-step

Let's first have a look at the constructor:

solver().addConstraintSet(contactConstraint);
solver().addConstraintSet(kinematicsConstraint);
solver().addTask(postureTask);
self.qpsolver.addConstraintSet(self.kinematicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)

They are very similar. We use existing objects in the base class to setup a basic controller:

  1. We add a contact constraint. This is ensuring that contacts surfaces don't move once they are set;
  2. We add a kinematics constraint. This is ensuring the robot is subject to joint position and velocity limits;
  3. We add a posture task to control the robot posture;
  4. We set the contacts. For now we set an empty set of contacts as the control does not include dynamics;

Now let's look at the reset function. This function is called when the controller is started (either when the interface starts or when the controllers are switched online).

void MyFirstController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);
}
def reset_callback(self, data):
    pass

Here we are simply delegating to the MCController class (implicitly in the Python case). reset_data contains the initial robot configuration provided by the interface. The default implementation makes sure that the robot is correctly initialized and that the posture task objective is set to the current robot's posture.

Then we have the run function. This function is going to be called for every iteration of the controller.

bool MyFirstController::run()
{
  return mc_control::MCController::run();
}
def run_callback(self):
    return True

Here we are also delegating the call to the MCController class (also implicitly in the Python case). The function should return true if everything goes well and false if the control should be interrupted. The default implementation runs the QP solver with the tasks and constraints provided by your program and use the resulting acceleration to update the desired robot's state.

Finally, the last piece of code allows mc_rtc to load your controller:

// Should not be in any namespace
CONTROLLER_CONSTRUCTOR("MyFirstController", MyFirstController)
@staticmethod
def create(robot, dt):
    env = mc_rbdyn.get_robot_module("env", mc_rtc.MC_ENV_DESCRIPTION_PATH, "ground")
    return MyFirstController([robot,env], dt)

It's unlikely you would need to modify this but it's vital not to forget it so we are mentioning it here.

Shaking the head left and right

We will start by adding two properties to our controller, one will hold the joint index for the head joint we are going to move, the other will tell us in which direction we are going.

// Added to MyFirstController.h in the private members
int jointIndex = 0;
bool goingLeft = true;
# Added inside MyFirstController __init__ method
self.jointIndex = 0
self.goingLeft = True

To initialize the joint index, we need to ask the robot class. In JVRC1, the head yaw joint is called NECK_Y, hence the following code in our controller constructors:

jointIndex = robot().jointIndexByName("NECK_Y");
self.jointIndex = self.robot().jointIndexByName("NECK_Y")

Next we will write a function to update the target accordingly if we want the robot to look left or right:

void MyFirstController::switch_target()
{
  if(goingLeft)
  {
    postureTask->target({{"NECK_Y", robot().qu()[jointIndex]}});
  }
  else
  {
    postureTask->target({{"NECK_Y", robot().ql()[jointIndex]}});
  }
  goingLeft = !goingLeft;
}
def switch_target(self):
    if self.goingLeft:
        self.postureTask.target({"NECK_Y": self.robot().qu[self.jointIndex]})
    else:
        self.postureTask.target({"NECK_Y": self.robot().ql[self.jointIndex]})
    self.goingLeft = not self.goingLeft

Note: it's also possible to update the full posture target by using postureTask->posture() to get the current target and postureTask->posture(new_posture) to change the target but we use a more readable version here.

Finally, we modify the run function to update the robot's behaviour according to the current task error:

bool MyFirstController::run()
{
  if(std::abs(postureTask->posture()[jointIndex][0] - robot().mbc().q[jointIndex][0]) < 0.05)
  {
    switch_target();
  }
  return mc_control::MCController::run();
}
def run_callback(self):
    if abs(self.postureTask.posture()[self.jointIndex][0] - self.robot().mbc.q[self.jointIndex][0]) < 0.05:
        self.switch_target()
    return True

Note: alternatively, we could monitor postureTask->eval().norm() which gives the total error of the task. However here we are only concerned with the head yaw joint.

Et voilà! You can run this controller and see that the robot is moving its head from left to right. For the next tutorial we will continue using this controller, however, the code samples will not reflect the existing head-moving code.

The full sources for this controller are available here.

Find out available joints in a robot

You might be tempted to try this controller with different robots but not knowing which joints are available in your robot, the following snippet will print out a list of one dof joint that can be used in the sample code interchangeably with NECK_Y:

import mc_rbdyn

# Here you can change the robot module name with your own robot
rm = mc_rbdyn.get_robot_module("JVRC1")
print("\n".join(["- {}".format(j.name()) for j in rm.mb.joints() if j.dof() == 1]))
#include <mc_rbdyn/RobotLoader.h>
int main()
{
  // Here you can change the robot module name with your own robot
  auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1");
  for(const auto & j : rm->mb.joints())
  {
    if(j.dof() == 1 && !j.isMimic()) { std::cout << "- " << j.name() << "\n"; }
  }
  return 0;
}
Next tutorial: Controlling the CoM