Upload files to "JiboBodyService"
Initial Commit
This commit is contained in:
25
JiboBodyService/Playground.sln
Normal file
25
JiboBodyService/Playground.sln
Normal file
@@ -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
|
||||
544
JiboBodyService/Program.cs
Normal file
544
JiboBodyService/Program.cs
Normal file
@@ -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 <Neck,Torso,Pelvis> 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<float>? 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<float>? 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<bool> 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<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(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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
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<byte>(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();
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user