Skip to content

Commit cee0638

Browse files
micahpearlmanmicahpearlman
committed
working urdf collision import
1 parent ade70b3 commit cee0638

File tree

3 files changed

+121
-20
lines changed

3 files changed

+121
-20
lines changed

‎Editor/ZOZeroSimMenu.cs‎

Lines changed: 21 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,7 @@ static void ImportOBJ(MenuCommand menuCommand) {
6767
return;
6868
}
6969

70-
GameObject go = ZOImportOBJ.Import(filePath);
70+
GameObject go = ZOImportOBJ.Import(filePath, true);
7171
GameObject prefabGo = MakeGameObjectPrefab(go, saveToDirectory);
7272
Object.DestroyImmediate(go);
7373
PrefabUtility.InstantiatePrefab(prefabGo);
@@ -86,7 +86,10 @@ static GameObject MakeGameObjectPrefab(GameObject gameObject, string saveToDirec
8686
if (!Directory.Exists(materialSaveToDirectory)) {
8787
Directory.CreateDirectory(materialSaveToDirectory);
8888
}
89-
SaveMaterial(meshRenderer.sharedMaterial, materialSaveToDirectory);
89+
if (meshRenderer.sharedMaterial != null) {
90+
SaveMaterial(meshRenderer.sharedMaterial, materialSaveToDirectory);
91+
}
92+
9093
}
9194

9295

