前回はロボットモデルファイルを読み込みリンク情報を取得する方法について書きました.今回は順運動学・逆運動学の計算をしてみます.

環境

OS: ubuntu 16.04 LTS
version: choreonoid-1.6.0

サンプルコード

今回のケースでは，腰を基準として左足先の位置をz軸へ変位させた場合の逆運動学を計算します．

kinematics.cpp

#include <iostream>
#include <cstring>

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

using namespace std;
using namespace cnoid;

// Degree -> Radian
double deg2rad(double degree)
{
    return degree * M_PI / 180.f;
}

// Radian -> Degree
double rad2deg(double radian)
{
    return radian * 180.f / M_PI;
}

int main()
{
    BodyLoader bodyloader;

    std::string model_path("/usr/share/choreonoid-1.6/model/SR1/SR1.body");
    BodyPtr robot = bodyloader.load(model_path.c_str());

    JointPathPtr lleg = getCustomJointPath(robot, robot->rootLink(), robot->link("LLEG_ANKLE_R"));

    robot->joint(14)->q() = deg2rad(-20);
    robot->joint(16)->q() = deg2rad(40);
    robot->joint(17)->q() = deg2rad(-20);

    lleg->calcForwardKinematics();

    for(int i=13;i<19;i++)
        cout << rad2deg(robot->joint(i)->q()) << endl;
    cout << "\n";

    Vector3d ref_pos = robot->link("LLEG_ANKLE_R")->p();
    Matrix3d ref_rot = robot->link("LLEG_ANKLE_R")->R();

    ref_pos.z() += 0.01;
    if(lleg->calcInverseKinematics(ref_pos, ref_rot)){
        for(int i=13;i<19;i++)
            cout << rad2deg(robot->joint(i)->q()) << endl;
    }

    return 0;
}

前回同様，モデルファイルをまず読み込みます．

ここで出てきたJointPathPtrですが，BodyPtrと同様でJointPathクラスを扱うためのポインタになります．JointPathクラスはリンク間の親子関係や順序を設定し運動学を計算する部分です．なのでこのクラスを呼び出して関数を叩くことで内部で逆運動学計算をすることができます．

getCustomJointPath関数はベースリンク，目標リンクを指定し，その間のリンク関係を定義します．第一引数がBodyPtr，第二引数がベースリンク，第三引数が目標リンクです．

以降から運動学の計算に移ります．

逆運動学を計算するために膝を少し曲げておきます．関節角度の操作は以下のようにして行うことが可能です．

robot->joint(関節ID)->q() = 関節角度(ラジアン)

関節角度を入れたら，リンクの位置・姿勢を更新するために順運動学計算をします．デフォルトではfalseですが，trueにすると速度・加速度の更新までやってくれるみたいです．

void JointPath::calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false)

続いて逆運動学の計算です．逆運動学の計算は以下の関数を呼び出せばOKです．

bool JointPath::calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R)

第一引数に目標リンクの位置，第二引数に目標リンクの姿勢を入れることで逆運動学を計算することが出来ます．choreonoidでは中のベクトルや行列はEigenを使用しているので以下のようにしてリンクの位置・姿勢を抜き出して目標位置・姿勢を入れてたものを関数の引数として入れることが可能です．

// 対象リンクの位置・姿勢を抜き出す．
Vector3d ref_pos = robot->link("LLEG_ANKLE_R")->p();
Matrix3d ref_rot = robot->link("LLEG_ANKLE_R")->R();

// 位置の変位
ref_pos.z() += 0.01;

// IK演算に失敗したらfalseを返す
lleg->calcInverseKinematics(ref_pos, ref_rot);

上記のサンプルをコンパイルして実行すると，逆運動学計算前の左足の角度と計算後の角度が表示されます．

まとめ

choreonoidのライブラリを使用して運動学の計算をしてみました．次は重心ヤコビアンあたりを...