mc_rtcを用いた最初のコントローラ

このチュートリアルでは、mc_rtc用の新しいコントローラを作成する方法について説明します。今回の例では、JVRC1ロボットの首を左右に振るという簡単な動作にトライします。

コントローラーのスケルトンを作成して実行する

注: mc_rtcのコントローラーは、すべてMCControllerクラスから派生しています。シンプルなコントローラーを記述する場合も、このクラスから派生させて必要な機能を記述してください。このチュートリアルでもその方法を使用します。ただし、より複雑なコントローラーを作成する場合は、有限オートマトン機能を使用することを推奨します。

$ mc_rtc_new_controller --help
使用方法: mc_rtc_new_controller [-h]
                             [プロジェクトディレクトリ] [コントローラークラス名]
                             [[コントローラー名]]

新しいmc_rtcコントローラープロジェクトを作成します。

位置指定引数:
   [プロジェクトディレクトリ]   プロジェクトのパス
  [コントローラークラス名]
                        コントローラークラスの名前
  [コントローラー名]     コントローラーの名前。デフォルトではコントローラークラス名が使用されます。


オプションの引数:
  -h, --help            このヘルプメッセージを表示して終了します。

$ mc_rtc_new_controller my_first_controller MyFirstController

$ mkdir -p build
$ cd build
# このビルドタイプは、デバッグ可能かつパフォーマンスに優れたコードを生成します
$ cmake ../ -DCMAKE_BUILD_TYPE=RelWithDebInfo
$ make
$ sudo make install

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

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

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)

from .my_first_controller import MyFirstController

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

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

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

スケルトンコードをひとつずつ理解する

まず、コンストラクターを見てみましょう。

• C++
• Python

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

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

これらは非常に似ています。ここでは、基底クラスの既存のオブジェクトを使用して、基本的なコントローラーをセットアップします。

1. 接触面制約条件を追加します。これで、接触面を設定した後にその接触面が動かなくなります。
2. キネマティクス制約条件を追加します。これで、関節の位置と速度の制限に従ってロボットが動くようになります。
3. ロボットの姿勢を制御するための姿勢制御タスクを追加します。
4. 接触面を設定します。今回は力学的制御は行わないため、空の接触面セットを設定します。

次に、reset関数を見てみましょう。この関数は、コントローラーの起動時（インターフェイスによって起動されたときや、コントローラーがオンラインになったとき）に呼び出されます。

• C++
• Python

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

def reset_callback(self, data):
    pass

ここでは、単純にMCControllerクラスにデリゲートします（Pythonの場合は暗黙的にデリゲートされます）。 reset_data reset_dataには、インターフェイスによって与えられたロボットの初期状態が格納されています。デフォルトの実装では、ロボットが正しく初期化されたかどうかが確認され、姿勢制御タスクの目標として現在のロボットの姿勢が正しく設定されたかどうかが確認されます。

次に、run関数が定義されています。この関数は、コントローラーのループ処理が実行されるたびに呼び出されます。

• C++
• Python

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

def run_callback(self):
    return True

ここでも、MCControllerクラスにデリゲートしています（先ほどと同じく、Pythonの場合は暗黙的にデリゲートされます）。この関数は、すべての処理が正しく実行された場合はtrueを返し、制御が中断された場合はfalseを返します。デフォルトの実装では、ユーザーのプログラムによって指定されたタスクと制約条件と共に二次計画法ソルバーが実行され、処理の結果得られた加速度を使用してロボットの目標状態が更新されます。

そして、コードの最後でコントローラーがmc_rtcに読み込まれます。

• C++
• Python

// いかなる名前空間にも属さない
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)

この部分を修正する必要はおそらくありませんが、コメントに記載された点に注意してください。

首を左右に振る

まず、2つのプロパティをコントローラーに追加します。一つは、動かそうとしている首関節のインデックスを保持するプロパティ、もう一つは、首をどの方向に動かそうとしているかを示すプロパティです。

• C++
• Python

// MyFirstController.hのプライベートメンバーに追加する
int jointIndex = 0;
bool goingLeft = true;

# MyFirstControllerの__init__メソッド内に追加する
self.jointIndex = 0
self.goingLeft = True

関節のインデックスを初期化するには、ロボットのクラスを調べる必要があります。JVRC1では、首のヨー方向の関節名はNECK_Yとなっています。そこで、コントローラーのコンストラクターで以下のコードを実行します。

• C++
• Python

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

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

次に、ロボットの顔を左と右のどちらに向かせるかに応じて目標を更新する関数を記述します。

• C++
• Python

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

注: postureTask->posture()で現在の目標を取得し、postureTask->posture(new_posture)でその目標を変更して全身の姿勢の目標を更新することもできます。ただし、ここでは人間にとってより読みやすいバージョンを使用しました。

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

import mc_rbdyn

# 以下のロボットモジュールの名前を自分のロボットのものに変える
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()
{
  // 以下のロボットモジュールの名前を自分のロボットのものに変える
  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;
}