CLOiSim-2.7.2

[Major Changes]

* General: Upgrade Unity editor version -> 2020.3.26f1 (LTS)
* General: Update package version
  - com.unity.render-pipelines.universal: 10.7.0 -> 10.8.1
  - com.unity.toolchain.linux-x86_64: 0.1.19-preview -> 1.0.0
* Core: Remove all device port map table when model is removed(deallocated) in BridgeManager
* SDF.Parser: Adjust Dynamic Friction ratio to Static Friction(mu value in sdf parameter) in SDF.Implement.Collision
* Device: Modify finding target joint control in JointState module
* UI: Modify the behavior of ESC key pressing for UI AddModellist
* UI: create removing target object function in AddModel Class

[Minor Changes]

* Core.Main: Give a sleep time before import and deploying after choose from model list
* Core.Main: Set model immovable before deploy the mode from the model list
* Core.Main: Modify Default model/World/File Directories in MainScene.unity
* SDF.Parser: Apply filtering non-number character in pose string using reg-ex(supporting a exponentioal notation(ex: 1e-10))
* General: Add protection code
  - for null object accessing in MotorControl
  - SDF.import.Visual
  - SDF.Import.Link
  - MotorControl Module
  - Articulation Module
* General: Add debuging log in Articulation Module
* UI: Add new prining warning messsage method in SimulationDisplay
* UI: Adjust height offset of deploying model before confirm the location where to place.
* UI: Change default Main Camera control parameters in MainScene.unity
* UI: Update following object list when new model is added from model list

[Bug fix]

* Device: Fix Typo in MicomCommand
* Core.Main: Fix null object in Main.Display variable.

Assets/Scenes/MainScene.unity

@@ -1776,19 +1776,13 @@ MonoBehaviour:
   clearAllOnStart: 1
   worldFileName: lg_seocho_with_actors.world
   modelRootDirectories:
+  - ../cloi_resources
   - ../sample_resources/models/
-  - ../lgrs_resource/assets/models/
-  - ../../lgrs_resource/assets/models/
   - ../sample_resources/All
-  - ../cloi_resources
   worldRootDirectories:
   - ../sample_resources/worlds/
-  - ../lgrs_resource/worlds/
-  - ../../lgrs_resource/worlds/
   fileRootDirectories:
   - ../sample_resources/media/
-  - ../lgrs_resource/assets
-  - ../../lgrs_resource/assets
 --- !u!1 &314493398
 GameObject:
   m_ObjectHideFlags: 0
@@ -5489,13 +5483,13 @@ MonoBehaviour:
   blockControl: 0
   blockMouseWheelControl: 0
   mainSpeed: 2.5
-  shiftAdd: 5
-  maxShift: 90
-  camSens: 0.15
-  edgeWidth: 25
+  shiftAdd: 15
+  maxShift: 200
+  camSens: 0.1
+  edgeWidth: 20
   edgeSens: 0.1
-  edgeSensMax: 1.5
-  wheelMoveAmp: 50
+  edgeSensMax: 1
+  wheelMoveAmp: 70
 --- !u!20 &963194227
 Camera:
   m_ObjectHideFlags: 0

Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs

@@ -43,7 +43,6 @@ protected void PublishTfThread(System.Object threadObject)
 					tfMessage.Transform.Name = tf.childFrameId;
 
 					var tfPose = tf.GetPose();
-
 					DeviceHelper.SetCurrentTime(tfMessage.Header.Stamp);
 					DeviceHelper.SetVector3d(tfMessage.Transform.Position, tfPose.position);
 					DeviceHelper.SetQuaternion(tfMessage.Transform.Orientation, tfPose.rotation);

Assets/Scripts/Core/Modules/BridgeManager.cs

@@ -34,7 +34,6 @@ public class BridgeManager : IDisposable
 {
 	private const ushort MinPortRange = 49152;
 	private const ushort MaxPortRange = IPEndPoint.MaxPort;
-	private static SimulationDisplay simulationDisplay = null;
 	private static StringBuilder sbLogs = new StringBuilder();
 
 	private static Dictionary<string, ushort> haskKeyPortMapTable = new Dictionary<string, ushort>();
@@ -44,7 +43,6 @@ public class BridgeManager : IDisposable
 
 	public BridgeManager()
 	{
-		simulationDisplay = Main.Display;
 		ClearLog();
 	}
 
@@ -59,29 +57,43 @@ public void Dispose()
 		GC.SuppressFinalize(this);
 	}
 
