Purpose of Interpolation Factor in Error Vector Calculation for IK

[zorro]

Hello！

I'm currently working on an inverse kinematics problem and have come across a specific implementation detail that I'm hoping to get some clarification on.

In the code I'm working with（which is the getJacbian() from ikElement.cpp）, the error vector is calculated using an interpolation factor (\(dq\)). Specifically, the position and orientation errors are computed between the end effector's current pose and an interpolated pose, which is a fraction \(dq\) of the way towards the target pose. The errors are then divided by \(dq\) to form the final error vector. Here is the relevant code snippet:

```cpp

Code: Select all

if ((jacob.rows != 0) && (jacob.cols != 0))
{
    retVal = true;
    if (target != nullptr)
    {
        errVect.resize(jacob.rows, 1, 0.0);
        C7Vector constrBasePoseInv(C7Vector::identityTransformation);
        if (constraintBase != nullptr)
            constrBasePoseInv = constraintBase->getCumulativeTransformationPart1().getInverse();
        C7Vector tipTrRel(constrBasePoseInv * tip->getCumulativeTransformationPart1());
        C7Vector targetTrRel(constrBasePoseInv * target->getCumulativeTransformationPart1());
        targetTrRel.buildInterpolation(tipTrRel, targetTrRel, interpolationFactor);

        if (((constraints & ik_constraint_orientation) == ik_constraint_alpha_beta) || ((constraints & ik_constraint_orientation) == ik_constraint_gamma))
        {
            if ((constraints & ik_constraint_orientation) == ik_constraint_alpha_beta)
            {
                C3Vector tipXaxisProj(targetTrRel.Q.getInverse() * tipTrRel.Q.getMatrix().axis[0]);
                double angle = tipXaxisProj.getAngle(C3Vector::unitXVector);
                if (fabs(angle) > 0.001)
                {
                    if (tipXaxisProj(1) < 0.0)
                        angle = -angle;
                    C4Vector q(angle, C3Vector::unitZVector);
                    targetTrRel.Q *= q;
                }
            }
            else
            {
                C3Vector tipZaxis(tipTrRel.Q.getMatrix().axis[2]);
                C3Vector targetZaxis(targetTrRel.Q.getMatrix().axis[2]);
                double angle = targetZaxis.getAngle(tipZaxis);
                if (angle > 0.001)
                {
                    C4Vector q(angle, (targetZaxis ^ tipZaxis).getNormalized());
                    targetTrRel.Q = q * targetTrRel.Q;
                }
            }
        }

        C7Vector interpolTargetRel;
        double dq = 0.01; // Interpolation factor
        interpolTargetRel.buildInterpolation(tipTrRel, targetTrRel, dq);

        C3Vector euler((tipTrRel.Q.getInverse() * interpolTargetRel.Q).getEulerAngles());
        C3Vector dx_x(interpolTargetRel.X - tipTrRel.X);

        size_t rowIndex = 0;
        if ((constraints & ik_constraint_x) != 0)
        {
            errVect(rowIndex++, 0) = dx_x(0) / dq;
            if (equTypes != nullptr)
                equTypes->push_back(0);
        }
        if ((constraints & ik_constraint_y) != 0)
        {
            errVect(rowIndex++, 0) = dx_x(1) / dq;
            if (equTypes != nullptr)
                equTypes->push_back(1);
        }
        if ((constraints & ik_constraint_z) != 0)
        {
            errVect(rowIndex++, 0) = dx_x(2) / dq;
            if (equTypes != nullptr)
                equTypes->push_back(2);
        }
        if ((constraints & ik_constraint_orientation) != 0)
        {
            if ((constraints & ik_constraint_alpha_beta) != 0)
            {
                errVect(rowIndex++, 0) = backCompatibility * euler(0) / dq;
                errVect(rowIndex++, 0) = backCompatibility * euler(1) / dq;
                if (equTypes != nullptr)
                {
                    equTypes->push_back(3);
                    equTypes->push_back(4);
                }
            }
            if ((constraints & ik_constraint_gamma) != 0)
            {
                errVect(rowIndex++, 0) = backCompatibility * euler(2) / dq;
                if (equTypes != nullptr)
                    equTypes->push_back(5);
            }
        }
    }
}

return retVal;

```

From my understanding, this approach effectively calculates the position and orientation errors and scales them by the factor \(dq\). Mathematically, this seems equivalent to directly using the difference between the target pose and the current pose to form the error vector.

So what is the significance or benefit of using this interpolation factor (\(dq\)) in the error calculation process instead of directly computing the difference between the target and current poses?

Does this method provide any numerical advantages or help in specific cases where direct computation might fail or be less efficient? Any insights or explanations would be greatly appreciated.

Thank you!

[zorro]

Sorry! I missed this line of code:

Code: Select all

targetTrRel.buildInterpolation(tipTrRel, targetTrRel, interpolationFactor);

It seems that all subsequent operations are based on this interpolated targetTrRel, but why is interpolation done again later (interpolTargetRel.buildInterpolation(tipTrRel, targetTrRel, dq);)?