2020年10月04日
XML
カテゴリ: Linux
関節トルクの計算部分に先立ち、ZMPから足裏に作用する床反力の推定部分を作成しました。
今回は、念のため、計算結果の一覧や、コードを印刷して、マーカーでチェックしながら、
間違いがないか丁寧に作業しています。

そうそう、先日書いたコードもついでにマーカーでチェックしていたら、教科書と違うっ!!
って部分を見つけたのだけど、修正したコードを実行したら、とんでもない結果に・・・。
10年前に自分で書いたコードを確認しても、やっぱり、そっちが正解みたいで、
教科書の間違いって、困るよね・・・って思っていたら!!!!

ヒューマノイドロボット(改訂2版) ​発売されます!! さっそく買わねば(笑)。


正解ではないです。だから、「推定」って言葉を使っている。
より正確には、Z軸回りのモーメントとかも考える必要があるけど、10年前同様、
今回もそっちは無視。サーボモーターのフィードバックに使える、ある程度の精度があれば良し!

この部分だけのコードの抜粋を下に示します。

/********** 足裏反力の算定 **********/
LR = 0.0;
sR = 0.0;
Lzmp = 0.0;
szmp = 0.0;
ezmp = 0.0;
LRzmp = 0.0;
if ( step[s].lfoot_s == 1 ) {
if ( step[s].rfoot_s == 1 ) { // 両足支持
LR = sqrtf((step[s].rfoot_x - step[s].lfoot_x)*(step[s].rfoot_x - step[s].lfoot_x) + (step[s].rfoot_y - step[s].lfoot_y)*(step[s].rfoot_y - step[s].lfoot_y));
sR = atan2f(step[s].rfoot_y - step[s].lfoot_y,  step[s].rfoot_x - step[s].lfoot_x);
Lzmp = sqrtf((zmp[s].x - step[s].lfoot_x)*(zmp[s].x - step[s].lfoot_x) + (zmp[s].y - step[s].lfoot_y)*(zmp[s].y - step[s].lfoot_y));
szmp = atan2f(zmp[s].y - step[s].lfoot_y,  zmp[s].x - step[s].lfoot_x);
ezmp = Lzmp * sinf( szmp - sR );
LRzmp = Lzmp * cosf( szmp - sR );
if ( LRzmp < 0.0 ) { // 左足支持
lF[0] = Fx;
lF[1] = Fy;
lF[2] = Fz;
rF[0] = 0.0;
rF[1] = 0.0;
rF[2] = 0.0;
lT[0] = - Fy *(-link[13].c[2]) + Fz * (zmp[s].y - step[s].lfoot_y);
lT[1] =   Fx *(-link[13].c[2]) - Fz * (zmp[s].x - step[s].lfoot_x);
lT[2] = 0.0;
rT[0] = 0.0;
rT[1] = 0.0;
rT[2] = 0.0;
} else if ( LRzmp > LR ) { // 右足支持
lF[0] = 0.0;
lF[1] = 0.0;
lF[2] = 0.0;
rF[0] = Fx;
rF[1] = Fy;
rF[2] = Fz;
lT[0] = 0.0;
lT[1] = 0.0;
lT[2] = 0.0;
rT[0] = - Fy *(-link[ 7].c[2]) + Fz * (zmp[s].y - step[s].rfoot_y);
rT[1] =   Fx *(-link[ 7].c[2]) - Fz * (zmp[s].x - step[s].rfoot_x);
rT[2] = 0.0;
} else { // 両足支持
lF[0] = Fx / LR * (LR - LRzmp);
lF[1] = Fy / LR * (LR - LRzmp);
lF[2] = Fz / LR * (LR - LRzmp);
rF[0] = Fx / LR * LRzmp;
rF[1] = Fy / LR * LRzmp;
rF[2] = Fz / LR * LRzmp;
ex = ezmp * cosf( sR + PI/2 );
ey = ezmp * sinf( sR + PI/2 );
lT[0] = - lF[1] *(-link[13].c[2]) + lF[2] * ey;
lT[1] =   lF[0] *(-link[13].c[2]) - lF[2] * ex;
lT[2] = 0.0;
rT[0] = - rF[1] *(-link[ 7].c[2]) + rF[2] * ey;
rT[1] =   rF[0] *(-link[ 7].c[2]) - rF[2] * ex;
rT[2] = 0.0;
}
} else { // 左足支持
lF[0] = Fx;
lF[1] = Fy;
lF[2] = Fz;
rF[0] = 0.0;
rF[1] = 0.0;
rF[2] = 0.0;
lT[0] = - Fy *(-link[13].c[2]) + Fz * (zmp[s].y - step[s].lfoot_y);
lT[1] =   Fx *(-link[13].c[2]) - Fz * (zmp[s].x - step[s].lfoot_x);
lT[2] = 0.0;
rT[0] = 0.0;
rT[1] = 0.0;
rT[2] = 0.0;
}
} else { // 右足支持
lF[0] = 0.0;
lF[1] = 0.0;
lF[2] = 0.0;
rF[0] = Fx;
rF[1] = Fy;
rF[2] = Fz;
lT[0] = 0.0;
lT[1] = 0.0;
lT[2] = 0.0;
rT[0] = - Fy *(-link[ 7].c[2]) + Fz * (zmp[s].y - step[s].rfoot_y);
rT[1] =   Fx *(-link[ 7].c[2]) - Fz * (zmp[s].x - step[s].rfoot_x);
rT[2] = 0.0;
}





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

最終更新日  2020年10月04日 10時30分51秒
[Linux] カテゴリの最新記事


【毎日開催】
15記事にいいね!で1ポイント
10秒滞在
いいね! -- / --
おめでとうございます!
ミッションを達成しました。
※「ポイントを獲得する」ボタンを押すと広告が表示されます。
x
X

PR

キーワードサーチ

▼キーワード検索

プロフィール

nisiken2002

nisiken2002

お気に入りブログ

まだ登録されていません

© Rakuten Group, Inc.
Mobilize your Site
スマートフォン版を閲覧 | PC版を閲覧
Share by: