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

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