逆運動学ライブラリとX7のサンプルを追加 #10

ShotaAk · 2021-12-15T07:06:21Z

What does this implement/fix?

kinematicsライブラリに逆運動学を解く関数inverse_kinematics_LM()を追加します。
この関数には、杉原知道. Levenberg-Marquardt法による可解性を問わない逆運動学. 日本ロボット学会誌 Vol.29, 2011 に記載されたアルゴリズムを実装しています。

また、この関数を用いたCRANE-X7用のサンプル(x7_inverse_kinematics)を追加しています。

逆運動学関数とサンプルを追加するために、下記の変更を実施しています。

hardware関係

サーボモータの関節可動範囲を取得するための関数を追加します
- get_min_position_limit()、get_max_position_limit()

kinematics

逆運動学を計算するinverse_kinematics_LM()を追加します

kinematics_utils

下記の関数を追加します
- rotation_to_omega():回転行列から等価各軸ベクトルへ変換する
- find_route():目標リンクから根本リンク(ベースリンクは除く)までの経路をを抽出する
- get_q_list():リンク構成から指定されたIDの関節位置qを抽出する
- set_q_list():指定されたIDの関節位置qをリンク構成に書き込む
- calc_error_R():回転行列の差を求める
- calc_error_p():位置の差を求める
- calc_error():目標位置・姿勢と、リンクの位置・姿勢の差を求める
- calc_basic_jacobian():基礎ヤコビ行列を求める
parse_link_config_file()でリンクパラメータファイルからDynamixelのIDを取得しdxl_idにセットします
- 関節可動範囲はハードウェアから取得するため、パラメータファイルからは取得しません

linkクラス

対応するサーボモータのIDを格納するdxl_idを追加します
関節可動範囲を格納するmin_qとmax_qを追加します

テスト関連

IKのテストを実施するために、CRANE-X7のリンクパラメータファイルを追加します
FK、IKのテストを実施するtest_kinematics.cppを追加します
- FKのテストは、手計算した数値でチェックしやすいように、テスト用のリンクパラメータtest_links.csvを使用します
- IKのテストは、実際のロボットが動く範囲でIKが解けるか確認できるように、X7のリンクパラメータを使用します
- X7の後側、上側のIKに失敗するためコメントアウトしています
  - IKに失敗する原因が分かり次第、別PRで対応します
kinematics_utilsに追加した関数のテストを追加します

サンプル関連

READMEを更新します
x7_inverse_kinematicsサンプルを追加します

Does this close any currently open issues?

いいえ

How has this been tested?

Lintチェック、ユニットテストが通ることをGitHub Actionsで確認しています。

https://github.com/rt-net/rt_manipulators_cpp/actions/runs/1591540120

Ubuntu 20.04 環境で、x7_inverse_kinematicsサンプルが動作することを確認しています。

Any other comments?

Sciurus17のサンプルは、このPRがマージされたあとに作成します。

Sciurus17のハードウェアに対応するため（例、腰軸を除いてIKを解く）、
IK関数や、関連する関数を加工する可能性があります。どう実装するかは検討中です。

Checklists

I have read the CONTRIBUTING guidelines.
I have checked to ensure there aren't other open Pull Requests for the same change.

knjinki

レビューして動作確認しました。解の収束性に関わるコメントをいくつかしました。ただそれらを変更してもコメントアウト部分は通るようになっていませんが、現状のテストはそのまま通って実機でも動作することを確認しています。

rt_manipulators_lib/src/kinematics_utils.cpp

rt_manipulators_lib/include/kinematics_utils.hpp

rt_manipulators_lib/src/kinematics_utils.cpp

knjinki · 2021-12-21T02:29:38Z

rt_manipulators_lib/src/kinematics.cpp

