SMPL1(PlayMotion) のソース :: Besttechnology

• 差分^[1] を表示
• SMPL1(PlayMotion)^[2] へ行く。

只今編集中です。

**初めに [#mcf2cd20]
いきなりですがモーションを動かすサンプルプログラムです。~
モーションを再生するのは簡単です。1行書くだけです。~
1行書くだけとは言ってもそのために準備が必要です。~
準備はどのプログラムでも大体同じですから、まずは深く考えずコピペ(コピー&ペースト)します。~
~
最初のサンプルプログラムなので以下少し長い説明になってしまいましたが、難しい所は省略していますのでさらっと読んで頂くだけでもイメージはお分かり頂けると思います。

**モーションとは [#d1bffd31]
ここで言うモーションとは、ロボットの動作のことです。手を振ったり、お辞儀したり、起き上がったり、FREEDOM jr.IIIの動きはほとんどモーションです。~
モーションはよくペラペラ漫画に例えられます。ペラペラ漫画をもっと荒く（コマ数を少なく）したものです。例えばお辞儀するモーションなら3コマでできます。「直立姿勢」->「お辞儀した姿勢」->「直立姿勢」です。それを繋げてお辞儀する動作になります。~
~
[お辞儀の画像]~
~
大抵ロボットでモーションを再生するとロボットダンスのようにカクカクした動きになります。しかしFREEDOM jr.IIIは、コマとコマの間を補間して加減速することで滑らかな動きを実現しています。~
~
[モーション再生の動画]~
~
FREEDOMjr.IIIの歩行はモーションではありません。言葉の意味で言えばロボットが動くのでモーションと言えばモーションなのですが、上で説明したペラペラ漫画の要領で歩いているのではないのでここでは区別してモーションではないと言います。

**モーション再生のための準備(データ編) [#hb810bee]
早速サンプルプログラムの中身を見て行きましょう。SMPL1(PlyaMotion).cをGCC Developer Liteで開いてください。~
これがC言語のプログラムです。/*と*/で挟まれている所や行の先頭に//と書いてある字が水色の所はコメントです。コメントはプログラムを初めて見た人が分かるようにするための説明書きです。とりあえずその部分は飛ばして見て行きます。~
ここでは詳しく説明しませんので簡単に読んで先に進んでください。

***インクルード [#tc2bab38]
 // FREEDOMライブラリ
 #include <fd.h>
 // 人型構造定義
 #include "share/Humanoid16Axis.h"
 // 人型へのアクチュエータの割り当て
 #include "share/HumanoidServoParam.c"

最初にコピペする所です。詳しくは別の機会に説明します。
***ホームポジション [#tdad2989]
 /*=========================================================*/
 /* ホームポジションデータ(アクチュエータの制御値 0～1023)  */
 /*=========================================================*/
 const THumanoid Homeposition = {
  Servos:{
    RightLeg:{  511,   513,   718,   617,   514,},
    LeftLeg: {  520,   515,   300,   410,   510,},
    RightArm:{  512,   247,   464,},
    LeftArm: {  512,   761,   570,},
  },
 };
FREEDOMjr.IIIの全ての基準となる大事なデータです。大事ですが別の機会に説明します。
***モーションデータ [#d6c1f8b3]
 /*=============================================================*/
 /* モーションデータ(ホームポジションからのオフセット角度(x10倍)*/
 /*=============================================================*/
 // ホームポジションへ移動
 const THumanoidMotion GotoHome[] = {
   { Servos:{
       RightLeg:{    0,     0,     0,     0,     0,},
       LeftLeg: {    0,     0,     0,     0,     0,},
       RightArm:{    0,     0,     0,},
       LeftArm: {    0,     0,     0,},
     },
     Adj:ADJ_SACC_SDECEL, Div:2000,
   },
 };
 　　・
 　　（中略）
 　　・
   { Servos:{
       RightLeg:{    0,     0,     0,     0,     0,},
       LeftLeg: {    0,     0,     0,     0,     0,},
       RightArm:{    0,     0,     0,},
       LeftArm: {    0,     0,     0,},
     },
     Adj:ADJ_SACC_SDECEL, Div:1000
   },
 };
これがモーションのデータです。同じ塊が何個も並んでいるように見えると思いますがこれがペラペラ漫画の1コマです。詳しくは別の機会に説明します。
***モーション適用部位 [#k582c8c2]
 /*==============================================================*/
 /* モーションの適用部位設定                                     */
 /*==============================================================*/
 // 全身 (優先度1)
 TApplyPart WholeBody_1 = {
   Priority:  1,
   PartNum:  16,
   part:{
      &((THumanoid*)0)->Servos.RightLeg[0],
      &((THumanoid*)0)->Servos.RightLeg[1],
      &((THumanoid*)0)->Servos.RightLeg[2],
      &((THumanoid*)0)->Servos.RightLeg[3],
 　　・
 　　（中略）
 　　・
 // 右腕のみ (優先度10)
 TApplyPart OnlyArm = {
   Priority: 10,
   PartNum:   6,
   part:{
      &((THumanoid*)0)->Servos.RightArm[0],
      &((THumanoid*)0)->Servos.RightArm[1],
      &((THumanoid*)0)->Servos.RightArm[2],
   }
 };
これはモーションを体のどの部分が再生するかを設定するものです。普通は体全体がモーションを行いますが、FREEDOMjr.IIIでは好きな所だけを動かすことができます。これにより体の部位によって別々の動きをすることができます。体の分類の仕方も自由に決められます。詳しくは別の機会に説明します。~
~
ここまでは下準備です。本当はこんなに沢山書かなくても良いのですが説明のために書いています（ここでは説明しませんが）。~

**モーション再生のための準備(初期化編) [#laca6790]
mainのプログラムを見て行きます。文字通りここがメインの(主となる）プログラムです。~
 /*=============================================================*/
 /* Main                                                        */
 /*=============================================================*/
 void main (void) {
   // ライブラリ初期化
   fd_Init (115200, false, _MONITOR_VOLTAGE);
   fd_Wait (2000);
   // サーボパラメータ初期化
   fd_SetServoParameter (ConvToPPose (&Homeposition), SVParam, fd_SVSize(SVParam));
 　　・
 　　・
ここからは少しだけ詳しく説明します。
***メインプログラムの始まり [#a1ba6434]
 void main (void) {
メインのプログラムの始まりです。一番下の}がメインのプログラムの終わりです。
***初期化関数 [#seea5dd0]
   // ライブラリ初期化
   fd_Init (115200, false, _MONITOR_VOLTAGE);
FREEDOMjr.IIIを初期化する関数です。FREEDOMjr.IIIを動かすには最初に必ず初期化します。fd_Initが関数の名前です。その後ろの括弧で囲まれカンマで区切られた3つの数字/文字が関数のパラメータ（引数）です。以下引数を紹介します~
 115200
これは通信の速度(Baudrate)です。PCと直接接続する時にはPCとの通信速度、無線機器を使う場合はFREEDOMjr.IIIに搭載した無線機器との間の通信速度になります。前者の場合はSIMPLE TERMのBaudrateをこれに合わせる必要があります。後者の場合はFREEDOMjr.IIIに搭載した無線機器のBaudrateをこれに合わせる必要があります。~
 false
これは無線コントローラを使用するかどうかです。true(正)で無線コントローラを使う、false(誤)で使わないとなります。ここでは使わないのでfalseにします。~
 _MONITOR_VOLTAGE
これはは電圧が何ボルトを下回ったらアラームを鳴らすかです。電池を使って動かしている場合、電池の残量が減ってくると電圧が下がってきます。_MONITOR_VOLTAGE以下になるとアラームが鳴りそれを知らせてくれます。Lipoバッテリは電池の残量が少なくなっているまま使用し続けると破損してしまいます。このアラームがなってから充電するのではなく、アラームが鳴らないようにこまめに電池の残量をチェックし、減ってきたら充電してください。詳しくはバッテリのマニュアルをご覧ください。
***サーボパラメータ初期化関数 [#p3209e77]
   fd_SetServoParameter (ConvToPPose (&Homeposition), SVParam, fd_SVSize(SVParam));
サーボモータ(FREEDOMjr.IIIの関節)のパラメータを設定します。fd_SetServoParameterが関数の名前です。以下引数を紹介します。
 ConvToPPose (&Homeposition)
これはFREEDOMjr.IIIのホームポジションです。モーション再生のための準備(データ編)ででてきました。それをここで設定しています。~
ConvToPPoseは型の変換をしています。変数には型があります。型はその変数がどういう変数かを表します。つまりHomepositionがどういう変数かを変換しています。中味が変わってないのにどういう変数かを変えていいのかと思いますが、変換しても大丈夫なように変数を作っていますから大丈夫です。何故そんなことをするのかという事を説明すると少し話が長くなりますが、短く言うと人間が見やすい型からマイコンが計算しやすい型に変換しています。~
試しにConvToPPoseを消してコンパイルしてみましょう。下のログに黄色いwarningがでます。型が違いますよと言っています。それでもコンパイルは成功します。これをFREEDOMjr.IIIに書き込んで動かすと普通に動きます。型は違うけど中味は一緒なので普通に動くというわけです。ただwarningが出るのが嫌なのでConvToPPoseをしています。~
説明が長くなりましたがとりあえずはそういうものだと考えてください。
 SVParam
これはサーボのパラメータです。この値は別のファイルに書かれています。モーション再生のための準備(データ編)の最初に#include "share/HumanoidServoParam.c"というのがありましたがshareフォルダのHumanoidServoParam.cというファイルの中にSVParamが書かれています。SVParamの詳細は別の機会に説明します。
 fd_SVSize(SVParam)
これはSVParamのサイズです。fd_SVSizeはSVParamのサイズを計算する関数です。SVParamのサイズが分かっていればその値を直接書いても良いのですが、間違えると大変なことになるのでこのように書きます。
**モーションの再生 [#ca784938]
簡単と言いながらここまで長い説明になってしまいましたがようやくモーションの再生です。
  // ホームポジションへ移動
  fd_SetMotion (ConvToPPose (GotoHome), 1, fd_MSize(GotoHome), 100, &WholeBody_1);
モーションの再生はこの1行だけできます。コメントにあるようにホームポジション（直立姿勢）になるモーションです。以下引数の説明です。
 ConvToPPose (GotoHome)
GotoHomeは直立姿勢になるためのモーションのデータです。モーションデータの所に
 // ホームポジションへ移動
 const THumanoidMotion GotoHome[] = {
   { Servos:{
       RightLeg:{    0,     0,     0,     0,     0,},
       LeftLeg: {    0,     0,     0,     0,     0,},
       RightArm:{    0,     0,     0,},
       LeftArm: {    0,     0,     0,},
     },
     Adj:ADJ_SACC_SDECEL, Div:2000,
   },
 };
があります。これを読み込んでいます。
 1
GotoHomeのモーションの1コマ目から再生することを表します。
 fd_MSize(GotoHome)
GotoHomeのサイズ分のコマまで再生することを表します。GotoHomeは1コマしかありませんのでfd_MSize(GotoHome)は1になります。1コマしかないのに間違えて2を入れると大変なことになりますのでfd_MSize(GotoHome)と書きます。
 100
モーションを再生する速さです。速さと言っても200にすると速くなる訳ではありません。モーションデータの中でコマからコマへ移動する時間を設定するパラメータがあります。その時間の長さに対して%で指定します。100ならモーションデータの中で設定されている速さ（時間の長さ）で再生します。200に設定するとモーションデータの中で設定されている時間の倍の時間でモーションを再生します。つまり1/2の速さになります。逆に50にすると半分の時間（2倍の速さ）で再生します。
 &WholeBody_1
FREEDOMjr.IIIのどの部位がモーションを再生するかを設定します。WholeBody_1は体全体です。その前についている&はポインタです。ポインタはアドレス（住所）です。データを関数にそのまま全部どさっと渡すよりは、「データはここのアドレスにあります」と言った方が計算が軽くなるので、関数にデータを渡す時はほとんどポインタになっています。
**キー入力 [#wbea9129]
  while(1) {
    char c;
    fd_putc (c = fd_getc ());
    switch(c) {
      // 直立
      case '1':  
        fd_puts ("\n Home");
        fd_SetMotion (ConvToPPose (GotoHome), 1, fd_MSize(GotoHome), 100, &WholeBody_1);
        break;
 　　・
 　　・
ここからはキー入力によってどのモーションを再生するかを決めています。whileやswitchについてはC言語の話ですのでC言語の参考書等を参照してください。
fd_getc()でキー入力を1文字取得しています。その値が1なら直立姿勢、2ならお辞儀、3なら腕振り、4なら片足立ちのモーションを再生します。実際にFREEDOMjr.IIIに書き込んで実行してみてください。
**まとめ [#ndfe56cb]
モーションの再生するには
 fd_SetMotion
です。他のデータを用意したり、初期化したりなどはどのプログラムでも大体同じですのでサンプルプログラムからコピペすれば大丈夫です。~
最後に直立するだけのプログラムをできるだけ短く書いてみます。
 #include <fd.h>
 #include "share/Humanoid16Axis.h"
 #include "share/HumanoidServoParam.c"
 #include "share/HumanoidSampleFlashData.c"
 
 TApplyPart WholeBody = {
   Priority:  1,
   PartNum:  16,
   part:{
      &((THumanoid*)0)->Servos.RightLeg[0],
      &((THumanoid*)0)->Servos.RightLeg[1],
      &((THumanoid*)0)->Servos.RightLeg[2],
      &((THumanoid*)0)->Servos.RightLeg[3],
      &((THumanoid*)0)->Servos.RightLeg[4],
      &((THumanoid*)0)->Servos.LeftLeg[0],
      &((THumanoid*)0)->Servos.LeftLeg[1],
      &((THumanoid*)0)->Servos.LeftLeg[2],
      &((THumanoid*)0)->Servos.LeftLeg[3],
      &((THumanoid*)0)->Servos.LeftLeg[4],
      &((THumanoid*)0)->Servos.RightArm[0],
      &((THumanoid*)0)->Servos.RightArm[1],
      &((THumanoid*)0)->Servos.RightArm[2],
      &((THumanoid*)0)->Servos.LeftArm[0],
      &((THumanoid*)0)->Servos.LeftArm[1],
      &((THumanoid*)0)->Servos.LeftArm[2]
   }
 };
 
 void main (void) {
   fd_Init (115200, false, _MONITOR_VOLTAGE);
   fd_SetServoParameter (ConvToPPose (&HomePos), SVParam, fd_SVSize(SVParam));
   fd_SetMotion (ConvToPPose (WakeUp), 1, fd_MSize(WakeUp), 100, &WholeBody);
   while(1);
 }
こうなります。良く見ると最初の方に
 #include "share/HumanoidSampleFlashData.c"
というのが増えているのがわかります。実はこのHumanoidSampleFlashData.cの中にホームポジションやモーションのデータが入っています。それをインクルードすればこれだけで済むのです。

• SMPL1(PlayMotion) のバックアップ一覧^[4]
• SMPL1(PlayMotion) のバックアップソース(No. All)
  • 1: 2009-06-22 (月) 19:32:50^[5] sho^[6]
  • 2: 2009-06-22 (月) 20:20:18^[7] sho^[6]
  • 3: 2009-06-23 (火) 13:08:30^[8] sho^[6]
  • 現: 2009-06-23 (火) 18:12:08 sho^[6]