Your first controller with mc_rtc (sources)

MyFirstController.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

#pragma once

#include <mc_control/mc_controller.h>

#include "api.h"

struct MyFirstController_DLLAPI MyFirstController : public mc_control::MCController
{
  MyFirstController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config);

  bool run() override;

  void reset(const mc_control::ControllerResetData & reset_data) override;

  void switch_target();

private:
  mc_rtc::Configuration config_;
  std::string jointName = "NECK_Y";
  int jointIndex = 0;
  bool goingLeft = true;
};

MyFirstController.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

#include "MyFirstController.h"

MyFirstController::MyFirstController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController(rm, dt)
{
  jointIndex = robot().jointIndexByName(jointName);

  config_.load(config);
  solver().addConstraintSet(contactConstraint);
  solver().addConstraintSet(kinematicsConstraint);
  solver().addTask(postureTask);

  mc_rtc::log::success("MyFirstController init done");
}

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

void MyFirstController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);
}

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

CONTROLLER_CONSTRUCTOR("MyFirstController", MyFirstController)

api.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

#pragma once

#if defined _WIN32 || defined __CYGWIN__
#  define MyFirstController_DLLIMPORT __declspec(dllimport)
#  define MyFirstController_DLLEXPORT __declspec(dllexport)
#  define MyFirstController_DLLLOCAL
#else
// On Linux, for GCC >= 4, tag symbols using GCC extension.
#  if __GNUC__ >= 4
#    define MyFirstController_DLLIMPORT __attribute__((visibility("default")))
#    define MyFirstController_DLLEXPORT __attribute__((visibility("default")))
#    define MyFirstController_DLLLOCAL __attribute__((visibility("hidden")))
#  else
// Otherwise (GCC < 4 or another compiler is used), export everything.
#    define MyFirstController_DLLIMPORT
#    define MyFirstController_DLLEXPORT
#    define MyFirstController_DLLLOCAL
#  endif // __GNUC__ >= 4
#endif // defined _WIN32 || defined __CYGWIN__

#ifdef MyFirstController_STATIC
// If one is using the library statically, get rid of
// extra information.
#  define MyFirstController_DLLAPI
#  define MyFirstController_LOCAL
#else
// Depending on whether one is building or using the
// library define DLLAPI to import or export.
#  ifdef MyFirstController_EXPORTS
#    define MyFirstController_DLLAPI MyFirstController_DLLEXPORT
#  else
#    define MyFirstController_DLLAPI MyFirstController_DLLIMPORT
#  endif // MyFirstController_EXPORTS
#  define MyFirstController_LOCAL MyFirstController_DLLLOCAL
#endif // MyFirstController_STATIC

CMakeLists.txt

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

cmake_minimum_required(VERSION 3.1)

set(CXX_DISABLE_WERROR 1)
set(CMAKE_CXX_STANDARD 11)

set(PROJECT_NAME MyFirstController)
set(PROJECT_DESCRIPTION "MyFirstController")
set(PROJECT_URL "")

project(${PROJECT_NAME} CXX)

find_package(mc_rtc REQUIRED)

add_subdirectory(src)

install(FILES etc/MyFirstController.yaml
        DESTINATION "${MC_CONTROLLER_RUNTIME_INSTALL_PREFIX}/etc"
)

src/CMakeLists.txt

1
2
3
4
5
6
7
8
9
10

set(controller_SRC
  MyFirstController.cpp
)

set(controller_HDR
  MyFirstController.h
)

add_controller(${PROJECT_NAME} ${controller_SRC} ${controller_HDR})
set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DMyFirstController_EXPORTS")

my_first_controller.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

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)
        self.jointName = "NECK_Y"
        self.jointIndex = self.robot().jointIndexByName(self.jointName)
        self.goingLeft = True

    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

    def reset_callback(self, data):
        pass

    def switch_target(self):
        if self.goingLeft:
            self.postureTask.target({self.jointName: self.robot().qu[self.jointIndex]})
        else:
            self.postureTask.target({self.jointName: self.robot().ql[self.jointIndex]})
        self.goingLeft = not self.goingLeft

    @staticmethod
    def create(robot, dt):
        env = mc_rbdyn.get_robot_module("env", mc_rtc.MC_ENV_DESCRIPTION_PATH, "ground")
        return MyFirstController([robot, env], dt)

__init__.py

1

from my_first_controller import MyFirstController
Next tutorial: Controlling the CoM