開発日誌(26)
■６月２８日■
完成したとおぼしきカーブ歩行プログラムを実機に移植。
動かすと激しく踊りまくるラムダ・マーキュリー(>_<)。。。 ２歩目の検証しかしてなかったけど、３歩目で色々問題が出ました。 難しいなぁ。
またPCに戻ってデバッグして、なんとか↓ここまで。 引継ぎフレームが汚いけど、多分実用上は問題ないでしょう。
ちなみに昨日の夜にここまで出来たので実機で動かしてみたら、４歩目で新たな問題が出ました。(^_^;) PCで動かしても同様の問題が再現。
４歩目の問題を検討しているところで、カーブ歩行中にカーブの回転半径を変更することができない（変更するとおかしな動きになる）プログラムの作りになっていた事に気付いてしまった。
カーブ歩行でこんなにてこずるとは思わなかったなぁ～。 １歩進んで２歩下がる。
■６月２８日その２■
おかしくなるのは回転角度がπを超えるあたりだろうと思っていたのだけど、思いかけずに色んな不具合が出てしまうため、グローバル座標で考えるのをやめることにしました。
一番最初はローカル座標で考えるつもりだったのだけど、引継ぎの部分が複雑なので、グローバル座標で進めていたのだけど、これだけやったら、ローカル座標での引き継ぎも記述する手順がわかってきました。
１歩ずつローカル座標で生成した重心軌道をグローバル座標に変換してつないだのがこれ↓
つなぎ目がちょっといびつなのがわかるかと思いますが、問題ないでしょう。
先ほど、全組み合わせのテストが完了して、実機にも組み込んで動作することが確認できました。 やったー。
まだ、上半身の処理ができていないので、上半身を取り外さなければ歩けないのだけど、どうせ、負荷算出もやらなきゃならないのでこのまま先に進むつもり。
実機で動かすと、やはりexceptionが出ます。 あからさまに怪しいところを削除して動かすと、直進動作の時はk_cos( )とかk_sin( )で発生します。 さっきカーブ歩行で動かすと、malloc( )で発生していました。 それぞれ別の問題かもしれないなぁ。
今日はわんだほーの申し込み開始日。 開発の予定が大幅に遅れており、１９日までに自律でダッシュ２０００を動かすところまでは到底たどり着けそうにないため、今回の参戦は見合わせるつもりです。 ラジコン操作でってならもしかしたらなんとかなるかもしれないけど、それじゃ意味ないもんね。
■７月５日■
久しぶりの日誌更新。
先週は月曜日から木曜日まで毎日飲んでました。 最近、二日酔いになりやすいような気がしていたので、飲む量をセーブ、飲んだ後は糖分と水分の摂取に気をつけてみました。 飲む量をセーブしたので糖分摂取の効果はわからず。(^_^;)
そして、金曜日は打ち合わせと出張と飲み会のあおりでたまった仕事を片付けるべく残業でした。
さて、土曜日は久しぶりにロボットを触れると思ったのだけど、なーんかテンションが上がらなくって結局ロボット的なことは何もせずにうだうだと1日過ごしました。 休養ってことで、よしとしましょー(^^ゞ
そして、今日こそは進捗を。
まずは、関節のトルク算出を見直し。 いままでは直進だけだったので、目標ZMPを見れば支持脚がハッキリと判ったのだけど、カーブ歩行だと、目標ZMPを見ただけじゃどちらの足が支持脚なのか、両足支持なのかわからない。 これはモーション生成関数内でも同じことで、そこで判定している情報を取り出せるようにして支持脚設定に使えるようにしました。
ラムダ・マーキュリーはリンク脚なのだけど、リンクを想定した算出は行っていません。 そのため、計算結果に手を加えて補正？を加える必要があります。 具体的にはリンク前側と後側でそれぞれ軸があり、片側はフリーの軸なのだけど、リンクのため、前側の軸と同じ角度に拘束されます。 つまり、一つのサーボで二つの軸を拘束しているわけだから、2軸分の負荷が掛かることになります。
また、足首ピッチと膝（下）ピッチ、股関節ピッチと膝（上）ピッチはそれぞれリンクにて拘束されているため、負荷は平均化されます。（工学的には平均化していいか知りません）その補正を加えたのが、これ↓
グラフは右足の関節負荷です。 軸足となる1歩目の股関節ロール軸と4箇所のピッチ軸が大きな負荷を受けています。データは下半身だけの場合です。全身にしたらもっと大きくなるわけです。 足首ロールはZMPとほぼ一致するため、あまり大きな負荷となっていません。再生精度が高ければ恐らく負荷補正は必要無いのでしょう。
じゃぁ上半身をくわえたモデルでの計算をしてみようと思い、上半身サーボの制御関数を作ることにします。
腕はヨー軸なしにして簡単にしたから簡単にできるはず～、と思って逆キネ関数を作ろうとしたら、解けない・ 解けない！ これもしかしてヤコビヤンじゃなきゃ解けないのか？と思って各文献を調べたら、ずばり今回の腕の構造の逆キネ計算が出ている本がありまして、手先の姿勢角度がなければ解けないようです。 危ない危ない。
次に胴体ピッチに長円リンクを使っているので、この変換関数が要ります。 これは円と直線の交点を求めればよいわけだけど、２次関数を解けばいいのか？なんか難しそうだぞ？と思って調べてみると、２次関数からも解けるみたいだけど、円中心を通る直線と直交する線を使った方法がポピュラーらしい。 こんなの学校でやったっけなぁ？？
簡単と思ってた関数が意外とてこずって時間を食ってしまった。ちょっと休憩してから続きに着手。
今日中に全身版の関節負荷計算を終わらせたいなぁ。
■７月８日■
今日中に全身版の関節負荷計算を終わらせたいなぁ。 なーんて言ってたけど、まだできてない。 すっかりとスローペースになってしまいました。
特に腰のサーボは間違うと即サーボ焼けって心配があるので、実機で動かす前に十分検証しないと怖くて動かせないでいます。(^_^;)
でも、動かせばすぐ判るのが、数値だとこんがらがっちゃってわかんないんだよなー。 外して動かせばいいんだけどね。
腕の逆キネの実装に悩み中。 ラムダ・マーキュリーの腕の軸構造が意外とクセモノで、手先角度によっては解析できないケースが多すぎる。なんか使いにくいな。それと、マーキュリー脚は、左右対称ではなく、基本的に同じ足が、2本並んでいる形なので、逆キネも左右対称ではなく、同じ関数を同じ座標系で使うことができた。
腕は左右対称となり、軸構造（軸の極性ってのかな）が違うので逆キネ関数も対象形を用意すべきか、座標系を反転させるか、 できれば足と同じ構成にしたいので同じ座標系を使いたいのだが。。
■７月１２日■
ここんとこ、ウィークデーはほとんどロボット開発に時間を取る事ができていません。
さらにさらに週末仕事が終わらなかったので、「できんのか！」の打ち上げ前に会社に行って一仕事。 せっかくだからちょっと買い物に行ったりしたりで、土曜日はロボット開発はお休み。
今日の日曜日がロボットデーになります。 ロボットデーって言ってもパソコンに向かってプログラム組むってことなんだけどね。
やっと全身の質点とサーボ角度を反映した歩行データを生成することができました。
↑これは一気に５歩分のデータを生成した時の重心点軌道です。つぎはぎじゃないので継ぎ目がありません。 足間隔は80mmなので、重心点は支持脚を越えないようになってますね。よかったよかった。
↓そしてこれが全身版の関節負荷データ。 腕のデータも計算しているのだが、意味ないので除外。
７月５日のデータと比べると、股関節ピッチ（J1茶色）が１．３倍くらいになっています。 重量が倍近くなってるのでこれくらいの違いが出るんでしょうね。 適切な計算結果なのかどうかさっぱりわかりませんが、逆転していない点では合っている可能性高いです。
ちなみに、凡例に同じ関節名が２回出てきていますが、リンク側の質点計算用に生成されるシャドーのようなもの。特にJ0,J1のようにリンクになっていない部分はリンク側の質点を計算するためだけのデータで結果としてみるときは意味がありません。（計算途中で使う）
質点計算に使うサーボは全部で２３個あるのですが、今は上半身サーボは固定のままです。なのでちょっとズルをして、質点計算では角度列じゃなくて固定値で計算している状態です。 腕を振って歩くなどの応用が出来ない状態なので、いずれ近いうちに拡張する必要があります。 はじめから全サーボ対応でプログラムしておいてもよかったな。
こないだの練習会で、ぴしいさんがロボ子を歩かせるうんぬんで議論してたり、そのほか諸々のこともあって、雌形のロボットを作りたいという気持ちが高まっています。 テーマは膝を伸ばした歩行ということで片足６自由度ではなく７自由度か、７．５自由度くらいを与えることで女性的な歩行をさせてみたいというものです。
なんとか着手できないかなー。今やってるラムダ・マーキュリーが形になったら、ラムダを改修してって思ってたけどそれは止めて雌形に走ろうかな。 ラムダ、シグマ、ときたから雌形はシータかなー(^_^;)
■７月１２日その２■
上半身つけて歩かせてみました。 顔はこけると壊れるので外した状態で。
直進はまぁなんとなく歩くけど、カーブ歩行は全然だめだなー。重心が移ってない感じなので補正をかけたら改善するのか微妙。
それよりもexceptionが問題。 なぜかカーブ歩行の方が発生しにくい傾向はあるけど、どっちにしても実用に値しない。 発生する場所も様々なので、割り込みタイミングの問題の可能性は大きいってことで、タイマー割り込みを止めてみた。 そして、計算だけを無限ループで実行させると、問題は発生しません。（１分くらいしか回してませんけど）
割り込みでの処理が問題なんだなー。 タイマー割り込みがあると、サーボの動作に関わらず問題発生します。 まだ、割り込み処理では色々と細かな処理を実行している状態なので、直接問題に関与している処理はなになのかを細かく切り分けてみます。 まさか、割り込み発生自体が問題ってことはないだろうなぁ。
■７月１７日■
まさかの事が起こってしまいました。
割り込み自体が問題のようで、メインループで計算させて、タイマー割り込みをかけるとexceptionが発生します。割り込み処理では実行的な処理は一切なしで、割り込みコントローラの処理を行っているだけ。 発生時のメッセージ情報からだと、数値演算関数 k_sin( )とかk_cos( ) とかで発生しているようです。 そして、通常のコマンド待ち処理だけならタイマー割り込みを発生させてもおかしくなることはありません。 、、もしかしたら浮動小数点ユニットが動いている時に割り込みが発生するとおかしくなるとか？ もしそうならどうしようもないです。 （ そんなことってあるんかな？？？）
いままでは割り込みルーチンに居る時間をできるだけ短くするように処理を考えてきたけれど、割り込みルーチンで全てを処理するようにして、数値演算をするような処理は割り込み内で行うようにしなければなりません。
ではと、ZMP規範の歩行生成処理にどれくらい時間がかかっているのかを測定してみました。
タイマー割り込みのカウンターを使って計測すると、 大体71msくらいですね。 計算量が多いカーブ歩行で、初期の1歩の方が計算量が大きいのでこれが最大か。
割り込み周期を80msとかに伸ばすわけに行かないので、生成ルーチンを4分割くらいにして4回の割り込みで処理すればできるかも。 比較的そういう構造にできやすいプログラムなのでなんとかなりそう。
あ、 SEMBの方ではまだ関節負荷計算をやってなかった。 さすがに80msは超えないだろうけど、80msに近くなりそうです。結構な計算量だな。
もしこれでうまく行ったとしても、計算量が増えるたびに処理時間について気にし続けねばならない。 そういうタイプのシステムはあんまり好きじゃない。
メインループでやりたいなぁ(>_<)
■７月１８日■
日誌を読んでくれていた岡田さんからアドバイスをもらい、浮動小数点変数をすべてfloatからdoubleに変更しました。
SEMB1200AのFPUはdoubleで計算しているのでfloatで計算するとdoubleに変換してから計算するので遅くなるそうです。 すべての浮動小数点変数をdoubleにすることで、演算時間は71.5msから68msに縮まりました。
ですが、依然として不定期にexceptionが発生します。
この演算を割り込みルーチン側で行うべく検討していたのですが、20msのうちに、4回のサーボへのデータ送出とデータ受信があります。 送信と受信の間には1msくらい間を空けているので、20msの残り時間は10msくらいです。なので、10ms未満に処理を区切らなければなりません。
簡単なところで、区切ってみましたが、多質点モデルからのZMP導出の処理に30ms程度かかっているため、もっと細分化するにはこのZMPの導出を数回に区切らなければなりません。 結構大仕事になってしまった。あんまり細切れになると、計算が間に合わなくなるかもしれないのでさじ加減が難しいかも知れないです。
なにより、歩行周期が早くなると計算できなくなる可能性もあるのでやっぱり面白くない。 うぅむ、、困った。
明日はわんだほー。 今回は残念ながら出場できないので見学です。 あわよくば持っていこうとしていた自分がこっけいですorz。。。
■７月２２日■
わんだほーの次の日から3日間、屋久島へ皆既日食を観に行ってきました。 ニュースでもやってたと思うんですが、観れなかったんですけどね。。。(>_<)
屋久島まで行ったのに、縄文杉を見ることもできずにすごすごと戻ってきました。(^_^;)
でも、皆既域でしか体験できない皆既日食時の暗さを味わえました。 ものすごく急激に暗くなって、また急激に明るくなって行くんですね。次は皆既日食を見たいなぁ。
←こんな感じから、
←急激にこんな感じに暗く。。 絞り全開で撮影。
←彼方は皆既域じゃないので明るいんですね。
日食ツアーの前日はわんだほーでした。 と言ってもイチ見学者なので気軽な感じでブラブラしてました。
テレコムセンターの展望台から撮ったガンダム。 足元が見えない～(>_<)
テレコムセンターの展望台から見えた虹。 よく観ると二重になっている。
そして、夜景に栄える竜鬼Ⅱ
ドカ すげぇ
ラムダの方はそんなわけで進展ありません。 岡田さんに色々とアドバイスをいただいているのですが、どうやらうちの環境と岡田さんの環境では結果が異なるようです。 うちの環境はsugi3のホームページからダウンロードしてきて展開しただけの環境なので何がなんだかわからんので、岡田さんのHPに掲載されている手順で構築してみることにしました。 gcc 4.1.2で構築です。 いま、newlibのmakeでエラーが。 今夜中に進展あるかなー。
■７月２５日■
案の定どっぷりと浸かってしまった例外問題ですが、岡田さんの強力なサポートのおかげで少し光明が見えてきました。
まずは、同じソースでもうちと岡田さんでは問題の発生の仕方が異なるということだったので、岡田さんのHPの記述に従って環境を再構築しました。 その結果、メモリー関連では岡田さんと同じ結果が見えたのですが、浮動小数点演算はまったく出来なくなってしまいました。 なにやってもexceptionで止まってしまう。 その後色々と試したのですが、まったく変化がなかったので、結局はcygwinまで再インストール、それも開発環境はフルインストールして、クロスコンパイラ環境を再構築しました。
そこへ岡田さんからメールの一報が届き、sprintf( )が悪い影響を及ぼしているようだ、という情報をもらいまして、やっと岡田さんと見えているものが一緒になりました。
semb1200aのプログラムではprintf( )文が使えないので、デバッグ用にあちこちにsprintf( )で確認したい変数値を文字列にしてコンソールに表示させています。それをすべて排除しなければなりません。 ライブラリにもfloatやdoubleを数値表示する関数は用意されていないようなので、これは作らなきゃならないですね。
まずは、ソースから全てsprintfを削除してちゃんと動くのかを確認してみましょう。他にも問題あるかもしれないですからね。
光明が見えて元気になってきました。
make待ちであちこちのブログ見ていたら、普通の会社員の西さんからライバル宣言が！ いやいやそんなもったいない。というより、ライバルじゃなくて「同志」にしましょうよ。ライバルだと情報交換できなさそうなんで(^・^)
■７月２６日■
まだだめです。
新しい環境でも、前の環境と同じ症状で例外が発生するようにはなりました。 もうだんだんとこの問題を解決するのが主目的になってきてしまったようで、ZMP計算の記述をみてもなにをやってるのか忘れかけてきました。(^^ゞ バッファが少ないなぁ。。。
ちょっと前までは「目的はロボットプログラムなのだから、必要な機能が動けば手段は。。」と考えていたのだけれど、もう、だんだんと別の手段を選ぶのが「逃げ」に思えてきてしまってちょっとまずいですね。 正直、割り込みルーチン側でZMP計算をキチンと終わらせるのは相当めんどくさい処理なのでやりたくないってのもありますが。
■７月３１日■
やりました。 とうとうexception問題が解決しました。
29日にテストプログラムと検討用に改造したロボットプログラムで動作を確認し、先ほど割り込みでサーボを動かしながらの動作確認でどうやらexceptionは発生しないようです。
結果としてはgcc4.1.2で、コンパイルオプションで-Oをつけることで問題は発生しなくなりました。アドバイスをいただいた岡田さん、Zakさんありがとうございました。 でも、なぜ最適化オプションをつければうまくいくのかはわかりません。
gccを4.1.2に入れ替えたりして、sprintf()を使わないようにした時点でsugi3のところからDLしてきた開発キットでの状態とほぼ一緒になりました。そして最適化すれば解決したわけだから、sugi3の環境（gcc3.4.4)で、最適化すればsprintf( )も使えるようになるのかもしれません。 試してみようかな。。。
最適化オプションをつけた場合のコンパイル結果を比べてみようかと思って、
main( )
double a;
{
while(1){
a = sin(1000);
}
}
と言った感じのプログラムを作ってみたのですが、これで最適化オプションをつけてコンパイルするとただの空の無限ループが出来てしまって比べようがありませんでした。 その結果を見て、もしかしてラムダのプログラムを最適化した場合も何もしないようになってたために例外が発生しなくなったのならばえらいこっちゃ！と思って急いで動作確認しました。 動いてよかった(^_^;)
間に旅行に行ったり、仕事が忙しかったり、飲み会が多かったりしたのだけれど、7月12日から2週間以上はこの問題にどっぷりと浸かってたことになります。長かった。。 明日からやっとロボット開発に取り組めます。
■８月２日■
とりあえず順調に動いています。 よかった～。
カーブ歩行で関節軸の負荷を計算させてみると、歩数が増えるほどに負荷が増えていっている。 そんなことはないので計算に間違いがありますねー。姿勢変換をうまく取りこめられていないのか？単なる変数のクリア忘れ（増加一辺倒ではないので違うと思うが）なのか？
とりあえずは、カーブ歩行生成の二次元に展開した歩行生成のプログラム記述や、引継ぎ歩行の引継ぎ方法などを直進タイプにも反映させてからにします。
感覚としては全身タイプだと歩かせるのにまだまだ苦労しそうな気配です。
■８月５日■
カーブ歩行でのアルゴリズムを直進歩行にも適用しました。 ところが、またまたコンパイラの問題？らしいものに遭遇。
数値計算部分のデバッグは、SEMBに転送せず、PC上でプログラムを動かして数値をログに取って検証しています。 SEMBのexception騒ぎで、floatをすべてdoubleに置き換えたのですが、それをPC上で走らせると変な動きをします。
追跡してみると、
double pace = 0.5;
double dt = 0.02;
int c = pace / dt;
とすると、c=25のはずが、24となってしまう。 なんだこりゃー。ちなみに
int c = 0.0000000001 + pace / dt;
とすると、c=25になります。 たしか誤差が発生する問題があったような気がするが、こだわるところではないので、PC上ではマクロでfloatにしてコンパイルするようにしてSEMB上で動かした場合と同じ動きになるようにしました。
で、さらに進めると、今度は逆キネ変換でエラーで止まってしまう。これもexception騒ぎの時に計算不可状態を検出するように記述を加えたためで、マイナス値をルートしたり、1以上の数をacosしたりしないようにチェックしています。
追いかけると、引継ぎモーション生成段階で、多質点モデルでのZMP導出部で、引継ぎフレームを合成しているのだが、ここの計算結果がおかしい。
するとどうしてSEMBではちゃんと動いていたのか？ と思ったが、SEMB上では腰高さを230mmにして動かしているのに対してPCデバッグ環境では270mmと足を伸ばした姿勢となっていた。 そのため、ちょっと無理めな姿勢でもちゃんと逆キネできていたのでした。
おー、意外なところでバグ追い出しが出来たなと、いいように取ってみたのだが、2週間のブランクのためプログラムが読めない(^_^;) 2週間前の自分は既に他人となっておりました。コメントいっぱい入れてるつもりだけどまだまだ足りないようです。
というわけで、1歩進んで2歩下がっております(^^ゞ
■８月９日■
釈然としないまま作業を進めています。
まず、引継ぎモーション生成段階で作成する引継ぎフレームの合成についてはデバッグ完了。 おかしな姿勢を生成するバグは解消しました。
そして、カーブ歩行生成で作成したZMP規範の歩行生成ルーチンを直進歩行生成にも適用することができました。一本化はあまり意味がないとして見送り。
次に関節負荷算出ルーチンのデバッグです。直進でもやはり歩くほどに負荷が増えていってしまう。 プログラムをトレースして計算式を確認したところいくつかのバグを見つけはしたのだけど、計算結果に大きな差が生じるものではありませんでした。
↑ロール軸は問題ないが、ピッチ軸は歩くほどに負荷が増えている。計算がおかしいってこってす。
どう考えても怪しいのは空間速度・空間加速度の部分。ロボットは歩いて行ってるのに計算上の原点は歩き始めの空間原点を使って計算します。 なにより空間速度・空間加速度の考え方がイマイチ理解できていないのが辛い。
そこで、計算上の原点の考え方を変えてみることにしました。多質点モデルでのZMP導出では原点を移動させると計算結果がくるってしまいますが、関節負荷計算では質点の移動速度・加速度は計算に使用しません。また、関節軸姿勢の変化速度・加速度も計算しないので、原点が動くことは直接問題ないはずです。
あまり理解できていない空間速度の考え方を鵜呑みすれば、原点が変わっても影響ないはずだと思って確かめてみました。
これは各質点の空間速度のY軸要素です。原点を歩き始めの空間原点としたものと、目標ZMPとした場合に違いはありませんでした。
であるとすれば、関節負荷の計算に使用する値全てを目標ZMPにすれば良いのではと、計算してみたのがこれ↓
軸足が変わる瞬間、つまりZMPが移動する瞬間の関節負荷がほぼ全関節とも0となっているのがおかしいですが、値が増加しなくなりました。
この考え方が正しいのかどうかわかりませんが、これをカーブ歩行にも適用してみました。
ぐちゃぐちゃ。。 なーんとなく、ロボットが回転していっている様子が計算に反映されていないっぽい感じがします。 もう一息。
来週末は帰省して、再来週は関東組ロボット練習会かぁ、、それまでに解決できるかな。
・・・・・
ずばり、ロボットの回転量が反映されていませんでした。ちゃんと確認してから日誌に書くんだった。
この値が合ってるのかどうか怪しい部分はあるのですが、次は関節負荷をマーキュリー脚（リンク脚）の特徴である負荷の平均化作用を反映して平均化処理を加えます。
・・・
マーキュリー脚に合わせた補正を行いました。
↓右足
↓左足
股関節ヨー軸のパルスがなぜか左足の方が大きいようだけど、今はあまり追及しないことにします。
さて、次は歩かせてデータ取りをします。 補正の必要量とスレッシュドレベルを決定する必要があります。
■８月１０日■
歩かせる前にまだやることがありました。
引継ぎモーションの関節負荷算出。 これも、引継ぎモーションでの多質点モデルからのZMP導出の場合と同様に引き継ぎフレームを生成して関節角度の速度、加速度をうまく引き継ぐように考える必要があります。
引継ぎフレームの生成は関数にして取り扱えるようにしておいたので、時間をかけずできました。
そして関節負荷の計算をさせてみると、まったく引継ぎフレームが役に立っていません。 しばらく悩んだ末、引継ぎモーションの支持脚データがおかしいことに気付きました。引継ぎフレームの支持脚は正しいデータだったので、まったく引継ぎがうまく行っていなかったわけです。
↓これが引継ぎモーションの関節負荷計算結果です。
昨日の関節負荷グラフの横軸61辺りから引き継いでいます。おかしくないデータになっていると思います。
■８月１３日■
やっと直進とカーブ歩行が連続で実行できるようになりました～。 ここまで長かったなぁ。
直進歩行をカーブ歩行と同じように２次元拡張バージョンに書き換えて、引き継ぎモーション生成まで行うことができるようになりました。 初期モーションでも引継ぎモーションでも関節負荷の計算まで行っています。
動作テストを始めたとき、直進歩行を開始すると、引継ぎ部分で逆キネ変換エラーで止まってしまいます。 おっかしぃなぁ～と、しばらくソースを眺めたところ、引継ぎモーション生成の２週目、つまり目標ZMP列と多質点モデルからのZMP導出値との差分から重心軌道の補正値を算出する部分でエラーが出ているのを突き止めました。
たしか、ここは計算結果が発散するから差分値を強制的に修正したりして手を加えたはず。。 案の定、差分値列の先頭を強制的に0にする部分が漏れていました。 ふぅ、これでOK。
こういう部分は教科書には書いていなくて、試行錯誤で見つけた記述です。大事ですね。
歩行モーションは生成できるようになって、例外も発生せずにモーション再生ができるようになりました。 でも、まだ歩けないんです。(^^ゞ
関節負荷の計算は行っていますが、その計算結果は使っていません。 負荷計算結果を関節の指示角度に反映させるために現在の再生精度を測定する必要があります。 長らくexception問題で、測定部分はコメントアウトにしていたのですが、とうとう解禁の時がきました。
そして解禁して動かすと、、、、 exceptionで止まっちゃいました。＼(◎o◎)／ ・・・・・・・原因究明していないので、、バグだと思うんですけどね。 そろそろ時間なので今日はここまでです。
この週末は実家に帰省するのでロボット開発はできません。 そして翌週末は練習会。 また歩かないロボット持ってかなきゃならないのかなー。
■８月１８日■
この週末は、いつになく短い帰省をしてきました。 金曜日夜に出発して月曜日朝に戻ってきました。普通は5日程度なので極めて短い帰省でした。
車で夜移動なので、昨日は徹夜明け休暇(^_^;) 高速代1000円のおかげでこちらに戻る時は渋滞ばっかりでして、いつもなら３時くらいには着いてるところが朝の７時到着になりました。昨日は徹夜明けでほげーっと過ごしておりました。
いや、ほげーどころじゃない。 データサーバにNASを使ってるのだが、 突然「ピピピピ」アラームが鳴り出してLEDが赤く点滅。 リブートしてもアラームが出るばかりで立ち上がらない。 購入半年で壊れやがった。 アイオーデータです。(>_<) それを梱包して修理センターに送りつけたりしてました。
データは、サーバから更にセカンドディスクにバックアップしていたので消失の難は逃れたのだけれど、実はもう少しで大変なことになるところだった。
というのも、つい先日家族が使っているノートPCのOSがぶっとんで、ノートPCのデータがすべてなくなってしまうかもしれないという自体が発生して、ハードディスクを初期化しない状態でリカバリーすることでデータを保護したという事件がありました。ですので、「データサーバーに入れておけば、バックアップもするから安心だよー。」と進言した結果、危うく失いかけたデジカメデータをデータサーバーにコピーして、ノートPCからは削除したところだったのです。 進言どおりにバックアップが取れていれば問題なかったのだが、週1回のバックアップ実行前に先の事故があったものだから、事実上デジカメデータは消失してしまいました。 ところが、でかいデータなのになぜかすべてゴミ箱から救済できて助かった～ということがあったのでした。
さらにはノートPCにはコピーせず、直接サーバにストアしたデジカメデータもあったのだが、それは本当にたまたまCDに焼いたのでこれまた消失を免れたのでした。プログラムデータなんぞはもし消失してしまっても自分の頭からまた生成できるかもしれないけど、デジカメデータはもう取り戻せないからホントにやばいところでした。
さて、ラムダ。
exceptionが発生する原因はわからないままだけど、exceptionを発生させないで角度データの取得はできるようになりました。でも、なーんかおかしい。
↓これがサーボに与えている角度データ。 ところどころに０というデータがあるのだが、なぜこんなものがあるのかわからん。
↓そしてこれがキャプチャーデータ 汚い。 なんだこりゃ？？ 以前にキャプチャーした時は読み込みミスは皆無だったのになにがどうなっているのだろうか。
へんな値は別として、０の部分は指示データとキャプチャーデータで一致しているので、プログラム上の問題らしい。 あきらかに割り込み処理がからんでいるみたいなので憂鬱です。(^_^;)
■８月２３日■
昨日はロボット練習会。 でも、ちょっと前にDigikeyに注文していたBeagleBoardが届いたので、その周辺部品を買うために秋葉原に寄り道しました。
買ったのは
・ＡＣアダプター
・USBハブ（ACアダプタ付き）
・４GBSDメモリーカード
・SDHC対応のSDメモリーアダプタ
・USBキーボード
・USBマウス
・HDMI-DVIケーブル
あと、PCのマザーボードからCOMポートを引き出す時に使う、ヘッダーピン-Dsub9ﾋﾟﾝのケーブルが欲しかったのだが、30分くらい探しても見つからなかった。 荷物も重かったので諦めて練習会場へ。
BeagleBoardはCPUボードで、ストレージにSDカードを使うことでLinuxを動かすことができます。RAMは256MB、ペリフェラルとしてはUSBポートがあるのでハブを経由することで、キーボード・マウス・LANポートなどをつなげることができます。そのほか、オーディオ入出力、ビデオ出力もできて、Digikeyで15000円弱。 安い！
こいつを買って、ネットワークサーバーを作るってわけじゃなく、もちろんロボットの大脳にしてやろうというつもりなので、ビデオ出力もキーボードもマウスも本当は要らないんだけど、ま、一度はフルで動かしてみようかなってことで買い揃えてきました。 立ち上げに必須のコンソールコネクタが手に入らなかったけど。(^_^;)
コンソールコネクタからDsubに変換するのは3本の線を引っ張り出せばいいだけなので、結局は自分で作っちゃいました。
あとはHDMI-DVIケーブルのコネクタがでかくてディスプレイと干渉して挿せないという問題がありましたけど、コネクタカバーを一生懸命削ってなんとかつなぎました。
インストールは日経Linuxの2009年7月号を参考に。 誤植だらけだったけど、それ以外は順調に行きました。
↓CPUボードは3インチ×3インチ
↓LANにも無事つながってネットも見れました。
GPIOも使えるみたいだからLED点滅とかスイッチとかつなげてみましょうか。
このインストールのために、PCにLinux Ubuntuデスクトップをインストールしました。Linuxディストリビューションの進化はすごいですね。Windowsから乗り換えてもいいんじゃないかと思います。 アップデート情報も自動更新できちゃうみたいだし。 ・・・もうWindowsはどんどん重くなってきてイヤです。(ーー;)
ラムダですが、、
練習会でオサル親子に「今日はラムダが歩くところ見れますか。」という厳しいプレッシャーを聞き流し、キャプチャーのデバッグをしておりました。
金曜日に、仕事中に思いついたタイミングミスが読み取りミスの原因だと思いついて、その検証などやっておりましたが、さっぱり検討違いでした。がっくり。。
それよりも、いつも畳にパンチカーペットを敷いた上で動かしていたので気がつかなかったのだけど、サーボのトリム調整が取れていません。硬い床だとガタガタしてます。これはまずいな。 それに、足高さが指示値から大きくずれている。あれ？おかしいな。
というわけで、まずはサーボのトリム調整のやりなおし。 今はあるポートにつないだ状態でしかトリム調整できないようなプログラムしか作っていなかったので、いつでもどこでもトリム調整できるようにプログラムしましょう。 あと、足高さのズレはオフセット値の極性が間違えていたのが原因でした。
オフセット値の極性を修正すると、少しだけ歩けるようになってきました。いいぞいいぞ。
■８月２４日■
サーボの角度キャプチャー部分の重大なバグを発見して、exceptionの発生はほぼなくなりました。 ほぼ、というのは修正してから1度だけ出てしまいました。。(>_<) 終わったと思ったのに。。。
もっとも、どうやらmalloc( )はメモリーアロケーションエラーを返さないので、mallocエラーだった可能性もあります。 取得アドレスを監視して、RAMエリア外だとエラーだと判断する記述が要りそうです。
そちらの方は大幅に前進した感じがありますが、キャプチャーのエラーはまったく改善しません。 タイミングや手順ではなく、伝送路を見直した方がよさそうです。 以前にうまくいったときからの差分を考えると、、、ケーブルを束ねて整線したなぁ。 ばらしてみますか。
エラーだらけのキャプチャーデータを眺めてみたのですが、やはり、指示角度と大きな差があるのは股関節ロール。その他のサーボは顕著なズレはない。 負荷データを見ると膝の方が負荷が大きいことになっているからちょっと困ったな。 やっぱり負荷算出に何か問題がありそうな気がしてならない。
歩く様子を見て、キャプチャーデータと比較してみると、上体が支持脚側に移動しきらずに、遊脚を上げる動作が始まってしまうため、上体が遊脚側に倒れこんでしまっているために支持角度以上の実角度になってしまっているように見える。 つまり、両足支持期間を持つ必要があるということだ。 歩容生成ルーチンに手を入れなきゃならないな。
↓股関節ロール軸の指示角度と実角度 支持脚になっている時の角度のズレが大きい。
■８月２６日■
やっぱり負荷計算結果はおかしいと思いつつ、次に進んでいたのだが、いざ負荷データを活用する段になったらやっぱり気になって手が進まない。
ラムダからマーキュリーに移植をした当時の負荷計算結果はグローバル座標原点で計算していても問題なく計算できていたはず。。 記憶を手繰ると、ベクトル関数を整理した事を思い出してきた。 当時のバックアップが残っていないかどうかを確認してみたが、見当たらない。
仕方が無いのでラムダでの負荷計算のソースと照らし合わせてみた。
①おっ！空間加速度の算出で違う記述をしているところがある！ 教科書と比べてみると、、マーキュリーの方があってるやーん。ラムダのバグ発見。
②おぉ、ロボットの向きが軸の角速度・角加速度に反映されてないやん！（これは後ほどそれでいい事がわかる）
③あ、遊脚の角速度、角加速度の符号が違う！ 直してみたけど影響なし。（なぜ影響がないんだろう？）
④あれ～、質点の重力影響の項を省いたら負荷が全部ゼロになってしまった！（この辺りで重大なミスが内在していることを確信する）
⑤運動量と角運動量の計算式おかしくないか？（と会社で思いつく）→おかしくなかったのだが、ベクトルの掛け算の関数が間違えてるやーん。
⑥やった、グローバル座標を使ってまともな計算結果が出た！
⑦カーブ歩行だとものすごいインパルスが出てしまう。おかしいな。
⑧軸の角度・角速度・角加速度をファイルに出力して検査。 なんだこりゃ？全部同じ数値？角加速度がほとんど０でたまにでかいパルスが出るだけ？？？→ ②の折込が間違いだった。
そしてやっとそれらしい計算結果が出ました。 軸の角加速度ででかい値が出るのは当然。出ない方がおかしいのでした。
↓これが直進歩行の右足、左足。 遊脚の入れ替わりのタイミングでヨー軸にパルスが出てます。 パルスを除くともっとも負荷が大きいのは股関節ロール軸、補正が必要なのは10^9以上と考えてよさそう。 パルス部分に補正をかけた方がいいのかどうかは実験してみて確認ですかね。
↓そしてこれがカーブ歩行での右足、左足。 ほぼ直進時と同じような計算結果になっています。
股関節ロールに比べて、足首ロールはほとんど負荷がかかっていません。もちろん、これは計算によるものなので理想的な動きの結果であるからなのですが、ZMPとほぼ同じ位置にある軸なので、モーメントはほぼゼロになるのが正しいのです。
よーし、やっと負荷算出については晴れやかな気持ちになれた。 次は歩容の調整。 両足支持期間を設けるように拡張しなくてはならない。
■８月２８日■
昨日は関節負荷の計算がうまくいったので、気をよくして飲みに行ってました。 おかげで今日もぐったりしてしまって脳みそ動いてません(^_^;)
ロボワン富山、 出場しないのはもちろんのことなんだけど、観戦もどうしようかな、いままで地方開催は観に行ったことがないので今回も流すかなぁと考えておりましたが、平野さんのブログを見て、ふらりと観に行っちゃおうかとも考えたり。 9月末は、日食を観に行った7月後半に続いて仕事の山場なのでやっぱりすんなりと休めるかがちょっと不安ですが、平日に休暇を取るわけではないので大丈夫かな。
まずは予約だけはしておくか。
■８月２９日■
トリム調整のためのプログラムを作成。 というかラムダ・マーキュリーの制御プログラムに書き足したのだけなのだが。
簡単なプログラムのつもりだったのにあちこちひっかかって随分時間をかけてしまった。 やっと出来たと思って調整しようとしたら、いくつかのサーボが言うことを聞かなくなってしまった。 おっかしーなー。 ROMデータをいじるから注意しながらデバッグしたのだが、、、。
調べてみると、同じUARTにつながっている4個のサーボのうち、ID2のサーボのROMデータが書き換わっており、ID0になってしまっている。同じラインにID0がふたつになってしまってうまく動かなかったわけだ。
恐らくは、ID0向けのROMデータを送ったところがデータ化けを起こしたかなにかで、ID2のサーボが自分宛のデータだと思って受け取ってしまったのだろう。 チェックサムさえないからチェックのしようがないのだ。
今は、1ラインに4個のサーボを乗せており、ID0からID3を使っている。 これだと、1ビットエラーで、ID0がID1になったりID2になったり、 ID1はID0になったりID3になったり、ID2はID0になったりID3になったり、ID3はID1になったりID2になったりしてしまう。
ICS2.0ではIDは0から31までの32種類あるので、こんなに窮屈に使う必要はないわけで、ID1、ID2、ID4、ID8 と言う具合につかえば、最悪でも2ビットがひっくり返らないとデータの取り違えは起こらない。 最初からそうすりゃよかったな。 これからID変更するのは結構大変な作業だよ。(ーー;)
あと、おかしくなったサーボ３つのうち、2つはICSモードが解除され、PWMモードになってしまっていた。IDの書き換わりはなかったので先の現象とは別の問題。
これは恐らくだが、デバッグの段階でROMデータをデタラメに送ってしまった（4byteズレデータを送信してしまった）ことがあり、やっちゃったーという場面があったのだが、なんだか問題なさげだな？ということがありまして、恐らくそのときにサーボ側の処理として何らかの初期化が行われたのではないかと思います。 想像ですけどね。
ずれたデータ各レジスタ値としてはありえない値になることがあるからそれに対する保護処置はあるようだ。
明日はIDの付け直しからスタートです。(>_<)
サーボのID付け直しをするついでにケーブルの整線を解きます。 もしかしてケーブルを束ねたせいでノイズが乗ってキャプチャーに失敗するのかもしれないということで外して実験する予定でしたので。
IDの付け直しをしたらトリム調整の続き。 ID付け直した結果、違うサーボに書き込んでしまうような事態にはならなかったけど、エラーでどうしてもトリム値を書き込めないサーボがいくつかあった。 ううむ。。気持ち悪いがトリムだけのことだから、ここはスポット的にケーブル配線を変更し、トリム調整をすることに。
調整する前から比べると随分と計算どおりの位置に足がくるようになったのだが、まだ歩けない。
次に支持脚と遊脚の切り替わり時に両足支持期間を設けるように歩容生成ルーチンを修正する。 今は、両足が地面に着くのは1フレームだけなのだが、これを数フレーム設けて重心の移動を速やかに行う。
両足支持期間を設けることで、すり足気味に歩き出した。
でも、足上げ寸法を大きくするとばたついてこけてしまう。微妙だ。
MDFボード上で歩かせるとスリップ気味になんとか歩くのだが、カーペット上だとスリップしないので左右に吹っ飛び気味になる。 両足支持期間を大きくすると吹っ飛び度合いが増す。
これは、ZMP規範で重心軌道生成した場合に、支点付近の質点を計算に入れるとちょっとうまくないというラムダで遭遇した現象と同じです。やはり足首から下の質点はZMP算出から除外した方がよさそうである。（支持脚のみね）
ここまでの作業はまぁ順調。 次にキャプチャー部分に再挑戦。 exceptionの問題は解決したので、ちゃんと関節角度をラジアン表記でストアするようにプログラムを修正しました。
ここからが問題。
キャプチャー値どころか指示値もおかしな値に変換されてしまう。 計算式を見直したがわからない。 全部がおかしいわけじゃなくて部分的におかしな値になるのだ。
まーた割り込みの問題か？？？ やだなぁと思いつつ問題個所を特定すると、なんと倍精度値同士の掛け算がおかしい。 さすがにコンパイラエラーはありえないだろうと思いつつ色々と調べてみたがわからない。
最後は一定変化する値を計算して、やっと問題の法則性を見つけました。 自分で作った浮動小数点数の表示関数のバグでした。 え～ん(>_<) printf( )使えればこんな無駄時間過ごさなくてよかったのに。
＃そういや、scanfも使えないのかな。きっと使えないような気がする。。。 サーボ設定プログラムに使ってるのにな。。。自分で作るのイヤだな。
下らないバグに随分と時間を取られてしまいましたが、やっと解決。 これからは安定歩行するように歩容調整と足裏調整です。
いま、上半身はとりあえず形になっているが、肩周りや、背中に背負ったRPU-100などこけた時のダメージが大きそうな部分が残っています。今後はこけること前提の作業になるのでこの辺りの強化も考えなきゃならないです。