Jibo-Revival-Group/OriginalSDK
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Updated now actually moves the robot

Browse Source 
Hit Enter to add a new position, default is 0,0,0
This commit is contained in:
Zetoman
2026-04-03 16:23:49 +00:00
parent 279b1690d5
commit 658c8e0e57
1 changed files with 283 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

375
JiboBodyService/Program.cs
View File

@@ -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 ");
var positionString = (Console.ReadLine() ?? "").Trim();
Console.Write("Enter Jibo new Pose Body <Neck,Torso,Pelvis>" +
"\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<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,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<byte>(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<byte>(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<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...");
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<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();
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);