inverse-kinematics
Version:
Inverse kinematics for 2D and 3D applications
266 lines (265 loc) • 11.8 kB
JavaScript
var __assign = (this && this.__assign) || function () {
__assign = Object.assign || function(t) {
for (var s, i = 1, n = arguments.length; i < n; i++) {
s = arguments[i];
for (var p in s) if (Object.prototype.hasOwnProperty.call(s, p))
t[p] = s[p];
}
return t;
};
return __assign.apply(this, arguments);
};
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 { QuaternionO, V3O } from '.';
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 deltaAngle = _a.deltaAngle, learningRate = _a.learningRate, acceptedError = _a.acceptedError;
var _b = getJointTransforms(links, baseJoint), joints = _b.transforms, effectorPosition = _b.effectorPosition;
var error = V3O.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, linkIndex) {
var position = _a.position, _b = _a.rotation, rotation = _b === void 0 ? QuaternionO.zeroRotation() : _b, constraints = _a.constraints;
// For each, calculate partial derivative, sum to give full numerical derivative
var angleStep = V3O.fromArray([0, 0, 0].map(function (_, v3Index) {
var eulerAngle = [0, 0, 0];
eulerAngle[v3Index] = deltaAngle;
var linkWithAngleDelta = {
position: position,
rotation: QuaternionO.multiply(rotation, QuaternionO.fromEulerAngles(V3O.fromArray(eulerAngle))),
};
// Get remaining links from this links joint
var projectedLinks = __spreadArray([linkWithAngleDelta], links.slice(linkIndex + 1));
// Get gradient from small change in joint angle
var joint = joints[linkIndex];
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 angleStep;
}));
var steppedRotation = QuaternionO.multiply(rotation, QuaternionO.fromEulerAngles(angleStep));
return { position: position, rotation: steppedRotation, 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 learningRate = _a.learningRate, acceptedError = _a.acceptedError;
// 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 = V3O.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];
/**
* Following http://rodolphe-vaillant.fr/?e=114
*
* We found that if we didn't convert the world coordinate system here to local
* that it would give very unstable solutions. It seems that others have struggled
* with the same thing.
*
* https://github.com/zalo/zalo.github.io/blob/fb1b899ce9825b1123b0ebd2bfdce2459566e6db/assets/js/IK/IKExample.js#L67
*/
var inverseRotation = QuaternionO.inverse(joint.rotation);
var rotatedTarget = V3O.rotate(target, inverseRotation);
var rotatedEffector = V3O.rotate(effectorPosition, inverseRotation);
var rotatedJoint = V3O.rotate(joint.position, inverseRotation);
var directionToTarget = V3O.subtract(rotatedTarget, rotatedJoint);
var directionToEffector = V3O.subtract(rotatedEffector, rotatedJoint);
var angleBetween = QuaternionO.rotationFromTo(directionToEffector, directionToTarget);
var angleStep = QuaternionO.slerp(QuaternionO.zeroRotation(), angleBetween, typeof learningRate === 'function' ? learningRate(error) : learningRate);
var withAngleStep = { rotation: QuaternionO.multiply(rotation, angleStep), position: position, constraints: constraints };
adjustedLinks[index] = withAngleStep;
var adjustedJoints = getJointTransforms(adjustedLinks, baseJoint);
var withConstraint = applyConstraint(withAngleStep, adjustedJoints.transforms[index + 1]);
adjustedLinks[index] = withConstraint;
}
return {
links: adjustedLinks,
getErrorDistance: function () { return getErrorDistance(adjustedLinks, baseJoint, target); },
isWithinAcceptedError: undefined,
};
}
function applyConstraint(_a, joint) {
var position = _a.position, rotation = _a.rotation, constraints = _a.constraints;
if (constraints === undefined)
return { position: position, rotation: rotation };
if (isExactRotation(constraints)) {
if (constraints.type === 'global') {
var targetRotation = constraints.value;
var currentRotation = joint.rotation;
var adjustedRotation = QuaternionO.multiply(QuaternionO.multiply(rotation, QuaternionO.inverse(currentRotation)), targetRotation);
return { position: position, rotation: adjustedRotation, constraints: constraints };
}
else {
return { position: position, rotation: constraints.value, constraints: constraints };
}
}
var pitch = constraints.pitch, yaw = constraints.yaw, roll = constraints.roll;
var pitchMin;
var pitchMax;
if (typeof pitch === 'number') {
pitchMin = -pitch / 2;
pitchMax = pitch / 2;
}
else if (pitch === undefined) {
pitchMin = -Infinity;
pitchMax = Infinity;
}
else {
pitchMin = pitch.min;
pitchMax = pitch.max;
}
var yawMin;
var yawMax;
if (typeof yaw === 'number') {
yawMin = -yaw / 2;
yawMax = yaw / 2;
}
else if (yaw === undefined) {
yawMin = -Infinity;
yawMax = Infinity;
}
else {
yawMin = yaw.min;
yawMax = yaw.max;
}
var rollMin;
var rollMax;
if (typeof roll === 'number') {
rollMin = -roll / 2;
rollMax = roll / 2;
}
else if (roll === undefined) {
rollMin = -Infinity;
rollMax = Infinity;
}
else {
rollMin = roll.min;
rollMax = roll.max;
}
var lowerBound = [pitchMin, yawMin, rollMin];
var upperBound = [pitchMax, yawMax, rollMax];
var clampedRotation = QuaternionO.clamp(rotation, lowerBound, upperBound);
return { position: position, rotation: clampedRotation, constraints: copyConstraints(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 V3O.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 = [__assign({}, joint)];
for (var index = 0; index < links.length; index++) {
var currentLink = links[index];
var parentTransform = transforms[index];
var absoluteRotation = QuaternionO.multiply(parentTransform.rotation, (_a = currentLink.rotation) !== null && _a !== void 0 ? _a : QuaternionO.zeroRotation());
var relativePosition = V3O.rotate(currentLink.position, absoluteRotation);
var absolutePosition = V3O.add(relativePosition, parentTransform.position);
transforms.push({ position: absolutePosition, rotation: absoluteRotation });
}
var effectorPosition = transforms[transforms.length - 1].position;
return { transforms: transforms, effectorPosition: effectorPosition };
}
export function buildLink(position, rotation, constraints) {
if (rotation === void 0) { rotation = QuaternionO.zeroRotation(); }
return {
position: position,
rotation: rotation,
constraints: constraints,
};
}
function copyLink(_a) {
var rotation = _a.rotation, position = _a.position, constraints = _a.constraints;
return {
rotation: rotation,
position: __spreadArray([], position),
constraints: constraints === undefined ? undefined : copyConstraints(constraints),
};
}
function copyConstraints(constraints) {
var result = {};
if (isExactRotation(constraints)) {
return { type: constraints.type, value: __spreadArray([], constraints.value) };
}
var pitch = constraints.pitch, yaw = constraints.yaw, roll = constraints.roll;
if (typeof pitch === 'number') {
result.pitch = pitch;
}
else if (pitch !== undefined) {
result.pitch = __assign({}, pitch);
}
if (typeof yaw === 'number') {
result.yaw = yaw;
}
else if (yaw !== undefined) {
result.yaw = __assign({}, yaw);
}
if (typeof roll === 'number') {
result.roll = roll;
}
else if (roll !== undefined) {
result.roll = __assign({}, roll);
}
return result;
}
function isExactRotation(rotation) {
return rotation.value !== undefined;
}