feat(pose_3d): modello reale G1 (URDF + STL meshes) al posto della silhouette finta
Sostituisce torso/head/hips/gambe/spalle primitive in tab Pose 3D con il modello URDF G1 (g1_body29_hand14), caricato via urdf-loader 0.12.6. Vendor: - web/vendor/g1/g1.urdf + 64 mesh STL (~52 MB, copiati da G1_API_Server/assets/g1/) - web/vendor/three/addons/loaders/URDFLoader.js (npm urdf-loader@0.12.6), import path adattato a 'three/addons/loaders/STLLoader.js' (importmap del sito) e ColladaLoader stubbato (G1 URDF è solo STL) - web/vendor/three/addons/loaders/URDFClasses.js (companion del loader) pose_3d.js: - Aggiunge loadG1Urdf(scene) async che configura loader.loadMeshCb per servire le STL dal mount /static/vendor/g1/meshes/ - Best-effort: in caso di errore di parsing/fetch il body non viene aggiunto ma la tab continua a funzionare (polsi, UCS, gizmo)
This commit is contained in:
Adriano Dal Pastro committed
1 parent
18dce0d951
commit
8c09d79505
68 files changed
+2651
-37
No files matched your search
+469
@@ -0,0 +1,469 @@
|
||||
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 {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.urdfNode = null;
|
||||
this.urdfName = '';
|
||||
|
||||
}
|
||||
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.urdfNode = source.urdfNode;
|
||||
this.urdfName = source.urdfName;
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFCollider extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFCollider = true;
|
||||
this.type = 'URDFCollider';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFVisual extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFVisual = true;
|
||||
this.type = 'URDFVisual';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFLink extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFLink = true;
|
||||
this.type = 'URDFLink';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFJoint extends URDFBase {
|
||||
|
||||
get jointType() {
|
||||
|
||||
return this._jointType;
|
||||
|
||||
}
|
||||
|
||||
set jointType(v) {
|
||||
|
||||
if (this.jointType === v) return;
|
||||
this._jointType = v;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
switch (v) {
|
||||
|
||||
case 'fixed':
|
||||
this.jointValue = [];
|
||||
break;
|
||||
|
||||
case 'continuous':
|
||||
case 'revolute':
|
||||
case 'prismatic':
|
||||
this.jointValue = new Array(1).fill(0);
|
||||
break;
|
||||
|
||||
case 'planar':
|
||||
// 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':
|
||||
this.jointValue = new Array(6).fill(0);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
get angle() {
|
||||
|
||||
return this.jointValue[0];
|
||||
|
||||
}
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
|
||||
this.isURDFJoint = true;
|
||||
this.type = 'URDFJoint';
|
||||
|
||||
this.jointValue = null;
|
||||
this.jointType = 'fixed';
|
||||
this.axis = new Vector3(1, 0, 0);
|
||||
this.limit = { lower: 0, upper: 0 };
|
||||
this.ignoreLimits = false;
|
||||
|
||||
this.origPosition = null;
|
||||
this.origQuaternion = null;
|
||||
|
||||
this.mimicJoints = [];
|
||||
|
||||
}
|
||||
|
||||
/* Overrides */
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.jointType = source.jointType;
|
||||
this.axis = source.axis.clone();
|
||||
this.limit.lower = source.limit.lower;
|
||||
this.limit.upper = source.limit.upper;
|
||||
this.ignoreLimits = false;
|
||||
|
||||
this.jointValue = [...source.jointValue];
|
||||
|
||||
this.origPosition = source.origPosition ? source.origPosition.clone() : null;
|
||||
this.origQuaternion = source.origQuaternion ? source.origQuaternion.clone() : null;
|
||||
|
||||
this.mimicJoints = [...source.mimicJoints];
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
/* Public Functions */
|
||||
/**
|
||||
* @param {...number|null} values The joint value components to set, optionally null for no-op
|
||||
* @returns {boolean} Whether the invocation of this function resulted in an actual change to the joint value
|
||||
*/
|
||||
setJointValue(...values) {
|
||||
|
||||
// Parse all incoming values into numbers except null, which we treat as a no-op for that value component.
|
||||
values = values.map(v => v === null ? null : parseFloat(v));
|
||||
|
||||
if (!this.origPosition || !this.origQuaternion) {
|
||||
|
||||
this.origPosition = this.position.clone();
|
||||
this.origQuaternion = this.quaternion.clone();
|
||||
|
||||
}
|
||||
|
||||
let didUpdate = false;
|
||||
|
||||
this.mimicJoints.forEach(joint => {
|
||||
|
||||
didUpdate = joint.updateFromMimickedJoint(...values) || didUpdate;
|
||||
|
||||
});
|
||||
|
||||
switch (this.jointType) {
|
||||
|
||||
case 'fixed': {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
case 'continuous':
|
||||
case 'revolute': {
|
||||
|
||||
let angle = values[0];
|
||||
if (angle == null) return didUpdate;
|
||||
if (angle === this.jointValue[0]) return didUpdate;
|
||||
|
||||
if (!this.ignoreLimits && this.jointType === 'revolute') {
|
||||
|
||||
angle = Math.min(this.limit.upper, angle);
|
||||
angle = Math.max(this.limit.lower, angle);
|
||||
|
||||
}
|
||||
|
||||
this.quaternion
|
||||
.setFromAxisAngle(this.axis, angle)
|
||||
.premultiply(this.origQuaternion);
|
||||
|
||||
if (this.jointValue[0] !== angle) {
|
||||
|
||||
this.jointValue[0] = angle;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
case 'prismatic': {
|
||||
|
||||
let pos = values[0];
|
||||
if (pos == null) return didUpdate;
|
||||
if (pos === this.jointValue[0]) return didUpdate;
|
||||
|
||||
if (!this.ignoreLimits) {
|
||||
|
||||
pos = Math.min(this.limit.upper, pos);
|
||||
pos = Math.max(this.limit.lower, pos);
|
||||
|
||||
}
|
||||
|
||||
this.position.copy(this.origPosition);
|
||||
_tempAxis.copy(this.axis).applyEuler(this.rotation);
|
||||
this.position.addScaledVector(_tempAxis, pos);
|
||||
|
||||
if (this.jointValue[0] !== pos) {
|
||||
|
||||
this.jointValue[0] = pos;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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.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];
|
||||
|
||||
// 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',
|
||||
),
|
||||
);
|
||||
_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;
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
// 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);
|
||||
|
||||
// Calculate new transform
|
||||
_tempOrigTransform.premultiply(_tempTransform);
|
||||
this.position.setFromMatrixPosition(_tempOrigTransform);
|
||||
this.rotation.setFromRotationMatrix(_tempOrigTransform);
|
||||
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFMimicJoint extends URDFJoint {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.type = 'URDFMimicJoint';
|
||||
this.mimicJoint = null;
|
||||
this.offset = 0;
|
||||
this.multiplier = 1;
|
||||
|
||||
}
|
||||
|
||||
updateFromMimickedJoint(...values) {
|
||||
|
||||
const modifiedValues = values.map(x => x * this.multiplier + this.offset);
|
||||
return super.setJointValue(...modifiedValues);
|
||||
|
||||
}
|
||||
|
||||
/* Overrides */
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.mimicJoint = source.mimicJoint;
|
||||
this.offset = source.offset;
|
||||
this.multiplier = source.multiplier;
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFRobot extends URDFLink {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFRobot = true;
|
||||
this.urdfNode = null;
|
||||
|
||||
this.urdfRobotNode = null;
|
||||
this.robotName = null;
|
||||
|
||||
this.links = null;
|
||||
this.joints = null;
|
||||
this.colliders = null;
|
||||
this.visual = null;
|
||||
this.frames = null;
|
||||
|
||||
}
|
||||
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.urdfRobotNode = source.urdfRobotNode;
|
||||
this.robotName = source.robotName;
|
||||
|
||||
this.links = {};
|
||||
this.joints = {};
|
||||
this.colliders = {};
|
||||
this.visual = {};
|
||||
|
||||
this.traverse(c => {
|
||||
|
||||
if (c.isURDFJoint && c.urdfName in source.joints) {
|
||||
|
||||
this.joints[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFLink && c.urdfName in source.links) {
|
||||
|
||||
this.links[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFCollider && c.urdfName in source.colliders) {
|
||||
|
||||
this.colliders[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFVisual && c.urdfName in source.visual) {
|
||||
|
||||
this.visual[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
// Repair mimic joint references once we've re-accumulated all our joint data
|
||||
for (const joint in this.joints) {
|
||||
this.joints[joint].mimicJoints = this.joints[joint].mimicJoints.map((mimicJoint) => this.joints[mimicJoint.name]);
|
||||
}
|
||||
|
||||
this.frames = {
|
||||
...this.colliders,
|
||||
...this.visual,
|
||||
...this.links,
|
||||
...this.joints,
|
||||
};
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
getFrame(name) {
|
||||
|
||||
return this.frames[name];
|
||||
|
||||
}
|
||||
|
||||
setJointValue(jointName, ...angle) {
|
||||
|
||||
const joint = this.joints[jointName];
|
||||
if (joint) {
|
||||
|
||||
return joint.setJointValue(...angle);
|
||||
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
setJointValues(values) {
|
||||
|
||||
let didChange = false;
|
||||
for (const name in values) {
|
||||
|
||||
const value = values[name];
|
||||
if (Array.isArray(value)) {
|
||||
|
||||
didChange = this.setJointValue(name, ...value) || didChange;
|
||||
|
||||
} else {
|
||||
|
||||
didChange = this.setJointValue(name, value) || didChange;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return didChange;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
export { URDFRobot, URDFLink, URDFJoint, URDFMimicJoint, URDFVisual, URDFCollider };
|
||||
Reference in new issue
Block a user