Added postprocessor for Fanuc robots (LS Format) (#267)

* Added postprocessor for Fanuc robots (LS Format) * Added speed calculation for joint movements on Fanuc robots * Reverting changes in obsolete files
visose · Feb 5, 2024 · ac190db · ac190db
1 parent 0fbfab6
commit ac190db
Show file tree

Hide file tree

Showing 13 changed files with 445 additions and 2 deletions.
diff --git a/src/Robots.Grasshopper/Commands/CustomCommand.cs b/src/Robots.Grasshopper/Commands/CustomCommand.cs
@@ -10,7 +10,7 @@ public CustomCommand() : base("Custom command", "CustomCmd", "Custom command wri
     protected override void RegisterInputParams(GH_InputParamManager pManager)
     {
         pManager.AddTextParameter("Name", "N", "Name", GH_ParamAccess.item, "CustomCommand");
-        pManager.AddTextParameter("Manufacturer", "M", "Robot manufacturer, options:  ABB, KUKA, UR, Staubli, FrankaEmika, Doosan, Other, All. If you select 'All', the command will always be included irrespective of the manufacturer.", GH_ParamAccess.item, "All");
+        pManager.AddTextParameter("Manufacturer", "M", "Robot manufacturer, options:  ABB, KUKA, UR, Staubli, FrankaEmika, Doosan, Fanuc, Other, All. If you select 'All', the command will always be included irrespective of the manufacturer.", GH_ParamAccess.item, "All");
         pManager.AddTextParameter("Code", "C", "Command code", GH_ParamAccess.item);
         pManager.AddTextParameter("Declaration", "D", "Variable declaration and assignment", GH_ParamAccess.item);
         pManager[2].Optional = true;

diff --git a/src/Robots/Commands/Message.cs b/src/Robots/Commands/Message.cs
@@ -12,6 +12,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.Staubli, (_, _) => $"putln(\"{_message}\")");
         _commands.Add(Manufacturers.FrankaEmika, (_, _) => $"print(\"{_message}\")");
         _commands.Add(Manufacturers.Doosan, (_, _) => $"tp_log(\"{_message}\")");
+        _commands.Add(Manufacturers.Fanuc, (_, _) => $":MESSAGE[\"{_message}\"] ;");
     }
 
     public override string ToString() => $"Command (Message \"{_message}\")";

diff --git a/src/Robots/Commands/PulseDO.cs b/src/Robots/Commands/PulseDO.cs
@@ -18,6 +18,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.KUKA, CodeKuka);
         _commands.Add(Manufacturers.UR, CodeUR);
         _commands.Add(Manufacturers.Doosan, CodeDoosan);
+        _commands.Add(Manufacturers.Fanuc, CodeFanuc);
 
         _declarations.Add(Manufacturers.UR, DeclarationUR);
     }
@@ -63,6 +64,12 @@ string CodeDoosan(RobotSystem robotSystem, Target target)
         return $"set_digital_output({number}, ON, {_length:0.###}, OFF)";
     }
 
+    string CodeFanuc(RobotSystem robotSystem, Target target)
+    {
+        var number = GetNumber(robotSystem);
+        return $":DO[{number}]=PULSE, {_length:0.###}sec ;";
+    }
+
     string GetNumber(RobotSystem robotSystem)
     {
         var io = robotSystem.IO;

diff --git a/src/Robots/Commands/SetAO.cs b/src/Robots/Commands/SetAO.cs
@@ -18,6 +18,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.UR, CodeUR);
         _commands.Add(Manufacturers.Staubli, CodeStaubli);
         _commands.Add(Manufacturers.Doosan, CodeDoosan);
+        _commands.Add(Manufacturers.Fanuc, CodeFanuc);
 
         _declarations.Add(Manufacturers.ABB, DeclarationAbb);
         _declarations.Add(Manufacturers.KUKA, DeclarationKuka);
@@ -86,6 +87,12 @@ string CodeDoosan(RobotSystem robotSystem, Target target)
         return $"set_analog_output(ch={number}, val={Name})";
     }
 
+    string CodeFanuc(RobotSystem robotSystem, Target target)
+    {
+        var number = GetNumber(robotSystem);
+        return $":AO[{number}]={Value} ;";
+    }
+
     string GetNumber(RobotSystem robotSystem)
     {
         var io = robotSystem.IO;

diff --git a/src/Robots/Commands/SetDO.cs b/src/Robots/Commands/SetDO.cs
@@ -18,6 +18,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.UR, CodeUR);
         _commands.Add(Manufacturers.Staubli, CodeStaubli);
         _commands.Add(Manufacturers.Doosan, CodeDoosan);
+        _commands.Add(Manufacturers.Fanuc, CodeFanuc);
     }
 
     string CodeAbb(RobotSystem robotSystem, Target target)
