双腕コントローラ

このセクションの目的は、2本のアームを協調させ、アーム間の衝突制約を設定することで、協調動作を実現することです。

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

例

ここでは、UR5e をメインロボット、Kinova を第2ロボットとして読み込むシンプルなコントローラを構築します。これらはそれぞれ `mc_rtc` のロボットモジュール mc_ur5e, mc_ur5e_description と mc_kinova として提供されています。UR5e をメインロボットとして読み込むために、`mc_rtc` の設定ファイルを以下のように変更する必要があります。

MainRobot: UR5e
Enabled: DualArmController

次に、双腕ロボットを入力として受け取るコントローラを構築します。

  • UR5e
  • Kinova

そのために、コンストラクタを修正します。

#include <mc_rbdyn/RobotLoader.h>

DualArmController::DualArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController({rm,
    mc_rbdyn::RobotLoader::get_robot_module("KinovaDefault")}, dt)
@staticmethod
def create(robot, dt):
    kinova = mc_rbdyn.get_robot_module("KinovaDefault")
    return DualArmController([robot, kinova], dt)

ロボットのインデックス

この新しい例では、追加のロボットとして Kinova を読み込んでいます。これは、読み込むロボットモジュールのリストを指定することで実現しています。読み込まれたロボットは、モジュールが指定された順番に従ってインデックスが割り当てられます。つまり、メインロボットである UR5e は引き続きインデックス 0 を持ち、新たに Kinova ロボットがインデックス 1 として追加されます。

Kinova ロボットの初期位置

この時点でコントローラを起動すると、Kinova と UR5e が同じ位置に配置されていることに気付くでしょう。本チュートリアルでは、Kinova の位置を手動で再配置します。

// 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))
)
architecture_overview

タスクと制約

各ロボットに対して、通常どおりタスクと制約を追加します。異なる設定例を示すため、UR5e にはエンドエフェクタタスクを、Kinova には姿勢 (ポスチャ) タスクを追加します。さらに、動作中の衝突を回避するため、2台のロボット間に衝突制約を追加します。まず、UR5e ロボットに対するタスクと制約を追加します。

// ヘッダー内
#include <mc_tasks/EndEffectorTask.h>

struct DualArmController_DLLAPI DualArmController : public mc_control::MCController
{
  // ...
  std::shared_ptr<mc_tasks::EndEffectorTask> urEndEffectorTask_;
  // ...
}

// reset関数内
std::string urBody = "tool0";
urEndEffectorTask_ = std::make_shared<mc_tasks::EndEffectorTask>(urBody, robots(), 0, 1);
solver().addTask(urEndEffectorTask_);
# resetコールバック関数内
self._urEndEffectorTask = mc_tasks.EndEffectorTask("tool0", self.robots(), 0)
self._urEndEffectorTask.positionTask.stiffness(1)
self._urEndEffectorTask.orientationTask.stiffness(1)

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

// ヘッダー内
std::shared_ptr<mc_tasks::PostureTask> kinovaPostureTask_;
std::shared_ptr<mc_solver::KinematicsConstraint> kinovaKinematics_;
// reset関数内
kinovaPostureTask_ = std::make_shared<mc_tasks::PostureTask>(solver(), 1);
solver().addTask(kinovaPostureTask_);
kinovaKinematics_ = std::make_shared<mc_solver::KinematicsConstraint>(robots(), 1, solver().dt());
solver().addConstraintSet(*kinovaKinematics_);
# resetコールバック関数内
self._kinovaPostureTask = mc_tasks.PostureTask(self.qpsolver, 1, 5.0, 1000.0)
self._kinovaKinematicsConstraint = mc_solver.KinematicsConstraint(
    self.robots(), 1, self.qpsolver.dt()
)
self.qpsolver.addTask(self._kinovaPostureTask)
self.qpsolver.addConstraintSet(self._kinovaKinematicsConstraint)

最後に、2台のロボット間の衝突制約を追加します。

