Updated now actually moves the robot

Hit Enter to add a new position, default is 0,0,0

JiboBodyService/Program.cs

@@ -34,14 +34,23 @@ Console.CancelKeyPress += (_, e) =>
 
 while (!cts.IsCancellationRequested)
 {
-    Console.Write("Enter Jibo new Pose Body <Neck,Torso,Pelvis> in degress: 0-359, 0-359, 0-359 ");
+    Console.Write("Enter Jibo new Pose Body <Neck,Torso,Pelvis>" +
-    var positionString = (Console.ReadLine() ?? "").Trim();
+        "\r\nDefault is center for all 3 axis 0,0,0" +
+        "\r\nUse Degrees separated by a comma 0-359, 0-359, 0-359: ");
+    var positionString = (Console.ReadLine() ?? "0,0,0").Trim();
+    if (positionString.Length < 1) { positionString = "0,0,0"; }
     float[] position = [.. positionString.Split(',').Select(s => float.TryParse(s.Trim(), out var val) ? val : 0)];
 
-    if (string.IsNullOrWhiteSpace(positionString))
+    // According to the web service example,
+    // the position values should be in the range of 0-100, where 100 corresponds to 360 degrees.
+    if (!string.IsNullOrWhiteSpace(positionString))
     {
-        Console.WriteLine("Invalid Jibo Position entered.");
+        int index = 0;
-        continue;
+        Console.WriteLine("Convert to 0-100 scale");
+        foreach (var degrees in position)
+        {
+            position[index++] = (float)((degrees / 3.6) % 100);
+        }
     }
 
     JiboBodyControl js = new();
@@ -135,10 +144,15 @@ public static class Interval
     public static System.Timers.Timer Set(Action action, int interval)
     {
         var timer = new System.Timers.Timer(interval);
-        timer.Elapsed += (s, e) => {
+        timer.Elapsed += (s, e) =>
-            timer.Enabled = false;
+        {
-            action();
+            try
-            timer.Enabled = true;
+            {
+                timer.Enabled = false;
+                action();
+                timer.Enabled = true;
+            }
+            catch { return; }
         };
         timer.Enabled = true;
         return timer;
@@ -146,6 +160,7 @@ public static class Interval
 
     public static void Stop(System.Timers.Timer timer)
     {
+        if (timer == null) return;
         timer.Stop();
         timer.Dispose();
     }
@@ -153,6 +168,9 @@ public static class Interval
 
 public sealed class JiboBodyControl
 {
+    string Host = string.Empty;
+    enum Axis { Neck = 0, Torso = 1, Pelvis = 2 }
+
     const int BREAK = 2;
     const int POS_VEL = 7;
     const int VEL = 4;
@@ -188,6 +206,12 @@ public sealed class JiboBodyControl
         PoseArray = DegToRad(poseArray_deg);
     }
 
+    private void UpdatePositioning(string host)
+    {
+        JiboBodyState ??= SetupStateSocketAsync(host).Result;
+        UpdateStatus();
+    }
+
 
     public void Start(string jiboIp)
     {
@@ -198,7 +222,7 @@ public sealed class JiboBodyControl
         JiboBodyState = null;
 
         int tryCount = 5;
-        var host = jiboIp + ":8282";
+        Host = jiboIp + ":8282";
 
         do
         {
@@ -207,15 +231,14 @@ public sealed class JiboBodyControl
             {
                 Console.WriteLine("Waiting for initial body state...");
                 Task.Delay(delay, _cts.Token).Wait();
-                JiboBodyState = SetupStateSocketAsync(host).Result;
+                UpdatePositioning(Host);
-                UpdateStatus();
             }
         }
         while (JiboBodyState == null && !_cts.Token.IsCancellationRequested && --tryCount > 0);
 
         if (JiboBodyState != null)
         {
-            if (SetupCommandSocket(host).Result)
+            if (SetupCommandSocket(Host).Result)
             {
                 Console.WriteLine("Initial body state received. Ready for control loop...");
                 ControlLoop();
@@ -235,6 +258,17 @@ public sealed class JiboBodyControl
 
     public async Task StopAsync()
     {
+        if (IndexCheckInterval != null)
+        {
+            Interval.Stop(IndexCheckInterval);
+            IndexCheckInterval = null;
+        }
+        if (SpinInterval != null)
+        {
+            Interval.Stop(SpinInterval);
+            SpinInterval = null;
+        }
+
         var wsCloseTask = Task.Run(async () =>
         {
             _cts.Cancel();
@@ -249,18 +283,7 @@ public sealed class JiboBodyControl
         });
 
         await Task.WhenAny(wsCloseTask, Task.Delay(TimeSpan.FromSeconds(30), _cts.Token));
-        Console.WriteLine("WebCommunications have been shut down");
+        Console.WriteLine("WebCommunications have been shut down\r\nPress enter to set new position.");
-
-        if (IndexCheckInterval != null)
-        {
-            Interval.Stop(IndexCheckInterval);
-            IndexCheckInterval = null;
-        }
-        if (SpinInterval != null)
-        {
-            Interval.Stop(SpinInterval);
-            SpinInterval = null;
-        }
     }
 
     private async Task<bool> SetupCommandSocket(string host)
@@ -273,7 +296,7 @@ public sealed class JiboBodyControl
         {
             _ClientControlWebSocket = null;
             _ClientControlWebSocket = new ClientWebSocket();
-            _ClientControlWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60);
+            _ClientControlWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(10);
         }
         try
         {
@@ -298,6 +321,31 @@ public sealed class JiboBodyControl
         return false;
     }
 
+    private BodyState? GetBodyState(string json)
+    {
+        BodyState? bodyState = null;
+        var node = JsonNode.Parse(json);
+        if (node != null && node?["ts"]?[0]?.GetValue<float>() != 0)
+        {
+            using (var stringReader = new StringReader(json))
+            {
+                using var jsonReader = new JsonTextReader(stringReader);
+                var serializer = new Newtonsoft.Json.JsonSerializer();
+                try
+                {
+                    bodyState = serializer.Deserialize<BodyState>(jsonReader);
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine($"Error deserializing JSON: {ex.Message}");
+                    bodyState = null;
+                }
+            }
+        }
+
+        return bodyState;
+    }
+
     private async Task<BodyState?> SetupStateSocketAsync(string host)
     {
         if (wsAxisState == null)
@@ -308,7 +356,7 @@ public sealed class JiboBodyControl
         {
             _ClientWebSocket = null;
             _ClientWebSocket = new ClientWebSocket();
-            _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60);
+            _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(10);
         }
 
         try
@@ -319,63 +367,81 @@ public sealed class JiboBodyControl
                 Console.WriteLine("WebSocket connected.");
             }
 
-            var buffer = new byte[8192];
-
             if (_ClientWebSocket != null && _ClientWebSocket.State == WebSocketState.Open)
             {
                 WebSocketReceiveResult result;
                 using var ms = new MemoryStream();
-                var buff = new ArraySegment<byte>(new byte[8192]);
+                byte[] buffer = new byte[8192];
 
-                do
+                var receiveTask = Task.Run(async () =>
-                {
+                 {
-                    result = await _ClientWebSocket.ReceiveAsync(buffer, _cts.Token);
+                     int bytesReceived = 0;
+                     do
+                     {
+                         result = await _ClientWebSocket.ReceiveAsync(new ArraySegment<byte>(buffer), _cts.Token);
 
-                    if (result.MessageType == WebSocketMessageType.Close)
+                         if (result.MessageType == WebSocketMessageType.Close)
-                    {
+                         {
-                        wsAxisState = null;
+                             // Acknowledge the close to complete the handshake
-                        Console.WriteLine("WebSocket closed by server.");
+                             var status = result.CloseStatus ?? WebSocketCloseStatus.NormalClosure;
-                        return null;
+                             var desc = result.CloseStatusDescription;
-                    }
+                             try
+                             {
+                                 await _ClientWebSocket.CloseAsync(status, desc, CancellationToken.None);
+                             }
+                             catch
+                             {
+                                 // ignore errors during close acknowledgement
+                             }
+                             wsAxisState = null;
+                             break;
+                         }
 
-                    ms.Write(buffer, 0, result.Count);
+                         ms.Write(buffer, bytesReceived, result.Count);
-                }
+                         bytesReceived += result.Count;
-                while (!result.EndOfMessage);
+                     }
+                     while (!result.EndOfMessage && wsAxisState != null);
 
-                string json = Encoding.UTF8.GetString(ms.ToArray());
+                     if (wsAxisState == null)
-                Console.WriteLine("Data:" + json);
+                     {
+                         Console.WriteLine("WebSocket connection was closed.");
+                         JiboBodyState = null;
+                         return;
+                     }
 
-                BodyState? bodyState = null;
+                     if (result.EndOfMessage)
+                     {
+                         string json = Encoding.UTF8.GetString(ms.ToArray());
+                         JiboBodyState = GetBodyState(json);
+                     }
+                 });
                 
-                var node = JsonNode.Parse(json);
+                await receiveTask;
-                if (node != null && node?["ts"]?[0]?.GetValue<float>() != 0)
-                {
-                    using (var stringReader = new StringReader(json))
-                    {
-                        using var jsonReader = new JsonTextReader(stringReader);
-                        var serializer = new Newtonsoft.Json.JsonSerializer();
-                        try
-                        {
-                            bodyState = serializer.Deserialize<BodyState>(jsonReader);
-                        }
-                        catch (Exception ex)
-                        {
-                            Console.WriteLine($"Error deserializing JSON: {ex.Message}");
-                            bodyState = null;
-                        }
-                    }
-                }
 
-                return bodyState;
+                return JiboBodyState;
             }
             else
             {
                 Console.WriteLine("Could not Open WebSocket.");
+                JiboBodyState = null;
             }
         }
         catch (Exception ex)
         {
             Console.WriteLine($"Couldn\'t connect to body service\'s state websocket: {ex.Message}");
+            // Acknowledge the close to complete the handshake
+            var status = WebSocketCloseStatus.NormalClosure;
+            var desc = "";
+            try
+            {
+                await _ClientWebSocket.CloseAsync(status, desc, CancellationToken.None);
+            }
+            catch
+            {
+                // ignore errors during close acknowledgement
+            }
+            wsAxisState = null;
+            JiboBodyState = null;
         }
 
         return null;
@@ -383,12 +449,12 @@ public sealed class JiboBodyControl
 
     private void ControlLoop()
     {
+        bool IsPositioning = false;
+
         if (!_cts.Token.IsCancellationRequested)
         {
             if (JiboBodyState != null)
             {
-                Task.Delay(500, _cts.Token).Wait();
-
                 if (Finished)
                 {
                     Console.WriteLine("Jibo was already positioned.");
@@ -396,19 +462,42 @@ public sealed class JiboBodyControl
                 }
                 else
                 {
-                    Console.WriteLine("Positioning robot...");
+                    if (!IsPositioning)
-                    StartSpinningJibo();
+                    {
-                    IndexCheckInterval = Interval.Set(() => {
+                        IsPositioning = true;
-                        if (Finished)
+                        Console.WriteLine("Positioning robot...");
+                        StartSpinningJibo();
+                        IndexCheckInterval = Interval.Set(() =>
                         {
-                            StopAsync().Wait();
+                            if (Finished)
-                        }
+                            {
-                    }, 100);
+                                StopAsync().Wait();
+                            }
+                            else
+                            {
+                                JiboBodyState = null;
+                                UpdatePositioning(Host);
+                            }
+
+                        }, 30);
+                    }
                 }
             }
         }
     }
 
+    private async Task<bool> SendCommandAsync(ArraySegment<byte> buffer, string commandJson)
+    {
+        if (_ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open)
+        {
+            await _ClientControlWebSocket.SendAsync(buffer, WebSocketMessageType.Text, true, _cts.Token);
+            Console.WriteLine("Command sent: " + commandJson);
+            return true;
+        }
+
+        return false;
+    }
+
     private void StartSpinningJibo()
     {
         if (_ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open)
@@ -422,15 +511,30 @@ public sealed class JiboBodyControl
                     var buffer = new ArraySegment<byte>(bytes);
                     try
                     {
-                        _ClientControlWebSocket.SendAsync(buffer, WebSocketMessageType.Binary, true, _cts.Token).Wait();
+                        SendCommandAsync(buffer,commandJson).Wait();
-                        Console.WriteLine("Command sent: " + commandJson);
                     }
                     catch (Exception ex)
                     {
                         Console.WriteLine($"Error sending command: {ex.Message}");
+                        if (_ClientControlWebSocket?.State == WebSocketState.Closed)
+                        {
+                            // Acknowledge the close to complete the handshake
+                            var status = WebSocketCloseStatus.NormalClosure;
+                            var desc = "";
+                            try
+                            {
+                                _ClientControlWebSocket.CloseAsync(status, desc, CancellationToken.None);
+                            }
+                            catch
+                            {
+                                // ignore errors during close acknowledgement
+                            }
+                            StopAsync().Wait();
+                            return;
+                        }
                     }
                 }
-            }, 1000);
+            }, 2000);
         }
         else
         {
@@ -447,6 +551,68 @@ public sealed class JiboBodyControl
         return poseArray;
     }
 