@@ -66,6 +67,14 @@ string CodeDoosan(RobotSystem robotSystem, Target target)
         return $"set_digital_output({number}, {textValue})";
     }
 
+    string CodeFanuc(RobotSystem robotSystem, Target target)
+    {
+        var number = GetNumber(robotSystem);
+
+        string textValue = Value ? "ON" : "OFF";
+        return $":DO[{number}]={textValue} ;";
+    }
+
     string GetNumber(RobotSystem robotSystem)
     {
         var io = robotSystem.IO;

diff --git a/src/Robots/Commands/Stop.cs b/src/Robots/Commands/Stop.cs
@@ -11,6 +11,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.UR, (_, _) => "pause program");
         _commands.Add(Manufacturers.Doosan, (_, _) => "wait_nudge()");
         //_commands.Add(Manufacturers.Staubli, (_, __) => "wait(true)");
+        _commands.Add(Manufacturers.Fanuc, (_, _) => ":ABORT ;");
     }
 
     public override string ToString() => "Command (Stop)";

diff --git a/src/Robots/Commands/Wait.cs b/src/Robots/Commands/Wait.cs
@@ -12,6 +12,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.Staubli, CodeStaubli);
         _commands.Add(Manufacturers.FrankaEmika, CodePython);
         _commands.Add(Manufacturers.Doosan, CodeDoosan);
+        _commands.Add(Manufacturers.Fanuc, CodeFanuc);
 
         _declarations.Add(Manufacturers.ABB, DeclarationAbb);
         _declarations.Add(Manufacturers.KUKA, DeclarationKuka);
@@ -72,5 +73,10 @@ string CodeDoosan(RobotSystem robotSystem, Target target)
         return $"wait({Name})";
     }
 
+    string CodeFanuc(RobotSystem robotSystem, Target target)
+    {
+        return $":WAIT {Seconds:0.00}(sec) ;";
+    }
+
     public override string ToString() => $"Command (Wait {Seconds} secs)";
 }
diff --git a/src/Robots/Commands/WaitDI.cs b/src/Robots/Commands/WaitDI.cs
@@ -18,6 +18,7 @@ protected override void Populate()
         _commands.Add(Manufacturers.UR, CodeUR);
         _commands.Add(Manufacturers.Staubli, CodeStaubli);
         _commands.Add(Manufacturers.Doosan, CodeDoosan);
+        _commands.Add(Manufacturers.Fanuc, CodeFanuc);
     }
 
     string CodeAbb(RobotSystem robotSystem, Target target)
@@ -56,6 +57,14 @@ string CodeDoosan(RobotSystem robotSystem, Target target)
         return $"wait_digital_input({number}, {value})";
     }
 
+    string CodeFanuc(RobotSystem robotSystem, Target target)
+    {
+        var number = GetNumber(robotSystem);
+        if (value) { return $":WAIT (DI[{number}]) ;"; }
+        else{ return $":WAIT (!DI[{number}]) ;"; }
+
+    }
+
     string GetNumber(RobotSystem robotSystem)
     {
         var io = robotSystem.IO;

diff --git a/src/Robots/IO/FileIO.cs b/src/Robots/IO/FileIO.cs
@@ -167,6 +167,7 @@ static RobotSystem CreateRobotSystem(XElement element, Plane basePlane, bool loa
             Manufacturers.Staubli => new SystemStaubli(name, mechanicalGroups, io, basePlane, environment),
             Manufacturers.FrankaEmika => new SystemFranka(name, (RobotFranka)mechanicalGroups[0].Robot, io, basePlane, environment),
             Manufacturers.Doosan => new SystemDoosan(name, (RobotDoosan)mechanicalGroups[0].Robot, io, basePlane, environment),
+            Manufacturers.Fanuc => new SystemFanuc(name, mechanicalGroups, io, basePlane, environment),
 
             _ => throw new ArgumentException($" Manufacturer '{manufacturer} is not supported.")
         };
@@ -241,6 +242,7 @@ static Mechanism CreateMechanism(XElement element, bool loadMeshes)
                 Manufacturers.Staubli => new RobotStaubli(model, payload, basePlane, baseMesh, joints),
                 Manufacturers.FrankaEmika => new RobotFranka(model, payload, basePlane, baseMesh, joints),
                 Manufacturers.Doosan => new RobotDoosan(model, payload, basePlane, baseMesh, joints),
+                Manufacturers.Fanuc => new RobotFanuc(model, payload, basePlane, baseMesh, joints),
                 _ => throw new ArgumentException($" Manufacturer '{manufacturer}' not supported."),
             },
             "Positioner" => new Positioner(model, manufacturer, payload, basePlane, baseMesh, joints, movesRobot),