- TCP position and Axes are now displayed upon connection.

- Fix typos on UR compiler
- Reverted back to UR `movel` motion on online due to SW3.0 problems.
- Additional error handling for UR dis/connection.

CHANGELOG.md

@@ -19,13 +19,6 @@
 - [ ] The `HUMAN` compiler doesn't export actions with the real/abs and axis/cartesian problem, even though it should since it doesn't really need to apply the actions to the writer... Same for 'MACHINA' compiler...
 - [ ] `Do()` takes a Machina string, and deserializes it into an action! :)
 - [ ] Replace all Bridge parsing to just use `Do()` and the string instruction.
-- [ ] Have UR driver send handshake information (and update cursor)
-```
-  get_actual_joint_positions()
-  get_actual_tcp_pose()
-  get_inverse_kin(x, qnear)
-```
-- [ ] Have Machina display TCP position and Axes upon any connection.
 - [ ] Add 'EASE' mode to motion, as an option for UR robots to do MoveL
 
 
@@ -41,6 +34,10 @@
 ## BUILD 1424
 - UR now initializes state (no need to start with a `Transform`...)
 - Improved native type stringification.
+- TCP position and Axes are now displayed upon connection.
+- Fix typos on UR compiler
+- Reverted back to UR `movel` motion on online due to SW3.0 problems.
+- Additional error handling for UR dis/connection.
 
 ---
 # v0.8.4

src/Machina/Compilers/CompilerUR.cs