-	public static void DeallocateDevice(in List<ushort> devicePorts, in List<string> hashKeys)
+	private static void RemoveDevice(in ushort devicePort)
 	{
-		lock (deviceMapTable)
+		foreach (var deviceMap in deviceMapTable.ToList())
 		{
-			foreach (var devicePort in devicePorts)
+			var deviceMapValue = deviceMap.Value;
+			foreach (var partMaps in deviceMapValue.ToList())
 			{
-				foreach (var deviceMap in deviceMapTable)
+				var partMapsValue = partMaps.Value;
+				foreach (var portMaps in partMapsValue.ToList())
 				{
-					foreach (var partMaps in deviceMap.Value)
+					var portMapsValue = portMaps.Value;
+					foreach (var portMap in portMapsValue.ToList())
 					{
-						foreach (var portMaps in partMaps.Value)
-						{
-							var portMapList = portMaps.Value;
-							foreach (var portMap in portMapList.ToList())
-							{
-								if (portMap.Value == devicePort)
-								{
-									portMapList.Remove(portMap.Key);
-								}
-							}
-						}
+						if (portMap.Value == devicePort)
+							portMapsValue.Remove(portMap.Key);
 					}
+
+					if (portMapsValue.Count == 0)
+						partMapsValue.Remove(portMaps.Key);
 				}
+
+				if (partMapsValue.Count == 0)
+					deviceMapValue.Remove(partMaps.Key);
+			}
+
+			if (deviceMapValue.Count == 0)
+				deviceMapTable.Remove(deviceMap.Key);
+		}
+	}
+
+	public static void DeallocateDevice(in List<ushort> devicePorts, in List<string> hashKeys)
+	{
+		lock (deviceMapTable)
+		{
+			foreach (var devicePort in devicePorts)
+			{
+				RemoveDevice(devicePort);
 			}
 		}
 
@@ -245,7 +257,7 @@ public static ushort AllocateDevicePort(in string hashKey)
 		{
 			var errorMessage = string.Format("HashKey({0}) is already occupied.", hashKey);
 			Console.Error.WriteLine(errorMessage);
-			simulationDisplay?.SetErrorMessage(errorMessage);
+			Main.Display?.SetErrorMessage(errorMessage);
 			return 0;
 		}
 

Assets/Scripts/Devices/JointState.cs

@@ -44,21 +44,31 @@ protected override void GenerateMessage()
 			PushDeviceMessage<messages.JointStateV>(jointStateV);
 		}
 
