SDD Inverse Kinematics with constraints

Question

I have a question regarding my quest on onderstanding IK solvers.

After viewing this movie on YouTube: https://www.youtube.com/watch?v=MvuO9ZHGr6k I started recreating it with JavaScript. Just 2d on canvas.

I have come to this point:

function IK(x, y, joints) {

    this.location = new Vector(x, y);

    // 0: Not visible
    // 1: Just bones
    // 2: Angle indicators
    this.drawLevel = 2;

    this.joints = joints;

    this.target   = new Vector(150, 420);
    this.active   = this.joints.length - 1;
    this.run      = true;
    this.slow     = true;
    this.oneByOne = true;
    this.attempts = 0;
}

IK.prototype.resetActive = function() {

    this.active = this.joints.length - 1;
};

IK.prototype.getAngles = function() {

    var activeToTargetAngle = Math.atan2(
        this.target.y - (this.anchors[this.active].y + this.location.y),
        this.target.x - (this.anchors[this.active].x + this.location.x)
    ) * (180 / Math.PI);

    var endToActiveAngle = this.joints[this.active].angle  * (180 / Math.PI);

    if (this.active !== (this.anchors.length - 1)) {

        endToActiveAngle = Math.atan2(
            (this.location.y + this.anchors[this.anchors.length - 1].y) -
            (this.location.y + this.anchors[this.active].y),
            (this.location.x + this.anchors[this.anchors.length - 1].x) -
            (this.location.x + this.anchors[this.active].x)
        ) * (180 / Math.PI);
    }

    return {
        activeToTarget: activeToTargetAngle,
        endToActive: endToActiveAngle
    };
};

IK.prototype.makeAnchors = function() {

    this.anchors = [];
    this.anchors.push(new Vector(0, 0));
    this.anchors.push(new Vector(
        this.anchors[0].x + Math.cos(this.joints[0].angle) * this.joints[0].length,
        this.anchors[0].y + Math.sin(this.joints[0].angle) * this.joints[0].length
    ));
    this.anchors.push(new Vector(
        this.anchors[1].x + Math.cos(this.joints[1].angle) * this.joints[1].length,
        this.anchors[1].y + Math.sin(this.joints[1].angle) * this.joints[1].length
    ));
    this.anchors.push(new Vector(
        this.anchors[2].x + Math.cos(this.joints[2].angle) * this.joints[2].length,
        this.anchors[2].y + Math.sin(this.joints[2].angle) * this.joints[2].length
    ));
    this.anchors.push(new Vector(
        this.anchors[3].x + Math.cos(this.joints[3].angle) * this.joints[3].length,
        this.anchors[3].y + Math.sin(this.joints[3].angle) * this.joints[3].length
    ));
};

IK.prototype.onTarget = function() {

    var endAffector = new Vector(
        this.location.x + this.anchors[this.anchors.length -1].x,
        this.location.y + this.anchors[this.anchors.length -1].y
    );

    var targetDifference = this.target.distance(endAffector);

    if (targetDifference < 3) {

        //console.log('On target, stopping!');
        return true;
    }

    return false;
};

IK.prototype.notResolvable = function() {

    if (this.attempts > 5000) {
        console.log('Not resolvable, stopping!');
        return true;
    }

    return false;
};

IK.prototype.shift = function() {

    this.active -= 1;
    if (this.active < 0) {
        this.resetActive();
    }
};

IK.prototype.step = function() {

    this.makeAnchors();

    var angle = this.getAngles();

    // The angle is different, rotate!
    if (angle.endToActive != angle.activeToTarget) {

        for (var j = this.active; j < this.joints.length; j++) {

            if (angle.endToActive < angle.activeToTarget) {

                this.joints[j].angle += 0.01;
            } else {

                this.joints[j].angle -= 0.01;
            }
        }

        // Done rotating towards
        if (this.oneByOne) {

            if (Math.floor(angle.endToActive) == Math.floor(angle.activeToTarget)) {

                this.shift();
            }
        } else {

            this.shift();
        }
    }

    // Continue?
    if (this.onTarget() || this.notResolvable()) {

        this.run = false;
    }

    this.attempts += 1;
};

IK.prototype.direct = function() {

    while (this.run) {

        this.step();
    }
};

IK.prototype.update = function() {

    if (this.run) {
        if (this.slow) {
            this.step();
        } else {
            this.direct();
        }
    }

    if (this.drawLevel > 0) {
        this.draw();
    }
};

IK.prototype.draw = function() {

    this.drawStart();
    this.drawBones();
    this.drawConstraints();
    this.drawAngleLines();
};

(I left out all the draw functions since make the example more convoluted than needed)

This IK object is initialized as follows:

var ik = new IK(centerX, centerY,
    [{
        angle: 3.5,
        length: 50,
        min: Math.PI + -0.2,
        max: Math.PI + 0.2
    }, {
        angle: 2.7,
        length: 100,
        min: -1,
        max: -.3
    }, {
        angle: 2.5,
        length: 100,
        min: -1,
        max: 0
    }, {
        angle: 2.5,
        length: 50,
        min: -0.5,
        max: 0.5
    }]
);

So far so good. This is working like I see in the example on YouTube but now I wan't to take it a step further by adding angle constraints and this is where I'm getting stuck.

In the joints object that I send to the constructor I added min and max values that are added or subtracted from the current joint angle. Defining a valid rotation range.

After that I added this function, to check if the angle is inside the defined min / max range:

