Skip to content

Commit

Permalink
implement moveP
Browse files Browse the repository at this point in the history 
this implements the moveP motion type for UR robots
  • Loading branch information
@KonradJuenger
KonradJuenger committed Oct 8, 2021
1 parent 324cbdb commit 5c66b85
Show file tree
Hide file tree
Showing 9 changed files with 150 additions and 16 deletions.
14 changes: 7 additions & 7 deletions Robots.sln
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ VisualStudioVersion = 16.0.28714.193
MinimumVisualStudioVersion = 10.0.40219.1
Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Robots", "Robots\Robots.csproj", "{3896FB4D-9007-48FB-B302-6B289B15E9BF}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotsStandalone", "RobotsStandalone\RobotsStandalone.csproj", "{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}"
Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "RobotsStandalone", "RobotsStandalone\RobotsStandalone.csproj", "{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}"
EndProject
Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "RobotsGH", "RobotsGH\RobotsGH.csproj", "{3E69607D-A2A9-4061-86E8-2EA7CCB79601}"
ProjectSection(ProjectDependencies) = postProject
Expand All @@ -23,16 +23,16 @@ Global
Release|Any CPU = Release|Any CPU
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Debug|Any CPU.Build.0 = Debug|Any CPU
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Debug|Any CPU.ActiveCfg = Release|Any CPU
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Debug|Any CPU.Build.0 = Release|Any CPU
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Release|Any CPU.ActiveCfg = Release|Any CPU
{3896FB4D-9007-48FB-B302-6B289B15E9BF}.Release|Any CPU.Build.0 = Release|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Debug|Any CPU.Build.0 = Debug|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Debug|Any CPU.ActiveCfg = Release|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Debug|Any CPU.Build.0 = Release|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Release|Any CPU.ActiveCfg = Release|Any CPU
{2556FB1C-3A8D-469B-8553-46C66BCF8BE4}.Release|Any CPU.Build.0 = Release|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Debug|Any CPU.Build.0 = Debug|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Debug|Any CPU.ActiveCfg = Release|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Debug|Any CPU.Build.0 = Release|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Release|Any CPU.ActiveCfg = Release|Any CPU
{3E69607D-A2A9-4061-86E8-2EA7CCB79601}.Release|Any CPU.Build.0 = Release|Any CPU
EndGlobalSection
Expand Down
105 changes: 105 additions & 0 deletions Robots/RobotCellUR.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,66 @@ List<string> Program()

moveText = $" movej([{joints[0]:0.####}, {joints[1]:0.####}, {joints[2]:0.####}, {joints[3]:0.####}, {joints[4]:0.####}, {joints[5]:0.####}], a={axisAccel:0.####}, {speed}, r={zoneDistance})";
}
else if (programTarget.IsPMotion)
{
var cartesian = target as CartesianTarget;
var plane = cartesian.Plane;
plane.Transform(Transform.PlaneToPlane(Plane.WorldXY, target.Frame.Plane));
plane.Transform(Transform.PlaneToPlane(cell.BasePlane, Plane.WorldXY));
var axisAngle = cell.PlaneToNumbers(plane);

switch (cartesian.Motion)
{
case Motions.Joint:
{
double maxAxisSpeed = robot.Joints.Min(x => x.MaxSpeed);
double percentage = (cellTarget.DeltaTime > 0) ? cellTarget.MinTime / cellTarget.DeltaTime : 0.1;
double axisSpeed = percentage * maxAxisSpeed;
double axisAccel = target.Speed.AxisAccel;

string speed = null;
if (target.Speed.Time == 0)
speed = $"v={axisSpeed: 0.###}";
else
speed = $"t={target.Speed.Time: 0.###}";

moveText = $" movej(p[{axisAngle[0]:0.#####}, {axisAngle[1]:0.#####}, {axisAngle[2]:0.#####}, {axisAngle[3]:0.#####}, {axisAngle[4]:0.#####}, {axisAngle[5]:0.#####}],a={axisAccel:0.#####},{speed},r={zoneDistance})";
break;
}

case Motions.Linear:
{
double linearSpeed = target.Speed.TranslationSpeed / 1000;
double linearAccel = target.Speed.TranslationAccel / 1000;

string speed = null;
if (target.Speed.Time == 0)
// speed = $"v={linearSpeed: 0.000}";
speed = $"v={target.Speed.Name}";
else
speed = $"t={target.Speed.Time: 0.000}";

moveText = $" movel(p[{axisAngle[0]:0.#####}, {axisAngle[1]:0.#####}, {axisAngle[2]:0.#####}, {axisAngle[3]:0.#####}, {axisAngle[4]:0.#####}, {axisAngle[5]:0.#####}],a={linearAccel:0.#####},{speed},r={zoneDistance})";
break;
}
case Motions.constantSpeed:
{
double linearSpeed = target.Speed.TranslationSpeed / 1000;
double linearAccel = target.Speed.TranslationAccel / 1000;

string speed = null;
if (target.Speed.Time == 0)
// speed = $"v={linearSpeed: 0.000}";
speed = $"v={target.Speed.Name}";
else
speed = $"t={target.Speed.Time: 0.000}";

moveText = $" movep(p[{axisAngle[0]:0.#####}, {axisAngle[1]:0.#####}, {axisAngle[2]:0.#####}, {axisAngle[3]:0.#####}, {axisAngle[4]:0.#####}, {axisAngle[5]:0.#####}],a={linearAccel:0.#####},{speed},r={zoneDistance})";
break;
}
}
}

