Skip to content

Commit 204395e

Browse files
micahpearlmanmicahpearlman
committed
still working urdf export. in progress.
1 parent 3cf40a1 commit 204395e

File tree

7 files changed

+2034
-60
lines changed

7 files changed

+2034
-60
lines changed

‎Editor/ZOExportOBJ.cs‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -42,26 +42,26 @@ public static string MeshToString(MeshFilter meshFilter, Transform transform) {
4242
Vector3 v = r * nn;
4343
sb.Append(string.Format("vn {0} {1} {2}\n", -v.x, -v.y, v.z));
4444
}
45-
// sb.Append("\n");
46-
// foreach (Vector3 v in m.uv) {
47-
// sb.Append(string.Format("vt {0} {1}\n", v.x, v.y));
48-
// }
45+
sb.Append("\n");
46+
foreach (Vector3 v in m.uv) {
47+
sb.Append(string.Format("vt {0} {1}\n", v.x, v.y));
48+
}
4949

5050
for (int material = 0; material < m.subMeshCount; material++) {
5151
sb.Append("\n");
52-
//sb.Append("usemtl ").Append(mats[material].name).Append("\n");
53-
//sb.Append("usemap ").Append(mats[material].name).Append("\n");
52+
sb.Append("usemtl ").Append(mats[material].name).Append("\n");
53+
sb.Append("usemap ").Append(mats[material].name).Append("\n");
5454

5555
int[] triangles = m.GetTriangles(material);
5656
for (int i = 0; i < triangles.Length; i += 3) {
57-
// sb.Append(string.Format("f {0}/{0}/{0} {1}/{1}/{1} {2}/{2}/{2}\n",
58-
// triangles[i] + 1 + _startIndex,
59-
// triangles[i + 1] + 1 + _startIndex,
60-
// triangles[i + 2] + 1 + _startIndex));
61-
sb.Append(string.Format("f {0}//{0} {1}//{1} {2}//{2}\n", // pos, None, Norm
57+
sb.Append(string.Format("f {0}/{0}/{0} {1}/{1}/{1} {2}/{2}/{2}\n",
6258
triangles[i] + 1 + _startIndex,
6359
triangles[i + 1] + 1 + _startIndex,
6460
triangles[i + 2] + 1 + _startIndex));
61+
// sb.Append(string.Format("f {0}//{0} {1}//{1} {2}//{2}\n", // pos, None, Norm
62+
// triangles[i] + 1 + _startIndex,
63+
// triangles[i + 1] + 1 + _startIndex,
64+
// triangles[i + 2] + 1 + _startIndex));
6565

6666
}
6767
}

‎Runtime/Scripts/Document/ZOSimDocumentRoot.cs‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -258,7 +258,6 @@ public void ExportURDF(string exportDirectory) {
258258
XML = new XDocument(robot);
259259

260260
// go through the ZOSimOccurrences and convert into URDF Links and Joints
261-
List<XElement> links = new List<XElement>();
262261
foreach (Transform child in transform) {
263262
ZOSimOccurrence simOccurence = child.GetComponent<ZOSimOccurrence>();
264263
if (simOccurence) {
@@ -267,8 +266,6 @@ public void ExportURDF(string exportDirectory) {
267266
}
268267
}
269268

270-
robot.Add(links);
271-
272269
string urdfFilePath = Path.Combine(URDFExportDirectory, $"{Name}.urdf");
273270
XML.Save(urdfFilePath);
274271

‎Runtime/Scripts/Document/ZOSimOccurrence.cs‎

Lines changed: 28 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
using ZO.Sensors;
1515
using ZO.ROS.Unity;
1616
using ZO.ROS.Publisher;
17+
using ZO.Math;
1718

1819
namespace ZO.Document {
1920

@@ -135,7 +136,7 @@ public void Deserialize(ZOSimDocumentRoot documentRoot, JObject json) {
135136
Matrix4x4 matrix4X4 = new Matrix4x4(c0, c1, c2, c3);
136137
matrix4X4 = matrix4X4.transpose; // go from fusion360 row major to Unity column major
137138
rotation = ZO.Math.ZOMatrix4x4Util.GetRotation(matrix4X4);
138-
translation = ZO.Math.ZOMatrix4x4Util.GetTranslation(matrix4X4);
139+
translation = ZO.Math.ZOMatrix4x4Util.Position(matrix4X4);
139140
scale = ZO.Math.ZOMatrix4x4Util.GetScale(matrix4X4);
140141
}
141142

@@ -1016,35 +1017,36 @@ private static void DeserializeGeometricPrimitive(GameObject go, JObject primiti
10161017

10171018
public XElement XML { get; }
10181019

1019-
protected void BuildURDFVisuals(Transform trans, XElement link, XElement joint) {
1020+
protected void BuildURDFVisuals(Transform visualTransform, XElement link, XElement joint) {
10201021
// build 3d primitive if exists
1021-
MeshFilter meshFilter = trans.GetComponent<MeshFilter>();
1022+
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();
10221023
if (meshFilter) {
10231024

1024-
MeshRenderer meshRenderer = trans.GetComponent<MeshRenderer>();
1025+
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
10251026
Collider collider = null;
10261027
XElement visual = new XElement("visual");
1028+
visual.SetAttributeValue("name", visualTransform.name);
10271029
XElement geometry = new XElement("geometry");
10281030

10291031
if (meshFilter.sharedMesh.name.Contains("Cube")) {
10301032
XElement box = new XElement("box");
10311033

1032-
Vector3 boxSize = trans.localScale.Unity2RosScale();
1034+
Vector3 boxSize = visualTransform.localScale.Unity2RosScale();
10331035
box.SetAttributeValue("size", boxSize.ToXMLString());
10341036
geometry.Add(box);
10351037

1036-
collider = trans.GetComponent<BoxCollider>();
1038+
collider = visualTransform.GetComponent<BoxCollider>();
10371039
if (collider) {
10381040
//TODO: add box collider
10391041
}
10401042
}
10411043
if (meshFilter.sharedMesh.name.Contains("Sphere")) {
10421044
XElement sphere = new XElement("sphere");
1043-
float radius = trans.localScale.x / 2.0f;
1045+
float radius = visualTransform.localScale.x / 2.0f;
10441046
sphere.SetAttributeValue("radius", radius);
10451047
geometry.Add(sphere);
10461048

1047-
collider = trans.GetComponent<SphereCollider>();
1049+
collider = visualTransform.GetComponent<SphereCollider>();
10481050
if (collider) {
10491051
//TODO: add box collider
10501052
}
@@ -1061,14 +1063,14 @@ protected void BuildURDFVisuals(Transform trans, XElement link, XElement joint)
10611063
// }
10621064
if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
10631065
XElement cylinder = new XElement("cylinder");
1064-
float radius = trans.localScale.x / 2.0f;
1065-
float height = trans.localScale.y * 2.0f;
1066+
float radius = visualTransform.localScale.x / 2.0f;
1067+
float height = visualTransform.localScale.y * 2.0f;
10661068
cylinder.SetAttributeValue("radius", radius);
10671069
cylinder.SetAttributeValue("length", height);
10681070

10691071
geometry.Add(cylinder);
10701072

1071-
collider = trans.GetComponent<MeshCollider>();
1073+
collider = visualTransform.GetComponent<MeshCollider>();
10721074
if (collider) {
10731075
//TODO: add box collider
10741076
}
@@ -1077,10 +1079,10 @@ protected void BuildURDFVisuals(Transform trans, XElement link, XElement joint)
10771079

10781080
if (geometry.HasElements) {
10791081
// build origin
1080-
Vector3 xyz = trans.localPosition.Unity2Ros();
1081-
Vector3 rpy = new Vector3(-trans.localEulerAngles.z * Mathf.Deg2Rad,
1082-
trans.localEulerAngles.x * Mathf.Deg2Rad,
1083-
-trans.localEulerAngles.y * Mathf.Deg2Rad);
1082+
Vector3 xyz = visualTransform.localPosition.Unity2Ros();
1083+
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
1084+
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
1085+
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
10841086
// Vector3 xyz = jointTransform.position.Unity2Ros();
10851087
// Vector3 rpy = new Vector3(-transform.eulerAngles.z * Mathf.Deg2Rad,
10861088
// transform.eulerAngles.x * Mathf.Deg2Rad,
@@ -1089,6 +1091,7 @@ protected void BuildURDFVisuals(Transform trans, XElement link, XElement joint)
10891091
XElement origin = new XElement("origin");
10901092
origin.SetAttributeValue("xyz", xyz.ToXMLString());
10911093
origin.SetAttributeValue("rpy", rpy.ToXMLString());
1094+
10921095
visual.Add(origin);
10931096

10941097
visual.Add(geometry);
@@ -1157,14 +1160,16 @@ public void BuildURDFJoints(ZOSimDocumentRoot documentRoot, XElement robot, ZOSi
11571160
joint.SetAttributeValue("type", "fixed"); //HACK hardwired!
11581161
// build origin
11591162
Transform jointTransform = this.transform;
1160-
Vector3 xyz = jointTransform.localPosition.Unity2Ros();
1161-
Vector3 rpy = new Vector3(-jointTransform.localEulerAngles.z * Mathf.Deg2Rad,
1162-
jointTransform.localEulerAngles.x * Mathf.Deg2Rad,
1163-
-jointTransform.localEulerAngles.y * Mathf.Deg2Rad);
1164-
// Vector3 xyz = jointTransform.position.Unity2Ros();
1165-
// Vector3 rpy = new Vector3(-transform.eulerAngles.z * Mathf.Deg2Rad,
1166-
// transform.eulerAngles.x * Mathf.Deg2Rad,
1167-
// -transform.eulerAngles.y * Mathf.Deg2Rad);
1163+
// Vector3 xyz = jointTransform.localPosition.Unity2Ros();
1164+
// Vector3 rpy = new Vector3(-jointTransform.localEulerAngles.z * Mathf.Deg2Rad,
1165+
// jointTransform.localEulerAngles.x * Mathf.Deg2Rad,
1166+
// -jointTransform.localEulerAngles.y * Mathf.Deg2Rad);
1167+
1168+
// remember everything is relative to parent.
1169+
// TODO: THOUGH MAYBE IT IS RELATIVE TO PARENT JOINT WHICH MAY NOT BE THE SAME!!!
1170+
Matrix4x4 jointMatrix = this.transform.WorldTranslationRotationMatrix() * parent.transform.WorldTranslationRotationMatrix().inverse;
1171+
Vector3 xyz = jointMatrix.Position().Unity2Ros();
1172+
Vector3 rpy = jointMatrix.rotation.Unity2RosRollPitchYaw();
11681173

11691174
XElement origin = new XElement("origin");
11701175
origin.SetAttributeValue("xyz", xyz.ToXMLString());

‎Runtime/Scripts/Util/Extensions/ZOROSConversionExtensions.cs‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,12 @@ public static Quaternion Unity2Ros(this Quaternion q) {
2626
return new Quaternion(-q.z, q.x, -q.y, q.w);
2727
}
2828

29+
public static Vector3 Unity2RosRollPitchYaw(this Quaternion q) {
30+
return new Vector3(-q.eulerAngles.z * Mathf.Deg2Rad,
31+
q.eulerAngles.x * Mathf.Deg2Rad,
32+
-q.eulerAngles.y * Mathf.Deg2Rad);
33+
}
34+
2935
public static string ToXMLString(this Vector3 v) {
3036
return $"{v.x} {v.y} {v.z}";
3137
}

‎Runtime/Scripts/Util/Math/ZOMatrix4x4Util.cs‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
namespace ZO.Math {
44
public static class ZOMatrix4x4Util {
5-
public static Vector3 GetTranslation(this Matrix4x4 m) {
5+
public static Vector3 Position(this Matrix4x4 m) {
66
var col = m.GetColumn(3);
77
return new Vector3(col.x, col.y, col.z);
88
}
@@ -23,8 +23,25 @@ public static Quaternion GetRotation(this Matrix4x4 m) {
2323
return q;
2424
}
2525

26+
27+
/// <summary>
28+
/// Gets the scale vector for matrix
29+
/// </summary>
30+
/// <param name="m"></param>
31+
/// <returns>Vector3 scale vector</returns>
2632
public static Vector3 GetScale(this Matrix4x4 m) {
2733
return new Vector3(m.GetColumn(0).magnitude, m.GetColumn(1).magnitude, m.GetColumn(2).magnitude);
2834
}
35+
36+
/// <summary>
37+
/// Get the matrix from the Transform position and rotation while ignoring scale.
38+
/// See: Matrix4x4.TRS
39+
/// </summary>
40+
/// <param name="t"></param>
41+
/// <returns></returns>
42+
public static Matrix4x4 WorldTranslationRotationMatrix(this Transform t) {
43+
return Matrix4x4.TRS(t.position, t.rotation, Vector3.one);
44+
}
45+
2946
}
3047
}

0 commit comments

Comments
 (0)