@@ -108,21 +111,33 @@ static GameObject MakeGameObjectPrefab(GameObject gameObject, string saveToDirec
108111

109112

110113
public static void SaveMaterial(Material material, string saveToDirectory) {
114+
// if (material.name == "Default-Material") {
115+
// return;
116+
// }
111117
string path = Path.Combine(saveToDirectory, material.name + ".mat");
112118
path = FileUtil.GetProjectRelativePath(path);
113119
path = AssetDatabase.GenerateUniqueAssetPath(path);
114120
Debug.Log($"INFO: Saving material at path: {path}");
115-
AssetDatabase.CreateAsset(material, path);
116-
AssetDatabase.SaveAssets();
121+
if (AssetDatabase.Contains(material) == false) {
122+
AssetDatabase.CreateAsset(material, path);
123+
AssetDatabase.SaveAssets();
124+
} else {
125+
Debug.Log($"INFO: Material already exists: {material.name}");
126+
}
127+
117128
}
118129

119130
public static void SaveMesh(Mesh mesh, string saveToDirectory) {
120131
string path = Path.Combine(saveToDirectory, mesh.name + ".asset");
121132
path = FileUtil.GetProjectRelativePath(path);
122133
Debug.Log($"INFO: Saving mesh at path: {path}");
123134
path = AssetDatabase.GenerateUniqueAssetPath(path);
124-
AssetDatabase.CreateAsset(mesh, path);
125-
AssetDatabase.SaveAssets();
135+
if (AssetDatabase.Contains(mesh) == false) {
136+
AssetDatabase.CreateAsset(mesh, path);
137+
AssetDatabase.SaveAssets();
138+
} else {
139+
Debug.Log($"INFO: Mesh already exists: {mesh.name}");
140+
}
126141

127142
}
128143

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

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,9 @@
1010
namespace ZO.ImportExport {
1111
public class ZOImportOBJ {
1212

13-
public static bool SplitByMaterial {
14-
get; set;
15-
} = true;
13+
// public static bool SplitByMaterial {
14+
// get; set;
15+
// } = true;
1616

1717
struct OBJFace {
1818
public string materialName;
@@ -107,13 +107,13 @@ protected static Material[] ImportMTLFile(string mtlFilePath) {
107107
return materials.ToArray();
108108
}
109109

110-
public static GameObject Import(string objFilePath) {
110+
public static GameObject Import(string objFilePath, bool splitByMaterial ) {
111111
using (StreamReader streamReader = new StreamReader(objFilePath)) {
112-
return Import(streamReader, Path.GetFileNameWithoutExtension(objFilePath), Path.GetDirectoryName(objFilePath));
112+
return Import(streamReader, Path.GetFileNameWithoutExtension(objFilePath), Path.GetDirectoryName(objFilePath), splitByMaterial);
113113
}
114114
}
115115

116-
public static GameObject Import(StreamReader streamReader, string meshName, string workingDirectory) {
116+
public static GameObject Import(StreamReader streamReader, string meshName, string workingDirectory, bool splitByMaterial) {
117117

118118

119119
bool hasNormals = false;
@@ -145,7 +145,7 @@ public static GameObject Import(StreamReader streamReader, string meshName, stri
145145
string mtlFilePath = Path.Combine(workingDirectory, lineComponents[1]);
146146
materials = ImportMTLFile(mtlFilePath);
147147

148-
} else if ((lineComponents[0] == "g" || lineComponents[0] == "o") && SplitByMaterial == false) {
148+
} else if ((lineComponents[0] == "g" || lineComponents[0] == "o") && splitByMaterial == false) {
149149
cmesh = data;
150150
if (!objectNames.Contains(cmesh)) {
151151
objectNames.Add(cmesh);
@@ -156,7 +156,7 @@ public static GameObject Import(StreamReader streamReader, string meshName, stri
156156
materialNames.Add(cmaterial);
157157
}
158158

159-
if (SplitByMaterial) {
159+
if (splitByMaterial) {
160160
if (!objectNames.Contains(cmaterial)) {
161161
objectNames.Add(cmaterial);
162162
}
@@ -224,13 +224,13 @@ public static GameObject Import(StreamReader streamReader, string meshName, stri
224224
OBJFace f1 = new OBJFace();
225225
f1.materialName = cmaterial;
226226
f1.indexes = new int[] { indexes[0], indexes[1], indexes[2] };
227-
f1.meshName = (SplitByMaterial) ? cmaterial : cmesh;
227+
f1.meshName = (splitByMaterial) ? cmaterial : cmesh;
228228
faceList.Add(f1);
229229
if (indexes.Length > 3) {
230230

231231
OBJFace f2 = new OBJFace();
232232
f2.materialName = cmaterial;
233-
f2.meshName = (SplitByMaterial) ? cmaterial : cmesh;;
233+
f2.meshName = (splitByMaterial) ? cmaterial : cmesh;;
234234
f2.indexes = new int[] { indexes[2], indexes[3], indexes[0] };
235235
faceList.Add(f2);
236236
}

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

Lines changed: 90 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,6 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
3737
XmlNode[] xmlLinks = robot.GetChildrenByName("link");
3838
Dictionary<string, Tuple<XmlNode, GameObject>> goLinks = new Dictionary<string, Tuple<XmlNode, GameObject>>();
3939

40-
// find root link. which is a link without an explicit parent
4140

4241
// create links
4342
foreach (XmlNode xmlLink in xmlLinks) {
@@ -92,7 +91,7 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
9291
if (xmlMesh != null) {
9392
string meshFileName = xmlMesh.Attributes["filename"].Value;
9493
string meshFilePath = Path.Combine(workingDirectory, meshFileName);
95-
visualGeo = ZOImportOBJ.Import(meshFilePath);
94+
visualGeo = ZOImportOBJ.Import(meshFilePath, splitByMaterial: true);
9695
if (xmlMesh.Attributes["scale"] != null) {
9796
Vector3 scale = xmlMesh.Attributes["scale"].Value.FromURDFStringToVector3().Ros2UnityScale();
9897
visualGeo.transform.localScale = new Vector3(visualGeo.transform.localScale.x * scale.x, visualGeo.transform.localScale.y * scale.y, visualGeo.transform.localScale.z * scale.z);
@@ -123,11 +122,98 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
123122

124123
}
125124

126-
// get the origin
127-
// XmlNode origin =
125+
// process the collisions
126+
XmlNode[] xmlCollisions = xmlLink.GetChildrenByName("collision");
127+
128+
foreach (XmlNode xmlCollision in xmlCollisions) {
129+
130+
131+
XmlNode[] xmlGeometries = xmlCollision.GetChildrenByName("geometry");
132+
133+
foreach (XmlNode xmlGeom in xmlGeometries) {
134+
135+
GameObject collisionGeo = null;
136+
137+
XmlNode xmlBox = xmlGeom.GetChildByName("box");
138+
if (xmlBox != null) {
139+
collisionGeo = GameObject.CreatePrimitive(PrimitiveType.Cube);
140+
Vector3 size = xmlBox.Attributes["size"].Value.FromURDFStringToVector3();
141+
142+
collisionGeo.transform.localScale = size.Ros2UnityScale();
143+
}
144+
145+
XmlNode xmlCylinder = xmlGeom.GetChildByName("cylinder");
146+
if (xmlCylinder != null) {
147+
collisionGeo = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
148+
float radius = float.Parse(xmlCylinder.Attributes["radius"].Value);
149+
float length = float.Parse(xmlCylinder.Attributes["length"].Value);
150+
collisionGeo.transform.localScale = new Vector3(radius * 2.0f, length * 0.5f, radius * 2.0f);
151+
}
152+
153+
XmlNode xmlSphere = xmlGeom.GetChildByName("sphere");
154+
if (xmlSphere != null) {
155+
collisionGeo = GameObject.CreatePrimitive(PrimitiveType.Sphere);
156+
float radius = float.Parse(xmlSphere.Attributes["radius"].Value);
157+
collisionGeo.transform.localScale = new Vector3(radius * 2.0f, radius * 2.0f, radius * 2.0f);
158+
}
159+
160+
XmlNode xmlMesh = xmlGeom.GetChildByName("mesh");
161+
if (xmlMesh != null) {
162+
string meshFileName = xmlMesh.Attributes["filename"].Value;
163+
string meshFilePath = Path.Combine(workingDirectory, meshFileName);
164+
collisionGeo = ZOImportOBJ.Import(meshFilePath, splitByMaterial: false);
165+
166+
MeshFilter[] meshFilters = collisionGeo.GetComponentsInChildren<MeshFilter>();
167+
foreach (MeshFilter meshFilter in meshFilters) {
168+
MeshCollider meshCollider = collisionGeo.AddComponent<MeshCollider>();
169+
170+
// add mesh collider
171+
meshCollider.sharedMesh = meshFilter.sharedMesh;
172+
meshCollider.convex = true;
173+
174+
// remove mesh renderer and mesh filter
175+
GameObject.DestroyImmediate(collisionGeo.GetComponent<MeshRenderer>());
176+
GameObject.DestroyImmediate(collisionGeo.GetComponent<MeshFilter>());
177+
178+
}
179+
180+
181+
if (xmlMesh.Attributes["scale"] != null) {
182+
Vector3 scale = xmlMesh.Attributes["scale"].Value.FromURDFStringToVector3().Ros2UnityScale();
183+
collisionGeo.transform.localScale = new Vector3(collisionGeo.transform.localScale.x * scale.x, collisionGeo.transform.localScale.y * scale.y, collisionGeo.transform.localScale.z * scale.z);
184+
}
185+
186+
collisionGeo.transform.localRotation = Quaternion.Euler(270, 90, 0);
187+
188+
189+
190+
191+
}
192+
if (collisionGeo != null) {
193+
// set parent
194+
collisionGeo.transform.SetParent(goCollisionsEmpty.transform);
195+
if (xmlCollision.Attributes["name"] != null) {
196+
collisionGeo.name = xmlCollision.Attributes["name"].Value;
197+
}
198+
199+
// set transform
200+
XmlNode xmlOrigin = xmlCollision.GetChildByName("origin");
201+
if (xmlOrigin != null) {
202+
Tuple<Vector3, Quaternion> transform = OriginXMLToUnity(xmlOrigin);
203+
collisionGeo.transform.localPosition += transform.Item1;
204+
collisionGeo.transform.localRotation *= transform.Item2;
205+
206+
}
207+
208+
}
209+
210+
}
211+
212+
}
128213

129214
}
130215

216+
131217
// create hierarchy from joints
132218
XmlNode[] xmlJoints = robot.GetChildrenByName("joint");
133219
foreach (var goLink in goLinks) {

0 commit comments

Comments
 (0)