else
{
var cartesian = target as CartesianTarget;
Expand Down Expand Up @@ -388,9 +448,54 @@ List<string> Program()
moveText = $" movel(p[{axisAngle[0]:0.#####}, {axisAngle[1]:0.#####}, {axisAngle[2]:0.#####}, {axisAngle[3]:0.#####}, {axisAngle[4]:0.#####}, {axisAngle[5]:0.#####}],a={linearAccel:0.#####},{speed},r={zoneDistance})";
break;
}

case Motions.constantSpeed:
{
double linearSpeed = target.Speed.TranslationSpeed / 1000;
double linearAccel = target.Speed.TranslationAccel / 1000;

string speed = null;
if (target.Speed.Time == 0)
// speed = $"v={linearSpeed: 0.000}";
speed = $"v={target.Speed.Name}";
else
speed = $"t={target.Speed.Time: 0.000}";

moveText = $" movep(p[{axisAngle[0]:0.#####}, {axisAngle[1]:0.#####}, {axisAngle[2]:0.#####}, {axisAngle[3]:0.#####}, {axisAngle[4]:0.#####}, {axisAngle[5]:0.#####}],a={linearAccel:0.#####},{speed},r={zoneDistance})";
break;
}
}
}






























foreach (var command in programTarget.Commands.Where(c => c.RunBefore))
{
string commands = command.Code(program, target);
Expand Down
13 changes: 9 additions & 4 deletions Robots/Robots.csproj
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,18 @@
<Project Sdk="Microsoft.NET.Sdk">

<PropertyGroup>
<TargetFramework>net471</TargetFramework>
<TargetFramework>net48</TargetFramework>
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
<AssemblyName>Robots</AssemblyName>
<PackageId>Robots</PackageId>
</PropertyGroup>

<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|AnyCPU'">
<OutputPath>C:\Users\vicen\AppData\Roaming\Grasshopper\Libraries</OutputPath>
<OutputPath>C:\Users\elab\Documents\bauhaus40\KHBots\built\</OutputPath>
</PropertyGroup>

<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|AnyCPU'">
<OutputPath>C:\Users\vicen\AppData\Roaming\Grasshopper\Libraries</OutputPath>
<OutputPath>C:\Users\elab\Documents\bauhaus40\KHBots\built\</OutputPath>
</PropertyGroup>

<ItemGroup>
Expand All @@ -24,7 +26,10 @@

<ItemGroup>
<Reference Include="ABB.Robotics.Controllers.PC">
<HintPath>..\..\..\..\..\..\..\Program Files (x86)\ABB\SDK\PCSDK 2019\ABB.Robotics.Controllers.PC.dll</HintPath>
<HintPath>..\..\..\..\..\..\Program Files (x86)\ABB\RobotStudio 2020\Bin\ABB.Robotics.Controllers.PC.dll</HintPath>
</Reference>
<Reference Include="RobotStudio.Services.RobApi">
<HintPath>..\..\..\..\..\..\Program Files (x86)\ABB\RobotStudio 2020\Bin\RobotStudio.Services.RobApi.dll</HintPath>
</Reference>
</ItemGroup>

Expand Down
19 changes: 18 additions & 1 deletion Robots/Target.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ namespace Robots
{
[Flags]
public enum RobotConfigurations { None = 0, Shoulder = 1, Elbow = 2, Wrist = 4, Undefined = 8 }
public enum Motions { Joint, Linear, Circular, Spline }
public enum Motions { Joint, Linear, Circular, Spline, constantSpeed }

public abstract class Target : IToolpath
{
Expand Down Expand Up @@ -74,12 +74,14 @@ public class CartesianTarget : Target
public Plane Plane { get; set; }
public RobotConfigurations? Configuration { get; set; }
public Motions Motion { get; set; }
public bool pTarget;

public CartesianTarget(Plane plane, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable<double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Plane = plane;
this.Motion = motion;
this.Configuration = configuration;
this.pTarget = false;
}

public CartesianTarget(Plane plane, Target target, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, IEnumerable<double> external = null) : this(plane, configuration, motion, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
Expand Down Expand Up @@ -220,6 +222,7 @@ public class ProgramTarget

internal bool IsJointTarget => Target is JointTarget;
public bool IsJointMotion => IsJointTarget || (Target as CartesianTarget).Motion == Motions.Joint;
public bool IsPMotion => (Target as CartesianTarget).Motion == Motions.constantSpeed;
public Plane WorldPlane => Kinematics.Planes[Kinematics.Planes.Length - 1];
public int Index => cellTarget.Index;

Expand Down Expand Up @@ -345,6 +348,20 @@ public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double
var joints = allJoints.RangeSubset(0, 6);
return new JointTarget(joints, Target, external);
}

else if (IsPMotion)
{
Plane prevPlane = GetPrevPlane(prevTarget);
Plane plane = robot.CartesianLerp(prevPlane, Plane, t, start, end);
// Plane plane = CartesianTarget.Lerp(prevTarget.WorldPlane, this.WorldPlane, t, start, end);
// Target.RobotConfigurations? configuration = (Abs(prevTarget.cellTarget.TotalTime - t) < TimeTol) ? prevTarget.Kinematics.Configuration : this.Kinematics.Configuration;

var target = new CartesianTarget(plane, Target, prevTarget.Kinematics.Configuration, Motions.constantSpeed, external);
target.pTarget = true;
// target.Frame = Frame.Default;
return target;
}

else
{
Plane prevPlane = GetPrevPlane(prevTarget);
Expand Down
2 changes: 1 addition & 1 deletion RobotsGH/Properties/AssemblyInfo.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ public class RobotsInfo : GH_AssemblyInfo
public RobotsInfo() { }
public override string Name => "Robots";
public override Bitmap Icon => Properties.Resources.iconRobot;
public override string Description => "Provides components to visualize and create programs for ABB, KUKA and UR robots. Developed as an open source tool to program the robots at Bartlett School of Architecture.";
public override string Description => "Fork of visose/Robots: Provides components to visualize and create programs for ABB, KUKA and UR robots. Developed as an open source tool to program the robots at Bartlett School of Architecture.";
public override Guid Id => new Guid("0c4dd17f-db66-4895-9565-412eb167503f");
public override string AssemblyVersion => "0.0.5";
public override GH_LibraryLicense License => GH_LibraryLicense.opensource;
Expand Down
4 changes: 4 additions & 0 deletions RobotsGH/Properties/launchSettings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,10 @@
"RobotsGH": {
"commandName": "Executable",
"executablePath": "C:\\Program Files\\Rhino 6\\System\\Rhino.exe"
},
"KHBots": {
"commandName": "Executable",
"executablePath": "C:\\Program Files\\Rhino 6\\System\\Rhino.exe"
}
}
}
6 changes: 4 additions & 2 deletions RobotsGH/RobotsGH.csproj
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
<Project Sdk="Microsoft.NET.Sdk">

<PropertyGroup>
<TargetFramework>net471</TargetFramework>
<TargetFramework>net48</TargetFramework>
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
<RootNamespace>Robots</RootNamespace>
<LangVersion>8.0</LangVersion>
</PropertyGroup>

<PropertyGroup>
<OutputPath>C:\Users\vicen\AppData\Roaming\Grasshopper\Libraries</OutputPath>
<OutputPath>C:\Users\elab\Documents\bauhaus40\KHBots\built\</OutputPath>
<AssemblyName>RobotsGH</AssemblyName>
<PackageId>RobotsGH</PackageId>
</PropertyGroup>

<ItemGroup>
Expand Down
1 change: 1 addition & 0 deletions RobotsGH/Target.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,6 +349,7 @@ private void AddMotion(object sender, EventArgs e)
valueList.ListItems.Clear();
valueList.ListItems.Add(new GH_ValueListItem("Joint", "\"Joint\""));
valueList.ListItems.Add(new GH_ValueListItem("Linear", "\"Linear\""));
valueList.ListItems.Add(new GH_ValueListItem("constantSpeed", "\"constantSpeed\""));
Instances.ActiveCanvas.Document.AddObject(valueList, false);
parameter.AddSource(valueList);
parameter.CollectData();
Expand Down
2 changes: 1 addition & 1 deletion RobotsStandalone/RobotsStandalone.csproj
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk">

<PropertyGroup>
<TargetFramework>net471</TargetFramework>
<TargetFramework>net48</TargetFramework>
<ApplicationIcon />
<OutputType>WinExe</OutputType>
<StartupObject />
Expand Down

0 comments on commit 5c66b85

Please sign in to comment.