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) =>
+        {
+            try
+            {
                 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,55 +321,9 @@ public sealed class JiboBodyControl
         return false;
     }
 
-    private async Task<BodyState?> SetupStateSocketAsync(string host)
+    private BodyState? GetBodyState(string json)
     {
-        if (wsAxisState == null)
-        {
-            wsAxisState = new Uri($"ws://{host}/axis_state");
-        }
-        if (_ClientWebSocket == null || _ClientWebSocket.State != WebSocketState.Open)
-        {
-            _ClientWebSocket = null;
-            _ClientWebSocket = new ClientWebSocket();
-            _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60);
-        }
-           
-        try
-        {
-            if (_ClientWebSocket.State != WebSocketState.Open)
-            {
-                await _ClientWebSocket.ConnectAsync(wsAxisState, _cts.Token);
-                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]);
-
-                do
-                {
-                    result = await _ClientWebSocket.ReceiveAsync(buffer, _cts.Token);
-
-                    if (result.MessageType == WebSocketMessageType.Close)
-                    {
-                        wsAxisState = null;
-                        Console.WriteLine("WebSocket closed by server.");
-                        return null;
-                    }
-
-                    ms.Write(buffer, 0, result.Count);
-                }
-                while (!result.EndOfMessage);
-
-                string json = Encoding.UTF8.GetString(ms.ToArray());
-                Console.WriteLine("Data:" + json);
-
         BodyState? bodyState = null;
-
         var node = JsonNode.Parse(json);
         if (node != null && node?["ts"]?[0]?.GetValue<float>() != 0)
         {
@@ -368,14 +345,103 @@ public sealed class JiboBodyControl
 
         return bodyState;
     }
+
+    private async Task<BodyState?> SetupStateSocketAsync(string host)
+    {
+        if (wsAxisState == null)
+        {
+            wsAxisState = new Uri($"ws://{host}/axis_state");
+        }
+        if (_ClientWebSocket == null || _ClientWebSocket.State != WebSocketState.Open)
+        {
+            _ClientWebSocket = null;
+            _ClientWebSocket = new ClientWebSocket();
+            _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(10);
+        }
+
+        try
+        {
+            if (_ClientWebSocket.State != WebSocketState.Open)
+            {
+                await _ClientWebSocket.ConnectAsync(wsAxisState, _cts.Token);
+                Console.WriteLine("WebSocket connected.");
+            }
+
+            if (_ClientWebSocket != null && _ClientWebSocket.State == WebSocketState.Open)
+            {
+                WebSocketReceiveResult result;
+                using var ms = new MemoryStream();
+                byte[] buffer = new byte[8192];
+
+                var receiveTask = Task.Run(async () =>
+                 {
+                     int bytesReceived = 0;
+                     do
+                     {
+                         result = await _ClientWebSocket.ReceiveAsync(new ArraySegment<byte>(buffer), _cts.Token);
+
+                         if (result.MessageType == WebSocketMessageType.Close)
+                         {
+                             // Acknowledge the close to complete the handshake
+                             var status = result.CloseStatus ?? WebSocketCloseStatus.NormalClosure;
+                             var desc = result.CloseStatusDescription;
+                             try
+                             {
+                                 await _ClientWebSocket.CloseAsync(status, desc, CancellationToken.None);
+                             }
+                             catch
+                             {
+                                 // ignore errors during close acknowledgement
+                             }
+                             wsAxisState = null;
+                             break;
+                         }
+
+                         ms.Write(buffer, bytesReceived, result.Count);
+                         bytesReceived += result.Count;
+                     }
+                     while (!result.EndOfMessage && wsAxisState != null);
+
+                     if (wsAxisState == null)
+                     {
+                         Console.WriteLine("WebSocket connection was closed.");
+                         JiboBodyState = null;
+                         return;
+                     }
+
+                     if (result.EndOfMessage)
+                     {
+                         string json = Encoding.UTF8.GetString(ms.ToArray());
+                         JiboBodyState = GetBodyState(json);
+                     }
+                 });
+                
+                await receiveTask;
+
+                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,18 +462,41 @@ public sealed class JiboBodyControl
                 }
                 else
                 {
+                    if (!IsPositioning)
+                    {
+                        IsPositioning = true;
                         Console.WriteLine("Positioning robot...");
                         StartSpinningJibo();
-                    IndexCheckInterval = Interval.Set(() => {
+                        IndexCheckInterval = Interval.Set(() =>
+                        {
                             if (Finished)
                             {
                                 StopAsync().Wait();
                             }
-                    }, 100);
+                            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()
     {
@@ -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)
+        {
+            var currentMode = bodyState.Pelvis.Mode;
+            if (currentMode <= BREAK)
             {
                 mode = POS_VEL;
             }
+        }
         else
         {
            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)
+        {
+            var currentMode = bodyState.Torso.Mode;
+            if (currentMode <= BREAK)
             {
                 mode = POS_VEL;
             }
+        }
         else
         {
            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)
+        {
+            var currentMode = bodyState.Neck.Mode;
+            if (currentMode <= BREAK)
             {
                 mode = POS_VEL;
             }
+        }
         else
         {
            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);