+    private float GetAxisRange(Axis axis)
+    {
+        if (JiboBodyState != null)
+        {
+            switch (axis)
+            {
+                case Axis.Pelvis:
+                    {
+                        var pelvis = JiboBodyState.Pelvis;
+                        var mode = pelvis.Mode;
+                        float lim = 1;
+                        if (mode == 4)
+                        {
+                            lim = pelvis.VelLimit;
+                        }
+                        else if (mode == 6)
+                        {
+                            lim = pelvis.CurLimit;
+                        }
+
+                        return lim;
+                    }
+                case Axis.Torso:
+                    {
+                        var torso = JiboBodyState.Torso;
+                        var mode = torso.Mode;
+                        float lim = 1;
+                        if (mode == 4)
+                        {
+                            lim = torso.VelLimit;
+                        }
+                        else if (mode == 6)
+                        {
+                            lim = torso.CurLimit;
+                        }
+
+                        return lim;
+                    }
+                case Axis.Neck:
+                    {
+                        var neck = JiboBodyState.Neck;
+                        var mode = neck.Mode;
+                        float lim = 1;
+                        if (mode == 4)
+                        {
+                            lim = neck.VelLimit;
+                        }
+                        else if (mode == 6)
+                        {
+                            lim = neck.CurLimit;
+                        }
+
+                        return lim;
+                    }
+                default:
+                    return 0.0f;
+            }
+        }
+
+        return 0.0f;
+    }
+
     public void UpdateStatus()
     {
         if (JiboBodyState != null)
@@ -463,7 +629,9 @@ public sealed class JiboBodyControl
             NeckPositioned = (Math.Abs(Math.Abs(neck.Pos) -
                 Math.Abs(PoseArray[0])) < tolerance);
 
-            Finished = false;
+            Task.Delay(500, _cts.Token).Wait();
+
+            Finished = PelvisPositioned && TorsoPositioned && NeckPositioned;
         }
         else
         {
@@ -478,47 +646,68 @@ public sealed class JiboBodyControl
             return null;
         }
 
