RPU-10で6軸逆運動学の計算と速度

Tweet

さっそくRPU-10(内部はATmega128)をGDLにてプログラムしてみました。

ザックリ移植したんで、変数名がへっぽこなのはご勘弁ください。
何をしたかというととりあえず、RPU-10のスペックが知りたかったので、逆運動学の計算を延々とさせるプログラムを書いてみました。

//----------------------------------------------------------------------------------------
// 逆運動学の計算速度の計測テスト
//
// 環境 RPU-10、GDL V2.00
// 説明 右足2回でワンセット、2回計算するごとにMoniterLEDを点滅させます。
// 高速なので目視ではわかりませんが、オシロスコープで計測します。
// だいたい、10msec間隔で波形が確認できると思います。
// 途中からゴミみたいな変数名ですみませんorz
//
// AUTHORED BY Micchy @ Dream Drive !!
//----------------------------------------------------------------------------------------
#include <avr/pgmspace.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <stdio.h>
#include <math.h>
#include <avr/boot.h>
#include <avr/wdt.h>

#include <sv.h>
#include <rs.h>

#define SQUARE(x) ((x) * (x))
#define RADIANS(deg) ((deg) * M_PI / 180.0)
#define DEGREES(rad) ((rad) * 180.0 / M_PI)
#define a 48
#define b 59
#define c 5
#define d 28
#define e 2.8

//6軸のサーボの値を一時的に保存するグローバル変数
int sv1,sv2,sv3,sv4,sv5,sv6;

int xyz(int goal_x,int goal_y,int goal_z,float goal_yo,float goal_pi,float goal_ro){

 float x,y,z,yo,r,si,dax,day,daL,he_s,he_das,h,arufa,beta,d_arufa,d_beta,dd_beta;
 float sig,ipu,aaa,bbb,ccc,ddd,eee,fff,ggg,hhh,iii;
 float temp;

 x = goal_x + d;
 y = goal_y - e;
 z = goal_z;
 yo = goal_yo;

 r = sqrt( (x * x ) + ( y * y ) );
 if (x==0 && y==0)si = 0;
 else si = atan2(y,x);
 temp = si - RADIANS(yo);
 dax = r * cos(temp) - d;
 day = r * sin(temp) + e;

 temp = sqrt( SQUARE(z) + SQUARE(day) ) - c;
 daL =sqrt( SQUARE(dax) + SQUARE(temp) );

 he_s = (a + b + daL )/2;
 he_das =sqrt(he_s *(he_s - a)*(he_s-b)*(he_s-daL));
 h = 2 * he_das / daL;

 arufa = asin( h / a );
 beta = acos( h / a );
 d_arufa = asin( h / b );
 d_beta = acos( h / b );
 dd_beta = beta + d_beta;
 sig = asin(dax / daL);
 ipu = atan(day/z);

 aaa = DEGREES(arufa + sig);
 bbb = DEGREES(d_arufa - sig);
 ccc = 180-DEGREES(dd_beta);
 ddd = DEGREES(ipu);
 eee = yo;

 fff = goal_pi * cos(RADIANS(goal_yo));
 ggg = goal_pi * sin(RADIANS(goal_yo));
 hhh = -goal_ro * sin(RADIANS(goal_yo));
 iii = goal_ro*cos(RADIANS(goal_yo));

 sv1 = 10 * (-eee);
 sv3 = 10 * (ddd);
 sv2 = 10 * (aaa);
 sv4 = 10 * (-ccc);
 sv5 = 10 * (-bbb + hhh - fff);
 sv6 = 10 * (ddd - iii + ggg);

 return 0;
}

int main( void )
{

 RPU_InitConsole( br115200 ); // RPU-10ライブラリの初期化
 SV_Init( br115200 ); // サーボ制御ライブラリの初期化
 sei(); // 割り込み処理開始

// １秒待つ（よく知らないけど必要らしい）
 RPU_ResetTimerCounter();
 while( RPU_GetTimerCounter10() < 100 );

// 起動メッセージの表示
 rs0_puts( "逆運動学 計算速度 実測TEST" );

 while( 1 ){

  xyz(0,30,104,0,0,0); //右足分計算
  xyz(0,30,104,0,0,0); //右足分計算(2回目)
  LED_ON; //LED点灯
  xyz(0,30,104,0,0,0); //右足分
  xyz(0,30,104,0,0,0); //右足分計算(2回目)
  LED_OFF; //LED消灯

 }

 return 1;
}

G-ROBOTSは直交軸が微妙に直交してなかったり、意外と煩雑な計算になってます。
設計段階でちゃんと直交させれば計算は楽になるのですが、逆に可動域が小さくなるので、このあたりは、設計とプログラムのバランスですね。
もう少し簡単な機構なら計算が少なくなります。

で、実行してみた結果ですが、

こんな感じです。

RPU-10を分解してMoniterLEDの根本をオシロスコープでモニターしてみました。
2脚ワンセットの計算でMoniterLEDが約10msで点滅してます。
というわけで計算自体は10msで可能ということになります。

あと、サーボとの通信、コントローラーからのインプット、歩容の計算、これらを含めて最終的に制御周期を何msに出来るか…。
って感じですね。