ローカル座標のグローバル座標への転換戦略

表題のことがテーマである。
ずっと悩んでいる(笑)
(1)まず、片足に重心がほぼ乗っているかどうかの場合を分ける。
(2)乗っていない場合。乗っていなくても、相対的に重心がかかっている足は確定できる。その相対的支持足には四つのポイントがある。
z軸の値が小さなものが少なくとも二つはあるはずだ。
ロボットの体型が決まっていれば、その点を基準に他の足や脚の座標が全て決められるはずだ。
(※ここで注意しておくと、以後、足のくるぶしから先の部分を足(英語でfoot)といい、その他の英語でlegと言われる部分は脚ということにする)
その中で、z座標が負になるような点があったら、初めの2点の角度を変えて、それらの点がゼロになるように角度を変えなければならない。
逆に逆足が中空に上がってしまった場合は、そこを大地にくっつけるように回転させる必要がある。
(3)重心がその脚だけに乗っている場合は、その脚全体が大地についていることを前提に体型を載せる必要がある。そして、もし、逆足が大地から上がっている場合は、それをそのままにしておいて良い。重心がほとんど乗っていないわけだから。

シミュレータ、膝の動き

シミュレーターで、先のAshiro1のCosmファイルに膝を曲げる動きだけを加えたものを食わせて見た。

明らかに不自然だ。実際のロボットだったら変なことになっただろう。
シミュレーターがもっと自然な動きになるように工夫する必要がある。次の図を見てもらいたい。
後ろになった足がかかとを曲げ高くすることによって膝が周り、体全体が前のめりになり、一体の状態まで来ると後ろ足が上がり、前足のみで立つようになる。
重心が支えている足の一定近くに来るまでは、後ろ足をあげられないようにすれば良い。
それまでは、T点とS点で支えるような形にするのである。
さらに、近傍の中に入って、反対側の足がつり上がっている状態においては、その足の足首の角度が体全体の傾きを規定する、とすれば良い。
また、この近傍の大きさは、体に働いている慣性力の影響を受ける。体が強く動いている時には、この近傍は大きくなるだろう。逆に、体がゆっくりとしか動いていないときは、相対的にこん近傍は小さくなる。

シミュレータの改良

重心を支えている足の位置を固定するようにした。先のものは、ロボットの原点を動かないようにしたので、形はサーボによる決まるものだが、不自然だった。

ただし、側面図では、前方へ歩いて行ってしまうし、正面図では、スクリプトが微妙に(こちら側に向いて)左側に開きが大きくしてあるものだから左側に動いて行ってしまう(笑)

足郎2のシミュレーターの第1段階、ほぼ出来上がる

この間、計算に次ぐ計算をやってきた、足郎2のシミュレーターだが、スクリプトからサーボを動かして、ロボットの形を作るところまでは、ほぼ出来上がった。このシミュレータに、COSMで書いたスクリプトを読み込ませて、動きを確認する程度には、実行させることができる。実際のロボットの動きにどれだけ近くなるかは、ロボットの方を組み上げてみないとわからないが、そちらは、業者に部品を作ってもらっているところなので、まず、シミュレータの動きを確認するしかない。
計算式が複雑なので、プログラムのバグを取るのに苦労した。
ただし、まだ、重心と着地した足の関係など、グローバルな座標で描いてはいない。ロボットの形をシミュレーションしているだけである。この動きを、重心との関係で、グローバルな座標に変換して表示するのはこれからの作業である。
このシミュレーターに足郎1を動かしたときのCOSMスクリプトを読み込ませて、動かしてみた結果が以下の動画である。

左が側面から見たもので、右側が正面から見たものであり、右足が緑色で、左足が青である。
足郎1には、膝がなかったので膝が動くスクリプトではないが、膝を動かすスクリプトも次に書いてみる。以下に、上記の動画の元になったスクリプトを記載しておく。

