Manual
User Manual:
Open the PDF directly: View PDF 
.
Page Count: 81
| Download | |
| Open PDF In Browser | View PDF |
軌道最適化による動作生成
リファレンスマニュアル
平成 31 年 2 月 11 日
室岡雅樹
murooka@jsk.t.u-tokyo.ac.jp
目次
1
2
3
4
5
軌道最適化による動作生成の基礎
1
1.1
タスク関数のノルムを最小にするコンフィギュレーションの探索 . . . . . . . . . . . . . . . . .
1
1.2
コンフィギュレーション二次形式の正則化項の追加 . . . . . . . . . . . . . . . . . . . . . . . .
2
1.3
コンフィギュレーション更新量の正則項の追加 . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
1.4
ソースコードと数式の対応 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
1.5
章の構成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
コンフィギュレーションとタスク関数
4
2.1
瞬時コンフィギュレーションと瞬時タスク関数 . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
2.2
軌道コンフィギュレーションと軌道タスク関数 . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
勾配を用いた制約付き非線形最適化
28
3.1
逐次二次計画法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
3.2
複数解候補を用いた逐次二次計画法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
3.2.1
複数解候補を用いた逐次二次計画法の理論 . . . . . . . . . . . . . . . . . . . . . . . . .
30
3.2.2
複数解候補を用いた逐次二次計画法の実装 . . . . . . . . . . . . . . . . . . . . . . . . .
34
動作生成の拡張
35
4.1
マニピュレーションの動作生成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
4.2
B スプラインを用いた関節軌道生成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
4.2.1
B スプラインを用いた関節軌道生成の理論 . . . . . . . . . . . . . . . . . . . . . . . . .
46
4.2.2
B スプラインを用いた関節軌道生成の実装 . . . . . . . . . . . . . . . . . . . . . . . . .
55
補足
67
5.1
既存のロボット基礎クラスの拡張 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
67
5.2
環境と接触するロボットの関節・リンク構造 . . . . . . . . . . . . . . . . . . . . . . . . . . . .
70
5.3
irteus の inverse-kinematics 互換関数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
i
ii
5.4
関節トルク勾配の計算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
76
1
軌道最適化による動作生成の基礎
1
1.1
タスク関数のノルムを最小にするコンフィギュレーションの探索
q ∈ Rnq を設計対象のコンフィギュレーションとする．例えば一般の逆運動学計算では，q はある瞬間のロ
ボットの関節角度を表すベクトルで，コンフィギュレーションの次元 nq はロボットの関節自由度数となる．
動作生成問題を，所望のタスクに対応するタスク関数 e(q) : Rnq → Rne について，次式を満たす q を得る
こととして定義する．
e(q) = 0
(1.1)
例えば一般の逆運動学計算では，e(q) はエンドエフェクタの目標位置姿勢と現在位置姿勢の差を表す 6 次元
ベクトルである．非線形方程式 (1.1) の解を解析的に得ることは難しく，反復計算による数値解法が採られる．
式 (1.1) が解をもたないときでも最善のコンフィギュレーションを得られるように一般化すると，式 (1.1) の
求解は次の最適化問題として表される 1 ．
min F (q)
q
(1.2a)
def
where F (q) =
1
∥e(q)∥2
2
(1.2b)
コンフィギュレーションが最小値 q min と最大値 q max の間に含まれる必要があるとき，逆運動学計算は次の制
約付き非線形最適化問題として表される．
min F (q)
q
s.t. q min ≤ q ≤ q max
(1.3)
例えば一般の逆運動学計算では，q min , q max は関節角度の許容範囲の最小値，最大値を表す．以降では，式 (1.3) の
制約を，より一般の形式である線形等式制約，線形不等式制約として次式のように表す 2 ．
min F (q)
q
(1.4a)
s.t. Aq = b̄
(1.4b)
Cq ≥ d¯
(1.4c)
制約付き非線形最適化問題の解法のひとつである逐次二次計画法では，次の二次計画問題の最適解として得
られる ∆q ∗k を用いて，q k+1 = q k + ∆q ∗k として反復更新することで，式 (1.4) の最適解を導出する 3 ．
1
min F (q k ) + ∇F (q k )T ∆q k + ∆q Tk ∇2 F (q k )∆q k
2
(1.5a)
s.t. A∆q k = b̄ − Aq k
(1.5b)
C∆q k ≥ d¯ − Cq k
(1.5c)
∆q k
1 任意の半正定値行列 W に対して，∥e(q)∥2
= e(q)T W e(q) = e(q)T S T Se(q) = ∥Se(q)∥2 を満たす S が必ず存在するので，
W
式 (1.2b) は任意の重み付きノルムを表現可能である．
2 式 (1.3) における関節角度の最小値，最大値に関する制約は次式のように表される．
⇔
q min ≤ q ≤ q max
)
( )
(
I
q min
q≥
−q max
−I
3 式 (1.5a) は F (q) を q の周りでテーラー展開し三次以下の項を省略したものに一致する．逐次二次計画法については，以下の書籍
k
の 18 章で詳しく説明されている．
Numerical optimization, S. Wright and J. Nocedal, Springer Science, vol. 35, 1999, http://www.xn--vjq503akpco3w.top/
literature/Nocedal_Wright_Numerical_optimization_v2.pdf.
2
∇F (q k ), ∇2 F (q k ) はそれぞれ，F (q k ) の勾配，ヘッセ行列 4 で，次式で表される．
(
)T
∂e(q)
∇F (q) =
e(q)
∂q
=
∇2 F (q) =
≈
=
(1.6a)
J (q)T e(q)
(
)T
m
∑
∂e(q)
∂e(q)
ei (q)∇2 ei (q) +
∂q
∂q
i=1
(
)T
∂e(q)
∂e(q)
∂q
∂q
(1.6b)
J (q)T J (q)
(1.6e)
(1.6c)
(1.6d)
ただし，ei (q) (i = 1, 2, · · · , m) は e(q) の i 番目の要素である．式 (1.6c) から式 (1.6d) への変形では e(q) の
def ∂ e(q )
ne ×nq
は e(q) のヤコビ行列である．
∂q ∈ R
式 (1.6a) , 式 (1.6d) から式 (1.5a) の目的関数は次式で表される 5 ．
二階微分がゼロであると近似している．J (q) =
=
def
1
1 T
e ek + eTk J k ∆q k + ∆q Tk J Tk J k ∆q k
2 k
2
1
∥ek + J k ∆q k ∥2
2
(1.7a)
(1.7b)
def
ただし，ek = e(q k ), J k = J (q k ) とした．
結局，逐次二次計画法で反復的に解かれる二次計画問題 (1.5) は次式で表される．
1
∆q Tk J Tk J k ∆q k + eTk J k ∆q k
2
s.t. A∆q k = b
min
∆q k
C∆q k ≥ d
(1.8a)
(1.8b)
(1.8c)
ここで，
b
= b̄ − Aq k
(1.9)
d
= d¯ − Cq k
(1.10)
とおいた．
1.2
コンフィギュレーション二次形式の正則化項の追加
式 (1.2a) の最適化問題の目的関数を，次式の F̂ (q) で置き換える．
F̂ (q) = F (q) + Freg (q)
1
where Freg (q) = q T W̄ reg q
2
(1.11)
(1.12)
目的関数 F̂ (q) の勾配，ヘッセ行列は次式で表される．
∇F̂ (q) =
∇F (q) + ∇Freg (q)
T
= J (q) e(q) + W̄ reg q
∇ F̂ (q) =
2
∇ F (q) + ∇ Freg (q)
2
2
≈ J (q)T J (q) + W̄ reg
4式
∇2 F (q
(1.13a)
(1.13b)
(1.13c)
(1.13d)
(1.5a) の
k ) の部分は一般にはラグランジュ関数の q k に関するヘッセ行列であるが，等式・不等式制約が線形の場合は
F (q k ) のヘッセ行列と等価になる．
5 式 (1.7b) は，以下の論文で紹介されている二次計画法によってコンフィギュレーション速度を導出する逆運動学解法における目的関
数と一致する．
Feasible pattern generation method for humanoid robots, F. Kanehiro et al., Proceedings of the 2009 IEEE-RAS International
Conference on Humanoid Robots, pp. 542-548, 2009.
3
したがって，式 (1.8) の二次計画問題は次式で表される．
(
)
(
)T
1
min ∆q Tk J Tk J k + W̄ reg ∆q k + J Tk ek + W̄ reg q k ∆q k
∆q k 2
1.3
(1.14a)
s.t. A∆q k = b
(1.14b)
C∆q k ≥ d
(1.14c)
コンフィギュレーション更新量の正則項の追加
Gauss-Newton 法と Levenberg-Marquardt 法の比較を参考に，式 (1.14a) の二次形式項の行列に，次式のよ
うに微小な係数をかけた単位行列を加えると，一部の適用例について逐次二次計画法の収束性が改善された 6 ．
(
)
(
)T
1
min ∆q Tk J Tk J k + W̄ reg + λI ∆q k + J Tk ek + W̄ reg q k ∆q k
(1.15a)
∆q k 2
s.t. A∆q k = b
(1.15b)
C∆q k ≥ d
(1.15c)
改良誤差減衰最小二乗法 7 を参考にすると，λ は次式のように決定される．
λ = λr F (q k ) + wr
(1.16)
λr と wr は正の定数である．
1.4
ソースコードと数式の対応
W reg
v reg
def
=
def
=
W̄ reg + λI
(1.17a)
W̄ reg q k
(1.17b)
とすると，式 (1.15) は次式で表される．
(
)
(
)T
1
min ∆q Tk J Tk J k + W ∆q k + J Tk ek + v reg
∆q k
∆q k 2
(1.18a)
s.t. A∆q k = b
(1.18b)
C∆q k ≥ d
(1.18c)
第 2 節や第 4 章で説明する***-configuration-task クラスのメソッドは式 (1.18) 中の記号と以下のように対
応している．
:config-vector
get q
:set-config
set q
:task-value
get e(q)
:task-jacobian
get J (q) =
:config-equality-constraint-matrix
get A
:config-equality-constraint-vector
get b
:config-inequality-constraint-matrix
get C
:config-inequality-constraint-vector
get d
:regular-matrix
get W reg
:regular-vector
get v reg
6 これは，最適化における信頼領域
7
def ∂ e(q )
∂q
(trust region) に関連している．
Levenberg-Marquardt 法による可解性を問わない逆運動学, 杉原 知道, 日本ロボット学会誌, vol. 29, no. 3, pp. 269-277, 2011.
4
章の構成
1.5
第 2 章では，コンフィギュレーション q の取得・更新，タスク関数 e(q) の取得，タスク関数のヤコビ行列
def ∂ e(q )
J (q) = ∂ q の取得，コンフィギュレーションの等式・不等式制約 A, b, C, d の取得のためのクラスを説明
する．第 2.1 節ではコンフィギュレーション q が瞬時の情報，第 2.2 節ではコンフィギュレーション q が時系
列の情報を表す場合をそれぞれ説明する．
第 3 章では，第 2 章で説明されるクラスを用いて逐次二次計画法により最適化を行うためのクラスを説明
する．
第 4 章では，用途に応じて拡張されたコンフィギュレーションとタスク関数のクラスを説明する．第 4.1 節
では，マニピュレーションのために，ロボットに加えて物体のコンフィギュレーションを計画する場合を説明
する．第 4.2 節では，ロボットの関節位置の軌道を B スプライン関数でパラメトリックに表現する場合を説明
する．いずれにおいても，最適化では第 3 章で説明された逐次二次計画法のクラスが利用される．
第 5 章では，その他の補足事項を説明する．第 5.1 節では，jskeus で定義されているクラスの拡張について
説明する．第 5.2 節では，環境との接触を有するロボットの問題設定を記述するためのクラスについて説明す
る．第 5.4 節では，関節トルクを関節角度で微分したヤコビ行列を導出するための関数について説明する．
2
2.1
コンフィギュレーションとタスク関数
瞬時コンフィギュレーションと瞬時タスク関数
instant-configuration-task
:super
:slots
propertied-object
( robot-env robot-environment instance)
( theta-vector θ [rad] [m])
( wrench-vector ŵ [N] [Nm])
( torque-vector τ [Nm])
( phi-vector ϕ [rad] [m])
( num-kin Nkin := |T kin-trg | = |T kin-att |)
( num-contact Ncnt := |T cnt-trg | = |T cnt-att |)
( num-variant-joint Nvar -joint := |Jvar |)
( num-invariant-joint Ninvar -joint := |Jinvar |)
( num-drive-joint Ndrive-joint := |Jdrive |)
( num-posture-joint Nposture-joint := |Jposture |)
( num-external Nex := number of external wrenches)
( num-collision Ncol := number of collision check pairs)
( dim-theta dim(θ) = Nvar -joint )
( dim-wrench dim(ŵ) = 6Ncnt )
( dim-torque dim(τ ) = Ndrive-joint )
( dim-phi dim(ϕ) = Ninvar -joint )
( dim-variant-config dim(q var ))
( dim-invariant-config dim(q invar ))
( dim-config dim(q))
( dim-task dim(e))
( kin-scale-mat-list Kkin )
( target-posture-scale kposture )
[class]
5
( norm-regular-scale-max kmax )
( norm-regular-scale-offset koff )
( torque-regular-scale ktrq )
( variant-joint-list Jvar )
( invariant-joint-list Jinvar )
( drive-joint-list Jdrive )
( kin-target-coords-list T kin-trg )
( kin-attention-coords-list T kin-att )
( contact-target-coords-list T cnt-trg )
( contact-attention-coords-list T cnt-att )
( variant-joint-angle-margin margin of θ [deg] [mm])
( invariant-joint-angle-margin margin of ϕ [deg] [mm])
( delta-linear-joint trust region of linear joint configuration [mm])
( delta-rotational-joint trust region of rotational joint configuration [deg])
( contact-constraint-list list of contact-constraint instance)
( posture-joint-list Jposture )
trg
( posture-joint-angle-list θ̄ )
ex
ex
( external-wrench-list {wex
1 , w 2 , · · · , w Nex })
( external-coords-list {T1ex , T2ex , · · · , TNexex })
( collision-pair-list list of bodyset-link or body pair)
( collision-distance-margin-list list of collision distance margin)
( only-kinematics? whether to consider only kinematics or not)
∂e
( variant-task-jacobi buffer for ∂ q
)
var
∂e
( invariant-task-jacobi buffer for ∂ q
)
invar
∂e
( task-jacobi buffer for ∂ q )
( collision-theta-inequality-constraint-matrix buffer for C col,θ )
( collision-phi-inequality-constraint-matrix buffer for C col,ϕ )
( collision-inequality-constraint-vector buffer for C col )
瞬時コンフィギュレーション q (l) と瞬時タスク関数 e(l) (q (l) ) のクラス．
このクラスの説明で用いる全ての変数は，時間ステップ l を表す添字をつけて x(l) と表されるべきだが，
このクラス内の説明では省略して x と表す．また，以降では，説明文やメソッド名で，“瞬時” や “instant”
を省略する．
コンフィギュレーション q の取得・更新，タスク関数 e(q) の取得，タスク関数のヤコビ行列
∂ e(q )
∂q
の取
得，コンフィギュレーションの等式・不等式制約 A, b, C, d の取得のためのメソッドが定義されている．
コンフィギュレーション・タスク関数を定めるために，初期化時に以下を与える
• ロボット・環境
robot-environment ロボット・環境を表す robot-environment クラスのインスタンス
variant-joint-list Jvar 時変関節
invariant-joint-list Jinvar 時不変関節 (与えなければ時不変関節は考慮されない)
drive-joint-list Jdrive 駆動関節 (与えなければ関節駆動トルクは考慮されない)
• 幾何拘束
kin-target-coords-list T kin-trg 幾何到達目標位置姿勢リスト
6
kin-attention-coords-list T kin-att 幾何到達着目位置姿勢リスト
kin-scale-mat-list Kkin 幾何拘束の座標系，重みを表す変換行列のリスト
• 接触拘束
contact-target-coords-list T cnt-trg 接触目標位置姿勢リスト
contact-attention-coords-list T cnt-att 接触着目位置姿勢リスト
contact-constraint-list 接触レンチ制約リスト
• コンフィギュレーション拘束 (必要な場合のみ)
posture-joint-list Jposture 着目関節リスト
posture-joint-angle-list θ̄
trg
着目関節の目標関節角
target-posture-scale kposture コンフィギュレーション拘束の重み
• 干渉回避拘束 (必要な場合のみ)
collision-pair-list 干渉回避する bodyset-link もしくは body のペアのリスト
collision-distance-margin 干渉回避の距離マージン (全てのペアで同じ値の場合)
collision-distance-margin-list 干渉回避の距離マージンのリスト (ペアごとに異なる値の場合)
• 外レンチ (必要な場合のみ)
external-wrench-list 外レンチのリスト (ワールド座標系で表す)
external-coords-list 外レンチの作用点座標のリスト (位置のみを使用)
• 目的関数の重み
norm-regular-scale-max kmax コンフィギュレーション更新量正則化の重み最大値
norm-regular-scale-offset koff コンフィギュレーション更新量正則化の重みオフセット
torque-regular-scale ktrq トルク正則化の重み
コンフィギュレーション q は以下から構成される．
(
q := θ T
ŵT
τT
ϕT
)T
(2.1)
θ ∈ RNvar -joint 時変関節角度 [rad] [m]
ŵ ∈ R6Ncnt 接触レンチ [N] [Nm]
τ ∈ RNdrive-joint 関節駆動トルク [Nm] [N]
ϕ ∈ RNinvar -joint 時不変関節角度 [rad] [m]
ŵ は次式のように，全接触部位でのワールド座標系での力・モーメントを並べたベクトルである．
(
)T
ŵ =
(2.2)
wT1 wT2 · · · wTNcnt
)T
(
=
(2.3)
f T1 nT1 f T2 nT2 · · · f TNcnt nTNcnt
タスク関数 e(q) は以下から構成される．
(
e(q) := ekinT (q) eeom-transT (q)
)T
eeom-rotT (q) etrqT (q) epostureT (q)
ekin (q) ∈ R6Nkin 幾何到達拘束 [rad] [m]
eeom-trans (q) ∈ R3 力の釣り合い [N]
eeom-rot (q) ∈ R3 モーメントの釣り合い [Nm]
etrq (q) ∈ RNdrive-joint 関節トルクの釣り合い [rad] [m]
(2.4)
7
eposture (q) ∈ RNposture-joint 関節角目標 [rad] [m]
:init &key (name)
(robot-env)
(variant-joint-list (send robot-env :variant-joint-list))
[method]
(invariant-joint-list (send robot-env :invariant-joint-list))
(drive-joint-list (send robot-env :drive-joint-list))
(only-kinematics?)
(kin-target-coords-list)
(kin-attention-coords-list)
(contact-target-coords-list)
(contact-attention-coords-list)
(variant-joint-angle-margin 3.0)
(invariant-joint-angle-margin 3.0)
(delta-linear-joint)
(delta-rotational-joint)
(contact-constraint-list (send-all contact-attention-coords-list :get :contact-constraint))
(posture-joint-list)
(posture-joint-angle-list)
(external-wrench-list)
(external-coords-list)
(collision-pair-list)
(collision-distance-margin 0.01)
(collision-distance-margin-list)
(kin-scale 1.0)
(kin-scale-list)
(kin-scale-mat-list)
(target-posture-scale 0.001)
(norm-regular-scale-max (if only-kinematics? 0.001 1.000000e-05))
(norm-regular-scale-offset 1.000000e-07)
(torque-regular-scale 1.000000e-04)
&allow-other-keys
Initialize instance
:robot-env
return robot-environment instance
[method]
:variant-joint-list
return Jvar
[method]
:invariant-joint-list
return Jinvar
[method]
:drive-joint-list
return Jdrive
[method]
:only-kinematics?
[method]
return whether to consider only kinematics or not
8
:theta
[method]
return θ
:wrench
[method]
return ŵ
:torque
return τ
[method]
:phi
[method]
return ϕ
:num-kin
return Nkin := |T kin-trg | = |T kin-att |
[method]
:num-contact
return Ncnt := |T cnt-trg | = |T cnt-att |
[method]
:num-variant-joint
[method]
return Nvar -joint := |Jvar |
:num-invariant-joint
[method]
return Ninvar -joint := |Jinvar |
:num-drive-joint
return Ndrive-joint := |Jdrive |
[method]
:num-posture-joint
return Ntarget-joint := |Jtarget |
[method]
:num-external
return Nex := number of external wrench
[method]
:num-collision
[method]
return Ncol := number of collision check pairs
:dim-variant-config
[method]
dim(qvar ) :=
=
dim(θ) + dim(ŵ) + dim(τ )
(2.5)
Nvar -joint + 6Ncnt + Ndrive-joint
(2.6)
return dim(qvar )
:dim-invariant-config
[method]
return dim(qinvar ) := dim(ϕ) = Ninvar -joint
:dim-config
[method]
return dim(q) := dim(qvar ) + dim(qinvar )
:dim-task
[method]
dim(e)
:=
dim(ekin ) + dim(eeom-trans ) + dim(eeom-rot ) + dim(etrq ) + dim(eposture )
(2.7)
=
6Nkin + 3 + 3 + Ndrive-joint + Nposture-joint
(2.8)
9
return dim(e)
:variant-config-vector
θ
return qvar := ŵ
τ
[method]
:invariant-config-vector
return qinvar := ϕ
[method]
:config-vector
[method]
:set-theta theta-new &key (relative? nil)
(apply-to-robot? t)
[method]
(
) θ
ŵ
qvar
return q :=
=
τ
qinvar
ϕ
Set θ.
:set-wrench wrench-new &key (relative? nil)
Set ŵ.
[method]
:set-torque torque-new &key (relative? nil)
[method]
Set τ .
:set-phi phi-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set ϕ.
:set-variant-config variant-config-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set qvar .
:set-invariant-config invariant-config-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set qinvar .
:set-config config-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set q.
:kin-target-coords-list
[method]
kin-trg
Tm
= {pkin-trg
, Rkin-trg
} (m = 1, 2, · · · , Nkin )
m
m
(2.9)
return T kin-trg := {T1kin-trg , T2kin-trg , · · · , TNkin-trg
}
kin
:kin-attention-coords-list
[method]
kin-att
kin-att
Tm
= {pkin-att
, Rm
} (m = 1, 2, · · · , Nkin )
m
(2.10)
10
return T kin-att := {T1kin-att , T2kin-att , · · · , TNkin-att
}
kin
:contact-target-coords-list
[method]
cnt-trg
Tm
= {pcnt-trg
, Rcnt-trg
} (m = 1, 2, · · · , Ncnt )
m
m
(2.11)
}
return T cnt-trg := {T1cnt-trg , T2cnt-trg , · · · , TNcnt-trg
cnt
:contact-attention-coords-list
[method]
cnt-att
Tm
= {pcnt-att
, Rcnt-att
} (m = 1, 2, · · · , Ncnt )
m
m
(2.12)
return T cnt-att := {T1cnt-att , T2cnt-att , · · · , TNcnt-att
}
cnt
:contact-constraint-list
return list of contact-constraint instance
[method]
:wrench-list
return {w1 , w2 , · · · , wNcnt }
[method]
:force-list
return {f 1 , f 2 , · · · , f Ncnt }
[method]
:moment-list
return {n1 , n2 , · · · , nNcnt }
[method]
:external-wrench-list
[method]
return
ex
{wex
1 , w2 , · · ·
, wex
Nex }
:external-force-list
return
[method]
ex
{f ex
1 , f2 , · · ·
, f ex
Nex }
:external-moment-list
ex
ex
return {nex
1 , n2 , · · · , nNex }
[method]
:mg-vec
return mg
[method]
:cog &key (update? t)
return pG (q)
[method]
:kinematics-task-value &key (update? t)
[method]
ekin (q) =
ekin (θ, ϕ)
kin
e1 (θ, ϕ)
kin
e2 (θ, ϕ)
=
..
.
ekin
m (θ, ϕ) =
ekin
Nkin (θ, ϕ)
)
(
pkin-trg
(θ, ϕ) − pkin-att
(θ, ϕ) )
m
m
(
∈ R6 (m = 1, 2, · · · , Nkin )
Kkin
a Rkin-trg
(θ, ϕ)Rkin-att
(θ, ϕ)T
m
m
(2.13)
(2.14)
(2.15)
11
a(R) は姿勢行列 R の等価角軸ベクトルを表す．
return ekin (q) ∈ R6Nkin
:eom-trans-task-value &key (update? t)
[method]
eeom-trans (q)
eeom-trans (ŵ)
=
N
cnt
∑
=
f m − mg +
m=1
(2.16)
Nex
∑
f ex
m
(2.17)
m=1
return eeom-trans (q) ∈ R3
:eom-rot-task-value &key (update? t)
eeom-rot (q) =
=
[method]
eeom-rot (θ, ŵ, ϕ)
N
cnt
∑
{(
(2.18)
)
}
pcnt-trg
(θ, ϕ) − pG (θ, ϕ) × f m + nm
m
m=1
+
Nex
∑
ex
ex
{(pex
m (θ, ϕ) − pG (θ, ϕ)) × f m + nm }
(2.19)
m=1
return eeom-rot (q) ∈ R3
:torque-task-value &key (update? t)
etrq (q)
=
[method]
etrq (θ, ŵ, τ , ϕ)
= τ+
N
cnt
∑
(2.20)
grav
(θ, ϕ) +
τ cnt
m (θ, ϕ) − τ
m=1
= τ+
N
cnt
∑
Nex
∑
τ ex
m (θ, ϕ)
T
grav
(θ, ϕ) +
J cnt-att
drive-joint,m (θ, ϕ) w m − τ
m=1
τ cnt
m (θ, ϕ)
(2.21)
m=1
Nex
∑
T ex
J ex
drive-joint,m (θ, ϕ) w m (2.22)
m=1
は m 番目の接触部位にかかる接触レンチ wm による関節トルク，τ grav
m (θ, ϕ) は自重による関
節トルクを表す．
return etrq (q) ∈ RNdrive-joint
:posture-task-value &key (update? t)
[method]
eposture (q) =
eposture (θ)
(
)
trg
= kposture θ̄ − θ̄
θ̄
trg
(2.23)
(2.24)
, θ̄ は着目関節リスト Jposture の目標関節角と現在の関節角．
return eposture (q) ∈ RNposture-joint
:task-value &key (update? t)
ekin (q)
ekin (θ, ϕ)
eom-trans eom-trans
e
(q) e
(ŵ)
eom-rot
eom-rot (θ, ŵ, ϕ)
return e(q) :=
(q)
e
= e
etrq (q) etrq (θ, ŵ, τ , ϕ)
eposture (q)
eposture (θ)
[method]
12
:kinematics-task-jacobian-with-theta
=
∂ekin
∂θ
∂ ekin
1
∂θ
∂ ekin
2
∂θ
..
.
∂ ekin
N
[method]
(2.25)
kin
∂θ
}
{
kin-att
= Kkin J kin-trg
(θ,
ϕ)
−
J
(θ,
ϕ)
(m = 1, 2, · · · , Nkin )
θ,m
θ,m
∂ekin
m
∂θ
return
(2.26)
∂ekin
∈ R6Nkin ×Nvar -joint
∂θ
:kinematics-task-jacobian-with-phi
[method]
∂ ekin
1
∂ϕ
∂ ekin
2
∂ϕ
= .
..
∂ ekin
kin
∂e
∂ϕ
Nkin
(2.27)
∂ϕ
{
}
= Kkin J kin-trg
(θ, ϕ) − J kin-att
(θ, ϕ)
(m = 1, 2, · · · , Nkin )
ϕ,m
ϕ,m
∂ekin
m
∂ϕ
return
(2.28)
∂ekin
∈ R6Nkin ×Ninvar -joint
∂ϕ
:eom-trans-task-jacobian-with-wrench
∂eeom-trans
∂ ŵ
(
=
(
=
[method]
∂ eeom-trans
∂f 1
I3
O3
∂ eeom-trans
∂ n1
···
I3
···
)
∂ eeom-trans
∂f N
cnt
∂ eeom-trans
∂ nNcnt
)
(2.29)
(2.30)
O3
eom-trans
return ∂ e
∈ R3×6Ncnt
∂ ŵ
:eom-rot-task-jacobian-with-theta
∂eeom-rot
∂θ
=
[method]
N
cnt
∑
{
(
)}
−[f m ×] J cnt-trg
(θ, ϕ) − J Gθ (θ, ϕ)
θ,m
m=1
+
Nex
∑
( ex
)}
{
−[f ex
m ×] J θ,m (θ, ϕ) − J Gθ (θ, ϕ)
m=1
=
[( N
cnt
∑
fm +
m=1
−
N
cnt
∑
Nex
∑
m=1
fm +
∑Nex
m=1
∂eeom-rot
∂θ
) ]
f ex
m
× J Gθ (θ, ϕ)
m=1
[f m ×]J cnt-trg
(θ, ϕ)
θ,m
m=1
∑Ncnt
(2.31)
−
Nex
∑
ex
[f ex
m ×]J θ,m (θ, ϕ)
(2.32)
m=1
f ex
m = mg つまり，eom-trans-task が成立すると仮定すると次式が成り立つ．
=
[mg×] J Gθ (θ, ϕ) −
N
cnt
∑
m=1
[f m ×]J cnt-trg
(θ, ϕ) −
θ,m
Nex
∑
m=1
ex
[f ex
m ×]J θ,m (θ, ϕ)
(2.33)
13
eom-rot
return ∂ e
∈ R3×Nvar -joint
∂θ
:eom-rot-task-jacobian-with-wrench
∂eeom-rot
∂ ŵ
∂eeom-rot
∂f m
∂eeom-rot
∂nm
(
=
[method]
∂ eeom-rot
∂f 1
∂ eeom-rot
∂ n1
···
∂ eeom-rot
∂f N
cnt
∂ eeom-rot
∂ nNcnt
)
[( cnt-trg
) ]
pm
(θ, ϕ) − pG (θ, ϕ) × (m = 1, 2, · · · , Ncnt )
=
= I 3 (m = 1, 2, · · · , Ncnt )
(2.34)
(2.35)
(2.36)
eom-rot
return ∂ e
∈ R3×6Ncnt
∂ ŵ
:eom-rot-task-jacobian-with-phi
∂eeom-rot
∂ϕ
[method]
{
(
)}
−[f m ×] J cnt-trg
(θ,
ϕ)
−
J
(θ,
ϕ)
Gϕ
ϕ,m
N
cnt
∑
=
m=1
Nex
∑
{
( ex
)}
+
−[f ex
m ×] J ϕ,m (θ, ϕ) − J Gϕ (θ, ϕ)
m=1
[( N
cnt
∑
=
fm +
m=1
−
N
cnt
∑
) ]
Nex
∑
f ex
m
m=1
fm +
∑Nex
m=1
∂eeom-rot
∂ϕ
× J Gϕ (θ, ϕ)
m=1
cnt-trg
[f m ×]J ϕ,m
(θ, ϕ) −
m=1
∑Ncnt
(2.37)
Nex
∑
ex
[f ex
m ×]J ϕ,m (θ, ϕ)
(2.38)
m=1
f ex
m = mg つまり，eom-trans-task が成立すると仮定すると次式が成り立つ．
= [mg×] J Gϕ (θ, ϕ) −
N
cnt
∑
[f m ×]J cnt-trg
(θ, ϕ) −
ϕ,m
m=1
Nex
∑
ex
[f ex
m ×]J ϕ,m (θ, ϕ)
(2.39)
m=1
eom-rot
return ∂ e
∈ R3×Ninvar -joint
∂ϕ
:torque-task-jacobian-with-theta
[method]
∂etrq
∂θ
=
N
cnt
∑
Nex
∑
∂τ grav
∂τ cnt
∂τ ex
m
m
−
+
∂θ
∂θ
∂θ
m=1
m=1
(2.40)
trq
return ∂ e ∈ RNdrive-joint ×Nvar -joint
∂θ
:torque-task-jacobian-with-wrench
∂etrq
∂ ŵ
∂etrq
∂wm
[method]
(
=
∂ etrq
∂ w1
∂ etrq
∂ w2
···
∂ etrq
∂ w Ncnt
)
T
= J cnt-att
(m = 1, 2, · · · , Ncnt )
drive-joint,m (θ, ϕ)
(2.41)
(2.42)
trq
return ∂ e ∈ RNdrive-joint ×6Ncnt
∂ ŵ
:torque-task-jacobian-with-phi
[method]
14
∂etrq
∂ϕ
=
N
cnt
∑
Nex
∑
∂τ cnt
∂τ grav
∂τ ex
m
m
m
−
+
∂ϕ
∂ϕ
∂ϕ
m=1
m=1
(2.43)
trq
return ∂ e ∈ RNdrive-joint ×Ninvar -joint
∂ϕ
:torque-task-jacobian-with-torque
[method]
∂etrq
∂τ
= I Ndrive-joint
(2.44)
trq
return ∂∂eτ ∈ RNdrive-joint ×Ndrive-joint
:posture-task-jacobian-with-theta &key (update? nil)
(
∂eposture
∂θ
{
)
=
i,j
[method]
−kposture
(Jposture,i = Jvar ,j )
0
otherwise
(2.45)
posture
return ∂ e
∈ RNposture-joint ×Nvar -joint
∂θ
:variant-task-jacobian
[method]
6Nkin
3
∂e
∂q var
=
Nvar -joint
∂ ekin
∂θ
∂ eeom-rot
∂θ
∂ etrq
∂θ
∂ eposture
∂θ
6Ncnt
3
Ndrive-joint
Nposture-joint
Ndrive-joint
∂ eeom-trans
∂ ŵ
∂ eeom-rot
∂ ŵ
∂ etrq
∂ ŵ
∂ etrq
∂τ
(2.46)
∂e
return ∂ q
∈ R(6Nkin +3+3+Ndrive-joint +Nposture-joint )×(Nvar -joint +6Ncnt +Ndrive-joint )
var
:invariant-task-jacobian
[method]
6Nkin
3
∂e
∂q invar
=
3
Ndrive-joint
Nposture-joint
Ninvar -joint
∂ ekin
∂ϕ
eom-rot
∂e
∂
ϕ
∂ etrq
∂ϕ
(2.47)
return ∂ q∂ e ∈ R(6Nkin +3+3+Ndrive-joint +Nposture-joint )×Ninvar -joint
invar
:task-jacobian
[method]
15
∂e
∂q
(
=
∂e
∂ q var
∂e
∂ q invar
3
3
Ndrive-joint
Nposture-joint
(2.48)
Nvar -joint
∂ ekin
∂θ
∂ eeom-rot
∂θ
∂ etrq
∂θ
∂ eposture
∂θ
6Nkin
=
)
6Ncnt
∂ eeom-trans
∂ ŵ
∂ eeom-rot
∂ ŵ
∂ etrq
∂ ŵ
Ndrive-joint
∂ etrq
∂τ
Ninvar -joint
∂ ekin
∂ϕ
∂ eeom-rot
∂ϕ
∂ etrq
∂ϕ
(2.49)
e ∈ R(6Nkin +3+3+Ndrive-joint +Nposture-joint )×(Nvar -joint +6Ncnt +Ndrive-joint +Ninvar -joint )
return ∂∂ q
:theta-max-vector &key (update? nil)
return θ max ∈ RNvar -joint
[method]
:theta-min-vector &key (update? nil)
return θ min ∈ RNvar -joint
[method]
:delta-theta-limit-vector &key (update? nil)
[method]
get trust region of θ
return ∆θ limit
:theta-inequality-constraint-matrix &key (update? nil)
{
⇔
⇔
θ min ≤ θ + ∆θ ≤ θ max
−∆θ limit ≤ ∆θ ≤ ∆θ limit (if ∆θ limit is set)
I
θ min − θ
−I
∆θ ≥ −(θ max − θ)
I
−∆θ
limit
−I
−∆θ limit
Cθ ∆θ ≥ dθ
[method]
(2.50)
(2.51)
(2.52)
I
−I
4Nvar -joint ×Nvar -joint
return Cθ :=
I ∈R
−I
:theta-inequality-constraint-vector
&key (update? t)
θ min − θ
−(θ max − θ)
∈ R4Nvar -joint
return dθ :=
−∆θ
limit
−∆θ limit
[method]
:wrench-inequality-constraint-matrix &key (update? t)
[method]
6
接触レンチ w ∈ R が満たすべき制約（非負制約，摩擦制約，圧力中心制約）が次式のように表される
とする．
C w w ≥ dw
(2.53)
16
Ncnt 箇所の接触部位の接触レンチを並べたベクトル ŵ の不等式制約は次式で表される．
C w,m (wm + ∆wm ) ≥ dw,m (m = 1, 2, · · · , Ncnt )
C w,m ∆wm ≥ dw,m − C w,m wm (m = 1, 2, · · · , Ncnt )
C w,1
∆w1
dw,1 − C w,1 w1
C w,2
dw,2 − C w,2 w2
∆w2
. ≥
..
..
.
.
.
.
⇔
⇔
C w,Ncnt
⇔
∆wNcnt
return C ŵ :=
(2.55)
(2.56)
dw,Ncnt − C w,Ncnt wNcnt
C ŵ ∆ŵ ≥ dŵ
(2.54)
(2.57)
C w,1
∈ RNwrench-ineq ×dim(ŵ)
C w,2
..
.
C w,Ncnt
:wrench-inequality-constraint-vector &key (update? t)
dw,1 − C w,1 w1
dw,2 − C w,2 w2
∈ RNwrench-ineq
return dŵ :=
..
.
[method]
dw,Ncnt − C w,Ncnt wNcnt
:torque-max-vector &key (update? nil)
return τ max ∈ R
[method]
Ndrive-joint
:torque-min-vector &key (update? nil)
[method]
return τ min ∈ RNdrive-joint
:torque-inequality-constraint-matrix &key (update? nil)
⇔
⇔
(
return Cτ :=
I
−I
[method]
τ min ≤ τ + ∆τ ≤ τ max
( )
(
)
I
τ min − τ
∆τ ≥
−I
−(τ max − τ )
(2.58)
Cτ ∆τ ≥ dτ
(2.60)
(2.59)
)
∈ R2Ndrive-joint ×Ndrive-joint
:torque-inequality-constraint-vector
&key (update? t)
(
)
τ min − τ
return dτ :=
∈ R2Ndrive-joint
−(τ max − τ )
[method]
:phi-max-vector &key (update? nil)
return ϕmax ∈ RNinvar -joint
[method]
:phi-min-vector &key (update? nil)
return ϕmin ∈ RNinvar -joint
[method]
:delta-phi-limit-vector &key (update? nil)
get trust region of ϕ
[method]
return ∆ϕlimit
17
:phi-inequality-constraint-matrix &key (update? nil)
{
⇔
⇔
I
ϕmin ≤ ϕ + ∆ϕ ≤ ϕmax
−∆ϕlimit ≤ ∆ϕ ≤ ∆ϕlimit (if ∆ϕlimit is set)
I
ϕmin − ϕ
−I
∆ϕ ≥ −(ϕmax − ϕ)
I
−∆ϕ
limit
−∆ϕlimit
−I
Cϕ ∆ϕ ≥ dϕ
[method]
(2.61)
(2.62)
(2.63)
−I
4Ninvar -joint ×Ninvar -joint
return Cϕ :=
I ∈R
−I
:phi-inequality-constraint-vector
&key (update? t)
ϕmin − ϕ
−(ϕmax − ϕ)
∈ R4Ninvar -joint
return dϕ :=
−∆ϕ
limit
−∆ϕlimit
[method]
:variant-config-inequality-constraint-matrix &key (update? nil)
[method]
C θ ∆θ ≥ dθ
C ŵ ∆ŵ ≥ dŵ
C τ ∆τ ≥ dτ
Cθ
∆θ
dθ
C ŵ
∆ŵ ≥ dŵ
Cτ
∆τ
dτ
⇔
⇔
return C var :=
C var ∆q var ≥ dvar
(2.64)
(2.65)
(2.66)
Cθ
∈ RNvar -ineq ×dim(q var )
C ŵ
Cτ
:variant-config-inequality-constraint-vector &key (update? t)
dθ
return dvar := dŵ ∈ RNvar -ineq
[method]
dτ
:invariant-config-inequality-constraint-matrix &key (update? nil)
⇔
[method]
C ϕ ∆ϕ ≥ dϕ
(2.67)
C invar ∆q invar ≥ dinvar
(2.68)
return C invar := C ϕ ∈ RNinvar -ineq ×dim(q invar )
:invariant-config-inequality-constraint-vector &key (update? t)
[method]
18
return dinvar := dϕ ∈ RNinvar -ineq
:config-inequality-constraint-matrix &key (update? nil)
(update-collision? nil)
C var ∆q var ≥ dvar
C
≥ dinvar
invar
(∆q invar )
∆q var
≥ dcol
C col ∆q
invar
(
)
C var
dvar
∆q var
≥ dinvar
C invar
∆q invar
dcol
C col
⇔
⇔
return C :=
C var
C∆q ≥ d
[method]
(2.69)
(2.70)
(2.71)
C invar ∈ RNineq ×dim(q )
C col
:config-inequality-constraint-vector &key (update? t)
[method]
(update-collision? nil)
dvar
return d := dinvar ∈ RNineq
dcol
:variant-config-equality-constraint-matrix &key (update? nil)
return Avar ∈ R0×dim(q var ) (no equality constraint)
[method]
:variant-config-equality-constraint-vector &key (update? t)
return bvar ∈ R0 (no equality constraint)
[method]
:invariant-config-equality-constraint-matrix &key (update? nil)
return A
∈ R0×dim(q invar ) (no equality constraint)
[method]
:invariant-config-equality-constraint-vector &key (update? t)
[method]
invar
return binvar ∈ R (no equality constraint)
0
:config-equality-constraint-matrix &key (update? nil)
return A ∈ R0×dim(q ) (no equality constraint)
[method]
:config-equality-constraint-vector &key (update? t)
return b ∈ R0 (no equality constraint)
[method]
:torque-regular-matrix &key (update? nil)
(only-variant? nil)
[method]
二次形式の正則化項として次式を考える．
Ftau (q) =
τ
2
τ max
= τ T W̄ trq τ
(ベクトルの要素ごとの割り算を表す)
(2.72)
(2.73)
19
ここで，
W̄ trq
:=
1
2
τmax
,1
∈ Rdim(τ )×dim(τ )
1
2
τmax
,2
..
.
1
(2.74)
2
τmax
,N
drive-joint
only-variant? is true:
W trq
dim(θ)
dim(ŵ)
dim(τ )
dim(θ)
:= dim(ŵ)
dim(q var )×dim(q var )
∈R
dim(τ )
(2.75)
W̄ trq
otherwise:
W trq
dim(θ)
dim(ŵ)
dim(τ )
dim(θ)
dim(ŵ)
:=
dim(τ )
dim(ϕ)
dim(ϕ)
W̄ trq
∈ Rdim(q )×dim(q )
(2.76)
return W trq
:torque-regular-vector &key (update? t)
[method]
(only-variant? nil)
v̄ trq
:=
W̄ trq τ
τ1
=
2
τmax
,1
τ2
2
τmax ,2
..
.
τdim(τ )
2
τmax
,dim(τ )
∈ Rdim(τ )
(2.77)
(2.78)
only-variant? is true:
1
v trq
dim(θ)
:= dim(ŵ)
dim(τ )
dim(q var )
∈R
(2.79)
v̄ trq
otherwise:
dim(θ)
v trq
return v trq
1
dim(ŵ)
∈ Rdim(q )
:=
dim(τ ) v̄ trq
dim(ϕ)
(2.80)
20
:torque-ratio
τ
return τ max
:=
[method]
τ1
τmax ,1
τ2
τmax ,2
..
.
τNdrive-joint
τmax ,Ndrive-joint
:regular-matrix
[method]
W reg := min(kmax , ∥e∥2 + koff )I + ktrq W trq
(2.81)
return W reg ∈ Rdim(q )×dim(q )
:regular-vector
[method]
v reg := ktrq v trq
(2.82)
return v reg ∈ Rdim(q )
:update-collision-inequality-constraint
[method]
リンク 1 とリンク 2 の最近点を p1 , p2 とする．リンク 1 とリンク 2 が干渉しない条件を，最近点 p1 , p2
の距離が dmargin 以上である条件に置き換えて考える．これは次式で表される．
dT12 (p1 − p2 ) ≥ dmargin
(2.83)
where d12 = p1 − p2
(2.84)
コンフィギュレーションが ∆q だけ更新されてもこれが成立するための条件は次式で表される．
dT12 {(p1 + ∆p1 ) − (p2 + ∆p2 )} ≥ dmargin
(2.85)
where ∆p1 = J θ,1 ∆θ + J ϕ,1 ∆ϕ
(2.86)
∆p2 = J θ,2 ∆θ + J ϕ,2 ∆ϕ
∂pi
∂pi
, J ϕ,i =
(i = 1, 2)
J θ,i =
∂θ
∂ϕ
(2.87)
(2.88)
これは以下のように変形される．
dT12 {(p1 + J θ,1 ∆θ + J ϕ,1 ∆ϕ) − (p2 + J θ,2 ∆θ + J ϕ,2 ∆ϕ)} ≥ dmargin
− J θ,2 )∆θ +
⇔
dT12 (J θ,1
⇔
cTcol,var ∆θ
+
dT12 (J ϕ,1
cTcol,invar ∆ϕ
where cTcol,var =
cTcol,invar
dcol =
dT12 (J θ,1
=
− p2 ) − dmargin )
(2.89)
(2.90)
≥ dcol
(2.91)
− J θ,2 )
(2.92)
− J ϕ,2 )
(2.93)
− p2 ) − dmargin )
(2.94)
dT12 (J ϕ,1
−(dT12 (p1
− J ϕ,2 )∆ϕ ≥
−(dT12 (p1
i 番目の干渉回避リンクペアに関する行列，ベクトルをそれぞれ cTcol,var ,i , cTcol,invar ,i , dcol,i とする．i =
21
1, 2, · · · , Ncol の全てのリンクペアにおいて干渉が生じないための条件は次式で表される．
(
)
(
) ∆θ
≥ dcol
C col,θ C col,ϕ
∆ϕ
cTcol,var ,1
..
∈ RNcol ×dim(θ )
C col,θ :=
.
T
ccol,var ,Ncol
cTcol,invar ,1
..
∈ RNcol ×dim(ϕ) ,
C col,ϕ :=
.
cTcol,invar ,Ncol
dcol,1
.
Ncol
.
dcol :=
. ∈R
dcol,Ncol
(2.95)
(2.96)
(2.97)
(2.98)
update inequality matrix C col,θ , C col,ϕ and inequality vector dcol for collision avoidance
:collision-theta-inequality-constraint-matrix
return C
∈ RNcol ×dim(θ )
[method]
:collision-phi-inequality-constraint-matrix
return C
∈ RNcol ×dim(ϕ)
[method]
:collision-inequality-constraint-matrix &key (update? nil)
[method]
col,θ
col,ϕ
(
C col := Ncol
dim(θ)
dim(ŵ)
dim(τ )
dim(ϕ)
C col,θ
O
O
C col,ϕ
)
(2.99)
return C col ∈ RNcol ×dim(q )
:collision-inequality-constraint-vector &key (update? nil)
return dcol ∈ RNcol
[method]
:update-viewer
Update viewer.
[method]
:print-status
Print status.
[method]
2.2
軌道コンフィギュレーションと軌道タスク関数
trajectory-configuration-task
:super
propertied-object
:slots
( instant-config-task-list list of instant-config-task instance)
( num-instant-config-task L)
( dim-variant-config dim(q var ))
( dim-invariant-config dim(q invar ))
[class]
22
( dim-config dim(q))
( dim-task dim(e))
( norm-regular-scale-max kmax )
( norm-regular-scale-offset koff )
( adjacent-regular-scale kadj )
( torque-regular-scale ktrq )
e)
( task-jacobi buffer for ∂∂q
軌道コンフィギュレーション q と軌道タスク関数 e(q) のクラス．
以降では，説明文やメソッド名で，“軌道” や “trajectory” を省略する．
コンフィギュレーション q の取得・更新，タスク関数 e(q) の取得，タスク関数のヤコビ行列
∂ e(q )
∂q
の取
得，コンフィギュレーションの等式・不等式制約 A, b, C, d の取得のためのメソッドが定義されている．
コンフィギュレーション・タスク関数を定めるために，初期化時に以下を与える
• 瞬時のコンフィギュレーション・タスクのリスト
instant-config-task-list instant-configuration-task のリスト
• 目的関数の重み
norm-regular-scale-max kmax コンフィギュレーション更新量正則化の重み最大値
norm-regular-scale-offset koff コンフィギュレーション更新量正則化の重みオフセット
adjacent-regular-scale kadj 隣接コンフィギュレーション正則化の重み
torque-regular-scale ktrq トルク正則化の重み
コンフィギュレーション q は以下から構成される．
(
q := q (1)T
var
(2)T
q var
···
(L)T
q var
q Tinvar
)T
(2.100)
ここで，
(1)
(2)
(L)
q invar := q invar = q invar = · · · = q invar
(l)
(2.101)
(l)
q var , q invar (l = 1, 2, · · · , L) は l 番目の瞬時の時変，時不変コンフィギュレーションを表す．
タスク関数 e(q) は以下から構成される．
(
e(q) := e(1)T (q (1)
var , q invar )
(2)
e(2)T (q var , q invar )
···
)T
(L)
e(L)T (q var , q invar )
(2.102)
(l)
e(l) (q var , q invar ) (l = 1, 2, · · · , L) は l 番目の瞬時のタスク関数を表す．
:init &key (name)
(instant-config-task-list)
[method]
(norm-regular-scale-max 1.000000e-04)
(norm-regular-scale-offset 1.000000e-07)
(adjacent-regular-scale 0.005)
(torque-regular-scale 0.001)
Initialize instance
:instant-config-task-list
return instant-config-task-list
[method]
23
:dim-variant-config
return dim(qvar ) :=
[method]
∑L
(l)
l=1 dim(q var )
:dim-invariant-config
[method]
(l)
return dim(qinvar ) := dim(q invar ) (l = 1, 2, · · · , L で同じ)
:dim-config
return dim(q) := dim(qvar ) + dim(qinvar )
[method]
:dim-task
∑L
return dim(e) := l=1 dim(e(l) )
[method]
:variant-config-vector
(1)
q var
(2)
q var
return qvar :=
..
.
[method]
(L)
q var
:invariant-config-vector
[method]
(l)
return qinvar := q invar (l = 1, 2, · · · , L で同じ)
:config-vector
(
return q :=
qvar
qinvar
)
(1)
q var
(2)
q var
[method]
..
= .
(L)
q var
q invar
:set-variant-config variant-config-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set qvar .
:set-invariant-config invariant-config-new &key (relative? nil)
(apply-to-robot? t)
[method]
Set qinvar .
:set-config config-new &key (relative? nil)
(apply-to-robot? t)
[method]
Set q.
:task-value &key (update? t)
(1) (1)
e (q var , q invar )
(2) (2)
e (q var , q invar )
return e(q) :=
..
.
[method]
(L)
e(L) (q var , q invar )
:variant-task-jacobian
[method]
24
∂ e(1)
(1)
∂ q var
∂e
∂q var
=
O
∂ e(L)
(L)
∂ q var
∂ e(2)
(2)
∂ q var
..
.
O
(2.103)
∂e
return ∂ q
∈ Rdim(e)×dim(q var )
var
:invariant-task-jacobian
[method]
∂e
∂q invar
∂ e(1)
∂q
∂ einvar
(2)
∂ q invar
= .
..
∂ e(L)
∂ q invar
(2.104)
return ∂ q∂ e ∈ Rdim(e)×dim(q invar )
invar
:task-jacobian
[method]
∂e
∂q
(
=
∂e
∂ q var
∂ e(1)
(1)
∂ q var
=
∂e
∂ q invar
)
(2.105)
O
∂ e(2)
(2)
∂ q var
∂ e(1)
∂ q invar
∂ e(2)
∂ q invar
..
.
∂ e(L)
(L)
∂ q var
O
∂ e(L)
∂ q invar
(2.106)
e ∈ Rdim(e)×dim(q )
return ∂∂ q
:variant-config-inequality-constraint-matrix &key (update? nil)
C var :=
C (1)
var
O
C (2)
var
..
.
[method]
(2.107)
C (L)
var
O
return C var ∈ RNvar -ineq ×dim(q var )
:variant-config-inequality-constraint-vector &key (update? t)
d(1)
var
(2)
dvar
dvar :=
..
.
d(L)
var
return dvar ∈ RNvar -ineq
[method]
(2.108)
25
:invariant-config-inequality-constraint-matrix &key (update? nil)
(l)
C invar := C invar (l = 1, 2, · · · , L で同じ)
[method]
(2.109)
return C invar ∈ RNinvar -ineq ×dim(q invar )
:invariant-config-inequality-constraint-vector &key (update? t)
[method]
(l)
dinvar := dinvar (l = 1, 2, · · · , L で同じ)
(2.110)
return dinvar ∈ RNinvar -ineq
:config-inequality-constraint-matrix &key (update? nil)
[method]
(update-collision? nil)
C :=
C var
C invar ∈ RNineq ×dim(q )
(2.111)
C col
return C ∈ RNineq ×dim(q )
:config-inequality-constraint-vector &key (update? t)
(update-collision? nil)
dvar
[method]
d := dinvar
(2.112)
dcol
return d ∈ RNineq
:variant-config-equality-constraint-matrix &key (update? nil)
Avar
:=
A(1)
var
[method]
O
A(2)
var
..
.
(2.113)
A(L)
var
O
return Avar ∈ RNvar -eq ×dim(q var )
:variant-config-equality-constraint-vector &key (update? t)
bvar
b(1)
var
return bvar ∈ RNvar -eq
(2)
bvar
:=
..
.
b(L)
var
[method]
(2.114)
26
:invariant-config-equality-constraint-matrix &key (update? nil)
[method]
(l)
Ainvar := Ainvar (l = 1, 2, · · · , L で同じ)
(2.115)
return Ainvar ∈ RNinvar -eq ×dim(q invar )
:invariant-config-equality-constraint-vector &key (update? t)
[method]
(l)
binvar := binvar (l = 1, 2, · · · , L で同じ)
(2.116)
return binvar ∈ RNinvar -eq
:config-equality-constraint-matrix &key (update? nil)
(
A :=
[method]
)
Avar
∈ RNeq ×dim(q )
Ainvar
(2.117)
return A ∈ RNeq ×dim(q )
:config-equality-constraint-vector &key (update? t)
(
b :=
[method]
bvar
)
(2.118)
binvar
return b ∈ RNeq
:update-collision-inequality-constraint
[method]
(l)
(l)
(l)
update inequality matrix C col,θ , C col,ϕ and inequality vector dcol for collision avoidance (l = 1, 2, · · · , L)
:collision-inequality-constraint-matrix &key (update? nil)
(l)
Ĉ col,θ
C col
:=
:=
(l)
(
[method]
dim(θ (l) )
dim(ŵ(l) )
dim(τ (l) )
(l)
O
O
Ncol
C col,θ
(1)
Ĉ col,θ
(2)
Ĉ col,θ
(1)
C col,ϕ
..
.
(L)
Ĉ col,θ
)
(2.119)
(2)
C col,ϕ
..
.
(L)
C col,ϕ
(2.120)
return C col ∈ RNcol ×dim(q )
:collision-inequality-constraint-vector &key (update? nil)
[method]
dcol
:=
(1)
dcol
(2)
dcol
.
.
.
(L)
dcol
return dcol ∈ RNcol
(2.121)
27
:adjacent-regular-matrix &key (update? nil)
[method]
二次形式の正則化項として次式を考える．
Fadj (q) =
L−1
∑
∥θ l+1 − θ l ∥2
(2.122)
l=1
T
= q W adj q
(2.123)
ここで，
I¯adj
:=
dim(θ (l) )
dim(θ (l) )
dim(ŵ(l) )
dim(ŵ(l) )
dim(τ (l) )
I
(l)
dim(q (l)
var )×dim(q var )
∈R
(2.124)
(l)
W̄ adj
W adj
:=
:=
dim(τ )
I¯adj
−I¯adj
¯
−I adj 2I¯adj
O
(
)
W̄ adj
O
−I¯adj
..
.
−I¯adj
2I¯adj
−I¯adj
O
)
∈ Rdim(q var )×dim(q var(2.125)
−I¯adj
I¯adj
(2.126)
return W adj ∈ Rdim(q )×dim(q )
:adjacent-regular-vector &key (update? t)
[method]
v adj
:=
W adj q
(2.127)
return v adj ∈ Rdim(q )
:torque-regular-matrix &key (update? nil)
W̄ trq
:=
(
W trq
:=
[method]
(1)
W trq
O
(2)
W trq
..
.
∈ Rdim(q var )×dim(q var )
(2.128)
(L)
O
W trq
)
W̄ trq
(2.129)
O
return W trq ∈ Rdim(q )×dim(q )
:torque-regular-vector &key (update? t)
[method]
v̄ trq
:=
(1)
v trq
(2)
v trq
. ∈ Rdim(q var )
.
.
(2.130)
(L)
v trq
:=
v trq
(
)
v̄ trq
0
(2.131)
28
return v trq ∈ Rdim(q )
:regular-matrix
[method]
W reg := min(kmax , ∥e∥2 + koff )I + kadj W adj + ktrq W trq
(2.132)
return W reg ∈ Rdim(q )×dim(q )
:regular-vector
[method]
v reg := kadj v adj + ktrq v trq
(2.133)
return v reg ∈ Rdim(q )
:update-viewer
Update viewer.
[method]
:print-status
[method]
Print status.
:play-animation &key (robot-env)
[method]
(loop? t)
(visualize-callback-func)
Play motion.
:generate-robot-state-list &key (robot-env)
[method]
(joint-name-list (send-all (send robot-env :robot :joint-list) :name))
(root-link-name (send (car (send robot-env :robot :links)) :name))
(step-time 0.004)
(divide-num 100)
(limb-list (list :rleg :lleg :rarm :larm))
Generate and return robot state list.
3
3.1
勾配を用いた制約付き非線形最適化
逐次二次計画法
sqp-optimization
[class]
:super
propertied-object
:slots
( config-task instance of configuration-task)
( qp-retval buffer for QP return value)
( qp-status buffer for QP status)
( qp-int-status QP status)
( task-value buffer for task value e(q))
e)
( task-jacobian buffer for task jacobian ∂∂ q
29
( dim-config-buf-matrix matrix buffer)
( convergence-check-func function to check convergence)
( failure-callback-func callback function of failure)
( pre-process-func pre-process function)
( post-process-func post-process function)
( i buffer for iteration count)
( status status of sqp optimization)
( no-visualize? whether to supress visualization)
( no-print? whether to supress print)
逐次二次計画法のクラス．
instant-configuration-task クラスや trajectory-configuration-task クラスの instance (以降，configurationtask と呼ぶ) が与えられた時に，configuration-task のタスク関数ノルム二乗 ∥e(q)∥2 を最小にするコン
フィギュレーション q を反復計算により求める．
:init &key (config-task)
(convergence-check-func)
(failure-callback-func)
[method]
(pre-process-func)
(post-process-func)
(no-visualize?)
(no-print?)
&allow-other-keys
Initialize instance
:config-task
Return configuration-task instance
[method]
:optimize &key (loop-num 100)
(loop-num-min)
[method]
(update-viewer-interval 1)
(print-status-interval 10)
Optimize
In each iteration, do following:
1. check convergence
2. call pre-process function
3. print status
4. solve QP and update configuration
5. call post-process function
30
Solve following QP:
1
min
∆q (k)T W ∆q (k) + v T ∆q (k)
(k)
2
∆q
(3.1)
s.t. A∆q (k) = b
(3.2)
≥d
(
)T (
)
∂e(q (k) )
∂e(q (k) )
where W =
+ W reg
∂q (k)
∂q (k)
(
)T
∂e(q (k) )
e(q (k) ) + v reg
v=
∂q (k)
(3.3)
C∆q
(k)
(3.4)
(3.5)
and update configuration:
q (k+1) = q (k) + ∆q (k)∗
(3.6)
:iteration
Return iteration index.
[method]
:status
Return status of sqp optimization.
[method]
3.2
3.2.1
複数解候補を用いた逐次二次計画法
複数解候補を用いた逐次二次計画法の理論
式 (1.4a) の最適化問題に逐次二次計画法などの制約付き非線形最適化手法を適用すると，初期値から勾配方
向に進行して至る局所最適解が得られると考えられる．したがって解は初期値に強く依存する．
式 (1.4a) の代わりに，以下の最適化問題を考える．
}
∑{
min
F (q (i) ) + kmsc Fmsc (q̂; i)
q̂ i∈I
s.t. Aq (i) = b̄
(3.7)
i∈I
(3.8)
Cq (i) ≥ d¯ i ∈ I
(
def
where q̂ = q (1)T q (2)T
···
q (Nmsc )T
)T
def
I = {1, 2, · · · , Nmsc }
1∑
def
Fmsc (q̂; i) = −
log ∥d(q (i) , q (j) )∥2
2
(3.9)
(3.10)
(3.11)
(3.12)
j∈I
j̸=i
def
d(q (i) , q (j) ) = q (i) − q (j)
(3.13)
Nmsc は解候補の個数で，事前に与えるものとする．msc は複数解候補 (multiple solution candidates) を表す．
これは，複数の解候補を同時に探索し，それぞれの解候補 q (i) が本来の目的関数 F (q (i) ) を小さくして，なお
かつ，解候補どうしの距離が大きくなるように最適化することを表している．これにより，初期値に依存した
唯一の局所解だけでなく，そこから離れた複数の局所解を得ることが可能となり，通常の最適化に比べてより
良い解が得られることが期待される．以降では，解候補どうしの距離のコストを表す項 Fmsc (q̂; i) を解候補分
散項と呼ぶ 8 ．
8 解分散項の
log を無くすことは適切ではない．なぜなら，d = ∥d(q (i) , q (j) )∥ として，解分散項の勾配は，
(
)
(
)
∂
∂
1
1
1
1
− log d2 = − → −∞ (d → +0)
− log d2 = − → 0 (d → ∞)
∂d
2
d
∂d
2
d
(3.14)
31
解候補分散項のヤコビ行列，ヘッセ行列の各成分は次式で得られる 9 ．
∂Fmsc (q̂; i)
∂q (i)
1∑ ∂
−
log ∥d(q (i) , q (j) )∥2
(i)
2
∂q
j∈I
∇i Fmsc (q̂; i) =
=
j̸=i
−
=
∑
j∈I
j̸=i
−
=
∑
j∈I
j̸=i
1
∥d(q (i) , q (j) )∥2
(
∂d(q (i) , q (j) )
∂q (i)
(3.16a)
(3.16b)
)T
d(q (i) , q (j) )
(3.16c)
d(q (i) , q (j) )
∥d(q (i) , q (j) )∥2
(3.16d)
(3.16e)
∂Fmsc (q̂; i)
k ∈ I ∧ k ̸= i
∂q (k)
1∑ ∂
= −
log ∥d(q (i) , q (j) )∥2
(k)
2
∂q
j∈I
∇k Fmsc (q̂; i) =
(3.17a)
(3.17b)
j̸=i
1 ∂
= −
log ∥d(q (i) , q (k) )∥2
2 ∂q (k)
(
)T
1
∂d(q (i) , q (k) )
= −
d(q (i) , q (k) )
∥d(q (i) , q (k) )∥2
∂q (k)
=
(3.17c)
(3.17d)
d(q (i) , q (k) )
∥d(q (i) , q (k) )∥2
(3.17e)
(3.17f)
∂ 2 Fmsc (q̂; i)
∂q (i)2
)
}−1
∑ ∂ ({
(i)
(j) 2
(i)
(j)
= −
∥d(q , q )∥
d(q , q )
∂q (i)
j∈I
∇2ii Fmsc (q̂; i) =
j̸=i
= −
∑(
∑(
j∈I
j̸=i
= −
∑
(3.18b)
{
}−2
{
}−1 )
−2 ∥d(q (i) , q (j) )∥2
d(q (i) , q (j) )d(q (i) , q (j) )T + ∥d(q (i) , q (j) )∥2
(3.18c)
I
j∈I
j̸=i
= −
(3.18a)
−
2
1
d(q (i) , q (j) )d(q (i) , q (j) )T +
I
∥d(q (i) , q (j) )∥4
∥d(q (i) , q (j) )∥2
H(q (i) , q (j) )
)
(3.18d)
(3.18e)
j∈I
j̸=i
となり，最適化により，コンフィギュレーションが近いときほど離れるように更新し，遠くなるとその影響が小さくなる効果が期待され
る．それに対し，log がない場合の勾配は，
(
)
(
)
∂
1
∂
1
− d2 = −d → 0 (d → +0)
− d2 = −d → −∞ (d → ∞)
(3.15)
∂d
2
∂d
2
となり，コンフィギュレーションが遠くなるほど離れるように更新し，近いときはその影響が小さくなる．これは，コンフィギュレーショ
ンが一致する勾配ゼロの点と，無限に離れ発散する最適値をもち，これらは最適化において望まない挙動をもたらす．
9 ヘッセ 行 列 の 導 出 は 以 下 を 参 考 に し た ．https://math.stackexchange.com/questions/175263/
gradient-and-hessian-of-general-2-norm
32
ただし，
2
def
H(q (i) , q (j) ) = −
∇2ik Fmsc (q̂; i)
∥d(q (i) , q (j) )∥4
d(q (i) , q (j) )d(q (i) , q (j) )T +
1
∥d(q (i) , q (j) )∥2
I
∂ 2 Fmsc (q̂; i)
k ∈ I ∧ k ̸= i
∂q (i) ∂q (k)
(
)
{
}−1
∑ ∂
(i)
(j) 2
(i)
(j)
= −
∥d(q , q )∥
d(q , q )
∂q (k)
j∈I
=
(3.19)
(3.20a)
(3.20b)
j̸=i
({
)
}−1
∂
(i)
(k) 2
(i)
(k)
= − (k)
(3.20c)
∥d(q , q )∥
d(q , q )
∂q
( {
)
}−2
{
}−1
= − 2 ∥d(q (i) , q (k) )∥2
d(q (i) , q (k) )d(q (i) , q (k) )T − ∥d(q (i) , q (k) )∥2
I(3.20d)
= −
2
1
d(q (i) , q (k) )d(q (i) , q (k) )T +
I
∥d(q (i) , q (k) )∥4
∥d(q (i) , q (k) )∥2
= H(q (i) , q (k) )
(3.20e)
(3.20f)
∂ 2 Fmsc (q̂; i)
k ∈ I ∧ k ̸= i
∂q (k)2
({
)
}−1
∂
(i)
(k) 2
(i)
(k)
=
∥d(q , q )∥
d(q , q )
∂q (k)
(
)
1
2
(i)
(k)
(i)
(k) T
d(q , q )d(q , q ) +
I
= − −
∥d(q (i) , q (k) )∥4
∥d(q (i) , q (k) )∥2
(3.21b)
= −H(q (i) , q (k) )
(3.21d)
∇2kk Fmsc (q̂; i) =
∇2kl Fmsc (q̂; i)
∂ 2 Fmsc (q̂; i)
k ∈ I ∧ l ∈ I ∧ k ̸= i ∧ l ̸= i ∧ k ̸= l
∂q (k) ∂q (l)
)
({
}−1
∂
(i)
(k)
(i)
(k) 2
d(q
,
q
)
=
∥d(q
,
q
)∥
∂q (l)
= O
=
(3.21a)
(3.21c)
(3.22a)
(3.22b)
(3.22c)
33
したがって，解候補分散項のヤコビ行列，ヘッセ行列は次式で表される．
∇Fmsc (q̂; i)
∂Fmsc (q̂; i)
∂ q̂
d(q (i) ,q (1) )
(i)
(1)
2
∥d(q ,q )∥
..
.
d
(q (i) ,q (i−1) )
∥d(q (i) ,q (i−1) )∥2
(i)
(j)
∑
− j∈I d(q(i) ,q(j) ) 2
∥d(q ,q )∥
j̸=i
d(q (i) ,q (i+1) )
(i)
(i+1)
2
∥d(q ,q
)∥
.
..
(i)
(Nmsc )
d(q ,q
)
∥d(q (i) ,q (Nmsc ) )∥2
∑
∇Fmsc (q̂; i)
=
=
v msc
def
=
i∈I
−
∑
2
∑
−
=
=
W msc
def
=
(3.23c)
..
.
(Nmsc )
(j)
d(q
,q )
(N
)
(j)
2
msc
∥d(q
,q )∥
1
(3.24a)
···
i−1
i
−H i,1
..
.
i−1
H
i
···
i,1
i+1
..
.
Nmsc
∑
∇2 Fmsc (q̂; i)
∑
− j∈I H 1,j
j̸=i
H 2,1
2
..
.
H Nmsc ,1
···
i+1
Nmsc
H i,1
..
.
−H i,i−1
H i,i−1
H i,i−1
∑
− j∈I H i,j
H i,i+1
j̸=i
···
H i,Nmsc
−H i,i+1
H i,i+1
..
.
..
.
(3.24b)
−H i,Nmsc
H i,Nmsc
(3.24c)
i∈I
=
(3.23d)
∂ 2 Fmsc (q̂; i)
∂ q̂ 2
1
..
.
=
(3.23b)
d(q (1) ,q (j) )
j∈I
∥d(q (1) ,q (j) )∥2
j̸=1
j∈I
j̸=Nmsc
∇2 Fmsc (q̂; i)
(3.23a)
H 1,2
∑
− j∈I H 2,j
···
H 1,Nmsc
H 2,Nmsc
j̸=i
..
H Nmsc ,2
.
···
−
..
.
∑
j∈I
j̸=i
H Nmsc ,j
(3.24d)
ただし，H(q (i) , q (j) ) を H i,j と略して記す．また，d(q (i) , q (j) ) = −d(q (j) , q (i) ), H i,j = H j,i を利用した．
∑
解候補分散項 i∈I Fmsc (q̂; i) による二次計画問題の目的関数 (式 (1.5a) ) は次式で表される．
}
∑{
1
Fmsc (q̂ k ; i) + ∇Fmsc (q̂ k ; i)T ∆q̂ k + ∆q̂ Tk ∇2 Fmsc (q̂ k ; i)∆q̂ k
(3.25)
2
i∈I
{
{
}
}T
∑
∑
∑
1
T
2
=
Fmsc (q̂ k ; i) +
∇ Fmsc (q̂ k ; i) ∆q̂ k
(3.26)
∇Fmsc (q̂ k ; i)
∆q̂ k + ∆q̂ k
2
i∈I
i∈I
i∈I
∑
1
=
Fmsc (q̂ k ; i) + v Tmsc ∆q̂ k + ∆q̂ Tk W msc ∆q̂ k
(3.27)
2
i∈I
34
W msc が必ずしも半正定値行列ではないことに注意する必要がある．以下のようにして W msc に近い正定
値行列を計算し用いることで対処する 10 ．W msc が次式のように固有値分解されるとする．
W msc = V msc D msc V −1
msc
(3.28)
ただし，D msc は固有値を対角成分にもつ対角行列，V msc は固有ベクトルを並べた行列である．このとき W msc
に近い正定値行列 W̃ msc は次式で得られる．
−1
W̃ msc = V msc D +
msc V msc
(3.29)
ただし，D +
msc は D msc の対角成分のうち，負のものを 0 で置き換えた対角行列である．
式 (3.7) において，解候補を分散させながら，最終的に本来の目的関数を最小にする解を得るために，SQP
のイテレーションごとに，解候補分散項のスケール kmsc を次式のように更新することが有効である．
kmsc ← min(γmsc kmsc , kmsc-min )
(3.30)
γmsc は 0 < γmsc < 1 なるスケール減少率，kmsc-min はスケール最小値を表す．
3.2.2
複数解候補を用いた逐次二次計画法の実装
sqp-msc-optimization
:super
:slots
[class]
sqp-optimization
( num-msc number of multiple solution candidates Nmsc )
( config-task-list list of configuration-task instance)
( dispersion-scale kmsc )
( dispersion-scale-min kmsc-min , minimum of kmsc )
( dispersion-scale-decrease-ratio γmsc , decrease ration of kmsc )
( config-vector-dist2-min minimum squared distance of configuration vector)
( dispersion-matrix buffer for W msc )
複数回候補を用いた逐次二次計画法のクラス．
instant-configuration-task クラスや trajectory-configuration-task クラスの instance (以降，configurationtask と呼ぶ) が与えられた時に，configuration-task のタスク関数ノルム二乗 ∥e(q)∥2 を最小にするコン
フィギュレーション q を，複数の解候補を同時に考慮しながら反復計算により求める．
:init &rest args &key (num-msc 3)
(dispersion-scale 0.01)
[method]
(dispersion-scale-min 0.0)
(dispersion-scale-decrease-ratio 0.5)
(config-vector-dist2-min 1.000000e-10)
&allow-other-keys
Initialize instance
:config-task-list
Return list of configuration-task instance
[method]
10 W
が 対 称 行 列 で あ る こ と か ら ，以 下 を 参 考 に し た ．https://math.stackexchange.com/questions/648809/
msc
how-to-find-closest-positive-definite-matrix-of-non-symmetric-matrix#comment1689831_649522
35
:dispersion-matrix
[method]
式 (3.23d) 参照．
return W msc ∈ RNmsc dim(q )×Nmsc dim(q )
:dispersion-vector
[method]
式 (3.24d) 参照．
return v msc ∈ RNmsc dim(q )
4
4.1
動作生成の拡張
マニピュレーションの動作生成
robot-object-environment
:super
robot-environment
:slots
( obj O)
( obj-with-root-virtual Ô)
[class]
ロボットと物体とロボット・環境間の接触のクラス．
以下を合わせた関節・リンク構造に関するメソッドが定義されている．
1. 浮遊ルートリンクのための仮想関節付きのロボットの関節
2. 物体位置姿勢を表す仮想関節
3. 接触位置を定める仮想関節
関節・リンク構造を定めるために，初期化時に以下を与える
robot R ロボット (cascaded-link クラスのインスタンス)．
object O 物体 (cascaded-link クラスのインスタンス)．関節をもたないことを前提とする．
contact-list {C1 , C2 , · · · , CNC } 接触 (2d-planar-contact クラスなどのインスタンス) のリスト．
ロボット R に，浮遊ルートリンクの変位に対応する仮想関節を付加した仮想関節付きロボット R̂ を内部
で保持する．同様に，物体 O に，物体の変位に対応する仮想関節を付加した仮想関節付き物体 Ô を内部
で保持する．
:init &key (robot)
[method]
(object)
(contact-list)
(root-virtual-mode :6dof )
(root-virtual-joint-class-list)
(root-virtual-joint-axis-list)
Initialize instance
:object &rest args
return O
[method]
:object-with-root-virtual &rest args
[method]
36
return Ô
instant-manipulation-configuration-task
:super
:slots
[class]
instant-configuration-task
( robot-obj-env robot-object-environment instance)
( wrench-obj-vector ŵobj [N] [Nm])
( num-contact-obj Ncnt-obj := |T cnt-trg-obj |)
( num-act-react Nact-react := |P act-react |)
( dim-wrench-obj dim(ŵobj ) = 6Ncnt-obj )
( contact-target-coords-obj-list T cnt-trg-obj )
( contact-constraint-obj-list list of contact-constraint instance for object)
( act-react-pair-list P act-react )
マニピュレーションにける瞬時コンフィギュレーション q (l) と瞬時タスク関数 e(l) (q (l) ) のクラス．マニ
ピュレーション対象の物体の瞬時コンフィギュレーションや瞬時タスク関数を含む．
このクラスの説明で用いる全ての変数は，時間ステップ l を表す添字をつけて x(l) と表されるべきだが，
このクラス内の説明では省略して x と表す．また，以降では，説明文やメソッド名で，“瞬時” や “instant”
を省略する．
コンフィギュレーション q の取得・更新，タスク関数 e(q) の取得，タスク関数のヤコビ行列
∂ e(q )
∂q
の取
得，コンフィギュレーションの等式・不等式制約 A, b, C, d の取得のためのメソッドが定義されている．
コンフィギュレーション・タスク関数を定めるために，instant-configuration-task の設定に加えて，初期
化時に以下を与える
• ロボット・物体・環境
robot-object-environment ロボット・物体・環境を表す robot-object-environment クラスのイ
ンスタンス
• 物体の接触拘束
contact-target-coords-obj-list T cnt-trg-obj 物体の接触目標位置姿勢リスト
contact-constraint-obj-list 物体の接触レンチ制約リスト
• 作用・反作用の拘束
act-react-pair-list P act-react 作用・反作用の関係にあるロボット・物体の接触目標位置姿勢ペア
のリスト
コンフィギュレーション q は以下から構成される．
θ ∈ RNvar -joint 時変関節角度 [rad] [m]
ŵ ∈ R6Ncnt ロボットの接触レンチ [N] [Nm]
ŵobj ∈ R6Ncnt-obj 物体の接触レンチ [N] [Nm]
τ ∈ RNdrive-joint 関節駆動トルク [Nm] [N]
ϕ ∈ RNinvar -joint 時不変関節角度 [rad] [m]
ŵ は次式のように，全接触部位でのワールド座標系での力・モーメントを並べたベクトルである．
(
)T
ŵ =
(4.1)
wT1 wT2 · · · wTNcnt
)T
(
=
(4.2)
f T1 nT1 f T2 nT2 · · · f TNcnt nTNcnt
37
タスク関数 e(q) は以下から構成される．
ekin (q) ∈ R6Nkin 幾何到達拘束 [rad] [m]
eeom-trans (q) ∈ R3 ロボットの力の釣り合い [N]
eeom-rot (q) ∈ R3 ロボットのモーメントの釣り合い [Nm]
eeom-trans-obj (q) ∈ R3 物体の力の釣り合い [N]
eeom-rot-obj (q) ∈ R3 物体のモーメントの釣り合い [Nm]
etrq (q) ∈ RNdrive-joint 関節トルクの釣り合い [rad] [m]
eposture (q) ∈ RNposture-joint 関節角目標 [rad] [m]
:init &rest args &key (contact-target-coords-obj-list)
(contact-constraint-obj-list)
[method]
(act-react-pair-list)
&allow-other-keys
Initialize instance
:robot-obj-env
return robot-object-environment instance
[method]
:wrench-obj
[method]
return ŵobj
:num-contact-obj
[method]
return Ncnt-obj := |T
cnt-trg-obj
|
:dim-variant-config
[method]
dim(qvar ) :=
=
dim(θ) + dim(ŵ) + dim(ŵobj ) + dim(τ )
(4.3)
Nvar -joint + 6Ncnt + 6Ncnt-obj + Ndrive-joint
(4.4)
return dim(qvar )
:dim-task
[method]
dim(e)
:=
=
dim(ekin ) + dim(eeom-trans ) + dim(eeom-rot ) + dim(eeom-trans-obj )
+dim(eeom-rot-obj ) + dim(etrq ) + dim(eposture )
(4.5)
6Nkin + 3 + 3 + 3 + 3 + Ndrive-joint + Nposture-joint
(4.6)
return dim(e)
:variant-config-vector
return qvar
:config-vector
[method]
θ
ŵ
:=
ŵ
obj
τ
[method]
38
θ
(
)
ŵ
qvar
return q :=
=
ŵobj
qinvar
τ
ϕ
:set-wrench-obj wrench-obj-new &key (relative? nil)
Set ŵobj .
[method]
:set-variant-config variant-config-new &key (relative? nil)
[method]
(apply-to-robot? t)
Set qvar .
:contact-target-coords-obj-list
[method]
cnt-trg-obj
Tm
= {pcnt-trg-obj
, Rcnt-trg-obj
} (m = 1, 2, · · · , Ncnt-obj )
m
m
(4.7)
}
return T cnt-trg-obj := {T1cnt-trg-obj , T2cnt-trg-obj , · · · , TNcnt-trg-obj
cnt-obj
:contact-constraint-obj-list
return list of contact-constraint instance for object
[method]
:wrench-obj-list
return {wobj ,1 , wobj ,2 , · · · , wobj ,Nobj }
[method]
:force-obj-list
return {f obj ,1 , f obj ,2 , · · · , f obj ,Ncnt-obj }
[method]
:moment-obj-list
[method]
return {nobj ,1 , nobj ,2 , · · · , nobj ,Ncnt-obj }
:mg-obj-vec
[method]
return mobj g
:cog-obj &key (update? t)
return pGobj (q)
[method]
:eom-trans-obj-task-value &key (update? t)
[method]
eeom-trans-obj (q) =
eeom-trans-obj (ŵobj )
∑
(4.8)
Ncnt-obj
=
f obj ,m − mobj g
(4.9)
m=1
return eeom-trans-obj (q) ∈ R3
:eom-rot-obj-task-value &key (update? t)
eeom-rot-obj (q)
=
eeom-rot-obj (θ, ŵobj , ϕ)
∑ {(
Ncnt-obj
=
[method]
m=1
)
}
pcnt-trg-obj
(θ, ϕ) − pGobj (θ, ϕ) × f obj ,m + nobj ,m
m
(4.10)
(4.11)
39
return eeom-rot-obj (q) ∈ R3
:task-value &key (update? t)
ekin (q)
ekin (θ, ϕ)
eom-trans
e
(q)
eeom-trans (ŵ)
eeom-rot (q) eeom-rot (θ, ŵ, ϕ)
return e(q) := eeom-trans-obj (q) = eeom-trans-obj (ŵobj )
eeom-rot-obj (q) eeom-rot-obj (θ, ŵobj , ϕ)
etrq (q)
etrq (θ, ŵ, τ , ϕ)
posture
posture
e
(q)
e
(θ)
[method]
:eom-trans-obj-task-jacobian-with-wrench-obj
[method]
∂eeom-trans-obj
∂ ŵobj
(
=
(
=
∂ eeom-trans-obj
∂ f obj ,1
I3
O3
∂ eeom-trans-obj
∂ nobj ,1
∂ eeom-trans-obj
∂ f obj ,N
···
cnt-obj
)
···
I3
∂ eeom-trans-obj
∂ nobj ,Ncnt-obj
)
(4.12)
(4.13)
O3
eom-trans-obj
return ∂ e
∈ R3×6Ncnt-obj
∂ ŵ obj
:eom-rot-obj-task-jacobian-with-theta
∂eeom-rot-obj
∂θ
[method]
(
)}
∑ {
−[f obj ,m ×] J cnt-trg-obj
(θ,
ϕ)
−
J
(θ,
ϕ)
Gobj
θ
θ,m
Ncnt-obj
=
m=1
∑
Ncnt-obj
=
Ncnt-obj
f obj ,m × J Gobj θ (θ, ϕ) −
m=1
∑Ncnt-obj
m=1
∑
(4.14)
[f obj ,m ×]J cnt-trg-obj
(θ, ϕ) (4.15)
θ,m
m=1
f obj ,m = mobj g つまり，eom-trans-obj-task が成立すると仮定すると次式が成り立つ．
∑
Ncnt-obj
∂eeom-rot-obj
∂θ
[mobj g×] J Gobj θ (θ, ϕ) −
=
[f obj ,m ×]J cnt-trg-obj
(θ, ϕ)
θ,m
(4.16)
m=1
eom-rot-obj
return ∂ e
∈ R3×Nvar -joint
∂θ
:eom-rot-obj-task-jacobian-with-wrench-obj
(
[method]
∂eeom-rot-obj
∂ ŵobj
=
∂eeom-rot-obj
∂f obj ,m
=
∂eeom-rot-obj
∂nobj ,m
= I 3 (m = 1, 2, · · · , Ncnt-obj )
∂ eeom-rot-obj
∂ f obj ,1
[(
∂ eeom-rot-obj
∂ nobj ,1
···
∂ eeom-rot-obj
∂ f obj ,N
cnt-obj
∂ eeom-rot-obj
∂ nobj ,Ncnt-obj
)
) ]
pcnt-trg-obj
(θ, ϕ) − pGobj (θ, ϕ) × (m = 1, 2, · · · , Ncnt-obj )
m
(4.17)
(4.18)
(4.19)
eom-rot-obj
return ∂ e
∈ R3×6Ncnt-obj
∂ ŵobj
:eom-rot-obj-task-jacobian-with-phi
[method]
40
∑ {
Ncnt-obj
∂eeom-rot-obj
∂ϕ
=
(
)}
−[f obj ,m ×] J cnt-trg-obj
(θ, ϕ) − J Gobj ϕ (θ, ϕ)
ϕ,m
m=1
∑
Ncnt-obj
=
Ncnt-obj
f obj ,m × J Gobj ϕ (θ, ϕ) −
m=1
∑Ncnt-obj
m=1
∑
(4.20)
[f obj ,m ×]J cnt-trg-obj
(θ, ϕ) (4.21)
ϕ,m
m=1
f obj ,m = mobj g つまり，eom-trans-obj-task が成立すると仮定すると次式が成り立つ．
∂eeom-rot-obj
∂ϕ
∑
Ncnt-obj
=
[mobj g×] J Gobj ϕ (θ, ϕ) −
[f obj ,m ×]J cnt-trg-obj
(θ, ϕ)
ϕ,m
(4.22)
m=1
eom-rot-obj
return ∂ e
∈ R3×Ninvar -joint
∂ϕ
:variant-task-jacobian
[method]
6Nkin
3
3
∂e
∂q var
=
Nvar -joint
∂ ekin
∂θ
∂ eeom-rot
∂θ
eom-rot-obj
∂e
∂θ
∂ etrq
∂θ
∂ eposture
∂θ
3
3
Ndrive-joint
Nposture-joint
6Ncnt
∂ eeom-trans
∂ ŵ
∂ eeom-rot
∂ ŵ
6Ncnt-obj
Ndrive-joint
∂ eeom-trans-obj
∂ ŵ obj
∂ eeom-rot-obj
∂ ŵ obj
∂ etrq
∂ ŵ
∂ etrq
∂τ
(4.23)
∂e
return ∂ q
∈ R(6Nkin +3+3+3+3+Ndrive-joint +Nposture-joint )×(Nvar -joint +6Ncnt +6Ncnt-obj +Ndrive-joint )
var
:invariant-task-jacobian
[method]
6Nkin
3
∂e
∂q invar
=
3
3
3
Ndrive-joint
Nposture-joint
Ninvar -joint
∂ ekin
∂ϕ
∂ eeom-rot
∂ϕ
eom-rot-obj
∂e
∂ϕ
∂ etrq
∂ϕ
(4.24)
return ∂ q∂ e ∈ R(6Nkin +3+3+3+3+Ndrive-joint +Nposture-joint )×Ninvar -joint
invar
:task-jacobian
[method]
41
∂e
∂q
(
=
∂e
∂ q var
∂e
∂ q invar
6Nkin
3
3
=
3
3
Ndrive-joint
Nposture-joint
)
(4.25)
Nvar -joint
∂ ekin
∂θ
∂ eeom-rot
∂θ
∂ eeom-rot-obj
∂θ
∂ etrq
∂θ
∂ eposture
∂θ
6Ncnt
6Ncnt-obj
Ndrive-joint
∂ eeom-trans
∂ ŵ
∂ eeom-rot
∂ ŵ
Ninvar -joint
∂ ekin
∂ϕ
(4.26)
∂ eeom-rot
∂ϕ
∂ eeom-trans-obj
∂ ŵ obj
∂ eeom-rot-obj
∂ ŵ obj
∂ etrq
∂ ŵ
∂ etrq
∂τ
∂ eeom-rot-obj
∂ϕ
∂ etrq
∂ϕ
e ∈ R(6Nkin +3+3+3+3+Ndrive-joint +Nposture-joint )×(Nvar -joint +6Ncnt +6Ncnt-obj +Ndrive-joint +Ninvar -joint )
return ∂∂ q
:wrench-obj-inequality-constraint-matrix &key (update? t)
[method]
6
物体の接触レンチ wobj ∈ R が満たすべき制約（非負制約，摩擦制約，圧力中心制約）が次式のように
表されるとする．
C wobj wobj ≥ dwobj
(4.27)
Ncnt-obj 箇所の接触部位の接触レンチを並べたベクトル ŵobj の不等式制約は次式で表される．
C wobj ,m (wobj ,m + ∆wobj ,m ) ≥ dwobj ,m (m = 1, 2, · · · , Ncnt-obj )
⇔
⇔
⇔
(4.28)
C wobj ,m ∆wobj ,m ≥ dwobj ,m − C wobj ,m wobj ,m (m = 1, 2, · · · , Ncnt-obj )
(4.29)
dwobj ,1 − C wobj ,1 wobj ,1
C wobj ,1
∆wobj ,1
dwobj ,2 − C wobj ,2 wobj ,2
C wobj ,2
∆wobj ,2
(4.30)
≥
.
..
..
.
.
.
.
dwobj ,Ncnt-obj − C wobj ,Ncnt-obj wobj ,Ncnt-obj
C wobj ,Ncnt-obj
∆wobj ,Ncnt-obj
C ŵobj ∆ŵobj ≥ dŵobj
return C ŵobj :=
(4.31)
C wobj ,1
∈ RNwrench-obj -ineq ×6Ncnt-obj
C wobj ,2
..
.
C wobj ,Ncnt-obj
:wrench-obj-inequality-constraint-vector
&key (update?t)
dwobj ,1 − C wobj ,1 wobj ,1
dwobj ,2 − C wobj ,2 wobj ,2
return dŵobj :=
∈ RNwrench-obj -ineq
.
..
dwobj ,Ncnt-obj − C wobj ,Ncnt-obj wobj ,Ncnt-obj
[method]
:variant-config-inequality-constraint-matrix &key (update? nil)
[method]
42
⇔
⇔
return C var :=
C θ ∆θ ≥ dθ
C ∆ŵ ≥ d
ŵ
ŵ
C ŵobj ∆ŵobj ≥ dŵobj
C ∆τ ≥ d
τ
τ
dθ
Cθ
∆θ
∆ŵ dŵ
C ŵ
≥
∆ŵ d
C ŵobj
obj
ŵobj
dτ
∆τ
Cτ
C var ∆q var ≥ dvar
(4.32)
(4.33)
(4.34)
Cθ
∈ RNvar -ineq ×dim(q var )
C ŵ
C ŵobj
Cτ
:variant-config-inequality-constraint-vector
&key (update? t)
dθ
dŵ
∈ RNvar -ineq
return dvar :=
d
ŵobj
dτ
[method]
:act-react-equality-constraint-matrix &key (update? nil)
[method]
43
ロボット・物体間の接触レンチに関する作用・反作用の法則は次式のように表される．
⇔
ŵi(m) + ŵobj ,j(m) = 0 (m = 1, 2, · · · , Nact-react )
(4.35)
Aact-react,robot,m ŵ + Aact-react,obj ,m ŵobj = 0 (m = 1, 2, · · · , Nact-react )
(4.36)
i(m) 番目
(
where Aact-react,robot,m =
O6
⇔
I6
···
)
O6
O6
j(m) 番目
(
Aact-react,obj ,m =
···
O6
O6
O6
···
I6
Aact-react,robot ŵ + Aact-react,obj ŵobj = 0
Aact-react,robot,1
..
where Aact-react,robot =
.
···
)
O6
O6
∈ R6×6Ncnt (4.37)
∈ R6×6Ncnt-obj(4.38)
∈ R6Nact-react ×6Ncnt
Aact-react,robot,Nact-react
Aact-react,obj ,1
..
∈ R6Nact-react ×6Ncnt-obj
=
.
Aact-react,obj ,Nact-react
(4.39)
(4.40)
Aact-react,obj
(
⇔
⇔
⇔
Aact-react
ŵ
ŵobj
)
= 0 ∈ R6Nact-react
(4.41)
(4.42)
(
)
where Aact-react = Aact-react,robot Aact-react,obj ∈ R6Nact-react ×(6Ncnt +6Ncnt-obj )
(
)
ŵ + ∆ŵ
Aact-react
=0
ŵobj + ∆ŵobj
(
)
∆ŵ
Aact-react
= bact-react
∆ŵobj
(
)
ŵ
where bact-react = −Aact-react
ŵobj
(4.43)
(4.44)
(4.45)
(4.46)
i(m), j(m) は作用・反作用の関係にある接触レンチの m 番目の対におけるロボット，物体の接触レンチ
のインデックスである．
return Aact-react ∈ R6Nact-react ×(6Ncnt +6Ncnt-obj )
:act-react-equality-constraint-vector &key (update? t)
return bact-react ∈ R6Nact-react
[method]
:variant-config-equality-constraint-matrix &key (update? nil)
[method]
(
Aact-react
⇔
∆ŵobj
(
O
)
∆ŵ
Aact-react
= bact-react
(4.47)
∆θ
)
∆ŵ
O
∆ŵ = bact-react
obj
(4.48)
∆τ
⇔
(
return Avar := O
Aact-react
Avar ∆q var = bvar
)
O ∈ R6Nact-react ×dim(q var )
(4.49)
44
:variant-config-equality-constraint-vector &key (update? t)
return bvar := bact-react ∈ R
[method]
6Nact-react
:invariant-config-equality-constraint-matrix &key (update? nil)
return A
∈ R0×dim(q invar ) (no equality constraint)
[method]
:invariant-config-equality-constraint-vector &key (update? t)
return binvar ∈ R0 (no equality constraint)
[method]
:config-equality-constraint-matrix &key (update? nil)
[method]
invar
⇔
⇔
(
Avar ∆q var = bvar
(
)
(
) ∆q
var
= bvar
Avar O
∆q invar
(4.50)
A∆q = b
(4.52)
(4.51)
)
return A := Avar
O ∈ RNeq ×dim(q )
:config-equality-constraint-vector &key (update? t)
return b := bvar ∈ RNeq
[method]
:torque-regular-matrix &key (update? nil)
(only-variant? nil)
[method]
二次形式の正則化項として次式を考える．
τ
Ftau (q) =
2
(ベクトルの要素ごとの割り算を表す)
τ max
= τ T W̄ trq τ
ここで，
W̄ trq
:=
(4.53)
(4.54)
1
2
τmax
,1
∈ Rdim(τ )×dim(τ )
1
2
τmax
,2
..
.
1
(4.55)
2
τmax
,N
drive-joint
only-variant? is true:
W trq
dim(θ)
dim(ŵ)
dim(ŵobj )
dim(τ )
dim(θ)
dim(ŵ)
:=
dim(ŵobj )
∈ Rdim(q var )×dim(q var )
dim(τ )
(4.56)
W̄ trq
otherwise:
dim(θ)
W trq
dim(ŵ)
:= dim(ŵobj )
dim(τ )
dim(ϕ)
dim(θ)
dim(ŵ)
dim(ŵobj )
dim(τ )
W̄ trq
dim(ϕ)
∈ Rdim(q )×dim(q ) (4.57)
45
return W trq
:torque-regular-vector &key (update? t)
[method]
(only-variant? nil)
v̄ trq
:=
W̄ trq τ
=
(4.58)
τ1
2
τmax
,1
τ2
2
τmax ,2
..
.
τdim(τ )
2
τmax
,dim(τ )
∈ Rdim(τ )
(4.59)
only-variant? is true:
1
v trq
dim(θ)
dim(ŵ)
:=
dim(ŵobj )
dim(τ )
∈ Rdim(q var )
(4.60)
v̄ trq
otherwise:
dim(θ)
v trq
1
dim(ŵ)
∈ Rdim(q )
:= dim(ŵobj )
dim(τ )
v̄ trq
dim(ϕ)
(4.61)
return v trq
:collision-inequality-constraint-matrix &key (update? nil)
(
C col := Ncol
dim(θ) dim(ŵ) dim(ŵobj )
C col,θ
O
O
[method]
dim(τ )
dim(ϕ)
O
C col,ϕ
)
(4.62)
return C col ∈ RNcol ×dim(q )
:update-viewer
Update viewer.
[method]
:print-status
Print status.
[method]
46
4.2
4.2.1
B スプラインを用いた関節軌道生成
B スプラインを用いた関節軌道生成の理論
一般の B スプライン基底関数の定義
B スプライン基底関数は以下で定義される．
{
1 if ti ≤ t < ti+1
def
bi,0 (t) =
0 otherwise
t − ti
ti+n+1 − t
def
bi,n (t) =
bi,n−1 (t) +
bi+1,n−1 (t)
ti+n − ti
ti+n+1 − ti+1
(4.63)
(4.64)
ti はノットと呼ばれる．
使用区間を指定してノットを一様とする場合の B スプライン基底関数
ts , tf を B スプラインの使用区間の初期，終端時刻とする．
n < m とする．
tn = ts
(4.65)
tm = tf
(4.66)
とする．ti (0 ≤ i ≤ n + m) が等間隔に並ぶとすると，
i−n
(tf − ts ) + ts
m−n
mts − ntf
= hi +
m−n
ti
=
(4.67)
(4.68)
ただし，
def
h =
tf − ts
m−n
(4.69)
式 (4.68) を式 (4.63) , 式 (4.64) に代入すると，B スプライン基底関数は次式で得られる．
{
bi,0 (t) =
bi,n (t) =
1
if ti ≤ t < ti+1
0
otherwise
(t − ti )bi,n−1 (t) + (ti+n+1 − t)bi+1,n−1 (t)
nh
(4.70)
(4.71)
以降では，n を B スプラインの次数，m を制御点の個数と呼ぶ．
B スプラインの凸包性
式 (4.70) , 式 (4.71) で定義される B スプライン基底関数 bi,n (t) は次式のように凸包性を持つ．
m−1
∑
bi,n (t) = 1 (ts ≤ t ≤ tf )
(4.72)
i=0
0 ≤ bi,n (t) ≤ 1 (i = 0, 1, · · · , m − 1, ts ≤ t ≤ tf )
(4.73)
47
B スプラインの微分
B スプライン基底関数の微分に関して次式が成り立つ 11 ．
ḃn (t) =
dbn (t)
= Dbn−1 (t)
dt
(4.74)
ただし，
bn (t)
D
def
=
b0,n (t)
b1,n (t)
..
.
∈ Rm
bm−1,n (t)
1 −1
1
1
h
O
def
=
(4.75)
O
−1
..
.
..
.
..
.
∈ Rm×m
−1
1
(4.76)
したがって，k 階微分に関して次式が成り立つ．
b(k)
n (t) =
d(k) bn (t)
= D k bn−k (t)
dt(k)
(4.77)
B スプラインによる関節角軌道の表現
j 番目の関節角軌道 θj (t) を次式で表す．
def
θj (t) =
m−1
∑
pj,i bi,n (t) = pTj bn (t) ∈ R (ts ≤ t ≤ tf )
(4.78)
i=0
ただし，
pj =
pj,0
pj,1
..
.
∈ Rm , bn (t) =
pj,m−1
b0,n (t)
b1,n (t)
..
.
∈ Rm
(4.79)
bm−1,n (t)
以降では，pj を制御点，bn (t) を基底関数と呼ぶ．制御点 pj を決定すると関節角軌道が定まる．制御点 pj を
動作計画の設計変数とする．
j = 1, 2, · · · , Njoint 番目の関節角軌道を並べたベクトル関数は，
T
T
p1 bn (t)
p1
θ1 (t)
T
T
θ2 (t) p2 bn (t) p2
def
=
= . bn (t) = P bn (t) ∈ RNjoint
θ(t) =
..
..
.
.
.
.
θNjoint (t)
pTNjoint bn (t)
(4.80)
pTNjoint
ただし，
def
P =
pT1
pT2
..
.
∈ RNjoint ×m
pTNjoint
11 数学的帰納法で証明できる．http://mat.fsv.cvut.cz/gcg/sbornik/prochazkova.pdf
(4.81)
48
式 (4.80) は，制御点を縦に並べたベクトルとして分離して，以下のようにも表現できる．
θ(t) =
θ1 (t)
θ2 (t)
..
.
T
bn (t)
=
bTn (t)p1
bTn (t)p2
..
.
=
bTn (t)pNjoint
θNjoint (t)
O
bTn (t)
..
.
bTn (t)
O
p1
p2
..
.
= B n (t)p ∈ RNjoint (4.82)
pNjoint
ただし，
B n (t) =
bTn (t)
O
bTn (t)
def
..
.
∈ RNjoint ×mNjoint , p def
=
bTn (t)
O
p1
p2
..
.
∈ RmNjoint
(4.83)
pNjoint
B スプラインによる関節角軌道の微分
式 (4.80) と式 (4.74) から，関節角速度軌道は次式で得られる．
θ̇(t) =
P ḃn (t)
(4.84)
= P Dbn−1 (t)
pT1
.
.
=
. Dbn−1 (t)
pTNjoint
pT1 Dbn−1 (t)
..
=
.
pTNjoint Dbn−1 (t)
T
bn−1 (t)D T p1
..
=
.
bTn−1 (t)D T pNjoint
T
bn−1 (t)D T
O
p1
.
..
.
=
.
.
T
T
O
bn−1 (t)D
pNjoint
T
T
D
O
p1
O
bn−1 (t)
.
..
..
.
=
.
.
.
T
T
O
D
pNjoint
O
bn−1 (t)
(4.85)
= B n−1 (t)D̂ 1 p
(4.91)
(4.86)
(4.87)
(4.88)
(4.89)
(4.90)
ただし，
D̂ 1
=
DT
O
DT
..
O
.
DT
∈ RmNjoint ×mNjoint
(4.92)
49
同様にして，関節角軌道の k 階微分は次式で得られる．
θ (k) (t)
d(k) θ(t)
dt(k)
= P D k bn−k (t)
=
(4.93)
(4.94)
= B n−k (t)D̂ k p
(4.95)
ただし，
D̂ k
=
(D k )T
O
..
.
= (D̂ 1 )k ∈ RmNjoint ×mNjoint
(4.96)
k T
O
(D )
計算時間は式 (4.94) のほうが式 (4.95) より速い．
エンドエフェクタ位置姿勢拘束のタスク関数
関節角 θ ∈ RNjoint からエンドエフェクタ位置姿勢 r ∈ R6 への写像を f (θ) で表す．
r = f (θ)
(4.97)
関節角軌道が式 (4.82) で表現されるとき，エンドエフェクタ軌道は次式で表される．
r(t) = f (θ(t)) = f (B n (t)p)
(4.98)
l = 1, 2, · · · , Ntm について，時刻 tl でエンドエフェクタの位置姿勢が r l であるタスクのタスク関数は次式
で表される．以降では，tl をタイミングと呼ぶ．
e1 (p, t)
r 1 − f (θ(t1 ))
r 1 − f (B n (t1 )p)
e2 (p, t) r 2 − f (θ(t2 )) r 2 − f (B n (t2 )p)
def
∈ R6Ntm
e(p, t) =
=
=
..
..
..
.
.
.
eNtm (p, t)
r Ntm − f (θ(tNtm ))
r Ntm − f (B n (tNtm )p)
(4.99)
ただし，
el (p, t)
t
def
=
def
=
r l − f (θ(tl )) = r l − f (B n (tl )p) ∈ R6 (l = 1, 2, · · · , Ntm )
t1
t2
. ∈ RNtm
.
.
(4.100)
(4.101)
tNtm
このタスクを実現する関節角軌道は，次の評価関数を最小にする制御点 p，タイミング t を求めることで導
出することができる．
F (p, t)
def
=
=
1
∥e(p, t)∥2
2
Ntm
1∑
∥r l − f (θ(tl ))∥2
2
(4.102)
(4.103)
l=1
tm
1∑
∥r l − f (B n (tl )p)∥2
2
N
=
l=1
(4.104)
50
また，l 番目の幾何拘束の許容誤差を etol,l ≥ 0 ∈ R6 とする場合，タスク関数 ẽl (p, t) は次式で表される．
el,i (p, t) − etol,l,i
def
ẽl,i (p, t) =
el,i (p, t) + etol,l,i
0
el,i (p, t) > etol,l,i
el,i (p, t) < −etol,l,i
otherwise
(i = 1, 2, · · · , 6)
(4.105)
ẽl,i (p, t) は ẽl (p, t) の i 番目の要素である．el,i (p, t) は e(p, t) の i 番目の要素である．
タスク関数を制御点で微分したヤコビ行列
式 (4.104) を目的関数とする最適化問題を Gauss-Newton 法，Levenberg-Marquardt 法や逐次二次計画法で
解く場合，タスク関数 (4.99) のヤコビ行列が必要となる．
各時刻でのエンドエフェクタ位置姿勢拘束のタスク関数 el (p, t) の制御点 p に対するヤコビ行列は次式で求
められる．
∂el (p, t)
∂p
∂
{r l − f (B n (tl )p)}
∂p
∂
= − f (B n (tl )p)
∂p
∂f
∂θ
= −
∂θ θ =θ (tl ) ∂p
=
∂
{B n (tl )p}
∂p
= −J (θ(tl ))B n (tl )
= −J (θ(tl ))
(4.106)
(4.107)
(4.108)
(4.109)
(4.110)
途中の変形で，θ(p; t) = B n (t)p であることを利用した．ただし，
def
J =
∂f
∂θ
(4.111)
タスク関数をタイミングで微分したヤコビ行列
各時刻でのエンドエフェクタ位置姿勢拘束のタスク関数 el (p, t) のタイミング t に対するヤコビ行列は次式
で求められる．
∂el (p, t)
∂tl
=
=
=
∂
{r l − f (P bn (tl ))}
∂tl
∂
− f (P bn (tl ))
∂tl
∂f
∂θ
−
∂θ θ =θ (tl ) ∂tl
=
∂
{P bn (tl )}
∂tl
−J (θ(tl ))P ḃn (tl )
=
−J (θ(tl ))P Dbn−1 (tl )
=
−J (θ(tl ))
途中の変形で，θ(p; t) = P bn (t) であることを利用した．
(4.112)
(4.113)
(4.114)
(4.115)
(4.116)
(4.117)
51
初期・終端関節速度・加速度のタスク関数とヤコビ行列
初期，終端時刻の関節速度，加速度はゼロであるべきである．タスク関数は次式となる．
esv (p, t)
ef v (p, t)
esa (p, t)
def
=
θ̇(ts )
(4.118)
=
B n−1 (ts )D̂ 1 p
(4.119)
=
P Dbn−1 (ts )
(4.120)
=
θ̇(tf )
(4.121)
=
B n−1 (tf )D̂ 1 p
(4.122)
=
P Dbn−1 (tf )
(4.123)
=
θ̈(ts )
(4.124)
=
B n−2 (ts )D̂ 2 p
(4.125)
def
def
2
P D bn−2 (ts )
(4.126)
=
θ̈(tf )
(4.127)
=
B n−2 (tf )D̂ 2 p
(4.128)
=
ef a (p, t)
def
=
2
P D bn−2 (tf )
(4.129)
= B n−1 (ts )D̂ 1
(4.130)
= B n−1 (tf )D̂ 1
(4.131)
= B n−2 (ts )D̂ 2
(4.132)
= B n−2 (tf )D̂ 2
(4.133)
制御点で微分したヤコビ行列は次式で表される．
∂esv (p, t)
∂p
∂ef v (p, t)
∂p
∂esa (p, t)
∂p
∂ef a (p, t)
∂p
初期時刻，終端時刻で微分したヤコビ行列は次式で表される．
∂esv (p, t)
∂ts
∂ef v (p, t)
∂tf
∂esa (p, t)
∂ts
∂ef a (p, t)
∂tf
∂bn−1 (ts )
= P D 2 bn−2 (ts )
∂ts
∂bn−1 (tf )
= PD
= P D 2 bn−2 (tf )
∂tf
∂bn−2 (ts )
= P D2
= P D 3 bn−3 (ts )
∂ts
∂bn−2 (tf )
= P D2
= P D 3 bn−3 (tf )
∂tf
= PD
(4.134)
(4.135)
(4.136)
(4.137)
関節角上下限制約
式 (4.78) の関節角軌道定義において，
pj ≤ θmax,j 1m
(4.138)
のとき，B スプラインの凸包性 (式 (4.72) , 式 (4.73) ) より次式が成り立つ．ただし，1m ∈ Rm は全要素が 1
52
の m 次元ベクトルである．
m−1
∑
θj (t) =
pj,i bi,n (t)
(4.139)
θmax,j bi,n (t)
(4.140)
i=0
m−1
∑
≤
i=0
=
θmax,j
m−1
∑
bi,n (t)
(4.141)
i=0
=
θmax,j
(4.142)
同様に，θmin,j 1m ≤ pj とすれば，θmin,j ≤ θj (t) が成り立つ．
したがって，j 番目の関節角の上下限を θmax,j , θmin,j とすると，次式の制約を制御点に課すことで，関節角
上下限制約を満たす関節角軌道が得られる．
θmin,j 1m ≤ pj ≤ θmax,j 1m (j = 1, 2, · · · , Njoint )
(4.143)
つまり，
Êθ min ≤ p ≤ Êθ max
( )
(
)
I
Êθ min
p≥
−I
−Êθ max
⇔
(4.144)
(4.145)
ただし，
Ê =
1m
0m
1m
def
..
.
0m
∈ RmNjoint ×Njoint
(4.146)
1m
これは，逐次二次計画法の中で，次式の不等式制約となる．
(
I
−I
)
(
∆p ≥
)
Êθ min − p
−Êθ max + p
(4.147)
関節角速度・角加速度上下限制約
式 (4.78) と式 (4.74) より，関節角速度軌道，角加速度軌道は次式で表される．
θ̇j (t) =
pTj ḃn (t) = pTj Dbn−1 (t) = (D T pj )T bn−1 (t) ∈ R (ts ≤ t ≤ tf )
θ̈j (t) =
pTj b̈n (t)
=
pTj D 2 bn−2 (t)
= ((D ) pj ) bn−2 (t) ∈ R (ts ≤ t ≤ tf )
2 T
T
(4.148)
(4.149)
j 番目の関節角速度，角加速度の上限を vmax,j , amax,j とする．関節角上下限制約の導出と同様に考えると，
次式の制約を制御点に課すことで，関節角速度・角加速度上下限制約を満たす関節角軌道が得られる．
−vmax,j 1m ≤ D T pj ≤ vmax,j 1m (j = 1, 2, · · · , Njoint )
(4.150)
−amax,j 1m ≤ (D ) pj ≤ amax,j 1m (j = 1, 2, · · · , Njoint )
(4.151)
2 T
53
つまり，
⇔
⇔
−Êv max ≤ D̂ 1 p ≤ Êv max
)
(
)
(
D̂ 1
−Êv max
p≥
−D̂ 1
−Êv max
(4.152)
−Êamax ≤ D̂ 2 p ≤ Êamax
(
)
(
)
D̂ 2
−Êamax
p≥
−D̂ 2
−Êamax
(4.154)
(4.153)
(4.155)
これは，逐次二次計画法の中で，次式の不等式制約となる．
(
(
)
D̂ 1
−D̂ 1
D̂ 2
−D̂ 2
(
∆p ≥
)
(
∆p ≥
−Êv max − D̂ 1 p
)
−Êv max + D̂ 1 p
−Êamax − D̂ 2 p
−Êamax + D̂ 2 p
(4.156)
)
(4.157)
タイミング上下限制約
タイミングが初期，終端時刻の間に含まれる制約は次式で表される．
⇔
⇔
ts ≤ tl ≤ tf (l = 1, 2, · · · , Ntm )
(4.158)
ts 1 ≤ t ≤ tf 1
( )
(
)
I
ts 1
t≥
−I
−tf 1
(4.159)
(4.160)
これは，逐次二次計画法の中で，次式の不等式制約となる．
(
I
−I
)
(
∆t ≥
ts 1 − t
−tf 1 + t
)
(4.161)
(4.162)
また，タイミングの順序が入れ替わることを許容しない場合，その制約は次式で表される．
tl ≤ tl+1 (l = 1, 2, · · · , Ntm − 1)
(4.163)
⇔
−tl + tl+1 ≥ 0 (l = 1, 2, · · · , Ntm − 1)
(4.164)
⇔
D tm t ≥ 0
(4.165)
ただし，
D tm
=
−1
1
−1
O
1
..
O
.
−1
∈ R(Ntm −1)×Ntm
(4.166)
1
これは，逐次二次計画法の中で，次式の不等式制約となる．
D tm ∆t ≥ −D tm t
(4.167)
54
関節角微分二乗積分最小化
関節角微分の二乗積分は次式で得られる．
∫ tf
Fsqr,k (p) =
θ (k) (t)
2
dt
ts
∫ tf
=
2
B n−k (t)D̂ k p
ts
∫ tf
=
(4.168)
(
dt
)T (
B n−k (t)D̂ k p
ts
{∫
tf
= pT
(
(4.169)
)
B n−k (t)D̂ k p dt
)T
B n−k (t)D̂ k
(4.170)
}
B n−k (t)D̂ k dt p
(4.171)
ts
= pT H k p
(4.172)
ただし，
∫
Hk
tf
=
(
)T
B n−k (t)D̂ k
(4.173)
B n−k (t)D̂ k dt
ts
B n−k (t)D̂ k
=
=
=
(
)T
B n−k (t)D̂ k
B n−k (t) =
=
T
k
bn−k (t)
O
(D )T
O
..
..
(4.174)
.
.
O
bTn−k (t)
O
(D k )T
T
bn−k (t)(D k )T
O
..
(4.175)
.
T
k T
O
bn−k (t)(D )
(
)T
k
O
D bn−k (t)
.
.
(4.176)
.
(
)T
O
D k bn−k (t)
(
T (
)T
)T
k
k
D
b
(t)
O
D
b
(t)
O
n−k
n−k
..
..
(4.177)
.
.
(
)T
(
)T
k
k
O
D bn−k (t)
O
D bn−k (t)
(
)(
)T
k
k
O
D bn−k (t) D bn−k (t)
..
(4.178)
.
(
)(
)T
O
D k bn−k (t) D k bn−k (t)
これを逐次二次計画問題において，二次形式の正則化項として目的関数に加えることで，滑らかな動作が生成
されることが期待される．
動作期間の最小化
動作期間 (tf − ts ) の二乗は次式で表される．
2
|t1 − tNtm |
1
−1
= tT
t
−1
1
Fduration (t) =
(4.179)
(4.180)
55
ただし，初期時刻 ts = t1 ，終端時刻 tf = tNtm がタイミングベクトル t の最初，最後の要素であるとする．こ
れを逐次二次計画問題において，二次形式の正則化項として目的関数に加えることで，短時間でタスクを実現
する動作が生成されることが期待される．
4.2.2
B スプラインを用いた関節軌道生成の実装
bspline-configuration-task
:super
:slots
propertied-object
( robot robot instance)
( control-vector p)
( timing-vector t)
( num-kin Nkin := |T kin-trg | = |T kin-att |)
( num-joint Njoint := |J |)
( num-control-point Nctrl )
( num-timing Ntm )
( bspline-order B-spline order, n)
( dim-control-vector dim(p))
( dim-timing-vector dim(t))
( dim-config dim(q))
( dim-task dim(e))
( num-collision Ncol := number of collision check pairs)
( stationery-start-finish-task-scale kstat )
( first-diff-square-integration-regular-scale ksqr ,1 )
( second-diff-square-integration-regular-scale ksqr ,2 )
( third-diff-square-integration-regular-scale ksqr ,3 )
( motion-duration-regular-scale kduration )
( norm-regular-scale-max kmax ,p )
( norm-regular-scale-offset koff ,p )
( timing-norm-regular-scale-max kmax ,t )
( timing-norm-regular-scale-offset koff ,t )
( joint-list J )
( start-time ts )
( finish-time tf )
( kin-time-list {tkin-tm
, tkin-tm
, · · · , tkin-tm
})
1
2
Nkin
( kin-variable-timing-list list of bool. t for variable timing.)
( kin-target-coords-list T kin-trg )
( kin-attention-coords-list T kin-att )
( kin-pos-tolerance-list list of position tolerance etol,pos [m])
( kin-rot-tolerance-list list of rotation tolerance etol,rot [rad])
( joint-angle-margin margin of θ [deg] [mm])
( collision-pair-list list of bodyset-link or body pair)
( keep-timing-order? whether to keep order of timing t or not)
( bspline-matrix buffer for B n (t))
[class]
56
( diff-mat buffer for D k )
( diff-mat-list buffer for {D 1 , D 2 , · · · , D K })
( extended-diff-mat-list buffer for {D̂ 1 , D̂ 2 , · · · , D̂ K })
e)
( task-jacobi buffer for ∂∂q
( regular-mat buffer for W reg )
( regular-vec buffer for v reg )
B スプラインを利用した軌道生成のためのコンフィギュレーション q とタスク関数 e(q) のクラス．
コンフィギュレーション q の取得・更新，タスク関数 e(q) の取得，タスク関数のヤコビ行列
∂ e(q )
∂q
の取
得，コンフィギュレーションの等式・不等式制約 A, b, C, d の取得のためのメソッドが定義されている．
コンフィギュレーション・タスク関数を定めるために，初期化時に以下を与える
• ロボット
robot ロボットのインスタンス
joint-list J 関節
• B スプラインのパラメータ
start-time ts B スプラインの使用区間の初期時刻
finish-time tf B スプラインの使用区間の終端時刻
num-control-point Nctrl 制御点の個数
bspline-order n B スプラインの次数
• 幾何拘束
kin-target-coords-list T kin-trg 幾何到達目標位置姿勢リスト
kin-attention-coords-list T kin-att 幾何到達着目位置姿勢リスト
, · · · , tkin-tm
} 幾何到達タイミングリスト
, tkin-tm
kin-time-list {tkin-tm
2
1
Nkin
kin-variable-timing-list 幾何到達タイミングが可変か (t)，固定か (nil) のリスト．このリスト内
の t の個数がタイミング t の次元 Ntm となる．
コンフィギュレーション q は以下から構成される．
( )
p
q :=
t
(4.181)
p ∈ RNctrl Njoint 制御点 (B スプライン基底関数の山の高さ) [rad] [m]
t ∈ RNtm タイミング (幾何拘束タスクの課される時刻) [sec]
タスク関数 e(q) は以下から構成される．
(
e(q) :=
ekin (q)
)
estat (q)
∈ R6Nkin +4Njoint
ekin (q) ∈ R6Nkin 幾何到達拘束 [rad] [m]
estat (q) ∈ R4Njoint 初期，終端時刻静止拘束 [rad][rad/s][rad/s2 ][m][m/s][m/s2 ]
(4.182)
57
:init &key (name)
(robot)
(joint-list (send robot :joint-list))
[method]
(start-time 0.0)
(finish-time 10.0)
(num-control-point 10)
(bspline-order 3)
(kin-time-list)
(kin-variable-timing-list (make-list (length kin-time-list) :initial-element nil))
(kin-target-coords-list)
(kin-attention-coords-list)
(kin-pos-tolerance-list (make-list (length kin-time-list) :initial-element 0.0))
(kin-rot-tolerance-list (make-list (length kin-time-list) :initial-element 0.0))
(joint-angle-margin 3.0)
(collision-pair-list)
(keep-timing-order? t)
(stationery-start-finish-task-scale 0.0)
(first-diff-square-integration-regular-scale 0.0)
(second-diff-square-integration-regular-scale 0.0)
(third-diff-square-integration-regular-scale 0.0)
(motion-duration-regular-scale 0.0)
(norm-regular-scale-max 1.000000e-05)
(norm-regular-scale-offset 1.000000e-07)
(timing-norm-regular-scale-max 1.000000e-05)
(timing-norm-regular-scale-offset 1.000000e-07)
Initialize instance
:robot
return robot instance
[method]
:joint-list
[method]
return J
:num-kin
[method]
return Nkin := |T kin-trg | = |T kin-att |
:num-joint
return Njoint := |J |
[method]
:num-control-point
return Nctrl
[method]
:num-timing
return Ntm
[method]
:num-collision
[method]
return Ncol := number of collision check pairs
:dim-config
return dim(q) := dim(p) + dim(t) = Nctrl Njoint + Ntm
[method]
58
:dim-task
[method]
return dim(e) := dim(e
kin
) + dim(e
stat
) = 6Nkin + 4Njoint
:control-vector
[method]
return control vector p
:timing-vector
return timing vector t
[method]
:config-vector
[method]
return q :=
( )
p
t
:set-control-vector control-vector-new &key (relative? nil)
[method]
Set p.
:set-timing-vector timing-vector-new &key (relative? nil)
Set t.
[method]
:set-config config-new &key (relative? nil)
Set q.
[method]
:bspline-vector tm &key (order-offset 0)
[method]
bn (t)
b0,n (t)
b1,n (t)
∈ RNctrl
..
.
bNctrl −1,n (t)
:=
(4.183)
return bn (t)
:bspline-matrix tm &key (order-offset 0)
B n (t) :=
[method]
bTn (t)
O
∈ RNjoint ×Nctrl Njoint
bTn (t)
..
.
(4.184)
bTn (t)
O
return B n (t)
:differential-matrix &key (diff-order 1)
[method]
D
:=
1
h
1
O
return D k
−1
1
O
−1
..
.
..
.
..
.
∈ RNctrl ×Nctrl
−1
1
(4.185)
59
:extended-differential-matrix &key (diff-order 1)
D̂ k
:=
k
(D )T
O
[method]
O
..
.
∈ RNctrl Njoint ×Nctrl Njoint
(4.186)
(D k )T
return D̂ k
:bspline-differential-matrix tm &key (diff-order 1)
return B n−k (t)D̂ k ∈ RNjoint ×Nctrl Njoint
[method]
:control-matrix
[method]
P :=
pT1
pT2
..
.
∈ RNjoint ×Nctrl
(4.187)
pTnjoint
return P
:theta tm
[method]
return θ(t) = B n (t)p [rad][m]
:theta-dot tm &key (diff-order 1)
d(k) θ(t)
return θ (k) (t) =
= P D k bn−k (t) [rad/sk ][m/sk ]
dt(k)
[method]
:theta-dot-numerical tm &key (diff-order 1)
(delta-time 0.05)
[method]
return θ (k) (t) =
d(k) θ(t)
θ (k−1) (t + ∆t) − θ (k−1) (t)
=
[rad/sk ][m/sk ]
∆t
dt(k)
:apply-theta-to-robot tm
apply θ(t) to robot.
[method]
:kin-target-coords-list
[method]
kin-trg
Tm
= {pkin-trg
, Rkin-trg
} (l = 1, 2, · · · , Nkin )
l
l
(4.188)
return T kin-trg := {T1kin-trg , T2kin-trg , · · · , TNkin-trg
}
kin
:kin-attention-coords-list
[method]
kin-att
Tm
= {pkin-att
, Rkin-att
} (l = 1, 2, · · · , Nkin )
l
l
(4.189)
return T kin-att := {T1kin-att , T2kin-att , · · · , TNkin-att
}
kin
:kin-start-time
return tkin
:= tkin-tm
s
1
[method]
60
:kin-finish-time
return
tkin
f
[method]
:=
tkin-tm
Nkin
:motion-duration
[method]
return (tkin-tm
− tkin-tm
)
1
Nkin
:kinematics-task-value &key (update? t)
ekin (q)
ekin
l (p, t)
[method]
ekin (p, t)
kin
e1 (p, t)
kin
e2 (p, t)
=
..
.
kin
eNkin (p, t)
(
)
pkin-trg
− pkin-att
(p, t) )
l
l
(
=
∈ R6 (l = 1, 2, · · · , Nkin )
kin-att
T
a Rkin-trg
R
(p,
t)
l
l
=
(4.190)
(4.191)
(4.192)
a(R) は姿勢行列 R の等価角軸ベクトルを表す．
return ekin (q) ∈ R6Nkin
:stationery-start-finish-task-value &key (update? t)
[method]
estat (q) =
estat (p, t)
estat
sv (p, t)
stat
ef v (p, t)
= stat
esa (p, t)
estat
f a (p, t)
(4.193)
(4.194)
estat
sv (p, t)
:=
θ̇(tkin
s )
(4.195)
estat
f v (p, t)
:=
θ̇(tkin
f )
(4.196)
estat
sa (p, t)
:=
θ̈(tkin
s )
(4.197)
estat
f a (p, t)
:=
θ̈(tkin
f )
(4.198)
return estat (q) ∈ R4Njoint
:task-value &key (update?
t)
(
)
kin
e (q)
return e(q) :=
∈ R6Nkin +4Njoint
kstat estat (q)
[method]
:kinematics-task-jacobian-with-control-vector
式 (4.110) より，タスク関数 ekin を制御点 p で微分したヤコビ行列は次式で得られる．
kin
∂ e1
∂ pkin
∂ e2
∂p
∂ekin
=
..
∂p
.
kin
∂ eN
kin
∂p
∂ekin
l
= −J kin-att (θ(tkin-tm
))B n (tkin-tm
) (l = 1, 2, · · · , Nkin )
l
l
∂p
[method]
(4.199)
(4.200)
61
return
∂ekin
∈ R6Nkin ×Nctrl Njoint
∂p
:kinematics-task-jacobian-with-timing-vector
[method]
kin
式 (4.117) より，タスク関数 e
をタイミング t で微分したヤコビ行列は次式で得られる．
∂ekin
∂t
∂ ekin
l
∂t
[
の i 番目の列ベクトル
[
return
∂ekin
l
∂t
{
]
=
i
]
∂ ekin
l
∂t
i
∂ ekin
1
∂t
kin
∂e
2
∂t
= .
..
∂ ekin
Nkin
∂t
(4.201)
∈ R6 は次式で表される (i = 1, 2, · · · , Ntm )．
)
))P Dbn−1 (tkin-tm
−J kin-att (θ(tkin-tm
l
l
tkin-tm
and ti is identical
l
0
otherwise
(4.202)
∂ekin
∈ R6Nkin ×Ntm
∂t
:stationery-start-finish-task-jacobian-with-control-vector
式 (4.130) , 式 (4.131) , 式 (4.132) , 式 (4.133) より，タスク関数 e
[method]
stat
を制御点 p で微分したヤコビ行
列は次式で得られる．
∂estat
∂p
∂estat
sv (p, t)
∂p
∂estat
f v (p, t)
∂p
∂estat
sa (p, t)
∂p
∂estat
f a (p, t)
∂p
return
∂ estat
sv
∂p
∂e
stat
fv
∂p
= ∂e
stat
sa
∂p
∂ estat
fa
∂p
(4.203)
= B n−1 (tkin
s )D̂ 1
(4.204)
= B n−1 (tkin
f )D̂ 1
(4.205)
= B n−2 (tkin
s )D̂ 2
(4.206)
= B n−2 (tkin
f )D̂ 2
(4.207)
∂estat
∈ R4Njoint ×Nctrl Njoint
∂p
:stationery-start-finish-task-jacobian-with-timing-vector
[method]
stat
式 (4.134) , 式 (4.135) , 式 (4.136) , 式 (4.137) より，タスク関数 e
をタイミング t で微分したヤコ
ビ行列は次式で得られる．
∂estat
∂t
∂ estat
sv
t
∂ e∂stat
fv
∂t
=
∂ estat
∂sa
t
stat
∂ ef a
∂t
(4.208)
62
∂ estat
x
∂t
[
の i 番目の列ベクトル
[
[
[
[
return
]
∂ estat
x
∂t
i
∂estat
sv (p, t)
∂t
∂estat
f v (p, t)
∂t
∂estat
sa (p, t)
∂t
∂estat
f a (p, t)
∂t
∈ RNjoint は次式で表される (x ∈ {sv, f v, sa, f a}, i = 1, 2, · · · , Ntm )．
{
]
=
i
{
]
=
]
{
i
=
i
]
{
=
kin
P D 2 bn−2 (tkin
and ti is identical
s ) ts
0
otherwise
(4.209)
kin
P D 2 bn−2 (tkin
and ti is identical
f ) tf
0
otherwise
(4.210)
kin
P D 3 bn−3 (tkin
and ti is identical
s ) ts
0
otherwise
kin
and ti is identical
P D 3 bn−3 (tkin
f ) tf
0
i
otherwise
(4.211)
(4.212)
∂estat
∈ R4Njoint ×Ntm
∂t
:task-jacobian
[method]
Nctrl Njoint
∂ ekin
∂p
stat
kstat ∂ e
∂p
∂e
∂q
=
6Nkin
4Njoint
Ntm
∂ ekin
∂t
stat
kstat ∂ e∂ t
(4.213)
e = R(6Nkin +4Njoint )×(Nctrl Njoint +Ntm )
return ∂∂ q
:theta-max-vector &key (update? nil)
return θ max ∈ RNjoint
[method]
:theta-min-vector &key (update? nil)
return θ min ∈ RNjoint
[method]
:theta-inequality-constraint-matrix &key (update? nil)
式 (4.144) より，関節角度上下限制約は次式で表される．
[method]
⇔
⇔
Êθ min ≤ p + ∆p ≤ Êθ max
( )
(
)
I
Êθ min − p
∆p ≥
−I
−Êθ max + p
(4.214)
Cθ ∆p ≥ dθ
(4.216)
(4.215)
ただし，
Ê :=
1Nctrl
0Nctrl
1Nctrl
..
0Nctrl
.
∈ RNctrl Njoint ×Njoint
(4.217)
1Nctrl
1Nctrl ∈ RNctrl は全要素が 1 の Nctrl 次元ベクトルである．
( )
I
return Cθ :=
∈ R2Nctrl Njoint ×Nctrl Njoint
−I
:theta-inequality-constraint-vector
&key (update? t)
(
)
Êθ min − p
return dθ :=
∈ R2Nctrl Njoint
−Êθ max + p
[method]
63
:velocity-max-vector &key (update? nil)
[method]
return v max ∈ R
Njoint
:velocity-inequality-constraint-matrix &key (update? nil)
[method]
式 (4.152) より，関節速度上下限制約は次式で表される．
⇔
⇔
(
return Cθ̇ :=
D̂ 1
−D̂ 1
−Êv max ≤ D̂ 1 (p + ∆p) ≤ Êv max
(
)
(
)
D̂ 1
−Êv max − D̂ 1 p
∆p ≥
−D̂ 1
−Êv max + D̂ 1 p
(4.218)
Cθ̇ ∆p ≥ dθ̇
(4.220)
(4.219)
)
∈ R2Nctrl Njoint ×Nctrl Njoint
:velocity-inequality-constraint-vector
(
) &key (update? t)
−Êv max − D̂ 1 p
return dθ̇ :=
∈ R2Nctrl Njoint
−Êv max + D̂ 1 p
[method]
:acceleration-max-vector &key (update? nil)
return amax ∈ RNjoint
[method]
:acceleration-inequality-constraint-matrix &key (update? nil)
式 (4.154) より，関節加速度上下限制約は次式で表される．
[method]
⇔
⇔
(
return Cθ̈ :=
D̂ 2
−D̂ 2
−Êamax ≤ D̂ 2 (p + ∆p) ≤ Êamax
(
)
(
)
D̂ 2
−Êamax − D̂ 2 p
∆p ≥
−D̂ 2
−Êamax + D̂ 2 p
(4.221)
Cθ̈ ∆p ≥ dθ̈
(4.223)
(4.222)
)
∈ R2Nctrl Njoint ×Nctrl Njoint
:acceleration-inequality-constraint-vector
&key (update? t)
(
)
−Êamax − D̂ 2 p
return dθ̈ :=
∈ R2Nctrl Njoint
−Êamax + D̂ 2 p
[method]
:control-vector-inequality-constraint-matrix &key (update? nil)
[method]
⇔
⇔
Cθ
Cθ ∆p ≥ dθ
Cθ̇ ∆p ≥ dθ̇
Cθ̈ ∆p ≥ dθ̈
dθ
Cθ
Cθ̇ ∆p ≥ dθ̇
dθ̈
Cθ̈
Cp ∆p ≥ dp
(4.224)
(4.225)
(4.226)
return Cp := Cθ̇ ∈ RNp-ineq ×dim(p)
Cθ̈
:control-vector-inequality-constraint-vector &key (update? t)
[method]
64
dθ
return dp := dθ̇ ∈ RNp-ineq
dθ̈
:timing-vector-inequality-constraint-matrix &key (update? nil)
[method]
式 (4.159) より，タイミングが B スプラインの初期，終端時刻の間に含まれる制約は次式で表される．
ts 1 ≤ t + ∆t ≤ tf 1
( )
(
)
I
ts 1 − t
∆t ≥
−I
−tf 1 + t
⇔
(4.227)
(4.228)
また，式 (4.165) より，タイミングの順序が入れ替わることを許容しない場合，その制約は次式で表さ
れる．
⇔
D tm (t + ∆t) ≥ 0
(4.229)
D tm ∆t ≥ −D tm t
(4.230)
ただし，
D tm
−1
=
1
−1 1
O
..
.
−1
O
∈ R(Ntm −1)×Ntm
(4.231)
1
これらを合わせると，
I
ts 1 − t
−I ∆t ≥ −tf 1 + t ⇔
D tm
−D tm t
Ct ∆p ≥ dt
(4.232)
I
return Ct := −I ∈ R(3Ntm −1)×dim(t)
D tm
:timing-vector-inequality-constraint-vector &key (update? t)
ts 1 − t
return dt := −tf 1 + t ∈ R(3Ntm −1)
[method]
−D tm t
:config-inequality-constraint-matrix &key (update? nil)
(update-collision? nil)
[method]
{
⇔
⇔
(
return C :=
Cp ∆p ≥ dp
Ct ∆t ≥ dt
(
)( )
Cp
∆p
Ct
C∆q ≥ d
)
Cp
Ct
∈ RNineq ×dim(q )
∆t
(4.233)
≥
( )
dp
dt
(4.234)
(4.235)
65
:config-inequality-constraint-vector &key (update? t)
(update-collision? nil)
( )
dp
∈ RNineq
return d :=
dt
[method]
:config-equality-constraint-matrix &key (update? nil)
return A ∈ R0×dim(q ) (no equality constraint)
[method]
:config-equality-constraint-vector &key (update? t)
[method]
return b ∈ R (no equality constraint)
0
:square-integration-regular-matrix &key (diff-order 1)
(delta-time (/ (- finish-time start-time) 100.0))
[method]
式 (4.172) より，関節角微分の二乗積分は次式で得られる．
∫
Fsqr,k (p)
tf
=
θ (k) (t)
2
dt
(4.236)
ts
T
= p H sqr ,k p
(4.237)
ただし，
∫
H sqr ,k
tf
=
ts
∫
tf
=
ts
(
)T
B n−k (t)D̂ k B n−k (t)D̂ k dt
(
)(
)T
k
k
D
b
(t)
D
b
(t)
n−k
n−k
O
(4.238)
O
..
.
(
)(
D k bn−k (t) D k bn−k (t)
dt(4.239)
)T
これは二次形式の正則化項である．
return H sqr ,k ∈ Rdim(p)×dim(p)
:first-differential-square-integration-regular-matrix &key (delta-time (/ (- finish-time start-time)
100.0))
return H sqr ,1 ∈ Rdim(p)×dim(p)
[method]
:second-differential-square-integration-regular-matrix &key (delta-time (/ (- finish-time start-time)
100.0))
[method]
dim(p)×dim(p)
return H
∈R
sqr ,2
:third-differential-square-integration-regular-matrix &key (delta-time (/ (- finish-time start-time)
100.0))
return H sqr ,3 ∈ Rdim(p)×dim(p)
[method]
:control-vector-regular-matrix
[method]
W reg,p := min(kmax ,p , ∥e∥2 + koff ,p )I + ksqr ,1 H sqr ,1 + ksqr ,2 H sqr ,2 + ksqr ,3 H sqr ,3
(4.240)
return W reg,p ∈ Rdim(p)×dim(p)
:control-vector-regular-vector
[method]
66
v reg,p := (ksqr ,1 H sqr ,1 + ksqr ,2 H sqr ,2 + ksqr ,3 H sqr ,3 )p
(4.241)
return v reg,p ∈ Rdim(p)
:motion-duration-regular-matrix
式 (4.180) より，動作期間の二乗は次式で得られる．
Fduration (t) =
=
=
[method]
2
|t1 − tNtm |
1
−1
tT
t
−1
1
(4.242)
tT H duration t
(4.244)
(4.243)
これは二次形式の正則化項である．
return H duration ∈ Rdim(t)×dim(t)
:timing-vector-regular-matrix
[method]
W reg,t := min(kmax ,t , ∥e∥2 + koff ,t )I + kduration H duration
(4.245)
return W reg,t ∈ Rdim(t)×dim(t)
:timing-vector-regular-vector
[method]
v reg,t := kduration H duration t
(4.246)
return v reg,t ∈ Rdim(t)
:regular-matrix
[method]
(
W reg :=
)
W reg,p
W reg,t
(4.247)
return W reg ∈ Rdim(q )×dim(q )
:regular-vector
[method]
v reg
(
)
v reg,p
:=
v reg,t
(4.248)
return v reg ∈ Rdim(q )×dim(q )
:update-collision-inequality-constraint
[method]
Not implemented yet.
:update-viewer &key (trajectory-delta-time (/ (- finish-time start-time) 10.0))
Update viewer.
[method]
67
:print-status
[method]
Print status.
:print-motion-information
[method]
Print motion information.
:play-animation &key (robot)
[method]
(delta-time (/ (- finish-time start-time) 100.0))
(only-motion-duration? t)
(loop? t)
(visualize-callback-func)
Print motion animation.
:plot-theta-graph &key (joint-id nil)
[method]
(divide-num 200)
(plot-numerical? nil)
(only-motion-duration? t)
(dat-filename /tmp/bspline-configuration-task-plot-theta-graph.dat)
(dump-pdf ? nil)
(dump-filename (ros::resolve-ros-path package://eus qp/optmotiongen/logs/bspline-configur
Plot graph.
:generate-angle-vector-sequence &key (divide-num 100)
Generate angle-vector-sequence.
[method]
get-bspline-knot i n m x min x max h
[function]
ti
i−n
(tf − ts ) + ts
m−n
mts − ntf
= hi +
m−n
=
(4.249)
(4.250)
return knot ti for B-spline function
bspline-basis-func x i n m x min x max &optional (n-orig n) (m-orig m)
{
bi,0 (t)
=
bi,n (t)
=
1 if ti ≤ t < ti+1
0 otherwise
(t − ti )bi,n−1 (t) + (ti+n+1 − t)bi+1,n−1 (t)
nh
[function]
(4.251)
(4.252)
return B-spline function value bi,n (t).
5
5.1
補足
既存のロボット基礎クラスの拡張
joint
[class]
68
:super
:slots
propertied-object
(parent-link)
(child-link)
(joint-angle)
(min-angle)
(max-angle)
(default-coords)
(joint-velocity)
(joint-acceleration)
(joint-torque)
(max-joint-velocity)
(max-joint-torque)
(joint-min-max-table)
(joint-min-max-target)
:child-link &rest args
[method]
Returns child link of this joint. If any arguments is set, it is passed to the child-link.
Override to support the case that child-link is cascaded-link instantiate. Return the root link of child
cascaded-link instantiate in that case.
:axis-vector
Return joint axis vector. Represented in world coordinates.
[method]
return ai ∈ R3
:pos
[method]
Return joint position. Represented in world coordinates.
return pi ∈ R3
bodyset-link
[class]
:super
:slots
bodyset
(rot)
(pos)
(parent)
(descendants)
(worldcoords)
(manager)
(changed)
(geometry::bodies)
(joint)
(parent-link)
(child-links)
(analysis-level)
(default-coords)
(weight)
(acentroid)
69
(inertia-tensor)
(angular-velocity)
(angular-acceleration)
(spacial-velocity)
(spacial-acceleration)
(momentum-velocity)
(angular-momentum-velocity)
(momentum)
(angular-momentum)
(force)
(moment)
(ext-force)
(ext-moment)
:mg
[method]
return mg = ∥mg∥
:mg-vec
return mg
[method]
cascaded-link
[class]
:super
cascaded-coords
:slots
(rot)
(pos)
(parent)
(descendants)
(worldcoords)
(manager)
(changed)
(links)
(joint-list)
(bodies)
(collision-avoidance-links)
(end-coords-list)
:calc-jacobian-from-joint-list &key (union-joint-list)
[method]
(move-target)
(joint-list (mapcar #’(lambda (mt) (send-all (send self :link-list (send mt :par
(transform-coords (mapcar #’(lambda (mt) (make-coords)) move-target))
(translation-axis (mapcar #’(lambda (mt) t) move-target))
(rotation-axis (mapcar #’(lambda (mt) t) move-target))
union-joint-list list of all joints considered in jacobian. column num of jacobian is same with length of
70
union-joint-list.
move-target list of move-target.
joint-list list of joint-list which is contained in each chain of move-target.
transform-coords list of transform-coords of each move-target.
translation-axis list of translation-axis of each move-target.
rotation-axis list of rotation-axis of each move-target.
Get jacobian matrix from following two information: (1) union-joint-list and (2) list of move-target.
One recession compared with :calc-jacobian-from-link-list is that child-reverse is not supported. (Only
not implemented yet because I do not need such feature in current application.)
:calc-cog-jacobian-from-joint-list &key (union-joint-list)
(update-mass-properties t)
[method]
(translation-axis :z)
union-joint-list list of all joints considered in jacobian. column num of jacobian is same with length of
union-joint-list.
Get CoG jacobian matrix from union-joint-list.
:find-link-route to &optional from
[method]
Override to support the case that joint does not exist between links. Change from (send to :parent-link)
to (send to :parent).
5.2
環境と接触するロボットの関節・リンク構造
2d-planar-contact
[class]
:super
cascaded-link
:slots
( contact-coords Tcnt )
( contact-pre-coords Tcnt-pre )
二次元平面上の長方形領域での接触座標を表す仮想の関節・リンク構造．
:init &key (name contact)
[method]
(contact-pre-offset 100)
Initialize instance
:contact-coords &rest args
return Tcnt := {pcnt , Rcnt }
[method]
:contact-pre-coords &rest args
return Tcnt-pre := {pcnt-pre , Rcnt-pre }
[method]
:set-from-face &key (face)
(margin 150.0)
[method]
set coords and min/max joint angle from face.
71
robot-environment
[class]
:super
cascaded-link
:slots
( robot R)
( robot-with-root-virtual R̂)
( root-virtual-joint-list list of root virtual joint)
( contact-list {C1 , C2 , · · · , CNC })
( variant-joint-list Jvar )
( invariant-joint-list Jinvar )
( drive-joint-list Jdrive )
ロボットとロボット・環境間の接触のクラス．
以下を合わせた関節・リンク構造に関するメソッドが定義されている．
1. 浮遊ルートリンクのための仮想関節付きのロボットの関節
2. 接触位置を定める仮想関節
関節・リンク構造を定めるために，初期化時に以下を与える
robot R ロボット (cascaded-link クラスのインスタンス)．
contact-list {C1 , C2 , · · · , CNC } 接触 (2d-planar-contact クラスなどのインスタンス) のリスト．
ロボット R に，浮遊ルートリンクの変位に対応する仮想関節を付加した仮想関節付きロボット R̂ を内部
で保持する．
:init &key (robot)
[method]
(contact-list)
(root-virtual-mode :6dof )
(root-virtual-joint-class-list)
(root-virtual-joint-axis-list)
Initialize instance
:dissoc-root-virtual
dissoc root virtual parent/child structure.
[method]
:init-pose
set zero joint angle.
[method]
:robot &rest args
return R
[method]
:robot-with-root-virtual &rest args
[method]
return R̂
:contact-list &rest args
[method]
return {C1 , C2 , · · · , CNC }
:contact name &rest args
return Ci
[method]
:variant-joint-list &optional (jl :nil)
[method]
72
return Jvar
:invariant-joint-list &optional (jl :nil)
return Jinvar
[method]
:drive-joint-list &optional (jl :nil)
[method]
return Jdrive
:root-virtual-joint-list
[method]
return list of root virtual joint
5.3
irteus の inverse-kinematics 互換関数
cascaded-link
:super
:slots
[class]
cascaded-coords
(rot)
(pos)
(parent)
(descendants)
(worldcoords)
(manager)
(changed)
(links)
(joint-list)
(bodies)
(collision-avoidance-links)
(end-coords-list)
:inverse-kinematics-optmotiongen target-coords &key (stop 50)
[method]
(link-list)
(move-target)
(debug-view)
(revert-if-fail t)
(transform-coords target-coords)
(translation-axis (cond ((atom move-target) t) (t (make-li
(rotation-axis (cond ((atom move-target) t) (t (make-list (
(thre (cond ((atom move-target) 1) (t (make-list (length m
(rthre (cond ((atom move-target) (deg2rad 1)) (t (make-li
(collision-avoidance-link-pair :nil)
(collision-distance-limit 10.0)
(obstacles)
(min-loop)
(root-virtual-mode :fix)
73
(joint-angle-margin 0.0)
(posture-joint-list)
(posture-joint-angle-list)
(target-posture-scale 0.001)
(norm-regular-scale-max 0.01)
(norm-regular-scale-offset 1.000000e-07)
(pre-process-func)
(post-process-func)
&allow-other-keys
Solve inverse kinematics problem with sqp optimization. ;; target-coords, move-target, rotation-axis,
translation-axis ;; -¿ both list and atom OK. target-coords : The coordinate of the target that returns
coordinates. Use a list of targets to solve the IK relative to multiple end links simultaneously. Function
is not available to target-coords. link-list : List of links to control. When the target-coords is list, this
should be a list of lists. move-target : Specify end-effector coordinate. When the target-coords is list,
this should be list too. stop : Maximum number for IK iteration. Default is 50. debug-view : Set t
to show debug message and visualization. Use :no-message to just show the irtview image. Default is
nil. revert-if-fail : Set nil to keep the angle posture of IK solve iteration. Default is t, which return
to original position when IK fails. translation-axis : :x :y :z for constraint along the x, y, z axis. :xy
:yz :zx for plane. Default is t. rotation-axis : Use nil for position only IK. :x, :y, :z for the constraint
around axis with plus direction. When the target-coords is list, this should be list too. Default is t.
thre : Threshold for position error to terminate IK iteration. Default is 1 [mm]. rthre : Threshold for
rotation error to terminate IK iteration. Default is 0.017453 [rad] (1 deg).
cascaded-link
[class]
:super
cascaded-coords
:slots
(rot)
(pos)
(parent)
(descendants)
(worldcoords)
(manager)
(changed)
(links)
(joint-list)
(bodies)
(collision-avoidance-links)
(end-coords-list)
:inverse-kinematics-trajectory-optmotiongen target-coords-list &key (stop 50)
(move-target-list)
[method]
(debug-view)
(revert-if-fail t)
(transform-coords-list :nil)
(translation-axis-list :nil)
74
(rotation-axis-list :nil)
(thre 1.0)
(rthre (deg2rad 1))
(thre-list :nil)
(rthre-list :nil)
(collision-avoidance-link-pair :nil)
(collision-distance-limit 10.0)
(obstacles)
(min-loop)
(root-virtual-mode :fix)
(root-virtual-joint-invariant? nil)
(joint-angle-margin 0.0)
(posture-joint-list (make-list (length target
(posture-joint-angle-list (make-list (length
(norm-regular-scale-max 0.001)
(norm-regular-scale-offset 1.000000e-07)
(adjacent-regular-scale 0.0)
(pre-process-func)
(post-process-func)
&allow-other-keys
Solve inverse kinematics problem with sqp optimization. target-coords-list : The coordinate of the
target that returns coordinates. Use a list of targets to solve the IK relative to multiple end links
simultaneously. Function is not available to target-coords. move-target-list : Specify end-effector
coordinate. When the target-coords is list, this should be list too. stop : Maximum number for IK
iteration. Default is 50. debug-view : Set t to show debug message and visualization. Use :no-message
to just show the irtview image. Default is nil. revert-if-fail : Set nil to keep the angle posture of IK
solve iteration. Default is t, which return to original position when IK fails. translation-axis-list : :x :y
:z for constraint along the x, y, z axis. :xy :yz :zx for plane. Default is t. rotation-axis-list : Use nil for
position only IK. :x, :y, :z for the constraint around axis with plus direction. When the target-coords is
list, this should be list too. Default is t. thre : Threshold for position error to terminate IK iteration.
Default is 1 [mm]. rthre : Threshold for rotation error to terminate IK iteration. Default is 0.017453
[rad] (1 deg).
robot-model
[class]
:super
cascaded-link
:slots
(rot)
(pos)
(parent)
(descendants)
(worldcoords)
(manager)
(changed)
(links)
(joint-list)
(bodies)
(collision-avoidance-links)
75
(end-coords-list)
(larm-end-coords)
(rarm-end-coords)
(lleg-end-coords)
(rleg-end-coords)
(head-end-coords)
(torso-end-coords)
(larm-root-link)
(rarm-root-link)
(lleg-root-link)
(rleg-root-link)
(head-root-link)
(torso-root-link)
(larm-collision-avoidance-links)
(rarm-collision-avoidance-links)
(larm)
(rarm)
(lleg)
(rleg)
(torso)
(head)
(force-sensors)
(imu-sensors)
(cameras)
(support-polygons)
:limb limb method &rest args
[method]
Extend to support to call :inverse-kinematics-optmotiongen.
contact-ik-arg
:super
:slots
[class]
cascaded-link
( contact-coords Tcnt )
inverse-kinematics-optmotiongen の target-coords, translation-axis, rotation-axis, transform-coords 引
数に対応する接触座標を表す仮想の関節・リンク構造．
:init &key (target-coords)
(translation-axis)
[method]
(rotation-axis)
(transform-coords target-coords)
(name (send target-coords :name))
Initialize instance
:contact-coords &rest args
[method]
76
return Tcnt := {pcnt , Rcnt }
ik-arg-axis->axis-list ik-arg-axis
Convert translation-axis / rotatoin-axis to axis list.
[function]
generate-contact-ik-arg-from-rect-face &key (rect-face)
(name (send rect-face :name))
(margin (or (send rect-face :get :margin) 0))
[function]
Generate contact-ik-arg instance from rectangle face.
generate-contact-ik-arg-from-line-segment &key (line-seg)
(name (send line-seg :name))
[function]
(margin (or (send line-seg :get :margin) 0))
Generate contact-ik-arg instance from line segment.
axis->index axis
[function]
axis->sgn axis
[function]
5.4
関節トルク勾配の計算
get-link-jacobian-for-contact-torque &key (robot)
[function]
(drive-joint-list)
(contact-coords)
(contact-parent-link)
contact-coords に対応する接触部位の番号を m とする．contact-coords の位置姿勢を r m ∈ R6 ，drivejoint-list の関節角度ベクトルを ψ ∈ RNdrive-joint として，次式を満たすヤコビ行列 J m を返す．
ṙ m
= J m ψ̇
(5.1)
∑
Ndrive-joint
=
(
j (i)
m
=
Jm
=
(
j (i)
m ψ̇i
(5.2)
i=1
aψi × (pm − pψi )
aψi
j (1)
m
j (2)
m
···
)
drive-joint
jN
m
(5.3)
)
(5.4)
aψi , pψi ∈ R3 は i 番目の関節の回転軸ベクトルと位置である．
return J m ∈ R6×Ndrive-joint
get-contact-torque &key (robot)
(drive-joint-list)
(wrench-list)
[function]
(contact-target-coords-list)
(contact-attention-coords-list)
ロボットの接触部位に加わる接触レンチによって生じる関節トルク τ cnt は，以下で得られる．
τ cnt =
N
cnt
∑
J Tm wm
m=1
wm は m 番目の接触部位で受ける接触レンチである．
return τ cnt ∈ RNdrive-joint
(5.5)
77
get-contact-torque-jacobian &key (robot)
(joint-list)
(drive-joint-list)
[function]
(wrench-list)
(contact-target-coords-list)
(contact-attention-coords-list)
式 (5.4) の J m を以下のように分解して利用する．
(
Jm
)T
∂rx ,m
∂ψ
(
∂ry,m )T
j Tx ,m
T ∂ψ
j y,m (
)
T
∂rz ,m
jT
z ,m ∂ ψ
= T = (
)T
j R,m ∂rR,m
∂ψ
jT
(
)
P,m ∂rP,m T
∂ψ
j TY ,m
(
)T
(5.6)
∂rY ,m
∂ψ
これを式 (5.5) に代入すると，
τ cnt
=
N
cnt
∑
J Tm wm
(5.7)
m=1
=
N
cnt
∑
(
j x,m
j y,m
j z,m
j R,m
j P,m
m=1
=
N
cnt
∑
m=1
=
N
cnt
∑
m=1
(
fx,m
fy,m
) f
z,m
j Y,m
nx,m
n
y,m
nz,m
(5.8)
j x,m fx,m + j y,m fy,m + j z,m fz,m + j R,m nx,m + j P,m ny,m + j Y,m nz,m
(
)
(5.9)
)
∂rx,m
∂ry,m
∂rz,m
∂rR,m
∂rP,m
∂rY,m
(5.10)
fx,m +
fy,m +
fz,m +
nx,m +
ny,m +
nz,m
∂ψ
∂ψ
∂ψ
∂ψ
∂ψ
∂ψ
joint-list の関節角度ベクトルを θ ∈ RNjoint ，drive-joint-list の関節角度ベクトルを ψ ∈ RNdrive-joint とす
cnt
る．トルク勾配行列 ∂ τ は次式で得られる．
∂θ
)T
N
cnt (
∑
∂τ cnt
∂J m
=
(5.11)
wm
∂θ
∂θ
m=1
)
N
cnt (
∑
∂ 2 rx,m
∂ 2 ry,m
∂ 2 rz,m
∂ 2 rR,m
∂ 2 rP,m
∂ 2 rY,m
=
fx,m
+ fy,m
+ fz,m
+ nx,m
+ ny,m
+ nz,m
(5.12)
∂ψ∂θ
∂ψ∂θ
∂ψ∂θ
∂ψ∂θ
∂ψ∂θ
∂ψ∂θ
m=1
∂ 2 rx,m
∑
M (
=
fx,m
m=1
∂ψ ∂θ
fy,m
cnt
return ∂ τ ∈ RNdrive-joint ×Njoint
∂θ
fz,m
nx,m
ny,m
∂ 2 riy,mj
∂ψ
i ∂θj
2
r
∂
z,m
)
∂ψi ∂θj
nz,m
∂ 2 rR,m
∂ψi ∂θj
∂ 2 r
P,m
∂ψi ∂θj
∂ 2 rY,m
∂ψi ∂θj
(5.13)
i=1,··· ,Ndrive-joint ,j=1,··· ,Njoint
78
get-link-jacobian-for-gravity-torque &key (robot)
(drive-joint-list)
(gravity-link)
[function]
gravity-link のリンク番号を k とする．gravity-link の重心位置を pcog,k ∈ R3 ，drive-joint-list の関節角
度ベクトルを ψ ∈ RNdrive-joint として，次式を満たすヤコビ行列 J cog,k を返す．
ṗcog,k
= J cog,k ψ̇
Nk
∑
=
(5.14)
(i)
(5.15)
j cog,k ψ̇i
i=1
{
(i)
j cog,k
=
aψi × (pcog,k − pψi ) (1 ≤ i ≤ Nk )
03
(Nk + 1 ≤ i ≤ Ndrive-joint )
(5.16)
aψi , pψi ∈ R3 は i 番目の関節の回転軸ベクトルと位置である．gravity-link よりもルート側にある関節の
番号を 1, · · · , Nk ，gravity-link よりも末端側にある関節の番号を Nk + 1, · · · , Ndrive-joint とする．リン
クの重心位置と関節角度の依存関係から，ヤコビ行列の右には次式のように零ベクトルが並ぶ．
(
J cog,k
=
=
(
j cog,k
(1)
···
k
j cog,k
(1)
···
k
j cog,k
j cog,k
(N )
k
j cog,k
(N +1)
(N )
0 ···
···
)
0
N
drive-joint
j cog,k
)
(5.17)
(5.18)
return J cog,k ∈ R3×Ndrive-joint
get-gravity-torque &key (robot)
[function]
(drive-joint-list)
(gravity-link-list)
ロボットのリンク自重によって生じる関節トルク τ grav は，ロボットモーション P111 式 (3.3.22) より以
下で得られる．
∑
Ngravity-link
τ grav =
mk J Tcog,k g
(5.19)
k=1
mk は k 番目のリンクの質量である．
return τ grav ∈ RNdrive-joint
get-gravity-torque-jacobian &key (robot)
(joint-list)
[function]
(drive-joint-list)
(gravity-link-list)
式 (5.18) の J cog,k を以下のように分解して利用する．
J cog,k =
j Tcog,x ,k
T
j cog,y,k
j Tcog,z ,k
(
∂pcog,x ,k
∂ψ
)T
(
)T
∂p
cog,y,k
=
( ∂ ψ )T
∂pcog,z ,k
∂ψ
(5.20)
79
これを式 (5.19) に代入すると，
τ grav
=
∑
Ngravity-link
mk J Tcog,k g
(5.21)
k=1
∑
Ngravity-link
=
(
)
mk j cog,x ,k
j cog,y,k
k=1
∑
j cog,z ,k
0
0
g
(5.22)
Ngravity-link
=
mk gj cog,z ,k
(5.23)
k=1
∑
Ngravity-link
=
mk g
k=1
∂pcog,z ,k
∂ψ
(5.24)
joint-list の関節角度ベクトルを θ ∈ RNjoint ，drive-joint-list の関節角度ベクトルを ψ ∈ RNdrive-joint とす
grav
る．トルク勾配行列 ∂ τ
は次式で得られる．これは対称行列である．
∂θ
∑
Ngravity-link
∂τ grav
∂θ
=
∂ 2 pcog,z ,k
∂ψ∂θ
=
mk g
k=1
[ 2
∂ pcog,z ,k
∂ψi ∂θj
]
∂ 2 pcog,z ,k
∂ψ∂θ
(5.25)
(5.26)
i=1,··· ,Ndrive-joint ,j=1,··· ,Njoint
つまり
∂τ grav
∂θ
∑
Ngravity-link
=
grav
return ∂ τ
∈ RNdrive-joint ×Njoint
∂θ
k=1
∂ 2 pcog,z ,k
mk g
∂ψi ∂θj
i=1,··· ,Ndrive-joint ,j=1,··· ,Njoint
(5.27)
Source Exif Data:
File Type : PDF File Type Extension : pdf MIME Type : application/pdf PDF Version : 1.5 Linearized : No Page Mode : UseOutlines Page Count : 81 Creator : LaTeX with hyperref package Producer : dvipdfm Create Date : 2019:02:11 02:14:59+09:00EXIF Metadata provided by EXIF.tools