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); }