我试图用具有7个自由度的伪逆来动画角色的手臂。 整体FK是这样的：
（x，y，x）= 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();
}
}
IK解算器功能：
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。
答案 0 :(得分：2)
讨厌继续回答我自己的问题，但到目前为止，我一直在努力解决这个问题。 问题出现在两个地方：