重心を補正しながらの動作■８月２日■
８月になったので新しいページになった。
どうやら多分、重心計算ができた。
リンクリストを作って関節ブロック毎の情報をモデル化したらメモリーアロケーションエラーが頻発したり、足構造の原点が足首だったり胴体側だったりするので角度の符号が全部逆になったり、できたーと思ったら各脚の最後の関節の計算がすっぽり抜けてしまっていたりというミスがあって思いのほか手間がかかってしまった。
胴体傾きを変えても手を前に習えにしても、その重心の差に応じて下半身が動くのでこけることなく歩けるようになったのでまぁ、うまくいってるのだろう。
静的重心しか計算していないのだが、やはりモーメントも計算した方がいいようだ。今後の課題。
ただ、まだ上半身と下半身の動作の連携などが整っていないので重心計算の反映については考慮が足りない。そのため、体を傾けたりしても重心のずれを補償するのは次のステップになってしまっている。 あらかじめ計算して、補償値をつかんでおかなければせっかくの重心計算がもったいない。そのためには一度各関節角度を計算し、重心位置を計算して補償値を求める。その補償値を姿勢に反映して関節角度を決める・・というふうにしなければならない。 確認すると5回くらい繰り返せば収束するようだが、まぁ2回やればそこそこいいんじゃないかなーと思ってる。
■８月４日■
重心計算を行ったことで、以前から気になってきたことが（自分の中で）クローズアップしてきた。
それは関節のガタつきである。
ガタつきは機械分野ではいつも問題で、ロボットにおいてもそれは同じように問題だ。ガタつきは制御上は「関節の硬さ」になって見えてくる。１自由度に対して１つのアクチュエータで制御している場合、どれだけトルクがあってもガタつき分はフリーであり、トルクが発生しないことになる。（ガタつき分もセンスできたとしても制御できないので発振が起こる）これを解決する手段として、関節を開く側のアクチュエータと閉じる側のアクチュエータを設け、バランスを取りながら制御する、という方法が考えられる。双方のアクチュエータを引っ張り合うようにすれば関節は「硬く」なるわけだ。実際、人間などの生物の関節は2関節当たり６つの筋肉で制御しているらしく、同じような動きをさせたければそれくらいの数のアクチュエータを使わなければならないらしい。そこまで行かなくても、股関節などの荷重の大きな関節は1自由度当たり2つのアクチュエータで制御してインピーダンス（関節の硬さ）を制御せねばならないのだろうと思ったりはしている。
いま、問題にしているのはもっともっと低レベルで、ガタつきのせいで指示した姿勢になっていないということだ。
立っている状態で各関節毎に見ていくと（膝曲げ状態だからではあるが）膝関節には常に荷重がかかっている。そのため、膝関節の実角度は指示角度とは「ずれ」が発生してしまう。そのずれは胴体の姿勢角度に影響し、指示では直立しているはずの胴体が、大体実測で（加速度センサーで、だけど）５度傾いている。
一方、歩くときには、ちょっと前のめりになった方がいいのかなと体感的に考えていて、姿勢の指示パラメータにも重心のオフセット設定ができるようにしたりしている。ところが、ロボットの構造を変更した後に同じように歩かせようとした時、この重心オフセット値の調整ではどうにも調整ができないのだ。 いくらオフセットを大きくし、前のめりな重心設定にしても、歩かせると仰向けに倒れてしまう。で、更にオフセット値を大きくすると、前に倒れてしまい、立つことができなくなってしまう始末だ。
そこで、歩行時の基本姿勢を変更し、少しうつむき加減の設定をするとオフセット値を大きくしないでもうまく歩くようになる。うつむき加減を小さくしていくと、何のことはない、実際の姿勢が直立ならばちゃんと歩くらしい。
要はがたつきのため、実際の重心位置と計算上の重心位置にずれが生じており、質量の大きな上半身の傾きは致命的な影響を持っているということらしい。（影響度の計算まではしていません。）
RS601CRは関節角度の取得もできるので、プログラムとしてはそのような仕組みも取り込んではいるのだが、時間がかかるため、ある瞬間の全サーボの角度を取得することはできないのだ。そのため、動きを制御する中で実際の角度をフィードバックすることはできない。不本意ながら重心計算には指示角度を使っているのはそういうわけがあるのだ。でも、それを言い訳にしてロボットが歩けない理由にするわけには行かないのでなにか考えなければならない。
今考えているのはフィードフォワード方式をアレンジして適用できないかな、ということだ。フィードフォワードは、実はロボット制御の相当上位レイヤーでも取り込まなければならない考えだと思っていて、学習データや予測が必要となる。そこまでではなくとも静止時の姿勢から要フィードバック量を決定し、動作に取り込むということくらいはできないかなと考えている。
重力というアクチュエータが質点に対して働きかけるトルクは計算できるわけだから、ガタつきによるずれ量を事前に計算することもできるかも知れないのだが、うーーーん。。。
転倒判定にしても、重心計算にしても、あてずっぽうでやってるときにうまく行ってたことが、精密に計算するとうまく行かなくなる。 真理だ。
重心位置の、姿勢への取り込みの方法はそれこそフィードフォワードしなければ収束までの時間（計算量か、）がかかってしまう。ただ、歩行動作などはコマ送り制御なので現姿勢から目標姿勢までの重心の移動量は小さい。そのため、問題にしている差分は発生しにくいわけだ。
問題になるのは現在姿勢がはっきりしないときに行う絶対姿勢への移行。 サーボの機能を使って指定時間で指定角度になるように指示を送るため、１命令で目的の姿勢になる。そのため姿勢移行量が大きく、重心位置の誤差も大きい。
つまり、対処すべきは絶対姿勢への移行のみ、と考えている。こちらの方もコマ送り制御にすれば事実上の問題はなくなるかな、と。
■８月５日■
いろいろ手抜きだが、重心補正をしながらの姿勢変更（単純移行）ができた。
すごく簡単に考えていたのだが、サーボにトルクを入れた時の不定状態では姿勢パラメータをベースにした移行ができないので考えた挙句に角度ベースで移行するようにしてしまった。それじゃ重心補正ができないって気づいたのはずいぶんとあと。。。orz
そのうえ、何度かのサーボファームの変更で、サーボのトルクを入れたときには目標角度が現在角度になるようになっていたのを認識していなかった。 確か、以前は現在角度を取得して、指示角度として与えたのちにトルクを入れないとサーボがびっくり動作してしまっていたはず。 思い通りに動かないのでそのあたりのソースを眺めていたら角度を取得したり設定したりする記述がまったくなくなっていた。うーん、不思議だ。。
結局、現在の本当の姿勢とデータ上の姿勢が一致しているかどうかが不明な場合は今まで通りのサーボコマンドで移動。その後、双方のデータが一致していることが確実な状態では重心補正を折り込みつつ、コマ送りで姿勢を移行するようにした。姿勢の移行も各パラメータを線形移行するようにしたので、スクワットなどが綺麗に動くようになったはず。
下の動画は上半身の仰角を-45度から+45度まで変更しているだけなのだが、それによる重心の変化を折り込みつつ動作しているのでその他のパラメータを調整しないでも大丈夫って話です。モーメントまでは計算していないので速度が速くなると反動でこけるんだろうな。
人間でも、壁際に立つとお辞儀ができなくなる。（お尻が突き出て壁にぶつかり、前につんのめってしまう） ちょっとだけ人間に近づいた感じ。（*^_^*）
それはいいとして、胴体角度の傾きの件をどのように処理するかが当面の問題。
関節のガタつきはサーボの現在角度を取得すれば認識できるのか？ジャイロとの差は？ という点を確かめるためにデータ採取を。
目で見る限り、指示角度が垂直な時の胴体姿勢は加速度センサーの値と一致しているように見える。 それを前提とすれば、指示角度に関わらず、ガタつきによる誤差は５度程度。関節角度を読み取っても胴体の真の角度へは行き着かないらしい。ガタつきはポテンショメータの外側って事か。。。
転倒判定に使用する姿勢は仰角に５度を足して、考えた方がよさそうだ。
転倒判定の計算時に仰角を5度足してみたけどあまり変化なし。ふむぅ～、やっぱ重心計算の時に5度補正しなきゃおかしいよな。アホでした。（←で、これはやってない）
今日はちょっと上半身にも手を出した。
腕をPS2コントローラのアナログジョイスティックでコントロールできるようにしたら楽しいかな、と思ってちょいとやってみた。 けど、難しいな。マスタースレーブコントローラでも作らないとうまく動かせない。訓練とソフトの改良でなんとかなるかなー？？ ま、リモコンはテスト用なので凝る気はさらさらないので終わり。
っていうか、腕を動かしたら下半身のバランスを自動的に取るようにしてやろかと思ったんだけど、そうするとちょちょいと組んだプログラムじゃ無理ってことに後で気が付いた。昨日から同じことを繰り返してる感じ。。 まぁそのうちに、、ということで。
それより腕を動かしていたら、こないだから考えている、立ち上がりなどの動作をモーションを使わずにどうやって組み込むかということで４足歩行から立位へ移行するというやつを早くやりたくなってきた。背中を上にした普通の４足歩行とお腹を上にした４足歩行も作らないとならんのだなーとか。下半身の姿勢表現はすべて今の下半身姿勢でもいいんじゃないかなとか思えてきた。 胴体の姿勢を見て、実際は座ってるとか寝ているとか、よつんばいとかを判断すればいいかなと。
ま、そんなこんなでふらふらとしながらなのだけど、今週はなんとしても画像に着手したかったので、他の誘惑を早々に断ち切って画像に着手。
なんとか、キャプチャーチップが出しているYUV422フォーマットを理解したとこまではできた。次はCDTクラスの整備をしないとね。どんなんだったかすっかり忘れちゃったなぁ。
↑こないだまではライブラリ関数でRGBに変換してからPCに送っていたのだが、これはRPU-100で自分でRBGに変換したもの。CDTはYUVデータをそのまま使います。
↑でも、今後はこの間引きして4分の1のデータにしたタイプでプログラムを進めることにします。これくらい画素数があればいいんじゃないかなーと、詳しく見たければクリップすれば上の画像までは拡大（？）できる。
画像に写っているアイボは半年に一度のバッテリー充電祭。 完充電で保管してくださいとのこと（ソニーより）なので、バッテリーの延命のためにたまに出してくる。
アイボの自律プログラムも作りたいのだが、ラムダとアイボ両方ってのはさすがに時間的に無理。 アイボはバッテリーひとつで1時間くらい動くから本気で家の中で生活させられる。４足だからコケルこともほとんどないし、センサーもまぁ十分だし。ホントにすばらしい製品だった。 正直、ソフトは僕にとっては全然ダメでソニー製のプログラムで動かしたことはほとんどなかった。つまり、ロボットという機械（素材？）として優れているということ。アイボに比べると今のロボット商品はどれもこれもガラクタかおもちゃかって感じ。外してるなぁって思うけど、実は僕が外れてるだけなのかも。(~o~) 特にERS-7は完成度が高くってこれがこの世から消えていくなんてソニーはまったくもったいないことをすると思う。 儲からないから続ける方が企業的には儲からないってことなんだろうけどさ。
■８月６日■
CDTの原型を作成。
ホントに原型なのでまだ1チャンネルしか持ってないし、CDTエディタもないから検出色調整は超マニュアル。テーブル調整の煩雑さがあんまりなので、CDT本体のプログラムより先にエディタを作らなければならないって勢いです。
この画像をCDTで処理すると、
こんな感じで、ピンクボールだけが見えるようになる。テーブル調整ができていないのでいまいちですが、まぁ、動作が正常なことくらいはわかる。
このCDTって仕組みは実世界じゃほとんど役に立たないと思うのだけど、ロボカップのサッカーコートは色でコートやボールを認識させているので大変有効。 というよりCDTを前提にしているんじゃないかと思われる。YUVデータを範囲で切り分けるだけだし、画像データ自身も１プレーンで8色のデータまで保持できる。ロボカップにはこれで十分だと思います。問題は規定ではロボットの色は各ランドマークの色以外であること。黒を推奨していることからロボットに相手のロボットを認識させる考えがまだないこと。ビブスとかワッペンを義務付けて相手の認識をさせるべきだと思うのだが。
どうも画像が白飛び気味で色がわからない可能性があるのだが、カメラのブライト調整とかできないので困った。画像データを加工しても意味ないしなぁ。
■８月１１日■
CDT一応完成。
カメラ側とエディターは完成したけど、テーブルの編集方法にもう一工夫いりそう。画像から抽出したい色をピックする方法だけではちょっと編集が大変そう。色エリアから調整する方法も併用すべきかなと感じた。
UNDO機能をつけたけど、CLEARはUNDOできない。使ってみるとCLEARにもUNDOが効くようにしなきゃならないし、表示画像とテーブル編集用画像を分けたのもいまいちわかりにくい。編集ソフトって難しいな。もうちょっと工夫してみます。
その辺は改良していくとして、カメラの白飛びが問題。 ゲインを調整したいところだけど、安いカメラじゃAGCがかかっててマニュアル調整できない。もうちょっと調整機構がついた高いカメラにしなきゃならなそう。 高いっていっても15000円程度だが。レンズが付くかどうかわからないから秋葉原に行って、見て買わなきゃならないかな。
■８月１３日■
帰省先でCDTエディタの機能充実に励む。
マップの表示機能とか、チャンネル毎のクリアとか大体思いつくところは実装できた。本末転倒になっちゃいけないので使いやすさの追求に没頭しないように気をつけた。 あとはマップのコピーあたりができるようになればほぼ完成かな。
でも、調整は結構大変な作業。明るさの違いで色の具合は随分と変わってしまう。ロボカップみたいな閉じられた環境ならこれでもいいかもしれないけど、実世界ではやはり使えないと再認識した。色で分離しても、その色ってユニークじゃないから結局は領域判定をしなきゃならない。 あ、それより白飛び問題の方がでかいな。やはり早々に秋葉原にいってカメラを物色せねば。
さて、、明日も時間があるから、明日は上半身姿勢のモデル検討でもしようかなー。このお盆休み中に4足歩行できるようにしたいな。すると立ち上がりとかができるようになる！
■８月１４日■
CDTエディターは完成。
あとは使い勝手に合わせて徐々に改良していけば良いかと思う。
実際の編集はキャプチャーダイアログ側で行う。下側のカラフルな色マップが輝度チャンネル毎の色マップ。マップ中の四角形が色抽出範囲。そして色マップ中にちりばめられた星雲みたいなのが画像中の色分布となる。抽出範囲に加えたい画素をクリックすることで編集する。 クリックするのは上側のキャプチャー画像でも下側の色マップでも可能。 範囲を狭めることができないので、抽出色範囲が広すぎた場合は輝度レンジクリア(BR CLEAR)で抽出色範囲をクリアしてから設定しなおし。 大体そんな感じだ。それでも、ロボカップ会場に行ってから色調整するのは大変と思われるので、あらかじめ色抽出データを用意しておき、現地で微調整するといった感じだろうか。
下の画像を見てもわかるが、中央の色目の無い画素がほとんどだ。昼間の光ではこのようになってしまう。夜の蛍光灯のしたならもう少しましな色分布になるはずなのだが、これじゃ色判別には使えない。
アイボ時代に開発した物体認識プログラムも輝度だけではなく色情報も合わせて判断素材として物体を分離していた。やはりカメラ側をなんとかしなければならないようだ。
実は頭ユニットの詳細設計が完了しておらず、物体のトラッキングをさせたりすることができない。頭ユニットはもちろん作らなきゃならないんだけど、やればできることがわかってるからなんっていうか面白みが無い。転倒回避のようなうまく行かないアイテムは悶々としてフラストレーションがたまるけど、出来上がった時の達成感（開放感？）は気持ちいい。CDTや頭ユニットみたいにやればできることって、モチベーションが上がらなくて困る。やらなくて済むならそれでいいんだが、やらなきゃならないのは確実なのでさっさとやって終わらせてしまえばいいんだけどね。手間はそれなりにかかるから厄介だ。
※やればできるっていってもこの白飛び状況では色を使ったトラッキングはできないし、超広角にしたから見たくないものまで視野に入ってきてしまう。その辺りの処理方法を考えないとトラッキング動作はできないわけだから、「やればできる」って言うのはちょっとおこがましい言い方。ロボットをガシャガシャ動かすアイテムに比べたら幾分地味ってことかな。
とはいえ、頭ユニットができて全身制御の仕組みができたら、とうとう自律動作が可能になる。転倒時の受け身動作とか、まだまだできていないところがあるけどワクワクしてきたな。
■８月１６日■
帰省先から戻ってきました。 夏休みは土日を合わせてあと4日（今日を入れて）残ってます。異常な猛暑の中がんばる！
カメラの白飛び問題を解決するためにCCDカメラユニットを物色。やっぱこれ↓しかないかと思うのだが、秋葉原店には置いていない様子。つまり見て買うことができないのだ。問い合わせのメールは送ってみてはいるが返答あるかなー。せっかちなのですぐにでも注文してしまいそう。とりあえず近日中に秋葉原へ買出しに行く計画は見送りかな。
自分のHPを開くとCDTエディターの画像が目に入るので、うっかりそっちにスイッチが入ってしまう。 が、これからは上半身姿勢のモデル化と頭部ユニットの構造設計がメイン。サブとして転倒回避歩行を行うのだ。
上半身姿勢を考えたとき、下半身姿勢についても考え直さなければならない部分がある。
ラムダの場合、骨盤部分の姿勢 （ラムダの場合は両股関節の中点をロボット原点としている。その原点の姿勢のこと。原点の姿勢って変だな。） は両足の姿勢から出来上がるものなので、独立していない。原点の姿勢が基準だとすると、足の姿勢値が変わるだけなのだが、立位で考えると床面が神様。今は水平面に立っていることだけ考えているので水平面に立っている時の原点の姿勢が骨盤の姿勢ということになる。
2足で立っている時は、その「ある姿勢」を持った骨盤に上半身が乗っかるため、その影響で肩の座標や手先の座標がグローバルに決定するのだが、4足で立つ（というか四つん這い）の時は意味合いが異なる。骨盤から肩関節までの間には関節がないため、４足で水平面に立った時の胴体の姿勢が意味を持つことになる。これは前述の骨盤の姿勢とは異なるものになるため、その値を受け取っても意味がない。
でも、４足状態のための下半身姿勢、または４足状態での姿勢モデルを持つのはイヤなので現状の下半身姿勢モデルを継承したい。 ということで、４足姿勢の場合には下半身姿勢モデルのうちかしげ角（pan）とひねり角（roll）は足の姿勢値に割り戻す変換を行うことにする。
ついでと言っちゃなんだが、下半身姿勢の逆変換も完成させねばなるまい。使わないから作らないでほっといたけど、電源入れてから全部自分（ロボットのことね）で姿勢を管理するとなると、角度から姿勢作れなきゃいかん。
・・・逆変換なんか簡単だ。以前に作ろうとしたときは下半身姿勢のパラメータがもっと少なくて、すべての姿勢に対応していなかったけど、つま先立ち歩行をサポートした今はどんな姿勢も表現できるからすぐできるはず。 と思ったのだが、その、つま先立ち歩行をサポートした時に、接地位置ベースの姿勢表現モードと足首関節ベースの姿勢表現モードってのを作っていて、今回の場合は足首関節ベースで表現すべきなのだが、そっからの処理を考えていなかった。。。 足首関節ベースの姿勢表現ってのは、基本的に遊脚復帰動作の時の動きを表現しやすいように作ったので、それ以外の使い方を考えてなかった。 考えなきゃな。
■８月１７日■
関節角度からの姿勢取得に難航。
というのも、座ったり、四つん這いになったりという姿勢をサポートするとなると、関節の角度が大きくなり、いろんな変換関数に不具合が出る場合が増える。三角関数が周期関数なので90度や180度を超えると違う値でも式上は成り立ってしまい、変換を誤るとロボットがびっくり動作してしまうのだ。
今日、引っかかったのは、、、足の逆運動学計算で股関節の姿勢を算出するのに tan( ) を使うのだが、これが問題だった。 tan( )はπ/2 radian 周期の周期関数なので状況によりπ/2radianずれた値を出してしまうのだ。状況が判別できれば修正できるのだが、うまく判断できなかったのでとりあえず、一度順運動計算をして検査することにした。検査するのは角度から姿勢を取得した時だけにすれば問題ないだろう。ちょっとかっこ悪いけど仕方ない。
もうひとつはもっと問題。ある姿勢をとる関節角度の組み合わせは唯一ではない場合がある。その場合、関節角度と座標・姿勢値は相互変換できない。たとえば、下の画像の足の姿勢はどちらも同じ値を取る。だが、股関節部分のサーボの角度は全然違うのだ。
ロボットが動作を始めた時左側の姿勢をとっていたとする。関節角度から姿勢を算出し、そこからの動作を計算する時に右側の関節角度を算出してしまったら、それはもう、とんでもなくびっくり動作をしてしまうのだ。
他にも膝関節が逆に曲がったりすると同じ問題が起こる。人間がサポートするのが前提であれば、問題にならないのだが、電源を入れてからすべてロボットに任せるとなると問題。 やりかたとしては正規の姿勢になるまでは関節角度ベースで動作させて座標・姿勢値での制御に切り替えるというのがあるのだが、電源を入れて起き上がる様子にこだわりたいのでなんか手段を考えねばならない。 まぁ趣味の範囲ですが。
連続動作している状態なら関節角度が急激に変化する場合は他の組み合わせを試すというチェックを入れるしかないかなーと思ってる。
こういう姿勢ってありえると思うのだけど、実はこの姿勢が前述の問題にひっかかってしまうのだ。これってすべて大腿のロール軸が元凶なのだ。立ってる場合は問題ないけど、こういう姿勢になるとこの関節形態は自由度が減ってしまう。
■８月１９日■
とうとう夏休みも今日まで。
帰省から戻って４日の休みがあったが、そのうち少なくとも丸々２日は下半身姿勢の逆変換に費やした。お陰で足部の傾斜が状態での逆運動計算、順運動計算はとんでもない姿勢も含めてカンペキになった。
だが、目的の下半身姿勢の逆変換はというと、まだダメ。
足の姿勢から下半身姿勢を構築したいのだが、今使っている逆運動学計算は足首関節を原点としたもの。足首関節は正姿勢をとり、股関節に姿勢値を持つようにしている。下半身姿勢を構築するには左右の足の股関節姿勢が一致しなければならないので、股関節姿勢を足首関節姿勢に変換する変換式を作っていた。
ところが、そのようにして構築した下半身姿勢値は相互変換がうまくいかないのだ。どうやら、足姿勢のうちの「路面の傾斜値」が悪いらしい。 つま先立ち歩行の時に「路面の傾斜に対する姿勢」と言うことで路面傾斜値を追加したのだが、その部分の実装が適当らしく、うまくいかない原因になっているらしい。
というのも、はじめは股関節の姿勢を足首の姿勢に変換する式を作っていた。どうも自信が無かったので、股関節を原点とした順運動学計算式を作って検算することにした。すると、変換式で変換した足姿勢値と順運動学計算式で作った足姿勢値はピタリと一致する。どうやら変換式は正しいらしい。
すると、おかしいのは足首関節原点となっている逆運動学計算式の方である。 再考の余地アリだ。
これは立位では、およそ有り得ない姿勢だが、四つん這いや立ち上がる途中姿勢なら十分ありえる足姿勢。ちなみに右足のみ見てください。
これを足首原点で表現すると、
x:43.0 y:25.2 z:205.1 r:-24.2 t:-159.2 p:3.4
となるのだが、股関節原点で表現すると、
x:-30.8 y:97.2 z:184.8 fr:-154.6 ft:-162.5 fp:168.1
となる。
r,t,pは股関節の姿勢、fr,ft,fpは足首関節の姿勢である。
わかり易くするために座標軸を書き込むと、まず足首原点の場合
股関節の姿勢はr-p-tの順。
次に股関節原点の場合。
足首の姿勢はr-t-pの順。
足首原点の場合はまだいいとして、股関節原点の場合の足首の姿勢がfr:-154.6 ft:-162.5 fp:168.1というのは理解しにくいが、そうなっている。
ここまででわかったのは、まず足首関節原点の順運動学計算は正しいように見える。 そして、股関節原点の順運動学計算もどうやら正しい。間違っているかと思ったが、計算結果の見方を間違えていた。 そして、現在の足姿勢に用いている傾斜値と足首関節の姿勢は考え方が異なる。 （というか、正確には足姿勢に用いている傾斜値の定義がおかしいのだが。(~_~;) ） やっと状況が理解できたので、これから求めている変換式を作ってみたいと思います。
苦節４日間。できたぁ～。逆運動学計算式の方を直す方が筋なのだが、まずは一件落着。このあと、下半身原点（股関節の中点）のチルト角だけを与えた角度にするための変換も行わなければならない。 順運動学計算でやってしまったが、現状姿勢値からの変換の方が便利なのでそちらも検討したい。
作品１ 足を組んで寝そべるラムダ
right
x:-70.050 y:69.900 z:-151.400
r:-7.400 t:0.000 p:0.000
ft:-13.900 fp:24.500
left
x:-23.650 y:-82.500 z:-178.400
r:-2.200 t:0.000 p:-0.000
ft:-98.600 fp:3.200
width : -33.700 depth : 152.400 height : 178.400 hung : 27.000
tilt : 0.000 pan : 0.000 roll : 0.000
<right> roll : -7.400 tilt : -13.900 pan : 24.500
<left> roll : -2.200 tilt : -98.600 pan : 3.200
pivot: 0 pivot_pan_x: 0.000 gx: 0.000 gy: 7.000 gz: 200.000
作品２ 足を広げて座るラムダ
right
x:80.000 y:157.450 z:-29.200
r:-29.300 t:0.000 p:0.000
ft:-10.000 fp:-7.000
left
x:70.200 y:172.750 z:-35.200
r:-23.000 t:0.000 p:-0.000
ft:-11.200 fp:-6.600
width : 210.200 depth : -15.300 height : 35.200 hung : 6.000
tilt : 0.000 pan : 0.000 roll : 0.000
<right> roll : -29.300 tilt : -10.000 pan : -7.000
<left> roll : -23.000 tilt : -11.200 pan : -6.600
pivot: 0 pivot_pan_x: 0.000 gx: 0.000 gy: 7.000 gz: 200.000
作品３ ヨガをするラムダ
right
x:28.550 y:42.700 z:118.500
r:153.600 t:0.000 p:0.000
ft:83.000 fp:32.200
left
x:7.150 y:43.100 z:112.600
r:140.300 t:0.000 p:-0.000
ft:78.800 fp:54.100
width : 95.700 depth : -0.400 height : -112.600 hung : 5.900
tilt : 0.000 pan : 0.000 roll : 0.000
<right> roll : 153.600 tilt : 83.000 pan : 32.200
<left> roll : 140.300 tilt : 78.800 pan : 54.100
pivot: 0 pivot_pan_x: 0.000 gx: 0.000 gy: 7.000 gz: 200.000
あ、表示するパラメータを二つほど忘れてる。 まぁ、こーんな姿勢でも数値化できますよって話だけなのでよしとしよう。
関節角度列ではなく、姿勢値で表すことで、プログラムで動きを生成することができる。（正確には比較的簡単にできる）いま使っている姿勢値は立って歩く場合を想定して作ったものなので、このような特殊な姿勢を表現しても前述の意味があるかどうか怪しいが、座ったり、四つん這いになったりという程度なら意味を持つだろう。側転するとか、うつむけから仰向けになるとかって感じの動作を表現するのは難しいかもしれない。
結局、この連休に４足歩行どころか上半身姿勢にも着手できなかったが、やらなければならないのに大変そうだからイヤだなぁと思っていた下半身姿勢の逆変換が完成したのは大きな成果だな。
■８月２１日■
下半身姿勢を逆算するときに下半身原点の姿勢を指定できるようにした。 これで下半身姿勢の逆変換については完全に完了のはず。 これでジャイロから取得した胴体姿勢を適用することができ、実際の姿勢（たっているような姿勢だが、実際には寝転んでいるといった状態）を正確にデータ化できることになった。
一度関節角度列にしなければならないが、現在姿勢からpan角を0にした姿勢などに変換できるようになった。これで、あとは上半身モデルを連結すれば全身のモデルが出来上がる。（アタマはまだないけど。）
上半身姿勢は手先座標と肘の位置を管理すればいいのか、下半身のような歩行モデルを作った方がいいのかちょっと攻めあぐねている。四つん這いさせるのが当面の目標だが、四つん這いだけが目的ではないので、先まで見据えたモデルにしたい。とはいっても必要に応じて改良していくことになるので考えずに手を動かせって話だろうな。
■８月２２日■
加速度センサーから胴体姿勢を読み取り、下半身原点の姿勢として取り込めるようにした。
昨日はできたできたと喜んでいたが、pan角についての検証をしていなかった。案の定バグがあり、その修正を行った。
今日はとうとう上半身姿勢モデルと下半身姿勢モデルの連結までやろうと思っていたが、明日以降に繰り越し。まぁなんにせよ、何から手をつけていいかわからない構想段階よりもプログラムを書いてデバッグという段階は何をすればいいのかがはっきりしているので楽しいといえば楽しい。黙々と作業できるのがいいだけかもな。
■８月２４日■
ボードカメラが届いた！
結果から言うと今のところオールOK (*^^)v 超超広角レンズはギリギリだがピントがあうし、色合いも鮮やかに出ている。今までは出ていなかったケラレが下側にたっぷり出ている。レンズの中心がCCDの中心ではないようだ。
ちょっと調べてみたところ、色が抜けるのはオートホワイトバランストラッキング（AWT）が働いているかららしく、ホワイトバランスをロック出来ればいいらしい。または被写体にグレー系のものが写っていればよいらしい。 このカメラはトラッキングを切ることが出来るようなのでそのあたりも追って確認していきたい。
あと、このカメラはズーム機能がある。下の画像がズームした時の画像。アングルはちょっと違うが、上の画像とカメラ位置はほぼ同じ。
このズームはいわゆるデジタルズームで、CCDの取得エリアを小さくしているだけだが、超超広角レンズをつけるのでとてもありがたい機能。凝視するときはズームで！このカメラのCCDは40万画素。普通のボードカメラは25万画素が多いのでちょっとしまりがある画像だ。このアプリでの画面だと違いがわからないが、19インチディスプレイに映したら違いが明白。画素数が多いのでズームしても十分な解像度が得られる。悪いところは他のよりデカイ・重い・多分消費電力が大きいということ。自律まぁサーボよりは消費電力がはるかに小さいのでいいでしょう。どうせ今のバッテリーだと10分～15分くらいしか動けない運命なのだ。
ズームはON/OFFだけの2段階だが、1ビットの入力で制御出来るのでマイコンをつないで切り替えられるようにしてやればRPU-100から操作できる。AWTのオンオフもそれで行くつもり。
デジタル出力らしきコネクタも付いているのだが、仕様がわからないので使いようがないなぁ。
■８月２６日■
上半身モデルがとりあえず出来てプログラムに組み込んだのだが、うまく動かない。
腕の逆運動学計算が不完全なようだ。そういえば足の逆運動学関数を作った時に一緒に作ったのだが、動作確認したらおかしな部分があったのを思い出した。その時は足さえちゃんとできればいいやって話で、後回しにしたのだった。
足が６自由度に対して腕は４自由度だが、結構複雑な動きをする。今は手首から先がないから４自由度で済んでいるが、本格的に腕の機能を実現しようとすれば動作の複雑さは足の比ではない。
少なくとも肘の位置は制御できなければならないし、可動範囲や足との干渉なんかも考えなきゃならない。またまたこれでゴハンが何杯も食べられるような分野なのだ。
とはいえ、今やろうとしているのは単に「ある姿勢」で「ある座標」へ手先を持っていくということ。さして難しいお題じゃない。
でも、足の時と一緒でラムダの腕が動く範囲すべてにおいて、順運動学計算と逆運動学計算を相互に計算できるように関数を整備するのは結構大変なのだ。
微調整の結果、普通の動きは大抵大丈夫になった。（以前に作った関数は結構間違いがあった） まだ、肩関節Y軸の角度が90度以上開いた場合の腕の位置が未サポート。算出する手先の姿勢角度も正しいかどうか検証出来ていない。
上半身モデルを作るに当たって、腕の制御について少し考えたが、やはり難しい。腕で何をしようとしているかということでどうやって動かすべきなのかが変わる。
①下半身と同期せずに動く
②グローバル空間の座標に対して動く
③下半身と協調して動く
①は自分の体を触る場合など、②は手すりにつかまって歩くとか、荷物を持って歩くとか、③は四つん這いって感じもするけど、リーチングの方が協調度合いが大きいかな。
土曜日に秋葉原に買出しに行って来た。通販でしか扱ってないと思ってた買ったボードカメラは秋葉原にありました。「秋葉原に行こう！」って決意がもう少し早ければ・・。 RS302CDに使っているコネクタは秋葉原のヒロセで手に入りました。通販で買わないでよかったー。でも、他にもヒロセとJSTのコネクタを探しに行ったのだけど見つからなかった。JSTのは通販で買えるのだが、ヒロセのはまだ売ってるのが見つからない。思い切って他の手に入りやすいコネクタに変えちゃうのが手っ取りはやいかも。
材料そろったので頭部分の設計と製作に取り掛からねば。もうすぐ日差しも柔らかになるだろうからちょうどいい。しかし、、進捗はかばかしくない。こんなので来年の５月に間に合うのかな？
そういえば、どこかで「来年のロボカップヒューマノイドリーグは３ON３になる可能性が高い」って書いていたのを見たが、３台になったらもうお手上げだな。出場は不可能だ。そうなるとラムダのデビュー先が無くなるなぁ。
■８月２７日■
やっと自分の過去の所業を解き明かすことができました。ついでに過去の過ちも清算することができました。そして次の段階へ進めます。
大抵の腕の姿勢は順運動学計算と逆運動学計算で一致させることができたが、足の時と同様一部の姿勢では２パターン以上の組み合わせがあり、場合によってはびっくり動作をしてしまう。でもまぁ、とりあえずは足と同レベルにはなったかと思う。
そして上半身姿勢との変換も大丈夫そうだ。（あ、逆変換のテストやってないや）
↓腕の逆運動学計算のテストの様子。右腕の関節角度から座標と姿勢に変換し、また関節角度に変換して左腕を動かしている。（連続動作じゃないので静止画で(^_^;) ）
左の姿勢ならちゃんとトレースできるが、右側の姿勢だと上腕がひっくり返った姿勢になる。手先の座標と姿勢は指示通り。
この姿勢ならありえるもんなぁ～。
解明した所業をメモっとかなきゃなぁ。ラムダのページに載せてる腕の逆運動学計算のメモは間違っとります。（書いた本人も理解不能のメモですが。。。）
■８月２８日■
腕の逆運動学計算も解決したので、手を付いた時の動きについて検討する。
立とうとするときの動きとして、各姿勢から立位へ遷移していく動作を組み立てていく。 各姿勢と遷移動作を数値化できればどんな姿勢からでも自分で考えて（考えてはいなけど）立ち上がることができるようになるはずだ。
４足状態で重心が２足の支持多角形外に有るときは手をついていないと転がってしまう。下の画像の状態だと、もう重心を足側に移動することができないので腕側をヨチヨチと歩かせて手を付いている位置を足側に近づける必要がある。（ズリズリと地面を擦る動きは禁止が前提なので）
１足（どちらかの手）を上げるためには３足で転ばないようにする必要があるため、重心を「上げる手」と逆の方へ移動させなければならない。ここで問題。大腿はほぼ垂直だが、胴体がほぼ水平に寝ている状態。これだと股関節のY軸回転のサーボがZ軸回転になってしまっているため、胴体を左右にシフトする動作ができない。（逆に、手の方へ重心を移動して、足を手に近づけるという動作ならできるかもしれないな。）
たとえば下の画像のように大腿が水平になるような姿勢の場合は大腿のひねり軸がY軸回転動作となるため、胴体を左右にシフトする動作ができる。でも、この状態だと左右への重心シフトを行わなくても片手を上げることくらいはできる。
結局はラムダの足構造は４足歩行には向いていない構造ということだが、２足歩行ロボットは全体的にそういう傾向にあるだろう。２足歩行ロボットで４足歩行を行うのは意外に難しそうだ。
構想の段階では、動作が複雑になるので重心移動は左右シフトで行こう、姿勢についてもパンとかロールは考慮しないようにしようと考えていたが、重心移動を左右への回転で行うとなると、少なくともパンは考えなければならなそう。敷居が高くなった。
上半身と下半身を結合。
上半身を動かすと重心のずれを計算して下半身がちょいと動く。静的な計算なので大したことはないのだが、腕を振り上げてもあまりバランスを崩さないのでいい感じだ。
腕の動きを色々と考えているのだが足と違って用途が多様なのでモデル化するのは困難だ。座標や姿勢を管理するにしてもローカル座標で管理するのが適当なのかグローバル座標で管理すべきか悩むところ。
上の画像は手先座標をグローバルな位置に固定したまま下半身の姿勢を動かした様子。たとえば水が入ったコップを持ったまま歩くときなど、歩くことによってゆれるコップの動きをキャンセルするように腕を動かすが、あの感じだ。もちろんそんな高度なことはできなくて単に足の動作に合わせて腕座標を計算しているだけ。
ちなみに上体を-40度うつむき、20度かしげて40度旋回した状態。
先々歩いている時の旋回を上体や腕の動きでキャンセルさせるということをやりたいと思っている。そういうときに有効な座標の管理方法は何だろう？と考えるとなかなか夜も寝られなくなる課題だ。ボールを投げるときのように上半身と下半身を協調して大きな動作域を得るということも考えたい。リーチングにはその考えが必要と思っている。
まだまだプログラムのつくりが未熟なので姿勢値などが正規に設定されるまでサーボがびっくり動作するシーンがたくさんある。まだまだまだまだだ。