2006.11.4　初
2006.11.14 改
2006.12.13 改

■目次

1. 他のオブジェクトからの使い方
2. 歩行オブジェクトについて
3. 各クラスについて
4. 構造体
5. 型・状態変数
6. 定数
7. 座標系・姿勢系
8. アイボ歩行エディター
9. 課題

■１　他のオブジェクトからの使い方

以下の機能があります。

1. 停止する
2. 動く　停止命令が来るまで歩く
3. 歩く　あらかじめ歩数を決めておく
4. モーションの再生
5. 姿勢移行　その場で足踏みし、姿勢を変える
6. 微調整　体の中心線のどこかに中心を設定し、後ろ足だけで回転動作をする。
7. 頭部の情報の取得

オブジェクト間通信にて命令を受ける。情報伝達の構造体は　DirectionToIKM型　と　ResultToHV型　で行う。　　

■２　歩行オブジェクトについて

歩行オブジェクトは起動すると、各関節を定義し、現在の関節角度に設定してからゲインを与えます。
また、歩行パラメータファイルとモーションファイルを読み込み、指令待ち状態になります。
歩行エディタとの併用により他のオブジェクトからの指令を受けなくても動作させることができます。
この場合、歩行パラメータ及びモーションの編集が可能にまります。エディタからの指令により歩行パラメータファイルとモーションファイルを保存更新することができます。

■　歩行パラメータファイル

歩行パラメータファイルは　/MS/OPEN-R/MW/DATA/P/WALK.TXT で与えられる。
ファイル中のパラメータの働きはWalkParam型定義を参照のこと。

■モーションファイル

歩行パラメータファイルは　/MS/OPEN-R/MW/DATA/P/MOTION.TXT で与えられる。
このデータはMotionData型　KeyData型に格納される。　行頭の数字はMotionMode型に適用される。

■クラス定義

