protected override void SolveInstance(IGH_DataAccess DA)
{
MachinaBridgeSocket ms = null;
List <Machina.Action> actions = new List <Machina.Action>();
bool send = false;
if (!DA.GetData(0, ref ms))
{
return;
}
if (!DA.GetDataList(1, actions))
{
return;
}
if (!DA.GetData(2, ref send))
{
return;
}
if (ms.socket == null || !ms.socket.IsAlive)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Error, "Not valid Bridge connection.");
return;
}
List <string> instructions = new List <string>();
if (send)
{
string ins = "";
foreach (Machina.Action a in actions)
{
// If attaching a tool, send the tool description first.
// This is quick and dirty, a result of this component not taking the robot object as an input.
// How coud this be improved...? Should tool creation be an action?
if (a.type == Machina.ActionType.Attach)
{
ActionAttach aa = (ActionAttach)a;
ins = aa.tool.ToInstruction();
instructions.Add(ins);
ms.socket.Send(ins);
}
ins = a.ToInstruction();
instructions.Add(ins);
ms.socket.Send(ins);
}
DA.SetData(0, "Sent!");
}
else
{
DA.SetData(0, "Nothing sent");
}
DA.SetDataList(1, instructions);
}
public override byte[] GetBytesForNextAction(RobotCursor cursor)
{
if (!cursor.ApplyNextAction(out _action))
{
return(null); // cursor buffer is empty
}
_params = null;
switch (_action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
RotationVector rv = cursor.rotation.AA.ToRotationVector();
_params = new int[]
{
_action.id,
cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL,
(int)Math.Round(cursor.position.X * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M),
(int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Axes:
_params = new int[]
{
_action.id,
INST_MOVEJ_Q,
(int)Math.Round(cursor.joints.J1 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J2 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J3 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J4 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J5 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J6 * Geometry.TO_RADS * FACTOR_RAD),
};
break;
case ActionType.Wait:
ActionWait aa = _action as ActionWait;
_params = new int[]
{
_action.id,
INST_SLEEP,
(int)Math.Round(aa.millis * 0.001 * FACTOR_SEC)
};
break;
// Not implemented yet
//case ActionType.Message:
// ActionMessage am = (ActionMessage)action;
// dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
// am.message);
// break;
case ActionType.Attach:
ActionAttach aatt = _action as ActionAttach;
Tool t = aatt.tool;
RotationVector trv = t.TCPOrientation.ToRotationVector();
_params = new int[]
{
_action.id,
INST_SET_TOOL,
(int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M),
(int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(t.Weight * FACTOR_M)
};
break;
case ActionType.Detach:
_params = new int[]
{
_action.id,
INST_SET_TOOL,
0,
0,
0,
0,
0,
0,
0
};
break;
case ActionType.IODigital:
ActionIODigital aiod = _action as ActionIODigital;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aiod.pinNum,
aiod.on ? 1 : 0
};
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = _action as ActionIOAnalog;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aioa.pinNum,
(int)Math.Round(aioa.value * FACTOR_VOLT)
};
break;
// ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗
// ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗
// ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝
case ActionType.Speed:
_params = new int[]
{
_action.id,
INST_TCP_SPEED,
(int)Math.Round(cursor.speed * 0.001 * FACTOR_M)
};
break;
case ActionType.Acceleration:
_params = new int[]
{
_action.id,
INST_TCP_ACC,
(int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)
};
break;
case ActionType.JointSpeed:
_params = new int[]
{
_action.id,
INST_Q_SPEED,
(int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.JointAcceleration:
_params = new int[]
{
_action.id,
INST_Q_ACC,
(int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Precision:
_params = new int[]
{
_action.id,
INST_BLEND,
(int)Math.Round(cursor.precision * 0.001 * FACTOR_M)
};
break;
case ActionType.PushPop:
ActionPushPop app = _action as ActionPushPop;
if (app.push)
{
break;
}
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
Dictionary <int, int> poppedSettings = new Dictionary <int, int>();
// These are the states kept in the controller as of v1.0 of the driver
if (beforePop.Speed != cursor.speed)
{
poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
}
if (beforePop.Acceleration != cursor.acceleration)
{
poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
}
if (beforePop.JointSpeed != cursor.jointSpeed)
{
poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.JointAcceleration != cursor.jointAcceleration)
{
poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.Precision != cursor.precision)
{
poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M));
}
// Generate a buffer with all instructions, ids of -1 except for the last one.
_params = new int[3 * poppedSettings.Count];
int it = 0;
foreach (var setting in poppedSettings)
{
_params[3 * it] = it == poppedSettings.Count - 1 ? app.id : -1; // only attach the real id to the last instruction
_params[3 * it + 1] = setting.Key;
_params[3 * it + 2] = setting.Value;
it++;
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
}
if (_params == null)
{
return(null);
}
_buffer = Util.Int32ArrayToByteArray(_params, false);
return(_buffer);
}
protected override void SolveInstance(IGH_DataAccess DA)
{
string url = "";
bool connect = false;
List <Machina.Action> actions = new List <Machina.Action>();
bool send = false;
if (!DA.GetData(0, ref url))
{
return;
}
if (!DA.GetData(1, ref connect))
{
return;
}
if (!DA.GetDataList(2, actions))
{
return;
}
if (!DA.GetData(3, ref send))
{
return;
}
List <string> instructions = new List <string>();
bool connectedResult;
if (connect)
{
if (_ws == null || !_ws.IsAlive)
{
_ws = new WebSocket(url);
_ws.Connect();
}
connectedResult = _ws.IsAlive;
if (!connectedResult)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Error, "Could not connect to Machina Bridge App");
return;
}
}
else
{
if (_ws != null)
{
_ws.Close();
}
connectedResult = _ws.IsAlive;
}
DA.SetData(0, connectedResult);
if (send && connectedResult)
{
string ins = "";
foreach (Machina.Action a in actions)
{
// If attaching a tool, send the tool description first.
// This is quick and dirty, a result of this component not taking the robot object as an input.
// How coud this be improved...? Should tool creation be an action?
if (a.type == Machina.ActionType.Attach)
{
ActionAttach aa = (ActionAttach)a;
ins = aa.tool.ToInstruction();
instructions.Add(ins);
_ws.Send(ins);
}
ins = a.ToInstruction();
instructions.Add(ins);
_ws.Send(ins);
}
DA.SetData(1, "Sent!");
}
else
{
DA.SetData(1, "Nothing sent");
}
DA.SetDataList(2, instructions);
}
internal const int RES_JOINTS = 22; // ">22 0 0 0 0 90 0;"
// const int RES_EXTAX = 23;
/// <summary>
/// Given an Action and a RobotCursor representing the state of the robot after application,
/// return a List of strings with the messages necessary to perform this Action adhering to
/// the Machina-ABB-Server protocol.
/// </summary>
/// <param name="action"></param>
/// <param name="cursor"></param>
/// <returns></returns>
internal override List <string> GetActionMessages(Action action, RobotCursor cursor)
{
List <string> msgs = new List <string>();
switch (action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
//// MoveL/J X Y Z QW QX QY QZ
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9}{10}",
STR_MESSAGE_ID_CHAR,
action.id,
cursor.motionType == MotionType.Linear ? INST_MOVEL : INST_MOVEJ,
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.rotation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Axes:
// MoveAbsJ J1 J2 J3 J4 J5 J6
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8}{9}",
STR_MESSAGE_ID_CHAR,
action.id,
INST_MOVEABSJ,
Math.Round(cursor.joints.J1, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J2, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J3, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J4, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J5, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J6, Geometry.STRING_ROUND_DECIMALS_DEGS),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Speed:
// (setspeed V_TCP[V_ORI V_LEAX V_REAX])
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}"); // this accepts more velocity params, but those are still not implemented in Machina...
break;
case ActionType.Precision:
// (setzone FINE TCP[ORI EAX ORI LEAX REAX])
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); // this accepts more zone params, but those are still not implemented in Machina...
break;
case ActionType.Wait:
// !WaitTime T
ActionWait aw = (ActionWait)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_WAITTIME} {0.001 * aw.millis}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.Message:
// !TPWrite "MSG"
ActionMessage am = (ActionMessage)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_TPWRITE} \"{am.message}\"{STR_MESSAGE_END_CHAR}");
break;
case ActionType.Attach:
// !(settool X Y Z QW QX QY QZ KG CX CY CZ)
ActionAttach aa = (ActionAttach)action;
Tool t = aa.tool;
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13}{14}",
STR_MESSAGE_ID_CHAR,
action.id,
INST_TOOL,
Math.Round(t.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPOrientation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.Weight, Geometry.STRING_ROUND_DECIMALS_KG),
Math.Round(t.centerOfGravity.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.centerOfGravity.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.centerOfGravity.Z, Geometry.STRING_ROUND_DECIMALS_MM),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Detach:
// !(settool0)
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_NOTOOL}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IODigital:
// !SetDO "NAME" ON
ActionIODigital aiod = (ActionIODigital)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETDO} \"{aiod.pinName}\" {(aiod.on ? 1 : 0)}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IOAnalog:
// !SetAO "NAME" V
ActionIOAnalog aioa = (ActionIOAnalog)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETAO} \"{aioa.pinName}\" {aioa.value}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.PushPop:
ActionPushPop app = action as ActionPushPop;
if (app.push)
{
return(null);
}
else
{
// Only precision and speed are states kept on the controller
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
if (beforePop.Speed != cursor.speed)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}");
}
if (beforePop.Precision != cursor.precision)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}");
}
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
case ActionType.ExternalAxis:
string msg = $"{STR_MESSAGE_ID_CHAR}{action.id} {INST_EXT_JOINTS} ";
for (int i = 0; i < cursor.externalAxes.Length; i++)
{
// RAPID's StrToVal() will parse 9E9 into a 9E+9 num value, and ignore that axis on motions
msg += cursor.externalAxes[i]?.ToString() ?? "9E9";
if (i < cursor.externalAxes.Length - 1)
{
msg += " ";
}
}
msg += STR_MESSAGE_END_CHAR;
msgs.Add(msg);
break;
// CustomCode --> is non-streamable
// If the Action wasn't on the list above, it doesn't have a message representation...
default:
return(null);
}
return(msgs);
}
