diff --git a/resources/web/wwi/FloatingProtoParameterWindow.js b/resources/web/wwi/FloatingProtoParameterWindow.js index c04104f4569..a6e83796142 100644 --- a/resources/web/wwi/FloatingProtoParameterWindow.js +++ b/resources/web/wwi/FloatingProtoParameterWindow.js @@ -1,5 +1,6 @@ import FloatingWindow from './FloatingWindow.js'; import {VRML} from './protoVisualizer/vrml_type.js'; +import WbBallJoint from './nodes/WbBallJoint.js'; import WbCamera from './nodes/WbCamera.js'; import WbConnector from './nodes/WbConnector.js'; import WbDistanceSensor from './nodes/WbDistanceSensor.js'; @@ -9,7 +10,7 @@ import WbLidar from './nodes/WbLidar.js'; import WbLightSensor from './nodes/WbLightSensor.js'; import WbPen from './nodes/WbPen.js'; import WbRadar from './nodes/WbRadar.js'; -import WbSliderJoint from './nodes/WbSliderJoint.js'; +import WbJoint from './nodes/WbJoint.js'; import WbWorld from './nodes/WbWorld.js'; import WbRangeFinder from './nodes/WbRangeFinder.js'; import WbVector3 from './nodes/utils/WbVector3.js'; @@ -838,8 +839,7 @@ export default class FloatingProtoParameterWindow extends FloatingWindow { let numberOfJoint = 0; for (const key of keys) { const joint = nodes.get(key); - // No need to test for WbHinge2Joint as they are descendant of WbHingeJoint - if (joint instanceof WbHingeJoint || joint instanceof WbSliderJoint) { + if (joint instanceof WbJoint) { numberOfJoint++; let div = document.createElement('div'); @@ -848,7 +848,9 @@ export default class FloatingProtoParameterWindow extends FloatingWindow { let nameDiv = document.createElement('div'); const jointName = joint.endPoint ? joint.endPoint.name : numberOfJoint; - if (joint instanceof WbHinge2Joint) + if (joint instanceof WbBallJoint) + nameDiv.innerHTML = 'BallJoint 1: ' + jointName; + else if (joint instanceof WbHinge2Joint) nameDiv.innerHTML = 'Hinge2joint 1: ' + jointName; else if (joint instanceof WbHingeJoint) nameDiv.innerHTML = 'Hingejoint: ' + jointName; @@ -859,15 +861,47 @@ export default class FloatingProtoParameterWindow extends FloatingWindow { div.appendChild(nameDiv); const parameters = joint.jointParameters; div.appendChild(this.#createSlider(parameters, _ => { - if (parameters) { + if (parameters) parameters.position = _.target.value; - this.#view.x3dScene.render(); - } + else + joint.position = _.target.value; + this.#view.x3dScene.render(); })); this.joints.appendChild(div); - if (joint instanceof WbHinge2Joint) { + if (joint instanceof WbBallJoint) { + div = document.createElement('div'); + div.className = 'proto-joint'; + + nameDiv = document.createElement('div'); + nameDiv.innerHTML = 'BallJoint 2: ' + jointName; + nameDiv.className = 'proto-joint-name'; + div.appendChild(nameDiv); + const parameters2 = joint.jointParameters2; + div.appendChild(this.#createSlider(parameters2, _ => { + if (parameters2) + parameters2.position = _.target.value; + else + joint.position2 = _.target.value; + this.#view.x3dScene.render(); + })); + this.joints.appendChild(div); + + nameDiv = document.createElement('div'); + nameDiv.innerHTML = 'BallJoint 3: ' + jointName; + nameDiv.className = 'proto-joint-name'; + div.appendChild(nameDiv); + const parameters3 = joint.jointParameters3; + div.appendChild(this.#createSlider(parameters3, _ => { + if (parameters3) + parameters3.position = _.target.value; + else + joint.position3 = _.target.value; + this.#view.x3dScene.render(); + })); + this.joints.appendChild(div); + } else if (joint instanceof WbHinge2Joint) { div = document.createElement('div'); div.className = 'proto-joint'; @@ -877,10 +911,11 @@ export default class FloatingProtoParameterWindow extends FloatingWindow { div.appendChild(nameDiv); const parameters2 = joint.jointParameters2; div.appendChild(this.#createSlider(parameters2, _ => { - if (parameters2) { + if (parameters2) parameters2.position = _.target.value; - this.#view.x3dScene.render(); - } + else + joint.position2 = _.target.value; + this.#view.x3dScene.render(); })); this.joints.appendChild(div); } diff --git a/resources/web/wwi/nodes/WbBallJoint.js b/resources/web/wwi/nodes/WbBallJoint.js index 0a10b0eda32..c62b713056b 100644 --- a/resources/web/wwi/nodes/WbBallJoint.js +++ b/resources/web/wwi/nodes/WbBallJoint.js @@ -1,8 +1,13 @@ import WbHinge2Joint from './WbHinge2Joint.js'; +import WbQuaternion from './utils/WbQuaternion.js'; +import WbVector3 from './utils/WbVector3.js'; +import WbVector4 from './utils/WbVector4.js'; +import {isZeroAngle} from './utils/math_utilities.js'; export default class WbBallJoint extends WbHinge2Joint { #device3; #jointParameters3; + #position3; constructor(id) { super(id); this.#device3 = []; @@ -24,7 +29,22 @@ export default class WbBallJoint extends WbHinge2Joint { this.#jointParameters3 = jointParameters; } + get position3() { + return this.#position3; + } + + set position3(newPosition) { + if (this.#position3 === newPosition) + return; + + this.#position3 = newPosition; + if (typeof this.jointParameters3 === 'undefined') + this._updatePosition(); + } + preFinalize() { + this.#position3 = typeof this.jointParameters3 === 'undefined' ? 0 : this.jointParameters3.position; + super.preFinalize(); this.#device3.forEach(child => child.preFinalize()); this.#jointParameters3?.preFinalize(); @@ -48,4 +68,117 @@ export default class WbBallJoint extends WbHinge2Joint { super.delete(); } + + axis() { + const p2 = this.jointParameters2; + const p3 = this.jointParameters3; + if (typeof p2 === 'undefined') { + if (typeof p3 === 'undefined') + return new WbVector3(1, 0, 0); + else if (p3.axis().cross(new WbVector3(0, 0, 1)).isNull()) + return p3.axis().cross(new WbVector3(1, 0, 0)); + else + return p3.axis().cross(new WbVector3(0, 0, 1)); + } + return p2.axis(); + } + + axis2() { + return this.axis3().cross(this.axis()); + } + + axis3() { + const p2 = this.jointParameters2; + const p3 = this.jointParameters3; + if (typeof p3 === 'undefined') { + if (typeof p2 === 'undefined') + return new WbVector3(0, 0, 1); + else if (p2.axis.cross(new WbVector3(1, 0, 0)).isNull()) + return p2.axis.cross(new WbVector3(0, 0, 1)); + else + return p2.axis.cross(new WbVector3(1, 0, 0)); + } + return p3.axis; + } + + _updatePosition() { + if (typeof this.endPoint !== 'undefined') { + const position = typeof this.jointParameters !== 'undefined' ? this.jointParameters.position : this.position; + const position2 = typeof this.jointParameters2 !== 'undefined' ? this.jointParameters2.position : this.position2; + const position3 = typeof this.jointParameters3 !== 'undefined' ? this.jointParameters3.position : this.#position3; + + this.#updatePositions(position, position2, position3); + } + } + + _updateEndPointZeroTranslationAndRotation() { + if (typeof this.endPoint === 'undefined') + return; + + const ir = this.endPoint.rotation; + const it = this.endPoint.translation; + + let qp = new WbQuaternion(); + if (isZeroAngle(this.position) && isZeroAngle(this.position2) && isZeroAngle(this.#position3)) + this._endPointZeroRotation = ir; + else { + const axis = this.axis(); + const q = new WbQuaternion(); + q.fromAxisAngle(axis.x, axis.y, axis.z, -this.position); + + const axis2 = this.axis2(); + const q2 = new WbQuaternion(); + q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, -this.position2); + + const axis3 = this.axis3(); + const q3 = new WbQuaternion(); + q3.fromAxisAngle(axis3.x, axis3.y, axis3.z, -this.#position3); + + qp = q3.mul(q2).mul(q); + const iq = ir.toQuaternion(); + const qr = qp * iq; + qr.normalize(); + this._endPointZeroRotation = new WbVector4(); + this._endPointZeroRotation.fromQuaternion(qr); + } + const a = this.anchor(); + const t = it.sub(a); + this._endPointZeroTranslation = qp.mulByVec3(t).add(a); + } + + #updatePositions(position, position2, position3) { + this.position = position; + this.position2 = position2; + this.#position3 = position3; + let rotation = new WbVector4(); + const translation = this.#computeEndPointSolidPositionFromParameters(rotation); + if (!translation.almostEquals(this.endPoint.translation) || !rotation.almostEquals(this.endPoint.rotation)) { + this.endPoint.translation = translation; + this.endPoint.rotation = rotation; + } + } + + #computeEndPointSolidPositionFromParameters(rotation) { + const q = new WbQuaternion(); + const axis = this.axis(); + q.fromAxisAngle(axis.x, axis.y, axis.z, this.position); + + const q2 = new WbQuaternion(); + const axis2 = this.axis2(); + q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, this.position2); + + const q3 = new WbQuaternion(); + const axis3 = this.axis3(); + q3.fromAxisAngle(axis3.x, axis3.y, axis3.z, this.#position3); + + const qi = this._endPointZeroRotation.toQuaternion(); + let qp = q.mul(q2).mul(q3); + const a = this.anchor(); + const t = this._endPointZeroTranslation.sub(a); + const translation = qp.mulByVec3(t).add(a); + qp = qp.mul(qi); + qp.normalize(); + rotation.fromQuaternion(qp); + return translation; + } } diff --git a/resources/web/wwi/nodes/WbHinge2Joint.js b/resources/web/wwi/nodes/WbHinge2Joint.js index 01693e672d8..8778a581e8f 100644 --- a/resources/web/wwi/nodes/WbHinge2Joint.js +++ b/resources/web/wwi/nodes/WbHinge2Joint.js @@ -7,6 +7,7 @@ import {isZeroAngle} from './utils/math_utilities.js'; export default class WbHinge2Joint extends WbHingeJoint { #device2; #jointParameters2; + #position2; constructor(id) { super(id); this.#device2 = []; @@ -31,8 +32,21 @@ export default class WbHinge2Joint extends WbHingeJoint { this.#jointParameters2.onChange = () => this._updatePosition(); } + get position2() { + return this.#position2; + } + + set position2(newPosition) { + if (this.#position2 === newPosition) + return; + + this.#position2 = newPosition; + if (typeof this.jointParameters2 === 'undefined') + this._updatePosition(); + } + preFinalize() { - this.position2 = typeof this.jointParameters2 === 'undefined' ? 0 : this.jointParameters2.position; + this.#position2 = typeof this.jointParameters2 === 'undefined' ? 0 : this.jointParameters2.position; super.preFinalize(); this.#device2.forEach(child => child.preFinalize()); @@ -61,7 +75,7 @@ export default class WbHinge2Joint extends WbHingeJoint { _updatePosition() { if (typeof this.endPoint !== 'undefined') { const position = typeof this.jointParameters !== 'undefined' ? this.jointParameters.position : this.position; - const position2 = typeof this.jointParameters2 !== 'undefined' ? this.jointParameters2.position : this.position2; + const position2 = typeof this.jointParameters2 !== 'undefined' ? this.jointParameters2.position : this.#position2; this.#updatePositions(position, position2); } } @@ -74,7 +88,7 @@ export default class WbHinge2Joint extends WbHingeJoint { const it = this.endPoint.translation; let qp = new WbQuaternion(); - if (isZeroAngle(this.position) && isZeroAngle(this.position2)) + if (isZeroAngle(this.position) && isZeroAngle(this.#position2)) // Keeps track of the original axis if the angle is zero as it defines the second DoF axis this._endPointZeroRotation = ir; else { @@ -84,7 +98,7 @@ export default class WbHinge2Joint extends WbHingeJoint { const axis2 = this.axis2(); const q2 = new WbQuaternion(); - q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, -this.position2); + q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, -this.#position2); qp = q2.mul(q); const iq = ir.toQuaternion(); @@ -101,7 +115,7 @@ export default class WbHinge2Joint extends WbHingeJoint { #updatePositions(position, position2) { this.position = position; - this.position2 = position2; + this.#position2 = position2; let rotation = new WbVector4(); const translation = this.#computeEndPointSolidPositionFromParameters(rotation); @@ -118,15 +132,17 @@ export default class WbHinge2Joint extends WbHingeJoint { const q2 = new WbQuaternion(); const axis2 = this.axis2(); - q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, this.position2); + q2.fromAxisAngle(axis2.x, axis2.y, axis2.z, this.#position2); + const qi = this._endPointZeroRotation.toQuaternion(); let qp = q.mul(q2); const a = this.anchor(); const t = this._endPointZeroTranslation.sub(a); + const translation = qp.mulByVec3(t).add(a); qp = qp.mul(qi); qp.normalize(); rotation.fromQuaternion(qp); - return qp.mulByVec3(t).add(a); + return translation; } axis2() { diff --git a/resources/web/wwi/nodes/WbHingeJoint.js b/resources/web/wwi/nodes/WbHingeJoint.js index 427378948ea..0f3933d617c 100644 --- a/resources/web/wwi/nodes/WbHingeJoint.js +++ b/resources/web/wwi/nodes/WbHingeJoint.js @@ -6,6 +6,7 @@ import {isZeroAngle} from './utils/math_utilities.js'; export default class WbHingeJoint extends WbJoint { #device; + #position; constructor(id) { super(id); this.#device = []; @@ -19,6 +20,19 @@ export default class WbHingeJoint extends WbJoint { this.#device = device; } + get position() { + return this.#position; + } + + set position(newPosition) { + if (this.#position === newPosition) + return; + + this.#position = newPosition; + if (typeof this.jointParameters === 'undefined') + this._updatePosition(); + } + preFinalize() { super.preFinalize(); this.#device.forEach(child => child.preFinalize()); @@ -42,12 +56,12 @@ export default class WbHingeJoint extends WbJoint { _updatePosition() { if (typeof this.endPoint !== 'undefined') - this.#updatePosition(typeof this.jointParameters !== 'undefined' ? this.jointParameters.position : this.position); + this.#updatePosition(typeof this.jointParameters !== 'undefined' ? this.jointParameters.position : this.#position); } #updatePosition(position) { // called after an artificial move - this.position = position; + this.#position = position; let rotation = new WbVector4(); const translation = this.#computeEndPointSolidPositionFromParameters(rotation); if (!translation.almostEquals(this.endPoint.translation) || !rotation.almostEquals(this.endPoint.rotation)) { @@ -59,7 +73,7 @@ export default class WbHingeJoint extends WbJoint { #computeEndPointSolidPositionFromParameters(rotation) { const axis = this.axis().normalized(); const q = new WbQuaternion(); - q.fromAxisAngle(axis.x, axis.y, axis.z, this.position); + q.fromAxisAngle(axis.x, axis.y, axis.z, this.#position); const iq = this._endPointZeroRotation.toQuaternion(); const qp = q.mul(iq); if (qp.w !== 1) @@ -80,7 +94,7 @@ export default class WbHingeJoint extends WbJoint { const it = this.endPoint.translation; let qMinus; - const angle = this.position; + const angle = this.#position; if (isZeroAngle(angle)) { // In case of a zero angle, the quaternion axis is undefined, so we keep track of the original one this._endPointZeroRotation = ir; diff --git a/resources/web/wwi/test.html b/resources/web/wwi/test.html index 4c6d87a69e3..1aedda6bf20 100644 --- a/resources/web/wwi/test.html +++ b/resources/web/wwi/test.html @@ -55,7 +55,7 @@ //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/lego/mindstorms/protos/MindstormsRover.proto"); // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/mir/mir100/protos/Mir100.proto"); // missing lidars? //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/clearpath/moose/protos/Moose.proto"); - webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/softbank/nao/protos/Nao.proto"); // missing stuff + // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/softbank/nao/protos/Nao.proto"); // missing stuff //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/niryo/ned/protos/Ned.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/franka_emika/panda/protos/Panda.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/fp_robotics/p-rob3/protos/P-Rob3.proto"); @@ -81,7 +81,7 @@ //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/pal_robotics/tiago_steel/protos/TiagoSteel.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/pal_robotics/tiago_titanium/protos/TiagoTitanium.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/pal_robotics/tiago++/protos/Tiago++.proto"); - // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/mobsya/thymio/protos/Thymio2.proto"); // wrong textures? + webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/mobsya/thymio/protos/Thymio2Ball.proto"); // wrong textures? //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/kinematics/tinkerbots/protos/TinkerbotsFinger.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/kinematics/tinkerbots/protos/TinkerbotsDistanceSensor.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/kinematics/tinkerbots/protos/TinkerbotsWheel.proto");