c++ - OpenGL/Eigen3 を使用した逆運動学: 不安定なヤコビ疑似逆行列

Question

OpenGL、Eigen3、および「Jacobian pseudoinverse」メソッドを使用して、単純な逆運動学テストを実装しようとしています。

システムは「Jacobian 転置」アルゴリズムを使用して正常に動作しますが、「pseudoinverse」を使用しようとするとすぐに、ジョイントが不安定になり、けいれんを開始します (最終的には完全にフリーズします - 「Jacobian 転置」フォールバック計算を使用しない限り)。この問題を調査したところ、Jacobian.inverse()*Jacobian の行列式がゼロで反転できない場合があることがわかりました。ただし、同じ方法を使用すると主張するインターネット (Youtube) 上の他のデモを見たことがありますが、この問題はないようです。したがって、問題の原因がどこにあるのかわかりません。以下にコードを添付します。

*.h:

struct Ik{
    float targetAngle;
    float ikLength;
    VectorXf angles;
    Vector3f root, target;
    Vector3f jointPos(int ikIndex);
    size_t size() const;
    Vector3f getEndPos(int index, const VectorXf& vec);
    void resize(size_t size);
    void update(float t);
    void render();
    Ik(): targetAngle(0), ikLength(10){
    }
};

*.cpp:

size_t Ik::size() const{
    return angles.rows();
}

Vector3f Ik::getEndPos(int index, const VectorXf& vec){
    Vector3f pos(0, 0, 0);
    while(true){
        Eigen::Affine3f t;
        float radAngle = pi*vec[index]/180.0f;
        t = Eigen::AngleAxisf(radAngle, Vector3f(-1, 0, 0))
            * Eigen::Translation3f(Vector3f(0, 0, ikLength));
        pos = t * pos;

        if (index == 0)
            break;
        index--;
    }
    return pos;
}

void Ik::resize(size_t size){
    angles.resize(size);
    angles.setZero();
}

void drawMarker(Vector3f p){
    glBegin(GL_LINES);
    glVertex3f(p[0]-1, p[1], p[2]);
    glVertex3f(p[0]+1, p[1], p[2]);
    glVertex3f(p[0], p[1]-1, p[2]);
    glVertex3f(p[0], p[1]+1, p[2]);
    glVertex3f(p[0], p[1], p[2]-1);
    glVertex3f(p[0], p[1], p[2]+1);
    glEnd();
}

void drawIkArm(float length){
    glBegin(GL_LINES);
    float f = 0.25f;
    glVertex3f(0, 0, length);
    glVertex3f(-f, -f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(f, -f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(f, f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(-f, f, 0);
    glEnd();
    glBegin(GL_LINE_LOOP);
    glVertex3f(f, f, 0);
    glVertex3f(-f, f, 0);
    glVertex3f(-f, -f, 0);
    glVertex3f(f, -f, 0);
    glEnd();
}

void Ik::update(float t){
    targetAngle += t * pi*2.0f/10.0f;
    while (t > pi*2.0f)
        t -= pi*2.0f;
    target << 0, 8 + 3*sinf(targetAngle), cosf(targetAngle)*4.0f+5.0f;

    Vector3f tmpTarget = target;
    Vector3f targetDiff = tmpTarget - root;
    float l = targetDiff.norm();
    float maxLen = ikLength*(float)angles.size() - 0.01f;
    if (l > maxLen){
        targetDiff *= maxLen/l;
        l = targetDiff.norm();
        tmpTarget = root + targetDiff;
    }

    Vector3f endPos = getEndPos(size()-1, angles);
    Vector3f diff = tmpTarget - endPos;


    float maxAngle = 360.0f/(float)angles.size();

    for(int loop = 0; loop < 1; loop++){
        MatrixXf jacobian(diff.rows(), angles.rows());
        jacobian.setZero();
        float step = 1.0f;
        for (int i = 0; i < angles.size(); i++){
            Vector3f curRoot = root;
            if (i)
                curRoot = getEndPos(i-1, angles);
            Vector3f axis(1, 0, 0);
            Vector3f n = endPos - curRoot;
            float l = n.norm();
            if (l)
                n /= l;
            n = n.cross(axis);
            if (l)
                n *= l*step*pi/180.0f;
            //std::cout << n << "\n";

            for (int j = 0; j < 3; j++)
                jacobian(j, i) = n[j];
        }

        std::cout << jacobian << std::endl;
        MatrixXf jjt = jacobian.transpose()*jacobian;
        //std::cout << jjt << std::endl;
        float d = jjt.determinant();
        MatrixXf invJ; 
        float scale = 0.1f;
        if (!d /*|| true*/){
            invJ = jacobian.transpose();
            scale = 5.0f;
            std::cout << "fallback to jacobian transpose!\n";
        }
        else{
            invJ = jjt.inverse()*jacobian.transpose();
            std::cout << "jacobian pseudo-inverse!\n";
        }
        //std::cout << invJ << std::endl;

        VectorXf add = invJ*diff*step*scale;
        //std::cout << add << std::endl;
        float maxSpeed = 15.0f;
        for (int i = 0; i < add.size(); i++){
            float& cur = add[i];
            cur = std::max(-maxSpeed, std::min(maxSpeed, cur));
        }
        angles += add;
        for (int i = 0; i < angles.size(); i++){
            float& cur = angles[i];
            if (i)
                cur = std::max(-maxAngle, std::min(maxAngle, cur));
        }
    }
}

void Ik::render(){
    glPushMatrix();
    glTranslatef(root[0], root[1], root[2]);
    for (int i = 0; i < angles.size(); i++){
        glRotatef(angles[i], -1, 0, 0);
        drawIkArm(ikLength);
        glTranslatef(0, 0, ikLength);
    }
    glPopMatrix();
    drawMarker(target);
    for (int i = 0; i < angles.size(); i++)
        drawMarker(getEndPos(i, angles));
}

スクリーンショット

score 4 · Accepted Answer

システムが硬すぎるようです。

Eigen SVD メソッドを使用してみてください: http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.htmlを参照してください。これはより計算量が多いですが、おそらくより安全です。aX=b 問題を解く場合は、逆行列専用のメソッドを使用するのが最善です。(a はあなたのヤコビアンで、X はあなたが望むものです)。
最後に、対角線に 1.0001 などの係数を掛けて、行列の条件付けをごまかすようにしてください。これによって解決策が大きく変わることはありません (特にゲームの場合) が、より適切な解決策が可能になります。
RK4 などの時間反復方式を使用しない理由を知りたいです。

編集:私はあなたの広範な投稿の多くを読んでいなかったので、スプリングへの参照を削除しました. あなたの場合、要素には何らかの形の機械的相互作用があると思います。

score 1 · Accepted Answer

このようなものが機能するはずです。

VectorXf solveViaSVD(const MatrixXf & jacobian,const VectorXf & diff) {
     Eigen::JacobiSVD<MatrixXf> svd (jacobian,ComputeThinU | ComputeThinV);
     return svd.solve(diff);
}

問題は、あなたが言ったように、（J*J'）が特異である場合、メソッドが疑似逆行列を計算できないことです。

ウィキペディアによると、「[疑似逆行列]は、ゼロ以外のすべての対角要素をその逆数で置き換えることによって形成されます」。「非ゼロ」ポイントとしての計算pinv(A)は失敗します。inv(A'*A)*A'

c++ - OpenGL/Eigen3 を使用した逆運動学: 不安定なヤコビ疑似逆行列

2 に答える 2

Related

Reference