開発日誌(44)
トップへ戻る
開発日誌インデックスへ
前のページへ
次のページへ
そういえば、開発日誌の更新をしていなかった。
実は10月から山梨に転勤になり、単身赴任となりました。山梨に来て既に１ヶ月、これから寒い冬を迎えようとしています。
単身赴任寮が無いので、部屋を借りて家賃補助をもらって住むというスタイルになります。
先に単身赴任に来ている友人や上司の話を聞いて、甲府駅から歩いていける範囲で家を探しましたが、色んな条件からちょっと広すぎる2LDKの部屋になってしまいました。
さすがに家賃が規定額をはるかにオーバーしてしまうのですが、狭い思いをするのはヤダなと思いまして自己負担することに。
そのおかげで、広々と空間を使う事ができてます。
広いけど１人住まいだし、オールフローリングでほぼバリアフリー。ホームロボットの研究開発にはもってこいの環境になりました。
現実には、ちょっとスラブ厚が薄そうなのでロボットがちゃがちゃ歩かせるってわけにはいかないですが。
さて、ラムダ。
動力学フィルターができたので、実機実装に進みたいところですが、実機に実装して今までのシミュレーションみたいに数歩だけ歩かせるだけというのも面白くないので、自由に歩行できるようにプログラムを拡張する必要があります。
今は、歩き出しから停止するまでの歩行計画を立案し、そこに向けて歩容を生成しながら動くということにしていました。
動力学フィルターを使うためには未来の歩行計画が必要です。停止までとは言わないけれど1秒先くらいは必要になります。
自由に歩行ということは歩行計画がままならないということなので、都度歩行計画を追加していく必要があります。
すると、予見項が使えなくなるシーンと言うのが必ず出てくるので、新しい歩行計画で予見項を再構築するという仕組みが必要になります。
そのあたりの作り込みが必要になってくる、といった状況です。
未知の歩行計画と言っても、歩幅を変えたりペースを変えたりだけじゃ面白くないのでカーブ歩行を組込みます。
カーブって意外と面倒で左右で１歩の長さが違うとか、ロボットの方向を徐々に変えなきゃならないとか、オフライン歩容生成の時に結構な作り込みをしました。
そのおかげでロボット姿勢の管理関数やら目標ZMP列生成関数やらはオフラインの時のがそのまま使えます。
ホビーロボットの界隈で昔から注目しているのが、ギィさんと柴田さんの歩行モーションです。
ギィさんは最近はダンス一辺倒なのでアレですけど、柴田さんのブラックサンダーの歩行はいつも見ていて涙が出る程すばらしいと思ってます。
あそこまでヌルヌルと美しく歩ければ、ある種完成の域にあるのではないかと思います。 あのサイズのロボットだとあの速度で歩けなきゃダメかなと。
で、GoSimulation上だと無理に動かしても壊れないのでペース0.2秒でできるだけ大股で歩かせてみました。
結果、全然ダメです。 まず、ロボットの足の稼働範囲が大きな歩幅に対応していない。膝ﾏｹﾞﾏｹﾞ歩行でカッコ悪い。
更に、この速度で動かすとヨーモーメントが床面のグリップを上回ってしまうらしく、まともに歩けません。
足の稼働範囲が狭いのはわかっていた事で、今回このアイディアで実機を作る事を優先したのでひとまず諦めましょう。そのうち考えます。
ヨーモーメントについては【とうとう】腕振りでモーメントキャンセルをしてみます。
というのも、２足歩行ロボットに取組み初めて随分とたちますが、実は歩かせる時に腕を振らせたことがありません。
そこにポリシーがあるわけじゃなくて、腕を振らなきゃ歩けなくなるくらいの速度で歩かせたことがないのが多分原因だと思います。
あと、腕って結構厄介で、逆キネ計算で動かすのに制約を色々つけないと計算が大変な量になったりします。
＃これは腕の設計に依るが、冗長な場合が多く、直交軸じゃない場合も多く、逆キネの解析解が得られない場合もしばしば。。
足の場合は「平たんな床に立つ」などの制約を与える事で計算が簡略化されるケースがあるのだが、、。
そこに対して自分で腹落ちする方法が見いだせなかったため、というのもあります。
梶田先生の教科書にはモーメントについての計算も考慮していますので、もちろんラムダのZMP計算関数にもモーメント項の計算式は組み込んでいます。
でも、オフラインの時に比較したところ、影響が小さいと判断して今は計算していません。
それを使って、モーメントを打ち消すような腕の振りを生成させるというのも結構簡単に出来るかなとも思います。
今回はそこもすっ飛ばして支持脚軌道の逆位相で腕を振る事でキャンセルさせたいと思います。
＃腕の有る無しでのモーメントの差くらいは見ておくかな。
↑ 腕の振りを加えた歩行。 頑張ってペース早め歩幅広めにしています。
気付くともう年の瀬。
１か月以上日誌を更新していなかった。
新しい職場に慣れない、というかリズムが作れない、というか出張ばかりであまり家に居ない、というか出張に行くと飲みに行ってばかりの上に、山梨でも飲み会が重なったりで、あまり順調に開発が進んでいく訳もなく日が過ぎてまいりました。
完全にロボット開発をスルーしていたわけじゃなく、動力学フィルターの実機実装の検討は進めておりました。これが意外と難物で、簡単にできるとは思っていなかったけど、予想以上にめんどくさい。
オンライン生成と動力学フィルターは同じ予見器を使っています。
オンライン生成自体は単質点倒立振子の運動方程式を使っているので実機とは誤差があります。
そこでオンライン生成で計算した重心点座標になるようにロボットを歩かせた時に、ZMPがどの位置になるかを多質点モデルを使って計算します。その計算結果と、元々のZMP計画とを比較して差分を取ります。
その差分列を新たな目標としてオンライン生成で重心軌道を計算します。
これが動力学フィルターです。ZMP差分を使って再計算して足しこむって方法はオフライン生成と同じですね。
ZMP差分を計算するには
１．オンライン生成
２．多質点モデルによるZMP算出
が必要で、 最終的な重心位置の算出には
３．オンライン生成
４．ZMP差分を入力としたオンライン生成
が必要です。
１．と３．は予見ステップ数を隔ててはいますが同じ計算ですので、２度計算するのはもったいない。
そこで、ZMP差分を計算した段階で、オンライン生成とZMP差分をリストに格納して保存します。
最終的な計算をする段階でZMP差分リストを使って算出した補正量（４．）とオンライン生成結果リストの一番古いデータ（１．）を使って最終的な重心座標を決定します。
さて、歩行計画に変更が生じて、ずっとまっすぐに歩くつもりだったのが、急に曲がれとご主人が言っている、とします。
すると、予見データは１秒先まで用意しているので、１秒先から曲がりますよ〜、と言いたいところなのだがそれだと階段から転げ落ちるとか、取り返しのつかないことになるのかもしれない。
ですので、速やかに歩行計画の変更に応じる必要があります。
（予見器の未来項をどれだけ短縮しても使えるとかの検討は無意味なので行いません）
オンライン生成、多質点モデルによるZMP算出、ZMP差分を使った補正量計算、どれも速度、加速度を使った計算だし、予見器に至っては過去データの累積を利用しています。
更には歩行モーションの生成もZMP列を参考に生成している関係で、状態変数を使っています。
ですので、ある状態を自由に作り出すことは困難です。
予見器は歩き始めからの状態を再度行う必要があり、多質点モデルは目的のフレームの過去２フレームのデータが必要です。
任意の状態を再現するには過去のすべてのデータを保管しておく必要があり、きわめてナンセンスです。
私は、少し潔癖症？なところがありまして、無駄なことをやらないためならその無駄な事をする時間の何倍もの時間をかけてでも「やらない手段」を考えるというところがあります。
で、その潔癖にかけてなんとか計算量、メモリー量が適正な手段がないかと考えましたが、結局は目の覚めるような手段はみつかりませんでした。
結論としては「動力学フィルター計算と同時に動力学フィルターを使わない計算を実施する」ということでした。
動力学フィルターの予見項の計算にはオンライン生成が必要です。でも未来項のために１秒までも先の計算をしてしまったらそれを現状まで巻き戻すのは困難です。（できなくもない気はしている）ですので、１秒未来の計算と現在の計算を行い、計画が変更になったら１秒未来の計算は捨てて、新しい計画の元に現在の計算状態を使って１秒未来までを一気に計算して未来項を再構築します。
多質点モデルについては過去２フレームの姿勢さえあれば再構築できる手段はありますが、ここはひとまずモーション生成も同時進行します。
その結果、
１．未来項
１．１ オンライン生成（１秒未来）
１．２ 生成結果を使った歩行モーション生成
１．３ 多質点モデルによるZMP計算⇒ZMP差分計算
２．現在
２．１ ZMP差分からの補正量計算
２．２ 補正済データでの歩行モーション生成（⇒これが歩行モーション）
３．再構築用
３．１ オンライン生成（現在）
３．２ 生成結果を使った歩行モーション生成
３．３ 多質点モデルによるZMP計算
と、これだけのことをフレーム毎に計算します。 間に合うのかな。
３．２、３．３についてはZMP計画変更に備えての計算なので、計算だけ行って結果は利用しません。
この組み込みをGo Simulation!上のプログラムで実現してみました。
ちなみに、動力学フィルターを使わなくてもこれくらいは歩きます。
十分安定してる。。もう少しハードに動かすと差が出てくるようです。
実機実装するには更に課題があります。
それは予見項の再構築時間です。約１秒分の計算が必要です。（25ms周期で動かしていたら40コマ分）歩行を開始するときにも同様の計算が必要ですが、それは計算が終わってから歩き出せば良いので問題になりません。
歩いている最中にこれをやるとなるとサーボにコマンドを送った残り時間で行わねばならず、オーバーは許されません。
計算量が変わるわけじゃないので間に合うならいつも間に合うし、間に合わないならいつも間に合わないのですが。
更にはゆくゆくは制御周期を短くしたい。そのために現在のCortexM3からM4にしたいと思っているのに、そうすればサーボの通信速度も1Mbpsにできるから5msくらいにはできるかな？と期待しているのに、FPUが使えるから計算可能量増えるな、と思っているのに、予見項は40コマから200コマに増えるのできっと逆鞘です。
ですので、この計算は割り込みスレッドではなく、別スレッド（OS使ってるわけじゃないので、要はメインループ）で行う必要があります。
それも裏で再構築計算しながらも歩行計算は実施し続けるので、一時的にデータを二重化して計算が終わったら引き渡すという処理が必要です。
実はこういうのは結構好きで、動いたら気持ち良いのですが、、、ハマると地獄です。
ハマらないためにもシミュレーション環境、PC環境でじっかりとデバッグしてから実機実装したいと思います。
実機に載せるにはそれ以外にも問題がありました。
それは予見器により発生する累積誤差です。オフライン生成では累積誤差というものは無いのですが、オンライン生成では予見器を使っているため累積誤差が発生します。教科書にあるように制御系を拡大系にしているので誤差は抑制されてはいるのですが、実際に動かしてみると結構な誤差が発生します。
＃これ、決めつけてますが、制御学的にそうなのかどうか知りません。間違えていたら教えてください。実は私の実装ミスかもしれないです。
理想的には止まった時は速度がゼロなので重心点はロボットの真下になるような姿勢になるべきなのですが、ここに誤差が発生します。
3歩歩いただけで、1ミリから2ミリくらいは誤差が残って、次に歩き出すときに不正動作の原因になります。
カーブ歩行すると誤差は拡大し、コントローラを使って止まらずにうろうろ歩かせると20ミリくらい誤差が出たりします。
そのような状態で再度歩き出すと、予見器はフラッシュしてフレッシュなのでフラッとこけそうになります。
単純に線形補正して滑らかに補正してみたのですが、せっかく力学計算して軌道決めてるのにここでリニアに誤差を溶け込ませるとかかっこ悪いなと思ってました。
なにより線形補正だと滑らかにはなりませんでした。
これに対しては昨日アイディア？を思い付き、初期状態で計画ZMPと誤差値を合わせるとどうだろうと。
計算してみるとよさげです。 下のグラフの場合、初期オフセットに5ミリを入れています。
組み込んでみたのがこれです。
美しい。。
この処理をしない場合はこんな感じでした。2度目の歩き出しで、ちょっと乱れるのがわかると思います。
これも現場対応でこのようにしましたが、正しい対処があるのであれば教えてください。
実機に載せるとなると細かな部分でいろいろと作り込む必要が出てきて、だんだんとプログラムも読みにくくなってきました。
この再構築の仕組みとか、あとから読もうと思ったら結構大変だろうなーと今から思います。
実はオフライン歩行の時にはモーションの継ぎ合わせというのをやっていて、接続するフレームにおける質点毎の速度、加速度、ロボット自体の移動速度、方向などを引き継いで動いているところからのモーションを生成してさっと切り替えるという事をやってました。
プログラムを書いた本人が、やっている事はわかっている上で読んでも、すでに解読困難。どうしてこうやってるの？？なにやってるの？？って箇所ばかりで参考になりませんでした。(笑)
ドキュメントや図の類は結構気にして残しているつもりなのに、まだまだ十分な記録にはなってないってことですね。
年末です。単身赴任なので帰省せねばなりません。
母艦も機体も工具もすべてこっちに持ってきてるので帰ると何もできないのでどうしたものかなと。
車だから母艦を持って帰るかなーと思ってます。ディスプレイを置く机さえ無いけどどうやって作業しようか。。
あと、こういう場面が多いのでお出かけ環境も考える必要があります。とりあえず、今の母艦を買う前に使っていたノートＰＣは、Windows10を無償アップデートで入れたのですが、オモオモの上（7の頃も十分に重かった）、エクスプローラがおかしくて操作しているとエクスプローラが落ちます。（ＯＳは落ちないがしばらく操作不能になる）
クリアインストールしたら使えそうだけど、今更Win10を買ってこのPCに入れるってのもバカらしいので新しいノートPC（リーズナブルなやつ）をさっき発注しました。
このノートはLinuxでも入れちゃおうかなー。使わないだろうけどきっとさくさく動くはず (＾◇＾)
では、今年の更新はきっとこれが最後。皆様、良いお年を。来年もよろしくお願いします。<(_ _)>
あけましておめでとうございます。
新年なので改ページしたいところだけど、こないだ新しいページに入ったところなので続けて書くことにします。
年末年始はやはり、実家（川崎の自分のマンション）に帰るべきなんだろうけど、単身赴任のために一切合財を山梨に持ってきているものだから、開発環境はおろか、着る服さえも持って帰らなければいけない。
いくら車での移動とはいえ、荷物が多くなるのは準備も片付けも大変なのだが、やっぱ帰らなきゃダメだよね。ということで帰ることに。帰っちゃうとだらだらと年末年始のくだらないテレビを見続けることになるので（それはそれで良いのだが）ここはひとつデスクトップＰＣを持って帰って実家で開発作業をしようと。
こないだGoSimulation上のプログラムを実機実装を想定した形に書き換えてやっと動いたのだが、どうもその後動きがおかしい気がする。 なんだか実装を変える前と後とでは不安定なケースが多くなったような気がする。いや、不安定になってる。
これはつまりバグが入り込んだわけで、やっと動いたのにバグがあるとはこれはもうめんどくさい事になると決まっているので年末年始のまとめて時間が取れる時にやるしかないという事です。
ディープラーニングほどじゃないけど、多次元変数を使った制御モデルだと細かな数値差だと補正しちゃってあからさまにおかしくならなくてバグが見えにくい。（のではないかと思います。なんせ初めてなので。。）
今回も大体はうまく歩けるのにたまに不安定になったりこけたりする。物理シミュレータ上だと絶対安定歩行するわけじゃなくてゆらぎなのかカオスなのかわからないけど必ず同じ動作をするわけじゃなくて毎回違った動きをするのでなんかの都合で不安定だったりもあり得ます。
なので、不安定なのだけど気のせい？とも感じます。いや、気のせいであって欲しいと思ってるのかも。デバッグめんどうだから。。
これが実機だったらプログラムよりメカを疑ってネジ締めしてるとこです。
バグの存在を明確にするためにはやはりデータ取りが必要です。
歩行継続のために、計画ZMPを途中修正して予見データを再構築しているわけですが、直進を継続する場合もたとえば「3歩歩いて止まる」という計画を修正して途中から「ここからさらに5歩歩いて止まる」という風に変更するわけです。
最初の計画が3歩だと、すぐに停止フェーズの動作が予見データに入るのでややこしいですが、「5歩歩いて止まる」という計画にすれば、予見データは歩いている部分だけの状態というのが少し続きます。ここに「ここからさらに5歩歩いて止まる」とすれば、元々の予見データと再構築した予見データに差がなくなるというタイミングが作れます。
それをやったのがこのグラフ
【まっすぐ歩いただけのグラフ】
【直進を継続した場合のグラフ】
あまり差が無いように見えますが、300ミリ進んだ辺りの左右振幅が少し大きくなっています。
前後動作もほぼ直線のはずが、300ミリのあたりに少しひずみがあります。
シミュレータ画面ではこの振幅差のおかげでロボットは少しよろめきました。 これくらいの差でよろめいちゃうんですね。2足歩行が難しいはずです。
うまく動いていたらこのような差はできないはずなのに差ができてしまっているということはやはりバグがあるようです。
バグが確信できたので本格的にデバッグの開始です。
コードレビューしても集中力が続かないのでやはりデータを取ってバグを類推するという方法で行きます。
まず、フレームに抜けや重複がないかを調べましたが、どうやらそれは大丈夫そう。
次に引き継ぎ時の予見器のデータを比較すると、もう全然違います。最初からこれを見ればバグの存在は確信できたわけですが。
そして、予見器のデータを見たってなんにもわからんので、目標ZMPと、オンライン生成器の出力で動いた時の多質点モデルを使ったZMPの差（つまり、予見データの入力値）が再構築前後で差があることを発見しました。
3本目と4本目のグラフを見るのですが、おかしいところが2点。
まず、グラフの最初の2フレームで差分がある。
次にZMPが変化するポイントで差分がある。ただし、こちらは1フレームだけ。
1フレームずれがあるのかなと思ったけど、最初は2フレームずれがあるし？？？
最初の2フレームずれるってのは速度、加速度データが引き継げてないっぽいし、変化点で1フレームずれるってのは比較する際に1フレームずれてるっぽい。
結局、バグは2点ありました。
まず、すっかり忘れていましたが、計画ZMPと多質点モデルによるZMPを比較する際、同じフレームで比較すると差分が大きくなりすぎるので比較はひとつ前の計画ZMPと比較していた点です。
加速度を扱う場合、3点の位置情報が必要です。
a = (p3 - p2) - (p2 - p1) = p3 - 2 * p2 + p1
計画ZMPのように値が激変する場合、変化の結果が2フレーム現れます。そこで、f1〜f3の3フレームで求められる加速度は中間点f2の加速度として扱うことにしました。
そのため、差分計算をする際、「前のフレーム」を呼び出していたのですが、実機実装を目指した実装では、計画ZMP列を接続せずに計算していたため、計画変更点で「前のフレーム」が存在しない点がありました。
運の悪いことに先頭フレームの処理（つまり前のフレームが存在しない場合）も記載していたため、一見ちゃんと動いてしまっていたわけです。
もう一つのバグは、多質点モデル上のバグでした。
多質点モデルでZMPを計算するため、過去2フレームの位置データを保管しているのですが、リセット指示フラグが立っている場合は過去データをクリアして処理する記述があり、再構築時にそれが働いてしまっていたということです。
そのため、このバグはプログラム動作開始ご最初の再構築時だけ発生します。2度目の再構築ではリセット指示フラグは立っていないため、正常に動きます。
修正した結果、再構築前後で出力が一致しました。
完全に一致していないのでまだ何か隠れているのかも知れないですが、大きなバグは取れました。とりあえずこれで先に進みます。
とうとう動力学フィルターを持ったオンライン歩容生成プログラムを実機に実装する時がきました。
さてポーティング。
今回はPC環境でプログラムを書いている時も、実機に載せる事を考慮して書いていたので比較的簡単に実装できちゃったりして、とか心の奥底では考えていました。
とりあえず、質点モデルとかZMP歩行生成部とかその他の行列計算とか、IK、DK計算とかを実機プログラムに載せます。
で、現在のメインルーチンのままコンパイルしたところ、重大な問題を思い出しました。
「あ、malloc使えないんだった。」
STM32の環境だとメモリ管理が無いのでmalloc()とか、printf()とか、atof()とかが使えないんですね。
これらは関数内で不定長のメモリーを使うのでその場でメモリーを確保して処理をする系の関数です。
その他、ファイルシステムや標準出力、標準入力を使う関数も使えません。
つまりOSのシステムコールに頼っている部分がダメなわけです。OS無いわけですから当然です。
でも、小型の組み込みシステムならOSが無い場合も多いわけで、そのためにNewlibというパッケージ？があってそれをきちんと導入すればシステムコールの代替え関数を用意することができます。
それをやらないと実機実装どころじゃなかった。
元々自律型ロボットを作りたくてロボット道に入ったので、使ってきたコントローラは結構ヘビーなんですね。
ロボットキットはSpeecysからでした。
こいつのコントローラは確かPowerPC系のCPUでBSD-UNIXが載っててUNIXプログラムで動かしていました。
次に使ったのはSEMB1200Aってやつでこいつは実はOSなしだったんですけど、FPU積んでてRAMは512kバイト積んでました。
提供されてたライブラリは、多分Newlibでだと思うけどmalloc()使えてました。 確か開発言語はC++使ってたような覚えが。あ、C++はSpeecysだけか。
ZMP規範のオフライン歩容生成」をやってたのはこいつらでです。まともに歩いたのはSEMBからですね。
歩かなかったのはメカの問題でSpeecysのコントローラはいい奴でした。（まだ生きてますが）
Go Simulation上で動かしていた動力学フィルターのプログラムはこれらのコードを結構な量を流用して作っています。
そういう背景もあって動力学フィルターの実装にはmalloc()を多用しています。
まず、質点モデルではノード毎に接続点が自由数持てるようにしています。
この自由数というのはリストで表現しています。
ついでに手足や胴体もリストになっています。
たとえば物を持った場合、腕の先端に持ったモノを新たな質点としてロボット本体と一緒に重心計算ができます。
物を持った状態で歩かせると持った物の情報（質点や重量をどう決めるかが別途必要だけど）を考慮して歩くことができます。
多分、直立して持ってても動力学フィルターを働かせると重心点を補正して少し腰を引いて歩く、みたいになります。
オンライン生成の目標ZMP列もリストです。オフラインの時は2本のリストを持っていて、上手に切り替えて、裏で新たに生成して、ということをやってましたが、今回は1本のリストを継ぎ足して使っています。正確には裏で作った継ぎ足し部分を正規リストに継ぎ足すということをやってます。使い終わったデータ部分のメモリーはどんどんと解放していきます。
予見部分もリストです。基本的には目標ZMP列と同じ仕組みで、質点モデルから計算した計算ZMPとオンライン生成で計算した単振子モデルの計算ZMPの差の列を格納しています。
目標ZMP列が変更になって、予見データを再構築するときは現状データでロボットを動かしつつ、裏で差し替えデータを構築しますがここでも新たにリストを作り、切り替えのタイミングで差し替えます。
もうあちこちでmalloc()使いまくりです。malloc()がなければ生きていけません。
ということでmalloc()を使えるようにするための勉強です。
フェイスブックでつぶやいたところ、テクノロードの杉浦さんから「Coron+のライブラリはmalloc()対応している」という情報をいただきました。
実はSTM32事始めはCoronでして、シグマも初期の頃はCoronで動かしていました。 今はSTBeeを使っているのですが、Coronの開発環境の一部を今も使っています。
実はこの間にちょっと失敗が。
MentorGraphicsから落としてきたコンパイラ・ライブラリはホントにmalloc()対応していないのかな？と思ってシンボルテーブルを調べてみました。
$ nm libc.a | grep sbrk
U __malloc_sbrk_base
U _sbrk_r
0000002c B __malloc_max_sbrked_mem
00000408 D __malloc_sbrk_base
U _sbrk_r
lib_a-sbrkr.o:
U _sbrk
00000001 T _sbrk_r
lib_a-syssbrk.o:
U _sbrk_r
00000001 T sbrk
あれ？対応してるやん。って思ってしまいました。
Tってのが実体があるシンボルで、Uってのは呼び出し先シンボルで実体がない。
最後にT sbrk ってあるので実体があるとみてしまったのだが、よく見るとアンダーバーがない。これは内部関数_sbrk()とは別のユーザー関数sbrk()なんですね。sbrk()は_sbrk()がなくても動くわけだ。
この勘違いに気づかずにしばらくああでもないこうでもないと頑張ってしまいました。
で、Coron+で動くのを確認してから、って乗りで環境をダウンロードして、ライブラリをコピーして動かしてみたのですが、動かない。あれ？動かないやん。シンボルテーブル調べたらさっきと同じような出力に。
対応しているはずのライブラリなわけだから、やっぱ勉強しなきゃだめだなーということでCoron+のライブラリは横に置いておいて勉強することに。
色々調べた結果、OSなしの組み込み系で、システムコール系の受け渡しをさせるには、
1．syscall.cを組み込む。 どこからかこいつを探してきてリンクする。
2．libnosys.aをリンクする。
3. -specs=nosys.specs をコンパイルオプションに追加してコンパイルする。
辺りが具体的な処理として浮上しました。
結局は上記の処理で、_sbrk(),_read(),_write(),_seek(),_stat()とかの関数が追加させるということです。
3.は、実は2．とほぼ一緒です。specsファイルの中身はライブラリにlibnosys.aを追加する処理になってます。
1．のファイルの中身は上記のシステムコール系関数の実体が記述されています。
これでmakeは通るようになるはずです。
翻ってMentorGraphicsとCoron+のライブラリを見てみると、Coron+にはlibnosys.aもnosys.specも提供されています。
MentorGraphicsには無いのでやはり対応されていないらしい。
sbrk()はヒープと呼ばれる未使用のメモリー空間から必要に応じてメモリーを割り当てていきます。
そのヒープがどこにあるかを教えてやる必要があるらしく、このために外部変数_endを参照するらしいです。
リンカスクリプトでこの設定をやらなければならないらしい。
マイコンを触り始めたころは、Z80のインストラクションセットを全部記憶してて、機械語をソラで読めたという時期もありますが、マイコンが趣味だったころからUNIX、ロボットと移行するなかで、プログラムは記述性が重要。めんどくさい事には近づかないという方針で進めてきたので、環境構築とかもできるだけ人の足跡をたどるようにして苦労を排除してきました。
なので、いままでリンカスクリプトを触るようなところに踏み込まないでやってきましたが、とうとうその中を見る日が来たようです。
マイコンの世界では有名人であろう「ねむいさん」のブログ、「ねむいさんのブログ」は情報の宝庫です。書いていることが理解できないくらい詳しいです。ここにこの問題が少し載っていてそれを足がかりにリンカスクリプトの記述文法などを理解していきます。
定義された変数領域 .bssセクションの後ろに_sbrkが参照する_endが設定されるべき、ということなのでCoronのスクリプトやねむいさん情報などを合わせてこんな風にしました。
_Min_Heap_Size = 0x1000;
.bss :
{
. = ALIGN(4);
_sbss = .;
*(.bss)
*(COMMON)
. = ALIGN(4);
_ebss = . ;
} >RAM
.heap :
{
PROVIDE ( end = _ebss );
PROVIDE ( _end = _ebss );
. = . + _Min_Heap_Size;
. = ALIGN(4);
} >RAM
ALIGN(4)は4バイト単位で端数を飛ばす
*(.bss)ってのはオブジェクト内の定義済み変数(なんちゃら.bss)のことで、ここに配置される
で、.heapセクションの頭に_endを設定し、.bssセクションの終わり_ebssを代入しています。
ヒープのサイズってのが必要なのかどうかわかってないですが、_Min_Heap_Sizeとして0x1000バイトを設定しています。
「end」と「_end」があるのは処理系でどちらもあるのかな？と。
これでmakeすると
stbee-led.elf :
section size addr
.isr_vector 484 134230016
.flashtext 0 134230500
.text 16572 134230500
.data 2136 536870912
.bss 732 536873048
.heap 4096 536873780
._usrstack 256 536877876
.stab 58452 0
.stabstr 88228 0
.comment 110 0
.ARM.attributes 47 0
.debug_aranges 664 0
.debug_info 57594 0
.debug_abbrev 11799 0
.debug_line 10474 0
.debug_frame 2348 0
.debug_str 5179 0
.debug_loc 12924 0
.debug_ranges 1128 0
Total 273223
のようになり、ヒープが確保されているのがわかります。 でもこれでいいのかわかりませんが。
スタック(._usrstack)が0x0100バイト設定されてるけど、この後ろにヒープを持っていかなくていいのかな？
sbrk()で容量増やせるってイメージだけど、スタックエリアを挟んでエリアを確保するのかな？
とか色々疑問だらけです。
さて、コンパイルできたからいいかというと実は全然だめで、動きません。
実はMentorGraphicsで環境を作って動くのを確認していたんだけど、make cleanではSTM32の専用ライブラリlibcm3.aとかペリフェラルライブラリlibstd.aはクリアされません。
MentorGraphicsの環境にしたのは今のデスクトップの母艦を買ってからで、それまではもっと古いバージョンのgccコンパイラを使ってました。
その時に構築したライブラリがそのまま使えていたのでまぁいいやと。
でも、全部再構築した方がいろいろ不整合が出なくていいんじゃないの？と思ってmake distcleanしてライブラリから再構築してみたら、core_cm3.cのコンパイルでアセンブラがエラー出してしまいます。
これはもうデバッグ対象じゃなくてコンパイラの問題だからもういいやと思ってmakedistclean前のライブラリを復活させていました。
こないだLaunchpadから落としてきた環境はM4にもM4のFPUにも対応しているのでこれはいい！と思って動かしてみたのですが、この、従来のライブラリとリンクする方法ではうごきません。（後述、リンクスするライブラリが間違えていた）
ライブラリのバージョンは4.8.1、Launchpadは5.8.3、きっとこのバージョン違いが問題なのだろうと思っていました。
戻って、コンパイルできたものは「従来のlibcm3.a」＋「Coron+ライブラリ」＋MentorGraphicsコンパイラです。
うまぜこぜです。とりあえずコンパイラもCoronにしてみても結果は同じ。
これはもう、Coronで全部そろえるしかない。libcm3.aをCoron版で構築するしかないなということで調べてみる事に。
ちょっと調べたら「ねむいさんのブログ」にMentorGraphicsだと引っかかるという話が出てまして、ずばりこの問題が取り上げられてました。
解決策としては「ハードコーティングして回避」がねむいさんの答だったのですが、その後の識者からのコメントでアセンブラを制御してきれいに解決する策があると。ところがねむいさんはその解決策をリンクで示していてブログ本文には書いてないのです！
リンク先は「へなへなハイカー」とかいう山登りのブログになっててアセンブラーとは縁遠そうなところに飛びます。
仕方がないので今度はインラインアセンブラの勉強です。「早期破壊オペランド」というのをキーワードに探し回り、やっと見つけました。
__ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
↓
__ASM volatile ("strexb %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
__ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
↓
__ASM volatile ("strexh %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
この2か所です。わかってる人には数秒で終わる事を何時間も調べたり試行錯誤するのってホントにいやになりますね。
これで、MentorGraphics でも Coron+ でもmakeは通るようになりました。ｳﾚｼｲ
makeは通りましたが、ちゃんと動くのはMentorGraphics版だけ。Coron+もLunchpadもぴくりとも動きません。
現状のラムダのプログラムもいろんなペリフェラルを触って色々やってるので何が原因かわからないので、stroberry-linuxからSTBee-miniのサンプルプログラムLEDがチカチカするだけのプログラムを走らせてみると、Coron+のコンパイラで動きます。
これってペリフェラルライブラリとCM3ライブラリしかリンクしてないのです。
どうやらGCCのライブラリ群が問題らしい。MentorGraphics版ではthumb2、それ以外ではthumbってのしかないのでthumbのディレクトリーに入っているライブラリをリンクしているのですが、これが問題化？2じゃないからダメなの？
大体thumbって何？
と思って軽く調べたら、どうやら thumbってのは16ビット命令とか32ビット命令とかでコード効率化のための拡張インストラクションセットらしいです。
コンパイルもthumbってオプションつけてるし、これでいいよなーと思いながら、無印のライブラリをリンクしてみたりしても動かない。
あとはarmv7-mとかarmv7e-mとかの上位CPU（と思っていた）のライブラリしかない。
更にネットで情報を探していると、「ねむいさんのブログ」でSTM32などARM系のおすすめ開発環境（除く初心者）ってのがあってそこからのリンクで、Cortex-M3ってのはARMv7-mアーキってことがわかりました！
ARMv7ってARM7のことじゃないか。
ちなみにARMv6-mってのはCortex-M0+、ARMv7e-mってのはCortex-M4らしいです。
ということで、armv7-mのディレクトリーにあるライブラリをリンクしたら動きました。 いや、まじで無知って罪ですね。
動き出したのはいいのだけど、mallocはまともに動きません。
ああでもないこうでもないと試行錯誤することしばらく。
どうにもおかしいことがいくつか。
・外部変数endの後ろにいくつかの変数領域が配置されてしまっている。
・sbrk()で現状のヒープ先頭アドレスを取得するとRAMエリア外になっている。
ねむいさんのブログでは「_end」とあった外部変数ですが、newlib内のソースを確認したところ、参照しているのは「end」でした。
更には参照しているのはendの内容ではなく、endのアドレス。
つまりendは.bssセクションの最後でなければなりません。
これはリンカスクリプトの問題です。
.bssセクションの後ろに._usrstackというセクションが設定されていたのですが、スタックはRAMエリアの一番後ろに1024byte確保されていてそこから使われているようです。
このスタックがどう使われているのか見るのも実は苦労しました。
10個くらいのchar型変数を確保してわかりやすいデータを書き込んでみるということをやってみたのだけど、メモリダンプしても見当たらない。
これは「使われていない変数」ということで最適化で削除されてしまっていました。volatile char型で宣言してやっと見えました。
最適化恐るべし。
ということで、._usrstackというセクションは使われていないと断定して削除。
ヒープ領域というのも明確に宣言する必要はないと思いまして、このようにしました。
.bss :
{
. = ALIGN(4);
_sbss = .;
*(.bss)
*(COMMON)
. = ALIGN(4);
_ebss = . ;
} >RAM
.heap(NOLOAD) :
{
end = . ;
*(.heap)
} >RAM
これでシンボルテーブルはこんな風になりました。
20000b30 B errno
20000b34 B _ebss
20000b34 ? end
2000fc00 A __Stack_Init
20010000 A _estack
f1e0f85f a BootRAM
シンボル区分が「?」になってしまいましたが、.bssの最後になりました。
しかし、これでもやっぱりmallocは動きません。ヒープの先頭座標はおかしいままです。
で、newlibのソースを見て違和感があった部分に着目。提供ソースを疑いたくなかったので見て見ぬふりをしていましたが、仕方ない。
_sbrk()内でヒープのアドレスを管理しているstaticな変数があるのですが、この値がNULLの時にendのアドレスを代入するとなっています。これって初期状態のことだよね。でもこの変数初期化されていないので不定値なはず。
ということで、RAMを全部ゼロで埋め尽くしてからプログラムを動かしてみると、malloc動きました。
ちゃんとendの後ろに確保されている様子が見えました。
sbrk()で取得するアドレスも正常です。 これかぁ〜。
という事でnewlibの最新版2.5.0を落としてきてみましたが、内容が変わっていなかったのでソースをちょこっと修正。
static char * heap_end = NULL;
宣言時にNULLで初期化するだけです。そしてコンパイル。
mkdir build
cd build
../configure -target=arm-none-eabi -prefix=/opt/newlib --enable-multilib --enable-interworks
make; make install
最新版だとarm-v8とかにも対応しているみたいだけど対応が中途半端らしくアセンブラでエラーが出ます。
ソース見てもarm-v8向けの記述があるわけじゃないのでarm-v8のディレクトリを消して、無視しました。
このライブラリに差し替えたところ、メモリーを汚してもちゃんと動き出しました。
めでたしめでたし。
と思いましたが、mallocの一般的な動きと違うことに今気づいた。
ネットで調べると、mallocは未使用領域を使う。その時に使う領域を決めて（heap）そこからメモリを切り出す。
heapが足りなくなるとsbrk()で領域を拡大していく。
となっているが、newlibの_sbrk()とメモリーの使用状況を見ると、heapの先頭は.bssセクションを先頭として、スタックエンドまでの領域。これがheapに相当する。
sbrk()で引数に容量を入れると、現状のheap先頭を引数分だけずらす。つまりheapが小さくなる。
となっている。
free()の動きも未確認。mallocで確保した領域をfreeで解放したらちゃんと未使用領域に還元されるのだろうか。
されないと困るんだけどな。
領域の利用管理はheapの管理とはまた別のようなのでfree()で解放したらheapの先頭が変わるわけではなさそうだが。
ちょっと不安を残しつつ、STM32でmallocプロジェクトは一旦閉幕とします。
次は、とうとう実機実装かというとそんなに甘くありませんでした。
今まで使っていたCortex-M3は中容量（Medium-density）と呼ばれるものでFLASH:128kbyte RAM:20kbyteしかありません。
mallocを組み込んだ途端定義済変数がものすごく増えて、あっという間に20kbyteの容量を食い尽くしてしまいました。FLASHも満杯です。
動かしてみるにはCortex-M3大容量（High-density FLASH:512kbyte RAM:64kbyte）に移行しなければなりません。
もしくはCortex-M4(FLASH:1Mbyte RAM:192kbyte)に移行。
間違いなく時代はM4なのですが、きっとペリフェラルが変わってて簡単に立ち上がらないに決まってるのでM3(High-density)でとにかく動かしてみる事にします。
mallocも動いた事だし、さくっと乗っけてみました。
多分動力学フィルターはメモリー量から載らないだろうからまずは質点計算用のロボットモデルを載せてみます。その他必要になる関数群も載せちゃいます。
結果、STBee-MINIの20KbyteのRAMではロボットモデルさえ載らない。試算ではロボットモデルくらいは楽勝と思っていたのだけど、libnosysをリンクしたことでいろいろ変数が必要になったみたいです。
早速STBeeに切り替えです。STBeeはCoronより基板サイズがでかいので今後これで進めることは絶対あり得ないのですが、早く動かしてみたいというその一点で載せてみます。RAMも64Kbyteあれば大丈夫だろう。
ペリフェラルを移さなきゃならないのでめんどくさいなーと思っていたのですが、基板にあらかじめ実装されているLEDにアサインされているポートがが違うだけであとは全部一緒でした。
さて、第一段階
目標ZMP列の生成は難なくクリア ⇒ mallocはちゃんと動いてるらしい。
第二予見データ作成
ここでハマった。
たぶん、金曜日会社から帰って午前3時くらいまでやって全然わからんから寝る。次の日もごはん食べる以外はずーっとバグ探しで、おそらくは20時間くらいハマった。まぁ打つ手が思い浮かばなくて同じことをぐるぐる繰り返している時間も含めてですけど。
質点モデルでZMPを計算してるところで変数がnanになる。ここはリカーシブで呼び出したりしてめんどくさいとこなのでデータ全部吐き出させたりしておかしいところ、おかしいデータをあぶり出そうとしたんだが、リカーシブだからエラーが出たのがどの段階なのか調べるのも大変で、でもどんどんどんどん遡ってとうとう順運動学関数の中の二円の交点を計算するところで不審な動きを発見。 思いっきりくだらないバグが残ってた。(^｡^)
そのバグのおかげでエラーフラグが立ってるのにフラグを処理していない。その上にエラー起こしてもちゃんと値が返ってくるように見えてた
前回のデータを見ちゃうので少しだけ値が違うはずなんだけど少しだから挙動不審にまでならないんだな。
ちゃんとエラー処理して値はクリアにしておかないとダメですねー。
この辺りが素人プログラマーのダメなとこだな。第三者レビューとかしないし、チェックリストとかあったら引っかかるところだろう。
第三段階
とうとう動力学フィルター部と、補正値加えて最終的なモーションを生成するセクションに取りかかる。
ここから先は予見データ作成とほぼ同じことをやってるだけなので問題なく動くはず。なんだけど、まったく動かない。
つまり、割り込み期間中にモーション生成できないんですね。
今の構成は、20ms周期で割り込みがかかり、センサー取り込みとサーボ指令を行います。
できるだけペリフェラルに突き放し処理するようにDMAを使うようにしていてループを回してのウェイトとかは最小限にしています。
ですので割り込み時間はほとんどないのですが、その残り時間はメインループが持ってます。
重いモーション生成を行ったあと、センサーデータを取得してサーボ列にデータを送るというのは無謀でした。
時間がなくて処理が破たんしてます。
では、時間を持ってるメインループで処理をするようにしましょう。
フラグを使って、モーションデータの作成指示を請けて、メインループで作成、完成したら完了フラグで割り込みスレッドに通知します。
で、動きました！ が、間に合ってない。ブリブリ言いながらゆっくり動いてる。時間を持ってるメインループでも処理が間に合わないらしい。
調べると1フレーム抜けてるので予定動作の倍の遅さで動いてるわけで歩けるわけないですね。
すると、待てよ。計画ZMPの変更対応なんて絶対無理やん！
どれくらい無理なんだろう？ということで予見データ構築にかかる時間を計ってみたところ（割り込み周期単位での荒っぽい測定です）
40コマの予見データを作るのに0.5秒かかります。予見データの作り直しする意味ないやん。。
振り返って考えると、最初にSTM CortexM3CPUでサーボを動かしたのはシグマです。最初はSEMB1200で動かしてたんだけど、シグマだと動力学とか要らないのでCoron⇒STBee-MINIで動かしてました。その時は1コマの時間で5コマ分のIK計算ができて大したもんだなーと思ってたんですが、あれは比較的簡単なIK計算を6足分やってただけなので動力学フィルターとは計算量の桁が違う。
あれで5コマなんだからM3で動力学フィルター動かせるわけないやん。アホですね。
やはり一番時間がかかるのは質点モデルを使ったZMPの算出。計画修正付きの動力学フィルターで動かそうとすると1フレーム時間でこの計算を３回以上できなければならない。通常は１回ですが、修正で２回やるとしたら４０フレームの予見データ作成に２０フレーム必要。
結構敷居高いですね。
計算のほとんどが浮動小数点演算なのでM4にすればずいぶん得をするとは思うのですが、どの程度まで行けるか。。
M3⇒M4はペリフェラル周りが若干変わったと風のうわさに聞いたので手を出しあぐねていたのですが、Standard Peripheral Library使えば差分を吸収してくれるとかならうれしいんだけどなー。
M4を立ち上げつつ、もう一つ課題をこなす必要があります。
これはシミュレーションでは無視していたサーボ特性とか機械的特性。
ほんとはシミュレーション上でサーボ特性を再現してそのフォロー部分も作り上げるというのも面白い（というか、それがシミュレーションの本懐だが）と思ったのですが、正直大変だし、本機体が既にあるので現物使うところかなと。
今回のラムダの足構造は直交分離を目的とした二重非平行４節リンクです。
そのためリンク軸があちこちにあって、それだけで作りが甘くなってます。更には機体重量に対して構成部材の剛性も十分ではないなぁと思ってます。
変態的こだわり機械エンジニアである真広さんに見せたりしたらガラクタ扱いされるモノです。
ですが、これには狙いがありまして、その狙いは何かというと「衝突衝撃」です。
歩かせて問題になるのは膝のダレとか股関節ロールのダレとか色々ありますが、着地時の撥ねがあります。
撥ねちゃうと計算通りに動いてくれないんですよね。
ジャイロセンサーで振動吸収したりとか色々方法はあるんですが、機械的に撥ねを吸収したいというのが直交分離の真意の一つです。
しかし相反することですが、足全体で反発エネルギーを吸収するということは関節角度の再生精度を犠牲にするということになります。
この副作用を抑えるために補償機構が必要なります。
計算通りの関節角度になるように補償を加えることで撥ねを吸収し、計算歩行の精度を高めるための再生精度を確保すると。
前回のラムダではこの補償に関節荷重計算を使用しました。
関節のダレは関節にかかる荷重で決まります。荷重はF=Ma なので質量と加速度（関節だから慣性モーメントと角加速度ですが）で決まります。
各関節は連動していないのでZMP計算より更にめんどくさい計算が必要です。
それに対して直交分離型である今回のラムダは垂直荷重に対するサーボは１つ、水平荷重に対するサーボが１つです。
同じように関節荷重を計算するのも可能ですが、計算量は少し多くなるくらいですのでやっぱり大変。
更には先のガラクタの件で書いたようにあちこちで荷重が抜けたり梁がたわんだりしている可能性が高くて軸荷重計算値に信憑性がなさそう。
ということで垂直はZMPで代替えしてみようと思います。
ZMPと言っても片足で支えているか、両足で支えているが両足への荷重が均等か不均等化か？くらいで中間的な数値を使うのは両足指示で荷重が不均等な場合くらいですかね。
水平は自重と水平加速度ですが、慣性ついてるからあんまり関係ないかなーと思ってます。実験してみて要補償かどうか判断が必要かな。
実験したいのだが、ZMP計算をすると処理オーバーになってしまうので、一旦は計画ZMPを入力として補正値を決定してみます。
計画ZMPから重心点の運動を導くには多元方程式を解かなければならないけど、重心点の運動からZMPを計算するのは難しくありません。
これを使って、重心点の動きがどのようなZMPの動きを生むかをちょっと確かめてみます。
１．リニア
直線で動かすと、定速移動時は加速度はゼロなのでZMPは重心点の真下。動作開始時と動作停止時に大きな加速度が発生するのでZMPが大きく乱れます。
２．サインカーブ
sin()の２階微分は-sin()になるのでこんな風に。ZMP軌道に近そうに見えて加速度で見ると全然違います。
３．計画ZMPからの重心点軌道
加速度を見ると、接地点にZMPを残しつつ加速度を上げていき、ZMP切り替えポイントからは加速度を下げて、当たらな接地点に重心点を近づけていくすばらしい。
もうそろそろページを切り替える頃かなー。