Initial commit for Unity driven URDF Importer changes.

Author: mpinol

UnityEditorScripts/Extensions.meta

@@ -1,17 +1,4 @@
-/*
-© Siemens AG, 2017-2018
-Author: Dr. Martin Bischoff (martin.bischoff@siemens.com)
-
-Licensed under the Apache License, Version 2.0 (the "License");
-you may not use this file except in compliance with the License.
-You may obtain a copy of the License at
-<http://www.apache.org/licenses/LICENSE-2.0>.
-Unless required by applicable law or agreed to in writing, software
-distributed under the License is distributed on an "AS IS" BASIS,
-WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-See the License for the specific language governing permissions and
-limitations under the License.
-*/
+ 
 
 using System;
 using System.IO;
@@ -83,6 +70,15 @@ public static void MoveChildTransformToParent(this Transform parent, bool transf
             childTransform.localScale = Vector3.one;
         }
 
+        public static double[] ToRosRPY(this Vector3 transform)
+        {
+            Vector3 rpyVector = new Vector3(
+                -transform.z * Mathf.Deg2Rad,
+                transform.x * Mathf.Deg2Rad,
+                -transform.y * Mathf.Deg2Rad);
+            return rpyVector == Vector3.zero ? null : rpyVector.ToRoundedDoubleArray();
+        }
+
         public static Vector3 Ros2Unity(this Vector3 vector3)
         {
             return new Vector3(-vector3.y, vector3.z, vector3.x);
@@ -131,5 +127,103 @@ public static string SetSeparatorChar(this string path)
         {
             return path.Replace(Path.AltDirectorySeparatorChar, Path.DirectorySeparatorChar);
         }
+
+        /// <summary>
+        /// This function is written to compare two double arrays.
+        /// It returns true if each number respective number in the array is equal to within a delta amount to each other.
+        /// Vector3 == Vector3 only allows for a difference of 1e-5
+        /// </summary>
+        /// <param name="array">First double array</param>
+        /// <param name="array2">Second Double array</param>
+        /// <param name="delta">Allowed delta between numbers allowed</param>
+        /// <returns></returns>
+        public static bool DoubleDeltaCompare(this double[] array, double[] array2, double delta)
+        {
+            if (array.Length != array2.Length)
+                return false;
+            for (int i = 0; i < array.Length ; i++)
+            {
+                if ((array[i] >= array2[i] - delta) && (array[i] <= array2[i] + delta))
+                    continue;
+                else
+                    return false;
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// This function is written to compare two Vector3.
+        /// It returns true if each number respective number in the array is equal to within a delta amount to each other.
+        /// </summary>
+        /// <param name="array">First Vector3</param>
+        /// <param name="array2">Second Vector3</param>
+        /// <param name="delta">Allowed delta between numbers allowed</param>
+        /// <returns></returns>
+        public static bool VectorEqualDelta(this Vector3 source, Vector3 exported, double delta)
+        {
+            
+            return Vector3.SqrMagnitude(source - exported) < delta;
+        }
+
+        public static bool EqualsDelta(this double first, double second, double delta)
+        {
+            if (Math.Abs(first - second) <= Math.Abs(delta * first))
+                return true;
+            else
+                return false;
+        }
+
+        /// <summary>
+        /// Implments element-wise subtraction between two matrices
+        /// </summary>
+        /// <param name="first"></param>
+        /// <param name="second"></param>
+        /// <returns></returns>
+        public static Matrix4x4 Subtract(this Matrix4x4 first, Matrix4x4 second)
+        {
+            Matrix4x4 result = new Matrix4x4();
+            for(int i = 0; i < 4; i++)
+            {
+                for(int j = 0; j < 4; j++)
+                {
+                    result[i, j] = first[i, j] - second[i, j];
+                }
+            }
+            return result;
+        }
+
+        /// <summary>
+        /// Divides each float number of a Matrix4x4 type with a float scalar.
+        /// The function is added as it does not support element-wise division by float
+        /// https://docs.unity3d.com/ScriptReference/Matrix4x4.html
+        /// </summary>
+        /// <param name="first">Matrix divisor</param>
+        /// <param name="second">Float dividend</param>
+        /// <returns></returns>
+        public static Matrix4x4 FloatDivide(this Matrix4x4 first, float second)
+        {
+            Matrix4x4 result = new Matrix4x4();
+            for (int i = 0; i < 4; i++)
+            {
+                for (int j = 0; j < 4; j++)
+                {
+                    result[i, j] = first[i, j] / second;
+                }
+            }
+            return result;
+        }
+
+        /// <summary>
+        /// Function overload of ToString method of Matrix3x3 to achieve unrounded print of the matrix
+        /// </summary>
+        /// <param name="first">Matrix to be printed</param>
+        /// <returns></returns>
+        public static string ToString(this Matrix4x4 first)
+        {
+            return ("Matrix : " + first[0, 0] + " " + first[0, 1] + " " + first[0, 2] + " " + first[0, 3] + " " +
+                first[1, 0] + " " + first[1, 1] + " " + first[1, 2] + " " + first[1, 3] + " " +
+                first[2, 0] + " " + first[2, 1] + " " + first[2, 2] + " " + first[2, 3] + " " +
+                first[3, 0] + " " + first[3, 1] + " " + first[3, 2] + " " + first[3, 3]);
+        }
     }
 }

UnityEditorScripts/Extensions/HeaderExtensions.cs

@@ -0,0 +1,170 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace RosSharp.Control
+{
+    public enum RotationDirection { None = 0, Positive = 1, Negative = -1 };
+    public enum ControlType { PositionControl};
+
+    public class Controller : MonoBehaviour
+    {
+
+        private ArticulationBody[] articulationChain;
+        private Color[] prevColor;
+        private int previousIndex;
+
+        public ControlType control = ControlType.PositionControl;
+        public int selectedIndex;
+        public string jointName;
+        public float stiffness;
+        public float damping;
+        public float R, G, B, Alpha;
+        public float speed = 5f; // Units: degree/s
+        public float torque = 100f; // Units: Nm or N
+        public float acceleration = 5f;// Units: m/s^2 / degree/s^2
+
+        void Start()
+        {
+            previousIndex = selectedIndex = 1;
+            this.gameObject.AddComponent<FKRobot>();
+            articulationChain = this.GetComponentsInChildren<ArticulationBody>();
+
+            foreach (ArticulationBody joint in articulationChain)
+            {
+                joint.gameObject.AddComponent<JointControl>();
+                joint.jointFriction = 10;
+                joint.angularDamping = 10;
+            }
+            jointName = articulationChain[selectedIndex].name;
+            StoreColors(selectedIndex);
+            B = G = 0;
+            Alpha = R = 1;
+        }
+
+        void Update()
+        {
+            bool SelectionInput1 = Input.GetKeyDown("right");
+            bool SelectionInput2 = Input.GetKeyDown("left");
+
+            UpdateDirection(selectedIndex);
+
+            if (SelectionInput2)
+            {
+                if (selectedIndex == 1)
+                {
+                    selectedIndex = articulationChain.Length - 1;
+                }
+                else
+                {
+                    selectedIndex = selectedIndex - 1;
+                }
+                Highlight(selectedIndex);
+            }
+            else if (SelectionInput1)
+            {
+                if (selectedIndex == articulationChain.Length - 1)
+                {
+                    selectedIndex = 1;
+                }
+                else
+                {
+                    selectedIndex = selectedIndex + 1;
+                }
+                Highlight(selectedIndex);
+            }
+                
+            UpdateDirection(selectedIndex);
+        }
+
+        /// <summary>
+        /// Highlights the color of the robot by changing the color of the part to a color set by the user in the inspector window
+        /// </summary>
+        /// <param name="selectedIndex">Index of the link selected in the Articulation Chain</param>
+        private void Highlight(int selectedIndex)
+        {
+            if(selectedIndex == previousIndex)
+            {
+                return;
+            }
+
+            Renderer[] previousMaterialList = articulationChain[previousIndex].transform.GetChild(0).GetComponentsInChildren<Renderer>();
+
+            for (int counter = 0; counter < previousMaterialList.Length; counter++)
+            {
+                previousMaterialList[counter].material.color = prevColor[counter];
+            }
+            jointName = articulationChain[selectedIndex].name;
+            Renderer[] materialList = articulationChain[selectedIndex].transform.GetChild(0).GetComponentsInChildren<Renderer>();
+
+            StoreColors(selectedIndex);
+
+            foreach (var mesh in materialList)
+            {
+                Color tempColor = new Color(R, G, B, Alpha);
+                mesh.material.color = tempColor;
+            }
+
+        }
+
+        /// <summary>
+        /// Sets the direction of movement of the joint on every update
+        /// </summary>
+        /// <param name="jointIndex">Index of the link selected in the Articulation Chain</param>
+        private void UpdateDirection(int jointIndex)
+        {
+            float moveDirection = Input.GetAxis("Vertical");
+            JointControl current = articulationChain[jointIndex].GetComponent<JointControl>();            
+            if (previousIndex != jointIndex)
+            {
+                JointControl previous = articulationChain[previousIndex].GetComponent<JointControl>();            
+                previous.direction = RotationDirection.None;
+                previousIndex = jointIndex;
+            }
+
+            if (current.controltype != control)
+                UpdateControlType(current);
+
+            if (moveDirection > 0)
+            {
+                current.direction = RotationDirection.Positive;
+            }
+            else if (moveDirection < 0)
+            {
+                current.direction = RotationDirection.Negative;
+            }
+            else
+            {
+                current.direction = RotationDirection.None;
+            }
+
+
+        }
+
+        /// <summary>
+        /// Stores original color of the part being highlighted
+        /// </summary>
+        /// <param name="index">Index of the part in the Articulation chain</param>
+        private void StoreColors(int index)
+        {
+            Renderer[] materialLists = articulationChain[index].transform.GetChild(0).GetComponentsInChildren<Renderer>();
+            prevColor = new Color[materialLists.Length];
+            for (int counter = 0; counter < materialLists.Length; counter++)
+            {
+                prevColor[counter] = materialLists[counter].sharedMaterial.GetColor("_Color");
+            }
+        }
+
+        public void UpdateControlType(JointControl joint)
+        {
+            joint.controltype = control;
+            if(control == ControlType.PositionControl)
+            {
+                ArticulationDrive drive = joint.joint.xDrive;
+                drive.stiffness = stiffness;
+                drive.damping = 0;
+                joint.joint.xDrive = drive;
+            }
+        }
+    }
+}