From d78a3f9880b9c8244af299d82faa5600c04600cd Mon Sep 17 00:00:00 2001 From: visose Date: Fri, 22 Nov 2024 14:29:19 +0000 Subject: [PATCH] Fix init command, throw exception for Ignus if Declaration is used, formatting --- src/Robots/Commands/Command.cs | 26 ++++++---- src/Robots/Commands/Custom.cs | 3 +- .../PostProcessors/IgusPostProcessor.cs | 49 ++++++++++++------- src/Robots/Remotes/URRealTime.cs | 4 +- src/Robots/RobotArms/RobotIgus.cs | 4 ++ src/Robots/RobotSystems/SystemIgus.cs | 13 ++--- tests/Robots.Tests/ABBTests.cs | 2 +- tests/Robots.Tests/Performance.cs | 5 +- tests/Robots.Tests/URTests.cs | 2 +- 9 files changed, 65 insertions(+), 43 deletions(-) diff --git a/src/Robots/Commands/Command.cs b/src/Robots/Commands/Command.cs index a9ea03e..52baaee 100644 --- a/src/Robots/Commands/Command.cs +++ b/src/Robots/Commands/Command.cs @@ -1,29 +1,37 @@ + namespace Robots; public abstract class Command(string? name = null) : TargetAttribute(name) { public static Command Default { get; } = new Commands.Custom("DefaultCommand"); - protected Dictionary> _declarations = new(6); - protected Dictionary> _commands = new(6); + protected Dictionary> _declarations = new(8); + protected Dictionary> _commands = new(8); protected virtual void ErrorChecking(RobotSystem robotSystem) { } + protected virtual void Populate() { } public bool RunBefore { get; set; } + internal virtual IEnumerable Flatten() { if (this != Default) yield return this; } - internal string Declaration(Program program) + void Init(RobotSystem robot) { - var robot = program.RobotSystem; - ErrorChecking(robot); + if (_commands.Count > 0 || _declarations.Count > 0) + return; - _declarations.Clear(); - _commands.Clear(); + ErrorChecking(robot); Populate(); + } + + internal string Declaration(Program program) + { + var robot = program.RobotSystem; + Init(robot); if (_declarations.TryGetValue(robot.Manufacturer, out var declaration)) return declaration(robot); @@ -36,10 +44,8 @@ internal string Declaration(Program program) internal string Code(Program program, Target target) { - _declarations.Clear(); - _commands.Clear(); - Populate(); var robot = program.RobotSystem; + Init(robot); if (_commands.TryGetValue(robot.Manufacturer, out var command)) return command(robot, target); diff --git a/src/Robots/Commands/Custom.cs b/src/Robots/Commands/Custom.cs index eada335..be3f38c 100644 --- a/src/Robots/Commands/Custom.cs +++ b/src/Robots/Commands/Custom.cs @@ -1,4 +1,5 @@ -namespace Robots.Commands; + +namespace Robots.Commands; public class Custom : Command { diff --git a/src/Robots/PostProcessors/IgusPostProcessor.cs b/src/Robots/PostProcessors/IgusPostProcessor.cs index 9c7f1b3..aaff71b 100644 --- a/src/Robots/PostProcessors/IgusPostProcessor.cs +++ b/src/Robots/PostProcessors/IgusPostProcessor.cs @@ -1,6 +1,6 @@ using static System.Math; + namespace Robots; -using System.Linq; class IgusPostProcessor : IPostProcessor { @@ -19,7 +19,6 @@ class PostInstance public PostInstance(SystemIgus system, Program program) { - _system = system; _program = program; bool isMultiProgram = _program.MultiFileIndices.Count > 1; @@ -42,8 +41,8 @@ public PostInstance(SystemIgus system, Program program) groupCode.Add(SubModule(j)); } } - Code = [groupCode]; + Code = [groupCode]; } List MainModule() @@ -55,12 +54,11 @@ List MainModule() code.Add(""); code.Add("
"); - //_program. + if (is_multiProgram) { for (int i = 0; i < _program.MultiFileIndices.Count; i++) code.Add($""); - } else { @@ -74,19 +72,26 @@ List MainModule() List SubModule(int index) { - var code = new List - { + List code = + [ "", "", "
" - }; + ]; + if (index == 0) + { code.AddRange(TargetsCode(0, _program.MultiFileIndices[index + 1])); + } else if (index == _program.MultiFileIndices.Count - 1) + { code.AddRange(TargetsCode(_program.MultiFileIndices.Last(), _program.Targets.Count)); + } else + { code.AddRange(TargetsCode(_program.MultiFileIndices[index], _program.MultiFileIndices[index + 1])); + } code.Add(""); @@ -97,31 +102,32 @@ string ToolName() { var attributes = _program.Attributes; var toolsNames = new List(); + foreach (var tool in attributes.OfType().Where(t => !t.UseController)) { if (toolsNames.Count == 0) toolsNames.Add(tool.Name); } - if (toolsNames.Count == 0) - return ""; - else - return toolsNames[0];//We are only allowed to have a single tool + return toolsNames.Count switch + { + 0 => "", + _ => toolsNames[0] //We are only allowed to have a single tool + }; } List TargetsCode(int startIndex, int endIndex) { - - var instructions = new List(); + List instructions = []; int lineCounter = 1; for (int group = 0; group < _system.MechanicalGroups.Count; group++) { for (int j = startIndex; j < endIndex; j++) { - var programTarget = _program.Targets[j].ProgramTargets[group]; var target = programTarget.Target; + if (j == 0) { foreach (var command in _program.InitCommands) @@ -133,6 +139,7 @@ List TargetsCode(int startIndex, int endIndex) } string moveText = ""; + if (_system.MechanicalGroups[group].Externals.Count > 0) { _program.Warnings.Add("External axes not implemented in Staubli."); @@ -188,6 +195,15 @@ List TargetsCode(int startIndex, int endIndex) } } + + foreach(var command in programTarget.Commands) + { + var declaration = command.Declaration(_program); + + if (!string.IsNullOrWhiteSpace(declaration)) + throw new NotImplementedException("Declaration of a command is not implemented for Igus robots."); + } + foreach (var command in programTarget.Commands.Where(c => c.RunBefore)) { var instructionsStr = AddNumber(command.Code(_program, target), lineCounter); @@ -206,6 +222,7 @@ List TargetsCode(int startIndex, int endIndex) } } } + return instructions; } @@ -223,9 +240,7 @@ string AddNumber(string input, int number) { _program.Errors.Add("Error at add_number function!"); return "Error at add_number function"; - } } - } } diff --git a/src/Robots/Remotes/URRealTime.cs b/src/Robots/Remotes/URRealTime.cs index 107e215..b0435b4 100644 --- a/src/Robots/Remotes/URRealTime.cs +++ b/src/Robots/Remotes/URRealTime.cs @@ -1,4 +1,4 @@ -using System.Net; +using System.Net; using System.Net.Sockets; using System.Text; @@ -84,7 +84,7 @@ void UpdateBuffer() using var client = GetClient(); var stream = client.GetStream(); - stream.Read(_buffer, 0, _bufferLength); + _ = stream.Read(_buffer, 0, _bufferLength); Array.Reverse(_buffer); } diff --git a/src/Robots/RobotArms/RobotIgus.cs b/src/Robots/RobotArms/RobotIgus.cs index 0102948..34ff7d4 100644 --- a/src/Robots/RobotArms/RobotIgus.cs +++ b/src/Robots/RobotArms/RobotIgus.cs @@ -14,8 +14,10 @@ internal RobotIgus(string model, double payload, Plane basePlane, Mesh baseMesh, public override double DegreeToRadian(double degree, int i) { double radian = degree.ToRadians(); + if (i == 1 || i == 2) radian -= HalfPI; + radian = -radian; return radian; } @@ -24,8 +26,10 @@ public override double RadianToDegree(double radian, int i) { if (i != 0 && i != 5) radian = -radian; + if (i == 1 || i == 2) radian += HalfPI; + return radian.ToDegrees(); } diff --git a/src/Robots/RobotSystems/SystemIgus.cs b/src/Robots/RobotSystems/SystemIgus.cs index d38c1f8..b64c248 100644 --- a/src/Robots/RobotSystems/SystemIgus.cs +++ b/src/Robots/RobotSystems/SystemIgus.cs @@ -56,30 +56,25 @@ internal override void SaveCode(IProgram program, string folder) for (int i = 0; i < program.Code.Count; i++) { - if (!multiProgram) { string filePath = Path.Combine(folder, $"{program.Name}.xml"); var code = program.Code[i][0]; var joinedCode = string.Join("\r\n", code); File.WriteAllText(filePath, joinedCode); - } else { for (int j = 0; j < program.Code[i].Count; j++) { - string filePath; - if (j == 0) - filePath = Path.Combine(folder, $"{program.Name}.xml"); - else - filePath = Path.Combine(folder, $"{program.Name}_{j:000}.xml"); + string filePath = j == 0 + ? Path.Combine(folder, $"{program.Name}.xml") + : Path.Combine(folder, $"{program.Name}_{j:000}.xml"); + var joinedCode = string.Join("\r\n", program.Code[i][j]); File.WriteAllText(filePath, joinedCode); } } } - } - } diff --git a/tests/Robots.Tests/ABBTests.cs b/tests/Robots.Tests/ABBTests.cs index 6b908c2..1c2f83b 100644 --- a/tests/Robots.Tests/ABBTests.cs +++ b/tests/Robots.Tests/ABBTests.cs @@ -21,7 +21,7 @@ public ABBTests() var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed); var toolpath = new SimpleToolpath() { targetA, targetB }; - _program = new Program("TestProgram", robot, new[] { toolpath }); + _program = new Program("TestProgram", robot, [toolpath]); } [Test] diff --git a/tests/Robots.Tests/Performance.cs b/tests/Robots.Tests/Performance.cs index 46aff7e..445274f 100644 --- a/tests/Robots.Tests/Performance.cs +++ b/tests/Robots.Tests/Performance.cs @@ -52,7 +52,7 @@ void PerfTestAbb() Log("Toolpath"); - var program = new Program("TestProgram", robot, new[] { toolpath }, stepSize: 0.02); + var program = new Program("TestProgram", robot, [toolpath], stepSize: 0.02); Log("Program"); // 486 @@ -60,6 +60,7 @@ void PerfTestAbb() Debug.Assert(program.Duration == expected, "Test failed"); } +#pragma warning disable IDE0051 void PerfTestUR() { _watch.Restart(); @@ -79,7 +80,7 @@ void PerfTestUR() Log("Toolpath"); - var program = new Program("URTest", robot, new[] { toolpath }, stepSize: 0.01); + var program = new Program("URTest", robot, [toolpath], stepSize: 0.01); Log("Program"); diff --git a/tests/Robots.Tests/URTests.cs b/tests/Robots.Tests/URTests.cs index b6e387c..101ec57 100644 --- a/tests/Robots.Tests/URTests.cs +++ b/tests/Robots.Tests/URTests.cs @@ -21,7 +21,7 @@ public URTests() var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed); var toolpath = new SimpleToolpath() { targetA, targetB }; - _program = new Program("URTest", robot, new[] { toolpath }); + _program = new Program("URTest", robot, [toolpath]); } [Test]