Skip to content

Commit 4a2c470

Browse files
micahpearlmanmicahpearlman
committed
urdf fully standalone
1 parent dbd1f54 commit 4a2c470

File tree

2 files changed

+117
-219
lines changed

2 files changed

+117
-219
lines changed

‎Runtime/Scripts/Document/ZOSimOccurrence.cs‎

Lines changed: 0 additions & 216 deletions
Original file line numberDiff line numberDiff line change
@@ -1014,222 +1014,6 @@ private static void DeserializeGeometricPrimitive(GameObject go, JObject primiti
10141014

10151015
#endregion // ZOSerializationInterface
10161016

1017-
#region URDF
1018-
1019-
private List<Transform> _visualMeshesToExport = new List<Transform>();
1020-
private List<Transform> _collisionMeshesToExport = new List<Transform>();
1021-
protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
1022-
// build 3d primitive if exists
1023-
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();
1024-
if (meshFilter == null) {
1025-
// Check children
1026-
meshFilter = visualTransform.GetComponentInChildren<MeshFilter>();
1027-
}
1028-
if (meshFilter) {
1029-
1030-
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
1031-
Collider collider = null;
1032-
XElement visual = new XElement("visual");
1033-
visual.SetAttributeValue("name", visualTransform.name);
1034-
XElement geometry = new XElement("geometry");
1035-
1036-
if (meshFilter.sharedMesh.name.Contains("Cube")) {
1037-
XElement box = new XElement("box");
1038-
1039-
Vector3 boxSize = visualTransform.localScale.Unity2RosScale();
1040-
box.SetAttributeValue("size", boxSize.ToXMLString());
1041-
geometry.Add(box);
1042-
1043-
collider = visualTransform.GetComponent<BoxCollider>();
1044-
if (collider) {
1045-
//TODO: add box collider
1046-
}
1047-
} else if (meshFilter.sharedMesh.name.Contains("Sphere")) {
1048-
XElement sphere = new XElement("sphere");
1049-
float radius = visualTransform.localScale.x / 2.0f;
1050-
sphere.SetAttributeValue("radius", radius);
1051-
geometry.Add(sphere);
1052-
1053-
collider = visualTransform.GetComponent<SphereCollider>();
1054-
if (collider) {
1055-
//TODO: add box collider
1056-
}
1057-
1058-
} else if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
1059-
XElement cylinder = new XElement("cylinder");
1060-
float radius = visualTransform.localScale.x / 2.0f;
1061-
float height = visualTransform.localScale.y * 2.0f;
1062-
cylinder.SetAttributeValue("radius", radius);
1063-
cylinder.SetAttributeValue("length", height);
1064-
1065-
geometry.Add(cylinder);
1066-
1067-
collider = visualTransform.GetComponent<MeshCollider>();
1068-
if (collider) {
1069-
//TODO: add box collider
1070-
}
1071-
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);
1079-
}
1080-
1081-
if (geometry.HasElements) {
1082-
// build origin
1083-
Vector3 position = visualTransform.localPosition + anchorOffset;
1084-
Vector3 xyz = position.Unity2Ros();
1085-
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
1086-
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
1087-
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
1088-
1089-
XElement origin = new XElement("origin");
1090-
origin.SetAttributeValue("xyz", xyz.ToXMLString());
1091-
origin.SetAttributeValue("rpy", rpy.ToXMLString());
1092-
1093-
visual.Add(origin);
1094-
1095-
visual.Add(geometry);
1096-
link.Add(visual);
1097-
}
1098-
1099-
}
1100-
1101-
1102-
}
1103-
/// <summary>
1104-
/// URDF has a "flattened" hierarchy where the hierarchy is build by URDF joints.
1105-
/// </summary>
1106-
public void BuildURDFLink(XElement robot, Vector3 anchorOffset) {
1107-
1108-
XElement link = new XElement("link");
1109-
link.SetAttributeValue("name", Name);
1110-
robot.Add(link);
1111-
1112-
1113-
// build the visual elements
1114-
// go through the children
1115-
foreach (Transform child in transform) {
1116-
1117-
// check for any visuals
1118-
// NOTE: a gameobject named visuals is treated as a special container of visual objects
1119-
if (child.name.ToLower() == "visuals") {
1120-
// go through the children of the visuals and get all the models
1121-
foreach (Transform visualsChild in child) {
1122-
// check if it is a primitive type (cube, sphere, cylinder, etc)
1123-
BuildURDFVisuals(visualsChild, link, anchorOffset);
1124-
}
1125-
}
1126-
1127-
}
1128-
1129-
}
1130-
1131-
#if BLAHBLAH
1132-
protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, Matrix4x4 worldJointMatrix, ref List<URDFJoint> joints) {
1133-
1134-
// if we have a parent build a joint
1135-
if (parent) {
1136-
XElement jointX = new XElement("joint");
1137-
jointX.SetAttributeValue("name", $"{parent.Name}_to_{this.Name}");
1138-
// Transform jointTransform = this.transform;
1139-
Matrix4x4 jointMatrix = Matrix4x4.identity;
1140-
1141-
ZOHingeJoint hingeJoint = parent.GetComponent<ZOHingeJoint>();
1142-
if (hingeJoint != null) {
1143-
jointX.SetAttributeValue("type", "revolute");
1144-
1145-
// create axis
1146-
Vector3 axis = hingeJoint.UnityHingeJoint.axis.Unity2Ros();
1147-
XElement axisX = new XElement("axis");
1148-
axisX.SetAttributeValue("xyz", axis.ToXMLString());
1149-
jointX.Add(axisX);
1150-
1151-
// create limits
1152-
// TODO:
1153-
XElement limitX = new XElement("limit");
1154-
limitX.SetAttributeValue("effort", 10000f); // HACK
1155-
limitX.SetAttributeValue("velocity", 3.14f); // HACK
1156-
jointX.Add(limitX);
1157-
1158-
// Add the anchor position
1159-
jointMatrix = parent.transform.WorldTranslationRotationMatrix();
1160-
jointMatrix = jointMatrix.AddTranslation(hingeJoint.Anchor);
1161-
1162-
// save this off as the new world joint matrix
1163-
Matrix4x4 newWorldJointMatrix = jointMatrix;
1164-
1165-
// subtract out the parent root
1166-
jointMatrix = jointMatrix * worldJointMatrix.inverse;
1167-
worldJointMatrix = newWorldJointMatrix;
1168-
1169-
Vector3 xyz = jointMatrix.Position().Unity2Ros();
1170-
Vector3 rpy = jointMatrix.rotation.Unity2RosRollPitchYaw();
1171-
1172-
XElement origin = new XElement("origin");
1173-
origin.SetAttributeValue("xyz", xyz.ToXMLString());
1174-
origin.SetAttributeValue("rpy", rpy.ToXMLString());
1175-
jointX.Add(origin);
1176-
1177-
URDFJoint joint = new URDFJoint(this, parent, hingeJoint.Anchor, hingeJoint.ConnectedAnchor);
1178-
joints.Add(joint);
1179-
1180-
1181-
} else { // children of the parent even without an explicit joint are "fixed" joints
1182-
1183-
jointX.SetAttributeValue("type", "fixed");
1184-
jointMatrix = parent.transform.WorldTranslationRotationMatrix().inverse * this.transform.WorldTranslationRotationMatrix();
1185-
1186-
Vector3 xyz = jointMatrix.Position().Unity2Ros();
1187-
Vector3 rpy = jointMatrix.rotation.Unity2RosRollPitchYaw();
1188-
1189-
XElement origin = new XElement("origin");
1190-
origin.SetAttributeValue("xyz", xyz.ToXMLString());
1191-
origin.SetAttributeValue("rpy", rpy.ToXMLString());
1192-
jointX.Add(origin);
1193-
1194-
URDFJoint joint = new URDFJoint(this, parent, jointMatrix.Position(), Vector3.zero);
1195-
joints.Add(joint);
1196-
1197-
1198-
}
1199-
1200-
1201-
robot.Add(jointX);
1202-
1203-
XElement parentX = new XElement("parent");
1204-
parentX.SetAttributeValue("link", parent.Name);
1205-
jointX.Add(parentX);
1206-
1207-
XElement childX = new XElement("child");
1208-
childX.SetAttributeValue("link", this.Name);
1209-
jointX.Add(childX);
1210-
1211-
}
1212-
// recursively go through the children
1213-
foreach (Transform child in transform) {
1214-
ZOSimOccurrence simOccurrence = child.GetComponent<ZOSimOccurrence>();
1215-
if (simOccurrence) {
1216-
simOccurrence.BuildURDFJoints(robot, this, worldJointMatrix, ref joints);
1217-
}
1218-
}
1219-
1220-
}
1221-
1222-
#endif // 0
1223-
1224-
/// <summary>
1225-
/// Sets the state of the object from XML. XML could come from file or network or wherever.
1226-
/// </summary>
1227-
/// <param name="json"></param>
1228-
// void ZOURDFSerializationInterface.Deserialize(ZOSimDocumentRoot documentRoot, XElement xml) {
1229-
1230-
// }
1231-
1232-
#endregion // ZOURDFSerializationInterface
12331017

