private GameObject CreateFromData(URDFData data)
{
Transform actor = new GameObject(data.Name).transform;
actor.position = new Vector3(0f, 0f, 0f);
actor.rotation = Quaternion.identity;
List <Transform> Links = new List <Transform>();
List <Transform> Joints = new List <Transform>();
//Create Link Transforms
for (int i = 0; i < data.Links.Count; i++)
{
Transform link = CreateGeometry(data.Links[i].Geometry).transform;
link.name = data.Links[i].Name;
link.SetParent(actor, false);
Links.Add(link);
}
//Create Joint Transforms
for (int i = 0; i < data.Joints.Count; i++)
{
Transform joint = new GameObject().transform;
joint.name = data.Joints[i].Name;
joint.SetParent(actor);
Joints.Add(joint);
}
//Apply Parent-Child Relations
for (int i = 0; i < Joints.Count; i++)
{
Transform joint = Joints[i];
Transform parent = FindTransformByName(Links, data.GetJointData(joint.name).Parent);
Transform child = FindTransformByName(Links, data.GetJointData(joint.name).Child);
Transform parentJoint = actor;
string parentName = data.GetLinkData(parent.name).Name;
for (int j = 0; j < Joints.Count; j++)
{
if (data.GetJointData(Joints[j].name).Child == parentName)
{
parentJoint = Joints[j];
break;
}
}
joint.SetParent(parentJoint);
child.SetParent(joint);
}
Links = GetOrderedTransforms(actor.root, Links, new List <Transform>());
Joints = GetOrderedTransforms(actor.root, Joints, new List <Transform>());
for (int i = 0; i < Joints.Count; i++)
{
Transform joint = Joints[i];
Vector3 angles = -Mathf.Rad2Deg * ROSToUnity(data.GetJointData(joint.name).OriginRPY);
Quaternion rotation = Quaternion.Euler(angles);
joint.position = joint.parent.position + joint.parent.rotation * ROSToUnity(data.GetJointData(joint.name).OriginXYZ);
joint.rotation = joint.parent.rotation * rotation;
}
for (int i = 0; i < Links.Count; i++)
{
Transform link = Links[i];
Vector3 angles = -Mathf.Rad2Deg * ROSToUnity(data.GetLinkData(link.name).RPY);
Quaternion rotation = Quaternion.Euler(angles);
link.localPosition += ROSToUnity(data.GetLinkData(link.name).XYZ);
link.localRotation = rotation * link.localRotation;
}
//Initialize Links
for (int i = 0; i < Links.Count; i++)
{
//Nothing to do.
}
//Initialize Joints
for (int i = 0; i < Joints.Count; i++)
{
BioIK.KinematicJoint joint = Joints[i].gameObject.AddComponent <BioIK.KinematicJoint>();
URDFData.JointData jointData = data.GetJointData(joint.name);
if (jointData.Type == "fixed")
{
//Nothing to do
}
else
{
switch (jointData.Type)
{
case "prismatic":
joint.SetJointType(BioIK.JointType.Prismatic);
break;
case "revolute":
joint.SetJointType(BioIK.JointType.Revolute);
break;
case "continuous":
joint.SetJointType(BioIK.JointType.Continuous);
break;
}
joint.SetAnchor(Vector3.zero);
Vector3 axis = ROSToUnity(jointData.Axis);
if (joint.GetJointType() != BioIK.JointType.Prismatic)
{
axis = -axis;
}
joint.SetOrientation(Quaternion.FromToRotation(Vector3.right, axis).eulerAngles);
//joint.SetMaximumVelocity(jointData.Velocity);
//joint.SetMaximumAcceleration(jointData.Velocity);
BioIK.Motion motion = joint.GetXMotion();
motion.SetEnabled(true);
motion.SetLowerLimit(jointData.LowerLimit);
motion.SetUpperLimit(jointData.UpperLimit);
joint.Initialise();
}
}
if (Errors == 0)
{
Debug.Log("Successfully imported '" + actor.name + "'.");
}
else
{
Debug.Log(Errors + " errors or warnings during importing '" + actor.name + "'.\n" + Output);
}
Output = string.Empty;
Errors = 0;
return(actor.gameObject);
}
public URDFData(string path)
{
Failed = false;
AbsolutePath = String.Empty;
//RelativePath = String.Empty;
//AbsoluteFolder = String.Empty;
//RelativeFolder = string.Empty;
Materials = new List <MaterialData>();
Links = new List <LinkData>();
Joints = new List <JointData>();
RelativePath = path;
AbsolutePath = Application.dataPath + "/" + RelativePath;
RelativeFolder = RelativePath.Substring(0, RelativePath.LastIndexOf("/") + 1);
AbsoluteFolder = Application.dataPath + "/" + RelativeFolder;
XmlDocument file = new XmlDocument();
try {
file.Load(AbsolutePath);
} catch (Exception e) {
Debug.Log(e.Message);
Failed = true;
return;
}
XmlNode robotNode = null;
for (int i = 0; i < file.ChildNodes.Count; i++)
{
if (file.ChildNodes[i].Name == "robot")
{
robotNode = file.ChildNodes[i];
Name = robotNode.Attributes.GetNamedItem("name").Value;
}
}
//Read all first-layer childs of <robot ...>
for (int i = 0; i < robotNode.ChildNodes.Count; i++)
{
//Read Material
if (robotNode.ChildNodes[i].Name == "material")
{
XmlNode materialNode = robotNode.ChildNodes[i];
URDFData.MaterialData materialData = new URDFData.MaterialData(materialNode.Attributes.GetNamedItem("name").Value);
for (int j = 0; j < materialNode.ChildNodes.Count; j++)
{
if (materialNode.ChildNodes[j].Name == "color")
{
XmlNode colorNode = materialNode.ChildNodes[j];
XmlNode rgba = colorNode.Attributes.GetNamedItem("rgba");
if (rgba != null)
{
materialData.Color = ReadColor(rgba.Value);
}
}
if (materialNode.ChildNodes[j].Name == "texture")
{
XmlNode textureNode = materialNode.ChildNodes[j];
XmlNode fileName = textureNode.Attributes.GetNamedItem("filename");
if (fileName != null)
{
string texturePath = fileName.Value;
texturePath = texturePath.Substring(10);
texturePath = "Assets/" + RelativeFolder + texturePath;
materialData.Texture = texturePath;
}
}
}
Materials.Add(materialData);
}
//Read Link
if (robotNode.ChildNodes[i].Name == "link")
{
XmlNode linkNode = robotNode.ChildNodes[i];
//Multiple instances of visual are possible
string name = linkNode.Attributes.GetNamedItem("name").Value;
URDFData.LinkData linkData = null;
if (GetLinkData(name) != null)
{
linkData = GetLinkData(name);
}
else
{
linkData = new URDFData.LinkData(name);
}
bool visual = false;
bool inertial = false;
for (int j = 0; j < linkNode.ChildNodes.Count; j++)
{
if (linkNode.ChildNodes[j].Name == "visual" && !visual)
{
visual = true;
XmlNode visualNode = linkNode.ChildNodes[j];
for (int k = 0; k < visualNode.ChildNodes.Count; k++)
{
if (visualNode.ChildNodes[k].Name == "geometry")
{
XmlNode geometryNode = visualNode.ChildNodes[k];
for (int l = 0; l < geometryNode.ChildNodes.Count; l++)
{
if (geometryNode.ChildNodes[l].Name == "box")
{
linkData.Geometry = new URDFData.Box(ReadVector3(geometryNode.ChildNodes[l].Attributes.GetNamedItem("size").Value));
}
else if (geometryNode.ChildNodes[l].Name == "cylinder")
{
linkData.Geometry = new URDFData.Cylinder(ReadFloat(geometryNode.ChildNodes[l].Attributes.GetNamedItem("length").Value), ReadFloat(geometryNode.ChildNodes[l].Attributes.GetNamedItem("radius").Value));
}
else if (geometryNode.ChildNodes[l].Name == "sphere")
{
linkData.Geometry = new URDFData.Sphere(ReadFloat(geometryNode.ChildNodes[l].Attributes.GetNamedItem("radius").Value));
}
else if (geometryNode.ChildNodes[l].Name == "mesh")
{
string meshPath = geometryNode.ChildNodes[l].Attributes.GetNamedItem("filename").Value;
meshPath = meshPath.Substring(10);
meshPath = "Assets/" + RelativeFolder + meshPath;
XmlNode scaleNode = geometryNode.ChildNodes[l].Attributes.GetNamedItem("scale");
if (scaleNode != null)
{
//toLower
linkData.Geometry = new URDFData.Mesh(meshPath, ReadVector3(scaleNode.Value));
}
else
{
//toLower
linkData.Geometry = new URDFData.Mesh(meshPath, Vector3.one);
}
}
}
}
if (visualNode.ChildNodes[k].Name == "material")
{
XmlNode materialNode = visualNode.ChildNodes[k];
linkData.Material = materialNode.Attributes.GetNamedItem("name").Value;
if (materialNode.HasChildNodes)
{
URDFData.MaterialData materialData = new URDFData.MaterialData(linkData.Material);
for (int l = 0; l < materialNode.ChildNodes.Count; l++)
{
if (materialNode.ChildNodes[l].Name == "color")
{
XmlNode colorNode = materialNode.ChildNodes[l];
XmlNode rgba = colorNode.Attributes.GetNamedItem("rgba");
if (rgba != null)
{
materialData.Color = ReadColor(rgba.Value);
}
}
if (materialNode.ChildNodes[l].Name == "texture")
{
XmlNode textureNode = materialNode.ChildNodes[l];
XmlNode fileName = textureNode.Attributes.GetNamedItem("filename");
if (fileName != null)
{
string texturePath = fileName.Value;
texturePath = texturePath.Substring(10);
texturePath = "Assets/" + RelativeFolder + texturePath;
materialData.Texture = texturePath;
}
}
}
Materials.Add(materialData);
}
}
if (visualNode.ChildNodes[k].Name == "origin")
{
XmlNode originNode = visualNode.ChildNodes[k];
XmlNode xyz = originNode.Attributes.GetNamedItem("xyz");
if (xyz != null)
{
linkData.XYZ = ReadVector3(xyz.Value);
}
XmlNode rpy = originNode.Attributes.GetNamedItem("rpy");
if (rpy != null)
{
linkData.RPY = ReadVector3(rpy.Value);
}
}
}
}
if (linkNode.ChildNodes[j].Name == "inertial" && !inertial)
{
inertial = true;
XmlNode inertialNode = linkNode.ChildNodes[j];
for (int k = 0; k < inertialNode.ChildNodes.Count; k++)
{
if (inertialNode.ChildNodes[k].Name == "mass")
{
XmlNode massNode = inertialNode.ChildNodes[k];
linkData.Mass = ReadFloat(massNode.Attributes.GetNamedItem("value").Value);
}
}
}
}
Links.Add(linkData);
}
//Read Joint
if (robotNode.ChildNodes[i].Name == "joint")
{
XmlNode jointNode = robotNode.ChildNodes[i];
URDFData.JointData jointData = new URDFData.JointData(jointNode.Attributes.GetNamedItem("name").Value, jointNode.Attributes.GetNamedItem("type").Value);
for (int j = 0; j < jointNode.ChildNodes.Count; j++)
{
if (jointNode.ChildNodes[j].Name == "parent")
{
jointData.Parent = jointNode.ChildNodes[j].Attributes.GetNamedItem("link").Value;
}
if (jointNode.ChildNodes[j].Name == "child")
{
jointData.Child = jointNode.ChildNodes[j].Attributes.GetNamedItem("link").Value;
}
if (jointNode.ChildNodes[j].Name == "axis")
{
XmlNode xyz = jointNode.ChildNodes[j].Attributes.GetNamedItem("xyz");
if (xyz != null)
{
jointData.Axis = ReadVector3(xyz.Value);
}
}
if (jointNode.ChildNodes[j].Name == "origin")
{
XmlNode xyz = jointNode.ChildNodes[j].Attributes.GetNamedItem("xyz");
if (xyz != null)
{
jointData.OriginXYZ = ReadVector3(xyz.Value);
}
XmlNode rpy = jointNode.ChildNodes[j].Attributes.GetNamedItem("rpy");
if (rpy != null)
{
jointData.OriginRPY = ReadVector3(rpy.Value);
}
}
if (jointNode.ChildNodes[j].Name == "limit")
{
XmlNode effort = jointNode.ChildNodes[j].Attributes.GetNamedItem("effort");
if (effort != null)
{
jointData.Force = ReadFloat(effort.Value);
}
XmlNode velocity = jointNode.ChildNodes[j].Attributes.GetNamedItem("velocity");
if (velocity != null)
{
jointData.Velocity = ReadFloat(velocity.Value);
}
XmlNode lower = jointNode.ChildNodes[j].Attributes.GetNamedItem("lower");
if (lower != null)
{
jointData.LowerLimit = ReadFloat(lower.Value);
}
XmlNode upper = jointNode.ChildNodes[j].Attributes.GetNamedItem("upper");
if (upper != null)
{
jointData.UpperLimit = ReadFloat(upper.Value);
}
}
}
Joints.Add(jointData);
}
}
}
