前回の記事↓ odome.hatenablog.com
開発リポジトリ↓ github.com
概要
ヒューマノイドロボットのモデルが記述されたURDFファイルを読み込み、制御に有用な情報をROS 2パラメータとして利用可能なYAMLファイルで出力するPythonスクリプトを作成しました。本記事では、このスクリプトの紹介を行います。
（後述しているように動作確認はしていますが、どのURDFモデルでも正常に動作する保証はいたしかねます。生成されたYAMLファイルが正常が正しいかを確認することを推奨します。）
３行で説明
URDFファイルを読んでROS 2パラメータのYAMLファイルを生成するスクリプト、model_parameter_generator.pyを作った（source_code）。
URDFモデルはヒューマノイドロボットを対象とし、動作試験はROBOTIS OP2のXacroをURDFに変換したもので行った（source_code & readme）。
生成されるYAMLファイルは、全関節の情報、四肢（正確にはツリー構造の枝ごと）ごとのリンク長や名前をまとめたもの、全関節名をまとめたもの、計３つ生成し、主に/joint_statesトピックや運動学計算での活用を想定している。
動機
スクリプトを作成するに至った動機を以下に示します。
運動学計算や/joint_statesトピックの作成には、ロボットの各リンク長や関節名を、ユーザ側でURDFファイルを直接読んでコードに反映する必要がある。
制御対象とするロボットモデルを変更する際に、再度URDFを読んでコードに反映するという作業は苦痛。加えて、書き換えるべきコードを理解しておく必要がある。
ROS 2パラメータでURDF内の必要な情報を扱えれば、コード理解と書き換え作業を削減できるのでは？
つまりは、制御対象を変更する作業を、より簡単で容易にできるようにしたい、これが動機であり、目的となります。
使用方法
ここでは、本スクリプトの使用方法について示します。ソースコードは以下となっています。
使用方法
スクリプトを実行する前に，対象とするURDF ファイルが robot_description/models/＜ロボットの名前＞/ディレクトリ内に置かれていること，プログラム内で定義されているURDFファイル名とロボットの名前が正しいかを確認する．
本スクリプトであるrobot_tools/robot_tools/model_parameter_generator.pyを実行する．（以下、例）
cd robot_tools/robot_tools/ # 移動 ./model_parameter_generator.py # 実行
- 四肢などの URDF 内に含まれるツリーが表示されるので，指示通りに各ツリーの名前を入力する．（以下、例）
[INFO] Robot name is robotis_op2 [WARNING] Already robot_description/config/robotis_op2 dir. [INFO] List of tree in URDF file. (with fixed joints) tree 0 : ['base_joint'] tree 1 : ['head_pan', 'head_tilt', 'head_cam'] tree 2 : ['l_sho_pitch', 'l_sho_roll', 'l_el'] tree 3 : ['r_sho_pitch', 'r_sho_roll', 'r_el'] tree 4 : ['l_hip_yaw', 'l_hip_roll', 'l_hip_pitch', 'l_knee', 'l_ank_pitch', 'l_ank_roll'] tree 5 : ['r_hip_yaw', 'r_hip_roll', 'r_hip_pitch', 'r_knee', 'r_ank_pitch', 'r_ank_roll'] [INFO] Please input free joint_tree_name. [INFO] If nothing is inputted, joint_tree_name is 'names_group_<number>'. [INFO] tree 0 name? > # ここに、tree 0の名前を入力
- 右脚と左脚のツリー名を指定する．（以下、例）
name's number | joint_group_name | joint_group_value
--------------|------------------|------------------
0 | names_group_0 | ['base_joint']
1 | names_group_1 | ['head_pan', 'head_tilt', 'head_cam']
2 | names_group_2 | ['l_sho_pitch', 'l_sho_roll', 'l_el']
3 | names_group_3 | ['r_sho_pitch', 'r_sho_roll', 'r_el']
4 | names_group_4 | ['l_hip_yaw', 'l_hip_roll', 'l_hip_pitch', 'l_knee', 'l_ank_pitch', 'l_ank_roll']
5 | names_group_5 | ['r_hip_yaw', 'r_hip_roll', 'r_hip_pitch', 'r_knee', 'r_ank_pitch', 'r_ank_roll']
[INFO] What's left_leg's name? (please input name's number) > # ここに、左脚のname's numberを入力
出力結果
ここでは、本スクリプトが出力するYAMLファイルについて示します。
- param_＜ロボットの名前＞_joints.yaml
- 全関節の回転軸方向やリミットなどの情報を持つ。
- position_sensor_nameは、webotsで関節角度を取得する際に用いるセンサ名。以下同様。（以下、例）
/**: ros__parameters: robotis_op2_joints: base_joint: origin: xyz: - 0.0, 0.0, 0.0 rpy: - 0.0, 0.0, 0.0 head_pan: position_sensor_name: head_pan_sensor origin: xyz: 0 0 0.0205 rpy: 0 0 0 axis: 0 0 1 limit: effort: 2.8 lower: -2.6179939 upper: 2.6179939 velocity: 5.6548668 head_tilt: position_sensor_name: head_tilt_sensor origin: xyz: 0 0 0.03 rpy: 0 0.5759586531581288 0 axis: 0 -1 0 limit: effort: 2.8 lower: -2.6179939 upper: 2.6179939 velocity: 5.6548668 ...
- param_＜ロボットの名前＞_limb.yaml
- 四肢などのそれぞれの名前リストやリンク長のリストを持つ。
- 特に、運動学計算や/joint_statesトピックで有用であると思われる。
- 駆動しない関節（fixed）を含むリストと、含まないリスト両方を記録している。（以下、例）
/**: ros__parameters: robotis_op2_limb: limb_names: left_leg: l_leg right_leg: r_leg limb_without_fixed_joints: base: joint_names: null position_sensor_names: 'null' joint_numbers: null joint_unit_vector: null link_length: null joint_posture: null head: joint_names: - head_pan - head_tilt position_sensor_names: - head_pan_sensor - head_tilt_sensor joint_numbers: - 0 - 1 joint_unit_vector: - 0 0 1 - 0 -1 0 link_length: - 0 0 0.0205 - 0 0 0.03 joint_posture: - 0 0 0 - 0 0.5759586531581288 0 l_arm: joint_names: - l_sho_pitch - l_sho_roll - l_el position_sensor_names: - l_sho_pitch_sensor - l_sho_roll_sensor - l_el_sensor joint_numbers: - 2 - 3 - 4 joint_unit_vector: - 0 1 0 - -1 0 0 - 0 1 0 link_length: - 0 0.0575 0 - 0 0.0245 -0.016 - 0.016 0 -0.06 joint_posture: - 0 0 0 - 0.7853981633974483 0 0 - 0 -1.5707963267948966 0
- param_＜ロボットの名前＞_name_lists.yaml
- 全関節名と全position_sensor名を持つ。
- 特に、/joint_statesトピックで有用であると思われる。
- 駆動しない関節（fixed）を含むリストと、含まないリスト両方を記録している。（以下、例）
/**: ros__parameters: robotis_op2_name_lists: all_names_without_fixed_joints: all_joint_names: - head_pan - head_tilt - l_sho_pitch - l_sho_roll - l_el - r_sho_pitch - r_sho_roll - r_el - l_hip_yaw - l_hip_roll - l_hip_pitch - l_knee - l_ank_pitch - l_ank_roll - r_hip_yaw - r_hip_roll - r_hip_pitch - r_knee - r_ank_pitch - r_ank_roll all_position_sensor_names: - head_pan_sensor - head_tilt_sensor - l_sho_pitch_sensor - l_sho_roll_sensor - l_el_sensor - r_sho_pitch_sensor - r_sho_roll_sensor - r_el_sensor - l_hip_yaw_sensor - l_hip_roll_sensor - l_hip_pitch_sensor - l_knee_sensor - l_ank_pitch_sensor - l_ank_roll_sensor - r_hip_yaw_sensor - r_hip_roll_sensor - r_hip_pitch_sensor - r_knee_sensor - r_ank_pitch_sensor - r_ank_roll_sensor
そしてこれらROS 2パラメータファイルは、前々回の記事で紹介したParameter Serverに読み込まれ、各ノードからROS 2パラメータとして扱えるようになっています。
前々回の記事↓ odome.hatenablog.com
以上の実装によって、URDF内の情報をROS 2パラメータで扱えるようにしています。
最後に
今回は、URDFから情報を抽出してROS 2パラメータとして利用できるようにする、model_parameter_generator.pyの紹介を行いました。はじめに述べたように、どのURDFモデルでも正しいYAMLファイルを生成できることを保証することはできていませんが、スクリプトの利用によってロボットモデルを変更しやすくなったと思います。
今後の展望として、エラー処理の追加や、他URDFファイルでの実例を増やしていければ良いと考えています。
開発リポジトリ↓ github.com