-		public bool AddTarget(in string linkName, out SDF.Helper.Link link)
+		public bool AddTarget(in string targetLinkName, out SDF.Helper.Link link)
 		{
 			var childArticulationBodies = gameObject.GetComponentsInChildren<ArticulationBody>();
-
+			var rootModelName = string.Empty;
 			foreach (var childArticulationBody in childArticulationBodies)
 			{
-				if (childArticulationBody.name.Equals(linkName))
+				// Debug.Log (childArticulationBody.name + " | " + childArticulationBody.transform.parent.name);
+				if (childArticulationBody.isRoot)
+				{
+					rootModelName = childArticulationBody.name;
+					continue;
+				}
+
+				var parentModelName = childArticulationBody.transform.parent.name;
+				var linkName = ((rootModelName.CompareTo(parentModelName) == 0) ? "" : parentModelName + "::") + childArticulationBody.name;
+				// Debug.Log("!!!!!!! " + linkName);
+				if (linkName.Equals(targetLinkName))
 				{
 					var articulation = new Articulation(childArticulationBody);
 					articulation.SetDriveType(Articulation.DriveType.POSITION_AND_VELOCITY);
 
 					var jointState = new messages.JointState();
-					jointState.Name = linkName;
+					jointState.Name = targetLinkName;
 
-					articulationTable.Add(linkName, new Tuple<Articulation, messages.JointState>(articulation, jointState));
+					articulationTable.Add(targetLinkName, new Tuple<Articulation, messages.JointState>(articulation, jointState));
 
 					jointStateV.JointStates.Add(jointState);
 

Assets/Scripts/Devices/MicomCommand.cs

@@ -47,7 +47,7 @@ protected override void ProcessDevice()
 			}
 			else
 			{
-				Debug.LogWarning("ERROR: failed to pop deevice message");
+				Debug.LogWarning("ERROR: failed to pop device message");
 			}
 		}
 

Assets/Scripts/Devices/Modules/Articulation.cs

@@ -33,8 +33,11 @@ public Articulation(in GameObject target)
 
 	public void Reset()
 	{
-		_jointBody.velocity = Vector3.zero;
-		_jointBody.angularVelocity = Vector3.zero;
+		if (_jointBody != null)
+		{
+			_jointBody.velocity = Vector3.zero;
+			_jointBody.angularVelocity = Vector3.zero;
+		}
 	}
 
 	public void SetDriveType(in DriveType type)
@@ -62,7 +65,7 @@ protected void SetJointVelocity(in float velocity, in int targetDegree = 0)
 
 	private int GetValidIndex(in int index)
 	{
-		return (index >= _jointBody.dofCount) ? (_jointBody.dofCount - 1) : index;
+		return (_jointBody == null) ? -1 : ((index >= _jointBody.dofCount) ? (_jointBody.dofCount - 1) : index);
 	}
 
 	/// <returns>in radian for angular and in meters for linear</param>
@@ -184,7 +187,7 @@ public ArticulationDrive GetDrive()
 				break;
 
 			default:
-				Debug.LogWarning("unsupported joint type");
+				Debug.LogWarning("GetDrive() unsupported joint type: " + _jointType);
 				drive = new ArticulationDrive();
 				break;
 		}
@@ -249,7 +252,7 @@ public void SetDrive(in ArticulationDrive drive)
 				break;
 
 			default:
-				Debug.LogWarning("unsupported joint type");
+				Debug.LogWarning("SetDrive() unsupported joint type: " + _jointType);
 				break;
 		}
 	}

Assets/Scripts/Devices/Modules/MotorControl.cs

@@ -95,8 +95,15 @@ public void UpdateMotorFeedback(in float angularVelocity)
 		if (wheelList.TryGetValue(WheelLocation.LEFT, out var motorLeft) &&
 			wheelList.TryGetValue(WheelLocation.RIGHT, out var motorRight))
 		{
-			motorLeft.Feedback.SetRotatingTargetVelocity(angularVelocity);
-			motorRight.Feedback.SetRotatingTargetVelocity(angularVelocity);
+			if (motorLeft != null)
+			{
+				motorLeft.Feedback.SetRotatingTargetVelocity(angularVelocity);
+			}
+
+			if (motorRight != null)
+			{
+				motorRight.Feedback.SetRotatingTargetVelocity(angularVelocity);
+			}
 		}
 	}
 
@@ -109,16 +116,15 @@ private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelo
 		if (wheelList.TryGetValue(WheelLocation.LEFT, out var motorLeft) &&
 			wheelList.TryGetValue(WheelLocation.RIGHT, out var motorRight))
 		{
-			motorLeft.Feedback.SetMotionRotating(isRotating);
-			motorRight.Feedback.SetMotionRotating(isRotating);
-
 			if (motorLeft != null)
 			{
+				motorLeft.Feedback.SetMotionRotating(isRotating);
 				motorLeft.SetVelocityTarget(angularVelocityLeft);
 			}
 
 			if (motorRight != null)
 			{
+				motorRight.Feedback.SetMotionRotating(isRotating);
 				motorRight.SetVelocityTarget(angularVelocityRight);
 			}
 		}

Assets/Scripts/Main.cs

@@ -218,10 +218,10 @@ void Awake()
 		{
 			_infoDisplay = uiRoot.GetComponentInChildren<InfoDisplay>();
 			transformGizmo = uiRoot.GetComponentInChildren<RuntimeGizmos.TransformGizmo>();
+			simulationDisplay = uiRoot.GetComponentInChildren<SimulationDisplay>();
 
 			uiMainCanvasRoot = uiRoot.transform.Find("Main Canvas").gameObject;
 			followingList = uiMainCanvasRoot.GetComponentInChildren<FollowingTargetList>();
-			simulationDisplay = uiMainCanvasRoot.GetComponentInChildren<SimulationDisplay>();
 		}
 
 		gameObject.AddComponent<ObjectSpawning>();
