重心の制御

このチュートリアルでは、ロボットの質量中心（CoM: Center of Mass）を制御します。先のチュートリアルと同じく、質量中心を20cm下げてから元の位置に戻す関数を記述します。

接触面

質量中心を動かす前に、接触面を考慮する必要があります。先のコントローラーでは、接触面のセットは空のままでしたが、何も問題は起こりませんでした。しかし、質量中心を制御する場合、そのコマンドをそのまま使用すると、ロボットが地面の下に沈んだり空中に浮いたりしてしまいます。これは、単に接触面がないためです（姿勢の誤差を最小限に抑えつつ質量中心を動かしたい場合は、接触面を空にするのが最もよい方法です）。

そこで、ロボットの足と地面との間に接触面を追加します。

// コンストラクター内
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")

注: ロボットの現在の表面を「見つける」方法については、表面の可視化に関するチュートリアルを参照してください。

力学的制約条件

ここまでは、キネマティクスモードでコントローラーを実行してきました。次に力学モードに切り替えると、以下のことが行えるようになります。

  • 外力とそれに伴うトルクを計算する
  • 外力を接触面の摩擦円錐内に収める
  • トルクをロボットのトルク限界内に収める

モードを切り替えるには、コード内のkinematicsConstraintをdynamicsConstraintに変更します。kinematicsConstraintsによって適用されていた制約条件は、dynamicsConstraintでも適用されます。

質量中心タスクを作成して問題に追加する

以下の作業を行います。

  1. 質量中心タスクを作成する
  2. 作成したタスクを最適化問題に追加する
  3. 目標が正しく設定されているか確認する
// 質量中心タスクのヘッダーをインクルードする（ヘッダー）
#include <mc_tasks/CoMTask.h>
// クラス内のプライベートメンバー（ヘッダー）
std::shared_ptr<mc_tasks::CoMTask> comTask;
// コンストラクター内でタスクを作成して問題に追加する
comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0, 10.0, 1000.0);
solver().addTask(comTask);
// 姿勢制御タスクの剛性を下げる
postureTask->stiffness(1);
// reset関数内で、現在の質量中心に合わせてタスクをリセットする
comTask->reset();
# mc_tasksモジュールをインポートする
import mc_tasks

# コンストラクター内でタスクを作成して問題に追加する
self.comTask = mc_tasks.CoMTask(self.robots(), 0, 10.0, 1000.0)
self.qpsolver.addTask(self.comTask)
# 姿勢制御タスクの剛性を下げる
self.postureTask.stiffness(1)
# resetコールバック関数内で、現在の質量中心に合わせてタスクをリセットする
self.comTask.reset()

今回の例では、以下の点を考慮してタスクを作成します。

  1. robots()を使用する
  2. インデックスが0のロボットに適用する。インデックス0は常に、mc_rtcで読み込まれるメインロボットを指します。
  3. 目標に向かって剛性10.0で引っ張る。このパラメーターは、ロボットを目標に向かって引っ張る「ばね」の強さを規定します。
  4. 重み1000を割り当てる。この重みは、最適化問題におけるタスクの優先度を表します。今回の場合、デフォルトの姿勢の重みは5.0であるため、質量中心タスクのほうが優先されます。なお、タスクの誤差は正規化されないため、状況に応じて重みを調節する必要があります。
  5. 質量中心タスクと干渉しないように、姿勢制御タスクの剛性を下げる。

質量中心を上下に動かす

switch_com_target()関数を使用して、先のチュートリアルと同様のメソッドを実装します。ここでは、bool型変数comDownと質量中心の初期位置を表すEigen::Vector3d変数comZeroが追加されていると仮定します。

void MyFirstController::switch_com_target()
{
  // comZeroは、reset関数内で
  // 以下のように取得される:
  // comZero = comTask->com();
  if(comDown) { comTask->com(comZero - Eigen::Vector3d{0, 0, 0.2}); }
  else { comTask->com(comZero); }
  comDown = !comDown;
}
def switch_com_target(self):
    # self.comZeroは、質量中心タスクのリセット後、
    # reset関数内で以下のように取得される:
    # self.comZero = self.comTask.com()
    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

最後に、質量中心タスクの誤差を監視して目標を変更します。

bool MyFirstController::run()
{
  if(comTask->eval().norm() < 0.01)
  {
    switch_com_target();
  }
  return mc_control::MCController::run();
}
def run_callback(self):
    if self.comTask.eval().norm() < 0.01:
        self.switch_com_target()
    return True

このコントローラーを実行すると、ロボットが上下に動くのが分かります。次のチュートリアルでは、エンドエフェクターを動かす方法と、タスクの構成をディスクから読み込む方法について見ていきます。

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

次のチュートリアル: エンドエフェクタの制御方法 (及びロードの方法)