移動マニピュレータコントローラ

このセクションのでは、mc_rtc のマルチロボットフレームワークを用いて、移動マニピュレータコントローラを実装する方法を紹介します。本例は「マルチロボットコントローラ」チュートリアルで紹介した考え方を踏襲しつつ、接触や関節を持つオブジェクトを含む移動マニピュレータへと拡張したものです。

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

例

ここでは、UR5e をメインロボット、Dingo を第2ロボット、ドアを第3ロボットとして読み込むシンプルなコントローラを構築します。これらはそれぞれ、mc_rtc のロボットモジュール mc_ur5e、mc_ur5e_description、mc_dingo として提供されています。ドアは、すでにインストールされている mc_rtc_data パッケージに含まれています。UR5e をメインロボットとして読み込むために、mc_rtc の設定ファイルを以下のように変更します。

MainRobot: UR5eFloatingBase

Enabled: MobileArmController

それでは、移動マニピュレータを入力として受け取るコントローラを構築しましょう。

  • UR5e (フローティング)
  • Dingo
  • ドア
  • 地面

コンストラクタ

#include <mc_rbdyn/RobotLoader.h>

MobileArmController::MobileArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration &config)
: mc_control::MCController(
  {
    rm,
    mc_rbdyn::RobotLoader::get_robot_module("dingo"),
    mc_rbdyn::RobotLoader::get_robot_module("env/door"),
    mc_rbdyn::RobotLoader::get_robot_module("env/ground")
  },
  dt)
@staticmethod
def create(robot, dt):
    dingo = mc_rbdyn.get_robot_module("dingo")
    door = mc_rbdyn.get_robot_module("env/door")
    ground = mc_rbdyn.get_robot_module("env/ground")
    return MobileArmController([robot, dingo, door, ground], dt)

ロボットのインデックス

この新しい例では、Dingo とドアという 2 台の追加ロボットを読み込んでいます。これは、読み込むロボットモジュールのリストを指定することで実現しています。読み込まれたロボットには、指定した順にインデックスが割り当てられます。そのため、メインロボットである UR5e はインデックス 0、Dingo はインデックス 1、ドアはインデックス 2 となります。UR5e は Dingo と接触しており、Dingo は地面上を平面的に移動する必要があります。したがって、接触設定は以下のように更新する必要があります。

// In the constructor
Eigen::Vector6d dof = Eigen::Vector6d::Ones();
dof[2] = 0.0;
dof[3] = 0.0;
dof[4] = 0.0;
double friction = mc_rbdyn::Contact::defaultFriction;
addContact({"dingo", "ground", "Base", "AllGround", friction, dof});
// In the reset function after setting up initial position
addContact({"dingo", "ur5e", "Base", "Base"});
# In the constructor
dof = eigen.Vector6d.Zero()
dof[0] = 1.0
dof[1] = 1.0
dof[5] = 1.0
friction = mc_rbdyn.Contact.defaultFriction
self.addContact(
    mc_control.Contact(
        "dingo", "ground", "Base", "AllGround", friction, dof
    )
)
# In the reset function after setting up initial position
self.addContact(mc_control.Contact("dingo", "ur5e", "Base", "Base"))

また、ドアや地面では「env」を使用しているのに対し、Dingo をフローティングベースロボットとして定義するため、コンストラクタでロボットモジュールを読み込む際には「object」を使用します。

接触自体が制約であるため、初期位置を設定した後に addContact() を呼び出す必要があります。 次のセクションで示すように posW() を使用してロボット位置を移動しても、addContact() に使用されるフレームは移動しません。 この例では、ur5e の dingo に対する変換が固定されているため問題にはなりませんが、初期オフセットがより複雑な場合にはバグが発生します。

ロボットの初期位置

この時点でコントローラを起動すると、すべてのロボットが同じ位置に配置されていることが分かります。本チュートリアルでは、UR5e、Dingo、およびドアの位置を手動で再配置します。

// In the reset function
robots().robot(0).posW(
  sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(0.0, 0.0, 0.5)));
robots().robot(1).posW(
  sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(-0.25, 0.0, 0.0)));
robots().robot(2).posW(
  sva::PTransformd(sva::RotZ(M_PI), Eigen::Vector3d(2.0, 1.0, 0)));
# In the reset callback
self.robots().robot(0).posW(
    sva.PTransformd(sva.RotZ(0.0), eigen.Vector3d(0.0, 0.0, 0.5))
)
self.robots().robot(1).posW(
    sva.PTransformd(sva.RotZ(0.0), eigen.Vector3d(0.0, 0.0, 0.0))
)
self.robots().robot(2).posW(
    sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(2.0, 1.0, 0))
)
architecture_overview

タスクと制約条件

各ロボットに対して、通常どおりタスクと制約条件を追加します。まず、UR5e ロボットに対するタスクと制約条件を追加します。

// In the header
#include <mc_tasks/SurfaceTransformTask.h>
// ...
std::shared_ptr<mc_tasks::SurfaceTransformTask> handTask_;

