ChoreonoidのBodyライブラリを使用してVRMLで記述されたロボットモデルのリンク情報を取得する

自分の備忘録としても兼ねてchoreonoid関連に関して書くことにしました.

今回行うこと

choreonoidのライブラリ群を使用してVRMLで記述されたロボットモデルからリンク情報(位置・姿勢など)をC++から取得してみます.

環境

OS: ubuntu 16.04 LTS
version: choreonoid 1.6.0

choreonoidのインストール

$ sudo apt-get update
$ sudo apt-get install choreonoid libcnoid-dev

Open-RTMなどを使いたい方は別途インストールしましょう(こちらに詳しい話が書いてあります)．

サンプルコード

ここではChoreonoidに付属されているサンプルモデル「SR1」を使用します．VRMLモデルはgithubにサンプルが上がっているのでそちらから拝借しましょう．apt-getで持ってきていれば/usr/local/choreonoid-1.6/model以下にモデルファイルが存在するのでそれを読み込めれば出来ると思います．ここでは，コードと同じ階層にmodelディレクトリを作成しその中にVRMLファイルがあると仮定します．

get_link_info.cpp

#include <iostream>
#include <cstring>

#include <cnoid/Body>
#include <cnoid/BodyLoader>

using namespace std;
using namespace cnoid;

int main()
{
	BodyLoader bodyloader;

	std::string model_path("model/SR1.body");
	BodyPtr robot = bodyloader.load(model_path.c_str());

	cout << "dof: " << robot->numJoints() << endl;
	cout << "base link name: " << robot->rootLink()->name() << endl;
	cout << "base link pos: \n" << robot->rootLink()->p() << endl;
	cout << "base link rot: \n" << robot->rootLink()->R() << endl;

	return 0;
}

軽くコードの解説をしますが，まずchoreonoid内部のコードではGUI周辺をとりもつ部分と運動学・力学計算といった演算部分をとりもつ部分があります．今回扱うBodyクラスは運動学・力学計算やモデル構造管理などを扱う部分です．
BodyPtrはBodyクラスを扱うためのスマートポインタであり，このポインタを経由することで指定のVRMLモデルを読みBodyクラスの関数からモデルへのアクセスが出来ます．

BodyLoaderは先の通りVRMLモデルファイルを読み込み，BodyPtrのインスタンスへ返します．これでVRMLモデル内のリンク位置・姿勢や重心位置などをAPIで取得できるようになります．

以下では，ロボットの自由度，基底リンクの名前，位置，姿勢を取得して表示しています．

cout << "dof: " << robot->numJoints() << endl;
cout << "base link name: " << robot->rootLink()->name() << endl;
cout << "base link pos: \n" << robot->rootLink()->p() << endl;
cout << "base link rot: \n" << robot->rootLink()->R() << endl;

robot->link(ロボットのリンク名)とすることで指定のリンクの情報を取得できるようになります．

コンパイルするためにchoreonoidのライブラリをリンクしてあげます．

CMakeLists.txt

project(get_link_info)

include(CheckIncludeFiles)

set(CMAKE_CXX_FLAGS "-std=c++11")

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

add_executable(get_link_info get_link_info.cpp)

target_link_libraries(get_link_info ${CHOREONOID_LIBRARIES})

まとめ

choreonoidのライブラリを使ってVRMLモデルファイルのリンク情報をC++のAPI経由で取得してみました．次あたりは運動学あたりを…