// In the constructor or reset function
double iDist = 0.1;
double sDist = 0.05;
double damping = 0.0;
addCollisions("ur5e", "kinova_default", {{"*", "*", iDist, sDist, damping}});
# コンストラクタ内、またはresetコールバック関数内
iDist, sDist, damping = 0.1, 0.05, 0.0
self.addCollisions(
    "ur5e",
    "kinova_default",
    [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
)

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

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

  1. 姿勢タスクを用いて UR5e を待機姿勢 (スタンバイポーズ) に移動する
  2. UR5e のエンドエフェクタタスクを有効化する
  3. 両アームの繰り返し動作を開始する

そのため、コードは以下のようになります。

// ヘッダー内
enum ControllerPhase
{
  IDLE = 0,
  STARTED,
  MOVE
};
// コントローラーのプライベートプロパティ
ControllerPhase phase = IDLE;
// run関数内
bool DualArmController::run()
{
  if(phase_ == IDLE && 0 /** この条件は後で記述する */)
  {
    /** STARTEDフェーズをセットアップする */
    phase_ = STARTED;
  }
  else if(phase_ == STARTED && 0 /** この条件は後で記述する */)
  {
    /** MOVEフェーズをセットアップする */
    phase_ = MOVE;
  }
  else if(phase_ == MOVE && 0 /** この条件は後で記述する */) {}
  return mc_control::MCController::run();
}
# 定数を宣言する
class ControllerPhase:
    IDLE = 0
    STARTED = 1
    MOVE = 2


# コンストラクター内
def __init__(self, rm, dt):
    self._phase = IDLE


# runコールバック関数内
def run_callback(self):
    if self._phase == ControllerPhase.IDLE and False:  # この条件は後で記述する
        # STARTEDフェーズをセットアップする
        self._phase = ControllerPhase.STARTED
    elif self._phase == ControllerPhase.STARTED and False:  # この条件は後で記述する
        # MOVEフェーズをセットアップする
        self._phase = ControllerPhase.MOVE
    elif self._phase == ControllerPhase.MOVE and False:  # この条件は後で記述する
        # MOVEフェーズを継続する
        pass
    return True

フェーズ1: UR5e を待機姿勢に移動

このフェーズでは、姿勢タスクを用いてメインロボットである UR5e をあらかじめ定義された待機姿勢に移動させます。エンドエフェクタタスクとの競合を避けるため、このフェーズ中は一時的にエンドエフェクタタスクを無効化し、姿勢タスクが達成された後に再度有効化します。

注意: エンドエフェクタタスクを無効化しない場合、2つのタスクが互いに競合し、ロボットは目標姿勢に到達できません。

// In the run function
bool DualArmController::run()
{
  if(phase_ == IDLE)
  {
    postureTask->target({{"elbow_joint", {-M_PI / 2}}, {"wrist_2_joint", {M_PI / 2}}});
    phase_ = STARTED;
  }
  // ...
}
# runコールバック関数内
def run_callback(self):
    if self._phase == ControllerPhase.IDLE:
        self.postureTask.target(
            {
                "elbow_joint".encode("utf-8"): [-math.pi / 2],
                "wrist_2_joint".encode("utf-8"): [math.pi / 2],
            }
        )
        self._phase = ControllerPhase.STARTED
    # ...
architecture_overview

フェーズ2: UR5e のエンドエフェクタタスクを有効化

このフェーズでは、UR5e の姿勢タスクが達成されたかどうかを確認し、エンドエフェクタタスクの目標をリセットした上で有効化します。

// run関数内
bool DualArmController::run()
{
  // ...
  else if(phase_ == STARTED && postureTask->eval().norm() < 0.01 && postureTask->speed().norm() < 0.01)
  {
    phase_ = MOVE;
    solver().addTask(urEndEffectorTask_);
  }
  else if(phase_ == STARTED)
  {
    urEndEffectorTask_->reset();
  }
  // ...
  return mc_control::MCController::run();
}
# runコールバック関数内
def run_callback(self):
    # ...
    elif (
            self._phase == ControllerPhase.STARTED
            and self.postureTask.eval().norm() < 0.01
            and self.postureTask.speed().norm() < 0.01
        ):
            self._phase = ControllerPhase.MOVE
            self.qpsolver.addTask(self._urEndEffectorTask)
    elif self._phase == ControllerPhase.STARTED:
        self._urEndEffectorTask.reset()
    # ...
    return True

フェーズ3: 両アームの繰り返し動作を開始

このフェーズはメインフェーズであり、両アームが繰り返し動作を行います。UR5e ではエンドエフェクタを直線軌道で動かし、Kinova では第2関節を前後に振動させます。

// ヘッダー内
enum ControllerState
{
  GO = 0,
  RETURN
};

// コントローラーのプライベートプロパティ
ControllerState urState_ = RETURN;
ControllerState kinovaState_ = RETURN;

// 2つのサポート関数を追加
void DualArmController::runUr()
{
  if(urState_ == GO && urEndEffectorTask_->eval().norm() < 0.05 && urEndEffectorTask_->speed().norm() < 0.01)
  {
    urState_ = RETURN;
    urEndEffectorTask_->add_ef_pose({Eigen::Vector3d(0.0, -0.5, 0.0)});
  }
  else if(urState_ == RETURN && urEndEffectorTask_->eval().norm() < 0.01 && urEndEffectorTask_->speed().norm() < 0.05)
  {
    urState_ = GO;
    urEndEffectorTask_->add_ef_pose({Eigen::Vector3d(0.0, 0.5, 0.0)});
  }
}

void DualArmController::runKinova()
{
  if(kinovaState_ == GO && kinovaPostureTask_->eval().norm() < 0.01 && kinovaPostureTask_->speed().norm() < 0.01)
  {
    kinovaState_ = RETURN;
    kinovaPostureTask_->target(joint_2});
  }
  else if(kinovaState_ == RETURN && kinovaPostureTask_->eval().norm() < 0.01
          && kinovaPostureTask_->speed().norm() < 0.01)
  {
    kinovaState_ = GO;
    kinovaPostureTask_->target(joint_2});
  }
}

