Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

enable to read custom limb name from .yaml file #249

Merged
merged 1 commit into from
Apr 16, 2023
Merged

enable to read custom limb name from .yaml file #249

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 35 additions & 1 deletion euscollada/src/collada2eus_urdfmodel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <algorithm>


#include <boost/algorithm/string/predicate.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
Expand Down Expand Up @@ -257,6 +258,7 @@ class ModelEuslisp {
void printSensors();
void printSensorLists();
void printGeometries();
void printUniqueLimbs();

// print methods
void copyRobotClassDefinition ();
Expand Down Expand Up @@ -591,7 +593,7 @@ void ModelEuslisp::printMesh(const aiScene* scene, const aiNode* node, const Vec
bool limb_order_asc(const pair<string, size_t>& left, const pair<string, size_t>& right) { return left.second < right.second; }
void ModelEuslisp::readYaml (string &config_file) {
// read yaml
string limb_candidates[] = {"torso", "larm", "rarm", "lleg", "rleg", "head"}; // candidates of limb names
std::vector<string> limb_candidates; // limb names is given from yaml fiels

vector<pair<string, size_t> > limb_order;
#ifndef USE_CURRENT_YAML
Expand All @@ -607,6 +609,15 @@ void ModelEuslisp::readYaml (string &config_file) {
// yaml-cpp is greater than 0.5.0
doc = YAML::LoadFile(config_file.c_str());
#endif
// set limb_candidates from yaml fiels
for(YAML::const_iterator it=doc.begin();it != doc.end();++it) {
// -end-coords, *-vector
if ( boost::algorithm::ends_with(it->first.as<std::string>(), "-coords") ||
boost::algorithm::ends_with(it->first.as<std::string>(), "-vector") ) {
} else {
limb_candidates.push_back(it->first.as<std::string>());
}
}
Comment on lines +612 to +620
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

-coords、-vector以外に、sensors replace_xmlsといったキーがあります。
https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml#L73
これらのキーを除かないと、hrpsys_ros_bridge_tutorialsをbuildしたときに、

  8: at (write-min-max-table-to-robot-model-file (staro) "/home/hiraoka/hiraoka_ws/ast_ws/src/rtm-ros-robotics/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/staro.l" :margin 1.0)
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl FATAL ERROR on make-joint-min-max-table: unbound variable sensor_type_jt in (list rarm_joint0_jt rarm_joint1_jt rarm_joint2_jt rarm_joint3_jt rarm_joint4_jt rarm_joint5_jt rarm_joint6_jt rarm_joint7_jt larm_joint0_jt larm_joint1_jt larm_joint2_jt larm_joint3_jt larm_joint4_jt larm_joint5_jt larm_joint6_jt larm_joint7_jt rleg_joint0_jt rleg_joint1_jt rleg_joint2_jt rleg_joint3_jt rleg_joint4_jt rleg_joint5_jt lleg_joint0_jt lleg_joint1_jt lleg_joint2_jt lleg_joint3_jt lleg_joint4_jt lleg_joint5_jt head_joint0_jt head_joint1_jt chest_joint0_jt chest_joint1_jt sensor_type_jt translate_jt parent_link_jt sensor_name_jt translate_jt parent_link_jt sensor_type_jt sensor_name_jt rotate_jt sensor_name_jt sensor_type_jt parent_link_jt translate_jt rotate_jt sensor_name_jt sensor_type_jt parent_link_jt translate_jt sensor_name_jt rotate_jt sensor_type_jt parent_link_jt translate_jt parent_link_jt sensor_name_jt translate_jt sensor_type_jt rotate_jt), exitting...
Please comment in (load "irteus/irtrobot.l") in make-joint-min-max-table.l for debug
make[2]: *** [staro_joint_minmax_done] Error 1
make[1]: *** [CMakeFiles/staro_hrpsys_ros_bridge_tutorials_compile_joint_minmax.dir/all] Error 2

というエラーがでます。

今後どんなキーが追加されるか分からないので、#232 のようにyamlファイルにlimbs:を追加し、使用するlimbの一覧を取得できるようにした方がよいのではないでしょうか。

limbs: [rleg, lleg, torso, head, rarm, larm, right_hand, left_hand]
rleg:
  - RLEG_JOINT0  : rleg-crotch-y
  - RLEG_JOINT1  : rleg-crotch-r
  - RLEG_JOINT2  : rleg-crotch-p
  - RLEG_JOINT3  : rleg-knee-p
  - RLEG_JOINT4  : rleg-ankle-p
  - RLEG_JOINT5  : rleg-ankle-r
lleg:
  - LLEG_JOINT0  : lleg-crotch-y
  - LLEG_JOINT1  : lleg-crotch-r
  - LLEG_JOINT2  : lleg-crotch-p
  - LLEG_JOINT3  : lleg-knee-p
  - LLEG_JOINT4  : lleg-ankle-p
  - LLEG_JOINT5  : lleg-ankle-r
torso:
  - CHEST_JOINT0 : torso-waist-r
  - CHEST_JOINT1 : torso-waist-p
  - CHEST_JOINT2 : torso-waist-y
head:
  - HEAD_JOINT0  : head-neck-y
  - HEAD_JOINT1  : head-neck-p
rarm:
  - RARM_JOINT0  : rarm-collar-y
  - RARM_JOINT1  : rarm-shoulder-p
  - RARM_JOINT2  : rarm-shoulder-r
  - RARM_JOINT3  : rarm-shoulder-y
  - RARM_JOINT4  : rarm-elbow-p
  - RARM_JOINT5  : rarm-wrist-y
  - RARM_JOINT6  : rarm-wrist-r
  - RARM_JOINT7  : rarm-wrist-p
larm:
  - LARM_JOINT0  : larm-collar-y
  - LARM_JOINT1  : larm-shoulder-p
  - LARM_JOINT2  : larm-shoulder-r
  - LARM_JOINT3  : larm-shoulder-y
  - LARM_JOINT4  : larm-elbow-p
  - LARM_JOINT5  : larm-wrist-y
  - LARM_JOINT6  : larm-wrist-r
  - LARM_JOINT7  : larm-wrist-p
right_hand:
  - R_THUMB_JOINT0 : rhand-finger-0
  - R_THUMB_JOINT1 : rhand-finger-1
  - R_MIDDLE_JOINT0 : rhand-finger-2
left_hand:
  - L_THUMB_JOINT0 : lhand-finger-0
  - L_THUMB_JOINT1 : lhand-finger-1
  - L_MIDDLE_JOINT0 : lhand-finger-2

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

k-okada#1 に一つの修正案を作成しました。お役に立てれば幸いです。

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

-coords、-vector以外に、sensors replace_xmlsといったキーがあります。

そんなのがあるのね．
それぞれどういう役割になっていますか？
sensors については，eusにセンサモデルを足すものであれば，
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/make-pr2-model-file.l#L36
みたいな方法ではなくて，yamlからセンサを追加できたら嬉しいので，そちらが正攻法かとおもいました．
replace_xmls:
はどういう役割なんだろうか？

よいPRだとおもいますが，
１）冗長．larm: を書く場合はlimb: に追加すること，という必要ないルールが追加されている
（現状でlarm-end-coordsにはlarm: を前提にするという必要ないルールがあるから，いまさらなんだけど，なので
:limb
:larm
:links
- LLEG_JOINT0
-...
:end-coords
...
みたいな感じで書いたほうが良かったかな，とかおもいました．今から変えるのはやり過ぎなのでこれは必要ないと思うけど，
２） -coords、-vector以外に、sensors
replace_xmlsといったキーが何か別のプログラムで使われていて，そのプログラムは:larm
などは使わないとすると，一つのファイルに書く必然性はないのかな，とおもいました．

それぞれの役割について.

sensorsは、collada2eusとadd_sensor_to_collada.pyで使われています.

  • URDFにはセンサ情報が含まれていませんが、 rosrun euscollada collada2eus -I ./HRP2JSKNTS.urdf -C ./hrp2jsknts.yaml -O /tmp/hrp2jsknts.lとすると、yamlからセンサ情報を読んでセンサ情報を含んだeusモデルが生成されます。(この機能はrtmros系では使用されていません.)
  • URDFにはセンサ情報やエンドエフェクタの情報が含まれていませんが、rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml とすると、yamlからセンサやエンドエフェクタの位置を読んで、センサやエンドエフェクタの位置にセンサの質量0のリンクがついたURDFが生成されます。(この機能はrtmros系でも使用されていますが、https://github.com/start-jsk/rtmros_common/blob/57053f52d7deb5044607ab5ec12843dc6094514c/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L43 と機能が重複しているので、どちらか一方があれば十分だと思います。)

replace_xmlsは、add_sensor_to_collada.pyで使われています。

  • URDFにはgazeboシミュレーション用のパラメータの情報が含まれていませんが、rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml とすると、yamlの記述に従ってURDFのxmlのノードを追加・置換することで、gazeboシミュレーション用のパラメータの情報を追加します。(この機能はrtmros系でも使用されていますが、gazeboのシミュレーションを利用している人は少なくなっています)

補足情報として、rarm:等は、https://github.com/start-jsk/rtmros_common/blob/master/hrpsys_ros_bridge/scripts/controller_config_converter.py でも使われています。

  • rosrun hrpsys_ros_bridge controller_config_converter.py hrp2jsknts.yaml hrp2jsknts_controller_config.yamlとすると、yamlの記述に従ってcontroller_config.yamlを生成します。(この機能はrtmros系でも使用されています)

1)2)について

2点明確化・訂正させていただきます。

  • limbのデフォルト値をrleg,lleg,rarm,larm,head,torsoとしてあるので、従来通りrleg,lleg,rarm,larm,head,torsoしか使わない場合には、limb:に記述を追加する必要はありません。そのため、larm: を書くだけの場合にはlimb: に追加する必要はありません。hand:等を追加したい場合にはlimb: に追加しないといけないです。

  • add_sensor_to_collada.pyは、sensors: -end-coords: links: relpace_xmls:といったキーを使っているため、一つのファイルに書く必然性はあります。

hand:等を追加したい場合にlimb: に書かないといけないのは冗長だと僕も思います。また、現状の必要ないルール、必然性の無いファイル構成については、僕もB4の頃から同様の感想を抱いていました。そのため、自分は後方互換性等の責任を負う立場ではないこともあり、直して良いのでしたらどんどん直していきたいと思っていますし、自分用の個人ソフトウェアでは既にそうしています。一方で、後方互換性等を考えてなるべく今動いているプログラムをそのまま使えるようにすることを目指すなら、k-okada#1 のようにせざるをえないのかなあと思います。そこは、後方互換性等の責任を負う立場である @k-okada 先生にご判断いただきたいと考えております。

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

sensorsについては、rtmros系とは異なる経路でモデルを変換するhttps://github.com/makabe0510/robot_assembler/blob/80fc1577f8ca3e0a684fa8af77dc8ea38b5de0b7/sample/SAMPLE_HUMANOID.urdf.euscollada.yaml#L57-L63 などでも使われています。

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

replace_xmlsは、add_sensor_to_collada.pyで使われています。

  • URDFにはgazeboシミュレーション用のパラメータの情報が含まれていませんが、rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml とすると、yamlの記述に従ってURDFのxmlのノードを追加・置換することで、gazeboシミュレーション用のパラメータの情報を追加します。(この機能はrtmros系でも使用されていますが、gazeboのシミュレーションを利用している人は少なくなっています)

HIRONXJSKのgazeboシミュレーションもこの用途でreplace_xmlsを使っています。
https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml#L92-L223
https://github.com/start-jsk/rtmros_tutorials/blob/master/hironx_tutorial/doc/index.rst#hironxjsk-picking-demo
他のrtmros系ロボットに比べると、比較的最近まで動いていたかと思います（学校来れるようになってからあまり使ってませんが・・・）。

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

sensorsは、collada2eusとadd_sensor_to_collada.pyで使われています.

だとすると，yamlにsensors 入っていても問題ないのでは？となると，問題はreplace_xmlsでいいかな．
ちなみにこれは何をしているんだろう？

https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml#L158-L167
で，{L,R}LEG_LINK{5,6}のmass/ixx を0にしている？
これはもとのwrlでは1e-03/1e-02とあってコレオノイドはこのパラメータで動くけど，gazeboは0にしているのかな

https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml#L92-L223
みると逆か．コメントは重要だね．

# pr2_mechanism_model -> transmission_interface for gazebo with ros_control (temp patch, should be supported in collada_urdf)

はその通りなので，
ちなみにこの変換は wrl -> collada -> urdf ? wrl -> urdf 誰が8.07e-05の値を入れている collada_urdf　か何かで以下のルールを入れると解消する？

  1. イナーシャの最小値は0でなければ1e-3
    ２）massの最小値は1e-3
    nextageはこれで良さそう．HRP2は

replaced_xml: '<gazebo reference="R_INDEXPIP_R_LINK">\n <kp>140.0</kp>\n <kd>2800.0</kd>\n <mu1>1.5</mu1>\n <mu2>1.5</mu2>\n <fdir1>1 0 0</fdir1>\n <maxVel>10.0</maxVel>\n <minDepth>0.00</minDepth>\n </gazebo>'

はwrlにはない情報なのかな？指みたいに小さい物体の対応なのかな？

必要ないデータを読み込むのはかなり抵抗があるんだけど，add_sensor_to_collada.py -Cの引数を任意個とれるようにして，

rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml ./hrp2jsknts-replace-xml.yaml

とできると，euscollada 的には嬉しいけど，副作用大きすぎるだろうか？

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

# pr2_mechanism_model -> transmission_interface for gazebo with ros_control (temp patch, should be supported in collada_urdf)

はその通りなので，

こちらに関しては、ros/collada_urdf#35 でtransmission_interfaceを出力できるようにしたのですが、この時点ではstart-jsk/rtmros_gazebo#247 の問題があって最新のcollada_urdfを使えなかったのと、HIRONXJSK以外のロボットにもtransmission_interfaceを強制するのはアグレッシブすぎると感じたのですがhttps://github.com/start-jsk/rtmros_common/blob/57053f52d7deb5044607ab5ec12843dc6094514c/hrpsys_ros_bridge/cmake/compile_robot_model.cmake#L127 まで引数を渡す方法がすぐには実装できず、力尽きてしまった状態です。
start-jsk/rtmros_gazebo#247 は治っていると思うので、HIRONXJSK以外のロボットにもtransmission_interfaceを強制することにして、collada_urdf_jsk_patchでcollada_urdfの1.12.13を使うようにすれば（今は1.12.12）temp patch消せる気がしますが、そうしてみますか？

ちなみにこの変換は wrl -> collada -> urdf ? wrl -> urdf

wrl -> collada -> urdf だと思います。

誰が8.07e-05の値を入れている

これは、KawadaからもらったCADモデルから入っています。
https://github.com/start-jsk/rtmros_hrp2/blob/3ab613691eb742669c45c11458b95f047e463e63/hrp2_models/HIRONX/HIRONXmain.wrl#L373
真値かと言われると微妙な気もします。

collada_urdf　か何かで以下のルールを入れると解消する？

イナーシャの最小値は0でなければ1e-3
２）massの最小値は1e-3

これに関しては、あくまでgazebo用の最小値なので、やるならcollada_urdfに何かの引数を与えるとこのルールが適用されるみたいにするのが適切ですかね。
ただ、この値に関してはかなり怪しくて、ロボット全体の質量によって変わるような気がしていて、この値をロボットに関わらず画一的に適用するのはよくない気がしています。
例えば、https://answers.ros.org/question/288237/robot-arm-is-broken-link-by-link-in-gazebo/ では0.0001で動いたと報告されているのですが、HIRONXJSKでこうすると動きませんでした（ロボットがgazebo上で分解しました）。

必要ないデータを読み込むのはかなり抵抗があるんだけど，add_sensor_to_collada.py -Cの引数を任意個とれるようにして，

rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml ./hrp2jsknts-replace-xml.yaml

とできると，euscollada 的には嬉しいけど，副作用大きすぎるだろうか？

個人的にはこれはありな気がします。何か重大な問題を見落としてるかもしれないですが・・・。

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

問題はreplace_xmlsでいいかな．
ちなみにこれは何をしているんだろう？

gazeboのシミュレーションに所望の挙動をさせるためのパラチューンと、gazeboのシミュレーションに必要な設定です。

前者については、シミュレーションに所望の挙動をさせるために必要なパラメータは、シミュレーションアルゴリズムやロボットの形状、行わせる動作ごとに異なることが多いので、大本のwrlファイルを修正したりモデル変換時に一括でルールを入れれば解消するというものではありません。理想的なシミュレータが発明されない限りは、個別に設定してやらざるをえないと思います。後者については、gazebo_ros_controlを使うかrtmros_gazeboを使うのかによって、必要な設定が異なります。

{L,R}LEG_LINK{5,6}のmass/ixx を0にしている？
これはもとのwrlでは1e-03/1e-02とあってコレオノイドはこのパラメータで動くけど，gazeboは0にしているのかな

ここではgazebo用にイナーシャの値を大きくしています。choreonoidでもこのパラメータでは動かないので、別の場所で修正しています。基本的には、もとのwrlには実機の制御用に実機の値を入れるべきだろうと思います。

# pr2_mechanism_model -> transmission_interface for gazebo with ros_control (temp patch, should be supported in collada_urdf)

はその通りなので，

HIRONXはgazebo_ros_controlを使うのでその通りですが、https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hironx_tutorial/models/HIRONXJSK.gazebo.xacro#L8 脚型がある場合はhrpsysをgazeboと使うためにrtmros_gazeboを使うので、設定が異なります。 https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro#L5

ちなみにこの変換は wrl -> collada -> urdf ? wrl -> urdf 誰が8.07e-05の値を入れている

wrl かと思います。 https://github.com/start-jsk/rtmros_hrp2/blob/3ab613691eb742669c45c11458b95f047e463e63/hrp2_models/HIRONXJSK/HIRONXJSKmain.wrl#L401

replaced_xml: '<gazebo reference="R_INDEXPIP_R_LINK">\n <kp>140.0</kp>\n <kd>2800.0</kd>\n <mu1>1.5</mu1>\n <mu2>1.5</mu2>\n <fdir1>1 0 0</fdir1>\n <maxVel>10.0</maxVel>\n <minDepth>0.00</minDepth>\n </gazebo>'

はwrlにはない情報なのかな？指みたいに小さい物体の対応なのかな？

gazeboで、指みたいに小さいリンクが、比較的小さな力で物体と接触するときに、安定してシミュレーションするためのパラメータです。足のようにやや大きいリンクが地面と大きな力で接触するときには、異なるパラメータが必要になります。多点接触のように、指みたいに小さいリンクが、外界と大きな力で接触するときには異なるパラメータが必要になります。odeの限界という感があります。

必要ないデータを読み込むのはかなり抵抗があるんだけど，add_sensor_to_collada.py -Cの引数を任意個とれるようにして，

rosrun euscollada add_sensor_to_collada.py ./HRP2JSKNTS_WH.urdf -O /tmp/HRP2JSKNTS_WH_SENSORS.urdf -C ./hrp2jsknts.yaml ./hrp2jsknts-replace-xml.yaml

とできると，euscollada 的には嬉しいけど，副作用大きすぎるだろうか？

特定のシミュレーター・動作に固有のデータを同じファイルに書くのには違和感を感じるため、./hrp2jsknts.yaml ./hrp2jsknts-replace-xml.yamlと分けるのは嬉しいです。下に書くような懸念点がありますが、どうにかなる範囲だと思います。

https://github.com/search?q=org%3Astart-jsk+replace_xmls%3A&type=code
https://github.com/search?p=1&q=org%3Ajsk-ros-pkg+replace_xmls%3A&type=Code

を検索すると、rtmros_common, rtmros_tutorialsと、腱駆動で使われているようです。僕の知識不足のため、腱駆動は副作用が分かりません。rtmros_common, rtmros_tutorialsについては、全部変えることはそこまで難しくなく、その結果問題が発生した場合に対応するのも難しくありません。ただ、rtmros_commonで定義されているcmakeのマクロをrtmros_tutorialsで使う関係で、過渡期にはrtmros_commonのaptを使うかsourceを使うかによる混乱が予想されます。また、rtmros_common, rtmros_tutorialsはブランチを切っているOBが多いので(僕は切っていません)、それらのブランチのビルドを通すにはgit merge origin masterをしないといけなくなり、そういうブランチはだいたいorigin/masterとconflictしているので十分に理解した人でないとgit merge origin masterができない、という問題もあります。

あと、add_sensor_to_collada.pyはlinks:というキーもとれて、以前は利用されていた形跡があります。今利用しているコードは見つけられませんでした。
https://github.com/start-jsk/rtmros_choreonoid/blob/49da4ca7d8a1b1ad244f28b2ec3859be6f764607/hrpsys_ros_bridge_jvrc/models/jaxon_jvrc.yaml#L118

/* re-order limb name by lines of yaml */
BOOST_FOREACH(string& limb, limb_candidates) {
#ifdef USE_CURRENT_YAML
Expand Down Expand Up @@ -742,6 +753,14 @@ void ModelEuslisp::printRobotDefinition() {
for (vector<daeSensor>::iterator it = m_sensors.begin(); it != m_sensors.end(); it++) {
fprintf(fp, " %s-sensor-coords", it->name.c_str());
}
fprintf(fp, "\n ;; non-default limb names\n");
fprintf(fp, " ");
BOOST_FOREACH(link_joint_pair& limb, limbs) {
if( limb.first == "torso" || limb.first == "larm" || limb.first == "rarm" || limb.first == "lleg" || limb.first == "rleg" || limb.first == "head" ) {
continue;
}
fprintf(fp, " %s %s-end-coords %s-root-link", limb.first.c_str(), limb.first.c_str(), limb.first.c_str());
}
fprintf(fp, "\n )\n )\n");
// TODO: add openrave manipulator tip frame
}
Expand All @@ -768,6 +787,8 @@ void ModelEuslisp::printRobotMethods() {

printSensors();

printUniqueLimbs();

printGeometries();

fprintf(fp, " )\n");
Expand Down Expand Up @@ -1582,6 +1603,19 @@ void ModelEuslisp::printSensorLists() {
fprintf(fp, "))\n");
}

void ModelEuslisp::printUniqueLimbs() {
fprintf(fp, "\n ;; non-default limbs\n");
BOOST_FOREACH(link_joint_pair& limb, limbs) {
if( limb.first == "torso" || limb.first == "larm" || limb.first == "rarm" || limb.first == "lleg" || limb.first == "rleg" || limb.first == "head" ) {
continue;
}
fprintf(fp, " (:%s (&rest args) (unless args (setq args (list nil))) (send* self :limb :%s args))\n", limb.first.c_str(), limb.first.c_str());
fprintf(fp, " (:%s-end-coords () %s-end-coords)\n", limb.first.c_str(), limb.first.c_str());
fprintf(fp, " (:%s-root-link () %s-root-link)\n", limb.first.c_str(), limb.first.c_str());
}
}


#if URDFDOM_1_0_0_API
void ModelEuslisp::printGeometry (GeometrySharedPtr g, const Pose &pose,
#else
Expand Down