OpenGL / Eigen3 Reverse Kinematics: Unstable Jacobian Pseudo Inversion

Question

I am trying to implement a simple inverse kinematic test using OpenGL, Eigen3 and "Jacobian pseudoinverse" .

The system works fine using the "Jacobian transpose" algorithm, however, as soon as I try to use "pseudo-inverse", the joints become unstable and start jerking (they finally freeze completely) if I do not use the "Jacobian transpose backup calculation"), I investigated the problem and found out that in some cases Jacobian.inverse () * Jacobian has a zero determinant and cannot be inverted. However, I have seen other demos on the Internet (Youtube) that claim to use the same method and they don't seem to have this problem. Therefore, I do not know where the cause of the problem is. The code is attached below:

.

* 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){ } }; 

.

* castes:

 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

It looks like your system is too rigid.

• Try using the Eigen SVD methods: see http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.html . It is more computationally intensive, but also possibly more secure. If you solve the problem aX = b, it is best to use methods designed to invert the matrix. (a is your Jacobian, and X is what you want).
• Finally, try fooling the conditioning on your matrix by multiplying the diagonal by a factor of 1.0001. This will not change the solution much (especially for the game), but will give a better solution.
• I am curious why you decided not to use an iterative timing scheme like RK4.

edit: I did not read most of your extensive post, so I deleted the link to the sources. I think in your case the elements will have some form of mechanical interaction.

Answer 2

Something like this should work.

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

The problem, as you said, is that your method cannot calculate the pseudo-inverse when ( J*J' ) is singular.

Wikipedia says: "[pseudo-inverse] is formed by replacing each non-zero diagonal entry with its inverse." The calculation of pinv(A) as inv(A'*A)*A' does not correspond to a non-zero point.