ChoreonoidのSimpleControllerを使ってみる #choreonoid

前回はchoreoniodのプラグインを作成してみました。前回のものだとシミュレーションを回していない状態で、関節に力が入っていない状態であるためシミュレーションを実行するとその場で崩れ落ちてしまいます。今回はSimpleControllerを用いてPD制御を実装し、実際にシミュレーションを回して直立させて見ようと思います。

環境

OS: ubuntu 16.04 LTS
version: choreonoid-1.7.0

サンプルコード

#ifndef _PDCONTROLLER_
#define _PDCONTROLLER_

#include <iostream>
#include <string>
#include <cnoid/SimpleController>
#include <cnoid/Link>

using namespace std;
using namespace cnoid;

class PDController : public cnoid::SimpleController
{
    Body* ioBody;
    vector<double> qref, qold;
    vector<double> pgain, dgain;
    double dt;

public:
    virtual bool initialize(SimpleControllerIO* io) override
    {
        ioBody = io->body();
        dt = io->timeStep();

        for(int i=0; i < ioBody->numJoints(); ++i){
            Link* joint = ioBody->joint(i);
            joint->setActuationMode(Link::JOINT_TORQUE);
            io->enableIO(joint);
            qref.push_back(joint->q());
        }
        qold = qref;

        load_gain(ioBody->numJoints(), pgain, dgain);

        return true;
    }

    void load_gain(size_t numJoints, vector<double> &pgain, vector<double> &dgain)
    {
        FILE *fp;
        double temp_pgain, temp_dgain;
        std::string gain_filepath = "/usr/lib/choreonoid-1.7/simplecontroller/pdgain.txt";

        if((fp = fopen(gain_filepath.c_str(),"r")) == NULL)
        {
            std::cerr << "gain file not found." << std::endl;
            return ;
        }

        pgain.clear(); dgain.clear();

        pgain.resize(numJoints);
        dgain.resize(numJoints);

        int i=0;
        while(fscanf(fp,"%lf %lf",&temp_pgain,&temp_dgain) != EOF)
        {
            pgain[i] = temp_pgain;
            dgain[i] = temp_dgain;
            i++;
        }

    }

    virtual bool control() override
    {
        for(int i=0; i < ioBody->numJoints(); ++i){
            Link* joint = ioBody->joint(i);
            double q = joint->q();
            double dq = (q - qold[i]) / dt;
            double u = (qref[i] - q) * pgain[i] + (0.0 - dq) * dgain[i];
            qold[i] = q;
            joint->u() = u;
        }
        return true;
    }
};

#endif

このサンプルで行っていることは簡単なPD制御で、外部からPゲイン、Dゲインをファイルで読み込み、トルク制御を行っています(このサンプルではゲインファイルを外部で指定して読み込んでいますが、これはゲインの微調整時にわざわざコンパイルが面倒という理由で外部ファイルで定義していますが、基本そんなに変える値ではないのでコード内に直に定義でも良いと思います)。

SimpleControllerとはchoreonoid向けに設計されたコントローラです。並んでいる関数はプラグインのときと似ていますがロードされた際initializeが一度呼び出され、シミュレーションが実行されるとcontrolがループで呼びだされます。ループ周期はchoreonoid側で設定され、その周期毎にループが走ります。SimpleController内で実行周期を取得する場合はSimpleControllerIOクラスのtimeStep関数を用います。

SimpleControllerは他のプロセスとのやり取りまで提供するわけではなく、簡単なコントローラの実装を目的としています。なので例えばトルク制御だけでなく画像認識やマニピュレーションの経路計画などのモジュールを組み合わせる場合はBodyRTCを用いたほうが良いと思います(OpenRTM向け)。

まとめ

シミュレーションを回してロボットを立たせることができるようになりました。次は実際にこれを使って簡単に制御っぽいことをしてみますかね。