Я пытаюсь реализовать простой тест обратной кинематики, используя OpenGL, Eigen3 и "Jacobian псевдоинверсный" метод.
Система отлично работает с использованием алгоритма «Jacobian transpose», однако, как только я пытаюсь использовать «псевдоинверсию», суставы становятся нестабильными и начинают дергаться (в конце концов они полностью зависают - если я не использую резервное вычисление «Jacobian transpose»). Я исследовал проблему и обнаружил, что в некоторых случаях 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));
}