From 658c8e0e5783cd3ee3836a5671dd4b1568393c2a Mon Sep 17 00:00:00 2001 From: Zetoman Date: Fri, 3 Apr 2026 16:23:49 +0000 Subject: [PATCH] Updated now actually moves the robot Hit Enter to add a new position, default is 0,0,0 --- JiboBodyService/Program.cs | 375 ++++++++++++++++++++++++++++--------- 1 file changed, 283 insertions(+), 92 deletions(-) diff --git a/JiboBodyService/Program.cs b/JiboBodyService/Program.cs index ed27102..6029466 100644 --- a/JiboBodyService/Program.cs +++ b/JiboBodyService/Program.cs @@ -34,14 +34,23 @@ Console.CancelKeyPress += (_, e) => while (!cts.IsCancellationRequested) { - Console.Write("Enter Jibo new Pose Body in degress: 0-359, 0-359, 0-359 "); - var positionString = (Console.ReadLine() ?? "").Trim(); + Console.Write("Enter Jibo new Pose Body " + + "\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."); - continue; + int index = 0; + 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.Enabled = false; - action(); - timer.Enabled = true; + 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; - UpdateStatus(); + UpdatePositioning(Host); } } 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"); - - if (IndexCheckInterval != null) - { - Interval.Stop(IndexCheckInterval); - IndexCheckInterval = null; - } - if (SpinInterval != null) - { - Interval.Stop(SpinInterval); - SpinInterval = null; - } + Console.WriteLine("WebCommunications have been shut down\r\nPress enter to set new position."); } private async Task 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() != 0) + { + using (var stringReader = new StringReader(json)) + { + using var jsonReader = new JsonTextReader(stringReader); + var serializer = new Newtonsoft.Json.JsonSerializer(); + try + { + bodyState = serializer.Deserialize(jsonReader); + } + catch (Exception ex) + { + Console.WriteLine($"Error deserializing JSON: {ex.Message}"); + bodyState = null; + } + } + } + + return bodyState; + } + private async Task SetupStateSocketAsync(string host) { if (wsAxisState == null) @@ -308,9 +356,9 @@ public sealed class JiboBodyControl { _ClientWebSocket = null; _ClientWebSocket = new ClientWebSocket(); - _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60); + _ClientWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(10); } - + try { if (_ClientWebSocket.State != WebSocketState.Open) @@ -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(new byte[8192]); + byte[] buffer = new byte[8192]; - do - { - result = await _ClientWebSocket.ReceiveAsync(buffer, _cts.Token); + var receiveTask = Task.Run(async () => + { + int bytesReceived = 0; + do + { + result = await _ClientWebSocket.ReceiveAsync(new ArraySegment(buffer), _cts.Token); - if (result.MessageType == WebSocketMessageType.Close) - { - wsAxisState = null; - Console.WriteLine("WebSocket closed by server."); - return null; - } + 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, 0, result.Count); - } - while (!result.EndOfMessage); + ms.Write(buffer, bytesReceived, result.Count); + bytesReceived += result.Count; + } + while (!result.EndOfMessage && wsAxisState != null); - string json = Encoding.UTF8.GetString(ms.ToArray()); - Console.WriteLine("Data:" + json); + if (wsAxisState == null) + { + 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); + } + }); + + await receiveTask; - var node = JsonNode.Parse(json); - if (node != null && node?["ts"]?[0]?.GetValue() != 0) - { - using (var stringReader = new StringReader(json)) - { - using var jsonReader = new JsonTextReader(stringReader); - var serializer = new Newtonsoft.Json.JsonSerializer(); - try - { - bodyState = serializer.Deserialize(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..."); - StartSpinningJibo(); - IndexCheckInterval = Interval.Set(() => { - if (Finished) + if (!IsPositioning) + { + IsPositioning = true; + Console.WriteLine("Positioning robot..."); + StartSpinningJibo(); + IndexCheckInterval = Interval.Set(() => { - StopAsync().Wait(); - } - }, 100); + if (Finished) + { + StopAsync().Wait(); + } + else + { + JiboBodyState = null; + UpdatePositioning(Host); + } + + }, 30); + } } } } } + private async Task SendCommandAsync(ArraySegment 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(bytes); try { - _ClientControlWebSocket.SendAsync(buffer, WebSocketMessageType.Binary, true, _cts.Token).Wait(); - Console.WriteLine("Command sent: " + commandJson); + SendCommandAsync(buffer,commandJson).Wait(); } 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; - int mode; + float val = GetAxisRange(Axis.Pelvis); + 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);