Skip to content

Commit dbd1f54

Browse files
micahpearlmanmicahpearlman
committed
start of URDF export as seperate from occurrence
1 parent e503fa3 commit dbd1f54

File tree

5 files changed

+237
-161
lines changed

5 files changed

+237
-161
lines changed

‎Runtime/Scripts/Document/ZOSimDocumentRoot.cs‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
using ZO.ROS.Controllers;
1010
using ZO.ROS.Unity.Service;
1111
using ZO.Math;
12+
using ZO.ImportExport;
1213
namespace ZO.Document {
1314

1415
/// <summary>
@@ -261,9 +262,8 @@ public void ExportURDF(string exportDirectory) {
261262
foreach (Transform child in transform) { // BUG: Should only ever be one base object for URDF!!!
262263
ZOSimOccurrence simOccurence = child.GetComponent<ZOSimOccurrence>();
263264
if (simOccurence) {
264-
ZOSimOccurrence.BuildURDF(robot, simOccurence, this.transform.WorldTranslationRotationMatrix());
265-
// simOccurence.BuildURDFJoints(this, robot, null, this.transform.WorldTranslationRotationMatrix());
266-
265+
ZOExportURDF exportURDF = new ZOExportURDF();
266+
exportURDF.BuildURDF(robot, simOccurence, this.transform.WorldTranslationRotationMatrix());
267267
}
268268
}
269269

‎Runtime/Scripts/Document/ZOSimOccurrence.cs‎

Lines changed: 19 additions & 146 deletions
Original file line numberDiff line numberDiff line change
@@ -1016,11 +1016,15 @@ private static void DeserializeGeometricPrimitive(GameObject go, JObject primiti
10161016

10171017
#region URDF
10181018

1019-
public XElement XML { get; }
1020-
1019+
private List<Transform> _visualMeshesToExport = new List<Transform>();
1020+
private List<Transform> _collisionMeshesToExport = new List<Transform>();
10211021
protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
10221022
// build 3d primitive if exists
10231023
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();
1024+
if (meshFilter == null) {
1025+
// Check children
1026+
meshFilter = visualTransform.GetComponentInChildren<MeshFilter>();
1027+
}
10241028
if (meshFilter) {
10251029

10261030
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
@@ -1040,8 +1044,7 @@ protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector
10401044
if (collider) {
10411045
//TODO: add box collider
10421046
}
1043-
}
1044-
if (meshFilter.sharedMesh.name.Contains("Sphere")) {
1047+
} else if (meshFilter.sharedMesh.name.Contains("Sphere")) {
10451048
XElement sphere = new XElement("sphere");
10461049
float radius = visualTransform.localScale.x / 2.0f;
10471050
sphere.SetAttributeValue("radius", radius);
@@ -1052,17 +1055,7 @@ protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector
10521055
//TODO: add box collider
10531056
}
10541057

1055-
}
1056-
// if (meshFilter.sharedMesh.name.Contains("Capsule")) {
1057-
// collider = go.GetComponent<CapsuleCollider>();
1058-
// primitiveJSON = new JObject(
1059-
// new JProperty("type", "primitive.capsule"),
1060-
// new JProperty("dimensions", go.transform.localScale.ToJSON()),
1061-
// new JProperty("has_collisions", collider ? true : false),
1062-
// new JProperty("color", meshRenderer.sharedMaterial.color.ToJSON())
1063-
// );
1064-
// }
1065-
if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
1058+
} else if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
10661059
XElement cylinder = new XElement("cylinder");
10671060
float radius = visualTransform.localScale.x / 2.0f;
10681061
float height = visualTransform.localScale.y * 2.0f;
@@ -1076,19 +1069,22 @@ protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector
10761069
//TODO: add box collider
10771070
}
10781071

1072+
} else { // regular mesh so export meshes as OBJ
1073+
XElement mesh = new XElement("mesh");
1074+
mesh.SetAttributeValue("filename", $"{visualTransform.name}.obj");
1075+
mesh.SetAttributeValue("scale", visualTransform.localScale.ToXMLString());
1076+
geometry.Add(mesh);
1077+
1078+
_visualMeshesToExport.Add(visualTransform);
10791079
}
10801080

10811081
if (geometry.HasElements) {
10821082
// build origin
10831083
Vector3 position = visualTransform.localPosition + anchorOffset;
1084-
Vector3 xyz = position.Unity2Ros();//visualTransform.localPosition.Unity2Ros();
1084+
Vector3 xyz = position.Unity2Ros();
10851085
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
10861086
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
10871087
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
1088-
// Vector3 xyz = jointTransform.position.Unity2Ros();
1089-
// Vector3 rpy = new Vector3(-transform.eulerAngles.z * Mathf.Deg2Rad,
1090-
// transform.eulerAngles.x * Mathf.Deg2Rad,
1091-
// -transform.eulerAngles.y * Mathf.Deg2Rad);
10921088

10931089
XElement origin = new XElement("origin");
10941090
origin.SetAttributeValue("xyz", xyz.ToXMLString());
@@ -1100,27 +1096,6 @@ protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector
11001096
link.Add(visual);
11011097
}
11021098

