Помощь с алгоритмом обратной кинематики - PullRequest
Новая
       57

Помощь с алгоритмом обратной кинематики

1 голос
/ 03 июня 2011

Я пытаюсь реализовать CCD Inverse Kinematics в 2D

Эта функция должна делать 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();
        }

Спасибо

1 Ответ

2 голосов
/ 03 июня 2011

Нет, в указанном вами списке программ нет ничего явно неправильного.Вы правильно вычисляете изменение угла rotAng и новую позицию (endX, endY) конечного эффектора.

Вы можете вычислить rotAng более просто как 

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

, что даетидентичные результаты (при условии, что векторы отличны от нуля).

Я подозреваю, что ошибка находится где-то за пределами списка программ, который вы дали.Возможно, существует несоответствие между прямой кинематикой, предполагаемой в inverseKinematics(), и фактической прямой кинематикой, используемой в процедурах отображения и в других местах.Попробуйте пересчитать прямую кинематику в конце процедуры, чтобы увидеть, согласна ли остальная часть системы, что конечный эффектор находится в (endX, endY).

...