Skip to content

Commit 47fa2e6

Browse files
micahpearlmanmicahpearlman
committed
URDF inertial export
1 parent 32c35e5 commit 47fa2e6

File tree

2 files changed

+115
-82
lines changed

2 files changed

+115
-82
lines changed

‎Runtime/Scripts/Util/ImportExport/ZOExportURDF.cs‎

Lines changed: 114 additions & 81 deletions
Original file line numberDiff line numberDiff line change
@@ -126,12 +126,12 @@ public XDocument BuildURDF(ZOSimDocumentRoot documentRoot) {
126126
if (joints[0] == joint) { // if base joint do not apply any offset to parent link
127127
offset = Vector3.zero;
128128
}
129-
BuildURDFLink(joint.Parent, robot, offset);
129+
BuildLink(joint.Parent, robot, offset);
130130
links.Add(joint.Parent);
131131
}
132132
if (links.Contains(joint.Child) == false) { // build child link if not exist
133133
Vector3 offset = -1.0f * joint.ConnectedAnchor;
134-
BuildURDFLink(joint.Child, robot, offset);
134+
BuildLink(joint.Child, robot, offset);
135135
links.Add(joint.Child);
136136
}
137137
}
@@ -142,7 +142,7 @@ public XDocument BuildURDF(ZOSimDocumentRoot documentRoot) {
142142
link.SetAttributeValue("name", "World");
143143
robot.Add(link);
144144

145-
BuildURDFLink(simOccurrence, robot, Vector3.zero);
145+
BuildLink(simOccurrence, robot, Vector3.zero);
146146

147147
XElement jointX = new XElement("joint");
148148
jointX.SetAttributeValue("name", $"World_to_{simOccurrence.Name}");
@@ -255,12 +255,15 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, ZOSimOccu
255255
/// <summary>
256256
/// URDF has a "flattened" hierarchy where the hierarchy is build by URDF joints.
257257
/// </summary>
258-
protected void BuildURDFLink(ZOSimOccurrence simOccurrence, XElement robot, Vector3 anchorOffset) {
258+
protected void BuildLink(ZOSimOccurrence simOccurrence, XElement robot, Vector3 anchorOffset) {
259259

260260
XElement link = new XElement("link");
261261
link.SetAttributeValue("name", simOccurrence.Name);
262262
robot.Add(link);
263263

264+
// build inertial
265+
BuildLinkInertial(simOccurrence, link);
266+
264267

265268
// build the visual elements
266269
// go through the children
@@ -272,25 +275,25 @@ protected void BuildURDFLink(ZOSimOccurrence simOccurrence, XElement robot, Vect
272275
// go through the children of the visuals and get all the models
273276
foreach (Transform visualsChild in child) {
274277
// check if it is a primitive type (cube, sphere, cylinder, etc)
275-
BuildURDFVisuals(visualsChild, link, anchorOffset);
278+
BuildLinkVisuals(visualsChild, link, anchorOffset);
276279

277280
// we will do any collider that are attached to the visual
278-
BuildURDFCollisions(visualsChild, link, anchorOffset);
281+
BuildLinkCollisions(visualsChild, link, anchorOffset);
279282
}
280283
}
281284

285+
// build collisions
282286
if (child.name.ToLower() == "collisions") {
283287
// go through the children of the collisions and get all the models
284288
foreach (Transform collisionChild in child) {
285-
BuildURDFCollisions(collisionChild, link, anchorOffset);
289+
BuildLinkCollisions(collisionChild, link, anchorOffset);
286290
}
287291
}
288-
289292
}
290293
}
291294

292295

293-
protected void BuildURDFCollisions(Transform collisionTransform, XElement link, Vector3 anchorOffset) {
296+
protected void BuildLinkCollisions(Transform collisionTransform, XElement link, Vector3 anchorOffset) {
294297
Collider[] colliders = collisionTransform.GetComponentsInChildren<Collider>();
295298
foreach (Collider collider in colliders) {
296299
XElement collision = new XElement("collision");
@@ -371,91 +374,121 @@ protected void BuildURDFCollisions(Transform collisionTransform, XElement link,
371374
}
372375
}
373376

374-
protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
377+
protected void BuildLinkVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
375378
// build 3d primitive if exists
376-
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();
377-
if (meshFilter == null) {
378-
// Check children
379-
meshFilter = visualTransform.GetComponentInChildren<MeshFilter>();
380-
}
381-
if (meshFilter) {
382-
383-
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
384-
Collider collider = null;
385-
XElement visual = new XElement("visual");
386-
visual.SetAttributeValue("name", visualTransform.name);
387-
XElement geometry = new XElement("geometry");
388-
389-
if (meshFilter.sharedMesh.name.Contains("Cube")) {
390-
XElement box = new XElement("box");
391-
392-
Vector3 boxSize = visualTransform.localScale.Unity2RosScale();
393-
box.SetAttributeValue("size", boxSize.ToXMLString());
394-
geometry.Add(box);
395-
396-
collider = visualTransform.GetComponent<BoxCollider>();
397-
if (collider) {
398-
//TODO: add box collider
379+
// Check children
380+
MeshFilter[] meshFilters = visualTransform.GetComponentsInChildren<MeshFilter>();
381+
foreach (MeshFilter meshFilter in meshFilters) {
382+
if (meshFilter) {
383+
384+
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
385+
Collider collider = null;
386+
XElement visual = new XElement("visual");
387+
visual.SetAttributeValue("name", visualTransform.name);
388+
XElement geometry = new XElement("geometry");
389+
390+
if (meshFilter.sharedMesh.name.Contains("Cube")) {
391+
XElement box = new XElement("box");
392+
393+
Vector3 boxSize = visualTransform.localScale.Unity2RosScale();
394+
box.SetAttributeValue("size", boxSize.ToXMLString());
395+
geometry.Add(box);
396+
397+
} else if (meshFilter.sharedMesh.name.Contains("Sphere")) {
398+
XElement sphere = new XElement("sphere");
399+
float radius = visualTransform.localScale.x / 2.0f;
400+
sphere.SetAttributeValue("radius", radius);
401+
geometry.Add(sphere);
402+
403+
} else if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
404+
XElement cylinder = new XElement("cylinder");
405+
float radius = visualTransform.localScale.x / 2.0f;
406+
float height = visualTransform.localScale.y * 2.0f;
407+
cylinder.SetAttributeValue("radius", radius);
408+
cylinder.SetAttributeValue("length", height);
409+
410+
geometry.Add(cylinder);
411+
412+
413+
} else { // regular mesh so export meshes as OBJ
414+
XElement mesh = new XElement("mesh");
415+
mesh.SetAttributeValue("filename", $"{visualTransform.name}.obj");
416+
Vector3 scale = visualTransform.localScale;
417+
mesh.SetAttributeValue("scale", scale.ToXMLString());
418+
geometry.Add(mesh);
419+
420+
_visualMeshesToExport.Add(visualTransform);
399421
}
400-
} else if (meshFilter.sharedMesh.name.Contains("Sphere")) {
401-
XElement sphere = new XElement("sphere");
402-
float radius = visualTransform.localScale.x / 2.0f;
403-
sphere.SetAttributeValue("radius", radius);
404-
geometry.Add(sphere);
405422

406-
collider = visualTransform.GetComponent<SphereCollider>();
407-
if (collider) {
408-
//TODO: add box collider
409-
}
423+
if (geometry.HasElements) {
424+
// build origin
425+
Vector3 position = visualTransform.localPosition + anchorOffset;
426+
Vector3 xyz = position.Unity2Ros();
427+
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
428+
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
429+
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
410430

411-
} else if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
412-
XElement cylinder = new XElement("cylinder");
413-
float radius = visualTransform.localScale.x / 2.0f;
414-
float height = visualTransform.localScale.y * 2.0f;
415-
cylinder.SetAttributeValue("radius", radius);
416-
cylinder.SetAttributeValue("length", height);
431+
XElement origin = new XElement("origin");
432+
origin.SetAttributeValue("xyz", xyz.ToXMLString());
433+
origin.SetAttributeValue("rpy", rpy.ToXMLString());
417434

418-
geometry.Add(cylinder);
435+
visual.Add(origin);
419436

420-
collider = visualTransform.GetComponent<MeshCollider>();
421-
if (collider) {
422-
//TODO: add box collider
437+
visual.Add(geometry);
438+
link.Add(visual);
423439
}
424440

425-
} else { // regular mesh so export meshes as OBJ
426-
XElement mesh = new XElement("mesh");
427-
mesh.SetAttributeValue("filename", $"{visualTransform.name}.obj");
428-
Vector3 scale = visualTransform.localScale;
429-
mesh.SetAttributeValue("scale", scale.ToXMLString());
430-
geometry.Add(mesh);
431-
432-
_visualMeshesToExport.Add(visualTransform);
433-
}
434-
435-
if (geometry.HasElements) {
436-
// build origin
437-
Vector3 position = visualTransform.localPosition + anchorOffset;
438-
Vector3 xyz = position.Unity2Ros();
439-
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
440-
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
441-
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
442-
443-
XElement origin = new XElement("origin");
444-
origin.SetAttributeValue("xyz", xyz.ToXMLString());
445-
origin.SetAttributeValue("rpy", rpy.ToXMLString());
446-
447-
visual.Add(origin);
448-
449-
visual.Add(geometry);
450-
link.Add(visual);
451441
}
452442

453443
}
454-
455-
456444
}
457445

458-
446+
public void BuildLinkInertial(ZOSimOccurrence simOccurrence, XElement link) {
447+
Rigidbody rigidbody = simOccurrence.GetComponent<Rigidbody>();
448+
if (rigidbody != null) {
449+
XElement inertial = new XElement("inertial");
450+
XElement mass = new XElement("mass");
451+
mass.SetAttributeValue("value", rigidbody.mass);
452+
453+
// calculate the inertia components
454+
// Unity/PhysX is kinda weird in that it is represented by a diagonal vector and and rotation quaternion.
455+
Matrix4x4 lambdaMatrix = new Matrix4x4();
456+
lambdaMatrix[0,0] = rigidbody.inertiaTensor.x;
457+
lambdaMatrix[1,1] = rigidbody.inertiaTensor.y;
458+
lambdaMatrix[2,2] = rigidbody.inertiaTensor.z;
459+
lambdaMatrix[3,3] = 1.0f;
460+
461+
Matrix4x4 qMatrix = Matrix4x4.Rotate(rigidbody.inertiaTensorRotation);
462+
Matrix4x4 qMatrixTransposed = qMatrix.transpose;
463+
Matrix4x4 inertiaMatrix = qMatrix * lambdaMatrix * qMatrixTransposed;
464+
465+
float ixx = inertiaMatrix[2,2];
466+
float ixy = -inertiaMatrix[0,2];
467+
float ixz = inertiaMatrix[1,2];
468+
float iyy = inertiaMatrix[0,0];
469+
float iyz = -inertiaMatrix[0,1];
470+
float izz = inertiaMatrix[1,1];
471+
472+
XElement inertia = new XElement("inertia");
473+
inertia.SetAttributeValue("ixx", ixx);
474+
inertia.SetAttributeValue("ixy", ixy);
475+
inertia.SetAttributeValue("ixz", ixz);
476+
inertia.SetAttributeValue("iyy", iyy);
477+
inertia.SetAttributeValue("iyz", iyz);
478+
inertia.SetAttributeValue("izz", izz);
479+
480+
inertial.Add(inertia);
481+
482+
// center of mass
483+
XElement origin = new XElement("origin");
484+
origin.SetAttributeValue("xyz", rigidbody.centerOfMass.Unity2Ros().ToXMLString());
485+
origin.SetAttributeValue("rpy", Vector3.zero.ToXMLString()); // BUG: always zero?
486+
inertial.Add(origin);
487+
488+
489+
link.Add(inertial);
490+
}
491+
}
459492

460493

461494

‎package.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
{
22
"name": "com.fsstudio.zerosim",
3-
"version": "0.1.4",
3+
"version": "0.1.5",
44
"displayName": "ZeroSim",
55
"description": "ZeroSim ROS robotic simulator in Unity.",
66
"unity": "2020.1",

0 commit comments

Comments
 (0)