IK.prototype.withinConstraints = function() {

if (this.joints[this.active - 1] !== undefined) {

    var min = (this.joints[this.active - 1].angle + this.joints[this.active].min) * (180 / Math.PI);
    var max = (this.joints[this.active - 1].angle + this.joints[this.active].max) * (180 / Math.PI);
    var angle = this.joints[this.active].angle * (180 / Math.PI);

    if (angle < min || angle > max) {

        this.shift();

        return false;
    }
}

}

I change the step function to:

IK.prototype.step = function() {

    this.makeAnchors();

    var angle = this.getAngles();

    // The angle is different, rotate!
    if (angle.endToActive != angle.activeToTarget) {

        for (var j = this.active; j < this.joints.length; j++) {

        this.joints[j].angle =

              // Constraints not exeeded?
              this.withinConstraints() ?

              // Move towards or from target
              (
                  angle.endToActive < angle.activeToTarget ?
                  this.joints[j].angle + 0.01 : this.joints[j].angle - 0.01
              )

              // Keep value
              : this.joints[j].angle;
        }

        // Done rotating towards
        if (this.oneByOne) {

            if (Math.floor(angle.endToActive) == Math.floor(angle.activeToTarget)) {

            this.shift();
        }
    } else {

        this.shift();
    }
}

// Continue?
if (this.onTarget() || this.notResolvable()) {

    this.run = false;
}

this.attempts += 1;
};

This is just one of many attempts to make it work but I realize I simply don't know how to go about it.

This makes the rotation get stuck on one of the max or min angles. After that it won't continue or try to reach the target by going in the opposite direction. (which is logical with the above code).

I think I'm missing logics here:

// Move towards or from target
(
    angle.endToActive < angle.activeToTarget ?
    this.joints[j].angle + 0.01 : this.joints[j].angle - 0.01
)

The angle.endToActive < angle.activeToTarget is quite arbitrary.

Hopefully there is someone out there that can give me a push in the right direction.

---------- SOLUTION ---------

This is my solution for the constraints, if anyone might be interested.

I added the dir() function, this helps me find the desired rotation direction.

IK.prototype.dir = function(a, t) {

    var theta = t - a;

    if (theta < 0) {
        theta += 360;
    }

    if (theta > 180) {
        return false;
    }

    return true;
};

With that I changed the step function:

IK.prototype.step = function() {

    this.makeAnchors();

    var angle = this.getAngles();

    // The angle is different, rotate!
    if (angle.endToActive != angle.activeToTarget) {

        for (var j = this.active ; j < this.joints.length ; j++) {

            var min = 0;
            var max = 0;
            if (this.joints[j - 1] !== undefined) {
                min = this.joints[j - 1].angle + this.joints[j].min;
                max = this.joints[j - 1].angle + this.joints[j].max;
            } else {
                // Start joint
                min = this.joints[j].angle + this.joints[j].min;
                max = this.joints[j].angle + this.joints[j].max;
            }

            var a = 0;
            if (this.dir(angle.endToActive, angle.activeToTarget)) {
                a = this.joints[j].angle + 0.01;
            } else {
                a = this.joints[j].angle - 0.01;
            }

            if ((a > min && a < max)) {
                this.joints[j].angle = a;
            } else {
                this.shift();
            }
        }

        // Done rotating towards
        if (this.oneByOne) {

            if (Math.floor(angle.endToActive) == Math.floor(angle.activeToTarget)) {

                this.shift();
            }
        } else {

            this.shift();
        }
    }

    // Continue?
    if (this.onTarget() || this.notResolvable()) {

        this.run = false;
    }

    this.attempts += 1;
};

Now I have to optimize this script, I feel it's using more processor power than needed. But that's the next phase.

Answer 1

The gist of CCD is really nothing dazzling. Here's the description of a serial set-up (a chain of links and joints)

• you have an assembly of N joints and N links
• the first joint is called the root joint and has index 0
• joint i defines the frame where the rigid body for link i has its geometry described in (it's easier since people usually select a local frame axis to be aligned with a link)
• joint N-1 defines a special link, with index N-1 called an end effector

Assumptions:

• each joint i has an angular parameter that allows it to behave like a hinge that connects links i-1 and i (except for the root joint that controls the rotation of the entire chain with respect to a so-called world-frame of reference)
• each joint angle, denoted by theta_i is constrained in a subrange of [-2pi;2pi], bounded by theta_min_i and theta_max_i

The heart of the CCD algorithm is a concept akin to how a Gaussian relaxation solver proceeds:

• it iterates through all joints and for each joint i it tries to find the best angle theta_target_i that minimizes the distance (and orientation difference if you need that too) between the current position of the end effector and the goal position where you want the effector to reach
• when the optimal theta_target_i is found, it is clamped to the theta_min_i and theta_max_i interval (see numerical clamping)
• when joint i+1 is considered in the next iteration, the already modified kinematic chain (using the adjusted theta_target_i) will have moved closer to the target. Now joint i+1 needs to move it even further towards the goal position
• the iterative process can be executed in batches: you repeat the whole process going from i=0 to i=N-1 either K times or until your are satisfied with the distance error.

The answer to your question is that you should just perform the clamping and probably increase K (experimentally determinable) - see reference 1 in the Bibliography below.

Bibliography:

1. https://www.thinkmind.org/download.php?articleid=content_2012_4_10_60013
2. http://ttic.uchicago.edu/~tewari/research/saha10finite.pdf
3. http://digital.liby.waikato.ac.nz/conferences/ivcnz07/papers/ivcnz07-paper34.pdf