// In the reset function
handTask_ = std::make_shared<mc_tasks::SurfaceTransformTask>("Tool", robots(), 0);
# In the reset callback
self._handTask = mc_tasks.SurfaceTransformTask(
  "Tool", self.robots(), 0, 5.0, 1000.0
)

次に、Dingo ロボットに対するタスクを追加します。

// In the header
#include <mc_tasks/TransformTask.h>
// ...
std::shared_ptr<mc_tasks::TransformTask> dingoBaseTask_;

// In the reset function
dingoBaseTask_ =
  std::make_shared<mc_tasks::TransformTask>("base_link", robots(), 1, 2.0, 1000);
# In the reset callback
self._dingoBaseTask = mc_tasks.EndEffectorTask(
    "base_link", self.robots(), 1, 2.0, 1000.0
)

続いて、新たに追加した関節を持つロボットであるドアに対して、タスクと制約条件を追加します。

// In the header
std::unique_ptr<mc_solver::KinematicsConstraint> doorKinematics_;
std::shared_ptr<mc_tasks::PostureTask> doorPostureTask_;
// In the reset function
doorKinematics_ = std::make_unique<mc_solver::KinematicsConstraint>(robots(), 2, solver().dt());
solver().addConstraintSet(doorKinematics_);
doorPostureTask_ = std::make_shared<mc_tasks::PostureTask>(solver(), 2, 1.0, 1.0);
solver().addTask(doorPostureTask_);
# In the reset callback
self._doorKinematics = mc_solver.KinematicsConstraint(
    self.robots(), 2, self.solver().dt()
)
self.solver().addConstraintSet(self._doorKinematics)
self._doorPostureTask = mc_tasks.PostureTask(
    self.solver(), 2, 1.0, 1.0
)
self.solver().addTask(self._doorPostureTask)

最後に、衝突制約を追加します。

