Move Socket to Visualisation

BenoitGeslain · Feb 27, 2024 · 77ea8c2 · 77ea8c2
1 parent ffd7aea
commit 77ea8c2
Show file tree

Hide file tree

Showing 12 changed files with 125 additions and 121 deletions.
diff --git a/Visuo-haptic Toolkit/Assets/Demo Scenes/Corridor/Scenes/RedirectionCorridor.unity b/Visuo-haptic Toolkit/Assets/Demo Scenes/Corridor/Scenes/RedirectionCorridor.unity
@@ -1098,6 +1098,10 @@ PrefabInstance:
       propertyPath: scene.limbs.Array.size
       value: 1
       objectReference: {fileID: 0}
+    - target: {fileID: 7088029602675669135, guid: 7e46c8eb8c13fdb429130703b04c8216, type: 3}
+      propertyPath: scene.strategyDirection.x
+      value: 1
+      objectReference: {fileID: 0}
     - target: {fileID: 7088029602675669135, guid: 7e46c8eb8c13fdb429130703b04c8216, type: 3}
       propertyPath: scene.targets.Array.data[0]
       value: 
@@ -1140,10 +1144,10 @@ PrefabInstance:
     m_AddedComponents:
     - targetCorrespondingSourceObject: {fileID: 200869264966477082, guid: 7e46c8eb8c13fdb429130703b04c8216, type: 3}
       insertIndex: -1
-      addedObject: {fileID: 358983255}
+      addedObject: {fileID: 358983254}
     - targetCorrespondingSourceObject: {fileID: 200869264966477082, guid: 7e46c8eb8c13fdb429130703b04c8216, type: 3}
       insertIndex: -1
-      addedObject: {fileID: 358983254}
+      addedObject: {fileID: 358983255}
     - targetCorrespondingSourceObject: {fileID: 200869264966477082, guid: 7e46c8eb8c13fdb429130703b04c8216, type: 3}
       insertIndex: -1
       addedObject: {fileID: 358983261}

diff --git a/Visuo-haptic Toolkit/Assets/Demo Scenes/Corridor/Scripts/CorridorRedirection.cs b/Visuo-haptic Toolkit/Assets/Demo Scenes/Corridor/Scripts/CorridorRedirection.cs
@@ -21,11 +21,9 @@ public class CorridorRedirection : MonoBehaviour {
 		[SerializeField] private Transform start, end;
 		private float NormalizedDistance => Mathf.InverseLerp(start.position.z, end.position.z, UserHead.position.z);
 
-		private void Start() {
-			redirectionScript = Toolkit.Instance.gameObject.GetComponent<WorldRedirection>();
-		}
+        private void Start() => redirectionScript = Toolkit.Instance.gameObject.GetComponent<WorldRedirection>();
 
-		private void Update() {
+        private void Update() {
 
 			redirectionApplied = redirectionScript.GetAngularRedirection().eulerAngles.y;
 			if (redirectionApplied > 180f)
@@ -36,7 +34,6 @@ private void Update() {
 			} else {
 				redirectionScript.StartRedirection();
 			}
-			// Debug.Log(NormalizedDistance);
 		}
 	}
 }