class IKmove : public OObject { public: IKmove(); virtual ~IKmove() {} OSubject* subject[numOfSubject]; OObserver* observer[numOfObserver]; virtual OStatus DoInit (const OSystemEvent& event); virtual OStatus DoStart (const OSystemEvent& event); virtual OStatus DoStop (const OSystemEvent& event); virtual OStatus DoDestroy(const OSystemEvent& event); void Ready(const OReadyEvent& event); void Execution(const ONotifyEvent& event); void NotifySensor(const ONotifyEvent& event); //Server関連 void ListenCont (void* msg); void SendCont (void* msg); void ReceiveCont(void* msg); void CloseCont (void* msg); //Server関連ここまで private: void OpenPrimitives(); void NewCommandVectorData(); void SetJointGain(); void SetJointGain(int leg); void JointGainOff(int leg); void AdjustDiffJointValue(); void AdjustDiffJointValue(int leg); RCRegion* FindFreeRegion(); void ReadMotionTable(const char* path); void WriteMotionTable(const char* path); void ShowMotionTable(); void ReadWalkTable(const char* path); void WriteWalkTable(const char* path); void ShowWalkTable(); void ShowWalkNameList(); void ActionStart(); void StoreWalkData(vector<string>* vs); char* PresentWalkData(int index); void StoreMotionData(vector<string>* vs); char* PresentMotionData(int index); char* PresentLegData(int leg); char* PresentLegsData(); char* PresentAngleData(LegData* leg); void PresentPostureData(Posture* pos, char* ptr); void ReportResult(ResultToHV* mes); void StoreTargetPosture(vector<string>& vs); void StoreTargetDirection(vector<string>& vs); void* StoreParams(vector<string>& vs, MovingMode mode); void StackParams(void* ptr, MovingMode mode); void PrintStack(); void PopStack(); void InitSensorIndex(OSensorFrameVectorData* sensorVec); //解析系 void PresentFFTResult(char* data); //解析系ここまで //制御系 void Execution(int index); // IkmMode GetCommand(char* prompt); //制御系ここまで //Server関連 OStatus Listen (int index); OStatus Send (int index); OStatus Receive(int index); OStatus Close (int index); OStatus InitTCPConnection(int index); void SetReceiveData(int index); void SetSendData(int index, void* ptr, int size, const char* header); //Server関連ここまで static const size_t NUM_COMMAND_VECTOR = 2; static const size_t NUM_JOINTS = 16; static const size_t LINEBUFSIZE = 256; static const size_t TXTBUFSIZE = 16; IKmoveState ikmoveState; OPrimitiveID jointID[NUM_JOINTS]; RCRegion* region[NUM_COMMAND_VECTOR]; bool initSensorIndex; int jointidx[NUM_JOINTS]; Body body; MovingResult SetJointValue(int maxframe); IKpos gHead; //頭の重心位置 ActionCtrl ac; map<int, MotionData*> motionDataMap; //モーションデータ　//vectorでも良かったかも。 map<int, WalkParam*> walkDataMap; //歩行パラメータ //Server関連 antStackRef ipstackRef; TCPConnection connection[COMMSERV_CONNECTION_MAX]; //Server関連ここまで //関節データ int legDataSend; //関節データを表示する足のインデックス(0なら送らない） int sendFFT; //FFT計算結果をクライアントへ送るか？（0なら送らない） //FFT解析用 FFT fft1; //この段階では初期化されていない。 FFT fft2; //この段階では初期化されていない。 list<angleDataSet> ld; //姿勢監視 int waitCount; // bool end_timing; //アクションスタック list<DividedAction> stack; };

[topへ]

■３　各クラスについて

■　Body

class Body { public: Body(); ~Body(){} PostureData indicate; //指示値 PostureData real; //実際値　←実際値はセンサー値を適用して姿勢・重心点を計算する。 //更新タイミングがあるので常に正しいとは限らない。測定ﾌﾚｰﾑﾅﾝﾊﾞｰ、要求、回答などの仕組みが必要か。 int SetPos(LegPos legs[]); int SetPos(LegData legs[]); void SetAngle(LegData legs[]); void SetPosture(Posture &p); int SetTargetPos(LegPos legs[]); void SetTargetAngle(LegData legs[]); void SetTargetPosture(Posture &p); void SetTargetPosture(PoseType); void SetMode(MovingMode m); void SetMotionMode(MotionMode m); void SetSpeed(double s[]); void SetSpeed(double s); void SetFrame(double f); void SetMotionData(MotionData*); void SetKeyData(); void SetWalkParam(WalkParam*); void SetWpSp(WalkParam*); void SetRefLeg(int, int, int, int); void SetRefLeg(int r[]); void SetPhi(double); void PrintMotionData(char*); bool CheckTarget(); MovingResult Next(); MovingResult NextMotion(); MovingResult NextWalk(); MovingResult NextPosture(); MovingResult NextAdjust(); MovingMode GetMode(); void SetStop(); void ResetStop(); bool SetPose(PoseType); //modeがmmPoseならPoseType登録。 void Von(); void Voff(); void Vtoggle(); bool GetV(); ResultType GetRT(); //歩の切れ目、モーションの切れ目を知る。 void SetWalkStyleCmpLevel(int lvl); private: void SetInitMotionAxis(); void SetInitMotionAngle(); void SetInitMotionPosture(); int NextMotionAxis(); int NextMotionAngle(); int NextMotionPosture(); void SetInitWalk(WalkParam &_wp); void SetInitChangePosture(); void SetInitAdjustDir(); bool WalkStyleCheck(int lvl); //現在姿勢が歩行姿勢かどうかをチェック。事前にSetInitWalk() void CalcWalk(const double rad, Posture &p); int CalcWalk(const double rad); Posture RevCalcWalkStyle(double rad); //現姿勢をradの歩行状態とするPostureを返す。 void MakeWalkParamFromPos(Posture p, WalkParam &w); //wpとpをマージしてshiftWpを作成。 void CalcShiftPosWalk(const double rad, Posture &p, const WalkParam &w); //姿勢移行歩行　radの歩行姿勢pを返す。 int CalcSwPos(double rad); void CalcSwPos(double rad, Posture &ps); double CheckSwPos(); bool CheckStance(); int CalcAdjust(const double radian); double StepLegFunc(double rad, int leg, const WalkParam &rw); double HungLegFunc(double rad, int leg, const WalkParam &rw); double SwPosFunc(double rad, int leg, const WalkParam &rw, double sp = 0); double HungLegFunc2(double rad, int leg); double Xsign(int a, int b); double Ysign(int a, int b); int Xreverse(int leg); int Yreverse(int leg); //general MovingMode mode; list<PoseType> pose; bool stop; bool gain; //未実装 //common bool first; Posture startPosture; //姿勢モーション・姿勢移行で使用 Posture subPosture; //姿勢モーション・姿勢移行で使用 //motion MotionData* md; //現状のモーションデータへのポインタ //歩行で言うik.wpに相当する？モーションデータはikクラスで管理した方がよいか？ vector<KeyData>::iterator itev; //現状のキーフレームを示す。//listでも良かったかも。 MotionMode motionMode; PostureData target; double speed[4]; //足毎の速度 double legframes[4]; //足毎の必要フレーム数 LegPos uvector[4]; //１フレームで移動する座標量 LegAngle uangle[4]; //１フレームで移動する角度量 double frames; //完了までのフレーム数 //walk WalkParam wp; //歩行パラメータセット WalkParam shiftWp; //姿勢移行歩行用パラメータセット int wsc_cmp_lvl; //歩行姿勢の照合レベル 0が厳しい。大きいほど緩い double sp; //歩行開始位相 double pc; //歩行済歩数 double spd; double accel; double width; //wp.thita2を考慮した左右足幅 double base; //wp.thita1を考慮した前後足間隔 double radius[4]; //各足の中心からの距離 double base_angle[4]; //各足のﾆｭｰﾄﾗﾙ位置角度 double w[4]; //aibo原点から見た各足の座標(x) double b[4]; //aibo原点から見た各足の座標(y) //posture WalkParam wpSp; //姿勢変更用歩行パラメータセット bool inertiaSwPos; //姿勢移行用　慣性フラグ（第一足を上げる場合に慣性をつけるため、0.5サイクル増しにするかどうか） int refLeg[4]; //姿勢移行時の基準足　1:基準　0:基準に非ず Posture offsetSwPos; //姿勢移行用　オフセット移行量 Posture stanceSwPos; //姿勢移行用　スタンス移行量 double defaultSwPosPdTable[4]; //姿勢移行用　位相テーブル double defaultSwPosXdir; double defaultSwPosYdir; //adjust double phi; //角度微調整用　回転角度指定 double radBorder; //この値より回転半径が小さい足は基準足とする。 double offsetSA[4]; //角度微調整用　送り足動作における初期位置差分 offsetSA=offsetStartAngle // ResultType rt; //毎歩ごとにtrueに。読み出すとリセットされる。 bool v; //メッセージスイッチ };

■　関数

double StepLegFunc(double rad, int leg, WalkParam &rw)
足の前後の動作を生成する。

double HungLegFunc(double rad, int leg, WalkParam &rw)
足の上下の動作を生成する。

double SwPosFunc(double rad, int leg, WalkParam &rw)
姿勢移行時、足を上げている期間に姿勢の差分をシフトする動作生成に使用する。

この関数の生成はちょっとわかりづらいので補足的な図をつけておく。

dutyは1サイクル中に足を上げている期間なので、その期間でリニアに0から1に変化する。それ以外は足は接地しているので
変化しない。

double HungLegFunc2(double rad, int leg)

■　PostureData

class PostureData { public: PostureData(); ~PostureData(){} LegData leg[4]; Posture posture; void SetPosture(LegPos lo[], double h, double t1, double t2); void SetPosture(WalkParam &w); void SetPosture(Posture &p); int SetAxis(LegPos l[]); int SetAxis(LegData l[]); void SetAngle(LegData l[]); void SyncAxis(); //座標値に姿勢値を同期させる。 int SyncPosture(); //姿勢値に座標値を同期させる。 void Print(char* title); void GetGravity(IKpos h); void Von(); void Voff(); private: int ConvPosAxis(); void ConvAxisPos(); void ConvAxisPos(int f, int r); Point xp; //接地中心 Gravity gp; //重心点 static bool v; //メッセージフラグ };

■　LegData

class LegData { static int id_count; public: LegData(); LegData(int); ~LegData() {} int leg; LegPos pos; LegAngle angle; void SetLeg(int); int SetPos(double, double, double); //IK int SetPos(LegPos &); //IK int SetPos(LegData &); //IK void SetAngle(slongword, slongword, slongword); //IK void SetAngle(LegAngle &); //IK int SyncPos(); //座標に角度を同期させる IK void SyncAngle(); //角度に座標を同期させる KI int Add(const LegPos &); void Add(const LegAngle &); void Print(char*); bool CheckAngle(LegAngle &ang); Gravity GetGravityPoint(); //ローカル座標系の値を返す Gravity GetGravityPointG(); //アイボ座標系に変換した値を返す int GetId(); void Von(); void Voff(); private: void ConvKI(); //順運動計算 int ConvIK(); //逆運動計算 double lt; double dt; double la2; double lb2; double de2; double lmax2; double lrh; double lth; Gravity gr; //重心点１ Gravity gt; //重心点２ Gravity gl; int id; static bool v; //メッセージフラグ };

[topへ]

■４　構造体

■DirectionToIKMと関連構造体

他のオブジェクト（主に頭部モジュールを想定）との通信のためのクラス及び構造体。DirectionToIKMのメンバーdataに対して、それ以下の各構造体を目的に応じてキャストして使用する。　objectcomm.hにて定義されている。

class DirectionToIKM { public: 　DirectionToIKM(){} 　~DirectionToIKM(){} 　DirectionType type; 　int stop; 　DirectionData data; 　void Print(); };

struct DirectionData { 　char padding[56]; };

struct DirectionMove { //指定座標へ移動する。 　double speed; //速度 　double x; //目標位置X 　double y; //目標位置Y 　double angle; //目標位置での相対角度 　double step; //歩幅 　WalkType walk; // 歩行タイプ 　int standing; // 1:停止後直立姿勢になる。 };

struct DirectionWalk { //指定方向に歩行する。 　double speed; //速度 　double cx; //目標位置X 　double cy; //目標位置Y 　double step; //歩幅 　double cycle; //歩数 　WalkType walk; // 歩行タイプ 　int standing; // 1:停止後直立姿勢になる。 };

struct DirectionMotion { 　MotionType motion; 　char padding[52]; };

struct DirectionSwitch { 　double speed; 　double dx; 　double dy; 　double thita1; 　double thita2; 　double height; 　char padding[8]; };

struct DirectionAdjust { 　double speed; 　double angle; 　double cx; 　double cy; 　double cycle; 　char padding[16]; };

struct DirectionHeadInfo { 　IKpos head; 　char padding[32]; };

struct ResultToHV { 　ResultType result; //動作結果・現状を通知 　longword fn; //フレームナンバー 　Posture posture; //現在の姿勢を通知 result が　rt****Endの時のみ有効。 };

■WalkParam

struct WalkParam { 　string name; //歩行セット名 　double xamp; //左右の振幅 　double yamp; //前後の振幅 　double freq; //周波数 　double cycle; //サイクル数 　double height; //高さ 　double thita1; //前後傾き 　double thita2; //左右傾き（未実装） 　double h_offset[4]; //各足の高さオフセット 　double stancex[4]; //左右の足位置 　double stancey[4]; //前後の足位置 　double hungHgt; //足上げ高さ 　double stepAmp; //歩幅 　double stepAngle; //回転移動の歩幅（角度） 　double cx; //回転中心左右 　double cy; //回転中心前後 　double duty; //離地率 　double pdTable[4]; //位相遅れテーブル 　double xdir; //揺れ極性 　double ydir; //揺れ極性 　double accel1; //加速時加速度 　double accel2; //減速時加速度 cf. accel2 > 0 　double accel3; //速度変化時加速度 　WalkParam(); 　WalkParam(WalkParam* n); 　void Set(WalkParam* n); 　void Copy(WalkParam* n); };

■MotionData型　KeyData型

struct MotionData { 　string name; 　int keyframes; 　vector<KeyData> keyDataMap; };

struct KeyData { 　MotionMode mode; 　double speed[4]; 　int frames; 　LegPos legpos[4]; };

[topへ]

■５　型

■ResultTypeとDirectionType

オブジェクト間通信で使用する。状態変数。objectcomm.hで定義されている。

enum ResultType { 　rtUnknown, //posture 未確定 　rtWalking, //歩行中（1歩の動作の最中）姿勢変更の類は歩行に属する。 　rtStep, //１歩（posture更新） 　rtMoving, //モーション再生中（キーフレーム間） 　rtMotionKey, //モーションキーフレーム（posture更新） 　rtStop, //停止（posture更新） 　rtStoped, //停止中 　rtGainOff };

enum DirectionType { 　dtMove, 　dtWalk, 　dtApproach, 　dtMotion, 　dtSwitch, 　dtAdjust, 　dtHeadInfo };

■MotionMode型

enum MotionMode { 　MM_IK_SPD_SYNC, //線形移動で速度制御で同調（もっとも遅い足に合わせる） 　MM_IK_SPD_ASYN, //線形移動で速度制御で非同調（動作完了時期が合わない） 　MM_IK_FRM_SYNC, //線形移動でフレーム数制御（同調） 　MM_ANG_SPD_SYNC, //角度移動で角速度制御で同調 　MM_ANG_SPD_ASYN, //角度移動で角速度制御で非同調 　MM_ANG_FRM_SYNC, //角度移動でフレーム数制御（同調） 　MM_AANG_SPD_SYNC, //角度移動で角速度制御で同調(データは角度) 　MM_AANG_SPD_ASYN, //角度移動で角速度制御で非同調(データは角度) 　MM_AANG_FRM_SYNC, //角度移動でフレーム数制御(データは角度) 　MM_POS_ABS, //姿勢制御で絶対値制御（速度制御） 　MM_POS_REL //姿勢制御で相対値制御（速度制御） };

[topへ]

■６　定数

■７　座標系・姿勢系

■各脚の座標

各脚の座標は異なる。関節構造を基本に考え、各脚に対して同じ関数を適用できるようした。
Ｚ軸は通常の直交座標系に対して符号が反転している。足の付け根を原点として正の数値を取れるようにした。

■姿勢

姿勢は高さと角度の他に各脚のオフセットで構成されている。
角度の正負により高さを示す点が異なる。
各脚のオフセットは、X,Y値は脚付け根に対する足先座標の差を示す。
Z値は高さと角度により決定する基本Z値との差を示す。

姿勢のZ値を除くと脚座標系と姿勢系は一意に変換できる。（姿勢のZ値については前後それぞれの片側を0とおくことで一意となる）

■８　アイボ歩行エディター

PCアプリケーションを使って、歩容やモーションを編集する。

パラメータを設定し、コマンドを選んで実行（EXECUTE）する。
コマンドは　STORE WALK PLAY SAVE LOAD NEW DELETE MTSV MTLD がある。

モーションエディトにチェックすることでモーションエディトフォームが開く

姿勢移行にチェックすることで、姿勢移行フォームが開く

■モーションエディトフォーム

モーションの編集を行う。

モード

各足のチェックを外すと足のゲインが切れる。

■姿勢移行フォーム

姿勢移行と角度微調整の動作確認を行うフォーム。

■コマンド

WALK 歩行 　walk legDataSend sendFFT 　walk:歩行タイプナンバー 　legDataSend:関節角度表示制御 　sendFFT:FFT計算結果送信 PLAY モーション再生 　motion legDataSend sendFFT 　motion:モーションナンバー SWCH 姿勢移行 　stack 0:即時 1:スタック 　x y z :fr 　x y z :fl 　x y z :rr 　x y z :rl 　height thita1 thita2 　ref_fr ref_fl ref_rr ref_rl :支持脚 　freq xamp yamp hungHgt accel (accel1 = accel2 = accel) 　[legDataSend sendFFT] AJST 角度微調整 　stack 0:即時 1:スタック 　phi cycle cx cy freq xamp yamp hungHgt accl EXEC スタック実行 STOR 歩行パラメータストア 　歩行パラメータ列 SAVE 歩行パラメータテーブル保存 LOAD 歩行パラメータをPCにロード MTSV モーションデータテーブル保存 MTST モーションデータストア 　モーションデータ列 MTLD モーションデータをPCにロード FRAM GAOF ゲインOFF 　leg GAON ゲインON 　leg RDLG 足関節角度送信要求 CPOS 座標を姿勢データに変換 CANG 座標を角度データに変換 STOP 停止 MSGT メッセージトグル

■９　課題

現状の不足点、要改良点を上げる。

1. 歩行姿勢への移行が必要　現姿勢から直接歩行を開始できるようにしたい。【完了】
2. 角度微調整モードでの動作は支点となる足は接地したままとなるが、移動角度が大きいと動作可能範囲の関係で動けなくなる（動作に誤差が出る）。移動角度が大きい場合は適度に支点となる足も動かすようにする。

[topへ]