Dynamixel AX-12での歩行モーションでH8の限界が見えた

h83052.jpg


逆運動学ちっくにXYZから各関節角をもとめるプログラムを書いて、軌跡から歩行モーションのパターンを生成するプログラムを書いた。

ただ、これだとどうも、AX-12の剛性が弱すぎて、体重移動したあと片足を浮かせることができない。
よって、体重移動に合わせて、軸足のロール軸だけを体重移動を戻す方向に回転させることで、負荷をかけ片足を上げたときにトルクがつりあうような補正をかけることで、足踏みモーションを作った。

そこから、正弦波を用いた軌跡を関数でリアルタイムにパターンを作って実行する。

プログラムは下に書いてみた。
たぶん、コメントがないのでよく分からないと思うし、構造的に移植することはできないと思うが、問題なのは実行速度。

H8/3052だとこれを実行したとき1秒に4ループくらいしか実行してくれない。
double型の三角関数と逆三角関数をなんども計算するためだろうか・・・。
AX-12のボーレートをを56kでとっているので、その速度の性もあるかもしれないが、それでも遅いだろう。
計算なしで値だけ送ると1秒を20分割くらいは出来るので・・・。

となると、計算速度。
パイプライン処理できるRISCのマイコンは必須だろうか・・・。
ATmega128が1枚あるが・・・、それを使うか、とうとうSH2に手を出すか・・・。

sinテーブルを100倍したint型で配列で処理するという手もないこともないが、いつか限界が来ることを考えると、出来れば実行速度を上げたいところだと思う・・・。

あと、AX-12についてだが、内部温度エラーでトルクオフした際、マニュアルではTorque Enableを1にすればOKと書いてますが、上手くいかなかったのでベステクさんにメールしたところ

運用的に従来の方法(アラーム停止→最指令→復帰→連続運用)では
あえて復帰しない様にしてあります。
同時にトルクリミット値が0になりますのでアラーム要因が解消してい
れば再設定する事で動作します。

とのことでした。

以下が例のプログラム
Z方向の軌跡は正弦波の上半分だけ。
原点は腰が(0,0,0)で、前方にX軸、右方向にY軸、下向きにZ軸の正の方向を取る。

//左足
int new_xyz_r(int x,int y,int z){
  double l,a,b,c,d;
  int leg = 77;
  l = sqrt(x * x + y * y + z * z);
  c = asin(1.0 * x / l) * 57.3;
  d = atan(1.0 * y / z) * 57.3;
  a = acos(0.5 * l / leg) * 57.3;
  b = asin(0.5 * l / leg) * 57.3;
  pos_sav[3] = 819 - 3.413 * b; if(safe(3)) return 1;
  pos_sav[4] = 205 + 3.413 * b; if(safe(4)) return 1;
  pos_sav[1] = pos_sav[6] = 665 - 3.413 * d; if(safe(1)) return 1;
  pos_sav[5] = 512 - 3.413 * ( c + a ); if(safe(5)) return 1;
  pos_sav[2] = 512 - 3.413 * ( c - a ); if(safe(2)) return 1;
  return 0;
}
//左足
int new_xyz_l(int x,int y,int z){
  double l,a,b,c,d;
  int leg = 77;
  l = sqrt(x * x + y * y + z * z);
  c = asin(1.0 * x / l) * 57.3;
  d = atan(1.0 * y / z) * 57.3;
  a = acos(1.0 * l / leg) * 57.3;
  b = asin(1.0 * l / leg) * 57.3;
  pos_sav[9] = 205 + 3.413 * b;if(safe(9)) return 1;
  pos_sav[10] = 819 - 3.413 * b;if(safe(10)) return 1;
  pos_sav[7] = pos_sav[12] = 358 - 3.413 * d; if(safe(7)) return 1; //ここの符号でyの方向
  pos_sav[11] = 512 + 3.413 * ( c + a ); if(safe(11)) return 1;
  pos_sav[8] = 512 + 3.413 * ( c - a ); if(safe(8)) return 1;
  return 0;
}
void sin_g_move(int b,int k,int n,int hr , int hl , int go,int go2,int up ,int inz,int gain){ 
 // b    : 揺れ幅
 // k    : 回数
 // n    : フレーム分割数
 // hr   : 右足の歩幅
 // hl   : 左足の歩幅
 // go   : 前に傾ける値(そのままサーボの値)
 // go2  : 前方への体重移動
 // up   : 上がる足の高さ
 // inz  : 初期の足の長さ
 // gain : 足の付け根のロール軸補正ゲイン
  int i,y,j,rx,lx,rz,lz;
  for (j=0;j<k;j++){
    for (i=0;i<n;i++){
      y = 1.0 * b * sin(3.141592 * 2 * i / n);//基準位相
      rx = 1.0 * hr * sin(3.141592 * 2 * i / n - 3.141592 / 2 );//位相90-
      lx = 1.0 * hl * sin(3.141592 * 2 * i / n + 3.141592 / 2 );//位相90+
      rz = inz - 1.0 * up * sin(3.141592 * 2 * i / n );//位相180-
      lz = inz + 1.0 * up * sin(3.141592 * 2 * i / n);//
      //足上げだけZ軸ポジション確定
      if (rz > inz) rz = inz;
      if (lz > inz) lz = inz;
      //go2は体重移動の補正(8がBEST)
      new_xyz_r(rx - go2 ,y,rz);
      new_xyz_l(lx - go2 ,y,lz);
      //左右にゆれたときの腰の加重に関する補正(gain5がBEST)
      if (y < 0) pos_sav[6] = pos_sav[6] - ( -y ) * gain;
      if (y > 0) pos_sav[12]= pos_sav[12]+ (  y ) * gain;
      //前かがみの補正(20BEST)
      pos_sav[5]= pos_sav[5] - go;
      pos_sav[11]= pos_sav[11] + go;
      all_send();
    }
  }
}

Profile

me.jpg
Name : みっちー
小学校で電子工作にハマり、高校時代はゲームプログラミング、大学時代にロボット製作へどっぷり浸かりました。
社会人になっても、なにかとものを作るのが大好きで、日々ネタと仲間を求めて活動中です。

Dream Drive!!

bana.png
http://dream-drive.net

趣味のロボットとプログラムを中心としたコンテンツのサイトです。

My Robots

kolink2.jpg
KO-LINK2
第2世代 並行リンク足ロボット

kolink.jpg
KO-LINK
並行リンク足ロボット

BusterGX.jpg
BusterGX (Ex Walker)
G-ROBOT + ROBO-XERO

kumaco.jpg
KUMACO
ぬいぐるみロボット

SARUBO.jpg
SARUBO
JX-SYSTEM搭載ぬいぐるみロボット

kokuten3.jpg
KOKUTEN 3
リアルタイム逆運動学制御ロボット

meros.jpg
MEROS
膝なしロボット
(胸部マトリックスLED搭載)
by Mechaniker

kokuten12.jpg
KOKUTEN 1&2
はじめてのロボット
by Mechaniker

協賛広告

月別 アーカイブ

このブログ記事について

このページは、みっちーが2006年8月19日 23:00に書いたブログ記事です。

ひとつ前のブログ記事は「ぶたとDirect3Dのデバッグ」です。

次のブログ記事は「夏戦II in ROBO Country IVを振り返って。」です。

最近のコンテンツはインデックスページで見られます。過去に書かれたものはアーカイブのページで見られます。