I'm trying to implement simple inverse kinematics test using OpenGL, Eigen3 and "jacobian pseudoinverse" method.
The system works fine using "jacobian transpose" algorithm, however, as soon as I attempt to use "pseudoinverse", joints become unstable and start jerking around (eventually they freeze completely - unless I use "jacobian transpose" fallback computation). I've investigated the issue and turns out that in some cases jacobian.inverse()*jacobian has zero determinant and cannot be inverted. However, I've seen other demos on the internet (youtube) that claim to use same method and they do not seem to have this problem. So I'm uncertain where is the cause of the issue. 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){
}
};
*.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));
}
Any help will be appreciated.
