爪車

順運動学と逆運動学のプログラム

順運動学と逆運動学(6自由度)の簡易的なプログラム。
6自由度のリンクのみに対応しています。
本格的に用いるなら、OpenHRPChoreonoidの導入をお勧めします。

以下のコードはChoreonoidのソースコードを参考にしています。
行列計算にはEigenライブラリを用いています。

#include <iostream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
class Link;
double rad2deg(double rad); // radianからdegreeへ変換
double deg2rad(double deg); // degreeからradianへ変換
Eigen::Matrix3d rodrigues(const Eigen::Vector3d& axis, double q); // ロドリゲスの式
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R); // 行列対数関数
void calcForwardKinematics(Link* link); // 順運動学
Eigen::MatrixXd calcJacobian(Link* links, int numLinks, Link* targetLink); // ヤコビアンの計算
void calcInverseKinematics(Link* links, int numLinks, Link* targetLink, const Eigen::Vector3d& target_p, const Eigen::Matrix3d& target_R); // 逆運動学
Eigen::Matrix3d rotFromRpy(double r, double p, double y); // roll, pitch, yawから回転行列を求める
class Link
{
public:
Link() {
parent = NULL;
child = NULL;
sibling = NULL;
p.setZero();
R.setIdentity();
q = 0.0;
b.setZero();
a.setZero();
}
virtual ~Link() {}
std::string name; // リンクの名前
Link* parent; // 親リンク
Link* child; // 子リンク
Link* sibling; // 兄弟リンク
Eigen::Vector3d p; // 位置
Eigen::Matrix3d R; // 姿勢
double q; // 回転角度
Eigen::Vector3d b; // 親リンクからの相対位置
Eigen::Vector3d a; // 回転軸
};
// radianからdegree
double rad2deg(double rad)
{
return (180/M_PI) * rad;
}
// degreeからradian
double deg2rad(double deg)
{
return (M_PI/180) * deg;
}
// ロドリゲスの式
Eigen::Matrix3d rodrigues(const Eigen::Vector3d& axis, double q) {
return Eigen::AngleAxisd(q, axis).toRotationMatrix();
}
// 行列対数関数
// Choreonoid 1.4.0 から借用
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R)
{
    double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;
    if(fabs(alpha - 1.0) < 1.0e-6) {   //th=0,2PI;
        return Eigen::Vector3d::Zero();
    } else {
        double th = acos(alpha);
        double s = sin(th);
        if (s < std::numeric_limits<double>::epsilon()) {   //th=PI
            return Eigen::Vector3d( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th );
        }
        double k = -0.5 * th / s;
        return Eigen::Vector3d((R(1,2) - R(2,1)) * k,
                       (R(2,0) - R(0,2)) * k,
                       (R(0,1) - R(1,0)) * k);
    }
}
// 順運動学
void calcForwardKinematics(Link* link)
{
if (!link) return;
Link* parent = link->parent;
if (parent) {
link->p = parent->p + parent->R * link->b;
link->R = parent->R * rodrigues(link->a, link->q);
}
calcForwardKinematics(link->sibling);
calcForwardKinematics(link->child);
}
// ヤコビアンの計算
Eigen::MatrixXd calcJacobian(Link* links, int numLinks, Link* targetLink)
{
Eigen::MatrixXd J(6, numLinks); // (位置+姿勢の6自由度 x Joint数)
for (int i = 0; i < numLinks; ++i) {
Link* link = &links[i];
Eigen::Vector3d omega = link->R * link->a;
Eigen::Vector3d arm = targetLink->p - link->p;
Eigen::Vector3d dp = omega.cross(arm);
J(0, i) = dp(0);
J(1, i) = dp(1);
J(2, i) = dp(2);
J(3, i) = omega(0);
J(4, i) = omega(1);
J(5, i) = omega(2);
}
return J;
}
// 逆運動学 (6自由度)
void calcInverseKinematics(Link* links, int numLinks, Link* targetLink, const Eigen::Vector3d& target_p, const Eigen::Matrix3d& target_R)
{
    const int MAX_IK_ITERATION = 50;
    const double lambda = 0.5;
Link* rootLink = &links[0];
    Eigen::VectorXd dq(numLinks);
    Eigen::VectorXd v(6);
    for (int i = 0; i < MAX_IK_ITERATION ; ++i) {
    Eigen::MatrixXd J = calcJacobian(links, numLinks, targetLink);
    Eigen::Vector3d dp = target_p - targetLink->p;
    Eigen::Vector3d omega = targetLink->R * omegaFromRot(targetLink->R.transpose() * target_R);
    // 誤差の計算
    double err = sqrt(dp.dot(dp) + omega.dot(omega));
    if (err < 1e-6) {
//     std::cout << "err = " << err << std::endl;
//     std::cout << "iteration = " << i << std::endl;
    break;
    }
    v << dp, omega;
    dq = J.inverse() * v;
    for (int j = 0; j < numLinks; ++j) {
    links[j].q += lambda * dq(j);
    }
    calcForwardKinematics(rootLink);
    }
}
// roll, pitch, yawから回転行列を求める
// Choreonoid 1.4.0 から借用
Eigen::Matrix3d rotFromRpy(double r, double p, double y)
{
    const double cr = cos(r);
    const double sr = sin(r);
    const double cp = cos(p);
    const double sp = sin(p);
    const double cy = cos(y);
    const double sy = sin(y);
    Eigen::Matrix3d R;
    R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy,
         cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy,
         -sp  , sr*cp           , cr*cp;
    return R;
}
int main(int argc, char* argv[])
{
enum {
HIP_Y = 0,
HIP_R,
HIP_P,
KNEE_P,
ANKLE_P,
ANKLE_R,
NUM_LINKS
};
// リンクの設定
Link links[NUM_LINKS];
links[HIP_Y].name = "HIP_Y";
links[HIP_Y].parent = NULL;
links[HIP_Y].child = &links[HIP_R];
links[HIP_Y].b << 0.0, 0.0, 0.0;
links[HIP_Y].a << 0.0, 0.0, 0.1; // yaw
links[HIP_R].name = "HIP_R";
links[HIP_R].parent = &links[HIP_Y];
links[HIP_R].child = &links[HIP_P];
links[HIP_R].b << 0.0, 0.0, 0.0;
links[HIP_R].a << 1.0, 0.0, 0.0; // row
links[HIP_P].name = "HIP_P";
links[HIP_P].parent = &links[HIP_R];
links[HIP_P].child = &links[KNEE_P];
links[HIP_P].b << 0.0, 0.0, -0.02;
links[HIP_P].a << 0.0, 1.0, 0.0; // pitch
links[KNEE_P].name = "KNEE_P";
links[KNEE_P].parent = &links[HIP_P];
links[KNEE_P].child = &links[ANKLE_P];
links[KNEE_P].b << 0.0, 0.0, -0.1;
links[KNEE_P].a << 0.0, 1.0, 0.0; // pitch
links[ANKLE_P].name = "ANKLE_P";
links[ANKLE_P].parent = &links[KNEE_P];
links[ANKLE_P].child = &links[ANKLE_R];
links[ANKLE_P].b << 0.0, 0.0, -0.1;
links[ANKLE_P].a << 0.0, 1.0, 0.0; // pitch
links[ANKLE_R].name = "ANKLE_R";
links[ANKLE_R].parent = &links[ANKLE_P];
links[ANKLE_R].child = NULL;
links[ANKLE_R].b << 0.0, 0.0, 0.0;
links[ANKLE_R].a << 1.0, 0.0, 0.0; // row
// 順運動学
links[HIP_Y].q   = 0.0;
links[HIP_R].q   = 0.0;
links[HIP_P].q   = deg2rad(-45);
links[KNEE_P].q  = deg2rad(90);
links[ANKLE_P].q = deg2rad(-45);
links[ANKLE_R].q = 0.0;
Link* rootLink = &links[HIP_Y];
std::cout << "ForwardKinematics" << std::endl;
calcForwardKinematics(rootLink);
std::cout << "ANKLE_R p = " << links[ANKLE_R].p.transpose() << std::endl;
std::cout << "ANKLE_R R = \n" << links[ANKLE_R].R << std::endl;
// 逆運動学
Eigen::Vector3d target_p;
Eigen::Matrix3d target_R;
target_p << 0.0, 0.01, -0.12;
// target_R = rotFromRpy(0.0, deg2rad(10), 0.0);
target_R.setIdentity();
std::cout << "InverseKinematics" << std::endl;
calcInverseKinematics(links, NUM_LINKS, &links[ANKLE_R], target_p, target_R);
std::cout << "ANKLE_R p = " << links[ANKLE_R].p.transpose() << std::endl;
std::cout << "ANKLE_R R = \n" << links[ANKLE_R].R << std::endl;
return 0;
}

Top

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

10 | 2013/11 | 12
- - - - - 1 2
3 4 5 6 7 8 9
10 11 12 13 14 15 16
17 18 19 20 21 22 23
24 25 26 27 28 29 30