+    error = kinematics_utils::calc_error(target_p, target_R, calc_links[target_id]);
+
+    // 誤差が小さければ終了
+    if (error.norm() < 1.0E-6) {


epsilon = 1.0E-6とかでhpp側に定義してもよさそう。

どこまでの値をライブラリ外部から調整できるようにするか悩ましいところですが、
今回は全パラメータ変更できないようにしたいです（今の実装がそうなってます）。

ただ、数字を直接書くと意味が伝わりにくいので、変数にします。

be4bedb
にて、定数error_toleranceとして定義しました。

knjinki · 2021-12-21T02:38:22Z

rt_manipulators_lib/src/kinematics.cpp

+
+    // 減衰因子の計算
+    auto E = 0.5 * error.transpose() * We * error;
+    double omega = 0.001;  // 0.1 ~ 0.001 * 代表リンク長の2乗


代表リンク長l=0.1としたときに、0.1*l^2の値を使っているので、0.1 ~ 0.001の端の値（0.1側）と考えると数値の安定性としてはもしかしたら不安定よりかもしれない。安定性の高い手法とはいえ、安定性が理論的に保証されているわけではなく（文献中、大域的収束性云々のところ）、経験則的に0.1 ~ 0.001の範囲で十分というようなパラメータなので、0.1 ~ 0.001の間の0.01くらいを選択してもいいかもしれない。それでもテストのコメントアウト部分は通るようになりませんでしたが。

ありがとうございます。
0.01に変更します。

7434dab
にて0.01に変更しました。

knjinki · 2021-12-21T03:32:08Z

samples/samples02/README.md

+`kinematics::inverse_kinematics_LM()`には、
+[杉原 知道.
+Levenberg-Marquardt法による可解性を問わない逆運動学.
+日本ロボット学会誌 Vol.29, 2011](
+https://www.jstage.jst.go.jp/article/jrsj/29/3/29_3_269/_pdf)
+に記載されたアルゴリズムを実装しています。


実装について検証しきれてないという判断であれば、ここは一旦明記せずに保留してもいいかもしれません。
例えば注意書きとして、下記のようなものを書くなど。
kinematics::inverse_kinematics_LM()には数値解法による逆運動学の計算アルゴリズムが実装されています。任意の位置姿勢に対してロボットを動かしたい場合は、ロボットを動かす前に逆運動学の解が発散しないか等十分に検証して使用してください。

承知しました。文言を変更します。

9b0890c
にて対応しました。

knjinki · 2021-12-21T03:58:48Z

rt_manipulators_lib/src/kinematics.cpp

+
+  // qを0にリセットする
+  for (auto q_i = q_list.begin(); q_i != q_list.end(); ++q_i) {
+    q_i->second = 0;


Suggested change

q_i->second = 0;

q_i->second = -M_PI/4; // 特異点でない可動範囲内の適当な関節角

初期値0が特異姿勢なので数値解法の初期値としてあまり良くないため、これを適当な特異姿勢でない値に変更するのはありだと思います。数値解法として比較的安定した手法といっても、かなり不利な条件のスタートだと思います。
この値でも現状のテストは通りました。コメントアウト部分は通りませんでした。

たしかに、不利な状況から始める理由は無いですね。初期値を変更します。

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

knjinki · 2021-12-21T10:49:49Z

初期値については私の確認ミスでした。動作確認したのでマージします。

ShotaAk added 27 commits December 15, 2021 15:52

FKのテストをlink1まで追加

6d28128

FKのテストを実装

d921c50

cherry-pick find_routeを実装

93d5108

cherry-pick find_routeに例外処理を追加

aa5ab76

cherry-pick 型エイリアスを作成

079ee1e

get_q_listを実装

402d763

calc_errorを実装

c510331

calc_basic_jacobianを実装(テストは未実施)

b28fff5

cherry-pick set_q_listを実装

6d81346

calc_errorをcalc_error_Rにリネーム

b22cfcf

calc_error_pを実装

31ceea7

calc_errorを実装

a2d1331

Inverse Kinematics LMを実装。テスト方法について検討中

4282955

CRANE-X7のリンクパラメータを用いたテストを追加

3ba7ee6

失敗するテストをコメントアウト(後ほど修正する)

69cc2b6

linkクラスにdxl_idを追加

49c354b

linkクラスにset_q_within_limitを追加

1a80b08

set_q_listにwithin_limitオプションを追加

299b16b

IK演算時に関節位置制限を設ける。IKのテストに失敗しているため要調査

ca65517

rotation_to_omegaを実装

aa3a794

calc_error_Rの実装を修正。

dd3c677

IKを修正。後面のIKに失敗する、上面の特異姿勢IKに失敗する問題が残っている

94cee5b

関節可動範囲を取得できるようにhardwareを更新

64b191b

x7_inverse_kinematicsサンプルを追加

970968d

set_q_listのwithin_limitオプションのテストを追加

12a536f

サンプルとREADMEの更新

a95a767

READMEのlinkクラスを更新

0e82ed3

ShotaAk changed the title ~~逆運動学ライブラリとサンプルを追加~~ 逆運動学ライブラリとX7のサンプルを追加 Dec 17, 2021

ShotaAk added 2 commits December 17, 2021 18:04

READMEの目次を更新

1b7eeaf

fix typo

869c669

ShotaAk added the Type: Feature New Feature label Dec 17, 2021

ShotaAk marked this pull request as ready for review December 17, 2021 09:34

knjinki approved these changes Dec 21, 2021

View reviewed changes

Shota Aoki and others added 10 commits December 21, 2021 15:27

Update rt_manipulators_lib/src/kinematics_utils.cpp

14341af

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

Update rt_manipulators_lib/src/kinematics_utils.cpp

5196f70

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

rotation_to_omegaのコメントを修正

2e29b59

rotation_to_omegaをrotation_to_axis_angle_representationに変更

24f139f

Update rt_manipulators_lib/src/kinematics_utils.cpp

518abc1

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

Update rt_manipulators_lib/src/kinematics_utils.cpp

2ab8caf

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

inverse_kinematics_LMのパラメータを定数にした

be4bedb

IKのリファクタリング

9e1fcfc

IKのomegaを0.01に変更

7434dab

IKについてREADMEの文言を更新

9b0890c

knjinki merged commit fe52522 into main Dec 21, 2021

knjinki deleted the inverse_kinematics branch December 21, 2021 10:49

	q_i->second = 0;
	q_i->second = -M_PI/4; // 特異点でない可動範囲内の適当な関節角

逆運動学ライブラリとX7のサンプルを追加 #10

逆運動学ライブラリとX7のサンプルを追加 #10

Uh oh!

Conversation

ShotaAk commented Dec 15, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

What does this implement/fix?

hardware関係

kinematics

kinematics_utils

linkクラス

テスト関連

サンプル関連

Does this close any currently open issues?

How has this been tested?

Any other comments?

Checklists

Uh oh!

knjinki left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ShotaAk Dec 21, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

knjinki Dec 21, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

knjinki commented Dec 21, 2021

Uh oh!

Uh oh!

ShotaAk commented Dec 15, 2021 •

edited

Loading

ShotaAk Dec 21, 2021 •

edited

Loading

knjinki Dec 21, 2021 •

edited

Loading