空間加速度まで終わったー

100万ポイント山分け！1日5回検索で1ポイントもらえる

000000

ホーム | 日記 | プロフィール

【フォローする】【ログイン】

< 新しい記事

新着記事一覧(全1110件)

過去の記事 >

2020年10月18日

カテゴリ： Linux

前回作った、ロボットの胴体の位置・姿勢、空間速度、空間加速度から、再帰計算を使って全身の値を計算するサブルーチンを作った。
空間加速度、やっぱり実感わかないので、プログラムは印刷してマーカーでチェック、計算結果も一部抜粋して、エクセル上で確認計算。
・・・とりあえず、大丈夫だと思う・・・。

あと、外積のneon化ですが、うまく計算がハマれば、そこまで苦にならない感じがしました。これはこれで収穫かな。

neonは、スタック退避を考えたら、できるだけ少ないレジスタ群で計算したいところなので、ちょっと読みにくいコードになってしまいます。

コードを下に示します。

/********** 順運動学 **********/

voidFAK(LINK lin[],unsigned char j)

{

unsigned char i;

float32x4_t c0,c1,c2,r0,r1,r2,r3;

if ( j == 0 ) return;

if ( j == 1 ) {

} else {

i = lin[j].mother;

c0 = vld1q_f32( &lin[i].Rp[0] );

c1 = vld1q_f32( &lin[i].Rp[4] );

c2 = vld1q_f32( &lin[i].Rp[8] );

r0 = vmulq_n_f32( c0, lin[j].a[0] );

r1 = vmulq_n_f32( c1, lin[j].a[1] );

r2 = vmulq_n_f32( c2, lin[j].a[2] );

r0 = vaddq_f32( r0, r1 );

r3 = vaddq_f32( r0, r2 );

vst1q_f32( &lin[j].sw[0], r3 );

//lin[j].sv[0] = lin[j].Rp[13] * lin[j].sw[2] - lin[j].Rp[14] * lin[j].sw[1];

//lin[j].sv[1] = lin[j].Rp[14] * lin[j].sw[0] - lin[j].Rp[12] * lin[j].sw[2];

//lin[j].sv[2] = lin[j].Rp[12] * lin[j].sw[1] - lin[j].Rp[13] * lin[j].sw[0];

//lin[j].sv[3] = 0.0;

c0 = vdupq_n_f32(0.0);

c1 = vdupq_n_f32(0.0);

c2 = vdupq_n_f32(0.0);

r0 = vld1q_f32( &lin[j].Rp[12] );

c0 = vsetq_lane_f32( vgetq_lane_f32(r0,2),c0,1);

c0 = vsetq_lane_f32(-vgetq_lane_f32(r0,1),c0,2);

c1 = vsetq_lane_f32(-vgetq_lane_f32(r0,2),c1,0);

c1 = vsetq_lane_f32( vgetq_lane_f32(r0,0),c1,2);

c2 = vsetq_lane_f32( vgetq_lane_f32(r0,1),c2,0);

c2 = vsetq_lane_f32(-vgetq_lane_f32(r0,0),c2,1);

r0 = vmulq_n_f32(c0,vgetq_lane_f32(r3,0));

r1 = vmulq_n_f32(c1,vgetq_lane_f32(r3,1));

r2 = vmulq_n_f32(c2,vgetq_lane_f32(r3,2));

r0 = vaddq_f32(r0,r1);

r0 = vaddq_f32(r0,r2);

vst1q_f32( &lin[j].sv[0], r0 );

c0 = vld1q_f32( &lin[i].w[0] );

c0 = vmlaq_n_f32( c0,r3,lin[j].dq );

vst1q_f32( &lin[j].w[0], c0 );

c0 = vld1q_f32( &lin[i].v0[0] );

c0 = vmlaq_n_f32( c0,r0,lin[j].dq );

vst1q_f32( &lin[j].v0[0], c0 );

c0 = vdupq_n_f32(0.0);

c1 = vdupq_n_f32(0.0);

c2 = vdupq_n_f32(0.0);

r0 = vld1q_f32( &lin[i].v0[0] );

c0 = vsetq_lane_f32( vgetq_lane_f32(r0,2),c0,1);

c0 = vsetq_lane_f32(-vgetq_lane_f32(r0,1),c0,2);

c1 = vsetq_lane_f32(-vgetq_lane_f32(r0,2),c1,0);

c1 = vsetq_lane_f32( vgetq_lane_f32(r0,0),c1,2);

c2 = vsetq_lane_f32( vgetq_lane_f32(r0,1),c2,0);

c2 = vsetq_lane_f32(-vgetq_lane_f32(r0,0),c2,1);

r0 = vmulq_n_f32(c0,vgetq_lane_f32(r3,0));

r1 = vmulq_n_f32(c1,vgetq_lane_f32(r3,1));

r2 = vmulq_n_f32(c2,vgetq_lane_f32(r3,2));

r0 = vaddq_f32(r0,r1);

r3 = vaddq_f32(r0,r2);

r0 = vld1q_f32( &lin[i].w[0] );

r2 = vld1q_f32( &lin[j].sv[0] );

c0 = vsetq_lane_f32( vgetq_lane_f32(r0,2),c0,1);

c0 = vsetq_lane_f32(-vgetq_lane_f32(r0,1),c0,2);

c1 = vsetq_lane_f32(-vgetq_lane_f32(r0,2),c1,0);

c1 = vsetq_lane_f32( vgetq_lane_f32(r0,0),c1,2);

c2 = vsetq_lane_f32( vgetq_lane_f32(r0,1),c2,0);

c2 = vsetq_lane_f32(-vgetq_lane_f32(r0,0),c2,1);

r0 = vmulq_n_f32(c0,vgetq_lane_f32(r2,0));

r1 = vmulq_n_f32(c1,vgetq_lane_f32(r2,1));

r2 = vmulq_n_f32(c2,vgetq_lane_f32(r2,2));

r0 = vaddq_f32(r0,r1);

r2 = vaddq_f32(r2,r3);

r3 = vaddq_f32(r0,r2); // dsv

r2 = vld1q_f32( &lin[j].sw[0] );

r0 = vmulq_n_f32(c0,vgetq_lane_f32(r2,0));

r1 = vmulq_n_f32(c1,vgetq_lane_f32(r2,1));

r2 = vmulq_n_f32(c2,vgetq_lane_f32(r2,2));

r0 = vaddq_f32(r0,r1);

r2 = vaddq_f32(r0,r2); // dsw

r0 = vld1q_f32( &lin[i].dw[0] );

r1 = vld1q_f32( &lin[j].sw[0] );

r0 = vmlaq_n_f32( r0,r1,lin[j].ddq );

r0 = vmlaq_n_f32( r0,r2,lin[j].dq );

vst1q_f32( &lin[j].dw[0], r0 );

r0 = vld1q_f32( &lin[i].dv0[0] );

r1 = vld1q_f32( &lin[j].sv[0] );

r0 = vmlaq_n_f32( r0,r1,lin[j].ddq );

r0 = vmlaq_n_f32( r0,r3,lin[j].dq );

vst1q_f32( &lin[j].dv0[0], r0 );

}

FAK(lin,lin[j].sister);

FAK(lin,lin[j].child);

}

お気に入りの記事を「いいね！」で応援しよう

最終更新日 2020年10月18日 10時19分40秒

[Linux] カテゴリの最新記事

MPU-9250 2020年11月14日
上手く動かない理由（その２） 2020年11月08日
上手く動かない理由 2020年11月07日

【毎日開催】

15記事にいいね！で1ポイント

10秒滞在

いいね! -- / --

次の日記を探す

おめでとうございます！
ミッションを達成しました。

※「ポイントを獲得する」ボタンを押すと広告が表示されます。

キーワードサーチ

▼キーワード検索

nisiken2002

まだ登録されていません

スマートフォン版を閲覧 | PC版を閲覧

共同開発

共同開発者募集

初回ミーティング

開発初期段階

いろいろ♪

ＫＨＲ－１♪

R8C/15

導入編

ＣＰＵクロック

シリアル通信

戸惑い

パルス幅計測

Ａ／Ｄ変換

ＰＷＭ信号

モータ制御

C# .NET 入門

通信

FPGA

参考書

開発環境

空間加速度まで終わったー