@@ -321,9 +321,17 @@ private IEnumerator LoadModel(string modelPath, string modelFileName)
 
 			var targetObject = worldRoot.transform.Find(model.Name);
 
-			// yield return StartCoroutine(FreeObjectDeploy(targetObject));
 			var addingModel = uiMainCanvasRoot.GetComponentInChildren<AddModel>();
 			addingModel.SetAddingModelForDeploy(targetObject);
+
+			yield return new WaitForSeconds(0.1f);
+			yield return new WaitForEndOfFrame();
+
+			// for GUI
+			simulationDisplay?.ClearLogMessage();
+			followingList?.UpdateList();
+
+			yield return new WaitForEndOfFrame();
 		}
 
 		yield return null;

Assets/Scripts/Tools/DebugLogWriter.cs

@@ -11,13 +11,11 @@ public class DebugLogWriter : TextWriter
 {
 	private bool isError = false;
 	private bool showOnDisplay = false;
-	private SimulationDisplay simulationDisplay = null;
 
 	public DebugLogWriter(in bool errorLog = false)
 	{
 		//Debug.Log("Initialized!!!");
 		isError = errorLog;
-		simulationDisplay = Main.Display;
 	}
 
 	public override void Write(string value)
@@ -50,7 +48,7 @@ private void Print(in string value)
 			Debug.LogWarning(value);
 			if (showOnDisplay)
 			{
-				simulationDisplay?.SetErrorMessage(value);
+				Main.Display?.SetErrorMessage(value);
 				showOnDisplay = false;
 			}
 		}
@@ -60,7 +58,7 @@ private void Print(in string value)
 
 			if (showOnDisplay)
 			{
-				simulationDisplay?.SetEventMessage(value);
+				Main.Display?.SetEventMessage(value);
 				showOnDisplay = false;
 			}
 		}

Assets/Scripts/Tools/SDF/Implement/Implement.Collision.cs

@@ -18,7 +18,7 @@ public class Collision
 			private static readonly bool EnableMergeCollider = true;
 
 			private static readonly float ThresholdFrictionCombineMultiply = 0.01f;
-			private static readonly float DynamicFrictionRatio = 0.90f;
+			private static readonly float DynamicFrictionRatio = 0.95f;
 
 			private static readonly MCCookingOptions CookingOptions =
 					MCCookingOptions.EnableMeshCleaning |

Assets/Scripts/Tools/SDF/Import/Import.Link.cs

@@ -93,7 +93,7 @@ private static UE.ArticulationBody CreateArticulationBody(in UE.GameObject linkO
 				var articulationBody = linkObject.AddComponent<UE.ArticulationBody>();
 				articulationBody.velocity = UE.Vector3.zero;
 				articulationBody.angularVelocity = UE.Vector3.zero;
-				articulationBody.useGravity = linkHelper.useGravity;
+				articulationBody.useGravity = (linkHelper == null) ? false : linkHelper.useGravity;
 				articulationBody.jointType = UE.ArticulationJointType.FixedJoint;
 				articulationBody.mass = (inertial == null) ? 1e-07f : (float)inertial.mass;
 				articulationBody.linearDamping = 0.05f;

Assets/Scripts/Tools/SDF/Import/Import.Visual.cs

@@ -53,10 +53,13 @@ protected override void AfterImportVisual(in SDF.Visual visual, in System.Object
 
 					if (EnableOptimization)
 					{
-						var geometryTransform = visualObject.transform.GetChild(0);
-						if (geometryTransform.CompareTag("Geometry"))
+						if (visualObject.transform.childCount > 0)
 						{
-							Implement.Visual.OptimizeMeshes(geometryTransform);
+							var geometryTransform = visualObject.transform.GetChild(0);
+							if (geometryTransform.CompareTag("Geometry"))
+							{
+								Implement.Visual.OptimizeMeshes(geometryTransform);
+							}
 						}
 						else
 						{