diff --git a/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Logging/Socket.cs b/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Logging/Socket.cs
diff --git a/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/Scene.cs b/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/Scene.cs
@@ -66,11 +66,12 @@ public record Scene() {
 		[Ignore] public Vector3 previousHeadPosition;
 		[Ignore] public Quaternion previousHeadRotation;
 		[Ignore] public float previousRedirection;
+		[Ignore] public Vector3 strategyDirection;
 
-        /// <summary>
-        /// The position of the virtual limb is given by <c>physicalHand.position + Redirection</c>.
-        /// </summary>
-        [Ignore] public List<Vector3> LimbRedirection {
+		/// <summary>
+		/// The position of the virtual limb is given by <c>physicalHand.position + Redirection</c>.
+		/// </summary>
+		[Ignore] public List<Vector3> LimbRedirection {
             get => limbs.ConvertAll(limb => limb.virtualLimb[0].position - limb.physicalLimb.position);
             set {
 				foreach ((var limb, var v) in limbs.Zip(value)) {

diff --git a/...c Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/World Redirection/Strategies.cs b/...c Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/World Redirection/Strategies.cs
@@ -10,12 +10,10 @@ public virtual Vector3 SteerTo(Scene scene) {
 	}
 
 	public class NoSteering: WorldRedirectionStrategy {
-
         public override Vector3 SteerTo(Scene scene) => scene.physicalHead.forward;
     }
 
 	public class SteerToCenter: WorldRedirectionStrategy {
-
         public override Vector3 SteerTo(Scene scene) => scene.targets[0].position - scene.physicalHead.position;
     }
 
@@ -68,7 +66,7 @@ class SteerInDirection: WorldRedirectionStrategy {
         /// <returns></returns>
         public override Vector3 SteerTo(Scene scene) {
 			Debug.DrawRay(scene.physicalHead.position, Vector3.Reflect(scene.targets[0].position, scene.physicalHead.right));
-            return - scene.physicalHead.right;
+            return scene.strategyDirection;
         }
     }
 }
diff --git a/...c Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/World Redirection/Techniques.cs b/...c Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/World Redirection/Techniques.cs
@@ -117,6 +117,7 @@ public class Razzaque2001Hybrid: WorldRedirectionTechnique {
 
 		readonly Func<float, float, float, float> aggregate;
 
+
         /// <summary>
         /// By default, the aggregation function is the maximum by absolute value.
         /// </summary>

diff --git a/...it/Assets/Visuo-Haptic Toolkit/Scripts/Utils/Editor/Redirection/WorldRedirectionEditor.cs b/...it/Assets/Visuo-Haptic Toolkit/Scripts/Utils/Editor/Redirection/WorldRedirectionEditor.cs
@@ -24,9 +24,10 @@ public class WorldRedirectionEditor : Editor {
 		SerializedProperty applyDampening;
 		SerializedProperty applySmoothing;
 		SerializedProperty redirect;
+		SerializedProperty direction;
 
-        readonly string[] strategyTechniques = { "Razzaque2001OverTimeRotation", "Razzaque2001Rotational", "Razzaque2001Curvature", "Razzaque2001Hybrid" };
-        readonly string[] targetsStrategies = { "SteerToCenter", "SteerToMultipleTargets", "SteerInDirection" };
+		readonly string[] strategyTechniques = { "Razzaque2001OverTimeRotation", "Razzaque2001Rotational", "Razzaque2001Curvature", "Razzaque2001Hybrid" };
+        readonly string[] targetsStrategies = { "SteerToCenter", "SteerToMultipleTargets" };
 
 		private void OnEnable() {
 			technique = serializedObject.FindProperty("_technique");
@@ -41,10 +42,10 @@ private void OnEnable() {
 			virtualTarget = serializedObject.FindProperty("scene.virtualTarget");
 			origin = serializedObject.FindProperty("scene.origin");
 			targetsScene = serializedObject.FindProperty("scene.targets");
-			radius = serializedObject.FindProperty("scene.radius");
 			applyDampening = serializedObject.FindProperty("scene.applyDampening");
 			applySmoothing = serializedObject.FindProperty("scene.applySmoothing");
 			redirect = serializedObject.FindProperty("redirect");
+			direction = serializedObject.FindProperty("scene.strategyDirection");
 		}
 
 		public override void OnInspectorGUI() {
@@ -82,12 +83,12 @@ public override void OnInspectorGUI() {
 			if (strategyTechniques.Contains(technique.enumNames[technique.enumValueIndex])) {
 				if (targetsStrategies.Contains(strategy.enumNames[strategy.enumValueIndex])) {
 					EditorGUILayout.PropertyField(targetsScene, new GUIContent ("Targets"));
-				} else if (strategy.enumNames[strategy.enumValueIndex] == "SteerToOrbit") {
-					EditorGUILayout.PropertyField(radius, new GUIContent ("Radius"));
+					EditorGUILayout.PropertyField(applyDampening, new GUIContent("Apply Dampening"));
+					EditorGUILayout.PropertyField(applySmoothing, new GUIContent("Apply Smoothing"));
+				} else if (strategy.enumNames[strategy.enumValueIndex] == "SteerInDirection") {
+					EditorGUILayout.PropertyField(direction, new GUIContent ("Direction"));
 				}
 
-				EditorGUILayout.PropertyField(applyDampening, new GUIContent ("Apply Dampening"));
-				EditorGUILayout.PropertyField(applySmoothing, new GUIContent ("Apply Smoothing"));
 			}
 
 			serializedObject.ApplyModifiedProperties();

diff --git a/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Utils/InverseWeightedDistance.cs b/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Utils/InverseWeightedDistance.cs
@@ -10,12 +10,12 @@ public static Vector3 Interpolate(Vector3[] x, Vector3[] target, float p, Vector
             var weights = new float[x.Length];
             int idx = 0;
             foreach (var (origin, targ) in x.Zip(target)) {
-                var d = Vector3.Distance(origin, position);
+                var d = (origin - position).sqrMagnitude;
                 if (Mathf.Approximately(d, 0)) {
                     return targ;
                 }
                 else {
-                    weights[idx++] =  Mathf.Pow(d, -p);
+                    weights[idx++] =  Mathf.Pow(d, -p / 2);
                 }
             }
             return target.Zip(weights, (t, w) => w * t).Aggregate(Vector3.zero, (a, b) => a + b) / weights.Sum();

diff --git a/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Visualisation/Socket.cs b/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Visualisation/Socket.cs
@@ -0,0 +1,86 @@
+using System;
+using System.Net.Sockets;
+using System.Threading;
+
+using UnityEngine;
+
+using VHToolkit.Redirection;
+using VHToolkit.Redirection.WorldRedirection;
+
+namespace VHToolkit.Logging {
+
+	[Serializable]
+	struct WorldRedirectionData {
+		[SerializeField] public float overTime, rotational, curvature, hybrid, total;
+		[SerializeField] public float time;
+
+		public void AddTo(float overTime, float rotational, float curvature, float hybrid, float total, float time) {
+			this.overTime += Mathf.Abs(overTime);
+			this.rotational += Mathf.Abs(rotational);
+			this.curvature += Mathf.Abs(curvature);
+			this.hybrid += Mathf.Abs(hybrid);
+			this.total = total;
+			this.time = time;
+		}
+	}
+	public class Socket : MonoBehaviour {
+		private Scene scene;
+		private DateTime startTime;
+
+		private TcpClient client;
+
+		private Razzaque2001Hybrid loggingTechnique;
+
+		private WorldRedirectionData redirectionData;
+
+		private void Start() {
+			scene = Toolkit.Instance.GetComponent<WorldRedirection>().scene;
+			InvokeRepeating(nameof(StartSendingMessages), 1f, 0.5f);
+			loggingTechnique = new();
+		}
+
+		private void StartSendingMessages() {
+			if (client == null || !client.Connected) {
+				try {
+					client = new TcpClient("localhost", 13000) {
+						ReceiveTimeout = 500
+					};
+					startTime = DateTime.Now;
+				}
+				catch (SocketException) {}
+			} else {
+				Thread thread = new(() => SendMessage(client, redirectionData));
+				thread.Start();
+				redirectionData = new();
+			}
+		}
+
+		private void SendMessage(TcpClient client, WorldRedirectionData redirectionData) {
+			string json = JsonUtility.ToJson(redirectionData);
+			Debug.Log(json);
+			Debug.Log($"Sending: {json}");
+			// Translate the passed message into ASCII and store it as a Byte array.
+			Byte[] messageBytes = System.Text.Encoding.ASCII.GetBytes(json);
+
+			// Get a client stream for reading and writing.
+			NetworkStream stream = client.GetStream();
+
+			try {
+				// Send the message to the connected TcpServer.
+				stream.Write(messageBytes, 0, messageBytes.Length);
+				stream.Flush();
+			} catch (SocketException) {Debug.LogWarning("Socket closed.");}
+		}
+
+		private void Update() {
+			if (client != null && client.Connected) {
+				redirectionData.AddTo(Razzaque2001OverTimeRotation.GetRedirection(scene),
+									Razzaque2001Rotational.GetRedirection(scene),
+									Razzaque2001Curvature.GetRedirection(scene),
+									loggingTechnique.GetRedirection(scene),
+									(scene.HeadToHeadRedirection.eulerAngles.y > 180f) ? 360f - scene.HeadToHeadRedirection.eulerAngles.y : scene.HeadToHeadRedirection.eulerAngles.y,
+									(float)(DateTime.Now - startTime).TotalSeconds);
+			}
+		}
+	}
+}
diff --git a/...ic Toolkit/Scripts/Logging/Socket.cs.meta → ...lkit/Scripts/Visualisation/Socket.cs.meta b/...ic Toolkit/Scripts/Logging/Socket.cs.meta → ...lkit/Scripts/Visualisation/Socket.cs.meta
diff --git a/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Visualisation/TopViewCamera.CS b/Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Visualisation/TopViewCamera.CS
@@ -6,9 +6,7 @@ using UnityEngine;
 public class TopViewCamera : MonoBehaviour {
 	public Transform trackedObject;
 
-	private void Update() {
-		transform.SetPositionAndRotation(
+    private void Update() => transform.SetPositionAndRotation(
             new(trackedObject.position.x, transform.position.y, trackedObject.position.z),
-			Quaternion.Euler(90f, trackedObject.rotation.eulerAngles.y, 0f));
-	}
+            Quaternion.Euler(90f, trackedObject.rotation.eulerAngles.y, 0f));
 }
diff --git a/python/RedirectionPlotter.py b/python/RedirectionPlotter.py
@@ -6,7 +6,7 @@
 import matplotlib
 matplotlib.use('TkAgg')
 
-plt.ion() # turn on interactive mode (otherwise the window arises not before "show()"
+plt.ion() # turn on interactive mode
 fig = plt.figure(figsize=(10,6), constrained_layout=True)
 ax1 = plt.subplot(211)
 box = ax1.get_position()
@@ -15,14 +15,10 @@
 box = ax2.get_position()
 ax2.set_position([box.x0, box.y0, box.width * 0.9, box.height])
 
-# plt.subplots_adjust(hspace=0.5)
-# plt.tight_layout()
 
 otrs, rs, cs, hybrid, tots, ys = ([] for _ in range(6))
 
 
-
-
 serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 # bind the socket to a public host, and a well-known port
 serversocket.bind(("localhost", 13000))
@@ -37,16 +33,12 @@
             try:
                 chunk = clientsocket.recv(4096).decode()
             except ConnectionResetError:
-                print('Should close')
+                print("Connection reset")
                 break
             for m in re.finditer(r'\{[^}]*}', chunk):
                 d = json.loads(m[0])
-                otrs.append(d["overTime"])
-                rs.append(d["rotational"])
-                cs.append(d["curvature"])
-                hybrid.append(d["hybrid"])
-                tots.append(d["total"])
-                ys.append(d["time"])
+                for k, l in {"overTime": otrs, "rotational": rs, "curvature": cs, "hybrid": hybrid, "total": tots, "time": ys}.items():
+                    l.append(d[k])
 
             ax1.clear()
             ax2.clear()
@@ -55,19 +47,18 @@
             ax1.set_xlim((ys[-1] - 30, ys[-1]))
             ax1.set_title('Redirection amounts over time')
             ax1.set_ylabel('Redirection per second (degrees)')
-            for series, color, label in zip((otrs, rs, cs), 'rgy', ('Over time\nrotation', 'Rotational', 'Curvature')):
-                ax1.plot(ys[-60:], series[-60:], color=color, label=label, linewidth=0.5, linestyle="dashed")
-            ax1.fill_between(ys[-60:], hybrid[-60:], color='b', alpha=0.2)
-            ax1.plot(ys[-60:], hybrid[-60:], color='b', label='Hybrid', linewidth=0.5)
+            # for series, color, label in zip((otrs, rs, cs), 'rgy', ('Over time\nrotation', 'Rotational', 'Curvature')):
+            #     ax1.plot(ys[-60:], series[-60:], color=color, label=label, linewidth=0.5, linestyle="dashed")
+            ax1.stackplot(ys[-60:], otrs[-60:], rs[-60:], cs[-60:], labels=('Over time\nrotation', 'Rotational', 'Curvature'), linewidth=0.5, linestyle="dashed")
+            # ax1.fill_between(ys[-60:], hybrid[-60:], color='b', alpha=0.2)
+            # ax1.plot(ys[-60:], hybrid[-60:], color='b', label='Hybrid', linewidth=0.5)
 
             ax2.set_xticks(list(map(int, ys[::10])))
             ax2.set_xlim((ys[-1] - 30, ys[-1]))
             ax2.set_xlabel('Time from start (seconds)', loc='right')
             ax2.set_ylabel('Redirection amount (degrees)')
             ax2.plot(ys[-60:], tots[-60:], color='m', label='Total\nredirection', linewidth=1)
 
-            ax1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
-            ax2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
-
-            plt.pause(0.05)
-plt.ioff()
+            for ax in (ax1, ax2):
+                ax.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+            plt.pause(0.05)