Closed Chain Inverse Kinematics
A generalized inverse kinematics solver that supports closed chains for parallel kinematics systems, dynamic reconfiguration, and arbitrary joint configuration based on damped least squares error minimization techniques. Supports all variety of joints including combinations of rotation and translation degrees of freedom and is agnostic to visualization framework. Inspired by Marty Vona's MSim research work and using techniques outlined in this 2009 paper by Samuel Buss. Developed with some aid and advice from Marty Vona.
Solver being used on JPL's ATHLETE robot for full body IK
Examples
ATHLETE and Robonaut demo here!
Rover mobility settling demo here!
Partial degrees of freedom Goal demo here!
Model License Information
Robitics models used in the project are for demonstration purposes only and subject to the licenses of their respective projects.
Use
Simple 2 DoF System
import { Solver, Joint, Link, Goal, DOF } from 'closed-chain-ik';
// Create links and joints
const link1 = new Link();
const joint1 = new Joint();
joint.setDoF( DOF.EZ );
joint.setPosition( 0, 1, 0 );
joint.setDoFValues( Math.PI / 4 );
const link2 = new Link();
const joint2 = new Joint();
joint.setDoF( DOF.EX );
joint.setPosition( 0, 1, 0 );
joint.setDoFValues( Math.PI / 4 );
const link3 = new Link();
link3.setPosition( 0, 1, 0 );
// Create the goal
const goal = new Goal();
link3.getWorldPosition( goal.position );
link3.getWorldQuaternion( goal.quaternion );
// Create structure
link1.addChild( joint1 );
joint1.addChild( link2 );
link2.addChild( joint2 );
joint2.addChild( link3 );
goal.makeClosure( link3 );
// create solver
const solver = new Solver( link1 );
// ...
// move the goal around and solve
solver.solve();
Using a WebWorker Solver
import { WorkerSolve, Joint, Link, Goal, DOF } from 'closed-chain-ik';
// ... instantiate kinematic system...
const solver = new WorkerSolver( link1 );
// ...
// move the goal around and solve asynchronously
solver.solve();
Caveats
-
The web worker implementation uses ShareArrayBuffers which are not available on some platforms (Safari, Chrome for Android). See issue #44.
-
Smoothing out 3DoF non closure ball joint behavior is in progress. See issue #22.
-
Enabling SVD on the Solver can cause divergence on solvable systems and stutter. See #76.
API
Constants
DOF
Enumerated fields representing different degrees of freedom for Joints.
// Translation DoF
DOF.X, DOF.Y, DOF.Z,
// Euler Rotation DoF
DOF.EX, DOF.EY, DOF.EZ,
DOF_NAMES
An array of strings representing the names of the above degrees of freedom.
SOLVE_STATUS
Enumerated fields representing the state of a solve result.
// Error for all goals are within
// the threshold.
SOLVE_STATUS.CONVERGED,
// Error for the goals has begun
// to diverge.
SOLVE_STATUS.DIVERGED,
// Resulting angles has not changed
// significantly enough to reach the
// stall threshold.
SOLVE_STATUS.STALLED,
// The solve has reached the maximum
// number of allowed iterations.
SOLVE_STATUS.TIMEOUT,
SOLVE_STATUS_NAMES
An array of strings representing the names of the above solve statuses.
Functions
Set of utility functions including some for creating an ik system from and working with results from URDFLoader.
findRoots
findRoots( frames : Array<Frame> ) : Array<Frame>
Takes an array of frames to traverse including the closure joints and links and finds a set of unique nodes to treat as the roots of the connected trees for use in solving.
urdfRobotToIKRoot
urdfRobotToIKRoot( robot : URDFRobot, trimUnused : Boolean = false ) : Joint
Generates an IK three based on the provided URDFRobot
with the root joint having a all 6 degrees of freedom set. Returns the root joint. If trimUnused
is true then any dangling links that do not connect to non-fixed joints will be removed from the system.
setUrdfFromIK
setUrdfFromIK( robot : URDFRobot, ikRoot : Joint ) : void
Copies the joint values from robot
onto ikRoot
based on joint names.
setIKFromUrdf
setIKFromUrdf( ikRoot : Joint, robot : URDFRobot ) : void
Copies the joint values from ikRoot
onto robot
based on joint names.
Frame
A base class for Link
, Joint
, and Goal
representing a frame defined by a position and rotation in space.
.position
position : Float32Array[ 3 ]
The position of the frame. If this is modified directly setMatrixNeedsUpdate()
must be called.
.quaternion
quaternion : Float32Array[ 4 ]
The orientation of the frame. If this is modified directly setMatrixNeedsUpdate()
must be called.
.matrix
readonly matrix : Float32Array[ 16 ]
The local transform matrix composed from the position and quaternion.
.matrixWorld
readonly matrixWorld : Float32Array[ 16 ]
The world transform matrix computed based on the parent matrixWorld and this local matrix.
.parent
readonly parent : Frame
The parent frame this frame is a child of.
.children
readonly children : Array<Frame>
The set of child frames this frame is a parent of.
.setPosition
setPosition( x : Number, y : Number, z : Number ) : void
Sets the position of the frame.
.setWorldPosition
setWorldPosition( x : Number, y : Number, z : Number ) : void
Sets the positon of the frame in world space. Automatically computes the local position relative to the parent.
.getWorldPosition
getWorldPosition( target : FloatArray[ 3 ] ) : void
Gets the position of the frame in the world in the target
argument.
.setQuaternion
setQuaternion( x : Number, y : Number, z : Number, w : Number ) : void
Sets the orientation of the frame.
.setWorldQuaternion
setWorldQuaternion( x : Number, y : Number, z : Number, w : Number ) : void
Sets the orientation of the frame in world space. Automatically computes the local orientation relative to the parent.
.getWorldQuaternion
getWorldQuaternion( target : FloatArray[ 4 ] ) : void
Gets the quaternion of the frame in the world in the target
argument.
.traverseParents
traverseParents( callback : ( parent : Frame ) => Boolean ) : void
Fires the given callback for every parent starting with the closest. If callback
returns true then the traversal is stopped.
.traverse
traverse( callback : ( child : Frame ) => Boolean ) : void
Fires the given callback for every child recursively in breadth first order. If callback
returns true then the traversal is stopped.
.addChild
addChild( child : Frame ) : void
Adds a child to this frame and sets the childs parent to this frame. Throws an error if the child already has a parent.
.removeChild
removeChild( child : Frame ) : void
Removes the given child from this frame. Throws an error if the given frame is not a child of this frame.
.attachChild
attachChild( child : Frame ) : void
Adds the given frame as a child of this frame while preserving the world position of the child.
.detachChild
detachChild( child : Frame ) : void
Removes the given frame as a child of this frame while preserving the world position of the child.
.updateMatrix
updateMatrix() : void
Updates the local .matrix
field if it needs to be updated.
.updateMatrixWorld
updateMatrixWorld( includeChildren : Boolean = false ) : void
Updates the local .matrix
and .worldMatrix
fields if they need to be updated. Ensures parent matrices are up to date.
.setMatrixNeedsUpdate
setMatrixNeedsUpdate() : void
Flags this frame as needing a matrix and matrix world update.
.setMatrixWorldNeedsUpdate
setMatrixNeedsUpdate() : void
Flags this frame and all its children as needing a matrix world update.
Link
extends Frame
A Frame modeling a fixed connection between two Joints. Only Joints may be added as children.
.closureJoints
closureJoints : Array<Joint>
The set of joints that are connected to this indirectly via Joint.makeClosure
.
Joint
extends Frame
A dynamic Frame representing a kinematic joint arbitrarily defineable degrees of freedom. A degree of freedom indicates an offset value can be set. Only Links may be added as children and a Joint may only have a single child.
.child
readonly child : Link = null
Reference to the joint child.
.isClosure
readonly isClosure : Boolean = false
Whether or not the child relationship is a closure or not.
.dof
readonly dof : Array<DOF>
A list of all the free degrees of freedom.
.dofFlags
readonly dofFlags : Uint8Array[6]
A list of 0
and 1
flags with 1
corresonding to a field in dof
.
.dofValues
readonly dofValues : Float32Array[6]
The current joint values for all joint degrees of freedom.
.dofTarget
readonly dofTarget : Float32Array[6]
The joint value targets for each degree of freedom. Solver will attempt to solve for these targets if targetSet is true.
.dofRestPose
readonly dofRestPose : Float32Array[6]
The rest pose for each joint degree of freedom. Solver will attempt to move the joint towards this position when it does not compromise solving for the other goals and when restPoseSet is true.
.minDoFLimit
readonly minDoFLimit : Float32Array[6]
The minimum value limits for each joint degree of freeom.
.maxDoFLimit
readonly maxDoFLimit : Float32Array[6]
The maximum value limits for each joint degree of freeom.
.matrixDoF
readonly matrixDoF : Float32Array[16]
The matrix representing the transformation offset due to the current joint values.
.targetSet
targetSet : Boolean = false
When set to true
Solver will try to move this joints dofValues towards the target values.
.restPoseSet
restPoseSet : Boolean = false
When set to true
Solver will try to move this joints dofValues towards the rest pose values without compromising the other goals.
.setDoF
setDoF( ...dof : Array<DOF> ) : void
Sets the degrees of freedom of the joint. Arguments must be passed in X, Y, Z, EX, EY, EZ order without duplicate values. All relatd degree of freedom values are reset.
.clearDoF
clearDoF() : void
Clears all degrees of freedom.
.set[ * ]Values
setDoFValues( ...values : Array<Number> ) : void;
setRestPoseValues( ...values : Array<Number> ) : void;
setTargetValues( ...values : Array<Number> ) : void;
setMinLimits( ...values : Array<Number> ) : void;
setMaxLimits( ...values : Array<Number> ) : void;
The number of arguments must match the number of degrees of freedom of the joint.
.set[ * ]Value
setDoFValue( dof : DOF, value : Number ) : Boolean
setRestPoseValue( dof : DOF, value : Number ) : Boolean
setTargetValue( dof : DOF, value : Number ) : Boolean
setMinLimit( dof : DOF, value : Number ) : Boolean
setMaxLimit( dof : DOF, value : Number ) : Boolean
.get[ * ]Value
getDoFValue( dof : DOF ) : Number
getRestPoseValue( dof : DOF ) : Number
getTargetValue( dof : DOF ) : Number
getMinLimit( dof : DOF ) : Number
getMaxLimit( dof : DOF ) : Number
.makeClosure
makeClosure( child : Link ) : void
Declares the relationship between this joint and the given child link is a closure meaning there is no direct parent child relationship but the Solver will treat the closure link as a target for this joint to keep them closed.
Note that when making a closure connection between a Joint and a Link the link will not be added to the Joints children
array and instead will only be available on the child
field. The Joint will be appended to the Links closureJoints
array.
Goal
extends Joint
A Frame representing a goal to achieve for a connected Link. Set degrees of freedom represent fixed goals for a link to achieve as opposed to moveable degrees of freedom defined for Joints. A goal cannot have children and only be used to make a closure.
Solver
Class for solving the closure and target joint constraints of a sytem. As well as the listed fields a set of "options" are set on the object which are listed here:
// Whether or not to use the SVD when calculating the pseudo inverse of the jacobian
// which can result in a more numerically stable calculation. If the SVD cannot be calculated
// then the transpose method is used.
useSVD = true;
// The max amount of iterations to try to solve for. The solve will terminate
// with SOLVE_STATUS.TIMEOUT if this limit is exceeded.
maxIterations = 5;
// The threshold under which a joint is not considered to have really moved. If
// no joint is moved more than this threshold then the solve will terminate with
// SOLVE_STATUS.STALLED.
stallThreshold = 1e-4;
// The threshold for comparing how much error has changed between solve iterations.
// If the error has grown by more than this threshold then the solve will terminate
// with SOLVE_STATUS.DIVERGED.
divergeThreshold = 0.01;
// The fixed damping factor to use in the DLS calculation.
dampingFactor = 0.001;
// The factor with which to move the joints towards the rest pose if set.
restPoseFactor = 0.01;
// The thresholds with which to compute whether or not the translation or rotation
// goals have been met. If the error between target and goal is under these
// thresholds then the solve will terminate with SOLVE_STATUS.CONVERGED.
translationConvergeThreshold = 1e-3;
rotationConvergeThreshold = 1e-5;
// Factors to apply to the translation and rotation error in the error vector.
// Useful for weighting rotation higher than translation if they do not seem to
// be solved "evenly". Values are expected to be in the range [ 0, 1 ].
translationFactor = 1;
rotationFactor = 1;
// The amount to move a joint when calculating the change in error a joint has
// for a jacobian.
translationStep = 1e-3;
rotationStep = 1e-3;
// The step to take towards the IK goals when solving. Setting this to a larger value
// may solve more quickly but may lead also lead to divergence.
translationErrorClamp = 0.1;
rotationErrorClamp = 0.1;
.roots
roots : Array<Frame>
The list of roots that should be accounted for in a solve. When .updateStructure
is called the series of roots are traversed including closure joints to find all connected link hierarchies to use in the solve.
.constructor
constructor( roots : Array<Frame> )
Constructor takes a list of roots to solve for.
.solve
solve() : Array<SOLVE_STATUS>
Traverses the given set of roots to find joint chains to solve for and attempts to solve for the error in the system goals. A result is returned for each independent chain found in the system.
.updateStructure
updateStructure() : void
Must be called whenever parent child relationships and structural changes related to the tree change or .roots
is modified.
WorkerSolver
Implements the interface defined by Solver but runs the solve asynchronously on in a WebWorker. Results are automatically copied to the joint system being solved for.
⚠️ When SharedArrayBuffers are not available new copies ofArrayBuffer
are created every update and from the worker.
.results
results : Array<Solve_STATUS>
The list of the last results from the solve copied over from the WebWorker.
.updateSolverSettings
updateSolverSettings( settings : Object ) : void
Sets the solver settings in the WebWorker to the values in the given object. Valid "options" values are listed in the Solver docs.
.updateFrameState
updateFrameState( ...jointsToUpdate : Array<Joint> = [] ) : void
Copies the joint settings for the given joints to the WebWorker for a solve. "Joint settings" include everything except for joint values (that the solver would be solving for) and parent child relationships. If joint values or parent child relationhips change then updateStructure
must be called.
.solve
solve() : void
Starts a solve in the WebWorker if one is not active. The solve will terminate automatically if none of the results are SOLVE_STATUS.TIMEOUT
.
.stop
stop() : void
Terminate any active solve in the WebWorker.
.dispose
dispose() : void
Terminates the WebWorker and sets members to null.
IKRootsHelper
extends THREE.Group
A helper class for rendering the joints and links in a three.js scene. Renders frame relationships as lines and joints degrees of freedom with indicators based on the joint type.
.roots
roots : Array<Frame>
Set of roots to render in the helper visualization. If this is changed then .updateStructure
must be called.
.constructor
constructor( roots : Array<Frame> )
Takes the set of roots to visualize.
.setJointScale
setJointScale( scale : Number ) : this
Sets the scale of the joint indicators.
.setColor
setColor( color : Color | String | Number ) : this
Sets the color of the helper.
.setDrawThrough
setDrawThrough( drawThrough : Boolean ) : this
Sets whether the helper will draw through the environment.
.setResolution
setResolution( width : Number, height : Number ) : this
Sets the resolution of the renderer so the 2d lines can be rendered at the appropriate thickness.
.updateStructure
updateStructure() : void
Must be called if the structure of the IK system being visualized has changed.
.dispose
dispose() : void
Calls dispose
on all created materials and geometry in the tree.