Motion Commands

前のページ
目次
次のページ

このページの内容

モーションコマンドとエフェクターのコントロール

　AIBOをスムースに動かすためには、１秒間に数回〜数十回のコントロール信号を小刻みに更新する必要があります。
　例えば、AIBOの頭をスムースに右へ向けるためには、最初は動きを加速し、中間では一定の速度で動かし、最後は減速します。このような動き（モーション）を実現するには、モーターを回す力を微妙に調整しなければなりません。
　また別の例として、AIBOのLEDランプを素早く点滅させるためには、ある間隔でLEDを光らせ、別の間隔でLEDを消灯させることを素早く繰り返します。

　AIBOのオペレーティングシステムAperiosは、ロボットのエフェクター（effector：モーターやLED）の状態を8ミリ秒ごと（１秒間に125回）更新します。各回の更新はフレーム（frame)と呼ばれます。フレームは同時に複数のエフェクター状態を更新することができます。また、１度に４つのフレームをバッファに記憶するため、32ミリ秒ごとに新しいバッファを持つ必要があります。
　Tekkotsuでは、これらのバッファはモーションマネージャ（MotionManager）によって作られます。モーションマネージャは、32ミリ秒ごとに複数のモーションコマンド（MotionCommands: MCs）を実行するのです。これらモーションコマンドの実行結果は１つのバッファに集められ、Aperiosに渡されます（下図）。

　実例として、1秒に1回の割合でAIBOのLEDを点滅させることを考えてみましょう。必要な命令は、モーションマネージャが更新を問い合わせるたびに、LEDの新しい状態を計算するモーションコマンドです。
　LedMCは、モーションコマンドとLEDエンジンの派生クラスで、このような要求を実現してくれます。具体的には、LedMCのオブジェクト leds_mcを作成して、LEDの点滅周期をleds_mcに与え、モーションマネージャのリストに追加します（下図）。

　しかし、問題があります。モーションコマンドの計算を含むアプリケーションはMainプロセスで動いていますが、モーションマネージャはモーションプロセスで動いています。それでは、どうすればモーションマネージャが利用できるようにモーションコマンドを計算すればよいのでしょうか？

　解決策は、両方のプロセスが共有するメモリ領域にモーションコマンドを作るのです。このようにすると、モーションコマンドに両プロセスからアクセスできるので、それが動いている間も、他の動きをさせるために設定値を変更できます（下図）。

　しかし、モーションマネージャがモーションコマンドを呼び出している最中にモーションコマンドを修正するのは危険です（下図）。

　この問題を回避するために、Tekkotsuでは、MMAccessorと呼ばれる相互排除メカニズムを提供しています。MMAccessorは、Mainプロセスからモーションコマンドのメンバー関数を呼び出す必要があるとき、モーションマネージャを一時的に排除します（つまり、モーションマネージャからアクセスできなくします）。このような関数を呼びたいときは、いつでもMMAccessorを作成してモーションコマンドをロックします（ロックする＝カギをかけて他から使えなくする）。MMAccessorを削除するとモーションコマンドのロックは解除されます（下図）。

　もう１つ行う作業があります。モーションコマンドがモーションマネージャに渡されるとき、MC_IDと呼ばれる識別IDをモーションコマンドに割り当てます。モーションコマンドをロックするためには、MC_IDの値をMMAccessorのコンストラクターに渡します。また、モーションコマンドを削除するときは、モーションマネージャにMC_IDを伝えます。このようにMC_IDはいろいろな場所で使用します。通常、MC_IDはBehaviorインスタンス内のprotectedメンバーとして扱います。

まとめ


練習:　LEDを点滅させるためのモーションコマンドを設定しよう

　AIBOのLEDを点滅させるには、LedMCを使用します。また、ボタンを押すことにより、LED点滅パターンを変化させるようにしましょう。
　最初にモーションマネージャを扱うために、新しいファイルをいくつかインクルードします。
　また、モーションコマンドのMC_IDを記憶するために、protectedデータメンバーを作成します。
　それから、MyMotionLed() コンストラクターを初期化します。

　下記のプログラム（４つに分かれています）を、次のファイル名で保存して下さい。

　　　ファイル名：　MyMotionLed.h
　　　保　存　先：　マイドキュメント → usXX → projectフォルダ

　プログラムで赤字の箇所が新しく追加・変更された部分です。

#ifndef INCLUDED_MyMotionLed_h_
#define INCLUDED_MyMotionLed_h_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/LedMC.h"           // LedMCのヘッダファイル
#include "Motion/MMAccessor.h"      // MMAccessorのヘッダファイル
#include "Motion/MotionManager.h"   // MotionManagerのヘッダファイル

