public void ExportDefaultJointData_DefaultJoint_Succeeds()
{
Vector3 position = new Vector3(1, 2, 3);
Quaternion rotation = Quaternion.Euler(4, 5, 6);
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
linkObject.transform.position = position;
linkObject.transform.rotation = rotation;
Joint joint = UrdfJoint.ExportDefaultJoint(linkObject.transform);
Assert.AreEqual("base_link_joint", joint.name);
Assert.AreEqual("fixed", joint.type);
Assert.AreEqual(baseObject.name, joint.parent);
Assert.AreEqual(linkObject.name, joint.child);
Assert.AreEqual(new double[] { position[2], -position[0], position[1] }, joint.origin.Xyz);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[2] * Mathf.Deg2Rad, (float)joint.origin.Rpy[0]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(rotation.eulerAngles[0] * Mathf.Deg2Rad, (float)joint.origin.Rpy[1]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[1] * Mathf.Deg2Rad, (float)joint.origin.Rpy[2]);
Object.DestroyImmediate(baseObject);
Object.DestroyImmediate(linkObject);
}
private static Robot ExportRobotData(this UrdfRobot urdfRobot)
{
Robot robot = new Robot();
robot.ConstructFromFile(urdfRobot.FilePath, urdfRobot.gameObject.name);
List <string> linkNames = new List <string>();
foreach (UrdfLink urdfLink in urdfRobot.GetComponentsInChildren <UrdfLink>())
{
//Link export
if (linkNames.Contains(urdfLink.name))
{
EditorUtility.DisplayDialog("URDF Export Error",
"URDF export failed. There are several links with the name " +
urdfLink.name + ". Make sure all link names are unique before exporting this robot.",
"Ok");
return(null);
}
robot.links.Add(urdfLink.ExportLinkData());
linkNames.Add(urdfLink.name);
//Joint export
UrdfJoint urdfJoint = urdfLink.gameObject.GetComponent <UrdfJoint>();
if (urdfJoint != null)
{
robot.joints.Add(urdfJoint.ExportJointData());
}
else if (!urdfLink.IsBaseLink)
{
//Make sure that links with no rigidbodies are still connected to the robot by a default joint
robot.joints.Add(UrdfJoint.ExportDefaultJoint(urdfLink.transform));
}
}
robot.materials = UrdfMaterial.Materials.Values.ToList();
robot.plugins = urdfRobot.GetComponentInChildren <UrdfPlugins>().ExportPluginsData();
return(robot);
}