@@ -364,7 +364,7 @@ internal static bool GenerateInstructionDeclaration(
                     if (cursor.motionType == MotionType.Joint)
                     {
                         dec = string.Format(CultureInfo.InvariantCulture, 
-                            "  movej(target{0}, a={1}, v={2}, r={3})",
+                            "  movej({0}, a={1}, v={2}, r={3})",
                             GetPoseTargetValue(cursor),
                             Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
                             Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),
@@ -373,7 +373,7 @@ internal static bool GenerateInstructionDeclaration(
                     else
                     {
                         dec = string.Format(CultureInfo.InvariantCulture, 
-                            "  movep(target{0}, a={1}, v={2}, r={3})",
+                            "  movep({0}, a={1}, v={2}, r={3})",
                             GetPoseTargetValue(cursor),
                             Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M),
                             Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M),
@@ -403,7 +403,7 @@ internal static bool GenerateInstructionDeclaration(
 
                 case ActionType.Axes:
                     dec = string.Format(CultureInfo.InvariantCulture, 
-                        "  movej(target{0}, a={1}, v={2}, r={3})",
+                        "  movej({0}, a={1}, v={2}, r={3})",
                         GetJointTargetValue(cursor),
                         Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
                         Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),

src/Machina/Control.cs

@@ -368,6 +368,20 @@ public bool ConnectToDevice(string ip, int port)
             }
 
             logger.Info("Connected to " + parentRobot.Brand + " robot \"" + parentRobot.Name + "\" on " + _driver.IP + ":" + _driver.Port);
+            logger.Info("TCP:");
+            logger.Info("  " + this.IssueCursor.position);
+            logger.Info("  " + new Orientation(this.IssueCursor.rotation));
+            logger.Info("  " + this.IssueCursor.axes);
+            if (this.IssueCursor.externalAxesCartesian != null)
+            {
+                logger.Info("External Axes (TCP):");
+                logger.Info("  " + this.IssueCursor.externalAxesCartesian);
+            }
+            if (this.IssueCursor.externalAxesJoints != null)
+            {
+                logger.Info("External Axes (J): ");
+                logger.Info("  " + this.IssueCursor.externalAxesJoints);
+            }
             return true;
         }
 

src/Machina/Drivers/Communication/Protocols/URCommunicationProtocol.cs

@@ -99,7 +99,7 @@ public override byte[] GetBytesForNextAction(RobotCursor cursor)
                     _params = new int[]
                     {
                         _action.Id,
-                        cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEP,
+                        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),

src/Machina/Drivers/Communication/TCPCommunicationManagerUR.cs

@@ -76,7 +76,7 @@ internal class TCPCommunicationManagerUR
         private TcpClient _robotSocket;
         private NetworkStream _robotNetworkStream;
 
-        private string _driverScript;
+        //private string _driverScript;
 
         private Protocols.Base _translator;
         //private StringBuilder _sb = new StringBuilder();
@@ -116,25 +116,36 @@ internal bool Disconnect()
             if (_clientSocket != null)
             {
                 // Upload an empty script to stop the running program
-                LoadEmptyScript();
-                UploadScriptToDevice(_driverScript, false);
-
-                ClientSocketStatus = TCPConnectionStatus.Disconnected;
-                _clientSocket.Client.Disconnect(false);
-                _clientSocket.Close();
-                if (_clientNetworkStream != null) _clientNetworkStream.Dispose();
+                string emptyScript = LoadEmptyScript();
+                if (!UploadScriptToDevice(emptyScript, false))
+                {
+                    Logger.Error("Could not load empty script to robot");
+                }
 
-                _isServerListeningRunning = false;
+                try
+                {
+                    ClientSocketStatus = TCPConnectionStatus.Disconnected;
+                    _clientSocket.Client.Disconnect(false);
+                    _clientSocket.Close();
+                    if (_clientNetworkStream != null) _clientNetworkStream.Dispose();
 
-                // TESTING
-                _robotSocket.Close();
-                _robotSocket.Dispose();
-                _robotSocket = null;
+                    _isServerListeningRunning = false;
 
-                _serverSocket.Stop();
-                _serverSocket = null;
+                    // TESTING
+                    _robotSocket.Close();
+                    _robotSocket.Dispose();
+                    _robotSocket = null;
 
-                return true;
+                    _serverSocket.Stop();
+                    _serverSocket = null;
+                    return true;
+                }
+                catch (Exception ex)
+                {
+                    logger.Error("Something went wrong on disconnection:");
+                    logger.Error(ex);
+                    return false;
+                }
             }
 
             return false;
@@ -174,8 +185,13 @@ internal bool Connect()
                 _serverListeningThread.IsBackground = true;
                 _serverListeningThread.Start();
 
-                LoadDriverScript();
-                UploadScriptToDevice(_driverScript, false);
+                string drScript = LoadDriverScript();
+                if (!UploadScriptToDevice(drScript, false))
+                {
+                    logger.Error("Could not upload driver to robot");
+                    Disconnect();
+                    return false;
+                }
 
                 if (!WaitForInitialization())
                 {
@@ -190,7 +206,8 @@ internal bool Connect()
             {
                 logger.Error("Something went wrong trying to connect to robot...");
                 logger.Debug(ex);
-                throw new Exception();
+                Disconnect();
+                return false;
             }
 
             //return false;
@@ -308,7 +325,16 @@ private void ServerReceivingMethod(object obj)
 
                 // Perform a blocking call to accept requests.
                 // You could also user server.AcceptSocket() here.
-                _robotSocket = _serverSocket.AcceptTcpClient();
+                try
+                {
+                    _robotSocket = _serverSocket.AcceptTcpClient();
+                }
+                catch (Exception ex)
+                {
+                    logger.Error("Something went wrong waiting for client connection");
+                    logger.Error(ex);
+                    continue;  // does this work in a while loop?
+                }
                 logger.Verbose("Connected client: " + _robotIP);
 
                 _serverSendingThread = new Thread(ServerSendingMethod);
@@ -376,10 +402,6 @@ private void ServerReceivingMethod(object obj)
                     logger.Error(e);
                 }
 
-                //Console.WriteLine("Closing client");
-                //_robotSocket.Close();
-                //_robotSocket = null;
-
                 Thread.Sleep(30);
             }
 
@@ -533,31 +555,46 @@ private bool ProcessResponse(List<int> values)
             return true;
         }
 
-        private bool LoadEmptyScript()
+        private string LoadEmptyScript()
         {
-            _driverScript = Machina.IO.ReadTextResource("Machina.Resources.DriverModules.UR.empty.script");
-            return true;
+            string emptyScript = Machina.IO.ReadTextResource("Machina.Resources.DriverModules.UR.empty.script");
+            return emptyScript;
         }
 
