From 884e00acb70d98f7b0967aff6be4141e52512782 Mon Sep 17 00:00:00 2001 From: Ezra Brooks Date: Fri, 16 Feb 2024 12:40:25 -0700 Subject: [PATCH 1/3] Add planar joint support Fix test that expects 2 dofs for planar which has 3 use isNaN on individual DoFs instead of `== null` on all of them together Go back to checking for null, parsing should be fixed as of #282 Return early if all values are identical to current state _or null_ note to self Avoid allocating new memory for vectors during joint setting Revert added Rz DoF for planar joint type as far as I can tell, the URDF spec does not allow multi-dof limits add first cut at floating joint support formatting re-add Rz component to planar joints update setJointValue type in typescript definition test update fix block scoping cleanup --- javascript/src/URDFClasses.d.ts | 2 +- javascript/src/URDFClasses.js | 67 ++++++++++++++++++++++++++++--- javascript/test/URDFJoint.test.js | 2 +- 3 files changed, 63 insertions(+), 8 deletions(-) diff --git a/javascript/src/URDFClasses.d.ts b/javascript/src/URDFClasses.d.ts index e5f3ba8f..a49d2ac9 100644 --- a/javascript/src/URDFClasses.d.ts +++ b/javascript/src/URDFClasses.d.ts @@ -59,7 +59,7 @@ export interface URDFRobot extends URDFLink { visual: { [ key: string ]: URDFVisual }; frames: { [ key: string ]: Object3D }; - setJointValue(jointName: String, value0: Number, value1?: Number, value2?: Number): boolean; + setJointValue(jointName: String, ...values: number[]): boolean; setJointValues(values: { [ key: string ]: Number | Number[] }): boolean; getFrame(name: String): Object3D; diff --git a/javascript/src/URDFClasses.js b/javascript/src/URDFClasses.js index 5e48cb23..4c1a677c 100644 --- a/javascript/src/URDFClasses.js +++ b/javascript/src/URDFClasses.js @@ -1,6 +1,7 @@ -import { Object3D, Vector3 } from 'three'; +import { Euler, Object3D, Vector3 } from 'three'; const _tempAxis = new Vector3(); +const _tempEuler = new Euler(); class URDFBase extends Object3D { @@ -87,7 +88,9 @@ class URDFJoint extends URDFBase { break; case 'planar': - this.jointValue = new Array(2).fill(0); + // Planar joints are, 3dof: position XY and rotation Z. + this.jointValue = new Array(3).fill(0); + this.axis = new Vector3(0, 0, 1); break; case 'floating': @@ -241,10 +244,62 @@ class URDFJoint extends URDFBase { } - case 'floating': - case 'planar': - // TODO: Support these joint types - console.warn(`'${ this.jointType }' joint not yet supported`); + case 'floating': { + + // no-op if all values are identical to existing value or are null + if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate; + // Floating joints have six degrees of freedom: X, Y, Z, R, P, Y. + this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0]; + this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; + this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; + this.position.copy(this.origPosition); + // Respect origin RPY when setting position + _tempAxis.set(1, 0, 0).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, this.jointValue[0]); + _tempAxis.set(0, 1, 0).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, this.jointValue[1]); + _tempAxis.set(0, 0, 1).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, this.jointValue[2]); + + this.jointValue[3] = values[3] !== null ? values[3] : this.jointValue[3]; + this.jointValue[4] = values[4] !== null ? values[4] : this.jointValue[4]; + this.jointValue[5] = values[5] !== null ? values[5] : this.jointValue[5]; + this.quaternion.setFromEuler( + _tempEuler.set( + this.jointValue[3], + this.jointValue[4], + this.jointValue[5], + 'XYZ', + ), + ).premultiply(this.origQuaternion); + + this.matrixWorldNeedsUpdate = true; + return true; + } + + case 'planar': { + + // no-op if all values are identical to existing value or are null + if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate; + + this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0]; + this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; + this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; + + // Respect existing RPY when modifying the position of the X,Y axes + this.position.copy(this.origPosition); + + _tempAxis.set(1, 0, 0).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, this.jointValue[0]); + _tempAxis.set(0, 1, 0).applyEuler(this.rotation); + this.position.addScaledVector(_tempAxis, this.jointValue[1]); + this.quaternion + .setFromAxisAngle(this.axis, this.jointValue[2]) + .premultiply(this.origQuaternion); + + this.matrixWorldNeedsUpdate = true; + return true; + } } diff --git a/javascript/test/URDFJoint.test.js b/javascript/test/URDFJoint.test.js index ac646962..39937452 100644 --- a/javascript/test/URDFJoint.test.js +++ b/javascript/test/URDFJoint.test.js @@ -30,7 +30,7 @@ describe('URDFJoint', () => { expect(joint.jointValue).toHaveLength(1); joint.jointType = 'planar'; - expect(joint.jointValue).toHaveLength(2); + expect(joint.jointValue).toHaveLength(3); joint.jointType = 'floating'; expect(joint.jointValue).toHaveLength(6); From b56040780f646a9e0efc8711b9785d1b3d4ca4d2 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Apr 2024 12:51:37 -0400 Subject: [PATCH 2/3] Fix joint translations --- javascript/src/URDFClasses.js | 48 ++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/javascript/src/URDFClasses.js b/javascript/src/URDFClasses.js index 4c1a677c..58e4ba54 100644 --- a/javascript/src/URDFClasses.js +++ b/javascript/src/URDFClasses.js @@ -1,7 +1,12 @@ -import { Euler, Object3D, Vector3 } from 'three'; +import { Euler, Object3D, Vector3, Quaternion, Matrix4 } from 'three'; const _tempAxis = new Vector3(); const _tempEuler = new Euler(); +const _tempTransform = new Matrix4(); +const _tempOrigTransform = new Matrix4(); +const _tempQuat = new Quaternion(); +const _tempScale = new Vector3(1.0, 1.0, 1.0); +const _tempPosition = new Vector3(); class URDFBase extends Object3D { @@ -252,26 +257,27 @@ class URDFJoint extends URDFBase { this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0]; this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; - this.position.copy(this.origPosition); - // Respect origin RPY when setting position - _tempAxis.set(1, 0, 0).applyEuler(this.rotation); - this.position.addScaledVector(_tempAxis, this.jointValue[0]); - _tempAxis.set(0, 1, 0).applyEuler(this.rotation); - this.position.addScaledVector(_tempAxis, this.jointValue[1]); - _tempAxis.set(0, 0, 1).applyEuler(this.rotation); - this.position.addScaledVector(_tempAxis, this.jointValue[2]); - this.jointValue[3] = values[3] !== null ? values[3] : this.jointValue[3]; this.jointValue[4] = values[4] !== null ? values[4] : this.jointValue[4]; this.jointValue[5] = values[5] !== null ? values[5] : this.jointValue[5]; - this.quaternion.setFromEuler( + + // Compose transform of joint origin and transform due to joint values + _tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale); + _tempQuat.setFromEuler( _tempEuler.set( this.jointValue[3], this.jointValue[4], this.jointValue[5], 'XYZ', ), - ).premultiply(this.origQuaternion); + ); + _tempPosition.set(this.jointValue[0], this.jointValue[1], this.jointValue[2]); + _tempTransform.compose(_tempPosition, _tempQuat, _tempScale); + + // Calcualte new transform + _tempOrigTransform.premultiply(_tempTransform); + this.position.setFromMatrixPosition(_tempOrigTransform); + this.rotation.setFromRotationMatrix(_tempOrigTransform); this.matrixWorldNeedsUpdate = true; return true; @@ -286,16 +292,16 @@ class URDFJoint extends URDFBase { this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1]; this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2]; - // Respect existing RPY when modifying the position of the X,Y axes - this.position.copy(this.origPosition); + // Compose transform of joint origin and transform due to joint values + _tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale); + _tempQuat.setFromAxisAngle(this.axis, this.jointValue[2]); + _tempPosition.set(this.jointValue[0], this.jointValue[1], 0.0); + _tempTransform.compose(_tempPosition, _tempQuat, _tempScale); - _tempAxis.set(1, 0, 0).applyEuler(this.rotation); - this.position.addScaledVector(_tempAxis, this.jointValue[0]); - _tempAxis.set(0, 1, 0).applyEuler(this.rotation); - this.position.addScaledVector(_tempAxis, this.jointValue[1]); - this.quaternion - .setFromAxisAngle(this.axis, this.jointValue[2]) - .premultiply(this.origQuaternion); + // Calcualte new transform + _tempOrigTransform.premultiply(_tempTransform); + this.position.setFromMatrixPosition(_tempOrigTransform); + this.rotation.setFromRotationMatrix(_tempOrigTransform); this.matrixWorldNeedsUpdate = true; return true; From 2dc28e1a09573689b2fcd8dabcaf59ece3016a45 Mon Sep 17 00:00:00 2001 From: Garrett Johnson Date: Tue, 27 Aug 2024 20:05:53 -0700 Subject: [PATCH 3/3] Spelling --- javascript/src/URDFClasses.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/javascript/src/URDFClasses.js b/javascript/src/URDFClasses.js index 58e4ba54..ad10e286 100644 --- a/javascript/src/URDFClasses.js +++ b/javascript/src/URDFClasses.js @@ -298,7 +298,7 @@ class URDFJoint extends URDFBase { _tempPosition.set(this.jointValue[0], this.jointValue[1], 0.0); _tempTransform.compose(_tempPosition, _tempQuat, _tempScale); - // Calcualte new transform + // Calculate new transform _tempOrigTransform.premultiply(_tempTransform); this.position.setFromMatrixPosition(_tempOrigTransform); this.rotation.setFromRotationMatrix(_tempOrigTransform);