choreonoidはOpenHRP3の上位交換ソフトであり、OpenHRP3の開発終了後に重心はこっちにシフトした。
choreonoidはただのシミュレータだけではなく、ロボットを制御する上で基本な運動学、逆運動学、ヤコビ、逆動力学などのC++ライブラリも一通り揃っている。
これらのライブラリはOpenHRP時代からすでにあって、choreonoidはさらにフラッシュアップした感じ。
残念ながらこの辺のライブラリの使い方は公式サイトにはほぼなく、ソースコード読むことになる現状である。
なので、自分の使い方を晒す。よかったら読んでみてコピペしてみて使ってみてくたさい。

ROSのkdlだけでもう不自由を感じない！！の方は多分読まなくてもいい…かもしれない。

includeするヘッダー

自分はとりあえず以下のヘッダーをincludeする：

#include <cnoid/Body>
#include <cnoid/BodyLoader>
#include <cnoid/VRMLBodyLoader>
#include <cnoid/EigenTypes>
#include <cnoid/JointPath>
#include <cnoid/Jacobian>
#include <cnoid/EigenUtil>
#include <cnoid/Sensor>
#include <cnoid/Link>

Bodyクラス

ロボットの計算用モデルクラス。すべてがここから始まる。
Bodyクラスはロボットを構成する全リンクを束ねる上位的なクラスと思って良いと思う。Bodyクラスから全身のリンクツリー情報、それぞれリンクの情報(関節角度、角速度、姿勢、位置など)、センサのマウント位置、などなどほぼすべての情報にアクセスできる。
choreonoidはBodyクラスのスマートポインタBodyPtrを用意してくれたので、積極的に使おう。

Bodyクラスの生成

ロボットモデルファイルを読み込むことでBodyクラスを生成することができる。現在はvrmlとyamlで記述したmodelファイルに対応している。このBodyLoaderクラスがファイルの拡張子をみて内部で自動的に識別している。
以下のコードでBodyクラスへのスマートポインタを生成する。

cnoid::BodyLoader bl;
cnoid::BodyPtr m_robot;
m_robot = bl.load(char* path_to_model_file);

Bodyクラス常用メンバー関数

rootLink(): ロボットの一番"根元"リンクの参照を返す。このリンクの位置がロボット座標系の原点になる。順運動学計算はこのリンクからスタートして末端のリンクへ計算していくので、このリンクの位置姿勢と全身の関節角度が与えられれば全リンクの位置姿勢が決まる。
numJoints(): ロボットの自由度数
joint(int index) : linkへの参照を返す
calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false): 順運動学計算。各リンクの角度q()を与えてこの関数呼べば全リンク座標系(ワールド座標系からみた)の位置姿勢が更新される。の加速度レベルまでの順運動学計算する場合はcalcForwardKinematics(true, true)で
calcCenterOfMass(): 重心位置を返す。この関数を呼ぶ前にcalcForwardKinematics()を一度呼ぶとよい。
mass(): ロボットの質量
link(const std::string& name): リンク名(ロボットモデルファイルに記述されている名前)から対象リンクの参照を取得。
devices(): DeviceListの取得。センサ類の参照はこの関数で。例：

    DeviceList<ForceSensor> forceSensors = m_robot->devices();
    DeviceList<AccelerationSensor> AccelSensors = m_robot->devices();
    DeviceList<RateGyroSensor> RateGyroSensors = m_robot->devices();

するとID順で並ぶそれぞれの種類のdeviceリストが返される。ID順も同様にロボットモデルファイルに記述されている。

updateLinkTree(): リンクツリーの更新。リンク追加後に必ず呼ぶこと。

Linkクラス

リンクの詳細情報を管理するクラス。

Linkクラス常用メンバー関数

p(): リンクの根元にあるローカル座標系の位置
R(): リンクの根元にあるローカル座標系の姿勢
T(): リンクの根元にあるローカル座標系の位置姿勢を表すアフィン変換
c(): ローカル座標系からみた重心位置
q(): 関節角度
u(): トルク(逆動力学計算後に目標トルクはこの変数に入る)
addExternalForce(const Vector3& f_global, const Vector3& p_local): リンクに作用する外力(平行力)を設定する。重心位置に重力を設定するときはこの関数で。
appendChild(Link* link): リンクの追加。エンドエフェクタについて逆運動学を解くときによく使う。

JointPathクラス

親子関係が続いている複数個リンクからなるグループ。
ここからここまでの関節使って逆運動学解くのだ！を指定する。

JointPathクラスのスマートポインタJointPathPtrは用意してあるので、こっちも積極的に使いましょう。

JointPathクラスの生成

例えば以下のコードでnextageモデルの右腕のJointPath取得する。

JointPathPtr rarm = getCustomJointPath(m_robot, m_robot->link("CHEST_JOINT0"), m_robot->link("RARM_JOINT5"));

一つ紛らわしいのは、データ構造上はjoint1->link1->joint2->link2....となっているので、getCustomJointPathの2個目の引数をlink1に指定すると取得したJointPathはjoint2から始まるグループになる。これは気を付けて！

JointPathクラス常用メンバー関数

calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false): 順運動学計算。このJointPath上のリンクのみ更新される。
endLink(): 末端のリンク参照を返す
baseLink(): 根元のリンク参照を消す。
joint(int index) : JointPath上index番目のリンクへの参照を返す。indexは0から始まる。
calcJacobian(Eigen::MatrixXd& out_J): ヤコビ計算。
calcInverseKinematics(const Position& T): 逆運動学計算。ここのPositionはchoreonoidが定義した Eigen::Transform<double, 3, Eigen::AffineCompact> 位置姿勢を表すアフィン変換。計算結果の角度はそれぞれリンクのq()に入る。

その他

calcCMJacobian(Body* body, Link* base, Eigen::MatrixXd& J): 重心ヤコビの計算。二足歩行ロボットの場合はよく引数baseに支持脚の末端リンクを渡し、全身関節の変位量から重心位置の変位量へ射影するヤコビを計算する。この重心ヤコビJのサイズが3xDOFになる。

CMakeListsの書き方

pkgconfig使うと簡単にlibとヘッダーの場所を取得できる。

pkg_check_modules(CHOREONOID REQUIRED choreonoid-body-plugin)
include_directories(${CHOREONOID_INCLUDE_DIRS})
link_directories(${CHOREONOID_LIBRARY_DIRS})

add_executable(your_exe your_src.cpp)
target_link_libraries(your_exe ${CHOREONOID_LIBRARIES})

で行けるはず。

まとめ

ひとまず自分のよく使うクラスとメンバー関数を紹介してきた。逆動力学計算についてはまた別の記事で説明する予定。
また不定期更新するかもしれないが、とりあえず一旦アップする。

次回を待て！！

choreonoidの便利ライブラリ