From 9f944611b01eacd54dd54aa694ad0926b51759d0 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 20:13:25 -0700 Subject: [PATCH 01/20] Add goal --- src/core/Goal.js | 43 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/core/Goal.js b/src/core/Goal.js index ffcf742..d50455e 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -1,11 +1,50 @@ -import { Joint } from './Joint.js'; +import { DOF_NAMES, DOF } from './Joint.js'; +import { Frame } from './Frame.js'; -export class Goal extends Joint { +export class Goal extends Frame { constructor( ...args ) { super( ...args ); this.isGoal = true; + this.freeDoF = []; + + } + + setFreeDoF( ...args ) { + + args.forEach( ( dof, i ) => { + + if ( dof < 0 || dof >= 6 ){ + + throw new Error( 'Goal: Invalid degree of freedom enum ' + dof + '.' ); + + } + + if ( args.includes( dof, i + 1 ) ) { + + throw new Error( 'Goal: Duplicate degree of freedom ' + DOF_NAMES[ dof ] + 'specified.' ); + + } + + if ( i !== 0 && args[ i - 1 ] > dof ) { + + throw new Error( 'Goal: Joints degrees of freedom must be specified in position then rotation, XYZ order' ); + } + + } ); + + this.freeDoF = args; + + } + + setGoalDoF( ...args ) { + + const freeDoF = [ + DOF.X, DOF.Y, DOF.Z, + DOF.EX, DOF.EY, DOF.EZ, + ].filter( d => ! args.includes( d ) ); + this.setFreeDoF( ...freeDoF ); } From c23c552414145807810531bc5a99a112e301f9eb Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 20:17:54 -0700 Subject: [PATCH 02/20] Add functions --- src/core/Goal.js | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/core/Goal.js b/src/core/Goal.js index d50455e..f5fee48 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -8,6 +8,9 @@ export class Goal extends Frame { super( ...args ); this.isGoal = true; this.freeDoF = []; + this.child = null; + this.isClosure = false; + } @@ -48,4 +51,34 @@ export class Goal extends Frame { } + makeClosure( child ) { + + if ( ! child.isLink || this.children.length >= 1 || child.parent === this ) { + + throw new Error(); + + } else { + + this.children[ 0 ] = child; + this.child = child; + this.isClosure = true; + + } + + } + + removeChild( child ) { + + super.removeChild( child ); + this.child = null; + this.isClosure = false; + + } + + addChild() { + + throw new Error(); + + } + } From 77bcfa94c2df54c5aa6572ce7a90bd455288e8be Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 20:39:48 -0700 Subject: [PATCH 03/20] update --- src/core/Goal.js | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/core/Goal.js b/src/core/Goal.js index f5fee48..4f64f82 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -1,17 +1,19 @@ import { DOF_NAMES, DOF } from './Joint.js'; import { Frame } from './Frame.js'; +// TODO: we should just extend Joint here... export class Goal extends Frame { constructor( ...args ) { super( ...args ); this.isGoal = true; + this.isJoint = true; + this.freeDoF = []; this.child = null; this.isClosure = false; - } setFreeDoF( ...args ) { From 8b02e436f3dea87590ef29861d17d96ec80c8b44 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 20:52:46 -0700 Subject: [PATCH 04/20] Update ChainSolver.js --- src/core/ChainSolver.js | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index a869bef..b8ccf5a 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -541,6 +541,8 @@ export class ChainSolver { if ( relevantClosures.has( targetJoint ) || relevantConnectedClosures.has( targetJoint ) ) { + // TODO: If this is a Goal it only add 1 or 2 fields if only two axes are set. Quat is only + // needed if 3 eulers are used. // TODO: these could be cached per target joint // get the current error within the closure joint targetJoint.getClosureError( tempPos, tempQuat ); @@ -613,6 +615,8 @@ export class ChainSolver { // TODO: Having noted that is this really necessary? Is there any way that this doesn't just // jump to the solution and lock? How can we afford some slack? With a low weight? Does that // get applied here? + // TODO: If this joint happens to have three euler joints we need to use a quat here. Otherwise we + // use the euler angles. for ( let i = 0; i < rowCount; i ++ ) { outJacobian[ rowIndex + colIndex ][ colIndex ] = - 1; From 9b1b2a7d6c964bef70a462797cc7ace7980a144b Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 20:57:59 -0700 Subject: [PATCH 05/20] Update solver.js --- src/core/utils/solver.js | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index 55870cb..c0de174 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -18,6 +18,8 @@ export function accumulateClosureError( rotationErrorClamp, } = solver; + // TODO: If this is a Goal and we have less than three euler DoF we should use euler angles (and adjust the rows). Otherwise we + // should use a quat. joint.getClosureError( tempPos, tempQuat ); let isConverged = false; @@ -96,6 +98,8 @@ export function accumulateTargetError( // get the position delta const posDelta = vec3.distance( dofValues, dofTarget ); + // TODO: if three euler angles are being used we should set this to a quaternion to measure + // error rather than euler angles. // Before running this solver we try to ensure the target and restPose are minimized let rotDelta = dofTarget[ DOF.EX ] - dofValues[ DOF.EX ] + From bf23783e67767d47fe9b53cf66cca0076fe1741b Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 21:21:29 -0700 Subject: [PATCH 06/20] example cleanup --- example/index.js | 388 ++++++++++++++++++++++------------------------- 1 file changed, 183 insertions(+), 205 deletions(-) diff --git a/example/index.js b/example/index.js index b58c130..462fd68 100644 --- a/example/index.js +++ b/example/index.js @@ -65,257 +65,266 @@ const goalIcons = []; let selectedGoalIndex = - 1; let loadId = 0; -let gui; -const stats = new Stats(); -document.body.appendChild( stats.dom ); - -const outputContainer = document.getElementById( 'output' ); - -// init renderer -const renderer = new WebGLRenderer( { antialias: true } ); -renderer.setPixelRatio( window.devicePixelRatio ); -renderer.setSize( window.innerWidth, window.innerHeight ); -renderer.outputEncoding = sRGBEncoding; -document.body.appendChild( renderer.domElement ); +let averageTime = 0; +let averageCount = 0; +let gui, stats; +let outputContainer, renderer, scene, camera; +let solver, ikHelper, drawThroughIkHelper, ikRoot, urdfRoot; +let controls, transformControls, targetObject; +let mouse = new Vector2(); -const camera = new PerspectiveCamera( 50, window.innerWidth / window.innerHeight ); -camera.position.set( 8, 8, 8 ); +init(); +rebuildGUI(); +loadModel( loadATHLETE() ); +render(); -const scene = new Scene(); -scene.background = new Color( 0x131619 ); +function init() { -const directionalLight = new DirectionalLight(); -directionalLight.position.set( 1, 3, 2 ); -scene.add( directionalLight ); + stats = new Stats(); + document.body.appendChild( stats.dom ); -const ambientLight = new AmbientLight( 0x263238, 1 ); -scene.add( ambientLight ); + outputContainer = document.getElementById( 'output' ); -const controls = new OrbitControls( camera, renderer.domElement ); -const transformControls = new TransformControls( camera, renderer.domElement ); -transformControls.setSpace( 'local' ); -scene.add( transformControls ); + // init renderer + renderer = new WebGLRenderer( { antialias: true } ); + renderer.setPixelRatio( window.devicePixelRatio ); + renderer.setSize( window.innerWidth, window.innerHeight ); + renderer.outputEncoding = sRGBEncoding; + document.body.appendChild( renderer.domElement ); -transformControls.addEventListener( 'mouseDown', () => controls.enabled = false ); -transformControls.addEventListener( 'mouseUp', () => controls.enabled = true ); + camera = new PerspectiveCamera( 50, window.innerWidth / window.innerHeight ); + camera.position.set( 8, 8, 8 ); -const targetObject = new Group(); -targetObject.position.set( 0, 1, 1 ) -scene.add( targetObject ); -transformControls.attach( targetObject ); + scene = new Scene(); + scene.background = new Color( 0x131619 ); -let solver, ikHelper, drawThroughIkHelper, ikRoot, urdfRoot; -loadModel( loadATHLETE() ); -rebuildGUI(); -render(); + const directionalLight = new DirectionalLight(); + directionalLight.position.set( 1, 3, 2 ); + scene.add( directionalLight ); -window.addEventListener( 'resize', () => { + const ambientLight = new AmbientLight( 0x263238, 1 ); + scene.add( ambientLight ); - const w = window.innerWidth; - const h = window.innerHeight; - const aspect = w / h; + controls = new OrbitControls( camera, renderer.domElement ); + transformControls = new TransformControls( camera, renderer.domElement ); + transformControls.setSpace( 'local' ); + scene.add( transformControls ); - renderer.setSize( w, h ); + transformControls.addEventListener( 'mouseDown', () => controls.enabled = false ); + transformControls.addEventListener( 'mouseUp', () => controls.enabled = true ); - camera.aspect = aspect; - camera.updateProjectionMatrix(); + targetObject = new Group(); + targetObject.position.set( 0, 1, 1 ) + scene.add( targetObject ); + transformControls.attach( targetObject ); - ikHelper.setResolution( window.innerWidth, window.innerHeight ); - drawThroughIkHelper.setResolution( window.innerWidth, window.innerHeight ); + window.addEventListener( 'resize', () => { -} ); + const w = window.innerWidth; + const h = window.innerHeight; + const aspect = w / h; -window.addEventListener( 'keydown', e => { + renderer.setSize( w, h ); - switch( e.key ) { - case 'w': - transformControls.setMode( 'translate' ); - break; - case 'e': - transformControls.setMode( 'rotate' ); - break; - case 'q': - transformControls.setSpace( transformControls.space === 'local' ? 'world' : 'local' ); - break; - case 'f': - controls.target.set( 0, 0, 0 ); - controls.update(); - break; - } + camera.aspect = aspect; + camera.updateProjectionMatrix(); -} ); + ikHelper.setResolution( window.innerWidth, window.innerHeight ); + drawThroughIkHelper.setResolution( window.innerWidth, window.innerHeight ); -let startX = 0; -let startY = 0; -transformControls.addEventListener( 'mouseUp', () => { + } ); - if ( selectedGoalIndex !== - 1 ) { + window.addEventListener( 'keydown', e => { - const goal = extraGoals[ selectedGoalIndex ]; - const ikLink = goalToLinkMap.get( goal ); - if ( ikLink ) { + switch( e.key ) { + case 'w': + transformControls.setMode( 'translate' ); + break; + case 'e': + transformControls.setMode( 'rotate' ); + break; + case 'q': + transformControls.setSpace( transformControls.space === 'local' ? 'world' : 'local' ); + break; + case 'f': + controls.target.set( 0, 0, 0 ); + controls.update(); + break; + } - ikLink.updateMatrixWorld(); + } ); - ikLink.attachChild( goal ); - goal.setPosition( ...goal.originalPosition ); - goal.setQuaternion( ...goal.originalQuaternion ); - ikLink.detachChild( goal ); + transformControls.addEventListener( 'mouseUp', () => { - targetObject.position.set( ...goal.position ); - targetObject.quaternion.set( ...goal.quaternion ); + if ( selectedGoalIndex !== - 1 ) { - } + const goal = extraGoals[ selectedGoalIndex ]; + const ikLink = goalToLinkMap.get( goal ); + if ( ikLink ) { - } + ikLink.updateMatrixWorld(); -} ); + ikLink.attachChild( goal ); + goal.setPosition( ...goal.originalPosition ); + goal.setQuaternion( ...goal.originalQuaternion ); + ikLink.detachChild( goal ); -renderer.domElement.addEventListener( 'pointerdown', e => { + targetObject.position.set( ...goal.position ); + targetObject.quaternion.set( ...goal.quaternion ); - startX = e.clientX; - startY = e.clientY; + } -} ); + } -renderer.domElement.addEventListener( 'pointerup', e => { + } ); - if ( Math.abs( e.clientX - startX ) > 3 || Math.abs( e.clientY - startY ) > 3 ) return; + renderer.domElement.addEventListener( 'pointerdown', e => { - if ( ! urdfRoot ) return; + mouse.x = e.clientX; + mouse.y = e.clientY; - const { ikLink, result } = raycast( e ); + } ); - if ( ikLink === null ) { + renderer.domElement.addEventListener( 'pointerup', e => { - selectedGoalIndex = - 1; + if ( Math.abs( e.clientX - mouse.x ) > 3 || Math.abs( e.clientY - mouse.y ) > 3 ) return; - } + if ( ! urdfRoot ) return; - if ( e.button === 2 ) { + const { ikLink, result } = raycast( e ); - if ( ! ikLink ) { + if ( ikLink === null ) { - return; + selectedGoalIndex = - 1; } - if ( linkToGoalMap.has( ikLink ) ) { - - const goal = linkToGoalMap.get( ikLink ); - linkToGoalMap.delete( ikLink ); - goalToLinkMap.delete( goal ); + if ( e.button === 2 ) { - const i = extraGoals.indexOf( goal ); - extraGoals.splice( i, 1 ); + if ( ! ikLink ) { - const i2 = solver.roots.indexOf( goal ); - solver.roots.splice( i2, 1 ); - solver.updateStructure(); + return; - } + } - // normal in world space - const norm4 = new Vector4(); - norm4.copy( result.face.normal ); - norm4.w = 0; - norm4.applyMatrix4( result.object.matrixWorld ); + if ( linkToGoalMap.has( ikLink ) ) { - // create look matrix - const lookMat = mat4.create(); - const eyeVec = [ 0, 0, 0 ]; - const posVec = norm4.toArray(); - let upVec = [ 0, 1, 0 ]; - if ( Math.abs( posVec[ 1 ] ) > 0.9 ) { + const goal = linkToGoalMap.get( ikLink ); + linkToGoalMap.delete( ikLink ); + goalToLinkMap.delete( goal ); - upVec = [ 0, 0, 1 ]; + const i = extraGoals.indexOf( goal ); + extraGoals.splice( i, 1 ); - } - mat4.targetTo( lookMat, eyeVec, posVec, upVec ); + const i2 = solver.roots.indexOf( goal ); + solver.roots.splice( i2, 1 ); + solver.updateStructure(); - const rootGoalJoint = new Joint(); - rootGoalJoint.setPosition( - result.point.x, - result.point.y, - result.point.z, - ); - mat4.getRotation( rootGoalJoint.quaternion, lookMat ); + } - const goalLink = new Link; + // normal in world space + const norm4 = new Vector4(); + norm4.copy( result.face.normal ); + norm4.w = 0; + norm4.applyMatrix4( result.object.matrixWorld ); - const goalJoint = new Joint(); - ikLink.getWorldPosition( goalJoint.position ); - ikLink.getWorldQuaternion( goalJoint.quaternion ); - goalJoint.setMatrixNeedsUpdate(); + // create look matrix + const lookMat = mat4.create(); + const eyeVec = [ 0, 0, 0 ]; + const posVec = norm4.toArray(); + let upVec = [ 0, 1, 0 ]; + if ( Math.abs( posVec[ 1 ] ) > 0.9 ) { - rootGoalJoint.attachChild( goalLink ); - goalLink.attachChild( goalJoint ); - goalJoint.makeClosure( ikLink ); + upVec = [ 0, 0, 1 ]; - // save the relative position - ikLink.attachChild( rootGoalJoint ); - rootGoalJoint.originalPosition = rootGoalJoint.position.slice(); - rootGoalJoint.originalQuaternion = rootGoalJoint.quaternion.slice(); - ikLink.detachChild( rootGoalJoint ); + } + mat4.targetTo( lookMat, eyeVec, posVec, upVec ); + + const rootGoalJoint = new Joint(); + rootGoalJoint.setPosition( + result.point.x, + result.point.y, + result.point.z, + ); + mat4.getRotation( rootGoalJoint.quaternion, lookMat ); + + const goalLink = new Link; + + const goalJoint = new Joint(); + ikLink.getWorldPosition( goalJoint.position ); + ikLink.getWorldQuaternion( goalJoint.quaternion ); + goalJoint.setMatrixNeedsUpdate(); + + rootGoalJoint.attachChild( goalLink ); + goalLink.attachChild( goalJoint ); + goalJoint.makeClosure( ikLink ); + + // save the relative position + ikLink.attachChild( rootGoalJoint ); + rootGoalJoint.originalPosition = rootGoalJoint.position.slice(); + rootGoalJoint.originalQuaternion = rootGoalJoint.quaternion.slice(); + ikLink.detachChild( rootGoalJoint ); + + // update the solver + solver.roots.push( rootGoalJoint ); + solver.updateStructure(); - // update the solver - solver.roots.push( rootGoalJoint ); - solver.updateStructure(); + targetObject.position.set( ...rootGoalJoint.position ); + targetObject.quaternion.set( ...rootGoalJoint.quaternion ); - targetObject.position.set( ...rootGoalJoint.position ); - targetObject.quaternion.set( ...rootGoalJoint.quaternion ); + goalToLinkMap.set( rootGoalJoint, ikLink ); + linkToGoalMap.set( ikLink, rootGoalJoint ); + extraGoals.push( rootGoalJoint ); + selectedGoalIndex = extraGoals.length - 1; - goalToLinkMap.set( rootGoalJoint, ikLink ); - linkToGoalMap.set( ikLink, rootGoalJoint ); - extraGoals.push( rootGoalJoint ); - selectedGoalIndex = extraGoals.length - 1; + } else if ( e.button === 0 ) { - } else if ( e.button === 0 ) { + if ( ! transformControls.dragging ) { - if ( ! transformControls.dragging ) { + selectedGoalIndex = goalIcons.indexOf( result ? result.object.parent : null ); - selectedGoalIndex = goalIcons.indexOf( result ? result.object.parent : null ); + if ( selectedGoalIndex !== -1 ) { - if ( selectedGoalIndex !== -1 ) { + const ikgoal = extraGoals[ selectedGoalIndex ]; + targetObject.position.set( ...ikgoal.position ); + targetObject.quaternion.set( ...ikgoal.quaternion ); - const ikgoal = extraGoals[ selectedGoalIndex ]; - targetObject.position.set( ...ikgoal.position ); - targetObject.quaternion.set( ...ikgoal.quaternion ); + } else if ( ikLink && linkToGoalMap.has( ikLink ) ) { - } else if ( ikLink && linkToGoalMap.has( ikLink ) ) { + const goal = linkToGoalMap.get( ikLink ); + selectedGoalIndex = extraGoals.indexOf( goal ); + targetObject.position.set( ...goal.position ); + targetObject.quaternion.set( ...goal.quaternion ); - const goal = linkToGoalMap.get( ikLink ); - selectedGoalIndex = extraGoals.indexOf( goal ); - targetObject.position.set( ...goal.position ); - targetObject.quaternion.set( ...goal.quaternion ); + } } } - } + } ); -} ); + window.addEventListener( 'keydown', e => { -window.addEventListener( 'keydown', e => { + if ( selectedGoalIndex !== - 1 && e.code === 'Delete' ) { - if ( selectedGoalIndex !== - 1 && e.code === 'Delete' ) { + const goalToRemove = extraGoals[ selectedGoalIndex ]; + const i = solver.roots.indexOf( goalToRemove ); + solver.roots.splice( i, 1 ); + solver.updateStructure(); - const goalToRemove = extraGoals[ selectedGoalIndex ]; - const i = solver.roots.indexOf( goalToRemove ); - solver.roots.splice( i, 1 ); - solver.updateStructure(); + extraGoals.splice( selectedGoalIndex, 1 ); + selectedGoalIndex = - 1; - extraGoals.splice( selectedGoalIndex, 1 ); - selectedGoalIndex = - 1; + const link = goalToLinkMap.get( goalToRemove ); + goalToLinkMap.delete( goalToRemove ); + linkToGoalMap.delete( link ); - const link = goalToLinkMap.get( goalToRemove ); - goalToLinkMap.delete( goalToRemove ); - linkToGoalMap.delete( link ); + } - } + } ); -} ); +} function raycast( e ) { @@ -369,8 +378,6 @@ function raycast( e ) { } -let averageTime = 0; -let averageCount = 0; function render() { requestAnimationFrame( render ); @@ -572,35 +579,6 @@ function rebuildGUI() { solveFolder.add( solverOptions, 'restPoseFactor' ).min( 0 ).max( 1e-1 ).step( 1e-4 ).listen(); solveFolder.open(); - // ikRoot.traverse( c => { - - // if ( c instanceof Joint ) { - - // const folder = gui.addFolder( c.name ); - // folder.open(); - - // const names = Object.keys( DOF ); - // c.dof.forEach( ( dof, i ) => { - - // folder - // .add( c.dofValues, dof ) - // .name( names[ dof ] ) - // .min( - Math.PI ) - // .max( Math.PI ) - // .step( 0.001 ) - // .onChange( v => { - - // c.setMatrixDoFNeedsUpdate(); - - // } ) - // .listen(); - - // } ); - - // } - - // } ); - } function dispose( c ) { From 6869263bdf7a23f9eb00064d8ac18dc6c2943acb Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 22:00:04 -0700 Subject: [PATCH 07/20] comments, updates --- src/core/ChainSolver.js | 4 ++++ src/core/Goal.js | 40 ++-------------------------------------- 2 files changed, 6 insertions(+), 38 deletions(-) diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index b8ccf5a..a71b740 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -495,6 +495,7 @@ export class ChainSolver { let colIndex = 0; for ( let c = 0, tc = freeJoints.length; c < tc; c ++ ) { + // TODO: If this is a goal we should skip adding it to the jacabian columns const freeJoint = freeJoints[ c ]; const relevantClosures = affectedClosures.get( freeJoint ); const relevantConnectedClosures = affectedConnectedClosures.get( freeJoint ); @@ -701,6 +702,9 @@ export class ChainSolver { // TODO: We may be able to speed this up by using the square distance and length // to compare error. + // TODO: If this is a goal we shouldnt add to the free dof because they won't be added + // to the jacobian + // If this is a closure joint then we need to make sure we're solving // for the other child end to meet this joint so this error is important. if ( joint.isClosure ) { diff --git a/src/core/Goal.js b/src/core/Goal.js index 4f64f82..166b182 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -1,45 +1,17 @@ -import { DOF_NAMES, DOF } from './Joint.js'; +import { DOF } from './Joint.js'; import { Frame } from './Frame.js'; -// TODO: we should just extend Joint here... export class Goal extends Frame { constructor( ...args ) { super( ...args ); this.isGoal = true; - this.isJoint = true; - - this.freeDoF = []; - this.child = null; - this.isClosure = false; - } setFreeDoF( ...args ) { - args.forEach( ( dof, i ) => { - - if ( dof < 0 || dof >= 6 ){ - - throw new Error( 'Goal: Invalid degree of freedom enum ' + dof + '.' ); - - } - - if ( args.includes( dof, i + 1 ) ) { - - throw new Error( 'Goal: Duplicate degree of freedom ' + DOF_NAMES[ dof ] + 'specified.' ); - - } - - if ( i !== 0 && args[ i - 1 ] > dof ) { - - throw new Error( 'Goal: Joints degrees of freedom must be specified in position then rotation, XYZ order' ); - } - - } ); - - this.freeDoF = args; + this.setFreeDoF( ...args ); } @@ -69,14 +41,6 @@ export class Goal extends Frame { } - removeChild( child ) { - - super.removeChild( child ); - this.child = null; - this.isClosure = false; - - } - addChild() { throw new Error(); From 53b9d3acfc1ba2cc904c34cd67372e2e43fd87cc Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 22:07:59 -0700 Subject: [PATCH 08/20] remove more --- src/core/Goal.js | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/core/Goal.js b/src/core/Goal.js index 166b182..5bc61dc 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -25,22 +25,6 @@ export class Goal extends Frame { } - makeClosure( child ) { - - if ( ! child.isLink || this.children.length >= 1 || child.parent === this ) { - - throw new Error(); - - } else { - - this.children[ 0 ] = child; - this.child = child; - this.isClosure = true; - - } - - } - addChild() { throw new Error(); From 3e87d571d827c74b9d252d8e2ffb69d5561b2077 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Wed, 14 Oct 2020 22:16:32 -0700 Subject: [PATCH 09/20] Update serialization --- src/worker/serialize.js | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/worker/serialize.js b/src/worker/serialize.js index fef8e17..3d1b2a9 100644 --- a/src/worker/serialize.js +++ b/src/worker/serialize.js @@ -1,5 +1,6 @@ import { Joint } from '../core/Joint.js'; import { Link } from '../core/Link.js'; +import { Goal } from '../core/Goal.js'; // Takes a list of interconnected frames and serializes them into a non cyclic json representation export function serialize( frames ) { @@ -27,6 +28,17 @@ export function serialize( frames ) { isClosure, } = frame; + let type = 'Link'; + if ( frame.isGoal ) { + + type = 'Goal'; + + } else if ( frame.isJoint ) { + + type = 'Joint'; + + } + const res = { dof: dof ? dof.slice() : null, dofValues: dofValues ? dofValues.slice() : null, @@ -42,7 +54,7 @@ export function serialize( frames ) { position: position.slice(), quaternion: quaternion.slice(), children: null, - type: frame.isJoint ? 'Joint' : 'Link', + type, }; info.push( res ); @@ -99,8 +111,9 @@ export function deserialize( data ) { let frame; switch ( type ) { + case 'Goal': case 'Joint': - frame = new Joint(); + frame = type === 'Goal' ? new Goal() : new Joint(); frame.setDoF( ...dof ); frame.dofValues.set( dofValues ); From bb5a38b208c6fbc581bbbb6fc3c4568dbfedaa42 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 09:36:32 -0700 Subject: [PATCH 10/20] Comments, goal function update --- src/core/Goal.js | 18 ++++++++++++++++++ src/core/utils/solver.js | 3 ++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/core/Goal.js b/src/core/Goal.js index 5bc61dc..128f598 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -9,6 +9,24 @@ export class Goal extends Frame { this.isGoal = true; } + setDoF( ...args ) { + + // We don't support rotation goals that only specify 1 or 2 free rotation axes. + let rotCount = + Number( args.includes( DOF.EX ) ) + + Number( args.includes( DOF.EY ) ) + + Number( args.includes( DOF.EZ ) ); + + if ( rotCount !== 0 || rotCount !== 3 ) { + + throw new Error(); + + } + + super.setDoF( ...args ); + + } + setFreeDoF( ...args ) { this.setFreeDoF( ...args ); diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index c0de174..a24e1f5 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -99,7 +99,8 @@ export function accumulateTargetError( const posDelta = vec3.distance( dofValues, dofTarget ); // TODO: if three euler angles are being used we should set this to a quaternion to measure - // error rather than euler angles. + // error rather than euler angles. We should instead just always use quaternions for targets + // for now. // Before running this solver we try to ensure the target and restPose are minimized let rotDelta = dofTarget[ DOF.EX ] - dofValues[ DOF.EX ] + From fb4455d393795802213a4f9866ba8ca0fe9a0c9b Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 09:54:24 -0700 Subject: [PATCH 11/20] Handle goal case --- src/core/ChainSolver.js | 52 +++++++++++++++++++++++------- src/core/utils/solver.js | 68 +++++++++++++++++++++++++++++++++------- 2 files changed, 98 insertions(+), 22 deletions(-) diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index a71b740..9e74a3f 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -528,7 +528,7 @@ export class ChainSolver { let delta = dof < 3 ? translationStep : rotationStep; if ( freeJoint.getDeltaWorldMatrix( dof, delta, tempDeltaWorldMatrix ) ) { - delta *= -1; + delta *= - 1; } @@ -577,21 +577,51 @@ export class ChainSolver { vec4.subtract( tempQuat, tempQuat2, tempQuat ); vec4.scale( tempQuat, tempQuat, 1 / delta ); - // set translation - outJacobian[ rowIndex + 0 ][ colIndex ] = tempPos[ 0 ]; - outJacobian[ rowIndex + 1 ][ colIndex ] = tempPos[ 1 ]; - outJacobian[ rowIndex + 2 ][ colIndex ] = tempPos[ 2 ]; + if ( targetJoint.isGoal ) { - // set rotation - outJacobian[ rowIndex + 3 ][ colIndex ] = tempQuat[ 0 ]; - outJacobian[ rowIndex + 4 ][ colIndex ] = tempQuat[ 1 ]; - outJacobian[ rowIndex + 5 ][ colIndex ] = tempQuat[ 2 ]; - outJacobian[ rowIndex + 6 ][ colIndex ] = tempQuat[ 3 ]; + const { translationDoFCount, rotationDoFCount, dof } = targetJoint; + for ( let i = 0; i < translationDoFCount; i ++ ) { + + const d = dof[ i ]; + outJacobian[ rowIndex + i ][ colIndex ] = tempPos[ d ]; + + } + + if ( rotationDoFCount === 3 ) { + + outJacobian[ rowIndex + translationDoFCount + 0 ][ colIndex ] = tempQuat[ 0 ]; + outJacobian[ rowIndex + translationDoFCount + 1 ][ colIndex ] = tempQuat[ 1 ]; + outJacobian[ rowIndex + translationDoFCount + 2 ][ colIndex ] = tempQuat[ 2 ]; + outJacobian[ rowIndex + translationDoFCount + 3 ][ colIndex ] = tempQuat[ 3 ]; + + } + + } else { + + // set translation + outJacobian[ rowIndex + 0 ][ colIndex ] = tempPos[ 0 ]; + outJacobian[ rowIndex + 1 ][ colIndex ] = tempPos[ 1 ]; + outJacobian[ rowIndex + 2 ][ colIndex ] = tempPos[ 2 ]; + + // set rotation + outJacobian[ rowIndex + 3 ][ colIndex ] = tempQuat[ 0 ]; + outJacobian[ rowIndex + 4 ][ colIndex ] = tempQuat[ 1 ]; + outJacobian[ rowIndex + 5 ][ colIndex ] = tempQuat[ 2 ]; + outJacobian[ rowIndex + 6 ][ colIndex ] = tempQuat[ 3 ]; + + } } else { // if the target isn't relevant then there's no delta - for ( let i = 0; i < 7; i ++ ) { + let totalRows = 7; + if ( targetJoint.isGoal ) { + + targetRows = targetJoint.translationDoFCount + targetJoint.rotationDoFCount + + } + + for ( let i = 0; i < totalRows; i ++ ) { outJacobian[ rowIndex + i ][ colIndex ] = 0; diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index a24e1f5..a2b7407 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -18,10 +18,37 @@ export function accumulateClosureError( rotationErrorClamp, } = solver; + const { + translationDoFCount, + rotationDoFCount, + dofFlags, + dof, + } = joint; + // TODO: If this is a Goal and we have less than three euler DoF we should use euler angles (and adjust the rows). Otherwise we // should use a quat. joint.getClosureError( tempPos, tempQuat ); + let rowCount = 7; + if ( joint.isGoal ) { + + tempPos[ 0 ] *= dofFlags[ 0 ]; + tempPos[ 1 ] *= dofFlags[ 1 ]; + tempPos[ 2 ] *= dofFlags[ 2 ]; + rowCount = translationDoFCount; + + if ( rotationDoFCount === 0 ) { + + tempQuat[ 0 ] = 0; + tempQuat[ 1 ] = 0; + tempQuat[ 2 ] = 0; + tempQuat[ 3 ] = 0; + rowCount += 4; + + } + + } + let isConverged = false; let totalError = 0; const posMag = vec3.length( tempPos ); @@ -35,11 +62,8 @@ export function accumulateClosureError( } - totalError += posMag + rotMag; - if ( errorVector ) { - if ( posMag > translationErrorClamp ) { vec3.scale( tempPos, tempPos, translationErrorClamp / posMag ); @@ -52,20 +76,42 @@ export function accumulateClosureError( } - errorVector[ startIndex + 0 ][ 0 ] = tempPos[ 0 ]; - errorVector[ startIndex + 1 ][ 0 ] = tempPos[ 1 ]; - errorVector[ startIndex + 2 ][ 0 ] = tempPos[ 2 ]; + if ( joint.isGoal ) { + + for ( let i = 0; i < translationDoFCount; i ++ ) { + + const d = dof[ i ]; + errorVector[ startIndex + i ][ 0 ] = tempPos[ d ]; + + } + + if ( joint.rotationDoFCount === 3 ) { + + errorVector[ startIndex + translationDoFCount + 0 ][ 0 ] = tempQuat[ 0 ]; + errorVector[ startIndex + translationDoFCount + 1 ][ 0 ] = tempQuat[ 1 ]; + errorVector[ startIndex + translationDoFCount + 2 ][ 0 ] = tempQuat[ 2 ]; + errorVector[ startIndex + translationDoFCount + 3 ][ 0 ] = tempQuat[ 3 ]; - errorVector[ startIndex + 3 ][ 0 ] = tempQuat[ 0 ]; - errorVector[ startIndex + 4 ][ 0 ] = tempQuat[ 1 ]; - errorVector[ startIndex + 5 ][ 0 ] = tempQuat[ 2 ]; - errorVector[ startIndex + 6 ][ 0 ] = tempQuat[ 3 ]; + } + + } else { + + errorVector[ startIndex + 0 ][ 0 ] = tempPos[ 0 ]; + errorVector[ startIndex + 1 ][ 0 ] = tempPos[ 1 ]; + errorVector[ startIndex + 2 ][ 0 ] = tempPos[ 2 ]; + + errorVector[ startIndex + 3 ][ 0 ] = tempQuat[ 0 ]; + errorVector[ startIndex + 4 ][ 0 ] = tempQuat[ 1 ]; + errorVector[ startIndex + 5 ][ 0 ] = tempQuat[ 2 ]; + errorVector[ startIndex + 6 ][ 0 ] = tempQuat[ 3 ]; + + } } result.totalError = totalError; result.isConverged = isConverged; - result.rowCount = 7; + result.rowCount = rowCount; return result; } From 628158b6ab807461985cb30842d8bff3f30436e0 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 09:54:31 -0700 Subject: [PATCH 12/20] variable rename --- example/index.js | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/example/index.js b/example/index.js index 462fd68..6c63526 100644 --- a/example/index.js +++ b/example/index.js @@ -60,7 +60,7 @@ const solverOptions = { const goalToLinkMap = new Map(); const linkToGoalMap = new Map(); -const extraGoals = []; +const goals = []; const goalIcons = []; let selectedGoalIndex = - 1; @@ -158,7 +158,7 @@ function init() { if ( selectedGoalIndex !== - 1 ) { - const goal = extraGoals[ selectedGoalIndex ]; + const goal = goals[ selectedGoalIndex ]; const ikLink = goalToLinkMap.get( goal ); if ( ikLink ) { @@ -213,8 +213,8 @@ function init() { linkToGoalMap.delete( ikLink ); goalToLinkMap.delete( goal ); - const i = extraGoals.indexOf( goal ); - extraGoals.splice( i, 1 ); + const i = goals.indexOf( goal ); + goals.splice( i, 1 ); const i2 = solver.roots.indexOf( goal ); solver.roots.splice( i2, 1 ); @@ -274,8 +274,8 @@ function init() { goalToLinkMap.set( rootGoalJoint, ikLink ); linkToGoalMap.set( ikLink, rootGoalJoint ); - extraGoals.push( rootGoalJoint ); - selectedGoalIndex = extraGoals.length - 1; + goals.push( rootGoalJoint ); + selectedGoalIndex = goals.length - 1; } else if ( e.button === 0 ) { @@ -285,14 +285,14 @@ function init() { if ( selectedGoalIndex !== -1 ) { - const ikgoal = extraGoals[ selectedGoalIndex ]; + const ikgoal = goals[ selectedGoalIndex ]; targetObject.position.set( ...ikgoal.position ); targetObject.quaternion.set( ...ikgoal.quaternion ); } else if ( ikLink && linkToGoalMap.has( ikLink ) ) { const goal = linkToGoalMap.get( ikLink ); - selectedGoalIndex = extraGoals.indexOf( goal ); + selectedGoalIndex = goals.indexOf( goal ); targetObject.position.set( ...goal.position ); targetObject.quaternion.set( ...goal.quaternion ); @@ -308,12 +308,12 @@ function init() { if ( selectedGoalIndex !== - 1 && e.code === 'Delete' ) { - const goalToRemove = extraGoals[ selectedGoalIndex ]; + const goalToRemove = goals[ selectedGoalIndex ]; const i = solver.roots.indexOf( goalToRemove ); solver.roots.splice( i, 1 ); solver.updateStructure(); - extraGoals.splice( selectedGoalIndex, 1 ); + goals.splice( selectedGoalIndex, 1 ); selectedGoalIndex = - 1; const link = goalToLinkMap.get( goalToRemove ); @@ -336,9 +336,9 @@ function raycast( e ) { raycaster.setFromCamera( mouse, camera ); let results; - const fewerGoals = [ ...goalIcons ]; - fewerGoals.length = extraGoals.length; - results = raycaster.intersectObjects( fewerGoals, true ); + const intersectGoals = [ ...goalIcons ]; + intersectGoals.length = goals.length; + results = raycaster.intersectObjects( intersectGoals, true ); if ( results.length !== 0 ) { return { ikLink: null, result: results[ 0 ] }; @@ -382,7 +382,7 @@ function render() { requestAnimationFrame( render ); - const allGoals = extraGoals; + const allGoals = goals; const selectedGoal = allGoals[ selectedGoalIndex ]; if ( ikRoot ) { @@ -637,7 +637,7 @@ function loadModel( promise ) { urdfRoot = null; ikHelper = null; drawThroughIkHelper = null; - extraGoals.length = 0; + goals.length = 0; goalToLinkMap.clear(); linkToGoalMap.clear(); selectedGoalIndex = - 1; @@ -727,7 +727,7 @@ function loadModel( promise ) { ikRoot = ik; urdfRoot = urdf; - extraGoals.push( ...goals ); + goals.push( ...goals ); rebuildGUI(); From edd3e600e3c9d8137393a459028f69a0ec908713 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 19:08:18 -0700 Subject: [PATCH 13/20] small fixes --- example/index.js | 17 +++++++++-------- src/core/Goal.js | 6 +++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/example/index.js b/example/index.js index 6c63526..c922595 100644 --- a/example/index.js +++ b/example/index.js @@ -30,6 +30,7 @@ import { WorkerSolver, Link, Joint, + Goal, SOLVE_STATUS_NAMES, IKRootsHelper, @@ -690,26 +691,26 @@ function loadModel( promise ) { scene.add( urdf, ikHelper, drawThroughIkHelper ); - const goals = []; + const loadedGoals = []; goalMap.forEach( ( link, goal ) => { - goals.push( goal ); + loadedGoals.push( goal ); goalToLinkMap.set( goal, link ); linkToGoalMap.set( link, goal ); } ); - solver = params.webworker ? new WorkerSolver( [ ik, ...goals ] ) : new Solver( [ ik, ...goals ] ); + solver = params.webworker ? new WorkerSolver( [ ik, ...loadedGoals ] ) : new Solver( [ ik, ...loadedGoals ] ); solver.maxIterations = 3; solver.translationErrorClamp = 0.25; solver.rotationErrorClamp = 0.25; solver.restPoseFactor = 0.01; solver.divergeThreshold = 0.05; - if ( goals.length ) { + if ( loadedGoals.length ) { - targetObject.position.set( ...goals[ 0 ].position ); - targetObject.quaternion.set( ...goals[ 0 ].quaternion ); + targetObject.position.set( ...loadedGoals[ 0 ].position ); + targetObject.quaternion.set( ...loadedGoals[ 0 ].quaternion ); selectedGoalIndex = 0; } else { @@ -718,7 +719,7 @@ function loadModel( promise ) { } - goals.forEach( g => { + loadedGoals.forEach( g => { g.originalPosition = [ 0, 0, 0 ]; g.originalQuaternion = [ 0, 0, 0, 1 ]; @@ -727,7 +728,7 @@ function loadModel( promise ) { ikRoot = ik; urdfRoot = urdf; - goals.push( ...goals ); + goals.push( ...loadedGoals ); rebuildGUI(); diff --git a/src/core/Goal.js b/src/core/Goal.js index 128f598..e122348 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -1,7 +1,7 @@ import { DOF } from './Joint.js'; -import { Frame } from './Frame.js'; +import { Joint } from './Joint.js'; -export class Goal extends Frame { +export class Goal extends Joint { constructor( ...args ) { @@ -17,7 +17,7 @@ export class Goal extends Frame { Number( args.includes( DOF.EY ) ) + Number( args.includes( DOF.EZ ) ); - if ( rotCount !== 0 || rotCount !== 3 ) { + if ( rotCount !== 0 && rotCount !== 3 ) { throw new Error(); From 11ab1f95ad01d5f0c7b1631d3ce14e028359419d Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 19:34:48 -0700 Subject: [PATCH 14/20] Make it work --- example/index.js | 2 +- src/core/ChainSolver.js | 17 +++++++++++++---- src/core/Goal.js | 8 +++++--- src/core/utils/solver.js | 2 +- 4 files changed, 20 insertions(+), 9 deletions(-) diff --git a/example/index.js b/example/index.js index c922595..206e095 100644 --- a/example/index.js +++ b/example/index.js @@ -251,7 +251,7 @@ function init() { const goalLink = new Link; - const goalJoint = new Joint(); + const goalJoint = new Goal(); ikLink.getWorldPosition( goalJoint.position ); ikLink.getWorldQuaternion( goalJoint.quaternion ); goalJoint.setMatrixNeedsUpdate(); diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index 9e74a3f..33c1275 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -615,9 +615,14 @@ export class ChainSolver { // if the target isn't relevant then there's no delta let totalRows = 7; - if ( targetJoint.isGoal ) { + if ( targetJoint.isGoal ) { + + totalRows = targetJoint.translationDoFCount; + if ( targetJoint.rotationDoFCount === 3 ) { - targetRows = targetJoint.translationDoFCount + targetJoint.rotationDoFCount + totalRows += 4; + + } } @@ -768,8 +773,12 @@ export class ChainSolver { } - freeDoF += dofList.length - lockedDoF; - freeJoints.push( joint ); + if ( ! joint.isGoal ) { + + freeDoF += dofList.length - lockedDoF; + freeJoints.push( joint ); + + } if ( addToTargetList ) { diff --git a/src/core/Goal.js b/src/core/Goal.js index e122348..54614a6 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -7,6 +7,8 @@ export class Goal extends Joint { super( ...args ); this.isGoal = true; + this.setFreeDoF(); + } setDoF( ...args ) { @@ -27,19 +29,19 @@ export class Goal extends Joint { } - setFreeDoF( ...args ) { + setGoalDoF( ...args ) { this.setFreeDoF( ...args ); } - setGoalDoF( ...args ) { + setFreeDoF( ...args ) { const freeDoF = [ DOF.X, DOF.Y, DOF.Z, DOF.EX, DOF.EY, DOF.EZ, ].filter( d => ! args.includes( d ) ); - this.setFreeDoF( ...freeDoF ); + this.setDoF( ...freeDoF ); } diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index a2b7407..09e2878 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -37,7 +37,7 @@ export function accumulateClosureError( tempPos[ 2 ] *= dofFlags[ 2 ]; rowCount = translationDoFCount; - if ( rotationDoFCount === 0 ) { + if ( rotationDoFCount === 3 ) { tempQuat[ 0 ] = 0; tempQuat[ 1 ] = 0; From e811fa260ca5618bf7abb7503f5cfb8beffb99eb Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 20:17:58 -0700 Subject: [PATCH 15/20] update --- example/index.js | 2 +- src/core/ChainSolver.js | 7 +++++-- src/core/Goal.js | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/example/index.js b/example/index.js index 206e095..c922595 100644 --- a/example/index.js +++ b/example/index.js @@ -251,7 +251,7 @@ function init() { const goalLink = new Link; - const goalJoint = new Goal(); + const goalJoint = new Joint(); ikLink.getWorldPosition( goalJoint.position ); ikLink.getWorldQuaternion( goalJoint.quaternion ); goalJoint.setMatrixNeedsUpdate(); diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index 33c1275..72ec952 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -593,8 +593,10 @@ export class ChainSolver { outJacobian[ rowIndex + translationDoFCount + 1 ][ colIndex ] = tempQuat[ 1 ]; outJacobian[ rowIndex + translationDoFCount + 2 ][ colIndex ] = tempQuat[ 2 ]; outJacobian[ rowIndex + translationDoFCount + 3 ][ colIndex ] = tempQuat[ 3 ]; + rowIndex += 4; } + rowIndex += translationDoFCount; } else { @@ -608,6 +610,7 @@ export class ChainSolver { outJacobian[ rowIndex + 4 ][ colIndex ] = tempQuat[ 1 ]; outJacobian[ rowIndex + 5 ][ colIndex ] = tempQuat[ 2 ]; outJacobian[ rowIndex + 6 ][ colIndex ] = tempQuat[ 3 ]; + rowIndex += 7; } @@ -632,9 +635,9 @@ export class ChainSolver { } - } + rowIndex += totalRows; - rowIndex += 7; + } } diff --git a/src/core/Goal.js b/src/core/Goal.js index 54614a6..41f5eda 100644 --- a/src/core/Goal.js +++ b/src/core/Goal.js @@ -31,7 +31,7 @@ export class Goal extends Joint { setGoalDoF( ...args ) { - this.setFreeDoF( ...args ); + this.setDoF( ...args ); } From cd1a4529d4d47ca7f99d8cfefa2d76fbd90baaca Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 20:34:33 -0700 Subject: [PATCH 16/20] Add totalerror back --- src/core/utils/solver.js | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index 09e2878..624ffbb 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -62,6 +62,8 @@ export function accumulateClosureError( } + totalError += posMag + rotMag; + if ( errorVector ) { if ( posMag > translationErrorClamp ) { From 757c257c723765a900ccbcce990238eacb386396 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 21:01:02 -0700 Subject: [PATCH 17/20] add goal tests --- test/core/Goal.test.js | 87 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 test/core/Goal.test.js diff --git a/test/core/Goal.test.js b/test/core/Goal.test.js new file mode 100644 index 0000000..60bac95 --- /dev/null +++ b/test/core/Goal.test.js @@ -0,0 +1,87 @@ +import { Goal } from '../../src/core/Goal.js'; +import { DOF } from '../../src/core/Joint.js'; +import { Link } from '../../src/core/Link.js'; + +describe( 'Goal', () => { + + describe( 'setFreeDoF', () => { + + it( 'should set dof to the inverted DoF.', () => { + + const goal = new Goal(); + goal.setFreeDoF( DOF.X, DOF.Z ); + expect( goal.dof ).toEqual( [ DOF.Y, DOF.EX, DOF.EY, DOF.EZ ] ); + + goal.setFreeDoF( DOF.X, DOF.EX, DOF.EY, DOF.EZ ); + expect( goal.dof ).toEqual( [ DOF.Y, DOF.Z ] ); + + } ); + + } ); + + describe( 'setGoalDoF', () => { + + it( 'should set dof.', () => { + + const goal = new Goal(); + goal.setGoalDoF( DOF.X, DOF.Z ); + expect( goal.dof ).toEqual( [ DOF.X, DOF.Z ] ); + + } ); + + it( 'should fail if only a partial set of rotations are passed in.', () => { + + const goal = new Goal(); + let caught = false; + try { + + goal.setGoalDoF( DOF.EX ); + + } catch { + + caught = true; + + } + expect( caught ).toBeTruthy(); + + } ); + + } ); + + describe( 'addChild', () => { + + it( 'should not be abled to be called.', () => { + + const goal = new Goal(); + let caught; + caught = false; + try { + + goal.addChild( new Link() ); + + } catch { + + caught = true; + + } + expect( caught ).toBeTruthy(); + + caught = false; + try { + + goal.attachChild( new Link() ); + + } catch { + + caught = true; + + } + expect( caught ).toBeTruthy(); + + } ); + + } ); + +} ); + + From c78b22535b87d5dde1608c9c352f5b8938c6041e Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 21:55:52 -0700 Subject: [PATCH 18/20] add goals example --- example/goals.html | 44 +++++++ example/goals.js | 249 +++++++++++++++++++++++++++++++++++++ example/index.js | 2 - package.json | 2 +- src/core/ChainSolver.js | 4 +- src/core/Solver.js | 2 +- src/three/IKRootsHelper.js | 1 + 7 files changed, 298 insertions(+), 6 deletions(-) create mode 100644 example/goals.html create mode 100644 example/goals.js diff --git a/example/goals.html b/example/goals.html new file mode 100644 index 0000000..886504c --- /dev/null +++ b/example/goals.html @@ -0,0 +1,44 @@ + + + + + + +
+
+ + + diff --git a/example/goals.js b/example/goals.js new file mode 100644 index 0000000..372ec23 --- /dev/null +++ b/example/goals.js @@ -0,0 +1,249 @@ +import { + WebGLRenderer, + PerspectiveCamera, + Color, + Scene, + DirectionalLight, + AmbientLight, + sRGBEncoding, + Group, +} from 'three'; +import { + OrbitControls, +} from 'three/examples/jsm/controls/OrbitControls.js'; +import { + TransformControls +} from 'three/examples/jsm/controls/TransformControls.js'; +import { + GUI, +} from 'three/examples/jsm/libs/dat.gui.module.js'; +import Stats from 'three/examples/jsm/libs/stats.module.js'; +import { + Solver, + Link, + Joint, + IKRootsHelper, + Goal, + DOF, + SOLVE_STATUS_NAMES, +} from '../src/index.js'; + +const params = { + solvePosition: true, + solveRotation: false, +}; + +// TODO: why is the solve stalling so frequently? Only when goal is set with rotation. +// Matching rotation goal doesn't seem to work. +const solverOptions = { + maxIterations: 3, + divergeThreshold: 0.005, + stallThreshold: 1e-7, + translationErrorClamp: 0.25, + rotationErrorClamp: 0.25, + translationConvergeThreshold: 1e-4, + rotationConvergeThreshold: 1e-5, + restPoseFactor: 0.01, +}; + +let gui, stats; +let outputContainer, renderer, scene, camera; +let solver, ikHelper, ikRoot, goal; +let controls, transformControls, targetObject; + +init(); +render(); + +function init() { + + stats = new Stats(); + document.body.appendChild( stats.dom ); + + outputContainer = document.getElementById( 'output' ); + + // init renderer + renderer = new WebGLRenderer( { antialias: true } ); + renderer.setPixelRatio( window.devicePixelRatio ); + renderer.setSize( window.innerWidth, window.innerHeight ); + renderer.outputEncoding = sRGBEncoding; + document.body.appendChild( renderer.domElement ); + + camera = new PerspectiveCamera( 50, window.innerWidth / window.innerHeight ); + camera.position.set( 8, 8, 8 ); + + scene = new Scene(); + scene.background = new Color( 0x131619 ); + + const directionalLight = new DirectionalLight(); + directionalLight.position.set( 1, 3, 2 ); + scene.add( directionalLight ); + + const ambientLight = new AmbientLight( 0x263238, 1 ); + scene.add( ambientLight ); + + controls = new OrbitControls( camera, renderer.domElement ); + transformControls = new TransformControls( camera, renderer.domElement ); + transformControls.setSpace( 'local' ); + scene.add( transformControls ); + + transformControls.addEventListener( 'mouseDown', () => controls.enabled = false ); + transformControls.addEventListener( 'mouseUp', () => controls.enabled = true ); + + targetObject = new Group(); + targetObject.position.set( 0, 1, 1 ) + scene.add( targetObject ); + transformControls.attach( targetObject ); + + ikRoot = null; + let currRoot = null; + for ( let i = 0; i < 9; i ++ ) { + + const link = new Link(); + const joint = new Joint(); + joint.setPosition( 0, 0.5, 0 ); + joint.setDoF( 3 + i % 3 ); + joint.setDoFValues( Math.PI / 4 ); + joint.setRestPoseValues( Math.PI / 4 ); + joint.restPoseSet = true; + joint.setMinLimits( - 0.9 * Math.PI ); + joint.setMaxLimits( 0.9 * Math.PI ); + + link.addChild( joint ); + + if ( currRoot ) { + + currRoot.addChild( link ); + + } + + if ( ikRoot === null ) { + + ikRoot = link; + + } + + currRoot = joint; + + } + + const finalLink = new Link(); + finalLink.setPosition( 0, 0.5, 0 ); + currRoot.addChild( finalLink ); + + ikRoot.updateMatrixWorld( true ); + targetObject.matrix.set( ...finalLink.matrixWorld ).transpose(); + targetObject.matrix + .decompose( targetObject.position, targetObject.quaternion, targetObject.scale ); + + // TODO: rotation seems not to work here + goal = new Goal(); + goal.makeClosure( finalLink ); + + ikHelper = new IKRootsHelper( ikRoot ); + ikHelper.update(); + ikHelper.setResolution( window.innerWidth, window.innerHeight ); + ikHelper.traverse( c => { + + if ( c.material ) { + + c.material.color.set( 0xe91e63 ).convertSRGBToLinear(); + + } + + } ); + scene.add( ikHelper ); + + solver = new Solver( [ ikRoot, goal ] ); + Object.assign( solver, solverOptions ); + + function updateGoalDoF() { + + const dof = []; + if ( params.solvePosition ) { + + dof.push( DOF.X, DOF.Y, DOF.Z ); + + } + + if ( params.solveRotation ) { + + dof.push( DOF.EX, DOF.EY, DOF.EZ ); + + } + + goal.setGoalDoF( ...dof ); + + } + updateGoalDoF(); + + gui = new GUI(); + gui.width = 350; + gui.add( params, 'solvePosition' ).onChange( updateGoalDoF ); + gui.add( params, 'solveRotation' ).onChange( updateGoalDoF ); + + + transformControls.addEventListener( 'mouseUp', () => { + + ikRoot.updateMatrixWorld( true ); + targetObject.matrix.set( ...finalLink.matrixWorld ).transpose(); + targetObject.matrix + .decompose( targetObject.position, targetObject.quaternion, targetObject.scale ); + + } ); + + window.addEventListener( 'resize', () => { + + const w = window.innerWidth; + const h = window.innerHeight; + const aspect = w / h; + + renderer.setSize( w, h ); + + camera.aspect = aspect; + camera.updateProjectionMatrix(); + + ikHelper.setResolution( window.innerWidth, window.innerHeight ); + + } ); + + window.addEventListener( 'keydown', e => { + + switch( e.key ) { + case 'w': + transformControls.setMode( 'translate' ); + break; + case 'e': + transformControls.setMode( 'rotate' ); + break; + case 'f': + controls.target.set( 0, 0, 0 ); + controls.update(); + break; + } + + } ); + +} + +function render() { + + requestAnimationFrame( render ); + + goal.setPosition( + targetObject.position.x, + targetObject.position.y, + targetObject.position.z, + ); + goal.setQuaternion( + targetObject.quaternion.x, + targetObject.quaternion.y, + targetObject.quaternion.z, + targetObject.quaternion.w, + ); + + outputContainer.textContent = solver.solve().map( s => SOLVE_STATUS_NAMES[ s ] ).join( '\n' ); + + renderer.render( scene, camera ); + stats.update(); + +} diff --git a/example/index.js b/example/index.js index c922595..30a8556 100644 --- a/example/index.js +++ b/example/index.js @@ -30,9 +30,7 @@ import { WorkerSolver, Link, Joint, - Goal, SOLVE_STATUS_NAMES, - IKRootsHelper, setUrdfFromIK, } from '../src/index.js'; diff --git a/package.json b/package.json index c2a301e..5fa4711 100644 --- a/package.json +++ b/package.json @@ -2,7 +2,7 @@ "name": "closed-chain-ik", "type": "module", "scripts": { - "start": "parcel serve ./example/index.html", + "start": "parcel serve ./example/*.html", "test": "jest" }, "repository": { diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index 72ec952..06aecce 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -33,8 +33,8 @@ const dofResultInfo = { export const SOLVE_STATUS = { CONVERGED: 0, - STALLED: 2, - DIVERGED: 1, + STALLED: 1, + DIVERGED: 2, TIMEOUT: 3, }; diff --git a/src/core/Solver.js b/src/core/Solver.js index e11fe75..04d9cf4 100644 --- a/src/core/Solver.js +++ b/src/core/Solver.js @@ -13,7 +13,7 @@ export class Solver { this.restPoseFactor = 0.01; this.translationConvergeThreshold = 1e-3; - this.rotationConvergeThreshold = 1e-3; + this.rotationConvergeThreshold = 1e-5; this.translationFactor = 1; this.rotationFactor = 1; diff --git a/src/three/IKRootsHelper.js b/src/three/IKRootsHelper.js index 83644d2..f8ff40b 100644 --- a/src/three/IKRootsHelper.js +++ b/src/three/IKRootsHelper.js @@ -58,6 +58,7 @@ export class IKRootsHelper extends Group { for ( let i = 0, l = roots.length; i < l; i ++ ) { const root = roots[ i ]; + root.updateMatrixWorld( true ); root.traverse( c => { if ( c.isJoint ) { From a033d39c8061c4786e2e9306c3439f5cb60f11a1 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Thu, 15 Oct 2020 23:29:26 -0700 Subject: [PATCH 19/20] Fix some buuuugs --- example/goals.js | 16 ++++++++++++---- src/core/utils/solver.js | 5 ++++- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/example/goals.js b/example/goals.js index 372ec23..6f24086 100644 --- a/example/goals.js +++ b/example/goals.js @@ -29,6 +29,7 @@ import { } from '../src/index.js'; const params = { + controls: 'translate', solvePosition: true, solveRotation: false, }; @@ -38,12 +39,12 @@ const params = { const solverOptions = { maxIterations: 3, divergeThreshold: 0.005, - stallThreshold: 1e-7, + stallThreshold: 1e-4, translationErrorClamp: 0.25, rotationErrorClamp: 0.25, - translationConvergeThreshold: 1e-4, - rotationConvergeThreshold: 1e-5, - restPoseFactor: 0.01, + translationConvergeThreshold: 1e-3, + rotationConvergeThreshold: 1e-3, + restPoseFactor: 0.0001, }; let gui, stats; @@ -178,6 +179,11 @@ function init() { gui = new GUI(); gui.width = 350; + gui.add( params, 'controls', [ 'rotate', 'translate' ] ).listen().onChange( v => { + + transformControls.setMode( v ); + + } ); gui.add( params, 'solvePosition' ).onChange( updateGoalDoF ); gui.add( params, 'solveRotation' ).onChange( updateGoalDoF ); @@ -211,9 +217,11 @@ function init() { switch( e.key ) { case 'w': transformControls.setMode( 'translate' ); + params.controls = 'translate'; break; case 'e': transformControls.setMode( 'rotate' ); + params.controls = 'rotate'; break; case 'f': controls.target.set( 0, 0, 0 ); diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index 624ffbb..220b21b 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -37,12 +37,15 @@ export function accumulateClosureError( tempPos[ 2 ] *= dofFlags[ 2 ]; rowCount = translationDoFCount; - if ( rotationDoFCount === 3 ) { + if ( rotationDoFCount === 0 ) { tempQuat[ 0 ] = 0; tempQuat[ 1 ] = 0; tempQuat[ 2 ] = 0; tempQuat[ 3 ] = 0; + + } else { + rowCount += 4; } From 6db0a408c119d2f9a6dc7a9921e6041da0b1efc6 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Fri, 16 Oct 2020 00:02:03 -0700 Subject: [PATCH 20/20] remove unused function, comments --- src/core/ChainSolver.js | 7 +++---- src/core/Joint.js | 10 ---------- src/core/utils/solver.js | 3 +-- 3 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/core/ChainSolver.js b/src/core/ChainSolver.js index 06aecce..898211e 100644 --- a/src/core/ChainSolver.js +++ b/src/core/ChainSolver.js @@ -523,8 +523,6 @@ export class ChainSolver { let rowIndex = 0; // generate the adjusted matrix based on the epsilon for the joint. - // TODO: is it really necessary that this be different for translation vs - // rotation? It just needs to be a small epsilon... let delta = dof < 3 ? translationStep : rotationStep; if ( freeJoint.getDeltaWorldMatrix( dof, delta, tempDeltaWorldMatrix ) ) { @@ -544,8 +542,9 @@ export class ChainSolver { // TODO: If this is a Goal it only add 1 or 2 fields if only two axes are set. Quat is only // needed if 3 eulers are used. - // TODO: these could be cached per target joint - // get the current error within the closure joint + // TODO: these could be cached per target joint get the current error within the closure joint + + // Get the error from child towards the closure target targetJoint.getClosureError( tempPos, tempQuat ); if ( relevantConnectedClosures.has( targetJoint ) ) { diff --git a/src/core/Joint.js b/src/core/Joint.js index cfab54a..2e41eba 100644 --- a/src/core/Joint.js +++ b/src/core/Joint.js @@ -443,16 +443,6 @@ export class Joint extends Frame { } - // TODO: remove this and put it in solver - getDeltaClosureError( dof, delta, outPos, outQuat ) { - - this.getDeltaWorldMatrix( dof, delta, tempMatrix2 ); - - this.child.updateMatrixWorld(); - getMatrixDifference( tempMatrix2, this.child.matrixWorld, outPos, outQuat ); - - } - // Update matrix overrides // TODO: it might be best if we skip this and try to characterize joint error with quats in // the error vector diff --git a/src/core/utils/solver.js b/src/core/utils/solver.js index 220b21b..f5612cc 100644 --- a/src/core/utils/solver.js +++ b/src/core/utils/solver.js @@ -25,8 +25,7 @@ export function accumulateClosureError( dof, } = joint; - // TODO: If this is a Goal and we have less than three euler DoF we should use euler angles (and adjust the rows). Otherwise we - // should use a quat. + // Get the error from child towards the closure target joint.getClosureError( tempPos, tempQuat ); let rowCount = 7;