From 279b1690d50fa025522fd9f5fc496dba98cfead5 Mon Sep 17 00:00:00 2001 From: Zetoman Date: Thu, 2 Apr 2026 14:24:41 +0000 Subject: [PATCH] Upload files to "JiboBodyService" Initial Commit --- JiboBodyService/Playground.sln | 25 ++ JiboBodyService/Program.cs | 544 +++++++++++++++++++++++++++++++++ 2 files changed, 569 insertions(+) create mode 100644 JiboBodyService/Playground.sln create mode 100644 JiboBodyService/Program.cs diff --git a/JiboBodyService/Playground.sln b/JiboBodyService/Playground.sln new file mode 100644 index 0000000..c20a9f5 --- /dev/null +++ b/JiboBodyService/Playground.sln @@ -0,0 +1,25 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.14.36518.9 d17.14 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Playground", "Playground.csproj", "{6A9A0FF1-B1C7-B473-2247-6E2812719A3D}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {6A9A0FF1-B1C7-B473-2247-6E2812719A3D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {6A9A0FF1-B1C7-B473-2247-6E2812719A3D}.Debug|Any CPU.Build.0 = Debug|Any CPU + {6A9A0FF1-B1C7-B473-2247-6E2812719A3D}.Release|Any CPU.ActiveCfg = Release|Any CPU + {6A9A0FF1-B1C7-B473-2247-6E2812719A3D}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {9419FFD7-C773-4069-81CC-E9DBE3789B5A} + EndGlobalSection +EndGlobal diff --git a/JiboBodyService/Program.cs b/JiboBodyService/Program.cs new file mode 100644 index 0000000..ed27102 --- /dev/null +++ b/JiboBodyService/Program.cs @@ -0,0 +1,544 @@ +using Newtonsoft.Json; +using System.Data; +using System.Net.WebSockets; + +using System.Text; +using System.Text.Json; +using System.Text.Json.Nodes; + + +Console.Write("Enter Jibo IP: "); +var jiboIp = (Console.ReadLine() ?? "").Trim(); + + +if (string.IsNullOrWhiteSpace(jiboIp)) +{ + Console.WriteLine("No IP entered."); + return; +} + +var wsAxisState = new Uri($"ws://{jiboIp}:8282/axis_state"); +var wsAxisCommand = new Uri($"ws://{jiboIp}:8282/axis_command"); + +using var cts = new CancellationTokenSource(); + +Console.WriteLine($"Connecting to Jibo at {jiboIp}..."); +Console.WriteLine("Press Ctrl+C to quit."); + +Console.CancelKeyPress += (_, e) => +{ + e.Cancel = true; + cts.Cancel(); +}; + + +while (!cts.IsCancellationRequested) +{ + Console.Write("Enter Jibo new Pose Body in degress: 0-359, 0-359, 0-359 "); + var positionString = (Console.ReadLine() ?? "").Trim(); + float[] position = [.. positionString.Split(',').Select(s => float.TryParse(s.Trim(), out var val) ? val : 0)]; + + if (string.IsNullOrWhiteSpace(positionString)) + { + Console.WriteLine("Invalid Jibo Position entered."); + continue; + } + + JiboBodyControl js = new(); + + try + { + js.PositionRobot(position[0], position[1], position[2]); + js.Start(jiboIp); + } + catch (Exception ex) + { + Console.WriteLine($"Error: {ex.Message}"); + } + + + Console.ReadLine(); + js?.StopAsync().Wait(); +} + +return; + +public struct RobotBodyPart +{ + [JsonProperty(PropertyName = "ts")] + public List? Ts { get; set; } + [JsonProperty(PropertyName = "pos")] + public float Pos { get; set; } + [JsonProperty(PropertyName = "inc_pos")] + public float IncPos { get; set; } + [JsonProperty(PropertyName = "vel")] + public float Vel { get; set; } + [JsonProperty(PropertyName = "cur")] + public float Cur { get; set; } + [JsonProperty(PropertyName = "pwm")] + public float Pwm { get; set; } + [JsonProperty(PropertyName = "status")] + public int Status { get; set; } + [JsonProperty(PropertyName = "vel_limit")] + public float VelLimit { get; set; } + [JsonProperty(PropertyName = "acc_limit")] + public float AccLimit { get; set; } + [JsonProperty(PropertyName = "cur_limit")] + public float CurLimit { get; set; } + [JsonProperty(PropertyName = "mode")] + public int Mode { get; set; } + [JsonProperty(PropertyName = "ref")] + public float Ref { get; set; } + [JsonProperty(PropertyName = "ref_pos")] + public float RefPos { get; set; } + [JsonProperty(PropertyName = "ref_acc")] + public float RefAcc { get; set; } + [JsonProperty(PropertyName = "ticks")] + public int Ticks { get; set; } + [JsonProperty(PropertyName = "integrator")] + public float Integrator { get; set; } + [JsonProperty(PropertyName = "fault_status")] + public int FaultStatus { get; set; } + [JsonProperty(PropertyName = "index_count")] + public int IndexCount { get; set; } + [JsonProperty(PropertyName = "limiter_mode")] + public int LimiterMode { get; set; } +} + +public sealed class BodyState +{ + [JsonProperty(PropertyName = "ts")] + public List? Ts { get; set; } + + [JsonProperty(PropertyName = "pelvis")] + public RobotBodyPart Pelvis { get; set; } + + [JsonProperty(PropertyName = "neck")] + public RobotBodyPart Neck { get; set; } + + [JsonProperty(PropertyName = "torso")] + public RobotBodyPart Torso { get; set; } + + [JsonProperty(PropertyName = "lockout")] + public float Lockout { get; set; } + + [JsonProperty(PropertyName = "limiter_torque")] + public float LimiterTorque { get; set; } + + [JsonProperty(PropertyName = "limiter_lookahead_torque")] + public float LimiterLookaheadTorque { get; set; } + +} + +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.Enabled = true; + return timer; + } + + public static void Stop(System.Timers.Timer timer) + { + timer.Stop(); + timer.Dispose(); + } +} + +public sealed class JiboBodyControl +{ + const int BREAK = 2; + const int POS_VEL = 7; + const int VEL = 4; + + public BodyState? JiboBodyState { get; private set; } = null; + + bool PelvisPositioned = false; + bool TorsoPositioned = false; + bool NeckPositioned = false; + bool Finished = true; + + private float[] PoseArray = new float[3]; + + System.Timers.Timer? IndexCheckInterval; + System.Timers.Timer? SpinInterval; + + Uri? wsAxisState; + Uri? wsAxisCommand; + + readonly CancellationTokenSource _cts = new(); + ClientWebSocket? _ClientWebSocket = new(); + ClientWebSocket? _ClientControlWebSocket = new(); + + public void PositionRobot(float neckPos, float torsoPos, float pelvisPos) + { + //pose array, in degrees + var poseArray_deg = new float[3]; + poseArray_deg[0] = neckPos; + poseArray_deg[1] = torsoPos; + poseArray_deg[2] = pelvisPos; + + //pose array, converted to radian + PoseArray = DegToRad(poseArray_deg); + } + + + public void Start(string jiboIp) + { + PelvisPositioned = false; + TorsoPositioned = false; + NeckPositioned = false; + Finished = false; + JiboBodyState = null; + + int tryCount = 5; + var host = jiboIp + ":8282"; + + do + { + int delay = 1000; + if (JiboBodyState == null) + { + Console.WriteLine("Waiting for initial body state..."); + Task.Delay(delay, _cts.Token).Wait(); + JiboBodyState = SetupStateSocketAsync(host).Result; + UpdateStatus(); + } + } + while (JiboBodyState == null && !_cts.Token.IsCancellationRequested && --tryCount > 0); + + if (JiboBodyState != null) + { + if (SetupCommandSocket(host).Result) + { + Console.WriteLine("Initial body state received. Ready for control loop..."); + ControlLoop(); + } + else + { + Console.WriteLine("Failed to connect to command socket. Control loop will not start."); + Finished = true; + } + } + else + { + Console.WriteLine("Failed to receive initial body state. Control loop will not start."); + Finished = true; + } + } + + public async Task StopAsync() + { + var wsCloseTask = Task.Run(async () => + { + _cts.Cancel(); + if (_ClientWebSocket != null && _ClientWebSocket.State == WebSocketState.Open) + { + await _ClientWebSocket.CloseAsync(WebSocketCloseStatus.NormalClosure, null, default); + } + wsAxisState = null; + _ClientWebSocket = null; + Finished = true; + JiboBodyState = null; + }); + + 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; + } + } + + private async Task SetupCommandSocket(string host) + { + if (wsAxisCommand == null) + { + wsAxisCommand = new Uri($"ws://{host}/axis_command"); + } + if (_ClientControlWebSocket == null || _ClientControlWebSocket.State != WebSocketState.Open) + { + _ClientControlWebSocket = null; + _ClientControlWebSocket = new ClientWebSocket(); + _ClientControlWebSocket.Options.KeepAliveInterval = TimeSpan.FromSeconds(60); + } + try + { + if (_ClientControlWebSocket.State != WebSocketState.Open) + { + await _ClientControlWebSocket.ConnectAsync(wsAxisCommand, _cts.Token); + Console.WriteLine("Command WebSocket connected."); + } + else + { + Console.WriteLine("Could not Open Command WebSocket."); + return false; + } + + return _ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open; + } + catch (Exception ex) + { + Console.WriteLine($"Couldn\'t connect to body service\'s command websocket: {ex.Message}"); + } + + return false; + } + + private async Task 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(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(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() != 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; + } + else + { + Console.WriteLine("Could not Open WebSocket."); + } + } + catch (Exception ex) + { + Console.WriteLine($"Couldn\'t connect to body service\'s state websocket: {ex.Message}"); + } + + return null; + } + + private void ControlLoop() + { + if (!_cts.Token.IsCancellationRequested) + { + if (JiboBodyState != null) + { + Task.Delay(500, _cts.Token).Wait(); + + if (Finished) + { + Console.WriteLine("Jibo was already positioned."); + StopAsync().Wait(); + } + else + { + Console.WriteLine("Positioning robot..."); + StartSpinningJibo(); + IndexCheckInterval = Interval.Set(() => { + if (Finished) + { + StopAsync().Wait(); + } + }, 100); + } + } + } + } + + private void StartSpinningJibo() + { + if (_ClientControlWebSocket != null && _ClientControlWebSocket.State == WebSocketState.Open) + { + SpinInterval = Interval.Set(() => { + + string? commandJson = BuildCommands(); + if (commandJson != null) + { + var bytes = Encoding.UTF8.GetBytes(commandJson); + var buffer = new ArraySegment(bytes); + try + { + _ClientControlWebSocket.SendAsync(buffer, WebSocketMessageType.Binary, true, _cts.Token).Wait(); + Console.WriteLine("Command sent: " + commandJson); + } + catch (Exception ex) + { + Console.WriteLine($"Error sending command: {ex.Message}"); + } + } + }, 1000); + } + else + { + Console.WriteLine("Cannot send command, control WebSocket is not open."); + } + } + + private float[] DegToRad(float[] poseArray) + { + for (int i = 0; i < poseArray.Length; ++i) + { + poseArray[i] = float.DegreesToRadians(poseArray[i]); + } + return poseArray; + } + + public void UpdateStatus() + { + if (JiboBodyState != null) + { + var pelvis = JiboBodyState.Pelvis; + var torso = JiboBodyState.Torso; + var neck = JiboBodyState.Neck; + + var tolerance = 0.1; + PelvisPositioned = (Math.Abs(Math.Abs(pelvis.Pos) - + Math.Abs(PoseArray[2])) < tolerance); + TorsoPositioned = (Math.Abs(Math.Abs(torso.Pos) - + Math.Abs(PoseArray[1])) < tolerance); + NeckPositioned = (Math.Abs(Math.Abs(neck.Pos) - + Math.Abs(PoseArray[0])) < tolerance); + + Finished = false; + } + else + { + Finished = true; + } + } + + private string? BuildCommands() + { + if (JiboBodyState == null) + { + return null; + } + + Console.WriteLine("Starting positioning sequence..."); + string vpelvis; + string vtorso; + string vneck; + + int val = 0; + int mode; + + if (!PelvisPositioned) + { + mode = POS_VEL; + } + else + { + mode = BREAK; + } + vpelvis = Cmd(mode, val, PoseArray[2], JiboBodyState.Pelvis.Pos, JiboBodyState.Pelvis); + + if (!TorsoPositioned) + { + mode = POS_VEL; + } + else + { + mode = BREAK; + } + vtorso = Cmd(mode, val, PoseArray[1], JiboBodyState.Torso.Pos, JiboBodyState.Torso); + + if (!NeckPositioned) + { + mode = POS_VEL; + } + else + { + mode = BREAK; + } + vneck = Cmd(mode, val, PoseArray[0], JiboBodyState.Neck.Pos, JiboBodyState.Neck); + + StringBuilder cmd = new StringBuilder(); + cmd.Append('{'); + cmd.AppendFormat("\"ts\": [{0}, {1}],", JiboBodyState.Ts?[0], JiboBodyState.Ts?[1]); + cmd.AppendFormat("\"pelvis\": {0},", vpelvis); + cmd.AppendFormat("\"torso\": {0},", vtorso); + cmd.AppendFormat("\"neck\": {0}", vneck); + cmd.Append('}'); + + return cmd.ToString(); + } + + private string Cmd(int mode, int vel, float desPos, float actPos, RobotBodyPart state) + { + StringBuilder cmd = new StringBuilder(); + cmd.Append('{'); + cmd.AppendFormat("\"mode\": {0},", mode); + cmd.AppendFormat("\"value\": [{0}, {1}],", vel, desPos); + cmd.AppendFormat("\"vel_limit\": {0},", state.VelLimit); + cmd.AppendFormat("\"acc_limit\": {0},", state.AccLimit); + cmd.AppendFormat("\"cur_limit\": {0}", state.CurLimit); + cmd.Append('}'); + + return cmd.ToString(); + } + +} \ No newline at end of file