OpenGL/Eigen3を用いた逆運動学：不安定なヤコビアン擬似逆

Question

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

システムは "Jacobian Transpose"アルゴリズムを使用してうまく動作しますが、 "pseudoinverse"を使用しようとすると、ジョイントが不安定になり、動きが激しくなります。 ）。私はこの問題を調査したが、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)); 
} 

Answer 1

が鳴ります。

1. Eigen SVDメソッドを使用してください。http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.htmlを参照してください。これは計算上の強さが増しますが、おそらくもっと安全です。 aX = bの問題を解く場合は、行列を反転する方法を使用することをお勧めします。 （あなたのヤコビとXはあなたが望むものです）。
2. 最後に、対角に1.0001のような係数を掛けて、行列のコンディショニングを欺くようにしてください。これは解決策をあまり変えず（特にゲームの場合）、より良い解決を可能にします。
3. なぜRK4などの時間反復スキームを使用しないのが好きなのか不思議です。

編集：私はあなたの大量の記事の多くを読まなかったので、私は春のリファレンスを削除しました。あなたのケースでは、要素は機械的な相互作用の何らかの形を持っていると思います。

Answer 2

何かこれはうまくいくはずです。あなたが言ったように

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

問題は、あなたの方法は、（J*J'）が特異であるとき、擬似逆行列を計算するために失敗したこと、です。

Wikipediaは次のように述べています。 "[逆疑似逆行列]はすべて逆行列での0以外の対角エントリを置き換えることで形成されます。 pinv(A)をinv(A'*A)*A'として計算すると、「非ゼロ点」に失敗します。