プラグイン名:ConstraintIK
依存プラグイン:Grasp;GeometryHandler
・障害物を避ける、関節角度制約付きのIK。参考文献[1]の3章を実装。
・このプラグインの実装にはEQuadProg++[2]を使用しています。
・障害物は楕円体で近似します。この情報はyamlファイルに記述します。
・ロボットの楕円体と環境の(SetEnvした)オブジェクトの楕円体が衝突しないように制約をかけIKを解きます。
・カメラを設定してあると、アームとカメラを同時にIKを解きます。
- SaveObstacleShape
対象物の楕円体情報を保存する
- DisplayObstacleShape
楕円体を表示
- HideObstacleShape
楕円体表示のクリア
-SetCamera
IKを解く時のカメラを選択する
#include "../ConstraintIK/ConstraintIK.h
Vector3 p; // 目標位置 Matrix3 R; // 目標姿勢 // Solverのインスタンスを作成。この時対象となるアームを設定する。 ConstraintIKSolver* cik = new ConstraintIKSolver(PlanBase::instance()->targetArmFinger); // パラメータの設定(必要な場合) cik->setStep(10); //現在地から目標付近まで何分割して解くか(デフォルト20) cik->setMaxIteration(10); //目標付近での微調整の最大回数(デフォルト10) cik->setFixJoint(0); //固定したい関節を設定します(JointNumを指定 ) cik->setTranslationalJointCoeff(10) //並進関節が回転関節に比べどれくらい動きづらくするか設定します if(!cik->ik(p,R)){ cerr << "ik failed" << endl; } delete cik;
choreonoidから、オブジェクトに障害物となる楕円体を設定する方法を説明します。(ロボットについては下記詳細な説明を参考に手動で設定してください。)
1.対象オブジェクトをSetObjectします。
2.ConstraintIKバーのSaveObstacleShapeをクリックします。
3.ダイアログが表示されるので、分割パラメータ、近似方法選択します。
Clusteringが分割パラメータで、
・Number of Clustersを選択すると指定したクラスタ数に分割された後、楕円体が配置されます。
・Overlap volume ratioを選択するとクラスタ同士の重なりが指定した割合になるように分割された後、楕円体が配置されます。
Approximation typeが楕円体の配置方法で、
・boundingboxを選択するとクラスタのバウンディングボックスの辺に沿うように楕円体が配置されます。
・ellipsoidを選択するとクラスタを2次曲面で近似した時の楕円体が配置されます。(クラスタを2次曲面近似した結果,楕円体とならない場合は表示されません。)
4.showボタンをクリックすると楕円体が表示されます。もし、違う結果が見たい場合はパラメータを変更して再度showボタンをクリックします。
5.設定する楕円体が決定したらwriteボタンをクリックします。
6.[対象物名].yamlが作成されます。yamlファイルの最初の行を確認し、対象のhrpファイルパスと違う場合は修正してください。
「3.1.2.yamlファイル形式」を参考にし、手動でロボットのyamlファイルに楕円体情報を追加してください。
yamlファイルのサンプルを示します。
modelFile: ./boxL1Hrp.wrl collisionShape: - center: [0.2325 , 0 , 0.095] rpy: [0 , -0 , 0] length: [0.005 , 0.1475 , 0.005] security_distance: 0.01 effective_distance: 0.2 xi: 0.1
下記のような記述をロボットのyamlファイルに追加します。
collisionShape: - jointName: RARM_JOINT2 center: [-0.025, -0.015, -0.1] rpy: [0, 0, 0] length: [0.05, 0.05, 0.125] - jointName: RARM_JOINT1 center: [-0.025, -0.11, -0.14] rpy: [0, 0, 0] length: [0.05, 0.05, 0.12]
カメラ情報は下記のような記述をロボットのyamlファイルに追加します。
camera: - name: HEADCAMERA type: HEAD base: WAIST cameraLink: HEAD_JOINT1 direction: [1.0, 0.0, 0.0] pos: [0.09, 0.0, 0.08] - name: HANDCAMERA type: HAND base: WAIST cameraLink: LARM_JOINT5 direction: [-1.0, 0.0, 0.0] focal_distance: 0.5 pos: [-0.02, 0.0, -0.0]
目標位置・姿勢まで20分割して文献[1]のIKを使用して解いていきます。
このままでは、目標位置・姿勢までとの誤差が大きいことがあります。
そのため微調整のため、文献[1]のIKを誤差が十分小さくなるまで繰り返し解きます。
choreonoidを起動します。
extplugin/graspPlugin/ConstraintIK/project/box3.yamlを読み込みます(「ファイル」->「読み込み」->「OpenHRPモデルファイル」)。
アイテムタブのbox3を選択し、Plannerバーの「SetEnv」をクリックします。
ConstraintIKバーの「DisplayObstacleShape」を押し、4.1で設定した楕円体が表示されることを確認します。
ConstraintIKバーの「HideObstacleShape」を押し、楕円体が消えることを確認します。
choreonoidを終了します。
[1] 金広 他,"実機の物理的制約を考慮した即応的脚動作生成手法," 日本ロボット学会論文誌, vol. 28, no. 10, pp. 1251-1261, 2010.
[2] EQuadProg++, http://www.cs.cmu.edu/~bstephe1/eiquadprog.hpp.