+        // Grab current body state for command building
+        BodyState bodyState = JiboBodyState;
+
         Console.WriteLine("Starting positioning sequence...");
         string vpelvis;
         string vtorso;
         string vneck;
 
-        int val = 0;
+        float val = GetAxisRange(Axis.Pelvis);
-        int mode;
+        int mode = VEL;
 
         if (!PelvisPositioned)
         {
-            mode = POS_VEL;
+            var currentMode = bodyState.Pelvis.Mode;
+            if (currentMode <= BREAK)
+            {
+                mode = POS_VEL;
+            }
         }
         else
         {
-            mode = BREAK;
+           mode = BREAK;
         }
-        vpelvis = Cmd(mode, val, PoseArray[2], JiboBodyState.Pelvis.Pos, JiboBodyState.Pelvis);
+        vpelvis = Cmd(mode, val, PoseArray[2], bodyState.Pelvis.Pos, bodyState.Pelvis);
+
+        val = GetAxisRange(Axis.Torso);
+        mode = VEL;
 
         if (!TorsoPositioned)
         {
-            mode = POS_VEL;
+            var currentMode = bodyState.Torso.Mode;
+            if (currentMode <= BREAK)
+            {
+                mode = POS_VEL;
+            }
         }
         else
         {
-            mode = BREAK;
+           mode = BREAK;
         }