1103-
1104-
// // add name
1105-
// primitiveJSON.Add("name", go.name);
1106-
1107-
// // add transform
1108-
// primitiveJSON.Add("translation", go.transform.localPosition.ToJSON());
1109-
// primitiveJSON.Add("rotation_quaternion", go.transform.localRotation.ToJSON());
1110-
// primitiveJSON.Add("scale", go.transform.localScale.ToJSON());
1111-
1112-
1113-
// // generate physics material if necessary (friction, restitution)
1114-
// if (collider != null && collider.sharedMaterial != null) {
1115-
// primitiveJSON.Add("physics_material",
1116-
// new JObject(
1117-
// new JProperty("bounciness", collider.sharedMaterial.bounciness),
1118-
// new JProperty("dynamic_friction", collider.sharedMaterial.dynamicFriction),
1119-
// new JProperty("static_friction", collider.sharedMaterial.staticFriction)
1120-
// )
1121-
// );
1122-
// }
1123-
11241099
}
11251100

11261101

@@ -1153,89 +1128,7 @@ public void BuildURDFLink(XElement robot, Vector3 anchorOffset) {
11531128

11541129
}
11551130

1156-
protected readonly struct URDFJoint {
1157-
public URDFJoint(ZOSimOccurrence child, ZOSimOccurrence parent, Vector3 anchor, Vector3 connectedAnchor) {
1158-
Child = child;
1159-
Parent = parent;
1160-
Anchor = anchor;
1161-
ConnectedAnchor = connectedAnchor;
1162-
}
1163-
1164-
1165-
public ZOSimOccurrence Child {
1166-
get;
1167-
}
1168-
1169-
public ZOSimOccurrence Parent {
1170-
get;
1171-
}
1172-
1173-
public Vector3 Anchor {
1174-
get;
1175-
}
1176-
1177-
public Vector3 ConnectedAnchor {
1178-
get;
1179-
}
1180-
1181-
public static bool operator ==(URDFJoint a, URDFJoint b) {
1182-
return a.Equals(b);
1183-
}
1184-
public static bool operator !=(URDFJoint a, URDFJoint b) {
1185-
return !a.Equals(b);
1186-
}
1187-
1188-
}
1189-
1190-
public static void BuildURDF(XElement robot, ZOSimOccurrence simOccurrence, Matrix4x4 baseTransform) {
1191-
List<URDFJoint> joints = new List<URDFJoint>();
1192-
1193-
simOccurrence.BuildURDFJoints(robot, null, baseTransform, ref joints);
1194-
1195-
1196-
// build links
1197-
HashSet<ZOSimOccurrence> links = new HashSet<ZOSimOccurrence>();
1198-
foreach (URDFJoint joint in joints) {
1199-
if (links.Contains<ZOSimOccurrence>(joint.Parent) == false) { // build parent link if not exist
1200-
Vector3 offset = -1.0f * joint.ConnectedAnchor;
1201-
if (joints[0] == joint) { // if base joint do not apply any offset to parent link
1202-
offset = Vector3.zero;
1203-
}
1204-
joint.Parent.BuildURDFLink(robot, offset);
1205-
links.Add(joint.Parent);
1206-
}
1207-
if (links.Contains<ZOSimOccurrence>(joint.Child) == false) { // build child link if not exist
1208-
Vector3 offset = -1.0f * joint.ConnectedAnchor;
1209-
joint.Child.BuildURDFLink(robot, offset);
1210-
links.Add(joint.Child);
1211-
}
1212-
}
1213-
1214-
if (joints.Count == 0) { // if we don't have any joints then create a dummy world link and joint to first link
1215-
1216-
XElement link = new XElement("link");
1217-
link.SetAttributeValue("name", "World");
1218-
robot.Add(link);
1219-
1220-
simOccurrence.BuildURDFLink(robot, Vector3.zero);
1221-
1222-
XElement jointX = new XElement("joint");
1223-
jointX.SetAttributeValue("name", $"World_to_{simOccurrence.Name}");
1224-
jointX.SetAttributeValue("type", "fixed");
1225-
1226-
XElement parentX = new XElement("parent");
1227-
parentX.SetAttributeValue("link", "World");
1228-
jointX.Add(parentX);
1229-
1230-
XElement childX = new XElement("child");
1231-
childX.SetAttributeValue("link", simOccurrence.Name);
1232-
jointX.Add(childX);
1233-
robot.Add(jointX);
1234-
1235-
}
1236-
1237-
}
1238-
1131+
#if BLAHBLAH
12391132
protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, Matrix4x4 worldJointMatrix, ref List<URDFJoint> joints) {
12401133

12411134
// if we have a parent build a joint
@@ -1305,24 +1198,6 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, Matrix4x4
13051198
}
13061199

13071200

