Multi-robot controller (sources)

Note: api.h, CMakeLists.txt and src/CMakeLists.txt are the same as in the previous example

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

#pragma once

#include <mc_control/mc_controller.h>
#include <mc_tasks/CoMTask.h>
#include <mc_tasks/SurfaceTransformTask.h>

#include "api.h"

enum DoorPhase
{
  APPROACH = 0,
  HANDLE,
  OPEN
};

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_phase();

private:
  mc_rtc::Configuration config_;
  std::shared_ptr<mc_tasks::CoMTask> comTask;
  std::shared_ptr<mc_solver::KinematicsConstraint> doorKinematics;
  std::shared_ptr<mc_tasks::PostureTask> doorPosture;
  std::shared_ptr<mc_tasks::SurfaceTransformTask> handTask;
  DoorPhase phase = APPROACH;
};

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
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

#include "MyFirstController.h"

#include <mc_rbdyn/RobotLoader.h>

MyFirstController::MyFirstController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController({rm,
                            mc_rbdyn::RobotLoader::get_robot_module("env",
                                                                    std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH)
                                                                        + "/../mc_int_obj_description",
                                                                    std::string("door")),
                            mc_rbdyn::RobotLoader::get_robot_module("env",
                                                                    std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH),
                                                                    std::string("ground"))},
                           dt)
{
  config_.load(config);
  solver().addConstraintSet(contactConstraint);
  solver().addConstraintSet(dynamicsConstraint);
  solver().addTask(postureTask);
  addContact({robot().name(), "ground", "LeftFoot", "AllGround"});
  addContact({robot().name(), "ground", "RightFoot", "AllGround"});
  comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0, 10.0, 1000.0);
  solver().addTask(comTask);
  postureTask->stiffness(1);

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

bool MyFirstController::run()
{
  switch_phase();
  return mc_control::MCController::run();
}

void MyFirstController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);
  comTask->reset();
  robots().robot(1).posW(sva::PTransformd(sva::RotZ(M_PI), Eigen::Vector3d(0.7, 0.5, 0)));
  doorKinematics = std::make_shared<mc_solver::KinematicsConstraint>(robots(), 1, solver().dt());
  solver().addConstraintSet(*doorKinematics);
  doorPosture = std::make_shared<mc_tasks::PostureTask>(solver(), 1, 5.0, 1000.0);
  solver().addTask(doorPosture);
  handTask = std::make_shared<mc_tasks::SurfaceTransformTask>("RightGripper", robots(), 0);
  solver().addTask(handTask);
  handTask->target(sva::PTransformd(Eigen::Vector3d(0, 0, -0.025)) * robots().robot(1).surfacePose("Handle"));
}

void MyFirstController::switch_phase()
{
  if(phase == APPROACH && handTask->eval().norm() < 0.05 && handTask->speed().norm() < 1e-4)
  {
    // Add a new contact
    addContact({robot().name(), "door", "RightGripper", "Handle"});
    // Remove the surface transform task
    solver().removeTask(handTask);
    // Keep the robot in its current posture
    postureTask->reset();
    comTask->reset();
    // Target new handle position
    doorPosture->target({{"handle", {-1.0}}});
    // Switch phase
    phase = HANDLE;
  }
  else if(phase == HANDLE && doorPosture->eval().norm() < 0.01)
  {
    // Update door opening target
    doorPosture->target({{"door", {0.5}}});
    // Switch phase
    phase = OPEN;
  }
}

CONTROLLER_CONSTRUCTOR("MyFirstController", MyFirstController)

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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

import eigen
import math
import mc_control
import mc_rbdyn
import mc_rtc
import mc_solver
import mc_tasks
import sva

APPROACH = 0
HANDLE = 1
OPEN = 2


class MyFirstController(mc_control.MCPythonController):
    def __init__(self, rm, dt):
        self.qpsolver.addConstraintSet(self.dynamicsConstraint)
        self.qpsolver.addConstraintSet(self.contactConstraint)
        self.qpsolver.addTask(self.postureTask)
        self.addContact(self.robot().name(), "ground", "LeftFoot", "AllGround")
        self.addContact(self.robot().name(), "ground", "RightFoot", "AllGround")
        self.comTask = mc_tasks.CoMTask(self.robots(), 0, 10.0, 1000.0)
        self.qpsolver.addTask(self.comTask)
        self.postureTask.stiffness(1)
        self.phase = APPROACH

    def run_callback(self):
        self.switch_phase()
        return True

    def reset_callback(self, data):
        self.comTask.reset()
        self.robots().robot(1).posW(
            sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(0.7, 0.5, 0))
        )
        self.doorKinematics = mc_solver.KinematicsConstraint(
            self.robots(), 1, self.qpsolver.timeStep
        )
        self.qpsolver.addConstraintSet(self.doorKinematics)
        self.doorPosture = mc_tasks.PostureTask(self.qpsolver, 1, 5.0, 1000.0)
        self.qpsolver.addTask(self.doorPosture)
        self.handTask = mc_tasks.SurfaceTransformTask(
            "RightGripper", self.robots(), 0, 5.0, 1000.0
        )
        self.qpsolver.addTask(self.handTask)
        self.handTask.target(
            sva.PTransformd(eigen.Vector3d(0, 0, -0.025))
            * self.robots().robot(1).surfacePose("Handle")
        )

    def switch_phase(self):
        if (
            self.phase == APPROACH
            and self.handTask.eval().norm() < 0.05
            and self.handTask.speed().norm() < 1e-4
        ):
            # Add a new contact
            self.addContact(self.robot().name(), "door", "RightGripper", "Handle")
            # Remove the surface transform task
            self.qpsolver.removeTask(self.handTask)
            # Keep the robot in its current posture
            self.postureTask.reset()
            self.comTask.reset()
            # Target new handle position
            self.doorPosture.target({"handle": [-1.0]})
            # Switch phase
            self.phase = HANDLE
        elif self.phase == HANDLE and self.doorPosture.eval().norm() < 0.01:
            # Update door opening target
            self.doorPosture.target({"door": [0.5]})
            # Switch phase
            self.phase = OPEN

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

__init__.py

1

from my_first_controller import MyFirstController
Next tutorial: mc_rtc::Configuration general purpose configuration