Program(string name, RobotSystem robotSystem, List<List<List<string>>> code)
{
this.Name = name;
this.RobotSystem = robotSystem;
this.Code = code;
this.HasCustomCode = true;
}
Program(string name, RobotSystem robotSystem, List <List <List <string> > > code)
{
this.Name = name;
this.RobotSystem = robotSystem;
this.Code = code;
this.HasCustomCode = true;
}
Program(string name, RobotSystem robotSystem, List <int> multiFileIndices, List <List <List <string> > > code)
{
this.Name = name;
this.RobotSystem = robotSystem;
this.Code = code;
this.HasCustomCode = true;
this.MultiFileIndices = multiFileIndices;
}
internal CheckProgram(Program program, List <CellTarget> cellTargets, double stepSize)
{
this.robotSystem = program.RobotSystem;
this.program = program;
this.groupCount = cellTargets[0].ProgramTargets.Count;
FixFirstTarget(cellTargets[0]);
FixTargetAttributes(cellTargets);
this.indexError = FixTargetMotions(cellTargets, stepSize);
}
internal string Code(RobotSystem robotSystem, Target target)
{
switch (robotSystem.Manufacturer)
{
case (Manufacturers.ABB): return(CodeAbb(robotSystem, target));
case (Manufacturers.KUKA): return(CodeKuka(robotSystem, target));
case (Manufacturers.UR): return(CodeUR(robotSystem, target));
default: return(null);
}
}
internal Collision(Program program, IEnumerable <int> first, IEnumerable <int> second, Mesh environment, int environmentPlane, double linearStep, double angularStep)
{
this.program = program;
this.robotSystem = program.RobotSystem;
this.linearStep = linearStep;
this.angularStep = angularStep;
this.first = first;
this.second = second;
this.environment = environment;
this.environmentPlane = environmentPlane;
Collide();
}
public static List <Mesh> PoseMeshes(RobotSystem robot, List <KinematicSolution> solutions, List <Mesh> tools)
{
if (robot is RobotCell cell)
{
var meshes = solutions.SelectMany((_, i) => PoseMeshes(cell.MechanicalGroups[i], solutions[i].Planes, tools[i])).ToList();
return(meshes);
}
else
{
var ur = robot as RobotCellUR;
var meshes = PoseMeshesRobot(ur.Robot, solutions[0].Planes, tools[0]);
return(meshes);
}
}
public Program(string name, RobotSystem robotSystem, IEnumerable<IEnumerable<Target>> targets, Commands.Group initCommands = null, IEnumerable<int> multiFileIndices = null, double stepSize = 1.0)
{
if (targets.SelectMany(x => x).Count() == 0)
throw new Exception(" The program has to contain at least 1 target");
int targetCount = targets.First().Count();
foreach (var subTargets in targets)
{
if (subTargets.Count() != targetCount) throw new Exception(" All sub programs have to contain the same number of targets");
}
this.Name = name;
this.RobotSystem = robotSystem;
this.InitCommands = new Commands.Group();
if (initCommands != null) this.InitCommands.AddRange(initCommands.Flatten());
if (multiFileIndices != null && multiFileIndices.Count() > 0)
{
multiFileIndices = multiFileIndices.Where(x => x < targetCount);
this.MultiFileIndices = multiFileIndices.ToList();
this.MultiFileIndices.Sort();
if (this.MultiFileIndices.Count == 0 || this.MultiFileIndices[0] != 0) this.MultiFileIndices.Insert(0, 0);
}
else
this.MultiFileIndices = new int[1].ToList();
var cellTargets = new List<CellTarget>(targetCount);
int targetIndex = 0;
foreach (var subTargets in targets.Transpose())
{
var programTargets = subTargets.Select((x, i) => new ProgramTarget(subTargets[i], i));
var cellTarget = new CellTarget(programTargets, targetIndex);
cellTargets.Add(cellTarget);
targetIndex++;
}
var checkProgram = new CheckProgram(this, cellTargets, stepSize);
int indexError = checkProgram.indexError;
if (indexError != -1) cellTargets = cellTargets.GetRange(0, indexError + 1).ToList();
this.Targets = cellTargets;
this.simulation = new Simulation(this, checkProgram.keyframes);
if (Errors.Count == 0)
Code = RobotSystem.Code(this);
}
internal string Declaration(RobotSystem robotSystem)
{
ErrorChecking(robotSystem);
switch (robotSystem.Manufacturer)
{
case (Manufacturers.ABB): return(DeclarationAbb(robotSystem));
case (Manufacturers.KUKA): return(DeclarationKuka(robotSystem));
case (Manufacturers.UR): return(DeclarationUR(robotSystem));
default: return(null);
}
}
internal Collision(Program program, IEnumerable <int> first, IEnumerable <int> second, Mesh environment, int environmentPlane, double linearStep, double angularStep)
{
this.program = program;
this.robotSystem = program.RobotSystem;
this.linearStep = linearStep;
this.angularStep = angularStep;
this.first = first;
this.second = second;
this.environment = environment;
this.environmentPlane = environmentPlane;
if (first.Count() == 1 && second.Count() == 1)
{
_onlyOne = true;
_oneFirst = first.First();
_oneSecond = second.First();
}
Collide();
}
public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double start, double end)
{
double[] allJoints = JointTarget.Lerp(prevTarget.Kinematics.Joints, Kinematics.Joints, t, start, end);
var external = allJoints.RangeSubset(6, Target.External.Length);
if (IsJointMotion)
{
var joints = allJoints.RangeSubset(0, 6);
return(new JointTarget(joints, Target, external));
}
else
{
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.Linear, external);
// target.Frame = Frame.Default;
return(target);
}
}
internal Collision(Program program, IEnumerable <int> first, IEnumerable <int> second, Mesh environment, int environmentPlane, double linearStep, double angularStep)
{
#if RHINOCOMMON
this.program = program;
this.robotSystem = program.RobotSystem;
this.linearStep = linearStep;
this.angularStep = angularStep;
this.first = first;
this.second = second;
this.environment = environment;
this.environmentPlane = environmentPlane;
if (first.Count() == 1 && second.Count() == 1)
{
_onlyOne = true;
_oneFirst = first.First();
_oneSecond = second.First();
}
Collide();
#else
throw new NotImplementedException(" Collisions have to be reimplemented.");
#endif
}
protected virtual string CodeUR(RobotSystem robotSystem, Target target)
{
throw new Exception($"Command {this.GetType().Name} for UR not implemented.");
}
internal IEnumerable <Target> Lerp(CellTarget prevTarget, RobotSystem robot, double t, double start, double end)
{
return(ProgramTargets.Select((x, i) => x.Lerp(prevTarget.ProgramTargets[i], robot, t, start, end)));
}
public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double start, double end)
{
double[] allJoints = JointTarget.Lerp(prevTarget.Kinematics.Joints, Kinematics.Joints, t, start, end);
var external = allJoints.RangeSubset(6, Target.External.Length);
if (IsJointMotion)
{
var joints = allJoints.RangeSubset(0, 6);
return new JointTarget(joints, Target, external);
}
else
{
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, Target.Motions.Linear, external);
// target.Frame = Frame.Default;
return target;
}
}
internal IEnumerable<Target> Lerp(CellTarget prevTarget, RobotSystem robot, double t, double start, double end)
{
return ProgramTargets.Select((x, i) => x.Lerp(prevTarget.ProgramTargets[i], robot, t, start, end));
}
internal Collision(Program program, IEnumerable<int> first, IEnumerable<int> second, Mesh environment, int environmentPlane, double linearStep, double angularStep)
{
this.program = program;
this.robotSystem = program.RobotSystem;
this.linearStep = linearStep;
this.angularStep = angularStep;
this.first = first;
this.second = second;
this.environment = environment;
this.environmentPlane = environmentPlane;
Collide();
}
protected virtual void ErrorChecking(RobotSystem robotSystem)
{
}
public void Save(string folder) => RobotSystem.SaveCode(this, folder);
protected virtual string DeclarationUR(RobotSystem robotSystem) => null;
public Program(string name, RobotSystem robotSystem, IEnumerable <IEnumerable <Target> > targets, Commands.Group initCommands = null, IEnumerable <int> multiFileIndices = null, double stepSize = 1.0)
{
if (targets.SelectMany(x => x).Count() == 0)
{
throw new Exception(" The program has to contain at least 1 target");
}
int targetCount = targets.First().Count();
foreach (var subTargets in targets)
{
if (subTargets.Count() != targetCount)
{
throw new Exception(" All sub programs have to contain the same number of targets");
}
}
this.Name = name;
this.RobotSystem = robotSystem;
this.InitCommands = new Commands.Group();
if (initCommands != null)
{
this.InitCommands.AddRange(initCommands.Flatten());
}
if (multiFileIndices != null && multiFileIndices.Count() > 0)
{
multiFileIndices = multiFileIndices.Where(x => x < targetCount);
this.MultiFileIndices = multiFileIndices.ToList();
this.MultiFileIndices.Sort();
if (this.MultiFileIndices.Count == 0 || this.MultiFileIndices[0] != 0)
{
this.MultiFileIndices.Insert(0, 0);
}
}
else
{
this.MultiFileIndices = new int[1].ToList();
}
var cellTargets = new List <CellTarget>(targetCount);
int targetIndex = 0;
foreach (var subTargets in targets.Transpose())
{
var programTargets = subTargets.Select((x, i) => new ProgramTarget(subTargets[i], i));
var cellTarget = new CellTarget(programTargets, targetIndex);
cellTargets.Add(cellTarget);
targetIndex++;
}
var checkProgram = new CheckProgram(this, cellTargets, stepSize);
int indexError = checkProgram.indexError;
if (indexError != -1)
{
cellTargets = cellTargets.GetRange(0, indexError + 1).ToList();
}
this.Targets = cellTargets;
this.simulation = new Simulation(this, checkProgram.keyframes);
if (Errors.Count == 0)
{
Code = RobotSystem.Code(this);
}
}
internal CheckProgram(Program program, List<CellTarget> cellTargets, double stepSize)
{
this.robotSystem = program.RobotSystem;
this.program = program;
this.groupCount = cellTargets[0].ProgramTargets.Count;
FixFirstTarget(cellTargets[0]);
FixTargetAttributes(cellTargets);
this.indexError = FixTargetMotions(cellTargets, stepSize);
}