====== 運動学に関するプログラム ======
kinematics.pyプログラムでは以下を扱う．
* シリアルリンクアームの構造定義
* 順運動学
* ヤコビ行列
* 逆運動学の数値解法
* 逆運動学の数値解法の１ステップ ＝ 分解運動制御
基本的な使い方だけでなく，pythonで書かれたプログラムがどの程度の速度か，
またそれはロボットの制御に使えるものかを検討する．
このテストは以下の条件で行った．
* ProBook 474s
* メモリ：8 GB
* CPU：Core™ i5-3230M
* OS: Ubuntu 20.04
* jupyter notebook，python3
ここではkinematics.ipynbでの結果をベースにコメントを付け加えている．
===== モジュールのインポート =====
from kinematics import *
この内部での以下の依存モジュールがインポートされている．
import numpy as np
import numpy.linalg as la
from geo import *
==== 時間計測の関数 ====
ここだけのもの．通常の使用では不要．
import time
def test(n,fn):
i=0
start=time.time()
while i< n :
fn()
i += 1
end = time.time()
rslt=end-start
return rslt
===== シリアルリンクアームの構造定義方法 =====
アームの構造は以下のような辞書型で記述する．
arm_conf={'dof':dof,
'j_conf':[[[xyzabc of joint],joint_type],,],
'wrist':[xyzabc of wrist],
'tool':[xyzabc of tool]]
* dof : 自由度数
* j_conf : ジョイントの構造 [xyzabc_of_joint, joint_type]のリスト
* xyzabc_of_joint : 前のリンクから見た関節座標系をxyzabcの形式で表す
* joint_type : 0 - 5 で駆動軸を表す．0-2はx,y,zの並進，3-5はx,y,z周りの回転．
* wrist : 最終関節から見た手首座標系をxyzabcで表す
* tool : 手首に取り付けられたツール（ハンドなどのエンドエフェクタ）座標系をxyzabcで表す
関節構造の定義は，CG，物理シミュレーション，他のロボット分野（たとえばROS,OpenHRP）など
種々存在していていくつかはデファクト標準になっている．
ここでの定義は，運動学に関するエッセンスを分かりやすく切り出したものになっている．
また汎用性のためジョイント構造を関節位置と駆動軸の2つに分けて定義していが，
そのため計算量が増加してしまっているのはトレードオフである．
==== ６自由度アームの例 ====
たとえば[[articles:kinematics#６自由度アームの順運動学]]で用いたアームは以下のように記述される．
bh=0.1
l1h=0.05
l2h=0.4
l3h=0.3
l4h=0.1
l5h=0.1
l6h=0.05
l_hand=0.1
arm_conf={
'dof':6,
'j_conf':[[[0,0,bh,0,0,0],5],[[0,0,l1h,0,0,0],4],
[[0,0,l2h,0,0,0],4],[[0,0,l3h,0,0,0],5],
[[0,0,l4h,0,0,0],4],[[0,0,l5h,0,0,0],5]],
'wrist':[0,0,l6h,0,0,0],
'tool':[0,0,l_hand,pi,0,0]}
===== 順運動学計算 =====
==== 順運動関数 ====
順運動学計算の関数は，以下のように関節パラメタ（関節角など）とアームの２つを引数として，
手先の座標系（FRAME）を返す．
def calc_fk(joints,arm_conf) :
==== 使用例 ====
j1=[pi/3,pi/4,pi/2,pi/4,0,pi/6]
calc_fk(j1, arm_conf)
f:(m:[[-0.9280226546839175, -0.11736248290409608, -0.35355339059327384], [0.3244692640906438, -0.72091587349737, -0.6123724356957947], [-0.1830127018922195, -0.6830127018922195, 0.7071067811865474]],v:[0.3712310601229375, 0.6429910574805843, -0.026776695296636774])
==== 比較のための関数定 ====
汎用関数を用いずに[[articles:kinematics#６自由度アームの順運動学]]で示したように，
関節での座標変換を簡略化した関数を作成して速度を比較する．
# from geo import *
bh=0.1
l1h=0.05
l2h=0.4
l3h=0.3
l4h=0.1
l5h=0.1
l6h=0.05
l_hand=0.1
Tw=FRAME(xyzabc=[0,0,l6h,0,0,0])
Th=FRAME(xyzabc=[0,0,l_hand,pi,0,0])
def fk_6dof(th):
T1=FRAME(xyzabc=[0,0,bh,0,0,th[0]])
T2=FRAME(xyzabc=[0,0,l1h,0,th[1],0])
T3=FRAME(xyzabc=[0,0,l2h,0,th[2],0])
T4=FRAME(xyzabc=[0,0,l3h,0,0,th[3]])
T5=FRAME(xyzabc=[0,0,l4h,0,th[4],0])
T6=FRAME(xyzabc=[0,0,l5h,0,0,th[5]])
return T1*T2*T3*T4*T5*T6*Tw*Th
fk_6dof(j1)
f:(m:[[-0.9280226546839175, -0.11736248290409608, -0.35355339059327384], [0.3244692640906438, -0.72091587349737, -0.6123724356957947], [-0.1830127018922195, -0.6830127018922195, 0.7071067811865474]],v:[0.3712310601229375, 0.6429910574805843, -0.026776695296636774])
test(10000, lambda : calc_fk(j1, arm_conf))
4.557099342346191
test(10000, lambda : fk_6dof(j1))
2.1974411010742188
簡略化したものと比べて汎用のものはおよそ倍の時間がかかっている．
座標変換の演算回数が倍になっていることが大きく効いている．
またアームの構造をたどりながら計算していることも多少のオーバヘッドになっていることも予想される．
===== ヤコビ行列 =====
==== ヤコビ行列計算関数 ====
ヤコビ行列の計算も関節パラメタとアームの構造を引数として与えることで計算できる．
ヤコビ行列はnumpyの配列として返す．
またヤコビ行列を計算する過程で順運動学計算も行われているのでその結果も併せて返す．
逆運動学の数値解法ではヤコビ行列とともに順運動学も必要なので都合が良い．
def calc_fk_jacob(joints,arm_conf) :
==== 使用例 ====
calc_fk_jacob(j1,arm_conf)
(f:(m:[[-0.9280226546839175, -0.1173624829040961, -0.3535533905932739], [0.3244692640906438, -0.7209158734973699, -0.6123724356957946], [-0.18301270189221944, -0.6830127018922194, 0.7071067811865474]],v:[0.37123106012293755, 0.6429910574805843, -0.026776695296636704]),
array([[-6.42991057e-01, -8.83883476e-02, -2.29809704e-01,
2.77555756e-17, -2.15593109e-01, 0.00000000e+00],
[ 3.71231060e-01, -1.53093109e-01, -3.98042083e-01,
-2.77555756e-17, -1.98648278e-02, 0.00000000e+00],
[ 0.00000000e+00, -7.42462120e-01, -4.59619408e-01,
0.00000000e+00, -1.25000000e-01, -6.93889390e-18],
[ 0.00000000e+00, -8.66025404e-01, -8.66025404e-01,
3.53553391e-01, -3.62372436e-01, 3.53553391e-01],
[ 0.00000000e+00, 5.00000000e-01, 5.00000000e-01,
6.12372436e-01, 7.86566092e-01, 6.12372436e-01],
[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00,
-7.07106781e-01, 5.00000000e-01, -7.07106781e-01]]))
test(10000, lambda : calc_fk_jacob(j1,arm_conf))
8.52026104927063
===== 逆運動学の数値解法 =====
必須引数
* trans: 目標FRAME
* s_th: 初期関節パラメタ
* arm_conf: アームの構造定義
オプション引数
* step: 収束計算に入るまでのステップ刻み
* lim: 許容誤差 = 終了条件
* th_lim: 関節パラメタの最大変更幅（大きく動くのは特異点近傍）
* rlim: 特異値の切り捨てしきい値（特異点近傍で暴れるのを防ぐ）
* rt_rate = 並進／回転の比率．回転への重み付け
def ik(trans, s_th, arm_conf,
step=0.01,lim=0.0001,th_lim=0.05,rlim=0.001,
rt_rate = 1.0) :
==== 使用例 ====
* j1 : 関節角初期著，先の関節角
* j2 : 目標の関節角（ikを解いたときの真値）
* t2 : 目標FRAME（j2のときの手先座標系）
t1 = calc_fk(j1, arm_conf)
j2 = [-pi/4, pi/3, pi/4, -pi/6, pi/4, -pi/3]
t2 = calc_fk(j2, arm_conf)
tmp=ik(t2, j1, arm_conf)
tmp
[-0.7853992380411264,
1.0471823145659105,
0.7854289266268336,
-0.5236083304262702,
0.785385704890891,
-1.047186685374375]
関節角真値との比較
j2
[-0.7853981633974483,
1.0471975511965976,
0.7853981633974483,
-0.5235987755982988,
0.7853981633974483,
-1.0471975511965976]
目標FRAMEとの比較
calc_fk(tmp, arm_conf)
f:(m:[[-0.8736006964397756, 0.4713891354566923, -0.12088881732125217], [-0.43705845826583545, -0.6507483775451598, 0.6208916597769961], [0.21401338097939093, 0.5952468665148388, 0.7745188446164727]],v:[0.5483724110655246, -0.6733742353343456, 0.0528419499007637])
t2
f:(m:[[-0.8736006849457358, 0.4713886023536162, -0.12089097912352764], [-0.4370594868340854, -0.6507483361571917, 0.6208909791235274], [0.21401132734195855, 0.5952473339375414, 0.7745190528383291]],v:[0.5483767998160876, -0.6733767998160873, 0.05284261874940961])
==== 時間計測 ====
test(1000, lambda : ik(t2, j1, arm_conf))
214.67796325683594
約215msec.
たとえば軌道を10分割して経由点ごとにikを求めると約2秒．これが遅いか？速いか？
==== 刻み幅（step）を変えて時間計測 ====
tmp=ik(t2, j1, arm_conf, step=0.5)
tmp
[-0.7854053421388558,
1.0470834009055887,
0.7856294026312589,
-0.5236667018858575,
0.785300634569454,
-1.0471188119222254]
calc_fk(tmp, arm_conf)
f:(m:[[-0.8735997102232438, 0.4713932758501981, -0.12087979889577782], [-0.4370554078831119, -0.6507470712205464, 0.6208951761271848], [0.21402363593302334, 0.5952450157549413, 0.7745174332969585]],v:[0.5483455669661724, -0.6733568503070729, 0.05283732616862849])
test(100, lambda : ik(t2, j1, arm_conf, step=0.5))
5.4649951457977295
test(100, lambda : ik(t2, j1, arm_conf, step=0.2))
5.5400636196136475
test(100, lambda : ik(t2, j1, arm_conf, step=0.1))
5.442981719970703
test(100, lambda : ik(t2, j1, arm_conf, step=0.05))
5.426947355270386
test(100, lambda : ik(t2, j1, arm_conf, step=0.02))
11.032327651977539
刻み幅を小さくすると途中の計算が多くなるのは分かるが
stepが0.05以上でほぼ一定になっているのが何故かわからないので
ループカウンタを入れて調べてみた．
ステップ幅とループ回数
| ステップ幅| 全ループ回数| ステップ刻み回数| 収束演算回数|
| 0.01| 215| 214| 1|
| 0.02 | 109 | 108 | 1 |
| 0.05 | 54 | 51 | 3 |
| 0.1 | 53 | 48 | 5 |
| 0.2 | 53 | 45 | 8 |
| 0.5 | 53 | 37 | 16 |
ステップが小さい場合
* ステップ幅が0.02以下の場合は収束演算は1回で終了している．
* それ故，全体でかかる時間は分割回数に比例，すなわちステップ幅に逆比例することになる．
ステップが大きい場合
* ステップ幅が0.05以上では収束演算回数はステップ幅にほぼ比例しているように見える．
* ステップ刻み回数がステップ幅に逆比例していないのは，目標にまっすぐ向かっていないためと思われる．
* 全体のループ回数がほぼ一定なのは偶然と考えられる．
計算速度とのトレードオフで適切なステップ幅を予想すると，今回のケースでは0.05ではないだろうか．
その理由は，分割回数がステップ幅に逆比例しているギリギリのところ，すなわち目標にまっすぐ安定して向かっていると考えられるからである．
適切なステップ幅はアームの姿勢に依存する．しかし，0.05，今回のアームのサイズを1mのオーダで考えると5cm，というのは妥当なところである．
またrt_ratio=1.0としているので，アームのサイズにかかわらず0.05rad=2.86°というのも十分妥当性がある．
===== 逆運動学の数値解法の１ステップ ＝ 分解運動制御 =====
これはikの1ステップを切り出した関数である．
ikの数値解法の結果の分析から分かるように，step=0.02以下からなら一回の逆行列演算でlim=0.0001の精度で目標座標系になるように解が求められる．
逆に言えば，次の目標値が0.02以内ならヤコビ行列の線型方程式を一回解くだけで0.0001の精度で目標に到達するということである．
これを1mオーダのアームでいえば，次の目標が2cm以内なら0.1mmの精度で簡単に動作させることができるということになる．
この1ステップの動作を連続させる方法を分解運動制御という．
1ステップを単位時間で除して速度として制御することが多く，これを分解運動速度制御（resolved motion rate control, rmrc）と呼ぶ．
def ik_one_step(trans, s_th, arm_conf,
step=0.01,lim=0.0001,th_lim=0.05,rlim=0.001,
rt_rate = 1.0) :
==== 使用例 ====
ik_one_step(t2, j1, arm_conf)
[1.0408382827198988,
0.7759559830674535,
1.582579132467006,
0.7811428070265566,
0.006602255357083237,
0.5193434192274071]
この例では目標を遠方に置いているが，これで目標に回転も含めて0.01近づいたことになる．
任意の空間軌道を十分小さく(lim以下に)分割して現在関節パラメタに対してその都度この関数を呼び出してやれば
その軌道に沿った動作をさせることが可能となる．
==== 時間計測 ====
test(1000, lambda : ik_one_step(t2, j1, arm_conf))
1.088411808013916
一回の計算は約1msecである．その内，ヤコビ行列の生成に0.85msecかかっている．
また1cmで軌道を分割した場合で，10m/secの速度まで対応できることが分かる．これは軌道生成としては十分な性能である．
これをC言語などで実装すればサーボループに組み込んでも十分な性能を得ることができる．
この方式を用い手先座標系で直接サーボを実行することで，手先でのインピーダンス制御やハイブリッド制御が実現されている．