// run関数内
bool DualArmController::run()
{
  // ...
  else if(phase_ == MOVE)
  {
    runUr();
    runKinova();
  }
  return mc_control::MCController::run();
}
# 定数を宣言する
class ControllerState:
    GO = 0
    RETURN = 1

# コンストラクタ内
def __init__(self, rm, dt):
    self._ur_state = ControllerState.RETURN
    self._kinova_state = ControllerState.RETURN

# 2つのサポート関数を追加
def _run_kinova(self):
    if (
        self._kinova_state == ControllerState.GO
        and self._kinovaPostureTask.eval().norm() < 0.01
        and self._kinovaPostureTask.speed().norm() < 0.01
    ):
        self._kinova_state = ControllerState.RETURN
        self._kinovaPostureTask.target(
            {
                "joint_2".encode("utf-8"): [0.0],
            }
        )
    elif (
        self._kinova_state == ControllerState.RETURN
        and self._kinovaPostureTask.eval().norm() < 0.01
        and self._kinovaPostureTask.speed().norm() < 0.01
    ):
        self._kinova_state = ControllerState.GO
        self._kinovaPostureTask.target(
            {
                "joint_2".encode("utf-8"): [-math.pi / 4],
            }
        )

def _run_ur5e(self):
    if (
        self._ur_state == ControllerState.GO
        and self._urEndEffectorTask.positionTask.eval().norm() < 0.05
        and self._urEndEffectorTask.positionTask.speed().norm() < 0.05
    ):
        self._ur_state = ControllerState.RETURN
        self._urEndEffectorTask.add_ef_pose(sva.PTransformd(
            sva.RotZ(0), eigen.Vector3d(0.0, -0.5, 0.0)
        ))
    elif (
        self._ur_state == ControllerState.RETURN
        and self._urEndEffectorTask.positionTask.eval().norm() < 0.05
        and self._urEndEffectorTask.positionTask.speed().norm() < 0.05
    ):
        self._ur_state = ControllerState.GO
        self._urEndEffectorTask.add_ef_pose(sva.PTransformd(
            sva.RotZ(0), eigen.Vector3d(0.0, 0.5, 0.0)
        ))
        
# runコールバック関数内
def run_callback(self):
    # ...
    elif self._phase == ControllerPhase.MOVE:
        self._run_ur5e()
        self._run_kinova()
    return True
dual_arm_collision_avoidance_1 dual_arm_collision_avoidance_2

プレイタイム

ここまでで、双腕コントローラの設定を自由に試すことができます。各アームのタスク、制約、目標動作を変更してみてください。例えば、以下のような変更が可能です。

  • UR5e のタスクを変更する
  • Kinova のタスクを変更する
  • フェーズ3における UR5e の動作を変更する
  • フェーズ3における Kinova の動作を変更する

このコントローラーの完全なソースは、こちらから入手できます。また、環境設定を含めるソースコードは mc_rtc_tutorials リポジトリで確認できます。

次のチュートリアル: 移動マニピュレータコントローラ