7 DOF обратная кинематическая с якобианом и псевдообратным

Я застрял, пытаясь анимировать руку персонажа с помощью псевдообращения с 7 степенями свободы.
Общий ФК такой:

(х, у, х) =
TrootTshoulderRz (θ3) Ry (θ2) Rx (θ1) TelbowRy (θ5) Rx (θ4) TwristRy (θ7) Rz (θ6)
Pendeffector.

Я не уверен, что я делаю неправильно. Вот что у меня есть:
Это вычисляет преобразования и рисует результат, если это вызов рендеринга:

void BodyPart::DrawE(const Transform<float,3,2,0> *t,bool isCalculating)
{
if((isSimOn && isCalculating) || !isSimOn)
{
Affine3f trans;
trans = Affine3f::Identity();
mv = *t;
trans *= Translation3f(BodyPart::convertToEigenVector(Position + Pivot));
trans *= AngleAxisf(glm::radians(Rotation.z),Vector3f(0,0,1));
trans *= AngleAxisf(glm::radians(Rotation.y),Vector3f(0,1,0));
trans *= AngleAxisf(glm::radians(Rotation.x),Vector3f(1,0,0));
trans *= Translation3f(BodyPart::convertToEigenVector(-Position - Pivot));

trans *= Translation3f(BodyPart::convertToEigenVector(Position));
lmv = trans;
mv = mv * trans;if(isWrist)
{

Vector3f endf;
vec3 tef = Position+ 2.1f*Pivot;
endf << tef.x,tef.y,tef.z;
Vector3f e = mv * endf;
gEndEffector = e;  // world ef

endEffector = trans * endf; // local ef
}
}
if(!isCalculating)
{
const float *pSource = mv.data();
for (int i = 0; i < 16; i++) // to send to glLoadMatrixf()
modelM[i] = pSource[i];
Render();
}
}

ИК решатель функции:

int IkSimulator::step(double time)
{
time = 0.1;

Vector3f err(0.0f,0.0f,0.0f);
if(INDEX < (spline->cpCount-1)*100)
{

targetP = Vector3f(spline->pts[INDEX][0],spline->pts[INDEX][1],spline->pts[INDEX][2]);

do
{

float thetas[7];
currentP = ((Bob*)bob)->getEndEffector(); // in World cs
err = targetP - currentP;
Vector3f tP;
tP = (float)time*err+currentP;
MatrixXf j3 = ((Bob*)bob)->getJacobian3();
float *ts  = solve(j3,tP,thetas);
((Bob*)bob)->setDeltaThetas(ts[0],ts[1],ts[2],ts[3],ts[4],ts[5],ts[6]);
}while ((err).norm() > 0.1f);
((Bob*)bob)->addSplinePoints(vec3(currentP.x(),currentP.y(),currentP.z()));
INDEX++;
}
else
animTcl::OutputMessage("Reached end of the spline.\n") ;
return 0;
}
float* IkSimulator::solve(Eigen::MatrixXf J,Vector3f deltaP,float thetas[])
{
Vector3f JBeta = (J * J.transpose()).inverse() * deltaP;
Eigen::VectorXf Ts(7);
Ts = J.transpose() * JBeta;

thetas[0] = Ts(0);
thetas[1] = Ts(1);
thetas[2] = Ts(2);
thetas[3] = Ts(3);
thetas[4] = Ts(4);
thetas[5] = Ts(5);
thetas[6] = Ts(6);

return thetas;
}

Построение якобиана и обновление позиций, возвращаемых с шага симуляции:

