ロボットがジャンプできるか動力学計算する

勢いで書いたので低クオリティ！！
開発に関係ないところが忙しくて，今度ちゃんとまとめよう
運動方程式は間違っていないとおもうけど不備はありそうなので，参考程度に，，，

ジャンプできるか検証するロボット
f:id:Libra23:20210520203434j:plain


モデル化してパラメータを定義
f:id:Libra23:20210520214810j:plain

エネルギー保存からジャンプに必要な初速を設定
v_0 = \sqrt{2gh}
ラグランジュの運動方程式を求めるためにラグランジュ関数を計算
E_k = \frac{1}{2}m_1v_{g1}^2+\frac{1}{2}m_2v_{g2}^2+\frac{1}{2}I_1\dot{q_1}^2+\frac{1}{2}I_1(\dot{q_1}+\dot{q_2})^2
E_p = -m_1gl_{g1}cos(q_1)-m_2gl_1cos(q_1)-m_2gl_{g2}cos(q_1+q_2)
L = E_k - E_p
ラグランジュの運動方程式
M\begin{pmatrix} \ddot{q_1} \\ \ddot{q_2} \end{pmatrix}+h(q, \dot{q})+g(q)=\begin{pmatrix} \tau_1 \\ \tau_2 \end{pmatrix}

M = \begin{pmatrix} m_1l_{g1}^2+m_2(l_1^2+2l_1l_{g2}cos(q_2)+l_{g2}^2)+I_1+I_2 & m_2(l_1l_{g2}cos(q_2)+l_{g2}^2)+I_2 \\ m_2(l_1l_{g2}cos(q_2)+l_{g2}^2)+I_2 & m_2l_{g2}^2+I_2 \\ \end{pmatrix}
h(q, \dot{q}) = \begin{pmatrix} -m_2l_1l_{g2}(2\dot{q_1}+\dot{q2})sin(q_2)\dot{q_2} \\ m_2l_1l_{g2}\dot{q_1}^2sin(q_2) \end{pmatrix}
g(q) = \begin{pmatrix} m_1gl_{g1}sin(q_1)+m_2g(l_1sin(q_1)+l_{g2}sin(q_1+q_2)) \\ m_2gl_{g2}sin(q_1+q_2) \end{pmatrix}

ボディの質量をm，加速度をa，重力と慣性力からなる外力をF，ヤコビアンをJとすると
外力による関節トルク（エンドエフェクタ先端は自由端であるため外トルクなし）
\begin{pmatrix} \tau_{ex1} \\ \tau_{ex2} \end{pmatrix}=J^TF = J^T(m(a+g))
整理して
M\begin{pmatrix} \ddot{q_1} \\ \ddot{q_2} \end{pmatrix}+h(q, \dot{q})+g(q)=\begin{pmatrix} \tau_1 \\ \tau_2 \end{pmatrix}+J^T(m(a+g))
（※5/23 外力を追加する方を左辺から右辺に訂正）
またボディは等加速度運動するので
z = \frac{1}{2}at^2
初速0，初期位置 z = 0とすると
T = v_0/a
上記よりジャンプに必要な時間が分かる．この瞬間に脚を止める，脚を伸び切った状態にする．
（後者にしたいが，今回は加速度を指定した）

(i)初期姿勢を設定し，エンドエフェクタ先端位置を固定
(ii)ボディ位置更新
(iii)逆運動学で関節角度を計算
(iv)関節角速度と関節角加速度を計算
(v)動力学と外力から関節トルクを計算
(vi)時間を更新し目標速度に到達するまで(ii)から繰り返す

条件
支えるボディ質力m = 0.5 [kg]
ジャンプする高さh = 15 [mm]
加速度a = 2.0 [m/s2]

関節トルクと関節角速度を確認
横軸：時間[s]，縦軸：関節トルク[kg cm]（※5/23 上記訂正に基づいて訂正）
f:id:Libra23:20210522235625p:plain
横軸：時間[s]，縦軸：関節角速度[deg/s]
f:id:Libra23:20210520223621p:plain

使用しているサーボモーターでなんとかなりそう？
KRS-3302 ICS | 近藤科学


ソースはここを参照してください，，，
robot-ws/pupper_kinematic_test.cpp at master · Libra23/robot-ws · GitHub


毎回ラグランジュの運動方程式を求めると面倒なのでニュートンオイラー法で書きたい