-        vtorso = Cmd(mode, val, PoseArray[1], JiboBodyState.Torso.Pos, JiboBodyState.Torso);
+        vtorso = Cmd(mode, val, PoseArray[1], bodyState.Torso.Pos, bodyState.Torso);
+
+        val = GetAxisRange(Axis.Neck);
+        mode = VEL;
 
         if (!NeckPositioned)
         {
-            mode = POS_VEL;
+            var currentMode = bodyState.Neck.Mode;
+            if (currentMode <= BREAK)
+            {
+                mode = POS_VEL;
+            }
         }
         else
         {
-            mode = BREAK;
+           mode = BREAK;
         }
-        vneck = Cmd(mode, val, PoseArray[0], JiboBodyState.Neck.Pos, JiboBodyState.Neck);
+        vneck = Cmd(mode, val, PoseArray[0], bodyState.Neck.Pos, bodyState.Neck);
 
         StringBuilder cmd = new StringBuilder();
         cmd.Append('{');
-        cmd.AppendFormat("\"ts\": [{0}, {1}],", JiboBodyState.Ts?[0], JiboBodyState.Ts?[1]);
+        cmd.AppendFormat("\"ts\": [{0}, {1}],", bodyState.Ts?[0], bodyState.Ts?[1]);
         cmd.AppendFormat("\"pelvis\": {0},", vpelvis);
         cmd.AppendFormat("\"torso\": {0},", vtorso);
         cmd.AppendFormat("\"neck\": {0}", vneck);
@@ -527,8 +716,10 @@ public sealed class JiboBodyControl
         return cmd.ToString();
     }
 
-     private string Cmd(int mode, int vel, float desPos, float actPos, RobotBodyPart state)
+     private string Cmd(int mode, float vel, float desPos, float actPos, RobotBodyPart state)
     {
+        state.VelLimit = 10;
+
         StringBuilder cmd = new StringBuilder();
         cmd.Append('{');
         cmd.AppendFormat("\"mode\": {0},", mode);