class MyMotionLed : public BehaviorBase {
protected:
	MotionManager::MC_ID leds_id;     // モーションコマンドに対するID

public:
	MyMotionLed() : BehaviorBase("MyMotionLed"),	// コンストラクターを初期化
	leds_id(MotionManager::invalid_MC_ID) {}	// コンストラクターを初期化

　モーションマネージャにアクセスするためにグローバル変数　motman　を使用します。
　DoStart() は、SharedObject<LedMC>テンプレートクラスを使用してインスタンスleds_mcを作ります。
　つまり、leds_mcはSharedObjectクラスのインスタンスで、引数の型がLedMCです。
　SharedObjectのインスタンスは共有メモリ内に作成されます。
　これで、leds_mcが共有オブジェクトになります。

　1000ミリ秒ごとにLedを光らせるために、cycle(...)メソッドを呼びます。
　それから、addPersistentMotionを使用して、leds_mcをモーションマネージャに追加します。
　leds_mcはローカル変数なので、DoStart()が再び呼ばれると、失われます。
　以上でモーションコマンドを設定できました。
　leds_idはモーションマネージャのリストからleds_mcを指定するための識別IDになります。
　モーションコマンドは、モーションマネージャのリストから取り除かれるまで消えません。

  virtual void DoStart() {
	BehaviorBase::DoStart();
	std::cout << getName() << " is starting up." << endl;
	erouter->addListener(this,EventBase::buttonEGID);
	SharedObject<LedMC> leds_mc;
	leds_mc->cycle(RobotInfo::FaceLEDMask, 1000, 100.0);
	leds_id = motman->addPersistentMotion(leds_mc);
  }

　DoStop() は、モーションマネージャのリストからLedMCを削除します。

  virtual void DoStop() {
	motman->removeMotion(leds_id);
	std::cout << getName() << " is shutting down." << endl;
	BehaviorBase::DoStop();
  }

　processEvent() では、LedMCのパラメータを変更します。パラメータとは設定値のことです。
　パラメータ変更を安全に行うために、MMAccessorのインスタンスleds_accを作ります。
　このとき、識別IDのleds_idを使用します。これでモーションコマンドがロックされます。
　それから、cycle()関数を呼び、点滅周期を変化させます。
　最初の式1000-750*event.getMagnitude() において、getMagnitude()は、0〜1.0の間の値になります。
　ボタンを押したときが1.0で、離したときが0です。
　だから式の結果として、点滅周期はボタンを押している時が250ミリ秒で、押していない時が1000ミリ秒です。
　関数の最後の引数100.0は明るくしたり暗くしたりする速度を決める値です。
　　100.0の場合、すぐに明るくなります
　cycle()関数は正弦波を計算します。
　　正弦波の振幅は0〜1.0の間です。
　　したがって、正弦波に100.0を掛け算することは、波形の正の部分がすぐに1.0になるような階段波形を作り出します。

  virtual void processEvent(const EventBase &event) {
	switch ( event.getGeneratorID() ) {

		case EventBase::buttonEGID: {
			std::cout << getName() << " got event: " << event.getDescription() << std::endl;
			MMAccessor<LedMC> leds_acc(leds_id);
			leds_acc->cycle(RobotInfo::FaceLEDMask, int(1000-750*event.getMagnitude()), 100.0);
			break; };

		default:
			std::cout << "Unexpected event:" << event.getDescription() << std::endl;
	}
  }

};

#endif

課題

　LEDの種類や色を変更するには、次の赤字の箇所を変更します。

　　　　leds_mc->cycle(RobotInfo::FaceLEDMask, 1000, 100.0);

　次の表を参考にして、いくつかのLEDを光らせて試しなさい。

表．LEDの記号
記号場所色
HeadColorLEDMask頭の半円オレンジ
HeadWhiteLEDMask頭の半円ホワイト
WirelessLEDMask頭の頂上ブルー
ModeRedLEDMask耳の横レッド
ModeGreenLEDMask耳の横グリーン
ModeBlueLEDMask耳の横ブルー
FaceLEDMask顔ホワイト＆レッド
AllLEDMaskすべて
BackLEDMask背中の全て
FrBackColorLEDMask背中の前側ブルー
FrBackWhiteLEDMask背中の前側ホワイト
MdBackColorLEDMask背中の中央オレンジ
MdBackWhiteLEDMask背中の中央ホワイト
RrBackColorLEDMask背中の後側レッド
RrBackWhiteLEDMask背中の後側ホワイト

モーション終了を検出する

　いくつかのモーションコマンドは、モーションマネージャのリストから削除しなければ動き続けます（LedMCは、パラメータを cycle で設定すると動き続けます。しかし、パラメータを flash または cflash で設定すると、自動的に終了します。これらはLEDを一度だけ光らせるのです）。モーションコマンドは、そのモーションが終了するとイベントを発生させて終了したことを通知します。イベントには source IDが含まれます。この source ID はモーションコマンドのMC_IDと一致します。