-        private bool LoadDriverScript()
+        private string LoadDriverScript()
         {
-            _driverScript = Machina.IO.ReadTextResource("Machina.Resources.DriverModules.UR.machina_ur_driver.script");
+            string driverScript = Machina.IO.ReadTextResource("Machina.Resources.DriverModules.UR.machina_ur_driver.script");
 
             // @TODO: remove comments, trailing spaces and empty lines from script
-            _driverScript = _driverScript.Replace("{{HOSTNAME}}", _serverIP);
-            _driverScript = _driverScript.Replace("{{PORT}}", _serverPort.ToString());
+            driverScript = driverScript.Replace("{{HOSTNAME}}", _serverIP);
+            driverScript = driverScript.Replace("{{PORT}}", _serverPort.ToString());
 
-            return true;
+            return driverScript;
         }
 
 
         private bool UploadScriptToDevice(string script, bool consoleDump = false)
         {
+            if (ClientSocketStatus != TCPConnectionStatus.Connected)
+            {
+                logger.Error("Not connected to device");
+                return false;
+            }
+
             logger.Verbose("Uploading module to device...");
             if (consoleDump) logger.Debug(script);
 
-            _sendMsgBytes = Encoding.ASCII.GetBytes(script);
-            _clientNetworkStream.Write(_sendMsgBytes, 0, _sendMsgBytes.Length);
+            try
+            {
+                _sendMsgBytes = Encoding.ASCII.GetBytes(script);
+                _clientNetworkStream.Write(_sendMsgBytes, 0, _sendMsgBytes.Length);
+            }
+            catch (Exception ex)
+            {
+                logger.Error("Something went wrong trying to upload module to robot:");
+                logger.Error(ex);
+                return false;
+            }
 
             return true;
         }

src/Machina/Resources/DriverModules/UR/machina_ur_driver.script

@@ -71,7 +71,7 @@ def machina_ur_driver():
   state_acceleration = 0.2
   state_joint_speed = 0.35
   state_joint_acceleration = 0.7
-  state_blend_radius = 0.01
+  state_blend_radius = 0.0          # if not zero, targets with same TCP position (like rotations) won't execute!
 
   # FUNCTIONS --------------------------------------------------
 
@@ -129,6 +129,15 @@ def machina_ur_driver():
         pose = p[params6[1] / FACTOR_M, params6[2] / FACTOR_M, params6[3] / FACTOR_M, params6[4] / FACTOR_RAD, params6[5] / FACTOR_RAD, params6[6] / FACTOR_RAD]
         movel(pose, state_acceleration, state_speed, 0, state_blend_radius)
 
+      # This motion maintains linear speed and does radius blending
+      # However, for motion between targets with same TCP **position** (like a relative rotation), 
+      # it won't perform the motion because of blending proximity (SW3.0)...  
+      # Sticking mainly to movel for now.
+      elif action_code == INST_MOVEP:
+        params6 = socket_read_binary_integer(6)
+        pose = p[params6[1] / FACTOR_M, params6[2] / FACTOR_M, params6[3] / FACTOR_M, params6[4] / FACTOR_RAD, params6[5] / FACTOR_RAD, params6[6] / FACTOR_RAD]
+        movep(pose, state_acceleration, state_speed, state_blend_radius)
+
       elif action_code == INST_MOVEJ_P:
         params6 = socket_read_binary_integer(6)
         pose = p[params6[1] / FACTOR_M, params6[2] / FACTOR_M, params6[3] / FACTOR_M, params6[4] / FACTOR_RAD, params6[5] / FACTOR_RAD, params6[6] / FACTOR_RAD]
@@ -138,12 +147,6 @@ def machina_ur_driver():
         params6 = socket_read_binary_integer(6)
         joints = [params6[1] / FACTOR_RAD, params6[2] / FACTOR_RAD, params6[3] / FACTOR_RAD, params6[4] / FACTOR_RAD, params6[5] / FACTOR_RAD, params6[6] / FACTOR_RAD]
         movej(joints, state_joint_acceleration, state_joint_speed, 0, state_blend_radius)
