public GCodeToolpath(string file, CartesianTarget referenceTarget, Vector3d alignment)
{
using var reader = File.OpenText(file);
var parser = new GenericGCodeParser();
var code = parser.Parse(reader);
Toolpath = new FiveAxisToRobots(referenceTarget, alignment, code);
_targets = Toolpath.Targets;
}
Target CreateTarget(Point3d position, Speed speed, Zone zone = null)
{
var plane = Plane.WorldXY;
plane.Origin = position;
var frame = _att.Frame;
var target = new CartesianTarget(plane, null, Motions.Linear, _tool, speed, zone, null, frame, null);
return(target);
}
internal FiveAxisToRobots(CartesianTarget refTarget, Vector3d alignment, GCodeFile file)
{
_refTarget = refTarget;
_alignment = alignment;
var constructionPlane = Rhino.RhinoDoc.ActiveDoc.Views.ActiveView.ActiveViewport.GetConstructionPlane().Plane;
constructionPlane.Origin = Point3d.Origin;
var workPlane = _refTarget.Frame.Plane;
var xform = Transform.PlaneToPlane(Plane.WorldXY, workPlane);
constructionPlane.Transform(xform);
_mcs = new Frame(plane: constructionPlane, name: "MCS");
_gCodeMap = new Dictionary <(GCodeLine.LType letter, int number), Action <GCodeLine> >
{
{ (GCodeLine.LType.GCode, 0), RapidMove },
public override bool CastFrom(object source)
{
if (source is Target)
{
Value = source as Target;
return(true);
}
if (source is GH_Point)
{
Value = new CartesianTarget(new Plane((source as GH_Point).Value, Vector3d.XAxis, Vector3d.YAxis));
return(true);
}
if (source is GH_Plane)
{
Value = new CartesianTarget((source as GH_Plane).Value);
return(true);
}
if (source is GH_String)
{
string text = (source as GH_String).Value;
string[] jointsText = text.Split(',');
if (jointsText.Length != 6)
{
return(false);
}
var joints = new double[6];
for (int i = 0; i < 6; i++)
{
if (!GH_Convert.ToDouble_Secondary(jointsText[i], ref joints[i]))
{
return(false);
}
}
Value = new JointTarget(joints);
return(true);
}
return(false);
}
public static async Task <Program> CreateAsync()
{
var robot = await GetRobotAsync();
var planeA = Plane.WorldYZ;
var planeB = Plane.WorldYZ;
planeA.Origin = new Point3d(300, 200, 610);
planeB.Origin = new Point3d(300, -200, 610);
var speed = new Speed(300);
var targetA = new CartesianTarget(planeA, RobotConfigurations.Wrist, Motions.Joint);
var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed);
var toolpath = new SimpleToolpath()
{
targetA, targetB
};
return(new Program("TestProgram", robot, new[] { toolpath }));
}
public TestProgram()
{
var robot = RobotSystem.Load("Bartlett-IRB120", Plane.WorldXY);
var planeA = Plane.WorldYZ;
var planeB = Plane.WorldYZ;
planeA.Origin = new Point3d(300, 200, 610);
planeB.Origin = new Point3d(300, -200, 610);
var speed = new Speed(300);
var targetA = new CartesianTarget(planeA, RobotConfigurations.Wrist, Motions.Joint);
var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed);
var toolpath = new SimpleToolpath()
{
targetA, targetB
};
Program = new Program("TestProgram", robot, new[] { toolpath });
}
public URTests()
{
const string xml = "<RobotCell name=\"UR10\" manufacturer=\"UR\"><Mechanisms><RobotArm model=\"UR10\" manufacturer=\"UR\" payload=\"10\"><Base x=\"0.000\" y=\"0.000\" z=\"0.000\" q1=\"1.000\" q2=\"0.000\" q3=\"0.000\" q4=\"0.000\"/><Joints><Revolute number=\"1\" a =\"0\" d =\"127.3\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"120\"/><Revolute number=\"2\" a =\"-612\" d =\"0\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"120\"/><Revolute number=\"3\" a =\"-572.3\" d =\"0\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"180\"/><Revolute number=\"4\" a =\"0\" d =\"163.941\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"180\"/><Revolute number=\"5\" a =\"0\" d =\"115.7\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"180\"/><Revolute number=\"6\" a =\"0\" d =\"92.2\" minrange = \"-360\" maxrange =\"360\" maxspeed =\"180\"/></Joints></RobotArm></Mechanisms><IO><DO names=\"0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16\"/><DI names=\"0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16\"/><AO names=\"0,1\"/><AI names=\"0,1\"/></IO></RobotCell>";
var robot = FileIO.ParseRobotSystem(xml, Plane.WorldXY);
var planeA = Plane.WorldZX;
var planeB = Plane.WorldZX;
planeA.Origin = new Point3d(200, 100, 600);
planeB.Origin = new Point3d(700, 250, 600);
var speed = new Speed(300);
var targetA = new CartesianTarget(planeA, RobotConfigurations.Wrist, Motions.Joint);
var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed);
var toolpath = new SimpleToolpath()
{
targetA, targetB
};
_program = new Program("URTest", robot, new[] { toolpath });
}
public ABBTests()
{
const string xml = "<RobotCell name=\"IRB120\" manufacturer=\"ABB\"><Mechanisms><RobotArm model=\"IRB120\" manufacturer=\"ABB\" payload=\"3\"><Base x=\"0.000\" y=\"0.000\" z=\"0.000\" q1=\"1.000\" q2=\"0.000\" q3=\"0.000\" q4=\"0.000\"/><Joints><Revolute number=\"1\" a =\"0\" d =\"290\" minrange = \"-165\" maxrange =\"165\" maxspeed =\"250\"/><Revolute number=\"2\" a =\"270\" d =\"0\" minrange = \"-110\" maxrange =\"110\" maxspeed =\"250\"/><Revolute number=\"3\" a =\"70\" d =\"0\" minrange = \"-110\" maxrange =\"70\" maxspeed =\"250\"/><Revolute number=\"4\" a =\"0\" d =\"302\" minrange = \"-160\" maxrange =\"160\" maxspeed =\"320\"/><Revolute number=\"5\" a =\"0\" d =\"0\" minrange = \"-120\" maxrange =\"120\" maxspeed =\"320\"/><Revolute number=\"6\" a =\"0\" d =\"72\" minrange = \"-400\" maxrange =\"400\" maxspeed =\"420\"/></Joints></RobotArm></Mechanisms><IO><DO names=\"DO10_1,DO10_2\"/><DI names=\"DI10_1,DI10_2\"/></IO></RobotCell>";
var robot = FileIO.ParseRobotSystem(xml, Plane.WorldXY);
var planeA = Plane.WorldYZ;
var planeB = Plane.WorldYZ;
planeA.Origin = new Point3d(300, 200, 610);
planeB.Origin = new Point3d(300, -200, 610);
var speed = new Speed(300);
var targetA = new CartesianTarget(planeA, RobotConfigurations.Wrist, Motions.Joint);
var targetB = new CartesianTarget(planeB, null, Motions.Linear, speed: speed);
var toolpath = new SimpleToolpath()
{
targetA, targetB
};
_program = new Program("TestProgram", robot, new[] { toolpath });
}
public override bool CastFrom(object source)
{
switch (source)
{
case Target target:
Value = target;
return(true);
case GH_Point point:
Value = new CartesianTarget(new Plane(point.Value, Vector3d.XAxis, Vector3d.YAxis));
return(true);
case GH_Plane plane:
Value = new CartesianTarget(plane.Value);
return(true);
case GH_String text:
{
string[] jointsText = text.Value.Split(',');
if (jointsText.Length != 6)
{
return(false);
}
var joints = new double[6];
for (int i = 0; i < 6; i++)
{
if (!GH_Convert.ToDouble_Secondary(jointsText[i], ref joints[i]))
{
return(false);
}
}
Value = new JointTarget(joints);
return(true);
}
}
return(false);
}
void CreateTargets(List <List <SimpleTarget> > paths)
{
double totalDistance = 0;
var externalCustom = new[] { "motorValue" };
_targets.Add(HomeStart());
foreach (var path in paths)
{
var first = path[0].Location;
var last = path[path.Count - 1].Location;
var firstSafe = first; firstSafe.Origin += firstSafe.Normal * _att.SafeZOffset;
_targets.Add(CreateTarget(firstSafe, _att.SafeSpeed, _att.SafeZone, 0));
_targets.Add(CreateTarget(first, _att.ApproachSpeed, _att.ApproachZone, 0));
_targets.Add(CreateTarget(first, _att.ApproachSpeed, _att.ApproachZone, _startDistance));
for (int i = 1; i < path.Count; i++)
{
//double segmentLength = path[i - 1].DistanceTo(path[i]);
Plane position = path[i].Location;
double segmentLength = path[i].Length;
var zone = i != path.Count - 1 ? _att.ExtrusionZone : _att.ApproachZone;
_targets.Add(CreateTarget(position, _att.ExtrusionSpeed, zone, segmentLength));
}
_targets.Add(CreateTarget(last, _att.ExtrusionSpeed, _att.ApproachZone, _suckBack));
var lastOffset = last; lastOffset.Origin += lastOffset.Normal * (_att.SafeZOffset + _att.LayerHeight);
_targets.Add(CreateTarget(lastOffset, _att.ApproachSpeed, _att.SafeZone, 0));
SubPrograms.Add(_targets.Count);
}
_targets.Add(HomeEnd());
SubPrograms.RemoveAt(SubPrograms.Count - 1);
Target CreateTarget(Plane location, Speed speed, Zone zone, double externalDistance)
{
var frame = _att.Frame;
var tool = _att.Tool;
totalDistance += externalDistance * _extrusionFactor;
Command command = null;
if (externalDistance != 0)
{
string sign = externalDistance < 0 ? "+" : "-";
string code = $"motorValue:=motorValue{sign}{Abs(externalDistance):0.000}*extrusionFactor;";
var externalCommand = new Robots.Commands.Custom($"SetExternal{_targets.Count}", command: code);
externalCommand.RunBefore = true;
command = externalCommand;
}
var target = new CartesianTarget(location, null, Motions.Linear, tool, speed, zone, command, frame, new[] { totalDistance });
target.ExternalCustom = externalCustom;
return(target);
}
Target HomeStart()
{
var externalValue = ExternalValue(_loopDistance);
string declaration = $@"VAR num motorValue:= 0;
PERS num extrusionFactor:={_extrusionFactor: 0.000};
VAR robtarget current;
VAR num choice:=0;
";
string initCode = $@"current:= CRobT(\Tool:= {_att.Tool.Name} \WObj:= {_att.Frame.Name});
EOffsSet current.extax;";
string testCode = $@"TPReadFK choice,""Choose mode"",stEmpty,stEmpty,stEmpty,""Program"",""Test"";
current:= CRobT(\Tool:= {_att.Tool.Name} \WObj:= {_att.Frame.Name});
WHILE choice = 5 DO
motorValue:= motorValue - {externalValue:0.00}*extrusionFactor;
current.extax.eax_a:= motorValue;
MoveL Offs(current,0,{_loopDistance},0),{_att.ExtrusionSpeed.Name},{_att.ExtrusionZone.Name},{_att.Tool.Name} \WObj:= {_att.Frame.Name};
motorValue:= motorValue - {externalValue:0.00}*extrusionFactor;
current.extax.eax_a:= motorValue;
MoveL Offs(current,0,0,0),{_att.ExtrusionSpeed.Name},{_att.ExtrusionZone.Name},{_att.Tool.Name} \WObj:= {_att.Frame.Name};
ENDWHILE";
var initCommand = new Robots.Commands.Custom("Init", declaration: declaration, command: initCode);
initCommand.RunBefore = true;
var testCommand = new Robots.Commands.Custom("Test", command: testCode);
var command = new Group(new[] { initCommand, testCommand });
var home = new JointTarget(_att.Home, _att.Tool, _att.SafeSpeed, _att.SafeZone, command, _att.Frame, new[] { totalDistance });
home.ExternalCustom = externalCustom;
return(home);
}
Target HomeEnd()
{
var command = new Group()
{
new Message("Se acabó."),
new Stop()
};
var home = new JointTarget(_att.Home, _att.Tool, _att.SafeSpeed, _att.SafeZone, command, _att.Frame, new[] { totalDistance });
home.ExternalCustom = externalCustom;
return(home);
}
}
protected override void SolveInstance(IGH_DataAccess DA)
{
bool hasTarget = Params.Input.Any(x => x.Name == "Target");
bool hasJoints = Params.Input.Any(x => x.Name == "Joints");
bool hasPlane = Params.Input.Any(x => x.Name == "Plane");
bool hasConfig = Params.Input.Any(x => x.Name == "RobConf");
bool hasMotion = Params.Input.Any(x => x.Name == "Motion");
bool hasTool = Params.Input.Any(x => x.Name == "Tool");
bool hasSpeed = Params.Input.Any(x => x.Name == "Speed");
bool hasZone = Params.Input.Any(x => x.Name == "Zone");
bool hasCommand = Params.Input.Any(x => x.Name == "Command");
bool hasFrame = Params.Input.Any(x => x.Name == "Frame");
bool hasExternal = Params.Input.Any(x => x.Name == "External");
GH_Target sourceTarget = null;
if (hasTarget)
{
if (!DA.GetData("Target", ref sourceTarget))
{
return;
}
}
double[] joints = null;
var plane = new Plane();
Target.RobotConfigurations?configuration = null;
Target.Motions motion = Target.Motions.Joint;
Tool tool = null;
Speed speed = null;
Zone zone = null;
Command command = null;
Frame frame = null;
double[] external = null;
if (hasJoints)
{
GH_String jointsGH = null;
if (!DA.GetData("Joints", ref jointsGH))
{
return;
}
string[] jointsText = jointsGH.Value.Split(',');
if (jointsText.Length != 6)
{
return;
}
joints = new double[6];
for (int i = 0; i < 6; i++)
{
if (!GH_Convert.ToDouble_Secondary(jointsText[i], ref joints[i]))
{
return;
}
}
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is JointTarget)
{
joints = (sourceTarget.Value as JointTarget).Joints;
}
}
if (hasPlane)
{
GH_Plane planeGH = null;
if (hasPlane)
{
if (!DA.GetData("Plane", ref planeGH))
{
return;
}
}
plane = planeGH.Value;
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget)
{
plane = (sourceTarget.Value as CartesianTarget).Plane;
}
}
if (hasConfig)
{
GH_Integer configGH = null;
if (hasConfig)
{
DA.GetData("RobConf", ref configGH);
}
configuration = (configGH == null) ? null : (Target.RobotConfigurations?)configGH.Value;
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget)
{
configuration = (sourceTarget.Value as CartesianTarget).Configuration;
}
}
if (hasMotion)
{
GH_String motionGH = null;
DA.GetData("Motion", ref motionGH);
motion = (motionGH == null) ? Target.Motions.Joint : (Target.Motions)Enum.Parse(typeof(Target.Motions), motionGH.Value);
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget)
{
motion = (sourceTarget.Value as CartesianTarget).Motion;
}
}
if (hasTool)
{
GH_Tool toolGH = null;
DA.GetData("Tool", ref toolGH);
tool = toolGH?.Value;
}
else if (sourceTarget != null)
{
tool = sourceTarget.Value.Tool;
}
if (hasSpeed)
{
GH_Speed speedGH = null;
DA.GetData("Speed", ref speedGH);
speed = speedGH?.Value;
}
else if (sourceTarget != null)
{
speed = sourceTarget.Value.Speed;
}
if (hasZone)
{
GH_Zone zoneGH = null;
DA.GetData("Zone", ref zoneGH);
zone = zoneGH?.Value;
}
else if (sourceTarget != null)
{
zone = sourceTarget.Value.Zone;
}
if (hasCommand)
{
GH_Command commandGH = null;
DA.GetData("Command", ref commandGH);
command = commandGH?.Value;
}
else if (sourceTarget != null)
{
command = sourceTarget.Value.Command;
}
if (hasFrame)
{
GH_Frame frameGH = null;
DA.GetData("Frame", ref frameGH);
frame = frameGH?.Value;
}
else if (sourceTarget != null)
{
frame = sourceTarget.Value.Frame;
}
if (hasExternal)
{
GH_String externalGH = null;
if (!DA.GetData("External", ref externalGH))
{
external = new double[0];
}
else
{
string[] externalText = externalGH.Value.Split(',');
int length = externalText.Length;
external = new double[length];
for (int i = 0; i < length; i++)
{
if (!GH_Convert.ToDouble_Secondary(externalText[i], ref external[i]))
{
return;
}
}
}
}
else if (sourceTarget != null)
{
external = sourceTarget.Value.External;
}
Target target;
bool localCartesian = isCartesian;
if (hasTarget && !hasPlane && !hasJoints)
{
localCartesian = sourceTarget.Value is CartesianTarget;
}
if (localCartesian)
{
target = new CartesianTarget(plane, configuration, motion, tool, speed, zone, command, frame, external);
}
else
{
target = new JointTarget(joints, tool, speed, zone, command, frame, external);
}
if (sourceTarget != null)
{
target.ExternalCustom = sourceTarget.Value.ExternalCustom;
}
DA.SetData(0, new GH_Target(target));
}
protected override void SolveInstance(IGH_DataAccess DA)
{
bool hasTarget = Params.Input.Any(x => x.Name == "Target");
bool hasJoints = Params.Input.Any(x => x.Name == "Joints");
bool hasPlane = Params.Input.Any(x => x.Name == "Plane");
bool hasConfig = Params.Input.Any(x => x.Name == "RobConf");
bool hasMotion = Params.Input.Any(x => x.Name == "Motion");
bool hasTool = Params.Input.Any(x => x.Name == "Tool");
bool hasSpeed = Params.Input.Any(x => x.Name == "Speed");
bool hasZone = Params.Input.Any(x => x.Name == "Zone");
bool hasCommand = Params.Input.Any(x => x.Name == "Command");
bool hasFrame = Params.Input.Any(x => x.Name == "Frame");
bool hasExternal = Params.Input.Any(x => x.Name == "External");
GH_Target sourceTarget = null;
if (hasTarget) if (!DA.GetData("Target", ref sourceTarget)) return;
double[] joints = null;
var plane = new Plane();
Target.RobotConfigurations? configuration = null;
Target.Motions motion = Target.Motions.Joint;
Tool tool = null;
Speed speed = null;
Zone zone = null;
Command command = null;
Frame frame = null;
double[] external = null;
if (hasJoints)
{
GH_String jointsGH = null;
if (!DA.GetData("Joints", ref jointsGH)) return;
string[] jointsText = jointsGH.Value.Split(',');
if (jointsText.Length != 6) return;
joints = new double[6];
for (int i = 0; i < 6; i++)
if (!GH_Convert.ToDouble_Secondary(jointsText[i], ref joints[i])) return;
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is JointTarget) joints = (sourceTarget.Value as JointTarget).Joints;
}
if (hasPlane)
{
GH_Plane planeGH = null;
if (hasPlane) if (!DA.GetData("Plane", ref planeGH)) return;
plane = planeGH.Value;
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget) plane = (sourceTarget.Value as CartesianTarget).Plane;
}
if (hasConfig)
{
GH_Integer configGH = null;
if (hasConfig) DA.GetData("RobConf", ref configGH);
configuration = (configGH == null) ? null : (Target.RobotConfigurations?)configGH.Value;
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget) configuration = (sourceTarget.Value as CartesianTarget).Configuration;
}
if (hasMotion)
{
GH_String motionGH = null;
DA.GetData("Motion", ref motionGH);
motion = (motionGH == null) ? Target.Motions.Joint : (Target.Motions)Enum.Parse(typeof(Target.Motions), motionGH.Value);
}
else if (sourceTarget != null)
{
if (sourceTarget.Value is CartesianTarget) motion = (sourceTarget.Value as CartesianTarget).Motion;
}
if (hasTool)
{
GH_Tool toolGH = null;
DA.GetData("Tool", ref toolGH);
tool = toolGH?.Value;
}
else if (sourceTarget != null)
{
tool = sourceTarget.Value.Tool;
}
if (hasSpeed)
{
GH_Speed speedGH = null;
DA.GetData("Speed", ref speedGH);
speed = speedGH?.Value;
}
else if (sourceTarget != null)
{
speed = sourceTarget.Value.Speed;
}
if (hasZone)
{
GH_Zone zoneGH = null;
DA.GetData("Zone", ref zoneGH);
zone = zoneGH?.Value;
}
else if (sourceTarget != null)
{
zone = sourceTarget.Value.Zone;
}
if (hasCommand)
{
GH_Command commandGH = null;
DA.GetData("Command", ref commandGH);
command = commandGH?.Value;
}
else if (sourceTarget != null)
{
command = sourceTarget.Value.Command;
}
if (hasFrame)
{
GH_Frame frameGH = null;
DA.GetData("Frame", ref frameGH);
frame = frameGH?.Value;
}
else if (sourceTarget != null)
{
frame = sourceTarget.Value.Frame;
}
if (hasExternal)
{
GH_String externalGH = null;
if (!DA.GetData("External", ref externalGH))
{
external = new double[0];
}
else
{
string[] externalText = externalGH.Value.Split(',');
int length = externalText.Length;
external = new double[length];
for (int i = 0; i < length; i++)
if (!GH_Convert.ToDouble_Secondary(externalText[i], ref external[i])) return;
}
}
else if (sourceTarget != null)
{
external = sourceTarget.Value.External;
}
Target target;
bool localCartesian = isCartesian;
if (hasTarget && !hasPlane && !hasJoints)
localCartesian = sourceTarget.Value is CartesianTarget;
if (localCartesian)
target = new CartesianTarget(plane, configuration, motion, tool, speed, zone, command, frame, external);
else
target = new JointTarget(joints, tool, speed, zone, command, frame, external);
DA.SetData(0, new GH_Target(target));
}
public SpatialAttributes(IList <double> variables, CartesianTarget target, IList <double> speeds, IList <double> waits, IList <int> dos, IList <GeometryBase> environment)
{
if (variables.Count != 6)
{
throw new Exception(" There must be 6 variables.");
}
if (speeds.Count != 5)
{
throw new Exception(" There must be 5 speeds.");
}
if (waits.Count != 4)
{
throw new Exception(" There must be 4 wait times.");
}
if (dos.Count != 2)
{
throw new Exception(" There must be 2 digital outputs.");
}
Diameter = variables[0];
DistancePlunge = variables[1];
VerticalOffset = variables[2];
RotationOffset = variables[3];
DistanceAhead = variables[4];
DistanceHorizontal = variables[5];
Environment = new List <GeometryBase>(environment);
ReferenceTarget = target;
Approach = new Speed(name: "Approach", translation: speeds[0]);
Plunge = new Speed(name: "Plunge", translation: speeds[1]);
Fast = new Speed(name: "FastExtrusion", translation: speeds[2]);
Medium = new Speed(name: "MediumExtrusion", translation: speeds[3]);
Slow = new Speed(name: "SlowExtrusion", translation: speeds[4]);
var waitAfterStart = new Wait(waits[0]);
StopExtrusion = new Group()
{
new SetDO(dos[0], false), new SetDO(dos[1], false)
};
FastExtrusion = new Group()
{
new SetDO(dos[0], true), new SetDO(dos[1], false), waitAfterStart
};
MediumExtrusion = new Group()
{
new SetDO(dos[0], false), new SetDO(dos[1], true), waitAfterStart
};
SlowExtrusion = new Group()
{
new SetDO(dos[0], true), new SetDO(dos[1], true), waitAfterStart
};
LongWait = new Group()
{
StopExtrusion, new Wait(waits[3])
};
ShortWait = new Group()
{
StopExtrusion, new Wait(waits[2])
};
AheadCommand = new Group()
{
StopExtrusion, new Wait(waits[1])
};
}
public CSVConverter(string file, CartesianTarget referenceTarget, string mask, bool reverse, double cutSpeed = 0, Point3d?point = null)
{
var splitMask = mask.Split(',').Select(p => p.Trim().ToLower());
if (!splitMask.All(p => validParameters.Contains(p)))
{
throw new Exception(" Mask not valid.");
}
var parameterIndex = splitMask.ToDictionary(p => p, p => - 1);
int count = 0;
foreach (var parameter in splitMask)
{
parameterIndex[parameter] = count;
if (parameter == "position" || parameter == "normal" || parameter == "xaxis")
{
count += 3;
}
else
{
count += 1;
}
}
var lines = File.ReadAllLines(file);
var planes = new List <Plane>(lines.Length);
var speedValues = new List <double>(lines.Length);
var zoneValues = new List <double>(lines.Length);
var types = new List <double>(lines.Length);
foreach (var line in lines)
{
var fields = line.Split(',');
if (fields.Length != count)
{
continue; //throw new Exception(" Number of values in a line is not correct.");
}
var numbers = fields.Select(s =>
{
if (!double.TryParse(s, out double n))
{
throw new Exception(" Can't convert field to number.");
}
return(n);
}).ToArray();
Vector3d GetVector(int index) => new Vector3d(numbers[index], numbers[index + 1], numbers[index + 2]);
var position = parameterIndex.TryGetValue("position", out int positionIndex) ? (Point3d)GetVector(positionIndex) : referenceTarget.Plane.Origin;
var normal = parameterIndex.TryGetValue("normal", out int normalIndex) ? GetVector(normalIndex) : referenceTarget.Plane.Normal;
if (reverse)
{
normal *= -1.0;
}
var xaxis = parameterIndex.TryGetValue("xaxis", out int xaxisIndex) ? GetVector(xaxisIndex) : referenceTarget.Plane.XAxis;
if (point != null)
{
Point3d localPoint = (Point3d)point;
localPoint.Transform(Transform.PlaneToPlane(referenceTarget.Frame.Plane, Plane.WorldXY));
xaxis = localPoint - position;
}
var plane = new Plane(position, normal);
double angle = Vector3d.VectorAngle(plane.XAxis, xaxis, plane);
plane.Rotate(angle, plane.Normal);
planes.Add(plane);
double speed = 0;
if (parameterIndex.TryGetValue("speed", out int speedIndex))
{
speed = numbers[speedIndex];
speedValues.Add(speed * (1.0 / 60.0));
}
if (parameterIndex.TryGetValue("zone", out int zoneIndex))
{
zoneValues.Add(numbers[zoneIndex]);
}
if (parameterIndex.TryGetValue("type", out int typeIndex))
{
types.Add(numbers[typeIndex]);
}
else
{
types.Add(speed <= cutSpeed ? 1 : 0);
}
}
var distinctSpeeds = speedValues.Distinct().Select(s => new Speed(translation: s, rotationSpeed: referenceTarget.Speed.RotationSpeed));
var speeds = speedValues.Select(v => distinctSpeeds.First(s => s.TranslationSpeed == v)).ToList();
var distinctZones = zoneValues.Distinct().Select(z => new Zone(distance: z));
var zones = zoneValues.Select(v => distinctZones.First(z => z.Distance == v)).ToList();
for (int i = 0; i < planes.Count; i++)
{
var speed = parameterIndex.ContainsKey("speed") ? speeds[i] : referenceTarget.Speed;
var zone = parameterIndex.ContainsKey("zone") ? zones[i] : referenceTarget.Zone;
var target = new CartesianTarget(planes[i], null, Motions.Joint, referenceTarget.Tool, speed, zone, null, referenceTarget.Frame);
Targets.Add(target);
}
{
Polyline polyline = null;
for (int i = 0; i < planes.Count; i++)
{
bool cutting = types[i] == 1;
Point3d vertex = planes[i].Origin;
if (cutting)
{
if (polyline == null)
{
polyline = new Polyline();
ToolPath.Add(polyline);
}
polyline.Add(vertex);
}
else
{
polyline = null;
}
}
}
foreach (var polyline in ToolPath)
{
polyline.CollapseShortSegments(0.01);
}
}