練習：　頭を動かし、LEDを点滅させる

　AIBOの頭を動かすには、HeadPointerMC モーションコマンドを使用します。
　ボタンを押すと、頭を上げたり、下げたりするようにします。
　そして、頭を動かし終わったら、LEDを点滅させます。

　下記のプログラムを次のファイル名で保存してください。

　　　ファイル名：　MyMotionHead.h
　　　保　存　先：　マイドキュメント → usXX → projectフォルダ

　プログラムで赤字の箇所が新しく追加・変更された部分です。

#ifndef INCLUDED_MyMotionHead_h_
#define INCLUDED_MyMotionHead_h_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/LedMC.h"
#include "Motion/MMAccessor.h"
#include "Motion/MotionManager.h"

class MyMotionHead : public BehaviorBase {
protected:
	MotionManager::MC_ID leds_id;
	MotionManager::MC_ID head_id;

public:
	MyMotionHead() : BehaviorBase("MyMotionHead"), 
	leds_id(MotionManager::invalid_MC_ID),
	head_id(MotionManager::invalid_MC_ID) {}

　DoStart() の中で、2つのモーションコマンドをセットアップします。
　ローカル変数 leds_mc は、LedMCインスタンスで共有オブジェクトです。
　HeadPointerMCも共有オブジェクトです。
　->演算子を用いてsetMaxSpeedを呼びます。
　　これは頭を上下に動かす関節の変化量を1秒間に0.5ラジアンの最大速度に設定します。
　まだ、MMAccessor を作成する必要はありません（モーションコマンドがモーションマネージャのリストに追加されていないからです）。
　addPersistentMotionHead を使用して、モーションコマンドをモーションマネージャのリストに追加した後、motmanEGID のstatusETID に対するリスナーを登録します。
　リスナーのsource IDをモーションコマンドのIDである head_idに設定します。
　モーションマネージャは、HeadPointerMCインスタンスがモーションを終了するたびにイベントを発生させます。

  virtual void DoStart() {
	BehaviorBase::DoStart();
	std::cout << getName() << " is starting up." << endl;
	erouter->addListener(this,EventBase::buttonEGID);

	// LED モーションコマンド設定
	SharedObject<LedMC> leds_mc;
	leds_id = motman->addPersistentMotion(leds_mc);

	// Head pointer モーションコマンド設定
	SharedObject<HeadPointerMC> head_mc;
	head_mc->setMaxSpeed(RobotInfo::TiltOffset,0.5);
	head_id = motman->addPersistentMotion(head_mc);
	erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID);
  }

  virtual void DoStop() {
	motman->removeMotion(leds_id);
	motman->removeMotion(head_id);
	std::cout << getName() << " is shutting down." << endl;
	BehaviorBase::DoStop();
  }

　processEvent() では、ボタン押下イベントを受け取ったケースの処理を書きます。
　　　　lowpos = 頭を下げたときの下限値
　　　　highpos = 頭を上げたときの上限値
　getJointValue() は現在の頭の関節角度を得る関数で、この結果を val に代入します。
　　　val が中間値 midpos より大きければ頭を下げ、小さければ頭を上げます。
　　　つまり、ボタンを押すたびに頭を上げたり下げたりします。
　新しい目標角度を設定するには setJointValue() を使います。
　lowpos と highpos はRobotInfo名前空間の表から得ることができます。
　　　RobotInfo名前空間は、AIBOの機種ERS-2xx、ERS-7のどちらにも対応しています。
　　　機種によって頭を振る範囲が異なりますが、RobotInfo名前空間を使用することにより両方に対応できます。
　変数outputRanges と HeadOffset は Ers7Info.h（または ERS2xxInfo.h）に定義されています。
　変数TiltOffset は CommonInfo.h で定義されています。

　Headモーションが完了すると、イベントmotmanEGID が発生します。motmanEGID を受け取ったケースの処理を書きます。
　つまり、頭を動かし終わったら、顔LEDを500ミリ秒だけ光らせます。
　モーションが完了すると、イベントmotmanEGID が発生しますが、これを知るためにはリスナーを登録しておかなければなりません。

  virtual void processEvent(const EventBase &event) {

	// イベントGeneratorIDに応じて処理を切り替えます
	switch ( event.getGeneratorID() ) {

		// GeneratorIDがbuttonEGID（ボタンイベントの場合）
		case EventBase::buttonEGID:
			std::cout << getName() << " got button event: " << event.getDescription() << std::endl;

			// TypeIDがactivateETID（ボタンが押された場合）
			if (event.getTypeID() == EventBase::activateETID) {

				// 頭の角度の下限値（最も下を向いたときの角度）
				const float lowpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MinRange];
				// 頭の角度の上限値（最も上を向いたときの角度）
				const float highpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MaxRange];
				// 頭の角度の中間値
				const float midpos = (lowpos+highpos) / 2;
				// MMAccessorでロックする
				MMAccessor<HeadPointerMC> head_acc(head_id);
				// 現在の頭の角度を求める
				double val = head_acc->getJointValue(RobotInfo::TiltOffset);
				// 現在の頭の角度と中間値を比較した結果により、目標値を設定する
				head_acc->setJointValue(RobotInfo::TiltOffset, (val > midpos) ? lowpos : highpos);
			}
			break;

		// GeneratorIDがmotmanEGID（モーション完了イベントの場合）
		case EventBase::motmanEGID: {
			std::cout << getName() << " got motman event: " << event.getDescription() << std::endl;

			// MMAccessorでロックする
			MMAccessor<LedMC> leds_acc(leds_id);
			// LEDを500ミリ秒だけ光らせる
			leds_acc->flash(RobotInfo::FaceLEDMask, 500);

			break; };

		// その他のイベントの場合
		default:
			std::cout << "Unexpected event:" << event.getDescription() << std::endl;
	}
  }

};