-
-      # This motion maintains linear speed and does radius blending
-      elif action_code == INST_MOVEP:
-        params6 = socket_read_binary_integer(6)
-        pose = p[params6[1] / FACTOR_M, params6[2] / FACTOR_M, params6[3] / FACTOR_M, params6[4] / FACTOR_RAD, params6[5] / FACTOR_RAD, params6[6] / FACTOR_RAD]
-        movep(pose, state_acceleration, state_speed, state_blend_radius)
 
       elif action_code == INST_TCP_SPEED:
         params1 = socket_read_binary_integer(1)

src/Machina/Types/Orientation.cs

@@ -159,7 +159,7 @@ internal Orientation(Rotation r)
         public override string ToString()
         {
             return string.Format(CultureInfo.InvariantCulture, 
-                "[X:[{0}, {1}, {2}], Y:[{3}, {4}, {5}], Z:[{6}, {7}, {8}]]",
+                "Orientation[X:[{0}, {1}, {2}], Y:[{3}, {4}, {5}], Z:[{6}, {7}, {8}]]",
                 Math.Round(this.RM.m00, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m10, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m20, STRING_ROUND_DECIMALS_MM),
                 Math.Round(this.RM.m01, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m11, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m21, STRING_ROUND_DECIMALS_MM),
                 Math.Round(this.RM.m02, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m12, STRING_ROUND_DECIMALS_MM), Math.Round(this.RM.m22, STRING_ROUND_DECIMALS_MM));

src/Tests/TEST_NewAPITests/OfflineAPITests.cs

@@ -14,9 +14,11 @@ static void Main(string[] args)
         {
             Console.WriteLine("--> GENERAL TEST");
 
-            Robot arm = Robot.Create("ToolTesting", "ABB");
+            Robot arm = Robot.Create("ToolTesting", "UR");
 
-            ToolTesting(arm);
+            RotationTest(arm);
+
+            //ToolTesting(arm);
 
             //HelloRobot(arm);
 
@@ -88,7 +90,7 @@ static void Main(string[] args)
             arm.Compile(arm.IsBrand("ABB") ? @"C:\offlineTests.prg" :
                 arm.IsBrand("UR") ? @"C:\offlineTests.script" :
                 arm.IsBrand("KUKA") ? @"C:\offlineTests.src" :
-                arm.IsBrand("ZMORPH") ? @"C:\offlineTests.gcode" : @"C:\offlineTests.machina", true, true);
+                arm.IsBrand("ZMORPH") ? @"C:\offlineTests.gcode" : @"C:\offlineTests.machina", true, false);
 
             //List<string> code = arm.Export();
             //foreach (string s in code) Console.WriteLine(s);
@@ -101,6 +103,20 @@ static void Main(string[] args)
             Console.ReadKey();
         }
 
+        static void RotationTest(Robot bot)
+        {
+            bot.SpeedTo(50);
+            bot.Precision(1);
+            bot.TransformTo(600, 100, 500, -1, 0, 0, 0, 1, 0);
+            bot.Rotate(0, 1, 0, -90);
+            bot.Move(0, 200, 0);
+            bot.Move(0, 0, 200);
+            bot.Rotate(0, 1, 0, 90);
+            bot.Move(0, -200, 0);
+            bot.Move(0, 0, -200);
+            bot.AxesTo(0, -90, -90, -90, 90, 90);
+        }
+
 
         static void ToolTesting(Robot bot)
         {
@@ -140,13 +156,15 @@ static public void HelloRobot(Robot arm)
         {
             arm.ControlMode("offline");
 
-            arm.Message("Hello Robot!");
-            arm.SpeedTo(100);
-            arm.MoveTo(400, 300, 500);
+            //arm.Message("Hello Robot!");
+            arm.SpeedTo(50);
+            arm.MoveTo(600, 0, 500);
             arm.Rotate(0, 1, 0, -90);
             arm.Move(0, 0, 250);
             arm.Wait(2000);
-            arm.AxesTo(0, 0, 0, 0, 90, 0);
+            //arm.AxesTo(0, 0, 0, 0, 90, 0);
+            arm.AxesTo(0, -90, -90, -90, 90, 90);
+
 
             //List<string> program = arm.Compile();
             //Console.WriteLine(program);