逆运动学算法帮助

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

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

你可以更简单地计算rotAng

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

给出相同的结果（假设向量不为零）。

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