bio_ik
1.0.0
このリポジトリは、一般化された逆運動学問題を効率的に解決するためのさまざまな最適化手法の BSD ライセンス付きスタンドアロン実装を提供します。
モジュール全体は、修士論文の一部としてフィリップ・ルッペルによって実装されました。
Unity ストアで販売されていたオリジナルの「BioIK」アルゴリズムを C++ ベースで再実装するには、デフォルト以外のモードbio1使用できます。デフォルト モードのbio2_memeticこの実装とコードを共有せず、成功率、精度、効率の点でこの実装より優れていることが示されており、実際に精密なロボット アプリケーションに使用できます [4]。
ROS バージョン Indigo 以降 (wiki.ros.org) が必要です。このソフトウェアは、ROS Kinetic を使用した Ubuntu Linux 16.04 LTS で開発されましたが、ROS Indigo を使用した Ubuntu Linux 14.04 LTS でもテストされました。 ROS の新しいバージョンは動作するはずですが、多少の調整が必要な場合があります。バージョン固有の手順については、以下を参照してください。
bio_ikパッケージをダウンロードし、catkin ワークスペースに解凍します。
catkin_make実行してワークスペースをコンパイルします。
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
bio_ik を運動学ソルバーとして使用するように Moveit を構成します (次のセクションを参照)。
Moveit を使用してモーションを計画および実行するか、独自のコードをmove_groupノードとともに使用してロボットを動かします。
通常どおり、パブリック API は、 include/bio_ikサブディレクトリにあるbio_ikパッケージのパブリック ヘッダー ファイルで指定されます。いくつかのプライベート ヘッダー ファイルを含むソースはsrcサブディレクトリにあります。
使いやすさと既存のコードとの互換性のために、bio_ik アルゴリズムは Moveit キネマティクス プラグインとしてカプセル化されています。したがって、bio_ik は、デフォルトの Orocos/KDL ベースの IK ソルバーの直接の置き換えとして使用できます。エンドエフェクターの名前と 6-DOF ターゲット ポーズを指定すると、bio_ik は指定されたターゲットに到達する有効なロボット ジョイント構成を検索します。
私たちのテスト (以下を参照) では、成功率と解決時間の両方の点で、bio_ik は常に Orocos [1] ソルバーを上回り、trac-ik [2] と競合します。 bio_ik アルゴリズムは、ロボット ヘビのような高自由度システムにも使用でき、一部のターゲット ポーズが正確に到達できない低自由度アームの最適な近似解に自動的に収束します。
Moveit 設定ファイルは手動で作成することもできますが、最も簡単な方法は、ロボットの Moveit セットアップ アシスタントを実行し、エンド エフェクタを設定するときに IK ソルバーとして bio_ik を選択することです。構成が完了すると、ソルバーは標準 Moveit API を使用して呼び出すことも、MotionPlanning GUI プラグインを使用して rviz から対話的に使用することもできます。
ロボットに URDF (または xacro) モデルがあることを確認してください。
moveit セットアップ アシスタントを実行して、Moveit 構成ファイルを作成します。
rosrun moveit_setup_assistant moveit_setup_assistant
セットアップ アシスタントは、ワークスペース内で使用可能なすべての IK ソルバー プラグインを自動的に検索します。したがって、各エンド エフェクタのドロップダウン リストから IK ソルバとして [select bio_ik] を選択し、運動学パラメータ、つまりデフォルトの位置精度 (メートル) とタイムアウト (秒) を設定するだけです。一般的な 6-DOF または 7-DOF アームの場合、0.001 m (またはそれ以下) の精度と 1 ミリ秒のタイムアウトで問題ありません。より複雑なロボットでは、より長いタイムアウトが必要になる場合があります。
セットアップ アシスタントから moveit 構成ファイルを生成します。もちろん、お気に入りのテキスト エディターを使用してconfig/kinematics.yaml構成ファイルを編集することもできます。たとえば、PR2 ロボットの構成は次のようになります。
# example kinematics.yaml for the PR2 robot
right_arm:
# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
# kinematics_solver_attempts: 1
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
left_arm:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
# optional bio_ik configuration parameters
# center_joints_weight: 1
# minimal_displacement_weight: 1
# avoid_joint_limits_weight: 1
最初のテストとして、Moveit が作成したデモ起動を実行します。 rviz が実行されたら、モーション プランニング プラグインを有効にして、ロボットのエンド エフェクタの 1 つを選択します。 Rviz は、選択したエンドエフェクターの 6-D (位置と方向) インタラクティブ マーカーを表示する必要があります。インタラクティブ マーカーを移動し、bio_ik がロボットのポーズを計算する様子を観察します。
bio_ik デモ (以下を参照) もインストールした場合は、事前定義されたデモの 1 つを実行できるはずです。
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
これで、標準の Moveit API を使用して、C/C++ および Python プログラムから bio_ik を使用する準備が整いました。 C++ で IK ソリューションを明示的にリクエストするには:
robot_model_loader::RobotModelLoader robot_model_loader(robot);
auto robot_model = robot_model_loader.getModel();
auto joint_model_group = robot_model->getJointModelGroup(group);
auto tip_names = joint_model_group->getSolverInstance()->getTipFrames();
kinematics::KinematicsQueryOptions opts;
opts.return_approximate_solution = true; // optional
robot_state::RobotState robot_state_ik(robot_model);
// traditional "basic" bio_ik usage. The end-effector goal poses
// and end-effector link names are passed into the setFromIK()
// call. The KinematicsQueryOptions are empty.
//
bool ok = robot_state_ik.setFromIK(
joint_model_group, // joints to be used for IK
tip_transforms, // multiple end-effector goal poses
tip_names, // names of the end-effector links
attempts, timeout, // solver attempts and timeout
moveit::core::GroupStateValidityCallbackFn(),
opts // mostly empty
);
多くのロボット アプリケーションでは、単一のエンドエフェクター ポーズ以上のものを指定することが不可欠です。代表的な例としては、
bio_ik では、そのようなタスクは複数の個別の目標の組み合わせとして指定されます。
次に、アルゴリズムは、重み付けされた個々の目標から構築された二次誤差関数を最小化することによって、指定されたすべての目標を同時に満たすロボット構成を見つけようとします。現在の Moveit API は複数目標タスクを直接サポートしていませんが、KinematicQueryOptions クラスを提供します。したがって、bio_ik は単に定義済みのモーション ゴールのセットを提供し、ユーザー指定のゴールの組み合わせが Moveit 経由で IK ソルバーに渡されます。 Moveit では API の変更は必要ありませんが、IK ソルバーを使用すると、KinematicQueryOptions を介して重み付けされたゴールを渡すことになります。事前定義された目標には次のものが含まれます。
ロボットの動作の問題を解決するには、個々の目標を適切に組み合わせて構築することが重要です。
次の例では、PR2 ロボットの左右のグリッパーでバルブ ホイールをつかみ、ゆっくりと回転させます。
bio_ik::BioIKKinematicsQueryOptions ik_options;
ik_options.replace = true;
ik_options.return_approximate_solution = true;
auto* ll_goal = new bio_ik::PoseGoal();
auto* lr_goal = new bio_ik::PoseGoal();
auto* rl_goal = new bio_ik::PoseGoal();
auto* rr_goal = new bio_ik::PoseGoal();
ll_goal->setLinkName("l_gripper_l_finger_tip_link");
lr_goal->setLinkName("l_gripper_r_finger_tip_link");
rl_goal->setLinkName("r_gripper_l_finger_tip_link");
rr_goal->setLinkName("r_gripper_r_finger_tip_link");
ik_options.goals.emplace_back(ll_goal);
ik_options.goals.emplace_back(lr_goal);
ik_options.goals.emplace_back(rl_goal);
ik_options.goals.emplace_back(rr_goal);
また、いくつかの二次的な目標も設定しました。まず、PR2 のヘッドがバルブの中心を向くようにします。次に、可能であれば、すべてのジョイントでジョイント制限を回避したいと考えています。 3 番目に、IK ソリューションが以前の関節構成にできるだけ近いこと、つまり動作が小さく効率的であることを望みます。これは、MinimalDisplacementGoal を追加することで処理されます。 4 番目に、PR2 では非常に遅い胴体リフト動作を避けたいと考えています。これらはすべて簡単に指定できます。
auto* lookat_goal = new bio_ik::LookAtGoal();
lookat_goal->setLinkName("sensor_mount_link");
ik_options.goals.emplace_back(lookat_goal);
auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
ik_options.goals.emplace_back(avoid_joint_limits_goal);
auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal();
ik_options.goals.emplace_back(minimal_displacement_goal);
auto* torso_goal = new bio_ik::PositionGoal();
torso_goal->setLinkName("torso_lift_link");
torso_goal->setWeight(1);
torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 ));
ik_options.goals.emplace_back(torso_goal);
実際の回転動作では、ループ内で必要なグリッパー ポーズのセットを計算します。
for(int i = 0; ; i++) {
tf::Vector3 center(0.7, 0, 1);
double t = i * 0.1;
double r = 0.1;
double a = sin(t) * 1;
double dx = fmin(0.0, cos(t) * -0.1);
double dy = cos(a) * r;
double dz = sin(a) * r;
tf::Vector3 dl(dx, +dy, +dz);
tf::Vector3 dr(dx, -dy, -dz);
tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1)));
ll_goal->setPosition(center + dl + dg);
lr_goal->setPosition(center + dl - dg);
rl_goal->setPosition(center + dr + dg);
rr_goal->setPosition(center + dr - dg);
double ro = 0;
ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lookat_goal->setAxis(tf::Vector3(1, 0, 0));
lookat_goal->setTarget(rr_goal->getPosition());
// "advanced" bio_ik usage. The call parameters for the end-effector
// poses and end-effector link names are left empty; instead the
// requested goals and weights are passed via the ik_options object.
//
robot_state.setFromIK(
joint_model_group, // active PR2 joints
EigenSTL::vector_Affine3d(), // no explicit poses here
std::vector<std::string>(), // no end effector links here
0, 0.0, // take values from YAML file
moveit::core::GroupStateValidityCallbackFn(),
ik_options // four gripper goals and secondary goals
);
... // check solution validity and actually move the robot
}
コードを実行すると、PR2 はバルブホイールに手を伸ばして回転させます。時々、現在のアーム構成ではバルブに到達できず、ホイールを握り直すことがあります。
その他の例については、[3] と [4] を参照してください。
BioIK は、複数の目標と極小値を持つ非凸逆運動学問題の適切な解を効率的に見つけるために開発されました。ただし、一部のアプリケーションでは、これにより直感的でない結果が生じる可能性があります。特定の IK 問題に対して考えられる解決策が複数あり、ユーザーがどれを選択するかを明示的に指定していない場合、結果はすべての有効な解決策のセットからランダムに選択される可能性があります。デカルト パスを段階的に追跡すると、不要なジャンプや揺れなどが生じる可能性があります。BioIK を使用して滑らかな軌道を段階的に生成するには、目的の動作を明示的に指定する必要があります。これは 2 つの方法で実行できます。
BioIK は、グローバル オプティマイザーやローカル オプティマイザーなど、さまざまなソルバーを多数提供しています。デフォルトでは、BioIK はミーム グローバル オプティマイザー ( bio2_memetic ) を使用します。 MoveIt ロボット構成のkinematics.yamlファイルでmodeパラメーターを設定することで、別のソルバー クラスを選択できます。
例：
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
現在利用可能なローカル オプティマイザー:
gd, gd_2, gd_4, gd_8
gd_r, gd_r_2, gd_r_4, gd_r_8
gd_c, gd_c_2, gd_c_4, gd_c_8
jac, jac_2, jac_4, jac_8
命名規則: <solver type>_[<variant>_]<number of threads>
注:
gd_*ソルバーは、任意の目標タイプをサポートします。jac_*ソルバーはポーズ ゴールのみをサポートしますが、理論的には場合によってはより安定している可能性があります。gd_cバリアントは他のローカル ソルバーよりも優れたパフォーマンスを発揮しました。gd_c 、 gd 、またはjac ) がおそらく最適です。正規化を通じて、グローバル オプティマイザーが極小値を返すように強制できます。
インクリメンタル ロボット モーション (別名jogging ) の特定のケースの場合、最も簡単な解決策は、 PoseGoalと特別なRegularizationGoal の両方を指定することです。これにより、ジョイント スペース IK ソリューションを指定されたロボット シード コンフィギュレーションにできるだけ近づけようとします。通常、 PoseGoalには高い重みを使用し、レギュラライザーには小さい重みを使用します。
RegularizationGoalの代わりにMinimalDisplacementGoalを追加することもできます。どちらの目標も、IK ソリューションを現在/シード ロボットの状態に近づけることを目的としていますが、高速ロボット ジョイントと低速ロボット ジョイントの処理が若干異なります (たとえば、PR2 には高速の腕ジョイントとかなり遅い胴体リフト ジョイントがあります)。両方の目標を試して、どちらが自分のニーズに適しているかを確認するとよいでしょう。
7 自由度アーム上の一部の産業用ロボット コントローラーは、ジョイントが 1 つ追加された 6 自由度アームで作業しているかのように動作します。通常、追加のジョイントの値を指定すると、残りの 6 つのジョイントに対して IK ソリューションが検索されます。この動作は、エンドエフェクターのPoseGoalとロボット ジョイントの 1 つ (任意の 1 つ) のJointPositionGoal を組み合わせることによって、bio_ik で実現できます。
もう 1 つの便利なトリックは、ロボットの関節を中心に保つことです。これにより、ロボット (関節) を両方向に動かすことができるようになります。 PoseGoal をCenterJointsGoalと組み合わせ、必要に応じてRegularizationGaolも組み合わせます。
正則化とローカルgd_*ソルバーを組み合わせることもできます。
bio_ik ソルバーは、勾配ベースの最適化と遺伝的および粒子群の最適化を組み合わせたミーム アルゴリズムに基づいています。
内部的には、すべてのロボット ジョイント値のベクトルを使用して、さまざまな中間ソリューション (遺伝的アルゴリズムの遺伝子型) をエンコードします。最適化中に、有効なロボット構成のみが生成されるように、ジョイント値は常にアクティブなジョイントの下限値および上限値と照合してチェックされます。
個人の適応度を計算するには、与えられたすべての個人の目標に対する累積誤差が計算されます。誤差がゼロの個体は IK 問題の厳密な解になりますが、誤差が小さい個体は近似解に相当します。
個人は適応度によって分類され、勾配ベースの最適化が最適な少数の構成で試行され、多くの問題に対して高速な収束と優れたパフォーマンスが得られます。勾配ベースの最適化で解決策が見つからない場合は、一連の突然変異および組換え演算子によって新しい個体が作成され、結果として良好な検索空間探索が行われます。
詳細については、[3] および [4] を参照してください。ビデオ ゲーム キャラクターをアニメーション化するための初期の進化的アルゴリズムの詳細な説明については、[5] および [6] を参照してください。
従来のシングル エンド エフェクター API と、KinematicsQueryOptions に基づく高度なマルチ エンド エフェクター API の両方を使用して、多くの異なるロボット アームで bio_ik をテストしました。
1 つの簡単なセルフテストは、ランダムな有効なロボット構成を生成し、順運動学を実行して結果のエンドエフェクターのポーズを計算し、IK プラグインをクエリして適切なロボット ジョイント構成を見つけることで構成されます。次に、フォワード キネマティクスを再度実行し、生成された IK ソリューションのエンドエフェクター ポーズがターゲット ポーズと一致することを確認することで、成功がチェックされます。このアプローチは、数千または数百万のランダムなポーズに対して簡単に実行でき、ロボットのワークスペース全体をサンプリングして、選択した IK ソルバーの成功率と解決時間の推定値を迅速に生成できます。
もちろん、テストを実行するには、対応するロボット モデルをインストールする必要があり、多くの依存関係が追加されます。したがって、これらのテストは標準の bio_ik パッケージには含まれておらず、個別にパッケージ化されています。
便宜上、 pr2_bioik_moveitパッケージを提供しています。これには、PR2 サービス ロボット用の bio_ik デモもいくつか含まれています。これらは運動学のみのデモです。もちろん、実際のロボット (お持ちの場合) または Gazebo シミュレーター (Gazebo をインストールしている場合) でデモを実行してみることもできます。
PR2 記述パッケージ ( pr2_common内) とpr2_bioik_moveitパッケージを catkin ワークスペースに複製するだけです。
roscd
cd src
git clone https://github.com/PR2/pr2_common.git
git clone https://github.com/TAMS-Group/bioik_pr2.git
catkin_make
FK-IK-FK パフォーマンス テストについては、次を実行してください。
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
オロコスの運動学とダイナミクス、http://www.orocos.org
P. Beeson および B. Ames、 TRAC-IK: An open-source library for Improvement Solving of generic inverse kinematics 、IEEE RAS Humanoids Conference の議事録、ソウル、韓国、2015 年 11 月。
Philipp Ruppel、Norman Hendrich、Sebastian Starke、Jianwei Zhang、 Cost Functions to Specific Full-Body Motion and Multi-Goal Manipulation Tasks 、IEEE ロボティクスとオートメーションに関する国際会議 (ICRA-2018)、ブリスベン、オーストラリア。 DOI: 10.1109/ICRA.2018.8460799
Philipp Ruppel、 ROS におけるパフォーマンスの最適化と進化的逆運動学の実装、修士論文、ハンブルク大学、2017 PDF
Sebastian Starke、Norman Hendrich、Jianwei Zhang、 「リアルタイム関節運動運動のためのミーム進化アルゴリズム」 、IEEE Intl.進化計算会議 (CEC-2017)、p.2437-2479、2017 年 6 月 4 ～ 8 日、スペイン、サンセバスティアン。 DOI: 10.1109/CEC.2017.7969605
Sebastian Starke、Norman Hendrich、Dennis Krupke、Jianwei Zhang、多関節ロボットおよびヒューマノイドロボットの逆運動学のための多目的進化的最適化、IEEE Intl.インテリジェント ロボットおよびシステムに関するカンファレンス (IROS-2017)、2017 年 9 月 24 ～ 28 日、カナダ、バンクーバー
