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);
}
