Bug fix in setting transform of meshes during URDF Import (#15)

* Changing the orientation of the meshes by applying transform

* Update the function call to CorrectAxis

* Changing names of the variables for consistency

Author: vidurvij-Unity

Editor/CustomEditors/UrdfRobotEditor.cs

@@ -45,7 +45,7 @@ public override void OnInspectorGUI()
             GUILayout.Space(5);
             EditorGUILayout.PropertyField(axisType, new GUIContent("Axis Type"));
             serializedObject.ApplyModifiedProperties();
-            UrdfRobotExtensions.CorrectAxis(urdfRobot.gameObject, urdfRobot.choosenAxis);
+            UrdfRobotExtensions.CorrectAxis(urdfRobot.gameObject);
             GUILayout.Label("Helper Scripts", EditorStyles.boldLabel);
             DisplaySettingsToggle(new GUIContent("Controller Script"), urdfRobot.AddController);
             DisplaySettingsToggle(new GUIContent("Forward Kinematics Script"), urdfRobot.AddFkRobot);

Editor/NeedsRuntimeConversion/Extensions/UrdfRobotExtensions.cs

@@ -63,34 +63,36 @@ public static void Create(string filename, ImportSettings settings)
             Undo.RegisterCreatedObjectUndo(robotGameObject, "Create " + robotGameObject.name);
             Selection.activeObject = robotGameObject;
 
-            CorrectAxis(robotGameObject, settings.choosenAxis);
+            CorrectAxis(robotGameObject);
             CreateCollisionExceptions(robot, robotGameObject);
         }
 
-        public static void CorrectAxis(GameObject robot, ImportSettings.axisType axis = ImportSettings.axisType.yAxis)
+        public static void CorrectAxis(GameObject robot)
         {
-            UrdfVisual[] visualMeshList = robot.GetComponentsInChildren<UrdfVisual>();
-            UrdfCollision[] collisionMeshList = robot.GetComponentsInChildren<UrdfCollision>();
             UrdfRobot robotScript = robot.GetComponent<UrdfRobot>();
+            if (robotScript.CheckOrientation())
+                return;
+            Quaternion correctYtoZ = Quaternion.Euler(-90, 0, 90);
+            Quaternion correctZtoY = Quaternion.Inverse((correctYtoZ));
+            Quaternion correction = new Quaternion();
 
-            robotScript.choosenAxis = axis;
-            Quaternion correctZtoY = Quaternion.Euler(-90, 0, 90);
-            Quaternion correction = Quaternion.identity;
-
-            if (axis == ImportSettings.axisType.zAxis)
+            if (robotScript.choosenAxis == ImportSettings.axisType.zAxis)
+                correction = correctYtoZ;
+            else
                 correction = correctZtoY;
 
-
+            UrdfVisual[] visualMeshList = robot.GetComponentsInChildren<UrdfVisual>();
+            UrdfCollision[] collisionMeshList = robot.GetComponentsInChildren<UrdfCollision>();
             foreach (UrdfVisual visual in visualMeshList)
             {
-                visual.transform.localRotation = correction;
+                visual.transform.localRotation = visual.transform.localRotation * correction;
             }
 
             foreach (UrdfCollision collision in collisionMeshList)
             {
-                collision.transform.localRotation = correction;
+                collision.transform.localRotation = collision.transform.localRotation * correction;
             }
-
+            robotScript.SetOrientation();
         }
 
         private static void CreateCollisionExceptions(Robot robot, GameObject robotGameObject)

Runtime/UrdfComponents/UrdfRobot.cs

@@ -9,7 +9,8 @@ public enum GeometryTypes { Box, Cylinder, Sphere, Mesh }
     public class UrdfRobot : MonoBehaviour
     {
         public string FilePath;
-        public ImportSettings.axisType choosenAxis;
+        public ImportSettings.axisType choosenAxis ;
+        private ImportSettings.axisType currentOrientation = ImportSettings.axisType.yAxis;
         public List<CollisionIgnore> collisionExceptions;
         #region Configure Robot
 
@@ -49,6 +50,16 @@ public void ChangeToCorrectedSpace(bool rosSpace)
             this.transform.Rotate(0, 180, 0);
         }
 
+        public bool CheckOrientation()
+        {
+            return currentOrientation == choosenAxis;
+        }
+
+        public void SetOrientation()
+        {
+            currentOrientation = choosenAxis;
+        }
+
         public void AddController(bool controller)
         {
             if (controller && this.gameObject.GetComponent< RosSharp.Control.Controller>() == null)