マルチロボットコントローラ

なぜ複数のロボットを使用するのか

このセクションでは、1台のロボットだけでなく複数のロボットを同時に制御します。この手法を使用すると、数多くの自由度を制御する際に計算負荷が非常に高くなりますが、以下のことが可能となります。

  • ロボット間の制約条件（接触面や衝突メッシュなど）を指定する
  • 連携してタスクを実行する
  • 多関節オブジェクトを操作する

これらすべてを、インバースキネマティクスを明示的に行わずに実現できます。

これを実現するため、本フレームワークの二次計画法コントローラーは、一定の制約条件の下で、ロボットのシステム全体が考慮された目的関数の値を最小にします。

今回の例では、自由度2のシンプルなオブジェクト（ドア）の操作について見ていきます。ここでは、ドアのハンドルに向けてロボットの手を伸ばし、ハンドルを回してドアを開くという動作を実現します。

注: この記事では、読者がROSを使用してコントローラを実行・可視化できることを前提として説明します。

例

ここでは、JVRC1とドアを読み込むシンプルなコントローラーを構築します。このドアは、既にインストールされているmc_rtc_dataパッケージに含まれています。

では、複数のロボットを入力として受け取るコントローラーを構築しましょう。

  • JVRC1
  • ドア
  • 地面

これを実現するには、コンストラクターを以下のように編集します。

#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)
@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)

ロボットのインデックス

今回の新しい例では、追加のロボット（ドア）を読み込みました。その際、読み込むべきロボットモジュールのリストを指定しました。読み込まれたロボットには、モジュールを指定した順にインデックスが付けられています。すなわち、メインロボットのインデックスは0のままですが、ドアにはインデックス1が、地面にはインデックス2が付けられています。そのため、接触面の設定を以下のように変更する必要があります。

addContact({robot().name(), "ground", "LeftFoot", "AllGround"});
addContact({robot().name(), "ground", "RightFoot", "AllGround"});
self.addContact(self.robot().name(), "ground", "LeftFoot", "AllGround")
self.addContact(self.robot().name(), "ground", "RightFoot", "AllGround")

ドアの初期位置

この状態でコントローラーを起動すると、ドアとロボットが同じ場所に表示されるのが分かります。今回のチュートリアルの目的を達成するには、ドアの位置を手動で変更する必要があります。

// reset関数内
robots().robot(1).posW(sva::PTransformd(sva::RotZ(M_PI), Eigen::Vector3d(0.7, 0.5, 0)));
# resetコールバック関数内
self.robots().robot(1).posW(
    sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(0.7, 0.5, 0))
)

ドアのタスクと制約条件

新しい多関節ロボットを追加したため、このロボットにいくつかの制約条件を追加する必要があります。今回は、キネマティクス制約条件と姿勢制御タスクのみを追加します。

// ヘッダー内
std::shared_ptr<mc_solver::KinematicsConstraint> doorKinematics;
std::shared_ptr<mc_tasks::PostureTask> doorPosture;
// reset関数内
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)
# 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)

注: ドアは土台が固定されているため、この段階ではドアに接触面を追加しません。ただし、コントローラーに読み込まれているすべてのロボットに対して接触面制約条件を直接適用できます。

コントローラーのロジックを定義する

ここでは、コントローラーのロジックを以下のように定義します。

  1. ドアのハンドルに向けてロボットの手を伸ばす
  2. ドアのハンドルを回す
  3. ドアを開く

これらを反映したコードは以下のようになります。

// ヘッダー内
enum DoorPhase
{
  APPROACH = 0,
  HANDLE,
  OPEN
};
// コントローラーのプライベートプロパティ
DoorPhase phase = APPROACH;
// コントローラーの新しいメソッド
void switch_phase()
{
  if(phase == APPROACH && 0 /** この条件は後で記述する */)
  {
    /** HANDLEフェーズをセットアップする */
    phase = HANDLE;
  }
  else if(phase == HANDLE && 0 /** この条件は後で記述する */)
  {
    /** OPENフェーズをセットアップする */
    phase = OPEN;
  }
}
// run関数内でこれを呼び出す
bool MyFirstController::run()
{
  switch_phase();
  return mc_control::MCController::run();
}
# 定数を宣言する
APPROACH = 0
HANDLE = 1
OPEN = 2
# コンストラクター内
self.phase = APPROACH


# コントローラーの新しいメソッド
def switch_phase(self):
    if self.phase == APPROACH and False:  # この条件は後で記述する
        # HANDLEフェーズをセットアップする
        self.phase = HANDLE
    elif self.phase == HANDLE and False:  # この条件は後で記述する
        # OPENフェーズをセットアップする
        self.phase = OPEN


# runコールバック関数内でこれを呼び出す
def run_callback(self):
    self.switch_phase()
    return True

フェーズ1: ハンドルに手を伸ばす