1308-
// build origin
1309-
1310-
// Vector3 xyz = jointTransform.localPosition.Unity2Ros();
1311-
// Vector3 rpy = new Vector3(-jointTransform.localEulerAngles.z * Mathf.Deg2Rad,
1312-
// jointTransform.localEulerAngles.x * Mathf.Deg2Rad,
1313-
// -jointTransform.localEulerAngles.y * Mathf.Deg2Rad);
1314-
1315-
// remember everything is relative to parent.
1316-
// TODO: THOUGH MAYBE IT IS RELATIVE TO PARENT JOINT WHICH MAY NOT BE THE SAME!!!
1317-
1318-
// Vector3 xyz = jointMatrix.Position().Unity2Ros();
1319-
// Vector3 rpy = jointMatrix.rotation.Unity2RosRollPitchYaw();
1320-
1321-
// XElement origin = new XElement("origin");
1322-
// origin.SetAttributeValue("xyz", xyz.ToXMLString());
1323-
// origin.SetAttributeValue("rpy", rpy.ToXMLString());
1324-
// jointX.Add(origin);
1325-
13261201
robot.Add(jointX);
13271202

13281203
XElement parentX = new XElement("parent");
@@ -1333,10 +1208,6 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, Matrix4x4
13331208
childX.SetAttributeValue("link", this.Name);
13341209
jointX.Add(childX);
13351210

1336-
1337-
// build the links
1338-
// parent.BuildURDFLink(documentRoot, robot, parent);
1339-
// this.BuildURDFLink(documentRoot, robot, parent);
13401211
}
13411212
// recursively go through the children
13421213
foreach (Transform child in transform) {
@@ -1348,6 +1219,8 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, Matrix4x4
13481219

13491220
}
13501221

1222+
#endif // 0
1223+
13511224
/// <summary>
13521225
/// Sets the state of the object from XML. XML could come from file or network or wherever.
13531226
/// </summary>

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

Lines changed: 0 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -80,10 +80,6 @@ protected string MeshToString(MeshFilter meshFilter, Transform transform) {
8080
triangles[i] + 1 + _startIndex,
8181
triangles[i + 1] + 1 + _startIndex,
8282
triangles[i + 2] + 1 + _startIndex));
83-
// sb.Append(string.Format("f {0}//{0} {1}//{1} {2}//{2}\n", // pos, None, Norm
84-
// triangles[i] + 1 + _startIndex,
85-
// triangles[i + 1] + 1 + _startIndex,
86-
// triangles[i + 2] + 1 + _startIndex));
8783

8884
}
8985
}
@@ -103,7 +99,6 @@ protected string MaterialToString(Material material) {
10399
#if UNITY_EDITOR // AssetDatabase not available during runtime
104100
string assetPath = AssetDatabase.GetAssetPath(material.GetTexture("_MainTex"));
105101
string texName = Path.GetFileName(assetPath);
106-
// string exportPath = Path.Combine(dir, texName);
107102
sb.AppendFormat("map_Kd {0}", texName).AppendLine();
108103
#endif // #if UNITY_EDITOR
109104

@@ -186,8 +181,6 @@ public void BuildExportData(GameObject gameObject, bool makeSubmeshes, bool zero
186181
string assetPath = AssetDatabase.GetAssetPath(material.GetTexture("_MainTex"));
187182
if (string.IsNullOrEmpty(assetPath) == false) {
188183
TextureAssetPaths.Add(assetPath);
189-
// string texName = Path.GetFileName(assetPath);
190-
// File.Copy(assetPath, Path.Combine(directoryPath, texName));
191184
}
192185
#endif // UNITY_EDITOR
193186
}
@@ -202,7 +195,6 @@ public void BuildExportData(GameObject gameObject, bool makeSubmeshes, bool zero
202195
meshFilter = child.GetComponent<MeshFilter>();
203196

204197
if (meshFilter != null) {
205-
// meshString.Append(ZOObjExporterScript.MeshToString(meshFilter, transform));
206198
Material[] materials = meshFilter.GetComponent<Renderer>().sharedMaterials;
207199
foreach (Material material in materials) {
208200
if (materialNames.Contains(material.name) == false) {
@@ -215,8 +207,6 @@ public void BuildExportData(GameObject gameObject, bool makeSubmeshes, bool zero
215207
string assetPath = AssetDatabase.GetAssetPath(material.GetTexture("_MainTex"));
216208
if (string.IsNullOrEmpty(assetPath) == false) {
217209
TextureAssetPaths.Add(assetPath);
218-
// string texName = Path.GetFileName(assetPath);
219-
// File.Copy(assetPath, Path.Combine(directoryPath, texName));
220210
}
221211
#endif // UNITY_EDITOR
222212
}
@@ -226,8 +216,6 @@ public void BuildExportData(GameObject gameObject, bool makeSubmeshes, bool zero
226216
}
227217

228218
MtlLibraryString = mtlFileString.ToString();
229-
// string mtlFilePath = Path.Combine(directoryPath, $"{meshName}.mtl");
230-
// WriteToFile(mtlFileString.ToString(), mtlFilePath);
231219

232220
End();
233221

0 commit comments

Comments
 (0)