internal void Step(double time, bool isNormalized)
{
if (keyframes.Count == 1)
{
this.currentSimulationTarget = program.Targets[0].ShallowClone();
var firstKinematics = program.RobotSystem.Kinematics(keyframes[0].ProgramTargets.Select(x => x.Target), null);
foreach (var programTarget in this.currentSimulationTarget.ProgramTargets)
{
programTarget.Kinematics = firstKinematics[programTarget.Group];
}
return;
}
if (isNormalized)
{
time *= program.Duration;
}
time = Clamp(time, 0, duration);
if (time >= currentTime)
{
for (int i = currentTarget; i < keyframes.Count - 1; i++)
{
if (keyframes[i + 1].TotalTime >= time)
{
currentTarget = i;
break;
}
}
}
else
{
for (int i = currentTarget; i >= 0; i--)
{
if (keyframes[i].TotalTime <= time)
{
currentTarget = i;
break;
}
}
}
currentTime = time;
var cellTarget = keyframes[currentTarget + 1];
var prevCellTarget = keyframes[currentTarget + 0];
var prevJoints = prevCellTarget.ProgramTargets.Select(x => x.Kinematics.Joints);
var kineTargets = cellTarget.Lerp(prevCellTarget, program.RobotSystem, currentTime, prevCellTarget.TotalTime, cellTarget.TotalTime);
var kinematics = program.RobotSystem.Kinematics(kineTargets, prevJoints);
// var newSimulationTarget = cellTarget.ShallowClone(cellTarget.Index);
var newSimulationTarget = program.Targets[cellTarget.Index].ShallowClone();
foreach (var programTarget in newSimulationTarget.ProgramTargets)
{
programTarget.Kinematics = kinematics[programTarget.Group];
}
this.currentSimulationTarget = newSimulationTarget;
}
public ProgramTarget ShallowClone(CellTarget cellTarget)
{
var target = MemberwiseClone() as ProgramTarget;
target.cellTarget = cellTarget;
target.Commands = null;
return(target);
}
internal Simulation(Program program, List <CellTarget> targets)
{
this.program = program;
this.groupCount = targets.Count;
duration = program.Duration;
currentSimulationTarget = program.Targets[0].ShallowClone(0);
this.keyframes = targets;
}
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);
}
void CheckUndefined(CellTarget cellTarget, List <CellTarget> cellTargets)
{
if (cellTarget.Index < cellTargets.Count - 1)
{
int i = cellTarget.Index;
foreach (var target in cellTarget.ProgramTargets.Where(x => x.Kinematics.Configuration == Target.RobotConfigurations.Undefined))
{
if (!cellTargets[i + 1].ProgramTargets[target.Group].IsJointMotion)
{
program.Errors.Add($"Undefined configuration (probably due to a singularity) in target {target.Index} of robot {target.Group} before a linear motion");
indexError = i;
}
}
}
}
void FixFirstTarget(CellTarget firstTarget)
{
var fix = firstTarget.ProgramTargets.Where(x => !x.IsJointTarget);
if (fix.Count() > 0)
{
var kinematics = robotSystem.Kinematics(firstTarget.ProgramTargets.Select(x => x.Target));
foreach (var programTarget in fix)
{
var kinematic = kinematics[programTarget.Group];
if (kinematic.Errors.Count > 0)
{
program.Errors.Add($"Errors in target {programTarget.Index} of robot {programTarget.Group}:");
program.Errors.AddRange(kinematic.Errors);
}
programTarget.Target = new JointTarget(kinematic.Joints, programTarget.Target);
program.Warnings.Add($"First target in robot {programTarget.Group} changed to a joint motion using axis rotations");
}
}
}
void Collide()
{
System.Threading.Tasks.Parallel.ForEach(program.Targets, (cellTarget, state) =>
{
if (cellTarget.Index == 0)
{
return;
}
var prevcellTarget = program.Targets[cellTarget.Index - 1];
double divisions = 1;
int groupCount = cellTarget.ProgramTargets.Count;
for (int group = 0; group < groupCount; group++)
{
var target = cellTarget.ProgramTargets[group];
var prevTarget = prevcellTarget.ProgramTargets[group];
double distance = prevTarget.WorldPlane.Origin.DistanceTo(target.WorldPlane.Origin);
double linearDivisions = Ceiling(distance / linearStep);
double maxAngle = target.Kinematics.Joints.Zip(prevTarget.Kinematics.Joints, (x, y) => Abs(x - y)).Max();
double angularDivisions = Ceiling(maxAngle / angularStep);
double tempDivisions = Max(linearDivisions, angularDivisions);
if (tempDivisions > divisions)
{
divisions = tempDivisions;
}
}
// double step = 1.0 / divisions;
int j = (cellTarget.Index == 1) ? 0 : 1;
for (int i = j; i < divisions; i++)
{
double t = (double)i / (double)divisions;
var kineTargets = cellTarget.Lerp(prevcellTarget, robotSystem, t, 0.0, 1.0);
var kinematics = program.RobotSystem.Kinematics(kineTargets, displayMeshes: true);
var meshes = kinematics.SelectMany(x => x.Meshes).ToList();
if (this.environment != null)
{
Mesh currentEnvironment = this.environment.DuplicateMesh();
if (this.environmentPlane != -1)
{
currentEnvironment.Transform(Transform.PlaneToPlane(Plane.WorldXY, kinematics.SelectMany(x => x.Planes).ToList()[environmentPlane]));
}
meshes.Add(currentEnvironment);
}
var setA = first.Select(x => meshes[x]);
var setB = second.Select(x => meshes[x]);
var meshClash = Rhino.Geometry.Intersect.MeshClash.Search(setA, setB, 1, 1);
if (meshClash.Length > 0 && (!HasCollision || CollisionTarget.Index > cellTarget.Index))
{
HasCollision = true;
Meshes = new Mesh[] { meshClash[0].MeshA, meshClash[0].MeshB };
this.CollisionTarget = cellTarget;
state.Break();
}
}
});
}
void CheckUndefined(CellTarget cellTarget, List<CellTarget> cellTargets)
{
if (cellTarget.Index < cellTargets.Count - 1)
{
int i = cellTarget.Index;
foreach (var target in cellTarget.ProgramTargets.Where(x => x.Kinematics.Configuration == Target.RobotConfigurations.Undefined))
{
if (!cellTargets[i + 1].ProgramTargets[target.Group].IsJointMotion)
{
program.Errors.Add($"Undefined configuration (probably due to a singularity) in target {target.Index} of robot {target.Group} before a linear motion");
indexError = i;
}
}
}
}
internal void SetTargetKinematics(List <KinematicSolution> kinematics, List <string> errors, List <string> warnings, CellTarget prevTarget = null)
{
foreach (var target in this.ProgramTargets)
{
target.SetTargetKinematics(kinematics[target.Group], errors, warnings, prevTarget?.ProgramTargets[target.Group]);
}
}
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 ProgramTarget ShallowClone(CellTarget cellTarget)
{
var target = MemberwiseClone() as ProgramTarget;
target.cellTarget = cellTarget;
target.Commands = null;
return target;
}
internal void SetTargetKinematics(List<KinematicSolution> kinematics, List<string> errors, List<string> warnings, CellTarget prevTarget = null)
{
foreach (var target in this.ProgramTargets) target.SetTargetKinematics(kinematics[target.Group], errors, warnings, prevTarget?.ProgramTargets[target.Group]);
}
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 Simulation(Program program, List<CellTarget> targets)
{
this.program = program;
this.groupCount = targets.Count;
duration = program.Duration;
currentSimulationTarget = targets[0].ShallowClone(0);
this.keyframes = targets;
}
internal void Step(double time, bool isNormalized)
{
if (keyframes.Count == 1)
{
this.currentSimulationTarget = keyframes[0].ShallowClone();
var firstKinematics = program.RobotSystem.Kinematics(keyframes[0].ProgramTargets.Select(x => x.Target), null, displayMeshes: true);
foreach (var programTarget in this.currentSimulationTarget.ProgramTargets) programTarget.Kinematics = firstKinematics[programTarget.Group];
return;
}
if (isNormalized) time *= program.Duration;
time = Clamp(time, 0, duration);
if (time >= currentTime)
{
for (int i = currentTarget; i < keyframes.Count - 1; i++)
{
if (keyframes[i + 1].TotalTime >= time)
{
currentTarget = i;
break;
}
}
}
else
{
for (int i = currentTarget; i >= 0; i--)
{
if (keyframes[i].TotalTime <= time)
{
currentTarget = i;
break;
}
}
}
currentTime = time;
var prevJoints = new List<double[]>();
var cellTarget = keyframes[currentTarget + 1];
var prevCellTarget = keyframes[currentTarget + 0];
var newSimulationTarget = cellTarget.ShallowClone(cellTarget.Index);
foreach (var programTarget in prevCellTarget.ProgramTargets) prevJoints.Add(programTarget.Kinematics.Joints);
var kineTargets = cellTarget.Lerp(prevCellTarget, program.RobotSystem, currentTime, prevCellTarget.TotalTime, cellTarget.TotalTime);
var kinematics = program.RobotSystem.Kinematics(kineTargets, prevJoints, displayMeshes: true);
foreach (var programTarget in newSimulationTarget.ProgramTargets) programTarget.Kinematics = kinematics[programTarget.Group];
this.currentSimulationTarget = newSimulationTarget;
}
void Collide()
{
Parallel.ForEach(program.Targets, (cellTarget, state) =>
{
if (cellTarget.Index == 0)
{
return;
}
var prevcellTarget = program.Targets[cellTarget.Index - 1];
double divisions = 1;
int groupCount = cellTarget.ProgramTargets.Count;
for (int group = 0; group < groupCount; group++)
{
var target = cellTarget.ProgramTargets[group];
var prevTarget = prevcellTarget.ProgramTargets[group];
double distance = prevTarget.WorldPlane.Origin.DistanceTo(target.WorldPlane.Origin);
double linearDivisions = Ceiling(distance / linearStep);
double maxAngle = target.Kinematics.Joints.Zip(prevTarget.Kinematics.Joints, (x, y) => Abs(x - y)).Max();
double angularDivisions = Ceiling(maxAngle / angularStep);
double tempDivisions = Max(linearDivisions, angularDivisions);
if (tempDivisions > divisions)
{
divisions = tempDivisions;
}
}
var meshes = new List <Mesh>();
int j = (cellTarget.Index == 1) ? 0 : 1;
for (int i = j; i < divisions; i++)
{
double t = (double)i / (double)divisions;
var kineTargets = cellTarget.Lerp(prevcellTarget, robotSystem, t, 0.0, 1.0);
var kinematics = program.RobotSystem.Kinematics(kineTargets);
meshes.Clear();
// TODO: Meshes not a property of KinematicSolution anymore
// meshes.AddRange(kinematics.SelectMany(x => x.Meshes));
var tools = cellTarget.ProgramTargets.Select(p => p.Target.Tool.Mesh).ToList();
var robotMeshes = PoseMeshes(program.RobotSystem, kinematics, tools);
meshes.AddRange(robotMeshes);
if (this.environment != null)
{
if (this.environmentPlane != -1)
{
Mesh currentEnvironment = this.environment.DuplicateMesh();
currentEnvironment.Transform(Transform.PlaneToPlane(Plane.WorldXY, kinematics.SelectMany(x => x.Planes).ToList()[environmentPlane]));
meshes.Add(currentEnvironment);
}
else
{
meshes.Add(this.environment);
}
}
if (_onlyOne)
{
var meshA = meshes[_oneFirst];
var meshB = meshes[_oneSecond];
var meshClash = Rhino.Geometry.Intersect.Intersection.MeshMeshFast(meshA, meshB);
if (meshClash.Length > 0 && (!HasCollision || CollisionTarget.Index > cellTarget.Index))
{
HasCollision = true;
Meshes = new Mesh[] { meshA, meshB };
this.CollisionTarget = cellTarget;
state.Break();
}
}
else
{
var setA = first.Select(x => meshes[x]);
var setB = second.Select(x => meshes[x]);
var meshClash = Rhino.Geometry.Intersect.MeshClash.Search(setA, setB, 1, 1);
if (meshClash.Length > 0 && (!HasCollision || CollisionTarget.Index > cellTarget.Index))
{
HasCollision = true;
Meshes = new Mesh[] { meshClash[0].MeshA, meshClash[0].MeshB };
this.CollisionTarget = cellTarget;
state.Break();
}
}
}
});
}
void Collide()
{
System.Threading.Tasks.Parallel.ForEach(program.Targets, (cellTarget, state) =>
{
if (cellTarget.Index == 0) return;
var prevcellTarget = program.Targets[cellTarget.Index - 1];
double divisions = 1;
int groupCount = cellTarget.ProgramTargets.Count;
for (int group = 0; group < groupCount; group++)
{
var target = cellTarget.ProgramTargets[group];
var prevTarget = prevcellTarget.ProgramTargets[group];
double distance = prevTarget.WorldPlane.Origin.DistanceTo(target.WorldPlane.Origin);
double linearDivisions = Ceiling(distance / linearStep);
double maxAngle = target.Kinematics.Joints.Zip(prevTarget.Kinematics.Joints, (x, y) => Abs(x - y)).Max();
double angularDivisions = Ceiling(maxAngle / angularStep);
double tempDivisions = Max(linearDivisions, angularDivisions);
if (tempDivisions > divisions) divisions = tempDivisions;
}
// double step = 1.0 / divisions;
int j = (cellTarget.Index == 1) ? 0 : 1;
for (int i = j; i < divisions; i++)
{
double t = (double)i / (double)divisions;
var kineTargets = cellTarget.Lerp(prevcellTarget, robotSystem, t, 0.0, 1.0);
var kinematics = program.RobotSystem.Kinematics(kineTargets, displayMeshes: true);
var meshes = kinematics.SelectMany(x => x.Meshes).ToList();
if (this.environment != null)
{
Mesh currentEnvironment = this.environment.DuplicateMesh();
if (this.environmentPlane != -1)
currentEnvironment.Transform(Transform.PlaneToPlane(Plane.WorldXY, kinematics.SelectMany(x => x.Planes).ToList()[environmentPlane]));
meshes.Add(currentEnvironment);
}
var setA = first.Select(x => meshes[x]);
var setB = second.Select(x => meshes[x]);
var meshClash = Rhino.Geometry.Intersect.MeshClash.Search(setA, setB, 1, 1);
if (meshClash.Length > 0 && (!HasCollision || CollisionTarget.Index > cellTarget.Index))
{
HasCollision = true;
Meshes = new Mesh[] { meshClash[0].MeshA, meshClash[0].MeshB };
this.CollisionTarget = cellTarget;
state.Break();
}
}
});
}
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);
}
}
void FixFirstTarget(CellTarget firstTarget)
{
var fix = firstTarget.ProgramTargets.Where(x => !x.IsJointTarget);
if (fix.Count() > 0)
{
var kinematics = robotSystem.Kinematics(firstTarget.ProgramTargets.Select(x => x.Target));
foreach (var programTarget in fix)
{
var kinematic = kinematics[programTarget.Group];
if (kinematic.Errors.Count > 0)
{
program.Errors.Add($"Errors in target {programTarget.Index} of robot {programTarget.Group}:");
program.Errors.AddRange(kinematic.Errors);
}
programTarget.Target = new JointTarget(kinematic.Joints, programTarget.Target);
program.Warnings.Add($"First target in robot {programTarget.Group} changed to a joint motion using axis rotations");
}
}
}
