[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method by k-kimura · Pull Request #553 · euslisp/jskeus

k-kimura · 2019-07-20T05:12:49Z

euslispでriccati方程式を解くためのsolverを追加しました．

既存のriccati-equationは繰り返し計算により定常解を得るもの（イテレーション数などの設定が必要）で，うまく解けないことがありましたが
繰り返し計算をしない方法として新しく「有本-Potterの方法（固有値分解法）」というアルゴリズムを採用して，riccati方程式の解が求められるようになっています．
また，LQRのフィードバックゲインも同時に出力されるようにしています．

以下，倒立振子のLQR制御を例に，
riccati-equation（繰り返し計算）
riccati-equation-arimoto-potter-method（有本-Potterの方法）
matlab（octave）
を比較した結果です．

riccati-equation（繰り返し計算）

irteusgl$ ;; (send (instance riccati-equation :init AA bb cc QQ RR) :solve)
irteusgl$ (setq *riccati-result* (send (instance riccati-equation :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 2 4 '((1 0 0 0) (0 1 0 0))) 4 7) :solve))
(#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0)) #2f((-27.0313 0.0 0.0 0.0)))

irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation
       175719.519   0.000   0.000   0.000 
         0.000   4.000   0.000   0.000 
         0.000   0.000 175719.519   0.000 
         0.000   0.000   0.000   4.000 
#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0))
irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K
       -27.031   0.000   0.000   0.000 
#2f((-27.0313 0.0 0.0 0.0))

riccati-equation-arimoto-potter-method（有本-Potterの方法）

irteusgl$ ;; (send (instance riccati-equation-arimoto-potter-method :init AA BB QQ RR) :solve)
irteusgl$ (setq *riccati-result* (send (instance riccati-equation-arimoto-potter-method :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 4 4 '((4 0 0 0) (0 4 0 0) (0 0 4 0) (0 0 0 4))) (make-matrix 1 1 '((7)))) :solve))
(#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846)) #2f((-191.33 -0.755929 -59.3904 -1.3284)))

irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation
       35705.647 314.265 12343.274 441.199 
       314.265   7.029 111.060   4.176 
       12343.274 111.060 4331.802 156.495 
       441.199   4.176 156.495   5.828 
#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846))
irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K
       -191.330  -0.756 -59.390  -1.328 
#2f((-191.33 -0.755929 -59.3904 -1.3284))

matlab（octave）

>> riccati_solve_test
A =
     0.00000     0.00000     1.00000     0.00000
     0.00000     0.00000     0.00000     1.00000
    34.67763     0.00000     0.00000     0.00000
  -679.76829     0.00000     0.00000     0.00000

B =
    0.00000
    0.00000
   -1.27842
   32.73032

Q =
Diagonal Matrix
   4   0   0   0
   0   4   0   0
   0   0   4   0
   0   0   0   4

R =  7

P =
   35705.77395     314.26480   12343.29561     441.19909
     314.26480       7.02922     111.06011       4.17624
   12343.29561     111.06011    4331.80142     156.49470
     441.19909       4.17624     156.49470       5.82845

K =
  -191.32998    -0.75593   -59.39046    -1.32840

新しく実装したriccati-equation-arimoto-potter-method（有本-Potterの方法）のほうは，ほぼmatlab（octave）と同じ結果が得られました．

現状は，「有本-Potterの方法」のポイントとなるHamilton行列（A,B,Q,Rから作成）の固有ベクトルの計算に関して，
euslispの固有値分解の関数(eigen-decompose)が虚数までカバーできていないため
Hamilton行列の固有ベクトルが虚数（固有値も虚数）になる場合には例外処理しています．
（今のところ，倒立振子モデルの場合はHamilton行列の固有値，固有ベクトルは実数となっています）

…r method

k-okada · 2019-07-22T08:17:45Z

https://github.com/euslisp/jskeus/blob/master/irteus/test/mathtest.l みたいな形でサンプルプログラムができるとよいですね．

…

-- ◉ Kei Okada 2019年7月20日(土) 14:12 Kohei Kimura <notifications@github.com>:

euslispでriccati方程式を解くためのsolverを追加しました．既存のriccati-equationは繰り返し計算により定常解を得るもの（イテレーション数などの設定が必要）で，うまく解けないことがありましたが繰り返し計算をしない方法として新しく「有本-Potterの方法（固有値分解法）」というアルゴリズムを採用して，riccati方程式の解が求められるようになっています．また，LQRのフィードバックゲインも同時に出力されるようにしています．以下，倒立振子のLQR制御を例に， riccati-equation（繰り返し計算） riccati-equation-arimoto-potter-method（有本-Potterの方法） matlab（octave）を比較した結果です． - riccati-equation（繰り返し計算） irteusgl$ ;; (send (instance riccati-equation :init AA bb cc QQ RR) :solve) irteusgl$ (setq *riccati-result* (send (instance riccati-equation :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 2 4 '((1 0 0 0) (0 1 0 0))) 4 7) :solve)) (#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0)) #2f((-27.0313 0.0 0.0 0.0))) irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation 175719.519 0.000 0.000 0.000 0.000 4.000 0.000 0.000 0.000 0.000 175719.519 0.000 0.000 0.000 0.000 4.000 #2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0)) irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K -27.031 0.000 0.000 0.000 #2f((-27.0313 0.0 0.0 0.0)) - riccati-equation-arimoto-potter-method（有本-Potterの方法） irteusgl$ ;; (send (instance riccati-equation-arimoto-potter-method :init AA BB QQ RR) :solve) irteusgl$ (setq *riccati-result* (send (instance riccati-equation-arimoto-potter-method :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 4 4 '((4 0 0 0) (0 4 0 0) (0 0 4 0) (0 0 0 4))) (make-matrix 1 1 '((7)))) :solve)) (#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846)) #2f((-191.33 -0.755929 -59.3904 -1.3284))) irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation 35705.647 314.265 12343.274 441.199 314.265 7.029 111.060 4.176 12343.274 111.060 4331.802 156.495 441.199 4.176 156.495 5.828 #2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846)) irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K -191.330 -0.756 -59.390 -1.328 #2f((-191.33 -0.755929 -59.3904 -1.3284)) - matlab（octave） >> riccati_solve_test A = 0.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 34.67763 0.00000 0.00000 0.00000 -679.76829 0.00000 0.00000 0.00000 B = 0.00000 0.00000 -1.27842 32.73032 Q = Diagonal Matrix 4 0 0 0 0 4 0 0 0 0 4 0 0 0 0 4 R = 7 P = 35705.77395 314.26480 12343.29561 441.19909 314.26480 7.02922 111.06011 4.17624 12343.29561 111.06011 4331.80142 156.49470 441.19909 4.17624 156.49470 5.82845 K = -191.32998 -0.75593 -59.39046 -1.32840 新しく実装したriccati-equation-arimoto-potter-method（有本-Potterの方法）のほうは，ほぼmatlab（octave）と同じ結果が得られました．現状は，「有本-Potterの方法」のポイントとなるHamilton行列（A,B,Q,Rから作成）の固有ベクトルの計算に関して， euslispの固有値分解の関数(eigen-decompose)が虚数までカバーできていないため Hamilton行列の固有ベクトルが虚数（固有値も虚数）になる場合には例外処理しています．（今のところ，倒立振子モデルの場合はHamilton行列の固有値，固有ベクトルは実数となっています） ------------------------------ You can view, comment on, or merge this pull request online at: #553 Commit Summary - [irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method File Changes - *M* irteus/irtdyna.l <https://github.com/euslisp/jskeus/pull/553/files#diff-0> (63) Patch Links: - https://github.com/euslisp/jskeus/pull/553.patch - https://github.com/euslisp/jskeus/pull/553.diff — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#553?email_source=notifications&email_token=AADYNXBMHUOMCK2NIP6W7CDQAKNFDA5CNFSM4IFNS6AKYY3PNVWWK3TUL52HS4DFUVEXG43VMWVGG33NNVSW45C7NFSM4HANLBXQ>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AADYNXG5ZLQLQNXZ6AZ3VTTQAKNFDANCNFSM4IFNS6AA> .

… add sample test program

k-kimura · 2019-07-27T08:55:38Z

euslispの固有値分解の関数(eigen-decompose)が虚数までカバーできていないため

の問題を#554 の変更で対応させました．
これで，matlabのlqr関数とほぼ同じように動くかとおもいます．

それをテストする（matlabの結果と比較する）ためのriccati-equation-arimoto-potter-methodのサンプルプログラムも追加しました．
二輪倒立振子モデルの実装例(lqr-two-wheeled-inverted-pendulum)も中に含めています．

irteusgl$ (load "./irteus/test/riccati-equation-arimoto-potter-method-test.l")
TEST-NAME: riccati-equation-arimoto-potter-method-test
  now testing...
start testing [riccati-equation-arimoto-potter-method-test]
ALL RESULTS:
  TEST-NUM: 1
    PASSED:   1
    FAILURE:  0
RESULT: all-test
  TEST-NUM: 0
    PASSED:   0
    FAILURE:  0
RESULT: riccati-equation-arimoto-potter-method-test
  TEST-NUM: 1
    PASSED:   1
    FAILURE:  0
[ INFO] [1564217319.972668113]: cell* ROSEUS_EXIT(context*, int, cell**)

…tion-arimoto-potter-method

k-kimura · 2019-07-29T12:22:43Z

euslisp/EusLisp#393 と同様にこちらも
riccati-equationクラスとriccati-equation-arimoto-potter-methodクラスの説明が
jskeus/doc以下に反映されるようにしました．

k-okada · 2020-06-07T06:56:58Z

Thank you for contributing jskeus documentation

Please check latest documents before merging

PDF version of Japanese jmanual: jmanual.pdf
HTML version of Japanese manual: jmanual.html
Sphinx (ReST) version of Japanese manual: jmanual.rst

k-okada · 2020-06-11T01:42:07Z

@k-kimura これは今のriccati方程式を使っているプログラム（予見制御とかがそう？）には影響を与えないんだよね？
そうだとせっかくコードを追加しても誰も使わないものが出来ているだけなので，苦労してマージする意味があまりないので，例えば教科書的に考えると

(defclass riccatti-equation (:super propertied-object))
(defclass riccatti-equation-iteration-method (:super riccatti-equation)
(defclass riccatti-equation-arimoto-potter-method (:super riccatti-equation)

となっているべきだよね．ただ，肝心のpreview-controllerが

(defclass preview-controller   :super riccati-equation)

となっているので，こうしたからって何かいいことがあるとはならなそう，となると，

(defmethod riccatti
 (:init (AA bb cc QQ RR &key (method :iteration)))

となっていて，同じクラスの中でアルゴリズムを切り替えるような書き方にすると，
例えば irteus/demo/walk-motion.l で新しいriccatti 方程式を使ったテストが出来るのでくるはず．

で，もう一歩考えると，もし完全上位互換のものが作れているのであればそもそも切り替えても良い．

この辺は，riccatti 方程式を使っているユーザがどれぐらいいるかというのが問題で，ユーザがいないなら，そのまま色々切り替えていけば良いし，既に既存のユーザがいるなら，後方互換も意識して考えていく必要がある．

パット見，riccatti 方程式はpreview control に使われていて，このレイヤで歩行制御しているのはサンプルプログラムぐらいしか無いのかなぁ，とは思っているんだけどどうだろうか？　案外実は色々なところで活用されているのかな．

@k-kimura @Naoki-Hiraoka @s-noda @makabe0510 @hiroya1224

k-kimura · 2020-06-11T09:46:49Z

プログラムを作って，サンプルプログラムを入れて，ドキュメントを追加して，
の後，何故かかれこれ1年くらい経っていますが
取り急ぎ，このPRは最低限の制御ツール（倒立振子）を目的にして入れたもので
まずは，他のプログラム（予見制御など）と干渉がないように追加しました．

確かに

せっかくコードを追加しても誰も使わないものが出来ているだけなので，苦労してマージする意味があまりないので，

（プログラムコード等々を書いて追加するだけでもすでに苦労はしていますが）
は自分も気にかけていることなので，
https://github.com/jsk-ros-pkg/euslib/issues/334
でもさっそく議論が進められていますが
具体的に，新しいriccati-equation-arimoto-potter-methodをどのプログラムのどのソースコード部分で試せるようになるといいか，ご教授いただけると幸いです．
時間のあるときに試してみようとおもいます．
（riccati-equationまわりの箇所をコピペしてriccati-equation-arimoto-potter-methodに置換したのを追加する程度なら自分でもすぐ試せるかもしれないです）
現時点では，これまでのpushで最低限，倒立振子の制御系設計ツールとしては活用できるようになっているかとおもいます．

k-okada · 2020-06-11T11:08:28Z

どのプログラムのどのソースコード部分

これは普段から自分が使っているコードのタイポとかドキュメント直してPR送っていれば，誰がどの部分を使っているか可視化されるのでいいんだけど，そうでないとなかなかわからないですよね．
とはいえさっきの話だと，
https://github.com/jsk-ros-pkg/euslib/tree/master/demo/nozawa/multicontact_motion_planner/test
がまずは動けばいいんじゃないでしょうか？
ちなみに，これが動くように成るための環境設定，僕は１時間ぐらいかかったんだけど，，，，

k-kimura · 2020-06-11T12:10:05Z

ありがとうございます．
時間のあるときに試してみます．

まだ理解が足りていないかもしれませんが，
https://github.com/jsk-ros-pkg/euslib/tree/master/demo/nozawa/multicontact_motion_planner
を手元に入れてみて
https://github.com/jsk-ros-pkg/euslib/tree/master/demo/nozawa/multicontact_motion_planner/test
の下にあるtest_multicontact_motion_planner.lが，エラーなくroseus(irteusgl)で実行できればOKということで合ってますでしょうか？

k-okada · 2020-06-12T02:05:20Z

合ってますでしょうか？

はい．そういう意味でした．

[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potte…

df983e3

…r method

[irteus/irtdyna.l] Improve riccati-equation-arimoto-potter-method and…

15d8b98

… add sample test program

[irteus/irtdyna.l] Add document for riccati-equation and riccati-equa…

4d33de8

…tion-arimoto-potter-method

Merge branch 'master' into riccati_solver_arimoto_potter

99fc6c3

k-okada added the PR/MergeOK label Jun 7, 2020

k-okada added the question label Jun 11, 2020

k-okada added the PR/WaitForMoreFeedback label Dec 17, 2020

k-okada assigned k-kimura Dec 17, 2020

k-okada removed the PR/MergeOK label Nov 24, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method#553

[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method#553
k-kimura wants to merge 4 commits intoeuslisp:masterfrom
k-kimura:riccati_solver_arimoto_potter

k-kimura commented Jul 20, 2019

Uh oh!

k-okada commented Jul 22, 2019 via email

Uh oh!

k-kimura commented Jul 27, 2019

Uh oh!

k-kimura commented Jul 29, 2019

Uh oh!

k-okada commented Jun 7, 2020

Uh oh!

k-okada commented Jun 11, 2020

Uh oh!

k-kimura commented Jun 11, 2020

Uh oh!

k-okada commented Jun 11, 2020

Uh oh!

k-kimura commented Jun 11, 2020

Uh oh!

k-okada commented Jun 12, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

k-kimura commented Jul 20, 2019

Uh oh!

k-okada commented Jul 22, 2019 via email

Uh oh!

k-kimura commented Jul 27, 2019

Uh oh!

k-kimura commented Jul 29, 2019

Uh oh!

k-okada commented Jun 7, 2020

Uh oh!

k-okada commented Jun 11, 2020

Uh oh!

k-kimura commented Jun 11, 2020

Uh oh!

k-okada commented Jun 11, 2020

Uh oh!

k-kimura commented Jun 11, 2020

Uh oh!

k-okada commented Jun 12, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants