choreonoid 플러그인 만들기

지난번에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 공식 홈페이지를 참고했다(또는 대체적으로 유용한 것 같다...).상세한 해설은 저쪽에 기재되어 있으니 저쪽을 참조하는 것을 추천합니다.기본적으로choreonoid의GUI에2개의버튼을추가하고,그버튼을누르면목표의중심위치를설정하는구조다.
choreonoid에서 GUI 부분은 Qt로 구성되어 있으며 내부에 다양한 자물쇠 함수가 준비되어 있어 일반적인 Qt 코드를 쓰는 것보다 choreonoid의 GUI를 간단하게 조작할 수 있다.
일반적으로 말하는 신호와 슬롯은 아래의 기술을 통해 버튼 설정과 처리를 실현할 수 있다.

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

onButonClicked 함수는 버튼을 누를 때 실행되는 함수입니다.initialize 함수에서 정의합니다.여기서 하는 일은 버튼을 누르고 목표의 중심 위치를 설정한 후 대상 모델에 중심 아크비를 풀고 관절 각도를 계산하는 것이다.관절 각도를 계산하기 위해 뉴턴 랩슨으로 간단한 수렴 계산을 했다.중심×로봇의 자유도로 구성된 매트릭스로 기본 규칙이 아니기 때문에 보통 역 매트릭스를 풀 수 없다.여기를 QR로 분해한다.
이 플러그인은 동력학 시뮬레이션에서 실행되지 않기 때문에 관절 각도를 반영하더라도 GUI에 나타나지 않기 때문에 다음과 같이 GUI에 반영한다.

bodyItems[i]->notifyKinematicStateChange(true);

실행 후 아래와 같다.

이번 코드를 포함해 지금까지 샘플여기.이 공개됐다.
총결산
플러그인을 설치하여 비헤이비어를 확인합니다.다음은 동력학 시뮬레이션을 해보고 싶어서 심플컨트롤러로 PD 제어를 하고 싶어요.