544 lines
16 KiB
C#
544 lines
16 KiB
C#
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();
|
|
}
|
|
|
|
} |