c++ - 逆运动学算法的帮助

我正在尝试以2D方式实现CCD逆运动学

该功能应该执行CCD的1次迭代

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

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

我骨头的工作方式是:
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();
        }

谢谢

最佳答案

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

您可以更简单地计算rotAng

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

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

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