#endif

　ボタンを押すたびに、頭とLEDがどのように変化するか試してください。

エフェクターの名前

　Tekkotsu ではロボットの関節や他のエフェクターを記述するために、配列を使用します。
　例えば、各モータの最大速度は配列 RobotInfo::MaxOutputSpeed[] に保存されています。

　各関節が動く範囲にはソフトウェアでの限界とハードウェアでの限界があります。
　ソフトウェア限界は配列 RobotInfo::outputRanges[][] に保存されています。これは上述の head motion の例で使用しました。
　ハードウェア限界は配列 RobotInfo::mechanicalLimits[][] に保存されています。
　すべてのテーブルは同じ index を使用します。例えば、頭を上下に動かすモーターのindex はどのテーブルでも HeadOffset+TiltOffset です。
　Tekkotsu で関節を参照する際の約束として、これらのテーブルで offset を指定します（offsetというのは基準からの距離です）。これらの offset は整数ではなく HeadOffset のように記号で表現します。

　ここで、番号付けの決まりについて説明します。
　関節はロボットの構造に基づいてグループ化されています。このようにして、すべての足の関節は、一貫性のある offset を持ちます。頭、尻尾の関節も同様です。

　足の順番は RobotInfo::LegOrder_t で定義され、次の通りです。

値記号意味
0LFrLegOrderLeft Front Leg （左前脚）
1RFrLegOrderRight Front Leg （右前脚）
2LBkLegOrderLeft Back Leg （左後脚）
3RBkLegOrderRight Back Leg （右後脚）
　それぞれの足には3個の関節があり、RobotInfo::NumLegJointsの値が3に定義されています。
　それぞれの足の offset に対する記号もあり、次のようになります。
LFrLegOffset == LegOffset + LFrLegOrder * NumLegJoints
RFrLegOffset == LegOffset + RFrLegOrder * NumLegJoints
LBkLegOffset == LegOffset + LBkLegOrder * NumLegJoints
RBkLegOffset == LegOffset + RBkLegOrder * NumLegJoints

　1本の足における関節の順番は RobotInfo::REKOffset_t で定義されています。それぞれの関節の値は 0,1,2で、記号では、RotatorOffset, ElevatorOffset, KneeOffset です。Rotator と Elevator は共に股関節ですが、Rotatorは足を前後に動かす関節、Elevatorは足を開いたり閉じたりする関節です。Knee は膝関節です。

　特定の足の関節を参照するには、その足の offset を使用し、さらに関節の offset を追加します。例えば、右の後ろ足の膝関節は RBkLegOffset + KneeOffset で参照します。

　同様な決まりが頭部にも適用されます。
　頭の関節の始まりは RobotInfo::HeadOffset です。
　関節の順番は RobotInfo::TPROffset_t で定義されます。TPRというのは Tilt, Pan, and Roll のことです（ただし、ERS-7 はRollを持たず、代わりに第２のTiltとして Nodを持ちます）。TPROffset の値は、それぞれ TiltOffset, PanOffset, RollOffset （ERS-7の場合は NodOffset ）。例えば、頭部Tilt関節に関する情報にアクセスするには、indexとしてHeadOffset+TiltOffset を使用します。

　関節は常にすべての index により参照されます。ただ、１つ例外があり、HeadPointerMC モーションコマンドは、HeadOffset を使用せずにTPROffsetで参照します。例えば、HeadPointerMC を用いて頭部Panの角度を設定したいときは次のように書きます。

head_acc->setJointValue(PanOffset, panvalue)
　しかし、ロボット全体の姿勢としてPanの角度を調整したいときは、HeadPointerMC でなく PostureMC を使用するので、次のように書きます。
posture_acc->setOutputCmd(HeadOffset+PanOffset, panvalue)

PostureMCについては別のページで勉強します。

前のページ
目次
次のページ