inverse-kinematics
Version:
Inverse kinematics for 2D and 3D applications
171 lines (170 loc) • 8.61 kB
JavaScript
var __spreadArray = (this && this.__spreadArray) || function (to, from) {
for (var i = 0, il = from.length, j = to.length; i < il; i++, j++)
to[j] = from[i];
return to;
};
import { V2O } from '.';
import { clamp } from './math/MathUtils';
import { defaultCCDOptions, defaultFABRIKOptions, } from './SolveOptions';
/**
* Changes joint angle to minimize distance of end effector to target
*
* If given no options, runs in FABRIK mode
*/
export function solve(links, baseJoint, target, options) {
var _a, _b, _c, _d, _e;
if (options === void 0) { options = { method: 'FABRIK' }; }
switch (options.method) {
case 'FABRIK':
return solveFABRIK(links, baseJoint, target, {
method: 'FABRIK',
acceptedError: (_a = options.acceptedError) !== null && _a !== void 0 ? _a : defaultFABRIKOptions.acceptedError,
deltaAngle: (_b = options.deltaAngle) !== null && _b !== void 0 ? _b : defaultFABRIKOptions.deltaAngle,
learningRate: (_c = options.learningRate) !== null && _c !== void 0 ? _c : defaultFABRIKOptions.learningRate,
});
case 'CCD':
return solveCCD(links, baseJoint, target, {
method: 'CCD',
acceptedError: (_d = options.acceptedError) !== null && _d !== void 0 ? _d : defaultCCDOptions.acceptedError,
learningRate: (_e = options.learningRate) !== null && _e !== void 0 ? _e : defaultCCDOptions.learningRate,
});
}
}
function solveFABRIK(links, baseJoint, target, _a) {
var learningRate = _a.learningRate, deltaAngle = _a.deltaAngle, acceptedError = _a.acceptedError;
var _b = getJointTransforms(links, baseJoint), joints = _b.transforms, effectorPosition = _b.effectorPosition;
var error = V2O.euclideanDistance(target, effectorPosition);
if (error < acceptedError)
return { links: links.map(copyLink), isWithinAcceptedError: true, getErrorDistance: function () { return error; } };
if (joints.length !== links.length + 1) {
throw new Error("Joint transforms should have the same length as links + 1. Got " + joints.length + ", expected " + links.length);
}
var withAngleStep = links.map(function (_a, index) {
var _b = _a.rotation, rotation = _b === void 0 ? 0 : _b, position = _a.position, constraints = _a.constraints;
var linkWithAngleDelta = {
position: position,
rotation: rotation + deltaAngle,
};
// Get remaining links from this links joint
var projectedLinks = __spreadArray([linkWithAngleDelta], links.slice(index + 1));
// Get gradient from small change in joint angle
var joint = joints[index];
var projectedError = getErrorDistance(projectedLinks, joint, target);
var gradient = (projectedError - error) / deltaAngle;
// Get resultant angle step which minimizes error
var angleStep = -gradient * (typeof learningRate === 'function' ? learningRate(projectedError) : learningRate);
return { rotation: rotation + angleStep, position: position, constraints: constraints };
});
var adjustedJoints = getJointTransforms(withAngleStep, baseJoint).transforms;
var withConstraints = applyConstraints(withAngleStep, adjustedJoints);
return {
links: withConstraints,
getErrorDistance: function () { return getErrorDistance(withConstraints, baseJoint, target); },
isWithinAcceptedError: undefined,
};
}
function solveCCD(links, baseJoint, target, _a) {
var acceptedError = _a.acceptedError, learningRate = _a.learningRate;
// 1. From base to tip, point projection from joint to effector at target
var adjustedLinks = __spreadArray([], links.map(copyLink));
for (var index = adjustedLinks.length - 1; index >= 0; index--) {
var joints = getJointTransforms(adjustedLinks, baseJoint);
var effectorPosition = joints.effectorPosition;
var error = V2O.euclideanDistance(target, effectorPosition);
if (error < acceptedError)
break;
var link = adjustedLinks[index];
var rotation = link.rotation, position = link.position, constraints = link.constraints;
var joint = joints.transforms[index];
var directionToTarget = V2O.angle(V2O.subtract(target, joint.position));
var directionToEffector = V2O.angle(V2O.subtract(effectorPosition, joint.position));
var angleBetween = directionToEffector - directionToTarget;
var angleStep = -angleBetween * (typeof learningRate === 'function' ? learningRate(error) : learningRate);
var withAngleStep = { rotation: rotation + angleStep, position: position, constraints: constraints };
adjustedLinks[index] = withAngleStep;
var adjustedJoints_1 = getJointTransforms(adjustedLinks, baseJoint);
var withConstraint = applyConstraint(withAngleStep, adjustedJoints_1.transforms[index + 1]);
adjustedLinks[index] = withConstraint;
}
var adjustedJoints = getJointTransforms(adjustedLinks, baseJoint).transforms;
var withConstraints = applyConstraints(adjustedLinks, adjustedJoints);
return {
links: withConstraints,
getErrorDistance: function () { return getErrorDistance(withConstraints, baseJoint, target); },
isWithinAcceptedError: undefined,
};
}
function applyConstraint(_a, joint) {
var rotation = _a.rotation, position = _a.position, constraints = _a.constraints;
if (constraints === undefined)
return { position: position, rotation: rotation };
if (typeof constraints === 'number') {
var halfConstraint = constraints / 2;
var clampedRotation = clamp(rotation, -halfConstraint, halfConstraint);
return { position: position, rotation: clampedRotation, constraints: constraints };
}
if (isExactRotation(constraints)) {
if (constraints.type === 'global') {
var targetRotation = constraints.value;
var currentRotation = joint.rotation;
var deltaRotation = targetRotation - currentRotation;
return { position: position, rotation: rotation + deltaRotation, constraints: constraints };
}
else {
return { position: position, rotation: constraints.value, constraints: constraints };
}
}
else {
var clampedRotation = clamp(rotation, constraints.min, constraints.max);
return { position: position, rotation: clampedRotation, constraints: constraints };
}
}
function applyConstraints(links, joints) {
return links.map(function (link, index) { return applyConstraint(link, joints[index + 1]); });
}
/**
* Distance from end effector to the target
*/
export function getErrorDistance(links, base, target) {
var effectorPosition = getEndEffectorPosition(links, base);
return V2O.euclideanDistance(target, effectorPosition);
}
/**
* Absolute position of the end effector (last links tip)
*/
export function getEndEffectorPosition(links, joint) {
return getJointTransforms(links, joint).effectorPosition;
}
/**
* Returns the absolute position and rotation of each link
*/
export function getJointTransforms(links, joint) {
var _a;
var transforms = [joint];
for (var index = 0; index < links.length; index++) {
var currentLink = links[index];
var parentTransform = transforms[index];
var absoluteRotation = ((_a = currentLink.rotation) !== null && _a !== void 0 ? _a : 0) + parentTransform.rotation;
var relativePosition = V2O.rotate(currentLink.position, absoluteRotation);
var absolutePosition = V2O.add(relativePosition, parentTransform.position);
transforms.push({ position: absolutePosition, rotation: absoluteRotation });
}
var effectorPosition = transforms[transforms.length - 1].position;
var effectorRotation = transforms[transforms.length - 1].rotation;
return { transforms: transforms, effectorPosition: effectorPosition, effectorRotation: effectorRotation };
}
export function buildLink(position, rotation, constraint) {
if (rotation === void 0) { rotation = 0; }
return {
position: position,
rotation: rotation,
constraints: constraint,
};
}
function copyLink(_a) {
var rotation = _a.rotation, position = _a.position, constraint = _a.constraints;
return { rotation: rotation, position: __spreadArray([], position), constraints: constraint === undefined ? undefined : constraint };
}
function isExactRotation(rotation) {
return rotation.value !== undefined;
}