c++ - 帮助反向运动学算法

Question

我正在尝试在 2D 中实现 CCD 逆向运动学

这个函数应该做 1 次 CCD 迭代

现在作为测试用例，我从左脚开始，然后停在骨盆处。

每次调用此函数时，骨架的骨骼都会更新。

我的骨骼工作方式是:getFrameX,Y,Angle 返回骨骼/效应器末端的绝对位置。这些在 CCD 的每次迭代中都会更新。getAngle,X, Y 返回相对值。

二传手也一样。

现在它永远不会停留在一个位置，每次我咯咯地笑一下鼠标，它就会随机逆时针移动骨骼。

我想知道是否有明显明显的错误可以为我指明正确的调试方向。

void inverseKinematics(float targetX, float targetY, skl::Bone* targetBone)
{

    std::string stopBone = "Pelvis";
        //===
        // Track the end effector position (the final bone)
        double endX = targetBone->getFrameX();
        double endY = targetBone->getFrameY();

        //===
        // Perform CCD on the bones by optimizing each bone in a loop 
        // from the final bone to the root bone
        bool modifiedBones = false;
        targetBone = targetBone->getParent();

        while(targetBone->getName() != stopBone)
        {
            // Get the vector from the current bone to the end effector position.
            double curToEndX = endX - targetBone->getFrameX();
            double curToEndY = endY - targetBone->getFrameY();
            double curToEndMag = sqrt( curToEndX*curToEndX + curToEndY*curToEndY );

            // Get the vector from the current bone to the target position.
            double curToTargetX = targetX - targetBone->getFrameX();
            double curToTargetY = targetY - targetBone->getFrameY();
            double curToTargetMag = sqrt(   curToTargetX*curToTargetX
                + curToTargetY*curToTargetY );

            // Get rotation to place the end effector on the line from the current
            // joint position to the target position.
            double cosRotAng;
            double sinRotAng;
            double endTargetMag = (curToEndMag*curToTargetMag);
            if( endTargetMag <= 0.1f )
            {
                cosRotAng = 1.0f;
                sinRotAng = 0.0f;
            }
            else
            {
                cosRotAng = (curToEndX*curToTargetX + curToEndY*curToTargetY) / endTargetMag;
                sinRotAng = (curToEndX*curToTargetY - curToEndY*curToTargetX) / endTargetMag;
            }

            // Clamp the cosine into range when computing the angle (might be out of range
            // due to floating point error).
            double rotAng = acosf( max(-1.0f, min(1.0f,cosRotAng) ) );
            if( sinRotAng < 0.0f )
                rotAng = -rotAng;

            // Rotate the end effector position.
            endX = targetBone->getFrameX() + cosRotAng*curToEndX - sinRotAng*curToEndY;
            endY = targetBone->getFrameY() + sinRotAng*curToEndX + cosRotAng*curToEndY;

            // Rotate the current bone in local space (this value is output to the user)
            targetBone->setAngle(SimplifyAngle(targetBone->getAngle() + rotAng));

            // Check for termination
            double endToTargetX = (targetX-endX);
            double endToTargetY = (targetY-endY);
            if( endToTargetX*endToTargetX + endToTargetY*endToTargetY <= 1.0f )
            {
                // We found a valid solution.
                return;
            }

            // Track if the arc length that we moved the end effector was
            // a nontrivial distance.
            if( !modifiedBones && fabs(rotAng)*curToEndMag > 0.0001f )
            {
                modifiedBones = true;
            }

            targetBone = targetBone->getParent();
        }

谢谢

Answer 1

不，您提供的程序 list 没有明显错误。您正在正确计算角度变化 rotAng 和末端执行器的新位置 (endX, endY)。

您可以更简单地计算rotAng

double rotAng = 
    atan2(curToTargetY, curToTargetX) - atan2(curToEndY, curToEndX);

给出相同的结果(假设 vector 非零)。

我怀疑错误出在您提供的程序列表之外的某个地方。 inverseKinematics() 中假设的正​​向运动学与显示例程和其他地方使用的实际正向运动学之间可能存在差异。尝试在程序结束时重新计算正向运动学，看看系统的其余部分是否同意末端执行器位于 (endX, endY)。