Controlling the CoM (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

#pragma once

#include <mc_control/mc_controller.h>
#include <mc_tasks/CoMTask.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();

  void switch_com_target();

private:
  mc_rtc::Configuration config_;
  int jointIndex = 0;
  bool goingLeft = true;
  std::shared_ptr<mc_tasks::CoMTask> comTask;
  Eigen::Vector3d comZero;
  bool comDown = 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
35
36
37
38
39
40
41
42
43
44
45
46
47

#include "MyFirstController.h"

MyFirstController::MyFirstController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController(rm, 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()
{
  if(std::abs(postureTask->posture()[jointIndex][0] - robot().mbc().q[jointIndex][0]) < 0.05) { switch_target(); }
  if(comTask->eval().norm() < 0.01) { switch_com_target(); }
  return mc_control::MCController::run();
}

void MyFirstController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);
  comTask->reset();
  comZero = comTask->com();
}

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

void MyFirstController::switch_com_target()
{
  if(comDown) { comTask->com(comZero - Eigen::Vector3d{0, 0, 0.2}); }
  else { comTask->com(comZero); }
  comDown = !comDown;
}

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

import eigen
import mc_control
import mc_rbdyn
import mc_rtc
import mc_tasks


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.jointIndex = self.robot().jointIndexByName("NECK_Y")
        self.goingLeft = True
        self.comDown = True
        self.comZero = eigen.Vector3d.Zero()

    def run_callback(self):
        if (
            abs(
                self.postureTask.posture()[self.jointIndex][0]
                - self.robot().mbc.q[self.jointIndex][0]
            )
            < 0.05
        ):
            self.switch_target()
        if self.comTask.eval().norm() < 0.01:
            self.switch_com_target()
        return True

    def reset_callback(self, data):
        self.comTask.reset()
        self.comZero = self.comTask.com()

    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

    def switch_com_target(self):
        if self.comDown:
            self.comTask.com(self.comZero - eigen.Vector3d(0, 0, 0.2))
        else:
            self.comTask.com(self.comZero)
        self.comDown = not self.comDown

    @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 an end-effector (and loading)