軸配置を考慮したシミュレーション

軸配置を考慮したシミュレーションを行ってみました。足をそろえるときに状態が大きく動く結果になったのですがこれであっているのかな？と思いつつ実装してみたいと思います。軸配置が非直交軸、非同一リンク長であっても計算できることがわかりましたがやや面倒くさいです。