このフェーズでは、SurfaceTransformTaskを導入します。これは、以前に使用したEndEffectorTaskと非常によく似ていますが、このタスクはエンドエフェクターではなくロボットの表面を制御します。このタスクでは通常、目標を表すのが簡単になります。さらに、ドアの位置を動かしたり手を変更したりする際にコードを編集しなくて済むように、ドアのハンドルの位置を用いて目標を定義します。

// ヘッダー内
#include <mc_tasks/SurfaceTransformTask.h>
// プライベートメンバー内
std::shared_ptr<mc_tasks::SurfaceTransformTask> handTask;
// reset関数内
// タスクを作成してソルバーに追加する
handTask = std::make_shared<mc_tasks::SurfaceTransformTask>("RightGripper", robots(), 0, 5.0, 1000.0);
solver().addTask(handTask);
// ハンドルの位置を基準とした目標を設定する
handTask->target(sva::PTransformd(Eigen::Vector3d(0, 0, -0.025)) * robots().robot(1).surfacePose("Handle"));
# resetコールバック関数内
# タスクを作成してソルバーに追加する
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")
)

フェーズ2: ハンドルを動かす

Wフェーズ2では以下の2つの項目を設定します。

  1. フェーズ2をいつトリガーするか
  2. 何を実行するか

最初の項目については、SurfaceTransformTaskの実行を監視します。2番目の項目については、ドアとロボットのグリッパーとの間に接触面を追加し、ロボットのグリッパーに関するタスクを削除し（これは現在、接触面によって処理されています）、ハンドルの位置に関する目標を変更します。

// MyFirstController::switch_phase()を編集する
if(phase == APPROACH && handTask->eval().norm < 0.05 && handTask->speed().norm() < 1e-4)
{
  // 新しい接触面を追加する
  addContact({robot().name(), "door", "RightGripper", "Handle"});
  // 面変換タスクを削除する
  solver().removeTask(handTask);
  // ロボットの現在の姿勢を維持する
  postureTask->reset();
  comTask->reset();
  // 新しいハンドル位置を目標とする
  doorPosture->target(handle});
  // フェーズを切り替える
  phase = HANDLE;
}
# switch_phase(self)を編集する
if (
    self.phase == APPROACH
    and self.handTask.eval().norm() < 0.05
    and self.handTask.speed().norm() < 1e-4
):
    # 新しい接触面を追加する
    self.addContact(self.robot().name(), "door", "RightGripper", "Handle")
    # 面変換タスクを削除する
    self.qpsolver.removeTask(self.handTask)
    # ロボットの現在の姿勢を維持する
    self.postureTask.reset()
    self.comTask.reset()
    # 新しいハンドル位置を目標とする
    self.doorPosture.target({"handle": {-1.0}})
    # フェーズを切り替える
    self.phase = HANDLE

フェーズ3: ドアを開く

このフェーズは、前のフェーズと非常によく似ています。ロボットの手が接触して遷移が引き起こされたハンドルの位置をチェックし、ドアを開く動作に関する目標を設定します。

else if(phase == HANDLE && doorPosture->eval().norm() < 0.01)
{
  // ドア開の目標を更新する
  doorPosture->target(door});
  // Switch phase
  phase = OPEN;
}
elif self.phase == HANDLE and self.doorPosture.eval().norm() < 0.01:
    # ドア開の目標を更新する
    self.doorPosture.target({"door": [0.5]})
    # フェーズを切り替える
    self.phase = OPEN

いろいろ試してみよう

ここで、いくつかのパラメーターをいろいろと変更し、マルチロボットのアプローチのメリットを確認してみましょう。他のパラメーターは変更せずに、以下の項目を試してみてください。

  • ドアの位置を変更する
  • ドアのハンドルを回す角度を変更する
  • ドアを開く角度を変更する
  • ドアを開くのに使用するロボットの手を変更する

注: 当然ながら、設定したとおりに実行できない場合もあります。例えば、ロボットから10m離れた場所にドアを置けば、ドアのハンドルにロボットの手が届かなくなります。

有限オートマトン機能の紹介

このチュートリアルでは、mc_rtcのコントローラーについて紹介しました。次のチュートリアルでは、本フレームワークで用意されている高度なツールを使用して複雑なコントローラーのプログラミングとデバッグの仕方について説明します。これまでの説明では、非常に初歩的な方法でコントローラーのロジックをプログラミングしてきました。本フレームワークでは、有限オートマトン機能を紹介するチュートリアルや 実用的な有限オートマトンのチュートリアルで紹介されているようなシナリオを扱うための、大変強力な手段が用意されています。後者のチュートリアルでは、有限オートマトン機能を使って、今回のチュートリアルと同じ内容をプログラミングします。

このコントローラーの完全なソースは、こちらから入手できます。

次のチュートリアル: mc_rtc::Configuration 設定