IEnumerator DownloadRobot(string urdfPath, Dictionary <string, string> packages, URDFRobot ur) { using (UnityWebRequest www = UnityWebRequest.Get(urdfPath)) { yield return(www.SendWebRequest()); if (www.isNetworkError || www.isHttpError) { Debug.LogError(www.error); } else { Uri uri = new Uri(urdfPath); string workingPath = uri.Host + Path.GetDirectoryName(uri.PathAndQuery); URDFLoader.Options opt = new URDFLoader.Options() { workingPath = workingPath, loadMeshCb = (path, ext, done) => StartCoroutine(DownloadModel(path, ext, done)), target = ur }; URDFLoader.BuildRobot(www.downloadHandler.text, packages, opt); } } }
virtual protected URDFRobot CreateRobot(string urdf, Dictionary <string, string> packages) { URDFRobot ur = URDFLoader.LoadRobot(urdf, packages); ur.name = urdf; return(ur); }
public void ExportLink(bool zIsUp) { CreateBaseRefOrigin(zIsUp); MathTransform coordSysTransform = ActiveSWModel.Extension.GetCoordinateSystemTransformByName("Origin_global"); Matrix <double> GlobalTransform = MathOps.GetTransformation(coordSysTransform); LocalizeLink(URDFRobot.BaseLink, GlobalTransform); //Creating package directories URDFPackage package = new URDFPackage(PackageName, SavePath); package.CreateDirectories(); string meshFileName = package.MeshesDirectory + URDFRobot.BaseLink.Name + ".STL"; string windowsMeshFileName = package.WindowsMeshesDirectory + URDFRobot.BaseLink.Name + ".STL"; string windowsURDFFileName = package.WindowsRobotsDirectory + URDFRobot.Name + ".urdf"; string windowsManifestFileName = package.WindowsPackageDirectory + "manifest.xml"; //Creating manifest file PackageXMLWriter manifestWriter = new PackageXMLWriter(windowsManifestFileName); PackageXML Manifest = new PackageXML(URDFRobot.Name); Manifest.WriteElement(manifestWriter); //Customizing STL preferences to how I want them SaveUserPreferences(); SetSTLExportPreferences(); SetLinkSpecificSTLPreferences("", URDFRobot.BaseLink.STLQualityFine, ActiveSWModel); int errors = 0; int warnings = 0; //Saving part as STL mesh ActiveSWModel.Extension.SaveAs(windowsMeshFileName, (int)swSaveAsVersion_e.swSaveAsCurrentVersion, (int)swSaveAsOptions_e.swSaveAsOptions_Silent, null, ref errors, ref warnings); URDFRobot.BaseLink.Visual.Geometry.Mesh.Filename = meshFileName; URDFRobot.BaseLink.Collision.Geometry.Mesh.Filename = meshFileName; URDFRobot.BaseLink.Visual.Material.Texture.Filename = package.TexturesDirectory + Path.GetFileName(URDFRobot.BaseLink.Visual.Material.Texture.wFilename); string textureSavePath = package.WindowsTexturesDirectory + Path.GetFileName(URDFRobot.BaseLink.Visual.Material.Texture.wFilename); if (!String.IsNullOrWhiteSpace(URDFRobot.BaseLink.Visual.Material.Texture.wFilename)) { File.Copy(URDFRobot.BaseLink.Visual.Material.Texture.wFilename, textureSavePath, true); } //Writing URDF to file URDFWriter uWriter = new URDFWriter(windowsURDFFileName); //mRobot.addLink(mLink); URDFRobot.WriteURDF(uWriter.writer); ResetUserPreferences(); }
public override void OnInspectorGUI() { URDFRobot robot = (URDFRobot)target; // Options EditorGUILayout.LabelField("Options", EditorStyles.boldLabel); _useDeg = EditorGUILayout.Toggle("Edit in Degrees", _useDeg); _sort = EditorGUILayout.Toggle("Sort Alphabetically", _sort); _filter = EditorGUILayout.TextField("Filter", _filter); // Get the joints as a list so we can srot _list.Clear(); _list.AddRange(robot.joints.Keys); if (_sort) { _list.Sort(); } // Joints EditorGUILayout.Space(); EditorGUILayout.LabelField("Joints", EditorStyles.boldLabel); Regex re = new Regex(_filter, RegexOptions.ECMAScript | RegexOptions.IgnoreCase); foreach (string key in _list) { // If we don't match the regex, don't display this field if (_filter != "" && !re.IsMatch(key)) { continue; } // Display the joint fields EditorGUI.BeginChangeCheck(); float angle = robot.joints[key].angle * (_useDeg ? Mathf.Rad2Deg : 1); float newAngle = EditorGUILayout.FloatField(key, angle); if (EditorGUI.EndChangeCheck()) { newAngle *= (_useDeg ? Mathf.Deg2Rad : 1); robot.SetAngle(key, newAngle); } } }
void Awake() { Dictionary <string, string> packages = new Dictionary <string, string>(); foreach (var pkg in _packages) { string packagePath = pkg.path; if (packagePath.IndexOf("https://") != 0 && packagePath.IndexOf("https://") != 0) { packagePath = Path.Combine(Application.dataPath, packagePath); } packages[pkg.name] = packagePath; } string urdfPath = _fileName; if (urdfPath.IndexOf("https://") != 0 && urdfPath.IndexOf("https://") != 0) { urdfPath = Path.Combine(Application.dataPath, urdfPath); } robot = CreateRobot(urdfPath, packages); bool positive = _upAxis > 0; Axis axis = !positive ? (Axis)(-1 * (int)_upAxis) : _upAxis; Vector3 angles = Vector3.zero; if (axis == Axis.POS_X) { angles.x = positive ? 90 : -90; } if (axis == Axis.POS_Y) { angles.z = positive ? -90 : 90; } if (axis == Axis.POS_Z) { angles.x = positive ? 0 : 180; } robot.transform.rotation = Quaternion.Euler(angles); }
// Beginning method for exporting the full package public void ExportRobot(bool exportSTL = true) { //Setting up the progress bar logger.Info("Beginning the export process"); int progressBarBound = Common.GetCount(URDFRobot.BaseLink); iSwApp.GetUserProgressBar(out progressBar); progressBar.Start(0, progressBarBound, "Creating package directories"); //Creating package directories logger.Info("Creating package directories with name " + PackageName + " and save path " + SavePath); URDFPackage package = new URDFPackage(PackageName, SavePath); package.CreateDirectories(); URDFRobot.Name = PackageName; string windowsURDFFileName = package.WindowsRobotsDirectory + URDFRobot.Name + ".urdf"; string windowsCSVFileName = package.WindowsRobotsDirectory + URDFRobot.Name + ".csv"; string windowsPackageXMLFileName = package.WindowsPackageDirectory + "package.xml"; //Create CMakeLists logger.Info("Creating CMakeLists.txt at " + package.WindowsCMakeLists); package.CreateCMakeLists(); //Create Config joint names, not sure how this is used... logger.Info("Creating joint names config at " + package.WindowsConfigYAML); package.CreateConfigYAML(URDFRobot.GetJointNames(false)); //Creating package.xml file logger.Info("Creating package.xml at " + windowsPackageXMLFileName); PackageXMLWriter packageXMLWriter = new PackageXMLWriter(windowsPackageXMLFileName); PackageXML packageXML = new PackageXML(PackageName); packageXML.WriteElement(packageXMLWriter); //Creating RVIZ launch file Rviz rviz = new Rviz(PackageName, URDFRobot.Name + ".urdf"); logger.Info("Creating RVIZ launch file in " + package.WindowsLaunchDirectory); rviz.WriteFiles(package.WindowsLaunchDirectory); //Creating Gazebo launch file Gazebo gazebo = new Gazebo(URDFRobot.Name, PackageName, URDFRobot.Name + ".urdf"); logger.Info("Creating Gazebo launch file in " + package.WindowsLaunchDirectory); gazebo.WriteFile(package.WindowsLaunchDirectory); //Customizing STL preferences to how I want them logger.Info("Saving existing STL preferences"); SaveUserPreferences(); logger.Info("Modifying STL preferences"); SetSTLExportPreferences(); //Saving part as STL mesh AssemblyDoc assyDoc = (AssemblyDoc)ActiveSWModel; List <string> hiddenComponents = Common.FindHiddenComponents(assyDoc.GetComponents(false)); logger.Info("Found " + hiddenComponents.Count + " hidden components " + String.Join(", ", hiddenComponents)); logger.Info("Hiding all components"); ActiveSWModel.Extension.SelectAll(); ActiveSWModel.HideComponent2(); bool success = false; try { logger.Info("Beginning individual files export"); ExportFiles(URDFRobot.BaseLink, package, 0, exportSTL); success = true; } catch (Exception e) { logger.Error("An exception was thrown attempting to export the URDF", e); } finally { logger.Info("Showing all components except previously hidden components"); Common.ShowAllComponents(ActiveSWModel, hiddenComponents); logger.Info("Resetting STL preferences"); ResetUserPreferences(); } if (!success) { MessageBox.Show("Exporting the URDF failed unexpectedly. Email your maintainer " + "with the log file found at " + Logger.GetFileName()); return; } logger.Info("Writing URDF file to " + windowsURDFFileName); URDFWriter uWriter = new URDFWriter(windowsURDFFileName); URDFRobot.WriteURDF(uWriter.writer); ImportExport.WriteRobotToCSV(URDFRobot, windowsCSVFileName); logger.Info("Copying log file"); CopyLogFile(package); logger.Info("Resetting STL preferences"); ResetUserPreferences(); progressBar.End(); }
public static URDFRobot BuildRobot(string urdfContent, Dictionary <string, string> packages, Options options = new Options()) { if (options.loadMeshCb == null) { options.loadMeshCb = LoadMesh; } // load the XML doc XmlDocument doc = new XmlDocument(); doc.LoadXml(urdfContent); // Store the information about the link and the objects // indexed by link name Dictionary <string, XmlNode> xmlLinks = new Dictionary <string, XmlNode>(); Dictionary <string, XmlNode> xmlJoints = new Dictionary <string, XmlNode>(); // indexed by joint name Dictionary <string, URDFJoint> urdfJoints = new Dictionary <string, URDFJoint>(); Dictionary <string, URDFLink> urdfLinks = new Dictionary <string, URDFLink>(); // indexed by joint name Dictionary <string, string> parentNames = new Dictionary <string, string>(); // first node is the robot node (for the full robot) foreach (XmlNode n in doc.ChildNodes) { // first node is expected to be the robot // cycle through and find all the links for the robot first foreach (XmlNode xLink in n.ChildNodes) { if (xLink.Name == "link") { // Store the XML node for hte link xmlLinks.Add(xLink.Attributes["name"].Value, xLink); // create the link gameobject URDFLink urdfLink = new URDFLink(); urdfLink.name = xLink.Attributes["name"].Value; urdfLink.transform = new GameObject(urdfLink.name).transform; urdfLinks.Add(urdfLink.name, urdfLink); // Get the geometry node and skip it if there isn't one XmlNode[] visualNodes = GetXmlNodeChildrenByName(xLink, "visual"); List <GameObject> renderers = new List <GameObject>(); // Iterate over all the visual nodes foreach (var vn in visualNodes) { XmlNode geomNode = GetXmlNodeChildByName(vn, "geometry"); if (geomNode == null) { continue; } XmlNode matNode = GetXmlNodeChildByName(vn, "material"); Color col = Color.white; if (matNode != null) { XmlNode colNode = GetXmlNodeChildByName(matNode, "color"); if (colNode != null) { col = TupleToColor(colNode.Attributes["rgba"].Value); } // TODO: Load the textures // XmlNode texNode = GetXmlNodeChildByName(matNode, "texture"); // if (texNode != null) { } } // Get the mesh and the origin nodes XmlNode meshNode = GetXmlNodeChildByName(geomNode, "mesh"); XmlNode visOriginNode = GetXmlNodeChildByName(vn, "origin"); // take the visual origin and place on the renderer // use the visual origin to set the pose if necessary Vector3 visPos = Vector3.zero; if (visOriginNode != null && visOriginNode.Attributes["xyz"] != null) { visPos = TupleToVector3(visOriginNode.Attributes["xyz"].Value); } visPos = URDFToUnityPos(visPos); Vector3 visRot = Vector3.zero; if (visOriginNode != null && visOriginNode.Attributes["rpy"] != null) { visRot = TupleToVector3(visOriginNode.Attributes["rpy"].Value); } visRot = URDFToUnityRot(visRot); try { // try to load primitives if there is no mesh if (meshNode == null) { XmlNode primitiveNode = GetXmlNodeChildByName(geomNode, "box") ?? GetXmlNodeChildByName(geomNode, "sphere") ?? GetXmlNodeChildByName(geomNode, "cylinder"); if (primitiveNode != null) { GameObject go = null; switch (primitiveNode.Name) { case "box": go = GameObject.CreatePrimitive(PrimitiveType.Cube); go.transform.localScale = URDFToUnityPos(TupleToVector3(primitiveNode.Attributes[0].Value)); break; case "sphere": go = GameObject.CreatePrimitive(PrimitiveType.Sphere); go.transform.localScale = Vector3.one * (float.Parse(primitiveNode.Attributes[0].Value) * 2); break; case "cylinder": go = new GameObject(); var cPrimitive = GameObject.CreatePrimitive(PrimitiveType.Cylinder); cPrimitive.transform.parent = go.transform; var length = float.Parse(primitiveNode.Attributes[0].Value); var radius = float.Parse(primitiveNode.Attributes[1].Value); go.transform.localScale = new Vector3(radius * 2, length / 2, radius * 2); break; } Renderer r = go.GetComponent <Renderer>(); if (r == null) { r = go.GetComponentInChildren <Renderer>(); } go.transform.parent = urdfLink.transform; go.transform.localPosition = visPos; go.transform.localRotation = Quaternion.Euler(visRot); go.name = urdfLink.name + " geometry " + primitiveNode.Name; if (r) { r.material.color = col; renderers.Add(r.gameObject); if (Application.isPlaying) { Destroy(r.GetComponent <Collider>()); Destroy(r.GetComponent <Rigidbody>()); } else { DestroyImmediate(r.GetComponent <Collider>()); DestroyImmediate(r.GetComponent <Rigidbody>()); } } } } else { // load the STL file if possible // get the file path and split it string fileName = ResolveMeshPath(meshNode.Attributes["filename"].Value, packages, options.workingPath); Vector3 meshScale = Vector3.one; if (meshNode.Attributes["scale"] != null) { meshScale = TupleToVector3(meshNode.Attributes["scale"].Value); } meshScale = URDFToUnityScale(meshScale); // load all meshes string ext = Path.GetExtension(fileName).ToLower().Replace(".", ""); options.loadMeshCb(fileName, ext, models => { // create the rest of the meshes and child them to the click target for (int i = 0; i < models.Length; i++) { var trans = models[i].transform; trans.parent = urdfLink.transform; trans.localPosition = visPos; trans.localRotation = Quaternion.Euler(visRot); trans.localScale = meshScale; trans.name = urdfLink.name + " geometry " + i; foreach (Renderer r in trans.GetComponentsInChildren <Renderer>()) { r.material.color = col; } renderers.Add(trans.gameObject); // allows the urdf parser to be called from editor scripts outside of runtime without throwing errors // TODO: traverse over the children and do this if (Application.isPlaying) { Destroy(trans.GetComponent <Collider>()); Destroy(trans.GetComponent <Rigidbody>()); } else { DestroyImmediate(trans.GetComponent <Collider>()); DestroyImmediate(trans.GetComponent <Rigidbody>()); } } }); // save the geometry in the link urdfLink.geometry = renderers; } } catch (Exception e) { Debug.LogError("Error loading model for " + urdfLink.name + " : " + e.Message); } } } } // find all the joints next foreach (XmlNode xJoint in n.ChildNodes) { if (xJoint.Name == "joint") { string jointName = xJoint.Attributes["name"].Value; // store the joints indexed by child name so we can find it later // to properly indicate the parents in the joint list xmlJoints.Add(jointName, xJoint); // Get the links by name URDFLink parentLink = urdfLinks[GetXmlNodeChildByName(xJoint, "parent").Attributes["link"].Value]; URDFLink childLink = urdfLinks[GetXmlNodeChildByName(xJoint, "child").Attributes["link"].Value]; // Create the joint URDFJoint urdfJoint = new URDFJoint(); urdfJoint.name = jointName; urdfJoint.parentLink = parentLink; urdfJoint.transform = new GameObject(urdfJoint.name).transform; urdfJoint.type = xJoint.Attributes["type"].Value; // set the tree hierarchy // Parent the joint to its parent link urdfJoint.parentLink = parentLink; urdfJoint.transform.parent = parentLink.transform; parentLink.children.Add(urdfJoint); // Parent the child link to this joint urdfJoint.childLink = childLink; childLink.transform.parent = urdfJoint.transform; childLink.parent = urdfJoint; childLink.transform.localPosition = Vector3.zero; childLink.transform.localRotation = Quaternion.identity; // position the origin if it's specified XmlNode transNode = GetXmlNodeChildByName(xJoint, "origin"); Vector3 pos = Vector3.zero; if (transNode != null && transNode.Attributes["xyz"] != null) { pos = TupleToVector3(transNode.Attributes["xyz"].Value); } pos = URDFToUnityPos(pos); Vector3 rot = Vector3.zero; if (transNode != null && transNode.Attributes["rpy"] != null) { rot = TupleToVector3(transNode.Attributes["rpy"].Value); } rot = URDFToUnityRot(rot); // parent the joint and name it urdfJoint.transform.localPosition = pos; urdfJoint.transform.localRotation = Quaternion.Euler(rot); urdfJoint.originalRotation = urdfJoint.transform.localRotation; XmlNode axisNode = GetXmlNodeChildByName(xJoint, "axis"); if (axisNode != null) { Vector3 axis = TupleToVector3(axisNode.Attributes["xyz"].Value); axis.Normalize(); axis = URDFToUnityPos(axis); urdfJoint.axis = axis; } XmlNode limitNode = GetXmlNodeChildByName(xJoint, "limit"); if (limitNode != null) { if (limitNode.Attributes["lower"] != null) { urdfJoint.minAngle = float.Parse(limitNode.Attributes["lower"].Value); } if (limitNode.Attributes["upper"] != null) { urdfJoint.maxAngle = float.Parse(limitNode.Attributes["upper"].Value); } } // save the URDF joint urdfJoints.Add(urdfJoint.name, urdfJoint); } } } // loop through all the transforms until we find the one that has no parent URDFRobot robot = options.target; foreach (KeyValuePair <string, URDFLink> kv in urdfLinks) { // TODO : if there are multiple robots described, then we'll only be getting // the one. Should update to return a list of jointlists if necessary if (kv.Value.parent == null) { // find the top most node and add a joint list to it if (robot == null) { robot = kv.Value.transform.gameObject.AddComponent <URDFRobot>(); } else { kv.Value.transform.parent = robot.transform; kv.Value.transform.localPosition = Vector3.zero; kv.Value.transform.localRotation = Quaternion.identity; } robot.links = urdfLinks; robot.joints = urdfJoints; robot.IsConsistent(); return(robot); } } return(null); }
public static URDFRobot Parse(string urdfContent, Dictionary <string, string> packages, Options options = new Options()) { if (options.loadMeshCb == null) { options.loadMeshCb = LoadMesh; } // Parse the XML doc XmlDocument doc = new XmlDocument(); doc.LoadXml(urdfContent); // Store the information about the link and the objects indexed by link name Dictionary <string, XmlNode> xmlLinks = new Dictionary <string, XmlNode>(); Dictionary <string, XmlNode> xmlJoints = new Dictionary <string, XmlNode>(); // Indexed by joint name Dictionary <string, URDFJoint> urdfJoints = new Dictionary <string, URDFJoint>(); Dictionary <string, URDFLink> urdfLinks = new Dictionary <string, URDFLink>(); Dictionary <string, Material> urdfMaterials = new Dictionary <string, Material>(); // Indexed by joint name Dictionary <string, string> parentNames = new Dictionary <string, string>(); // First node is the <robot> node XmlNode robotNode = GetXmlNodeChildByName(doc, "robot"); string robotName = robotNode.Attributes["name"].Value; XmlNode[] xmlLinksArray = GetXmlNodeChildrenByName(robotNode, "link"); XmlNode[] xmlJointsArray = GetXmlNodeChildrenByName(robotNode, "joint"); XmlNode[] xmlMaterialsArray = GetXmlNodeChildrenByName(robotNode, "material", true); foreach (XmlNode materialNode in xmlMaterialsArray) { if (materialNode.Attributes["name"] != null) { string materialName = materialNode.Attributes["name"].Value; if (!urdfMaterials.ContainsKey(materialName)) { Material material = new Material(Shader.Find("Standard")); Color color = Color.white; XmlNode colorNode = GetXmlNodeChildByName(materialNode, "color"); if (colorNode != null) { color = TupleToColor(colorNode.Attributes["rgba"].Value); } material.color = color; urdfMaterials.Add(materialName, material); } } } // Cycle through the links and instantiate the geometry foreach (XmlNode linkNode in xmlLinksArray) { // Store the XML node for the link string linkName = linkNode.Attributes["name"].Value; xmlLinks.Add(linkName, linkNode); // create the link gameobject GameObject gameObject = new GameObject(linkName); URDFLink urdfLink = new URDFLink(); urdfLink.name = linkName; urdfLink.transform = gameObject.transform; urdfLinks.Add(linkName, urdfLink); // Get the geometry node and skip it if there isn't one XmlNode[] visualNodesArray = GetXmlNodeChildrenByName(linkNode, "visual"); List <GameObject> renderers = new List <GameObject>(); urdfLink.geometry = renderers; // Iterate over all the visual nodes foreach (XmlNode xmlVisual in visualNodesArray) { XmlNode geomNode = GetXmlNodeChildByName(xmlVisual, "geometry"); if (geomNode == null) { continue; } Material material = null; XmlNode materialNode = GetXmlNodeChildByName(xmlVisual, "material"); if (materialNode != null) { if (materialNode.Attributes["name"] != null) { string materialName = materialNode.Attributes["name"].Value; material = urdfMaterials[materialName]; } else { Color color = Color.white; XmlNode colorNode = GetXmlNodeChildByName(materialNode, "color"); if (colorNode != null) { color = TupleToColor(colorNode.Attributes["rgba"].Value); } material = new Material(Shader.Find("Standard")); material.color = color; // TODO: Load the textures // XmlNode texNode = GetXmlNodeChildByName(materialNode, "texture"); // if (texNode != null) { } } } // Get the mesh and the origin nodes XmlNode originNode = GetXmlNodeChildByName(xmlVisual, "origin"); // Extract the position and rotation of the mesh Vector3 position = Vector3.zero; if (originNode != null && originNode.Attributes["xyz"] != null) { position = TupleToVector3(originNode.Attributes["xyz"].Value); } position = URDFToUnityPos(position); Vector3 rotation = Vector3.zero; if (originNode != null && originNode.Attributes["rpy"] != null) { rotation = TupleToVector3(originNode.Attributes["rpy"].Value); } rotation = URDFToUnityRot(rotation); XmlNode meshNode = GetXmlNodeChildByName(geomNode, "mesh") ?? GetXmlNodeChildByName(geomNode, "box") ?? GetXmlNodeChildByName(geomNode, "sphere") ?? GetXmlNodeChildByName(geomNode, "cylinder"); try { if (meshNode.Name == "mesh") { // Extract the mesh path string fileName = ResolveMeshPath(meshNode.Attributes["filename"].Value, packages, options.workingPath); // Extract the scale from the mesh node Vector3 scale = Vector3.one; if (meshNode.Attributes["scale"] != null) { scale = TupleToVector3(meshNode.Attributes["scale"].Value); } scale = URDFToUnityScale(scale); // load all meshes string extension = Path.GetExtension(fileName).ToLower().Replace(".", ""); options.loadMeshCb(fileName, extension, models => { // create the rest of the meshes and child them to the click target for (int i = 0; i < models.Length; i++) { GameObject modelGameObject = models[i]; Transform meshTransform = modelGameObject.transform; // Capture the original local transforms before parenting in case the loader or model came in // with existing pose information and then apply our transform on top of it. Vector3 originalLocalPosition = meshTransform.localPosition; Quaternion originalLocalRotation = meshTransform.localRotation; Vector3 originalLocalScale = meshTransform.localScale; Vector3 transformedScale = originalLocalScale; transformedScale.x *= scale.x; transformedScale.y *= scale.y; transformedScale.z *= scale.z; meshTransform.parent = urdfLink.transform; meshTransform.localPosition = originalLocalPosition + position; meshTransform.localRotation = Quaternion.Euler(rotation) * originalLocalRotation; meshTransform.localScale = transformedScale; modelGameObject.name = urdfLink.name + " geometry " + i; renderers.Add(modelGameObject); // allows the urdf parser to be called from editor scripts outside of runtime without throwing errors // TODO: traverse over the children and do this if (Application.isPlaying) { Destroy(meshTransform.GetComponent <Collider>()); Destroy(meshTransform.GetComponent <Rigidbody>()); } else { DestroyImmediate(meshTransform.GetComponent <Collider>()); DestroyImmediate(meshTransform.GetComponent <Rigidbody>()); } } }); } else { // Instantiate the primitive geometry XmlNode primitiveNode = meshNode; GameObject primitiveGameObject = null; Transform primitiveTransform = null; switch (primitiveNode.Name) { case "box": { primitiveGameObject = GameObject.CreatePrimitive(PrimitiveType.Cube); primitiveTransform = primitiveGameObject.transform; Vector3 boxScale = TupleToVector3(primitiveNode.Attributes["size"].Value); boxScale = URDFToUnityPos(boxScale); primitiveTransform.localScale = boxScale; break; } case "sphere": { primitiveGameObject = GameObject.CreatePrimitive(PrimitiveType.Sphere); primitiveTransform = primitiveGameObject.transform; float sphereRadius = float.Parse(primitiveNode.Attributes["radius"].Value); primitiveTransform.localScale = Vector3.one * sphereRadius * 2; break; } case "cylinder": { primitiveGameObject = new GameObject(); primitiveTransform = primitiveGameObject.transform; GameObject cylinderPrimitive = GameObject.CreatePrimitive(PrimitiveType.Cylinder); cylinderPrimitive.transform.parent = primitiveTransform; float length = float.Parse(primitiveNode.Attributes["length"].Value); float radius = float.Parse(primitiveNode.Attributes["radius"].Value); primitiveTransform.localScale = new Vector3(radius * 2, length / 2, radius * 2); break; } } // Position the transform primitiveTransform.parent = urdfLink.transform; primitiveTransform.localPosition = position; primitiveTransform.localRotation = Quaternion.Euler(rotation); primitiveGameObject.name = urdfLink.name + " geometry " + primitiveNode.Name; Renderer primitiveRenderer = primitiveGameObject.GetComponent <Renderer>(); if (primitiveRenderer == null) { primitiveRenderer = primitiveGameObject.GetComponentInChildren <Renderer>(); } if (material != null) { primitiveRenderer.material = material; } renderers.Add(primitiveGameObject); // Dispose of unneeded components if (Application.isPlaying) { Destroy(primitiveRenderer.GetComponent <Collider>()); Destroy(primitiveRenderer.GetComponent <Rigidbody>()); } else { DestroyImmediate(primitiveRenderer.GetComponent <Collider>()); DestroyImmediate(primitiveRenderer.GetComponent <Rigidbody>()); } } } catch (Exception e) { Debug.LogError("Error loading model for " + urdfLink.name + " : " + e.Message); } } } // Cycle through the joint nodes foreach (XmlNode jointNode in xmlJointsArray) { string jointName = jointNode.Attributes["name"].Value; // store the joints indexed by child name so we can find it later // to properly indicate the parents in the joint list xmlJoints.Add(jointName, jointNode); // Get the links by name XmlNode parentNode = GetXmlNodeChildByName(jointNode, "parent"); XmlNode childNode = GetXmlNodeChildByName(jointNode, "child"); string parentName = parentNode.Attributes["link"].Value; string childName = childNode.Attributes["link"].Value; URDFLink parentLink = urdfLinks[parentName]; URDFLink childLink = urdfLinks[childName]; // Create the joint GameObject jointGameObject = new GameObject(jointName); URDFJoint urdfJoint = new URDFJoint(); urdfJoint.name = jointName; urdfJoint.parentLink = parentLink; urdfJoint.transform = jointGameObject.transform; urdfJoint.type = jointNode.Attributes["type"].Value; // Set the tree hierarchy // Parent the joint to its parent link urdfJoint.parentLink = parentLink; urdfJoint.transform.parent = parentLink.transform; parentLink.children.Add(urdfJoint); // Parent the child link to this joint urdfJoint.childLink = childLink; childLink.transform.parent = urdfJoint.transform; childLink.parent = urdfJoint; childLink.transform.localPosition = Vector3.zero; childLink.transform.localRotation = Quaternion.identity; // Position the origin if it's specified XmlNode transformNode = GetXmlNodeChildByName(jointNode, "origin"); Vector3 position = Vector3.zero; if (transformNode != null && transformNode.Attributes["xyz"] != null) { position = TupleToVector3(transformNode.Attributes["xyz"].Value); } position = URDFToUnityPos(position); Vector3 rotation = Vector3.zero; if (transformNode != null && transformNode.Attributes["rpy"] != null) { rotation = TupleToVector3(transformNode.Attributes["rpy"].Value); } rotation = URDFToUnityRot(rotation); // parent the joint and name it urdfJoint.transform.localPosition = position; urdfJoint.transform.localRotation = Quaternion.Euler(rotation); urdfJoint.originalRotation = Quaternion.Euler(rotation); XmlNode axisNode = GetXmlNodeChildByName(jointNode, "axis"); if (axisNode != null) { Vector3 axis = TupleToVector3(axisNode.Attributes["xyz"].Value); axis = URDFToUnityPos(axis); axis.Normalize(); urdfJoint.axis = axis; } XmlNode limitNode = GetXmlNodeChildByName(jointNode, "limit"); if (limitNode != null) { // Use double.parse to handle particularly large values. if (limitNode.Attributes["lower"] != null) { urdfJoint.minAngle = (float)double.Parse(limitNode.Attributes["lower"].Value); } if (limitNode.Attributes["upper"] != null) { urdfJoint.maxAngle = (float)double.Parse(limitNode.Attributes["upper"].Value); } } // save the URDF joint urdfJoints.Add(urdfJoint.name, urdfJoint); } // loop through all the transforms until we find the one that has no parent URDFRobot robot = options.target; foreach (KeyValuePair <string, URDFLink> kv in urdfLinks) { if (kv.Value.parent == null) { // find the top most node and add a joint list to it if (robot == null) { robot = kv.Value.transform.gameObject.AddComponent <URDFRobot>(); } else { kv.Value.transform.parent = robot.transform; kv.Value.transform.localPosition = Vector3.zero; kv.Value.transform.localRotation = Quaternion.identity; } robot.links = urdfLinks; robot.joints = urdfJoints; robot.IsConsistent(); return(robot); } } robot.name = robotName; return(null); }