Question

我试图将IK应用于人体骨骼的手臂，该手臂的肩部位置固定，并且只有该手臂在移动。

手臂的FK看起来像这样

(x,y,z) = Root_T * Shoulder_T * Shoulder_Rx(q_0) * Shoulder_Ry(q_1) * 
Shoulder_Rz(q_2) * Elbow_T * Elbow_Rx(q_3) * Hand_T * Hand_Rx(q_4) * Position

这是我的IK解算器功能

//Q_desired is a 5*1 Vector whose entries are initialized as 0.0
//TargetP is where I want the end effector to be
void BoneRig::solveIK(glm::vec3& targetP, Eigen::VectorXf& Q_desired) {
    //Basic setups
    float error = 10.0f;
    glm::vec3 errorP;
    Eigen::VectorXf deltaQ(5);
    glm::vec3 endEffector = glm::vec3(0.0f,0.0f,0.0f);
    Eigen::MatrixXf J(3,5);
    Eigen::MatrixXf J_plus(5,3);
    float deltaTime = 0.1;

    int limit = 1000;
    int limitCounter = 0;

    while(error > 0.01){
        //gets the current endEffector location and stores it in endEffector
        getEndEffectorP(endEffector);
        errorP = targetP - endEffector;

        Eigen::VectorXf deltaX(3);
        deltaX << (errorP * deltaTime).x, (errorP * deltaTime).y, (errorP * deltaTime).z;

        getJacobian(J, targetP);
        J_plus = J.transpose();
        deltaQ = J_plus * deltaX;

        Q_desired += deltaQ;
        setOrientation(Q_desired);

        error = glm::length(errorP);

        if(limitCounter>= limit){
            std::cout << "You could not solve this"<<std::endl;
            break;
        }
    }
}

我认为问题出在setOrientation(Q_desired);函数中。 Q_desired = Q_desired + deltaQ在每个循环中都会更新DOF角度，因此我会相应地更新每个关节的旋转矩阵。这是setOrientation(Q_desired)函数。

//Joints[15] is the Shoulder joint, Joints[16] is the Elbow joint.
//Joints[17] is the Hand joint
void BoneRig::setOrientation(const Eigen::VectorXf& ceta_d){
    glm::mat4 temp = glm::mat4(1.0f);
    Joints[15].setR(glm::rotate(temp,ceta_d(0),glm::vec3(1.0f,0.0f,0.0f)));
    Joints[15].setR(glm::rotate(Joints[15].returnR(),ceta_d(1),glm::vec3(0.0f,1.0f,0.0f)));
    Joints[15].setR(glm::rotate(Joints[15].returnR(),ceta_d(2),glm::vec3(0.0f,0.0f,1.0f)));
    temp = glm::mat4(1.0f);
    Joints[16].setR(glm::rotate(temp,ceta_d(3),glm::vec3(1.0f, 0.0f, 0.0f)));
    temp = glm::mat4(1.0f);
    Joints[17].setR(glm::rotate(temp,ceta_d(4),glm::vec3(1.0f, 0.0f, 0.0f)));
    return;
}

我很好奇是否应该在那些旋转轴上应用角度旋转（应该是全局轴，而不是局部轴吗？）我没有发布getJacobian（）方法，因为我认为这是正确的，但是如果这段代码没有缺陷，我会的。任何帮助，将不胜感激。预先感谢！

编辑这就是我得到手臂的雅可比矩阵的方式。请注意，只有肩部是球形窝关节，其余只是基于局部x轴的旋转关节。

void BoneRig::getJacobian(Eigen::MatrixXf& J, glm::vec3& targetWorld){
    glm::vec4 xAxis = glm::vec4(1.0f,0.0f,0.0f,0.0f);
    glm::vec4 yAxis = glm::vec4(0.0f,1.0f,0.0f,0.0f);
    glm::vec4 zAxis = glm::vec4(0.0f,0.0f,1.0f,0.0f);
    glm::mat4 temp = Joints[0].returnT() * Joints[0].returnR() * Joints[11].returnT() * Joints[11].returnR() * Joints[14].returnT() * Joints[14].returnR() * Joints[15].returnT() * Joints[15].returnR();
    glm::vec3 worldXAxis = glm::vec3(temp * xAxis);
    glm::vec3 worldYAxis = glm::vec3(temp * yAxis);
    glm::vec3 worldZAxis = glm::vec3(temp * zAxis);
    glm::vec3 worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));
    glm::vec3 p = targetWorld - worldPos;

    glm::vec3 upperPart = glm::cross(worldXAxis,p);
    J(0,0) = upperPart.x;
    J(1,0) = upperPart.y;
    J(2,0) = upperPart.z;

    upperPart = glm::cross(worldYAxis,p);
    J(0,1) = upperPart.x;
    J(1,1) = upperPart.y;
    J(2,1) = upperPart.z;

    upperPart = glm::cross(worldZAxis,p);
    J(0,2) = upperPart.x;
    J(1,2) = upperPart.y;
    J(2,2) = upperPart.z;

    temp = temp * Joints[16].returnT() * Joints[16].returnR();
    worldXAxis = glm::vec3(temp * xAxis);
    worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));
    p = targetWorld - worldPos;
    upperPart = glm::cross(worldXAxis,p);
    J(0,3) = upperPart.x;
    J(1,3) = upperPart.y;
    J(2,3) = upperPart.z;
    temp = temp * Joints[17].returnT() * Joints[17].returnR();

    worldXAxis = glm::vec3(temp * xAxis);

    worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));

    p = targetWorld - worldPos;
    upperPart = glm::cross(worldXAxis,p);

    J(0,4) = upperPart.x;
    J(1,4) = upperPart.y;
    J(2,4) = upperPart.z;
}

我将此公式用于Jacobian矩阵的每一列。轴应该以全局坐标计算，这就是我在代码中所做的。

Answer 1

先生。 Schertler是对的。代码中的所有内容都是正确的，除了我不应该使用J_plus = J.transpose。相反，我应该使用Moore-Penrose伪逆，即

J_plus = (J.transpose() * J).inverse() * J.transpose() // This is for Overdetermined systems
J_plus = J.transpose * (J*J.transpose()).inverse() // This is for Underdetermined systems

在这种情况下，因为这是一个只有3行Jacobian矩阵的5DOF系统，所以我应该使用第一个版本。

再次感谢@Nico Schertler！

雅可比矩阵的5自由度逆运动学

1 个答案: