Solves the closure and joint target constraints of a system of frames using damped least squares. Every independent chain of joints found in the roots is solved separately.
import { HemisphereLight, MathUtils } from 'three';
import URDFLoader from 'urdf-loader';
import { Solver, Goal, DOF, URDFUtils } from 'closed-chain-ik';
// scene, camera and renderer are initialized here
const URL = 'https://raw.githubusercontent.com/gkjohnson/urdf-loaders/master/urdf/T12/urdf/T12_flipped.URDF';
scene.add( new HemisphereLight( 0xffffff, 0x444444, 3 ) );
camera.position.set( 6, 5, 8 );
const robot = await new URDFLoader().loadAsync( URL );
robot.rotation.x = Math.PI / 2;
robot.position.y = 1.1;
for ( let i = 1; i <= 6; i ++ ) {
robot.setJointValue( `HP${ i }`, MathUtils.degToRad( 30 ) );
robot.setJointValue( `KP${ i }`, MathUtils.degToRad( 120 ) );
robot.setJointValue( `AP${ i }`, MathUtils.degToRad( - 60 ) );
}
robot.updateMatrixWorld( true );
scene.add( robot );
// an IK tree matching the robot, with its body held in place
const ik = URDFUtils.urdfRobotToIKRoot( robot );
URDFUtils.setIKFromUrdf( ik, robot );
ik.clearDoF();
// the first foot follows a goal circling above where it stands
const foot = ik.find( frame => frame.name === 'Foot1' );
const start = [ 0, 0, 0 ];
foot.getWorldPosition( start );
const goal = new Goal();
goal.setGoalDoF( DOF.X, DOF.Y, DOF.Z );
goal.makeClosure( foot );
const solver = new Solver( ik );
renderer.setAnimationLoop( time => {
const angle = time / 600;
goal.setPosition( start[ 0 ] + 0.4 * Math.cos( angle ), start[ 1 ] + 0.8 + 0.4 * Math.sin( angle ), start[ 2 ] );
solver.solve();
URDFUtils.setUrdfFromIK( robot, ik );
renderer.render( scene, camera );
} );On this page
Properties
useSVD: boolean = false
Use the SVD to compute the damped pseudo inverse of the jacobian, which adds damping in near singular directions to keep steps bounded near singularities. Falls back to the transpose method if the SVD cannot be computed.
maxIterations: number = 5
Maximum number of iterations per solve. The solve terminates with
SOLVE_STATUS.TIMEOUT when exceeded.
stallThreshold: number = 1e-4
If no joint moves more than this in an iteration the solve terminates with
SOLVE_STATUS.STALLED.
dampingFactor: number = 0.001
Base damping factor of the damped least squares solve.
divergeThreshold: number = 0.01
Amount the error may grow in a single step before the step is rejected and retried at
a smaller scale. If no scale keeps the error within this threshold the solve terminates
with SOLVE_STATUS.DIVERGED.
restPoseFactor: number = 0.01
Factor with which joints that have a rest pose set are moved toward it without compromising the other goals.
translationConvergeThreshold: number = 1e-3
Translation error under which a goal is considered met. The solve terminates with
SOLVE_STATUS.CONVERGED when every goal is met.
rotationConvergeThreshold: number = 1e-5
Rotation error under which a goal is considered met. The solve terminates with
SOLVE_STATUS.CONVERGED when every goal is met.
translationFactor: number = 1
Weight applied to translation error. Useful for balancing translation against rotation
when one is solved for more strongly than the other. Expected to be in [ 0, 1 ].
rotationFactor: number = 1
Weight applied to rotation error. Useful for balancing rotation against translation
when one is solved for more strongly than the other. Expected to be in [ 0, 1 ].
translationErrorClamp: number = 0.1
Maximum translation error targeted in a single step. Larger values may solve faster but are more likely to overshoot.
rotationErrorClamp: number = 0.1
Maximum rotation error targeted in a single step. Larger values may solve faster but are more likely to overshoot.
The roots to solve for. When updateStructure is called the roots are traversed,
including closure connections, to find every connected tree. If modified
updateStructure must be called.
Methods
new Solver(
// The roots of the trees to solve.
roots: Frame | Array<Frame>
)updateStructure(): voidRebuilds the joint chains to solve. Must be called whenever the parent child structure of
the trees, the degrees of freedom of a joint, or roots change.
solve(): Array<number>Runs a solve on every independent joint chain and returns a SOLVE_STATUS for each.