Skip to content

Commit 3a4a5e3

Browse files
micahpearlmanmicahpearlman
committed
sphere, box, capsule/cylinder collision support
1 parent 758d43d commit 3a4a5e3

File tree

3 files changed

+92
-144
lines changed

3 files changed

+92
-144
lines changed

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

Lines changed: 70 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,8 @@ public Vector3 ConnectedAnchor {
5454

5555
private List<Transform> _visualMeshesToExport = new List<Transform>();
5656
public List<Transform> VisualMeshesToExport {
57-
get {return _visualMeshesToExport; }
58-
}
57+
get { return _visualMeshesToExport; }
58+
}
5959

6060
private List<Transform> _collisionMeshesToExport = new List<Transform>();
6161
public List<Transform> CollisionMeshesToExport {
@@ -70,12 +70,12 @@ public static void ExportToDirectory(ZOSimDocumentRoot documentRoot, string dire
7070
urdfXML.Save(urdfFilePath);
7171

7272
// save out visual and collision meshes
73-
foreach(Transform meshTransform in exportURDF.VisualMeshesToExport) {
73+
foreach (Transform meshTransform in exportURDF.VisualMeshesToExport) {
7474
ZOExportOBJ exportOBJ = new ZOExportOBJ();
7575
exportOBJ.ExportToDirectory(meshTransform.gameObject, directoryPath, true, false, ZOExportOBJ.Orientation.URDF);
7676
}
7777

78-
foreach(Transform meshTransform in exportURDF.CollisionMeshesToExport) {
78+
foreach (Transform meshTransform in exportURDF.CollisionMeshesToExport) {
7979
ZOExportOBJ exportOBJ = new ZOExportOBJ();
8080
exportOBJ.ExportToDirectory(meshTransform.gameObject, directoryPath, true, false, ZOExportOBJ.Orientation.URDF);
8181
}
@@ -272,13 +272,79 @@ protected void BuildURDFLink(ZOSimOccurrence simOccurrence, XElement robot, Vect
272272
foreach (Transform visualsChild in child) {
273273
// check if it is a primitive type (cube, sphere, cylinder, etc)
274274
BuildURDFVisuals(visualsChild, link, anchorOffset);
275+
276+
// we will do any collider that are attached to the visual
277+
BuildURDFCollisions(visualsChild, link, anchorOffset);
278+
}
279+
}
280+
281+
if (child.name.ToLower() == "collisions") {
282+
// go through the children of the collisions and get all the models
283+
foreach (Transform collisionChild in child) {
284+
BuildURDFCollisions(collisionChild, link, anchorOffset);
275285
}
276286
}
277287

278288
}
279289
}
280290

281291

292+
protected void BuildURDFCollisions(Transform collisionTransform, XElement link, Vector3 anchorOffset) {
293+
Collider[] colliders = collisionTransform.GetComponentsInChildren<Collider>();
294+
foreach (Collider collider in colliders) {
295+
XElement collision = new XElement("collision");
296+
collision.SetAttributeValue("name", collisionTransform.name);
297+
XElement geometry = new XElement("geometry");
298+
Vector3 center = Vector3.zero;
299+
300+
if (collider.GetType() == typeof(BoxCollider)) {
301+
BoxCollider boxCollider = collider as BoxCollider;
302+
XElement box = new XElement("box");
303+
304+
Vector3 boxSize = new Vector3(boxCollider.size.x * collider.transform.localScale.x,
305+
boxCollider.size.y * collider.transform.localScale.y,
306+
boxCollider.size.z * collider.transform.localScale.z);
307+
box.SetAttributeValue("size", boxSize.Unity2RosScale().ToXMLString());
308+
geometry.Add(box);
309+
310+
center = new Vector3(boxCollider.center.x * boxCollider.transform.localScale.x,
311+
boxCollider.center.y * boxCollider.transform.localScale.y,
312+
boxCollider.center.z * boxCollider.transform.localScale.z);
313+
} else if (collider.GetType() == typeof(SphereCollider)) {
314+
SphereCollider sphereCollider = collider as SphereCollider;
315+
XElement sphere = new XElement("sphere");
316+
float radius = sphereCollider.radius * collider.transform.localScale.x;
317+
sphere.SetAttributeValue("radius", radius);
318+
geometry.Add(sphere);
319+
320+
center = new Vector3(sphereCollider.center.x * sphereCollider.transform.localScale.x,
321+
sphereCollider.center.y * sphereCollider.transform.localScale.y,
322+
sphereCollider.center.z * sphereCollider.transform.localScale.z);
323+
324+
325+
}
326+
327+
if (geometry.HasElements) {
328+
// build origin
329+
Vector3 position = collisionTransform.localPosition + anchorOffset + center;
330+
Vector3 xyz = position.Unity2Ros();
331+
Vector3 rpy = new Vector3(-collisionTransform.localEulerAngles.z * Mathf.Deg2Rad,
332+
collisionTransform.localEulerAngles.x * Mathf.Deg2Rad,
333+
-collisionTransform.localEulerAngles.y * Mathf.Deg2Rad);
334+
335+
XElement origin = new XElement("origin");
336+
origin.SetAttributeValue("xyz", xyz.ToXMLString());
337+
origin.SetAttributeValue("rpy", rpy.ToXMLString());
338+
339+
collision.Add(origin);
340+
341+
collision.Add(geometry);
342+
link.Add(collision);
343+
}
344+
345+
}
346+
}
347+
282348
protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
283349
// build 3d primitive if exists
284350
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();

‎Samples~/ZeroSimSamples/Robots/SimpleRobotArm/SimpleRobotArm.prefab‎

Lines changed: 20 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -761,8 +761,7 @@ GameObject:
761761
serializedVersion: 6
762762
m_Component:
763763
- component: {fileID: 2761172348953418728}
764-
- component: {fileID: 2761172348953418730}
765-
- component: {fileID: 2761172348953418731}
764+
- component: {fileID: 5858319252739143764}
766765
m_Layer: 0
767766
m_Name: Cylinder
768767
m_TagString: Untagged
@@ -779,60 +778,25 @@ Transform:
779778
m_GameObject: {fileID: 2761172348953418729}
780779
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
781780
m_LocalPosition: {x: 0, y: 0, z: 0}
782-
m_LocalScale: {x: 0.07, y: 0.1, z: 0.07}
781+
m_LocalScale: {x: 1, y: 1, z: 1}
783782
m_Children: []
784783
m_Father: {fileID: 2761172349558930090}
785784
m_RootOrder: 0
786785
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
787-
--- !u!33 &2761172348953418730
788-
MeshFilter:
789-
m_ObjectHideFlags: 0
790-
m_CorrespondingSourceObject: {fileID: 0}
791-
m_PrefabInstance: {fileID: 0}
792-
m_PrefabAsset: {fileID: 0}
793-
m_GameObject: {fileID: 2761172348953418729}
794-
m_Mesh: {fileID: 10206, guid: 0000000000000000e000000000000000, type: 0}
795-
--- !u!23 &2761172348953418731
796-
MeshRenderer:
786+
--- !u!136 &5858319252739143764
787+
CapsuleCollider:
797788
m_ObjectHideFlags: 0
798789
m_CorrespondingSourceObject: {fileID: 0}
799790
m_PrefabInstance: {fileID: 0}
800791
m_PrefabAsset: {fileID: 0}
801792
m_GameObject: {fileID: 2761172348953418729}
793+
m_Material: {fileID: 0}
794+
m_IsTrigger: 0
802795
m_Enabled: 1
803-
m_CastShadows: 1
804-
m_ReceiveShadows: 1
805-
m_DynamicOccludee: 1
806-
m_MotionVectors: 1
807-
m_LightProbeUsage: 1
808-
m_ReflectionProbeUsage: 1
809-
m_RayTracingMode: 2
810-
m_RayTraceProcedural: 0
811-
m_RenderingLayerMask: 1
812-
m_RendererPriority: 0
813-
m_Materials:
814-
- {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
815-
m_StaticBatchInfo:
816-
firstSubMesh: 0
817-
subMeshCount: 0
818-
m_StaticBatchRoot: {fileID: 0}
819-
m_ProbeAnchor: {fileID: 0}
820-
m_LightProbeVolumeOverride: {fileID: 0}
821-
m_ScaleInLightmap: 1
822-
m_ReceiveGI: 1
823-
m_PreserveUVs: 0
824-
m_IgnoreNormalsForChartDetection: 0
825-
m_ImportantGI: 0
826-
m_StitchLightmapSeams: 1
827-
m_SelectedEditorRenderState: 3
828-
m_MinimumChartSize: 4
829-
m_AutoUVMaxDistance: 0.5
830-
m_AutoUVMaxAngle: 89
831-
m_LightmapParameters: {fileID: 0}
832-
m_SortingLayerID: 0
833-
m_SortingLayer: 0
834-
m_SortingOrder: 0
835-
m_AdditionalVertexStreams: {fileID: 0}
796+
m_Radius: 0.035
797+
m_Height: 0.2
798+
m_Direction: 1
799+
m_Center: {x: 0, y: 0, z: 0}
836800
--- !u!1 &2761172349049173837
837801
GameObject:
838802
m_ObjectHideFlags: 0
@@ -976,7 +940,6 @@ MonoBehaviour:
976940
m_Script: {fileID: 11500000, guid: 5f63db7f27f0b322ab785c2fe0b3e4c1, type: 3}
977941
m_Name:
978942
m_EditorClassIdentifier:
979-
_zoSimDocumentFilePath: Assets/ZeroSimSamples/Assets/ZeroSimAssets/SimpleRobotArm.zosim
980943
--- !u!114 &2761172349568995195
981944
MonoBehaviour:
982945
m_ObjectHideFlags: 0
@@ -1239,8 +1202,7 @@ GameObject:
12391202
serializedVersion: 6
12401203
m_Component:
12411204
- component: {fileID: 2761172349721112100}
1242-
- component: {fileID: 2761172349721112102}
1243-
- component: {fileID: 2761172349721112103}
1205+
- component: {fileID: 2761172349580583619}
12441206
m_Layer: 0
12451207
m_Name: Cylinder
12461208
m_TagString: Untagged
@@ -1257,60 +1219,25 @@ Transform:
12571219
m_GameObject: {fileID: 2761172349721112101}
12581220
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
12591221
m_LocalPosition: {x: 0, y: 0, z: 0}
1260-
m_LocalScale: {x: 0.07, y: 0.1, z: 0.07}
1222+
m_LocalScale: {x: 1, y: 1, z: 1}
12611223
m_Children: []
12621224
m_Father: {fileID: 2761172348618385422}
12631225
m_RootOrder: 0
12641226
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
1265-
--- !u!33 &2761172349721112102
1266-
MeshFilter:
1267-
m_ObjectHideFlags: 0
1268-
m_CorrespondingSourceObject: {fileID: 0}
1269-
m_PrefabInstance: {fileID: 0}
1270-
m_PrefabAsset: {fileID: 0}
1271-
m_GameObject: {fileID: 2761172349721112101}
1272-
m_Mesh: {fileID: 10206, guid: 0000000000000000e000000000000000, type: 0}
1273-
--- !u!23 &2761172349721112103
1274-
MeshRenderer:
1227+
--- !u!136 &2761172349580583619
1228+
CapsuleCollider:
12751229
m_ObjectHideFlags: 0
12761230
m_CorrespondingSourceObject: {fileID: 0}
12771231
m_PrefabInstance: {fileID: 0}
12781232
m_PrefabAsset: {fileID: 0}
12791233
m_GameObject: {fileID: 2761172349721112101}
1234+
m_Material: {fileID: 0}
1235+
m_IsTrigger: 0
12801236
m_Enabled: 1
1281-
m_CastShadows: 1
1282-
m_ReceiveShadows: 1
1283-
m_DynamicOccludee: 1
1284-
m_MotionVectors: 1
1285-
m_LightProbeUsage: 1
1286-
m_ReflectionProbeUsage: 1
1287-
m_RayTracingMode: 2
1288-
m_RayTraceProcedural: 0
1289-
m_RenderingLayerMask: 1
1290-
m_RendererPriority: 0
1291-
m_Materials:
1292-
- {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
1293-
m_StaticBatchInfo:
1294-
firstSubMesh: 0
1295-
subMeshCount: 0
1296-
m_StaticBatchRoot: {fileID: 0}
1297-
m_ProbeAnchor: {fileID: 0}
1298-
m_LightProbeVolumeOverride: {fileID: 0}
1299-
m_ScaleInLightmap: 1
1300-
m_ReceiveGI: 1
1301-
m_PreserveUVs: 0
1302-
m_IgnoreNormalsForChartDetection: 0
1303-
m_ImportantGI: 0
1304-
m_StitchLightmapSeams: 1
1305-
m_SelectedEditorRenderState: 3
1306-
m_MinimumChartSize: 4
1307-
m_AutoUVMaxDistance: 0.5
1308-
m_AutoUVMaxAngle: 89
1309-
m_LightmapParameters: {fileID: 0}
1310-
m_SortingLayerID: 0
1311-
m_SortingLayer: 0
1312-
m_SortingOrder: 0
1313-
m_AdditionalVertexStreams: {fileID: 0}
1237+
m_Radius: 0.035
1238+
m_Height: 0.2
1239+
m_Direction: 1
1240+
m_Center: {x: 0, y: 0, z: 0}
13141241
--- !u!1 &2761172349790448138
13151242
GameObject:
13161243
m_ObjectHideFlags: 0

‎Samples~/ZeroSimSamples/Scenes/URDF_test.unity‎

Lines changed: 2 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -836,15 +836,15 @@ Transform:
836836
m_PrefabInstance: {fileID: 0}
837837
m_PrefabAsset: {fileID: 0}
838838
m_GameObject: {fileID: 290498956}
839-
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
839+
m_LocalRotation: {x: 0, y: 0.38268343, z: 0, w: 0.92387956}
840840
m_LocalPosition: {x: 0, y: 0.5, z: 0}
841841
m_LocalScale: {x: 1, y: 1, z: 1}
842842
m_Children:
843843
- {fileID: 10280180}
844844
- {fileID: 1357432954}
845845
m_Father: {fileID: 63962903}
846846
m_RootOrder: 2
847-
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
847+
m_LocalEulerAnglesHint: {x: 0, y: 45, z: 0}
848848
--- !u!114 &290498958
849849
MonoBehaviour:
850850
m_ObjectHideFlags: 0
@@ -4124,11 +4124,6 @@ PrefabInstance:
41244124
m_Modification:
41254125
m_TransformParent: {fileID: 0}
41264126
m_Modifications:
4127-
- target: {fileID: 2761172348088701093, guid: 89108e8d609b569839a30991930882ef,
4128-
type: 3}
4129-
propertyPath: m_Name
4130-
value: JointMarker1
4131-
objectReference: {fileID: 0}
41324127
- target: {fileID: 2761172348088701094, guid: 89108e8d609b569839a30991930882ef,
41334128
type: 3}
41344129
propertyPath: m_Materials.Array.data[0]
@@ -4144,11 +4139,6 @@ PrefabInstance:
41444139
propertyPath: m_Mesh
41454140
value:
41464141
objectReference: {fileID: 1164543194}
4147-
- target: {fileID: 2761172348238204255, guid: 89108e8d609b569839a30991930882ef,
4148-
type: 3}
4149-
propertyPath: m_Name
4150-
value: Hand
4151-
objectReference: {fileID: 0}
41524142
- target: {fileID: 2761172348238204256, guid: 89108e8d609b569839a30991930882ef,
41534143
type: 3}
41544144
propertyPath: m_Materials.Array.data[0]
@@ -4164,11 +4154,6 @@ PrefabInstance:
41644154
propertyPath: m_Mesh
41654155
value:
41664156
objectReference: {fileID: 623542022}
4167-
- target: {fileID: 2761172348259037097, guid: 89108e8d609b569839a30991930882ef,
4168-
type: 3}
4169-
propertyPath: m_Name
4170-
value: JointMarker3
4171-
objectReference: {fileID: 0}
41724157
- target: {fileID: 2761172348259037098, guid: 89108e8d609b569839a30991930882ef,
41734158
type: 3}
41744159
propertyPath: m_Materials.Array.data[0]
@@ -4184,16 +4169,6 @@ PrefabInstance:
41844169
propertyPath: m_Mesh
41854170
value:
41864171
objectReference: {fileID: 1575773637}
4187-
- target: {fileID: 2761172348352995842, guid: 89108e8d609b569839a30991930882ef,
4188-
type: 3}
4189-
propertyPath: m_Name
4190-
value: BaseCube
4191-
objectReference: {fileID: 0}
4192-
- target: {fileID: 2761172348352995842, guid: 89108e8d609b569839a30991930882ef,
4193-
type: 3}
4194-
propertyPath: m_IsActive
4195-
value: 1
4196-
objectReference: {fileID: 0}
41974172
- target: {fileID: 2761172348352995844, guid: 89108e8d609b569839a30991930882ef,
41984173
type: 3}
41994174
propertyPath: m_Materials.Array.data[0]
@@ -4204,11 +4179,6 @@ PrefabInstance:
42044179
propertyPath: m_Mesh
42054180
value:
42064181
objectReference: {fileID: 22652528}
4207-
- target: {fileID: 2761172348534080120, guid: 89108e8d609b569839a30991930882ef,
4208-
type: 3}
4209-
propertyPath: m_Name
4210-
value: Elbow
4211-
objectReference: {fileID: 0}
42124182
- target: {fileID: 2761172348534080122, guid: 89108e8d609b569839a30991930882ef,
42134183
type: 3}
42144184
propertyPath: m_Materials.Array.data[0]
@@ -4219,11 +4189,6 @@ PrefabInstance:
42194189
propertyPath: m_Mesh
42204190
value:
42214191
objectReference: {fileID: 1042415718}
4222-
- target: {fileID: 2761172348620982721, guid: 89108e8d609b569839a30991930882ef,
4223-
type: 3}
4224-
propertyPath: m_Name
4225-
value: JointMarker2
4226-
objectReference: {fileID: 0}
42274192
- target: {fileID: 2761172348620982722, guid: 89108e8d609b569839a30991930882ef,
42284193
type: 3}
42294194
propertyPath: m_Materials.Array.data[0]
@@ -4299,11 +4264,6 @@ PrefabInstance:
42994264
propertyPath: m_LocalEulerAnglesHint.z
43004265
value: 0
43014266
objectReference: {fileID: 0}
4302-
- target: {fileID: 2761172349569614648, guid: 89108e8d609b569839a30991930882ef,
4303-
type: 3}
4304-
propertyPath: m_Name
4305-
value: Shoulder
4306-
objectReference: {fileID: 0}
43074267
- target: {fileID: 2761172349569614650, guid: 89108e8d609b569839a30991930882ef,
43084268
type: 3}
43094269
propertyPath: m_Materials.Array.data[0]
@@ -4314,10 +4274,5 @@ PrefabInstance:
43144274
propertyPath: m_Mesh
43154275
value:
43164276
objectReference: {fileID: 358699685}
4317-
- target: {fileID: 2761172349971327372, guid: 89108e8d609b569839a30991930882ef,
4318-
type: 3}
4319-
propertyPath: m_Name
4320-
value: BaseCube
4321-
objectReference: {fileID: 0}
43224277
m_RemovedComponents: []
43234278
m_SourcePrefab: {fileID: 100100000, guid: 89108e8d609b569839a30991930882ef, type: 3}

0 commit comments

Comments
 (0)