前回はchoreonoidのライブラリを使って重心ヤコビアンを計算してみました。今回は実際に重心ヤコビアンを使って関節角度を計算してみます。見やすくするためにchoreonoidのプラグインを作ってみます。

環境

OS: ubuntu 16.04 LTS
version: choreonoid-1.7.0

サンプルコード


# include <cnoid/Plugin>
# include <cnoid/ItemTreeView>
# include <cnoid/BodyItem>
# include <cnoid/ToolBar>
# include <cnoid/Body>
# include <cnoid/JointPath>
# include <cnoid/Jacobian>
# include <cnoid/BodyLoader>
# include <cnoid/Link>

# include <Eigen/Core>

using namespace std;
using namespace cnoid;
using namespace Eigen;

class ComJacobianPlugin : public Plugin
{
public:
    
    ComJacobianPlugin() : Plugin("ComJacobianTest")
    {;
        require("Body");
    }
    
    virtual bool initialize()
    {
        ToolBar* bar = new ToolBar("ComJacobianTest");
        bar->addButton("Increment")
            ->sigClicked().connect(bind(&ComJacobianPlugin::onButtonClicked, this, 0.02));
        bar->addButton("Decrement")
                ->sigClicked().connect(bind(&ComJacobianPlugin::onButtonClicked, this, -0.02));
        addToolBar(bar);

        return true;
    }

    void onButtonClicked(double ref_com_pos)
    {
        ItemList<BodyItem> bodyItems =
            ItemTreeView::mainInstance()->selectedItems<BodyItem>();

		const double ik_gain = 0.5;
		const int iteration = 100;
		const double erreps = 1e-06;
		ColPivHouseholderQR<MatrixXd> QR;
		const double ik_lambda = 1.0e-12;

		BodyPtr robot = bodyItems[0]->body();

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

		// 順運動学でリンクの位置姿勢更新
		robot->calcForwardKinematics();

		// ワールド座標系における重心位置を更新
		Vector3d cur_com(robot->calcCenterOfMass());
		Vector3d ref_com(cur_com);
		ref_com.y() += ref_com_pos;

		// 目標関節角度の差分
		VectorXd dq(robot->numJoints());
		// 収束ループ
		for(int n=0;n<iteration;n++)
		{
			if(ref_com.dot(ref_com) < erreps) break;
			// 重心ヤコビアン
			MatrixXd J_com(3, robot->numJoints());

			// 重心ヤコビアンを計算
			calcCMJacobian(robot, leg->baseLink(), J_com);

			MatrixXd JJ = J_com * J_com.transpose() + ik_lambda*MatrixXd::Identity(J_com.rows(), J_com.rows());
			dq = J_com.transpose() * QR.compute(JJ).solve(ref_com - cur_com) * ik_gain;
		}
        
        for(size_t i=0; i < bodyItems.size(); ++i){
            for(int j=0; j < robot->numJoints(); ++j)
                robot->joint(j)->q() += dq[j];
            bodyItems[i]->notifyKinematicStateChange(true);
        }
    }
};


CNOID_IMPLEMENT_PLUGIN_ENTRY(ComJacobianPlugin)

上記のサンプルコードのプラグイン記述部分はこちらのChoreonoid公式HPを参考にしています(というかほぼ流用してる感じです...)。詳しい解説はそちらに記載されているのでそちらを参照することをおすすめします。基本的にはchoreonoidのGUI上にボタンを２つ追加し、そのボタンを押すと目標重心位置をセットするという仕組みです。

choreonoidではGUI部分はQtで構成されますが、内部でいろいろラッパー関数が用意されており、通常のQtのコードを書くよりも簡単にchoreonoidのGUIを操作できるようになっています。

俗に言うシグナルとスロットですが、以下のように記述することでボタン配置と処理を実現することができます。

bar->addButton("Increment")
            ->sigClicked().connect(bind(&ComJacobianPlugin::onButtonClicked, this, 0.02)

onButtonClicked関数はボタンが押された際に実行される関数です。initialize関数の下で定義しています。こちらでやっていることはボタンが押され目標重心位置がセットされると対象のモデルに対して重心ヤコビアンを解いて関節角度を計算します。関節角度を計算するために簡易的にニュートンラプソンを用いた収束計算を実装しています。重心ヤコビアンは３×ロボットの自由度で構成される行列で基本正則ではないので逆行列を通常解くことができないです。ここではQR分解を用いて解いています。

このプラグインは動力学シミュレーションで実行されないため、関節角度を反映してもGUI上では反映されないので、以下のようにしてGUI上に反映させます。

bodyItems[i]->notifyKinematicStateChange(true);

実行すると以下のようになります。

今回のコード含め今までのサンプルはこちらで公開しています。

まとめ

プラグインを実装して挙動を確認してみました。次は動力学シミュレーションでやってみたいので、SimpleControllerを用いてPD制御を行ってみようと思います。