// In the constructor
double iDist = 0.1;
double sDist = 0.05;
addCollisions(
  "dingo", "door", {{"*", "*", iDist, sDist, 0}}
);
addCollisions(
  "ur5e", "door", {{"*", "*", iDist, sDist, 0}}
);
# In the constructor
iDist, sDist, damping = 0.1, 0.05, 0.0
self.addCollisions(
    "dingo",
    "door",
    [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
)
self.addCollisions(
    "ur5e",
    "door",
    [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
)

注: ドアは固定されたベースを持つため、この段階ではドアに接触制約を追加していません。ただし、接触制約はコントローラに読み込まれているすべてのロボットに対して直接使用可能です。

コントローラロジックの設定

このパートでは、コントローラのロジックを設定します。目標は以下のとおりです。

  1. UR5e を待機姿勢に移動し、Dingo をドアの近くまで移動させる
  2. ロボットの手でドアのハンドルに到達する
  3. ドアのハンドルを回転させる
  4. ドアを開く
  5. ドアのハンドルを離し、待機姿勢に戻る

これらを反映したコードは以下のとおりです。

// In the header
enum ControllerPhase
{
  APPROACH = 0,
  HANDLE,
  OPEN,
  DONE
};
// A private property of our controller
ControllerPhase phase_ = APPROACH;
// In the run function
bool MobileArmController::run()
{
  if(phase_ == APPROACH && 0 /** we write this condition later */)
  {
    /** Setup the HANDLE phase */
    phase_ = HANDLE;
  }
  else if(phase_ == HANDLE && 0 /** we write this condition later */)
  {
    /** Setup the OPEN phase */
    phase_ = OPEN;
  }
  else if(phase_ == OPEN && 0 /** we write this condition later */) {
    /** Setup the OPEN phase */
    phase_ = DONE;
  }
  else if (phase_ == DONE && 0 /** we write this condition later */) {
    /** Continue the DONE phase */
  }
  return mc_control::MCController::run();
}
# Declare constants
class ControllerPhase:
    APPROACH = 0
    HANDLE = 1
    OPEN = 2
    DONE = 3

# In constructor
def __init__(self, rm, dt):
    self._phase = APPROACH

# In the run callback
def run_callback(self):
    if self._phase == ControllerPhase.APPROACH and False:  # We write this condition later
        # Setup the HANDLE phase
        self._phase = ControllerPhase.HANDLE
    elif (
        self._phase == ControllerPhase.HANDLE and False
    ):  # We write this condition later
        # Setup the OPEN phase
        self._phase = ControllerPhase.OPEN
    elif self._phase == ControllerPhase.OPEN and False:  # We write this condition later
        # Continue the DONE phase
        pass
    return True

フェーズ1: UR5e を待機姿勢に移動し、Dingo をドアの近くへ移動

このフェーズでは、姿勢タスクを用いてメインロボットである UR5e をあらかじめ定義された待機姿勢に移動させます。SurfaceTransformTask との競合を避けるため、このフェーズでは SurfaceTransformTask は追加しません。

// In the reset function
solver().addTask(dingoBaseTask_);
solver().addTask(postureTask.get());
postureTask->target({{"shoulder_lift_joint", {-M_PI / 2}}});
dingoBaseTask_->target({Eigen::Vector3d(1.5, 0.0, 0.0)});
# In the reset callback
self.qpsolver.addTask(self._dingoBaseTask)
self.qpsolver.addTask(self.postureTask)
self.postureTask.target(
    {
        "shoulder_lift_joint".encode("utf-8"): [-math.pi / 2],
    }
)
self._dingoBaseTask.add_ef_pose(
    sva.PTransformd(sva.RotZ(0), eigen.Vector3d(1.5, 0.0, 0.0))
)
architecture_overview

フェーズ2: ロボットの手でドアのハンドルに到達

このフェーズでは、SurfaceTransformTask を導入します。これは、以前使用した EndEffectorTask と非常によく似ていますが、エンドエフェクタではなくロボットの表面を制御するタスクです。これにより、目標をより簡単に表現できます。さらに、ドアの位置を変更したり使用する手を切り替えたりしてもコードを修正する必要がないよう、ドアのハンドル位置を用いて目標を定義します。また、タスク間の競合を避けるため、UR5e の姿勢タスクは削除します。

// In the run function, under phase_ == APPROACH
solver().removeTask(postureTask);
handTask_->reset();
solver().addTask(handTask_);
handTask_->target(sva::PTransformd(Eigen::Vector3d(0, 0, -0.05)) * robots().robot(2).surfacePose("Handle"));
phase_ = HANDLE;
# In the run callback, under self._phase == ControllerPhase.APPROACH
self.qpsolver.removeTask(self.postureTask)
self._handTask.reset()
self.qpsolver.addTask(self._handTask)
self._handTask.target(
    sva.PTransformd(eigen.Vector3d(0.0, 0.0, -0.05))
    * self.robots().robot(2).surfacePose("Handle")
)
self._phase = ControllerPhase.HANDLE
architecture_overview

フェーズ3: ドアのハンドルを回転

このフェーズでは、以下の 2 点を設定する必要があります。

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

1 点目については、SurfaceTransformTask の実行状況を監視します。2 点目については、ドアとロボットのグリッパーとの間に接触を追加し、グリッパーに対するタスクを削除します（以降は接触によって処理されます）。また、ハンドル位置に関する目標を変更します。

// In the run function, under phase_ == HANDLE
addContact({"ur5e", "door", "Tool", "Handle"});
solver().removeTask(handTask_);
postureTask->reset();
doorPostureTask_->target({{"handle", {-1.0}}});
# In the run callback, under self._phase == ControllerPhase.HANDLE
self.addContact(mc_control.Contact("ur5e", "door", "Tool", "Handle"))
self.qpsolver.removeTask(self._handTask)
self.postureTask.reset()
self._doorPosture.target(
    {
        "handle".encode("utf-8"): [-1.0],
    }
)
self._phase = ControllerPhase.OPEN
architecture_overview

フェーズ4: ドアを開く

このフェーズは前のフェーズと非常によく似ています。到達したハンドル位置を確認して遷移をトリガーし、その後、Dingo ロボットのエンドエフェクタタスクを削除します（以降は接触によって処理されます）。最後に、ドアを開くための目標を設定します。

// In the run function, under phase_ == OPEN
solver().removeTask(dingoBaseTask_);
doorPostureTask_->target({{"door", {M_PI / 2}}});
phase_ = DONE;
# In the run callback, under self._phase == ControllerPhase.OPEN
self.qpsolver.removeTask(self._dingoBaseTask)
self._doorPosture.target(
    {
        "door".encode("utf-8"): [math.pi / 2],
    }
)
self._phase = ControllerPhase.DONE
architecture_overview

フェーズ5: ドアのハンドルを離し、待機姿勢に戻る

最後のフェーズでは、UR5e とドアとの接触を削除してハンドルを離し、姿勢タスクを用いて待機姿勢に戻ります。また、Dingo ロボットが動かないように、エンドエフェクタタスクを再度追加します。

// In the run function, under phase_ == DONE
removeContact({"ur5e", "door", "Tool", "Handle"});
solver().addTask(dingoBaseTask_);
solver().addTask(postureTask);
# In the run callback, under self._phase == ControllerPhase.DONE
self.removeContact(mc_control.Contact("ur5e", "door", "Tool", "Handle"))
self._dingoBaseTask.reset()
self.qpsolver.addTask(self._dingoBaseTask)
self.qpsolver.addTask(self.postureTask)
architecture_overview

いろいろ試してみよう

ここまでで、いくつかのパラメータを変更しながらマルチロボットアプローチの利点を確認できます。他の設定を変更することなく、以下の項目を試してみてください。

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

このコントローラの完全なソースコードは こちら から参照できます。また、環境設定を含むソースコード一式は mc_rtc_tutorials リポジトリに含まれています。

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