12341018
}
12351019
}

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

Lines changed: 117 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -72,12 +72,12 @@ public void BuildURDF(XElement robot, ZOSimOccurrence simOccurrence, Matrix4x4 b
7272
if (joints[0] == joint) { // if base joint do not apply any offset to parent link
7373
offset = Vector3.zero;
7474
}
75-
joint.Parent.BuildURDFLink(robot, offset);
75+
BuildURDFLink(joint.Parent, robot, offset);
7676
links.Add(joint.Parent);
7777
}
7878
if (links.Contains(joint.Child) == false) { // build child link if not exist
7979
Vector3 offset = -1.0f * joint.ConnectedAnchor;
80-
joint.Child.BuildURDFLink(robot, offset);
80+
BuildURDFLink(joint.Child, robot, offset);
8181
links.Add(joint.Child);
8282
}
8383
}
@@ -88,7 +88,7 @@ public void BuildURDF(XElement robot, ZOSimOccurrence simOccurrence, Matrix4x4 b
8888
link.SetAttributeValue("name", "World");
8989
robot.Add(link);
9090

91-
simOccurrence.BuildURDFLink(robot, Vector3.zero);
91+
BuildURDFLink(simOccurrence, robot, Vector3.zero);
9292

9393
XElement jointX = new XElement("joint");
9494
jointX.SetAttributeValue("name", $"World_to_{simOccurrence.Name}");
@@ -194,11 +194,125 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, ZOSimOccu
194194
BuildURDFJoints(robot, child, childOfChild, worldJointMatrix, ref joints);
195195
}
196196
}
197+
}
198+
199+
/// <summary>
200+
/// URDF has a "flattened" hierarchy where the hierarchy is build by URDF joints.
201+
/// </summary>
202+
protected void BuildURDFLink(ZOSimOccurrence simOccurrence, XElement robot, Vector3 anchorOffset) {
203+
204+
XElement link = new XElement("link");
205+
link.SetAttributeValue("name", simOccurrence.Name);
206+
robot.Add(link);
207+
208+
209+
// build the visual elements
210+
// go through the children
211+
foreach (Transform child in simOccurrence.transform) {
212+
213+
// check for any visuals
214+
// NOTE: a gameobject named visuals is treated as a special container of visual objects
215+
if (child.name.ToLower() == "visuals") {
216+
// go through the children of the visuals and get all the models
217+
foreach (Transform visualsChild in child) {
218+
// check if it is a primitive type (cube, sphere, cylinder, etc)
219+
BuildURDFVisuals(visualsChild, link, anchorOffset);
220+
}
221+
}
222+
223+
}
224+
}
225+
226+
227+
private List<Transform> _visualMeshesToExport = new List<Transform>();
228+
private List<Transform> _collisionMeshesToExport = new List<Transform>();
229+
protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
230+
// build 3d primitive if exists
231+
MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();
232+
if (meshFilter == null) {
233+
// Check children
234+
meshFilter = visualTransform.GetComponentInChildren<MeshFilter>();
235+
}
236+
if (meshFilter) {
237+
238+
MeshRenderer meshRenderer = visualTransform.GetComponent<MeshRenderer>();
239+
Collider collider = null;
240+
XElement visual = new XElement("visual");
241+
visual.SetAttributeValue("name", visualTransform.name);
242+
XElement geometry = new XElement("geometry");
243+
244+
if (meshFilter.sharedMesh.name.Contains("Cube")) {
245+
XElement box = new XElement("box");
246+
247+
Vector3 boxSize = visualTransform.localScale.Unity2RosScale();
248+
box.SetAttributeValue("size", boxSize.ToXMLString());
249+
geometry.Add(box);
250+
251+
collider = visualTransform.GetComponent<BoxCollider>();
252+
if (collider) {
253+
//TODO: add box collider
254+
}
255+
} else if (meshFilter.sharedMesh.name.Contains("Sphere")) {
256+
XElement sphere = new XElement("sphere");
257+
float radius = visualTransform.localScale.x / 2.0f;
258+
sphere.SetAttributeValue("radius", radius);
259+
geometry.Add(sphere);
260+
261+
collider = visualTransform.GetComponent<SphereCollider>();
262+
if (collider) {
263+
//TODO: add box collider
264+
}
265+
266+
} else if (meshFilter.sharedMesh.name.Contains("Cylinder")) {
267+
XElement cylinder = new XElement("cylinder");
268+
float radius = visualTransform.localScale.x / 2.0f;
269+
float height = visualTransform.localScale.y * 2.0f;
270+
cylinder.SetAttributeValue("radius", radius);
271+
cylinder.SetAttributeValue("length", height);
272+
273+
geometry.Add(cylinder);
274+
275+
collider = visualTransform.GetComponent<MeshCollider>();
276+
if (collider) {
277+
//TODO: add box collider
278+
}
279+
280+
} else { // regular mesh so export meshes as OBJ
281+
XElement mesh = new XElement("mesh");
282+
mesh.SetAttributeValue("filename", $"{visualTransform.name}.obj");
283+
mesh.SetAttributeValue("scale", visualTransform.localScale.ToXMLString());
284+
geometry.Add(mesh);
285+
286+
_visualMeshesToExport.Add(visualTransform);
287+
}
288+
289+
if (geometry.HasElements) {
290+
// build origin
291+
Vector3 position = visualTransform.localPosition + anchorOffset;
292+
Vector3 xyz = position.Unity2Ros();
293+
Vector3 rpy = new Vector3(-visualTransform.localEulerAngles.z * Mathf.Deg2Rad,
294+
visualTransform.localEulerAngles.x * Mathf.Deg2Rad,
295+
-visualTransform.localEulerAngles.y * Mathf.Deg2Rad);
296+
297+
XElement origin = new XElement("origin");
298+
origin.SetAttributeValue("xyz", xyz.ToXMLString());
299+
origin.SetAttributeValue("rpy", rpy.ToXMLString());
300+
301+
visual.Add(origin);
302+
303+
visual.Add(geometry);
304+
link.Add(visual);
305+
}
306+
307+
}
308+
197309

198310
}
199311

200312

201313

314+
315+
202316
}
203317

204318
}

0 commit comments

Comments
 (0)