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)。

帮助反向运动学算法

1 个答案: