Replace speed component

visose · Jan 8, 2024 · 0fbfab6 · 0fbfab6
1 parent e72e229
commit 0fbfab6
Show file tree

Hide file tree

Showing 10 changed files with 106 additions and 33 deletions.
diff --git a/Directory.Build.props b/Directory.Build.props
@@ -24,7 +24,7 @@
     <AnalysisLevel>latest</AnalysisLevel>
     <NoWarn>MSB3270</NoWarn>
     <RootDir>$(MSBuildThisFileDirectory)</RootDir>
-    <ArtifactsPath>$(RootDir)artifacts\</ArtifactsPath>
+    <ArtifactsPath>$(RootDir)artifacts</ArtifactsPath>
   </PropertyGroup>
 
 </Project>
diff --git a/RELEASE b/RELEASE
@@ -2,6 +2,8 @@
   changes:
   - Fixed UR FromPlane wrong units
   - Added remark when Get/FromPlane robot system input disconnected
+  - New Speed component with accel inputs and a higher default accel.
+  - Franka Emika can use the axis accel input for dynamic_rel.
 
 - version: 1.6.6
   changes:

diff --git a/build/Robots.Build/Program.cs b/build/Robots.Build/Program.cs
@@ -23,7 +23,7 @@
         new Yak
         (
             props: props,
-            sourceFolder: "artifacts/bin/Robots.Grasshopper/net48",
+            sourceFolder: "artifacts/bin/Robots.Grasshopper/release",
             files:
             [
                 "Robots.dll",

diff --git a/src/Robots.Grasshopper/Target/CreateSpeed.cs → ...obots.Grasshopper/Obsolete/CreateSpeed.cs b/src/Robots.Grasshopper/Target/CreateSpeed.cs → ...obots.Grasshopper/Obsolete/CreateSpeed.cs
@@ -1,12 +1,13 @@
-using System.Drawing;
+using System.Drawing;
 using static System.Math;
 
 namespace Robots.Grasshopper;
 
+[Obsolete("Replace with the new CreateSpeed component")]
 public class CreateSpeed : GH_Component
 {
     public CreateSpeed() : base("Create speed", "Speed", "Creates a target speed.", "Robots", "Components") { }
-    public override GH_Exposure Exposure => GH_Exposure.tertiary;
+    public override GH_Exposure Exposure => GH_Exposure.hidden;
     public override Guid ComponentGuid => new("{BD11418C-74E1-4B13-BE1A-AF105906E1BC}");
     protected override Bitmap Icon => Util.GetIcon("iconSpeed");
 
@@ -35,4 +36,4 @@ protected override void SolveInstance(IGH_DataAccess DA)
         var speed = new Speed(translationSpeed, rotationSpeed, translationExternal, rotationExternal);
         DA.SetData(0, speed);
     }
-}
+}
diff --git a/src/Robots.Grasshopper/Robots.Grasshopper.csproj b/src/Robots.Grasshopper/Robots.Grasshopper.csproj
@@ -19,10 +19,10 @@
 
   <ItemGroup>
     <EmbeddedResource Include="Assets\Embed\**\*" />
-    <None Include="$(RootDir)build\Assets\iconRhino128.png" Pack="true" PackagePath="" />
+    <None Include="$(RootDir)build\Assets\iconRhino128.png" Pack="true" PackagePath="" Visible="false"/>
     <None Include="$(RootDir)build\Assets\iconRobot.png" CopyToOutputDirectory="PreserveNewest" Visible="false" />
     <None Include="Build\Robots.Rhino.targets" Pack="true" PackagePath="buildTransitive" />
-    <None Include="$(TargetDir)$(Product).dll" Pack="true" PackagePath="lib\net48" />
+    <None Include="$(TargetDir)$(Product).dll" Pack="true" PackagePath="lib\net48" Visible="false"/>
   </ItemGroup>
 
   <PropertyGroup Condition="$([MSBuild]::IsOSPlatform(Windows)) and $(Configuration) == 'Debug'">

diff --git a/src/Robots.Grasshopper/Target/CreateSpeedAccel.cs b/src/Robots.Grasshopper/Target/CreateSpeedAccel.cs
@@ -0,0 +1,49 @@
+using System.Drawing;
+using static System.Math;
+
+namespace Robots.Grasshopper;
+
+public class CreateSpeedAccel : GH_Component
+{
+    public CreateSpeedAccel() : base("Create speed", "Speed", "Creates a target speed.", "Robots", "Components") { }
+    public override GH_Exposure Exposure => GH_Exposure.tertiary;
+    public override Guid ComponentGuid => new("2849cac0-4006-4531-a2a3-a37cd7e31031");
+    protected override Bitmap Icon => Util.GetIcon("iconSpeed");
+
+    protected override void RegisterInputParams(GH_InputParamManager pManager)
+    {
+        pManager.AddNumberParameter("Translation", "T", "TCP translation speed (mm/s)", GH_ParamAccess.item, 100.0);
+        pManager.AddNumberParameter("Rotation", "R", "TCP rotation and swivel speed (rad/s)", GH_ParamAccess.item, PI);
+        pManager.AddNumberParameter("External translation", "Et", "External axes translation speed (mm/s)", GH_ParamAccess.item, 5000.0);
+        pManager.AddNumberParameter("External rotation", "Er", "External axes rotation speed (rad/s)", GH_ParamAccess.item, PI * 6);
+        pManager.AddNumberParameter("Accel translation", "At", "Used only in Doosan and UR (mm/s²)", GH_ParamAccess.item, 2500);
+        pManager.AddNumberParameter("Accel axis", "Aa", "Used in UR, Doosan and Franka Emika (rads/s²). For Franka, assumes max accel and jerk is 4PI.", GH_ParamAccess.item, 4 * PI);
+    }
+
+    protected override void RegisterOutputParams(GH_OutputParamManager pManager)
+    {
+        pManager.AddParameter(new SpeedParameter(), "Speed", "S", "Speed instance", GH_ParamAccess.item);
+    }
+
+    protected override void SolveInstance(IGH_DataAccess DA)
+    {
+        double translationSpeed = 0, rotationSpeed = 0,
+            translationExternal = 0, rotationExternal = 0,
+            translationAccel = 0, axisAccel = 0;
+
+        if (!DA.GetData(0, ref translationSpeed)) return;
+        if (!DA.GetData(1, ref rotationSpeed)) return;
+        if (!DA.GetData(2, ref translationExternal)) return;
+        if (!DA.GetData(3, ref rotationExternal)) return;
+        if (!DA.GetData(4, ref translationAccel)) return;
+        if (!DA.GetData(5, ref axisAccel)) return;
+
+        Speed speed = new(translationSpeed, rotationSpeed, translationExternal, rotationExternal)
+        {
+            TranslationAccel = translationAccel,
+            AxisAccel = axisAccel
+        };
+
+        DA.SetData(0, speed);
+    }
+}
diff --git a/src/Robots.Grasshopper/build/Robots.Rhino.targets b/src/Robots.Grasshopper/build/Robots.Rhino.targets
@@ -1,14 +1,14 @@
 <Project>
 
-	<Target Name="RemoveRobotsReferences" AfterTargets="AfterResolveReferences">
-		<ItemGroup>
-			<ReferenceCopyLocalPaths
-				Remove="@(ReferenceCopyLocalPaths)"
-				Condition="
-				%(ReferenceCopyLocalPaths.FileName) == 'Robots' 
-				or %(ReferenceCopyLocalPaths.FileName) == 'Robots.Grasshopper'"
-				/>
-		</ItemGroup>
-	</Target>
+  <Target Name="RemoveRobotsReferences" AfterTargets="AfterResolveReferences">
+    <ItemGroup>
+      <ReferenceCopyLocalPaths
+          Remove="@(ReferenceCopyLocalPaths)"
+          Condition="
+            %(ReferenceCopyLocalPaths.FileName) == 'Robots' 
+            or %(ReferenceCopyLocalPaths.FileName) == 'Robots.Grasshopper'"
+/>
+    </ItemGroup>
+  </Target>
 
-</Project>
+</Project>
diff --git a/src/Robots/PostProcessors/FrankxPostProcessor.cs b/src/Robots/PostProcessors/FrankxPostProcessor.cs
@@ -1,3 +1,4 @@
+using Rhino;
 using Rhino.Geometry;
 using static System.Math;
 
@@ -75,10 +76,9 @@ def program():
             code.Add(_indent + commands);
         }
 
-        code.Add("  robot.acceleration_rel = dynamic_rel\r\n  robot.jerk_rel = dynamic_rel");
-
         Motions? currentMotion = null;
         Tool? currentTool = null;
+        double? currentAccel = null;
 
         // Targets
 
@@ -89,16 +89,30 @@ def program():
 
             var beforeCommands = programTarget.Commands.Where(c => c.RunBefore);
 
-            if (currentMotion is not null && beforeCommands.Any())
-                MotionMove();
+            if (beforeCommands.Any())
+            {
+                if (currentMotion is not null)
+                    MotionMove();
+
+                foreach (var command in beforeCommands)
+                {
+                    string commands = command.Code(_program, target);
+                    code.Add(_indent + commands);
+                }
+            }
+
+            var accel = GetAccel(systemTarget);
 
-            foreach (var command in beforeCommands)
+            if (accel != currentAccel)
             {
-                string commands = command.Code(_program, target);
-                code.Add(_indent + commands);
+                if (currentMotion is not null)
+                    MotionMove();
+
+                code.Add($"  dynamic_rel = {accel:0.#####}\r\n  robot.acceleration_rel = dynamic_rel\r\n  robot.jerk_rel = dynamic_rel");
+                currentAccel = accel;
             }
 
-            double speed = GetSpeed(systemTarget);
+            var speed = GetSpeed(systemTarget);
 
             if (target is JointTarget joint)
             {
@@ -202,6 +216,13 @@ string Affine(Plane plane)
         return $"Affine({n[0]:0.#####}, {n[1]:0.#####}, {n[2]:0.#####}, {n[3]:0.#####}, {n[4]:0.#####}, {n[5]:0.#####}, {n[6]:0.#####})";
     }
 
+    static double GetAccel(SystemTarget systemTarget)
+    {
+        var programTarget = systemTarget.ProgramTargets[0];
+        var speed = programTarget.Target.Speed;
+        return RhinoMath.Clamp(speed.AxisAccel / (4 * PI), 0, 1);
+    }
+
     static double GetSpeed(SystemTarget systemTarget)
     {
         var programTarget = systemTarget.ProgramTargets[0];

diff --git a/src/Robots/PostProcessors/URScriptPostProcessor.cs b/src/Robots/PostProcessors/URScriptPostProcessor.cs
@@ -12,7 +12,7 @@ internal URScriptPostProcessor(SystemUR system, Program program)
     {
         _system = system;
         _program = program;
-        var groupCode = new List<List<string>> { Program() };
+        List<List<string>> groupCode = [Program()];
         Code = [groupCode];
 
         // MultiFile warning
@@ -23,10 +23,10 @@ internal URScriptPostProcessor(SystemUR system, Program program)
     List<string> Program()
     {
         string indent = "  ";
-        var code = new List<string>
-                {
-                    "def Program():"
-                };
+        List<string> code =
+        [
+            "def Program():"
+        ];
 
         // Attribute declarations
         var attributes = _program.Attributes;

diff --git a/src/Robots/TargetAttributes/Speed.cs b/src/Robots/TargetAttributes/Speed.cs
@@ -27,12 +27,12 @@ public class Speed(double translation = 100, double rotationSpeed = PI, double t
     /// </summary>
     public double TranslationAccel { get; set; } = 1000;
     /// <summary>
-    /// Axis/join acceleration in rads/s² (used in UR, Doosan)
+    /// Axis/joint acceleration in rads/s² (used in UR, Doosan, and Franka Emika)
     /// </summary>
     public double AxisAccel { get; set; } = PI;
 
     /// <summary>
-    /// Time in seconds it takes to reach the target. Optional parameter (used in UR)
+    /// Time in seconds it takes to reach the target. Optional parameter (used in UR and Doosan)
     /// </summary>
     public double Time { get; set; }