1

CCD Inverse Kinematics を 2D で実装しようとしています

この関数は、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();
        }

ありがとう

4

1 に答える 1

2

いいえ、あなたが提供したプログラムのリストには明らかな間違いはありません。角度の変化とエンドエフェクタのrotAng新しい位置を正しく計算しています。(endX, endY)

rotAng次のようにもっと簡単に計算できます

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

同一の結果が得られます (ベクトルが非ゼロであると仮定します)。

エラーは、提供されたプログラム リスト以外の場所にあると思われます。inverseKinematics()で想定されているフォワード キネマティクスと、表示ルーチンや他の場所で使用される実際のフォワード キネマティクスとの間に矛盾がある可能性があります。手順の最後に順運動学を再計算して、システムの残りの部分がエンドエフェクタが にあることに同意するかどうかを確認してください(endX, endY)。

于 2011-06-03T16:36:43.323 に答える