#######################################
# ashiro1-13.cosm
# 逆振り子二足歩行
# 2017年7月26日 作成
#######################################
%defspeed normal {
    interval:3
    steps:20
}
%defspeed fast {
    interval:3
    steps:10
}
#####################################
# かなり基本的なパラメータ
# いじるパラメータはここに置いておく
#####################################
# ただの逆振り子だと、40にすると倒れる
%param invpen_angle_right 35
%param invpen_angle_left 40
# 逆足を少し開いて、縮み加減にしないと倒れる
%param open_inv_right 15
%param open_inv_left 15
# 足を前に(わずかに)振り出す
%param step_forward 7
######################################
# 単純逆振り子
######################################
%defangles right_invpen {
#右に傾ける動きをする
    RightUpperRight:-$invpen_angle_right+$open_inv_right
    RightUpperLeft:+$invpen_angle_right-$open_inv_right
    LeftUpperRight:-$invpen_angle_left
    LeftUpperLeft:$invpen_angle_left
# 上の動きに合わせて、足も角度をつけて
    RightLowerRight:$invpen_angle_right
    RightLowerLeft:-$invpen_angle_right
    LeftLowerRight:$invpen_angle_left
    LeftLowerLeft:-$invpen_angle_left
}
%defangles left_invpen {
#左に傾ける動きをする
    RightUpperRight:$invpen_angle_right
    RightUpperLeft:-$invpen_angle_right
    LeftUpperRight:$invpen_angle_left-$open_inv_left
    LeftUpperLeft:-$invpen_angle_left+$open_inv_left
#
    RightLowerRight:-$invpen_angle_right
    RightLowerLeft:$invpen_angle_right
    LeftLowerRight:-$invpen_angle_left
    LeftLowerLeft:$invpen_angle_left
}
######################################
# 前へ
######################################
%defangles left_forward {
#左足を前に出す
    RightUpperFront:-$step_forward
    RightUpperBack:$step_forward
    LeftUpperFront:$step_forward
    LeftUpperBack:-$step_forward
}
%defangles right_forward {
#右足を前に出す
    RightUpperFront:$step_forward
    RightUpperBack:-$step_forward
    LeftUpperFront:-$step_forward
    LeftUpperBack:$step_forward
}
######################################
# 実行手続きの定義
# 即実行コマンドから最初に呼び出されるという意味で
# メインルーチン
# 初期歩行
######################################
%defexec walk {
    #stand:all
    exec:walk_sub,10
}
######################################
# サブルーチンの定義:通常歩行
######################################
%defexec walk_sub{
    speed:normal
    setAngle:right_invpen
    speed:fast
    setAngle:left_forward
    speed:normal
    setAngle:left_invpen
    speed:fast
    setAngle:right_forward
}
#################################
# 唯一の即実行プログラム:他は全て定義
# 全てはここから始まる
#################################
%exec walk 1
#################################

ロボット座標面への射影の長さの計算

先の記事の中に、足の長さのロボット座標面への射影の長さが計算できて、実際の足の長さと角度からそれを求めることができると指摘した。
その実際の計算を以下に示す。ただ、描くのが面倒になったので、ノートの写メを掲載しておくにとどめる。
<1、2ページ>

<3、4ページ>

ロボット座標の再構成(修正)

いざプログラムに書こうと思ったら、先の記事に書いた正面図が、ひざ関節が左右に曲がるというものになっているのに気づいた。足郎2は、ひざは前後にしか曲がらない(人間と同様)ので、修正した。

正面図は、変な格好だが、図は角度の正の向きを確認し、関節位置の座標を確定するためのものだから問題ない。
《正面図》
A点とB点の関係

y_{a}=y_{b}+l_{1}sin(\frac{\pi}{2}-\theta_{3})\\z_{a}=z_{b}-l_{1}cos(\frac{\pi}{2}-\theta_{3})


B点とC点の関係

y_{b}=y_{c}+l_{2}sin(\frac{\pi}{2}-\theta_{3})\\z_{b}=z_{c}-l_{2}cos(\frac{\pi}{2}-\theta_{3})


C点と原点の関係

y_{c}=y_{0}-\frac{l_{3}}{2}\\z_{c}=z_{0}


D点と原点の関係

y_{d}=y_{0}+\frac{l_{3}}{2}\\z_{d}=z_{0}


D点とE点の関係

y_{e}=y_{d}+l_{4}sin(\frac{\pi}{2}-\theta_{4})\\z_{e}=z_{d}-l_{4}cos(\frac{\pi}{2}-\theta_{4})


E点とF点の関係

y_{f}=y_{e}+l_{5}sin(\frac{\pi}{2}-\theta_{4})\\z_{f}=z_{e}-l_{5}cos(\frac{\pi}{2}-\theta_{4})


《側面図》
A点とB点の関係

x_{a}=x_{b}-m_{1}sin(\phi_{2}+\phi_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-m_{1}cos(\phi_{2}+\phi_{3}-\frac{\pi}{2})


B点とC点の関係

x_{b}=x_{c}+m_{2}sin(\frac{\pi}{2}-\phi_{3})\\z_{b}=z_{c}-m_{2}cos(\frac{\pi}{2}-\phi_{3})


C点とD点と原点の関係

x_{c}=x_{d}=x_{0}\\z_{c}=z_{d}=z_{0}


D点とE点の関係

x_{e}=x_{d}-m_{4}sin(\frac{\pi}{2}-\phi_{4})\\z_{e}=z_{d}-m_{4}cos(\frac{\pi}{2}-\phi_{4})


E点とF点の関係

x_{f}=x_{e}-m_{5}sin(\frac{\pi}{2}-\phi_{4}+\phi_{5})\\z_{f}=z_{e}-m_{5}cos(\frac{\pi}{2}-\phi_{4}+\phi_{5})


 

シミュレーションの長さの計算

一つ前の記事で、次に検討すると記述した長さについての関係性をまとめておく。

図において、OGは、足などの実体であり長さはLである。この長さが正面図において見えているのが、OLである。また、側面図において見えているのがOHとなる、それらの長さはlmである。正面図、側面図では、角度θとφが捉えられているとしよう。
このときまず、の位置は同じだから、

mcos\phi=lcos\theta


が成立する。
さらに、

L=\sqrt{m^{2}sin^{2}\phi+l^{2}sin^{2}\theta+m^{2}cos^{2}\phi}


が成立する。これら二つの式より、l,mLと角度の値から求めることができる。

ロボット座標の再構成

先の記事に書いて、シミュレーションで暫定的に動かしてみた座標の設定には、問題があった。

正面図と側面図を同時に書くと上記図のようになる。前回の問題は、ロボットの原点で座標が固定されていなかったことだ。原点の軸においては、回転していなくした。そこで固定して、実際の動きは、それぞれの位置をグローバル座標に変換して表示させるようにすべきだということ。
図はまた、角度の正の向きを定義している。単なる定義であって、定義を変えると途中の式の形は変わるが、結果に変化はない。
原点の座標をO(x_{0},y_{0},xz_{0})としてA,B,C,D,E,Fの各座標を示そう。ただし、\theta_{1},\theta_{6},\phi_{1},\phi_{6}は、座標これらの座標に直接関係せず、ただ、足の向きを規定しているだけなので、ここでは省略する。
《正面図》
A点とB点の関係

y_{a}=y_{b}-l_{1}sin(\theta_{2}+\theta_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-l_{1}cos(\theta_{2}+\theta_{3}-\frac{\pi}{2})


B点とC点の関係

y_{b}=y_{c}+l_{2}sin(\frac{\pi}{2}-\theta_{3})\\z_{b}=z_{c}-l_{2}cos(\frac{\pi}{2}-\theta_{3})


C点と原点の関係

y_{c}=y_{0}-\frac{l_{3}}{2}\\z_{c}=z_{0}


D点と原点の関係

y_{d}=y_{0}+\frac{l_{3}}{2}\\z_{d}=z_{0}


D点とE点の関係

y_{e}=y_{d}+l_{4}sin(\frac{\pi}{2}-\theta_{4})\\z_{e}=z_{d}-l_{4}cos(\frac{\pi}{2}-\theta_{4})


E点とF点の関係

y_{f}=y_{e}-l_{5}sin(\theta_{4}+\theta_{5}-\frac{\pi}{2})\\z_{f}=z_{e}-l_{5}cos(\theta_{4}+\theta_{5}-\frac{\pi}{2})



《側面図》

A点とB点の関係

x_{a}=x_{b}-m_{1}sin(\phi_{2}+\phi_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-m_{1}cos(\phi_{2}+\phi_{3}-\frac{\pi}{2})


B点とC点の関係

x_{b}=x_{c}+m_{2}sin(\frac{\pi}{2}-\phi_{3})\\z_{b}=z_{c}-m_{2}cos(\frac{\pi}{2}-\phi_{3})


C点とD点と原点の関係

x_{c}=x_{d}=x_{0}\\z_{c}=z_{d}=z_{0}


D点とE点の関係

x_{e}=x_{d}-m_{4}sin(\frac{\pi}{2}-\phi_{4})\\z_{e}=z_{d}-m_{4}cos(\frac{\pi}{2}-\phi_{4})


E点とF点の関係

x_{f}=x_{e}-m_{5}sin(\frac{\pi}{2}-\phi_{4}+\phi_{5})\\z_{f}=z_{e}-m_{5}cos(\frac{\pi}{2}-\phi_{4}+\phi_{5})


正面図と側面図のz軸の座標は一致しなければならないので、式的にはlmで調整されることになるだろう。
つまり、実際の足の長さ、L_{1}, L_{2}と与えられた角度から、l_{1}, l_{2}, l_{4}, l_{5}, m_{1}, m_{2}, m_{4} ,m_{5}の全てが計算できるはずだ。次回はそれを考えてみる。

足郎2シミュレーターの構成

足郎2のシミュレーター作りが面倒くさい。角度と位置の関係をどのように表すかがわかりにくい。先に書いたものは複雑すぎてわかりにくい。ある論文を参考に、座標を単純にしたことによってグラフィカルに描くことができるようになった。
まず、関節角度をランダムに少しずつ動かした動画は次のようになる(クリックすると再生するはず。マックしかダメか?)。

ここまでできれば、サーボモータの動きと連動できる。基本的には、COSM言語で書かれたプログラムをシミュレートできるようにしたい。
まず、理論的な構成を記録しておこう。上記の動きが実現しているのは、以下の理論、数式が基本的に正しかったことを示している。その検証のためのものだ。
左下に描かれているように、y軸とz軸のみの表現である。
複雑そうだが、数学的には極めて単純になる。
図に向かって左側の足
A点とB点の関係:

y_{a}=y_{b}-l_{1}sin(\frac{\pi}{2}-\theta_{1})\\z_{a}=z_{b}-l_{1}cos(\frac{\pi}{2}-\theta_{1})


B点とC点の関係:

y_{b}=y_{c}+l_{2}sin(\theta_{1}+\theta_{2}-\frac{\pi}{2})\\z_{b}=z_{c}-l_{2}cos(\theta_{1}+\theta_{2}-\frac{\pi}{2})


C点と原点の関係:

y_{c}=y_{0}-\frac{l_{3}}{2}cos(\theta_{1}+\theta_{2}+\theta_{3}-\pi)\\z_{c}=z_{0}-\frac{l_{3}}{2}sin(\theta_{1}+\theta_{2}+\theta_{3}-\pi)


図に向かって右側の足
原点とD点の関係:

y_{d}=y_{0}+\frac{l_{3}}{2}cos(\theta_{4}+\theta_{5}+\theta_{6}-\pi)\\z_{d}=z_{0}+\frac{l_{3}}{2}sin(\theta_{4}+\theta_{5}+\theta_{6}-\pi)


D点とE点の関係:

y_{e}=y_{d}+l_{4}sin(\theta_{5}+\theta_{6}-\frac{\pi}{2})\\z_{e}=z_{d}-l_{4}cos(\theta_{5}+\theta_{6}-\frac{\pi}{2})


E点とF点の関係:

y_{f}=y_{e}-l_{5}sin(\frac{\pi}{2}-\theta_{6})\\z_{f}=z_{e}-l_{5}cos(\frac{\pi}{2}-\theta_{6})


見ればすぐにわかるように、極めて対称的な関係が求められる。ここで、原点の座標と6つの角度、および各長さが与えられれば、全ての点が決まる。
ただし、六つの角度は全て自由に与えることはできず、

\theta_{1}+\theta_{2}+\theta_{3}=\theta_{4}+\theta_{5}+\theta_{6}


の関係が成立していなければならない。自由度は5なのである。
また、各長さは、実際の長さが、ここでは現れていないx方向の変位によってきまる。
基本的な枠組みはできたので、さらにシミュレーションができるようにする。

3Dプリンタの購入

ついに研究室に3Dプリンタがやってきた。どこまでのものができるかは未知数だが、また、それほど精密なものが製造できるわけではないが、全く新しい世界が広がることは確実だ。
写真のプリンタの上に乗っているのは、セットアップの中で作成された鎖だが、微妙によくできたようで、微妙に荒い。