From ba4f90037a172a8fe0020ce510472be9f483cfff Mon Sep 17 00:00:00 2001 From: visose Date: Mon, 3 Apr 2023 11:19:43 +0100 Subject: [PATCH] Add KDL --- Directory.Build.props | 7 +- NOTICE | 12 ++ RELEASE | 4 + build/Robots.Build/Program.cs | 20 +- .../Properties/launchSettings.json | 9 - build/Robots.Build/Robots.Build.csproj | 6 +- src/Robots/IO/FileIO.cs | 6 +- src/Robots/IO/OnlineLibrary.cs | 10 +- src/Robots/Kinematics/FrankaKdlKinematics.cs | 183 ++++++++++++++++++ .../Kinematics/FrankaNumericalKinematics.cs | 25 +-- .../PostProcessors/FrankxPostProcessor.cs | 4 +- src/Robots/Program/CheckProgram.cs | 34 ++-- src/Robots/Program/ProgramTarget.cs | 35 +++- src/Robots/Remotes/RemoteAbb.cs | 1 - src/Robots/RobotArms/RobotFranka.cs | 10 +- src/Robots/Robots.csproj | 2 + src/Robots/Targets/Toolpath.cs | 5 +- 17 files changed, 307 insertions(+), 66 deletions(-) delete mode 100644 build/Robots.Build/Properties/launchSettings.json create mode 100644 src/Robots/Kinematics/FrankaKdlKinematics.cs diff --git a/Directory.Build.props b/Directory.Build.props index 3ea8212..7a0bff9 100644 --- a/Directory.Build.props +++ b/Directory.Build.props @@ -2,7 +2,7 @@ Robots - 1.6.4 + 1.6.5 Robots Authors Create and simulate ABB, KUKA, UR, and Staubli robot programs. Robots;ABB;KUKA;UR;Staubli;Robotics @@ -21,7 +21,10 @@ latest enable enable - $(MSBuildThisFileDirectory)artifacts\ + + MSB3270 + $(MSBuildThisFileDirectory) + $(RootDir)artifacts\ $(ArtifactsDir)bin\$(MSBuildProjectName) $(ArtifactsDir)obj\$(MSBuildProjectName) diff --git a/NOTICE b/NOTICE index b1ecd6b..949b1a4 100644 --- a/NOTICE +++ b/NOTICE @@ -4,6 +4,18 @@ distributed under licenses different than the Robots software. In the event that we accidentally failed to list a required notice, please bring it to our attention. +Kdl.NetStandard (MIT) +* Copyright (c) 2020 Doug Slater +* https://github.com/slater1/Kdl.NetStandard/blob/master/LICENSE + +Orocos KDL (LGPL-v2) +* Copyright (с) Orocos +* https://www.orocos.org/content/orocos-licenses + +Math.NET (MIT) +* Copyright (с) 2002-2022 Math.NET +* https://github.com/mathnet/mathnet-numerics/blob/master/LICENSE-MKL.md + OPW Kinematics (Apache-2.0) * Copyright (с) Jonathan Meyer * https://github.com/Jmeyer1292/opw_kinematics/blob/master/LICENSE diff --git a/RELEASE b/RELEASE index 0259ecf..a47583b 100644 --- a/RELEASE +++ b/RELEASE @@ -1,3 +1,7 @@ +- version: 1.6.5 + changes: + - Added Orocos KDL solver for Franka Emika (Windows only) + - version: 1.6.4 changes: - Fixes Franka Emika inverse kinematics diff --git a/build/Robots.Build/Program.cs b/build/Robots.Build/Program.cs index f5faa87..32b584c 100644 --- a/build/Robots.Build/Program.cs +++ b/build/Robots.Build/Program.cs @@ -2,7 +2,8 @@ using RhinoPackager.Commands; var app = App.Create(args); -var github = new Github("visose", "Robots"); +Props props = new("Directory.Build.props"); +Github github = new("visose", "Robots"); app.Add(new ICommand[] { @@ -10,14 +11,18 @@ ( testProject: "tests/Robots.Tests/Robots.Tests.csproj" ), - new CheckVersion(github), + new CheckVersion + ( + props: props, + github: github + ), new Build ( buildProject: "src/Robots.Grasshopper/Robots.Grasshopper.csproj" ), new Yak ( - propsFile: "Directory.Build.props", + props: props, sourceFolder: "artifacts/bin/Robots.Grasshopper/net48", files: new [] { @@ -27,7 +32,9 @@ "RobotStudio.Services.RobApi.Desktop.dll", "RobotStudio.Services.RobApi.dll", "Renci.SshNet.dll", - "MathNet.Numerics.dll" + "MathNet.Numerics.dll", + "Kdl.NetStandard.dll", + "kdl_wrap.dll" }, tags: new [] { @@ -37,18 +44,21 @@ ), new Nuget ( + props: props, project: "src/Robots/Robots.csproj", targets: "netstandard2.0" ), new Nuget ( + props: props, project: "src/Robots.Grasshopper/Robots.Grasshopper.csproj", targets: "net48" ), new Release ( + props: props, github: github, - file: "RELEASE", + notesFile: "RELEASE", message: "> This **release** can only be installed through the package manager in **Rhino 7** using the `_PackageManager` command.\n> Check the [readme](../../blob/master/.github/README.md) for more details." ) }); diff --git a/build/Robots.Build/Properties/launchSettings.json b/build/Robots.Build/Properties/launchSettings.json deleted file mode 100644 index 59a3285..0000000 --- a/build/Robots.Build/Properties/launchSettings.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "profiles": { - "Robots.Build": { - "commandName": "Project", - "commandLineArgs": "debug", - "workingDirectory": "../../" - } - } -} \ No newline at end of file diff --git a/build/Robots.Build/Robots.Build.csproj b/build/Robots.Build/Robots.Build.csproj index 735f534..c4b3615 100644 --- a/build/Robots.Build/Robots.Build.csproj +++ b/build/Robots.Build/Robots.Build.csproj @@ -2,12 +2,14 @@ Exe - net6.0 + net7.0 + $(RootDir) + debug CA1852 - + \ No newline at end of file diff --git a/src/Robots/IO/FileIO.cs b/src/Robots/IO/FileIO.cs index 656636a..e9dd068 100644 --- a/src/Robots/IO/FileIO.cs +++ b/src/Robots/IO/FileIO.cs @@ -394,10 +394,8 @@ static Mesh GetToolMesh(string name) const ElementType type = ElementType.Tool; var doc = GetRhinoDoc(name, type); var layerName = $"{type}.{name}"; - var layer = doc.AllLayers.FirstOrDefault(x => x.Name.EqualsIgnoreCase(layerName)); - - if (layer is null) - throw new ArgumentException($" \"{name}\" is not in the 3dm file."); + var layer = doc.AllLayers.FirstOrDefault(x => x.Name.EqualsIgnoreCase(layerName)) + ?? throw new ArgumentException($" \"{name}\" is not in the 3dm file."); var objects = doc.Objects.Where(x => x.Attributes.LayerIndex == layer.Index); var meshes = objects.Select(o => o.Geometry).OfType(); diff --git a/src/Robots/IO/OnlineLibrary.cs b/src/Robots/IO/OnlineLibrary.cs index 3490540..08dc162 100644 --- a/src/Robots/IO/OnlineLibrary.cs +++ b/src/Robots/IO/OnlineLibrary.cs @@ -1,4 +1,4 @@ -using System.Security.Cryptography; +using System.Security.Cryptography; using Newtonsoft.Json; namespace Robots; @@ -82,10 +82,8 @@ async Task AddOnlineLibrariesAsync() { var uri = new Uri("https://api.github.com/repos/visose/robots/contents?ref=libraries"); var json = await _http.GetStringAsync(uri); - var files = JsonConvert.DeserializeObject>(json); - - if (files is null) - throw new ArgumentNullException(" Could not list libraries."); + var files = JsonConvert.DeserializeObject>(json) + ?? throw new ArgumentNullException(" Could not list libraries."); foreach (var file in files) { @@ -189,4 +187,4 @@ async Task DownloadFileAsync(string fileName) var bytes = await _http.GetByteArrayAsync(uri); File.WriteAllBytes(downloadPath, bytes); } -} \ No newline at end of file +} diff --git a/src/Robots/Kinematics/FrankaKdlKinematics.cs b/src/Robots/Kinematics/FrankaKdlKinematics.cs new file mode 100644 index 0000000..1b98346 --- /dev/null +++ b/src/Robots/Kinematics/FrankaKdlKinematics.cs @@ -0,0 +1,183 @@ +using Rhino.Geometry; +using K = Kdl; +using static Kdl.Joint; + +namespace Robots; + +class FrankaKdlKinematics : RobotKinematics, IDisposable +{ + const int _redundant = 2; + + readonly List _empty = new(0); + readonly double[] _midJoints; + readonly K.JntArray _qMin; + readonly K.JntArray _qMax; + readonly K.Chain _chain; + readonly K.ChainIkSolverVel_pinv _velSolver; + readonly K.ChainFkSolverPos_recursive _fkSolver; + readonly K.ChainIkSolverPos_NR_JL _ikSolver; + + public FrankaKdlKinematics(RobotArm robot) + : base(robot) + { + var joints = _mechanism.Joints; + _midJoints = joints.Map(j => j.Range.Mid); + _qMin = ToJntArray(joints.Map(j => double.MinValue)); + _qMax = ToJntArray(joints.Map(j => double.MaxValue)); + + _chain = CreateChain(); + + _velSolver = new(_chain, 1e-5, 150); + _fkSolver = new(_chain); + _ikSolver = new(_chain, _qMin, _qMax, _fkSolver, _velSolver, 100, 1e-4); + } + + K.Chain CreateChain() + { + var j = _mechanism.Joints; + K.Chain chain = new(); + + AddSegment(JointType._None, j[0].A, j[0].Alpha, j[0].D, 0); + AddSegment(JointType.RotZ, j[1].A, j[1].Alpha, j[1].D, 0); + AddSegment(JointType.RotZ, j[2].A, j[2].Alpha, j[2].D, 0); + AddSegment(JointType.RotZ, j[3].A, j[3].Alpha, j[3].D, 0); + AddSegment(JointType.RotZ, j[4].A, j[4].Alpha, j[4].D, 0); + AddSegment(JointType.RotZ, j[5].A, j[5].Alpha, j[5].D, 0); + AddSegment(JointType.RotZ, j[6].A, j[6].Alpha, 0, 0); + AddSegment(JointType.RotZ, 0, 0, j[6].D, 0); + return chain; + + void AddSegment(JointType jointType, double a, double z, double d, double t) + { + using K.Joint joint = new(jointType); + using var frame = K.Frame.DH_Craig1989(a, z, d, t); + using K.Segment segment = new(joint, frame); + chain.addSegment(segment); + } + } + + protected override int SolutionCount => 1; + + protected override double[] InverseKinematics(Transform transform, RobotConfigurations configuration, double[] external, double[]? prevJoints, out List errors) + { + errors = _empty; + + var current = prevJoints ?? _midJoints; + + double? redundantValue = + external.Length > 0 ? external[0] : prevJoints?[_redundant]; + + if (redundantValue is not null) + { + _qMin.set(_redundant, redundantValue ?? double.MinValue); + _qMax.set(_redundant, redundantValue ?? double.MaxValue); + } + + _ikSolver.setJointLimits(_qMin, _qMax); + + using var q_init = ToJntArray(current); + using K.Frame pos_goal = ToFrame(transform); + using K.JntArray q_sol = new(7); + var retVal = _ikSolver.CartToJnt(q_init, pos_goal, q_sol); + + if (retVal != 0) + { + var text = _ikSolver.strError(retVal); + + errors = new List(1) + { + $"Warning: Target unreachable ({text})" + }; + } + + var vals = FromJntArray(q_sol); + return vals; + } + + protected override Transform[] ForwardKinematics(double[] joints) + { + using var ja = ToJntArray(joints); + + var _ts = new Transform[joints.Length]; + using K.FrameVector frames = new(joints.Length + 1); + + for (int i = 0; i < frames.Capacity; i++) + frames.Add(new()); + + var kinematics_status = _fkSolver.JntToCart(ja, frames); + + for (int i = 0; i < joints.Length; i++) + { + using var frame = frames[i + 1]; + using var p = frame.p; + using var r = frame.M; + using var vx = r.UnitX(); + using var vy = r.UnitY(); + + Plane plane = new((Point3d)ToVector3d(p), ToVector3d(vx), ToVector3d(vy)); + _ts[i] = plane.ToTransform(); + } + + return _ts; + } + + Vector3d ToVector3d(K.Vector v) => new(v.x(), v.y(), v.z()); + + K.Frame ToFrame(Transform t) + { + using K.Rotation rotation = new( + t.M00, t.M01, t.M02, + t.M10, t.M11, t.M12, + t.M20, t.M21, t.M22 + ); + + using K.Vector vector = new(t.M03, t.M13, t.M23); + return new(rotation, vector); + } + + double[] FromJntArray(K.JntArray ja) + { + int count = (int)ja.rows(); + var vals = new double[count]; + + for (int i = 0; i < count; i++) + vals[i] = ja.get(i); + + return vals; + } + + K.JntArray ToJntArray(double[] joints) + { + var ja = new K.JntArray((uint)joints.Length); + + for (int i = 0; i < joints.Length; i++) + ja.set(i, joints[i]); + + return ja; + } + + bool _isDisposed; + + ~FrankaKdlKinematics() => Dispose(false); + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + void Dispose(bool disposing) + { + if (_isDisposed) + return; + + _isDisposed = disposing; + + _qMin.Dispose(); + _qMax.Dispose(); + _chain.Dispose(); + _velSolver.Dispose(); + _fkSolver.Dispose(); + _ikSolver.Dispose(); + } +} diff --git a/src/Robots/Kinematics/FrankaNumericalKinematics.cs b/src/Robots/Kinematics/FrankaNumericalKinematics.cs index 0dae440..d1e2542 100644 --- a/src/Robots/Kinematics/FrankaNumericalKinematics.cs +++ b/src/Robots/Kinematics/FrankaNumericalKinematics.cs @@ -14,7 +14,6 @@ class FrankaNumericalKinematics : RobotKinematics readonly List _empty = new(0); readonly double _sq2 = 1.0 / Sqrt(2); readonly double[] _midJoints; - readonly Transform _flangeRot; readonly Matrix _eye = new(7, 7); readonly Matrix _m77t = new(7, 7); @@ -37,7 +36,6 @@ public FrankaNumericalKinematics(RobotArm robot) { var joints = _mechanism.Joints; _midJoints = joints.Map(j => j.Range.Mid); - _flangeRot = Transform.Rotation(-PI, Point3d.Origin) * Transform.Rotation(-PI, Vector3d.XAxis, Point3d.Origin); } protected override int SolutionCount => 1; @@ -48,7 +46,9 @@ protected override double[] InverseKinematics(Transform transform, RobotConfigur const double tolerance = 1e-12; errors = _empty; - transform *= Transform.Rotation(PI, Point3d.Origin); + var plane = transform.ToPlane(); + plane = new(plane.Origin, plane.XAxis, -plane.YAxis); + transform = plane.ToTransform(); var euler = transform.ToEulerZYX(); SetValues(_x_target, euler); @@ -56,10 +56,9 @@ protected override double[] InverseKinematics(Transform transform, RobotConfigur var current = prevJoints ?? _midJoints; _q_current.SetValues(current); - double? redudantValue = + double? redundantValue = external.Length > 0 ? external[0] : prevJoints?[redundant]; - _q_current[6] *= -1; _q_current[6] -= PI * 0.25; double dis_min_all = double.MaxValue; @@ -73,9 +72,9 @@ protected override double[] InverseKinematics(Transform transform, RobotConfigur // Null-space handling _dq.Clear(); - if (redudantValue is not null) + if (redundantValue is not null) { - _dq[redundant] = redudantValue.Value - _q_current[redundant]; + _dq[redundant] = redundantValue.Value - _q_current[redundant]; _j_inv.Multiply(_j, _m77t); _identity.CopyTo(_eye); _eye.Subtract(_m77t, _m77t); @@ -101,9 +100,9 @@ protected override double[] InverseKinematics(Transform transform, RobotConfigur _x_target.Subtract(_v6t, _v6t); double new_dis = SquaredLength(_v6t); - if (redudantValue is not null) + if (redundantValue is not null) { - var y = redudantValue.Value - _q_current[redundant]; + var y = redundantValue.Value - _q_current[redundant]; new_dis += y * y; } @@ -128,27 +127,21 @@ protected override double[] InverseKinematics(Transform transform, RobotConfigur } error: - const double validTol = 1e-2; - var warn = dis_min_all < validTol ? "Warning" : "Error"; - errors = new List(1) { - $"{warn}: Target unreachable ({dis_min_all:e2})" + $"Warning: Target unreachable ({dis_min_all:e2})" }; end: var vals = _q_current.AsArray(); vals[6] += PI * 0.25; - vals[6] *= -1; return vals; } protected override Transform[] ForwardKinematics(double[] joints) { joints = joints.ToArray(); - joints[6] *= -1; var t = ModifiedDH(joints); - t[6] *= _flangeRot; return t; } diff --git a/src/Robots/PostProcessors/FrankxPostProcessor.cs b/src/Robots/PostProcessors/FrankxPostProcessor.cs index f84e743..4abd6b3 100644 --- a/src/Robots/PostProcessors/FrankxPostProcessor.cs +++ b/src/Robots/PostProcessors/FrankxPostProcessor.cs @@ -47,7 +47,9 @@ def program(): foreach (var tool in attributes.OfType()) { - code.Add($" {tool.Name} = {Affine(tool.Tcp)}"); + var tcp = tool.Tcp; + tcp = new(tcp.Origin, -tcp.XAxis, tcp.YAxis); + code.Add($" {tool.Name} = {Affine(tcp)}"); } foreach (var zone in attributes.OfType()) diff --git a/src/Robots/Program/CheckProgram.cs b/src/Robots/Program/CheckProgram.cs index 74160e1..b442dfc 100644 --- a/src/Robots/Program/CheckProgram.cs +++ b/src/Robots/Program/CheckProgram.cs @@ -208,6 +208,7 @@ int FixTargetMotions(List systemTargets, double stepSize) double time = 0; //int groups = systemTargets[0].ProgramTargets.Count; double[][]? prevJoints = null; + bool is7dof = _robotSystem.RobotJointCount == 7; for (int i = 0; i < systemTargets.Count; i++) { @@ -225,7 +226,7 @@ int FixTargetMotions(List systemTargets, double stepSize) } else { - var prevTarget = systemTargets[i - 1]; + var prevSystemTarget = systemTargets[i - 1]; // no interpolation { @@ -235,30 +236,32 @@ int FixTargetMotions(List systemTargets, double stepSize) { if (kineTargets[j] is CartesianTarget target && target.Motion == Motions.Linear) { - target.Configuration = prevTarget.ProgramTargets[j].Kinematics.Configuration; + target.Configuration = prevSystemTarget.ProgramTargets[j].Kinematics.Configuration; } } - var kinematics = _robotSystem.Kinematics(kineTargets, prevJoints); - prevJoints = kinematics.Map(k => k.Joints); - systemTarget.SetTargetKinematics(kinematics, _program.Errors, _program.Warnings, prevTarget); - - CheckUndefined(systemTarget, systemTargets); + if (!is7dof) + { + var kinematics = _robotSystem.Kinematics(kineTargets, prevJoints); + prevJoints = kinematics.Map(k => k.Joints); + systemTarget.SetTargetKinematics(kinematics, _program.Errors, _program.Warnings, prevSystemTarget); + } } double divisions = 1; - var linearTargets = systemTarget.ProgramTargets.Where(x => !x.IsJointMotion); + var linearTargets = systemTarget.ProgramTargets.Where(x => !x.IsJointMotion).ToList(); // if (linearTargets.Count() > 0) program.Errors.Clear(); foreach (var target in linearTargets) { - var prevPlane = target.GetPrevPlane(prevTarget.ProgramTargets[target.Group]); + var prevProgramTarget = prevSystemTarget.ProgramTargets[target.Group]; + var prevPlane = target.GetPrevPlane(prevProgramTarget); double distance = prevPlane.Origin.DistanceTo(target.Plane.Origin); double divisionsTemp = Ceiling(distance / stepSize); if (divisionsTemp > divisions) divisions = divisionsTemp; } - var prevInterTarget = prevTarget.ShallowClone(); + var prevInterTarget = prevSystemTarget.ShallowClone(); prevInterTarget.DeltaTime = 0; prevInterTarget.TotalTime = 0; prevInterTarget.MinTime = 0; @@ -273,7 +276,7 @@ int FixTargetMotions(List systemTargets, double stepSize) { double t = j / divisions; var interTarget = systemTarget.ShallowClone(); - var kineTargets = systemTarget.Lerp(prevTarget, _robotSystem, t, 0.0, 1.0); + var kineTargets = systemTarget.Lerp(prevSystemTarget, _robotSystem, t, 0.0, 1.0); var kinematics = _program.RobotSystem.Kinematics(kineTargets, prevJoints); prevJoints = kinematics.Map(k => k.Joints); interTarget.SetTargetKinematics(kinematics, _program.Errors, _program.Warnings, prevInterTarget); @@ -291,7 +294,7 @@ int FixTargetMotions(List systemTargets, double stepSize) target.SpeedType = speeds.Item4; } - if ((j > 1) && (Abs(slowestDelta - lastDeltaTime) > 1e-09)) + if ((j > 1) && (is7dof || (Abs(slowestDelta - lastDeltaTime) > 1e-09))) { Keyframes.Add(prevInterTarget.ShallowClone()); deltaTimeSinceLast = 0; @@ -335,6 +338,13 @@ int FixTargetMotions(List systemTargets, double stepSize) } // set target kinematics + if (is7dof) + { + var lastKinematics = prevInterTarget.ProgramTargets.MapToList(p => p.Kinematics); + systemTarget.SetTargetKinematics(lastKinematics, _program.Errors, _program.Warnings, prevInterTarget); + } + + CheckUndefined(systemTarget, systemTargets); systemTarget.TotalTime = time; systemTarget.DeltaTime = totalDeltaTime; diff --git a/src/Robots/Program/ProgramTarget.cs b/src/Robots/Program/ProgramTarget.cs index 5fea332..c60e4af 100644 --- a/src/Robots/Program/ProgramTarget.cs +++ b/src/Robots/Program/ProgramTarget.cs @@ -18,7 +18,21 @@ public class ProgramTarget public int Index => SystemTarget.Index; public bool IsJointMotion => IsJointTarget || ((CartesianTarget)Target).Motion == Motions.Joint; - public Plane WorldPlane => Kinematics.Planes[Kinematics.Planes.Length - 1]; + public Plane WorldPlane + { + get + { + if (_kinematics is null && Target is CartesianTarget cartesianTarget) + { + Plane targetPlane = cartesianTarget.Plane; + targetPlane.Orient(ref cartesianTarget.Frame.Plane); + return targetPlane; + } + + return Kinematics.Planes[Kinematics.Planes.Length - 1]; + } + } + internal bool IsJointTarget => Target is JointTarget; public KinematicSolution Kinematics @@ -105,7 +119,20 @@ public Plane GetPrevPlane(ProgramTarget prevTarget) public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double start, double end) { int jointCount = robot.RobotJointCount; - double[] allJoints = JointTarget.Lerp(prevTarget.Kinematics.Joints, Kinematics.Joints, t, start, end); + + var currentJoints = _kinematics?.Joints; + var prevJoints = prevTarget.Kinematics.Joints; + + if (currentJoints is null) + { + var j = (Target is JointTarget jointTarget) ? jointTarget.Joints : new double[jointCount]; + currentJoints = j.ToArray(); + + if (Target.External.Length == 1 && robot.RobotJointCount == 7) + currentJoints[2] = Target.External[0]; + } + + double[] allJoints = JointTarget.Lerp(prevJoints, currentJoints, t, start, end); double[] external; @@ -114,13 +141,13 @@ public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double int externalCount = system.MechanicalGroups[Group].Externals.Sum(e => e.Joints.Length); external = allJoints.RangeSubset(jointCount, externalCount); } - else if (robot.RobotJointCount == 7 && Target.External.Length > 0) + else if (robot.RobotJointCount == 7 && Target.External.Length == 1) { external = new[] { allJoints[2] }; } else { - external = new double[0]; + external = Array.Empty(); } if (IsJointMotion) diff --git a/src/Robots/Remotes/RemoteAbb.cs b/src/Robots/Remotes/RemoteAbb.cs index 3040110..f80043d 100644 --- a/src/Robots/Remotes/RemoteAbb.cs +++ b/src/Robots/Remotes/RemoteAbb.cs @@ -9,7 +9,6 @@ public class RemoteAbb : IRemote public void Pause() => throw NotImplemented(); public void Upload(IProgram p) => throw NotImplemented(); -#pragma warning disable IDE0060 internal RemoteAbb() { } Exception NotImplemented() => new NotImplementedException(" ABB SDK not supported in .NET Standard."); diff --git a/src/Robots/RobotArms/RobotFranka.cs b/src/Robots/RobotArms/RobotFranka.cs index 9814ee2..3a89a05 100644 --- a/src/Robots/RobotArms/RobotFranka.cs +++ b/src/Robots/RobotArms/RobotFranka.cs @@ -1,4 +1,6 @@ +using System.Runtime.InteropServices; using Rhino.Geometry; +using static System.Runtime.InteropServices.RuntimeInformation; using static System.Math; using static Robots.Util; @@ -9,7 +11,13 @@ public class RobotFranka : RobotArm internal RobotFranka(string model, double payload, Plane basePlane, Mesh baseMesh, Joint[] joints) : base(model, Manufacturers.UR, payload, basePlane, baseMesh, joints) { } - private protected override MechanismKinematics CreateSolver() => new FrankaNumericalKinematics(this); + private protected override MechanismKinematics CreateSolver() + { + if (IsOSPlatform(OSPlatform.Windows) && OSArchitecture == Architecture.X64) + return new FrankaKdlKinematics(this); + + return new FrankaNumericalKinematics(this); + } public override double DegreeToRadian(double degree, int i) => degree * (PI / 180.0); public override double RadianToDegree(double radian, int i) => radian * (180.0 / PI); diff --git a/src/Robots/Robots.csproj b/src/Robots/Robots.csproj index b6c55d1..4035ba2 100644 --- a/src/Robots/Robots.csproj +++ b/src/Robots/Robots.csproj @@ -7,9 +7,11 @@ Robots icon128.png $(Description) + + diff --git a/src/Robots/Targets/Toolpath.cs b/src/Robots/Targets/Toolpath.cs index 041dd2d..72a15ce 100644 --- a/src/Robots/Targets/Toolpath.cs +++ b/src/Robots/Targets/Toolpath.cs @@ -1,4 +1,4 @@ -using System.Collections; +using System.Collections; namespace Robots; @@ -31,8 +31,7 @@ public void Add(Target target) public IToolpath ShallowClone(List? targets = null) { - if (targets is null) - targets = _targets.ToList(); + targets ??= _targets.ToList(); var clone = (SimpleToolpath)MemberwiseClone(); clone._targets = targets;