Choreonoidで重心ヤコビアンを解く

前回はChoreonoidのライブラリを使用して運動学計算を行いました.今回は重心ヤコビアンを解いてみたいと思います.

環境

OS: ubuntu 16.04 LTS
version: choreonoid-1.6.0

サンプルコード

#include <iostream>
#include <cstring>

#include <cnoid/Body>
#include <cnoid/JointPath>
#include <cnoid/Jacobian>
#include <cnoid/BodyLoader>
#include <cnoid/Link>

using namespace std;
using namespace cnoid;

int main()
{
	BodyLoader bodyloader;
	
	string model_path("/usr/share/choreonoid-1.6/model/SR1/SR1.body");
	BodyPtr robot = bodyloader.load(model_path.c_str());

	JointPathPtr leg = getCustomJointPath(robot, robot->link("LLEG_ANKLE_R"), robot->link("RLEG_ANKLE_R"));

	robot->calcForwardKinematics();

	MatrixXd J_com(3, robot->numJoints());
	calcCMJacobian(robot, leg->baseLink(), J_com);

	std::cout << leg->baseLink()->name() << std::endl;
	std::cout << leg->numJoints() << std::endl;
	std::cout << J_com << std::endl;

	return 0;
}

重心ヤコビアンは，支持脚足裏が面に接地しているという条件のもと，ロボットの重心と関節間のヤコビアンのことを指し，これを用いることで別途得られる所望の重心運動の変位から全身関節運動への導出が出来るようになります．choreonoidでは重心ヤコビアン以外にも角運動量ヤコビアンなども実装されています．

詳細な理論などは論文を参照することをお勧めします.
http://www.mi.ams.eng.osaka-u.ac.jp/member/sugihara/pub/robomech02-1.pdf
http://www.mi.ams.eng.osaka-u.ac.jp/member/sugihara/pub/jrsj_cog.pdf

コードの説明です．重心ヤコビアンは一行関数を呼び出すだけで計算ができます．第一引数にはBodyクラスのインスタンス，第2引数にはベースリンク，第3引数は参照引数になっており，事前にサイズを初期化した行列を入れておきます．ここに重心ヤコビアンの計算結果が格納されます．重心と関節間のヤコビアンなのでサイズは3×ロボットの自由度になります．

void calcCMJacobian(Body* body, Link* base, Eigen::MatrixXd& J)

ですが，その前の準備が必要で，そのためにgetCustomJoitPath関数を呼び出します.ここでは左脚が床面に接地している状態で重心の変位量を入れた時の重心ヤコビアンを計算するとします．(このやり方であっているのかはわからないんですがシミュレータで逆行列といて動かした時はそれっぽい動きをしてました)

上記コードを実行すると計算結果が出てきます.でかい行列が表示されて見辛いですが...重心運動が変位した時これの逆行列解いて計算することで関節運動の変位量が出てくるんですが正則行列ではないので擬似逆行列を解く必要がありますね.

まとめ

重心ヤコビアンを解いてみましたが正直これだけじゃわからないので次は逆行列まで解いてPuluginとして実装してchoreonoid上で動かしてみようと思います