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