MatrixXf getJacobian3()
{
MatrixXf jMat(3,7);
Vector3f e = bParts[7]->endEffector; // in Wrist's local cs

Affine3f wrist;
wrist =  bParts[7]->lmv;   // lmv is the local transformation

Affine3f elbow;
elbow =  bParts[6]->lmv;

Affine3f shoulder;
shoulder =  bParts[5]->lmv;

Affine3f root;
root = bParts[1]->lmv;

AngleAxisf dtheta;
Affine3f amc1,amc2,amc3,amc4,amc5,amc6,amc7;
amc1 = getAffTranslation(5,true);
amc1 *= getAffRotation('z',5);
amc1 *= getAffRotation('y',5);
amc1 *= dtheta.fromRotationMatrix(getDerRotation(5,'x'));
amc1 *= getAffTranslation(5,true,true);
amc1 *= getAffTranslation(5);
amc1 =  root * amc1 * elbow * wrist;

amc2 =  getAffTranslation(5,true);
amc2 *= getAffRotation('z',5);
amc2 *= dtheta.fromRotationMatrix(getDerRotation(5,'y'));
amc2 *= getAffRotation('x',5);
amc2 *= getAffTranslation(5,true,true);
amc2 *= getAffTranslation(5);
amc2 = root * amc2 * elbow * wrist;
//amc2 = root * amc2;

amc3 =  getAffTranslation(5,true);
amc3 *= dtheta.fromRotationMatrix(getDerRotation(5,'z'));
amc3 *= getAffRotation('y',5);
amc3 *= getAffRotation('x',5);
amc3 *= getAffTranslation(5,true,true);
amc3 *= getAffTranslation(5);
amc3 =  root * amc3 * elbow * wrist;
//amc3 = root * amc3;

amc4 =  getAffTranslation(6,true);
//amc4 *= getAffRotation('z',6);
amc4 *= getAffRotation('y',6);
amc4 *= dtheta.fromRotationMatrix(getDerRotation(6,'x'));
amc4 *= getAffTranslation(6,true,true);
amc4 *= getAffTranslation(6);
amc4 =  root * shoulder * amc4 * wrist;

amc5 =  getAffTranslation(6,true);
//amc5 *= getAffRotation('z',6);
amc5 *= dtheta.fromRotationMatrix(getDerRotation(6,'y'));
amc5 *= getAffRotation('x',6);
amc5 *= getAffTranslation(6,true,true);
amc5 *= getAffTranslation(6);
amc5 = root * shoulder * amc5 * wrist;

amc6 =  getAffTranslation(7,true);
amc6 *= getAffRotation('y',7);
amc6 *= dtheta.fromRotationMatrix(getDerRotation(7,'z'));
//amc6 *= getAffRotation('x',6);
amc6 *= getAffTranslation(7,true,true);
amc6 *= getAffTranslation(7);
amc6 = root * shoulder * elbow * amc6;

amc7 =  getAffTranslation(7,true);
amc7 *= dtheta.fromRotationMatrix(getDerRotation(7,'y'));
amc7 *= getAffRotation('z',7);
//amc7 *= getAffRotation('x',6);
amc7 *= getAffTranslation(7,true,true);
amc7 *= getAffTranslation(7);
amc7 = root * shoulder * elbow * amc7;

Vector3f c1 = amc1 * e;
Vector3f c2 = amc2 * e;
Vector3f c3 = amc3 * e;
Vector3f c4 = amc4 * e;
Vector3f c5 = amc5 * e;
Vector3f c6 = amc6 * e;
Vector3f c7 = amc7 * e;

vector<Vector3f> cs;
cs.push_back(c1);
cs.push_back(c2);
cs.push_back(c3);
cs.push_back(c4);
cs.push_back(c5);
cs.push_back(c6);
cs.push_back(c7);

for(int i=0;i<7;i++)
for(int j=0;j<3;j++)
jMat(j,i) = cs[i](j);

return jMat;
}
void setDeltaThetas(float t1,float t2, float t3, float t4, float t5, float t6, float t7)
{

bParts[5]->Rotation.x += t1;
bParts[5]->Rotation.y += t2;
bParts[5]->Rotation.z += t3;

bParts[6]->Rotation.x += t4;
bParts[6]->Rotation.y += t5;

bParts[7]->Rotation.z += t6;
bParts[7]->Rotation.y += t7;

bParts[1]->DrawE(&Affine3f::Identity(),true); // Torso
bParts[5]->DrawE(&bParts[1]->mv,true);  // Shoulder
bParts[6]->DrawE(&bParts[5]->mv,true);  // Elbow
bParts[7]->DrawE(&bParts[6]->mv,true);  // Wrist
}

Функции полезности, используемые в создании J:

Matrix3f getDerRotation(int i,char c)
{
Matrix3f tMat;
float coeff = 3.14159265f/180.f;
//float coeff = 1.f;
vec3 r = coeff*bParts[i]->Rotation;
if(c =='x')
{
tMat(0,0) = 0.f;
tMat(1,0) = 0.f;
tMat(2,0) = 0.f;

tMat(0,1) = 0.f;
tMat(1,1) = -glm::sin(r.x);
tMat(2,1) = glm::cos(r.x);

tMat(0,2) = 0.f;
tMat(1,2) = -glm::cos(r.x);
tMat(2,2) = -glm::sin(r.x);

}
else if(c =='y')
{
tMat(0,0) = -glm::sin(r.y);
tMat(1,0) = 0.f;
tMat(2,0) = -glm::cos(r.y);

tMat(0,1) = 0.f;
tMat(1,1) = 0.f;
tMat(2,1) = 0.f;

tMat(0,2) = glm::cos(r.y);
tMat(1,2) = 0.f;
tMat(2,2) = -glm::sin(r.y);
}
else
{
tMat(0,0) = -glm::sin(r.z);
tMat(1,0) = glm::cos(r.z);
tMat(2,0) = 0.f;

tMat(0,1) = -glm::cos(r.z);
tMat(1,1) = -glm::sin(r.z);
tMat(2,1) = 0.f;

tMat(0,2) = 0.f;
tMat(1,2) = 0.f;
tMat(2,2) = 0.f;
}
return tMat;
}
AngleAxisf getAffRotation(char r,int i)
{
AngleAxisf rot;
float coeff = glm::pi<float>()/180;
//float coeff = 1.f;
vec3 rott = coeff*bParts[i]->Rotation;
if(r =='x')
rot = AngleAxisf(rott.x,Vector3f(1,0,0));
else if(r=='y')
rot = AngleAxisf(rott.y,Vector3f(0,1,0));
else
rot = AngleAxisf(rott.z,Vector3f(0,0,1));
return rot;
}
Translation3f getAffTranslation(int i,bool doPivot = false,bool pos = false)
{
Translation3f trans;
if(doPivot)
if(!pos)
trans = Translation3f(BodyPart::convertToEigenVector(bParts[i]->Position + bParts[i]->Pivot));
else
trans = Translation3f(BodyPart::convertToEigenVector(-bParts[i]->Position - bParts[i]->Pivot));
else
trans = Translation3f(BodyPart::convertToEigenVector(bParts[i]->Position));

return trans;
}

Процесс не возвращает правильные значения, рука идет везде, кроме того, где она должна. Также извините за мое спорадическое использование GLM.