Skip to content

Fix Euler rotation order to use ZYX (intrinsic) instead of default XYZ (intrinsic) #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
petertheprocess wants to merge 1 commit into
base: main
Choose a base branch
from
+14 −12
Open

Fix Euler rotation order to use ZYX (intrinsic) instead of default XYZ (intrinsic) #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions src/adapters/MJCFAdapter.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1588,11 +1588,12 @@ export class MJCFAdapter {
mesh.rotation.set(
fromtoRpy[0] + visual.origin.rpy[0],
fromtoRpy[1] + visual.origin.rpy[1],
fromtoRpy[2] + visual.origin.rpy[2]
fromtoRpy[2] + visual.origin.rpy[2],
'ZYX'
);
} else {
mesh.position.set(...visual.origin.xyz);
mesh.rotation.set(...visual.origin.rpy);
mesh.position.set(...visual.origin.xyz, 'ZYX');
mesh.rotation.set(...visual.origin.rpy, 'ZYX');
}
mesh.name = visual.name || 'visual';

Expand Down Expand Up @@ -1716,11 +1717,12 @@ export class MJCFAdapter {
mesh.rotation.set(
fromtoRpy[0] + collision.origin.rpy[0],
fromtoRpy[1] + collision.origin.rpy[1],
fromtoRpy[2] + collision.origin.rpy[2]
fromtoRpy[2] + collision.origin.rpy[2],
'ZYX'
);
} else {
mesh.position.set(...collision.origin.xyz);
mesh.rotation.set(...collision.origin.rpy);
mesh.rotation.set(...collision.origin.rpy,'ZYX');
}
mesh.name = collision.name || 'collision';

Expand Down Expand Up @@ -1798,7 +1800,7 @@ export class MJCFAdapter {
bodyOrigin.xyz[1] + joint.origin.xyz[1],
bodyOrigin.xyz[2] + joint.origin.xyz[2]
);
jointGroup.rotation.set(...bodyOrigin.rpy);
jointGroup.rotation.set(...bodyOrigin.rpy, 'ZYX');

// Recursively build child link
buildHierarchy(childLinkName, jointGroup);
Expand All @@ -1825,7 +1827,7 @@ export class MJCFAdapter {
// Create fixed connection group
const fixedGroup = new THREE.Group();
fixedGroup.position.set(...childBodyOrigin.xyz);
fixedGroup.rotation.set(...childBodyOrigin.rpy);
fixedGroup.rotation.set(...childBodyOrigin.rpy, 'ZYX');

// Recursively build child body and add to fixed group
buildHierarchy(childName, fixedGroup);
Expand All @@ -1844,7 +1846,7 @@ export class MJCFAdapter {
const rootLinkGroup = linkObjects.get(rootName);
if (rootLink.userData.bodyOrigin) {
rootLinkGroup.position.set(...rootLink.userData.bodyOrigin.xyz);
rootLinkGroup.rotation.set(...rootLink.userData.bodyOrigin.rpy);
rootLinkGroup.rotation.set(...rootLink.userData.bodyOrigin.rpy, 'ZYX');
}
buildHierarchy(rootName, rootGroup);
});
Expand All @@ -1855,7 +1857,7 @@ export class MJCFAdapter {
const firstLinkGroup = linkObjects.get(firstLink);
if (firstLinkObj.userData.bodyOrigin) {
firstLinkGroup.position.set(...firstLinkObj.userData.bodyOrigin.xyz);
firstLinkGroup.rotation.set(...firstLinkObj.userData.bodyOrigin.rpy);
firstLinkGroup.rotation.set(...firstLinkObj.userData.bodyOrigin.rpy, 'ZYX');
}
buildHierarchy(firstLink, rootGroup);
}
Expand Down
2 changes: 1 addition & 1 deletion src/adapters/USDAdapter.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -337,7 +337,7 @@ export class USDAdapter {
const mesh = this.createGeometryMesh(visual.geometry);
if (mesh) {
mesh.position.set(...visual.origin.xyz);
mesh.rotation.set(...visual.origin.rpy);
mesh.rotation.set(...visual.origin.rpy, 'ZYX');
mesh.name = visual.name;
linkGroup.add(mesh);
visual.threeObject = mesh;
Expand Down
4 changes: 2 additions & 2 deletions src/renderer/InertialVisualization.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -317,9 +317,9 @@ export class InertialVisualization {
} else if (inertial.origin && inertial.origin.rpy) {
// For URDF with RPY (though this is usually just for COM position, not inertia orientation)
const rpy = inertial.origin.rpy;
inertiaBox.rotation.set(rpy[0], rpy[1], rpy[2], 'XYZ');
inertiaBox.rotation.set(rpy[0], rpy[1], rpy[2], 'ZYX');
} else {
inertiaBox.rotation.set(0, 0, 0);
inertiaBox.rotation.set(0, 0, 0, 'ZYX');
}

inertiaBox.visible = this.showInertia;
Expand Down