internal void SetTargetKinematics(KinematicSolution kinematics, List<string> errors, List<string> warnings, ProgramTarget prevTarget)
{
this.Kinematics = kinematics;
if (errors != null && kinematics.Errors.Count > 0)
{
errors.Add($"Errors in target {this.Index} of robot {this.Group}:");
errors.AddRange(kinematics.Errors);
}
if (warnings != null && prevTarget != null && prevTarget.Kinematics.Configuration != kinematics.Configuration)
{
this.ChangesConfiguration = true;
warnings.Add($"Configuration changed to \"{kinematics.Configuration.ToString()}\" on target {this.Index} of robot {this.Group}");
}
else
this.ChangesConfiguration = false;
}
public Plane GetPrevPlane(ProgramTarget prevTarget)
{
Plane prevPlane = prevTarget.WorldPlane;
if (prevTarget.Target.Tool != Target.Tool)
{
var toolPlane = Target.Tool.Tcp;
toolPlane.Transform(Transform.PlaneToPlane(prevTarget.Target.Tool.Tcp, prevPlane));
prevPlane = toolPlane;
}
Plane framePlane = Target.Frame.Plane;
if (Target.Frame.IsCoupled)
{
var planes = prevTarget.cellTarget.Planes;
Plane prevCoupledPlane = planes[Target.Frame.CoupledPlaneIndex];
framePlane.Transform(Transform.PlaneToPlane(Plane.WorldXY, prevCoupledPlane));
}
prevPlane.Transform(Transform.PlaneToPlane(framePlane, Plane.WorldXY));
return prevPlane;
}
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 void SetTargetKinematics(KinematicSolution kinematics, List <string> errors, List <string> warnings, ProgramTarget prevTarget)
{
this.Kinematics = kinematics;
if (errors.Count == 0 && kinematics.Errors.Count > 0)
{
errors.Add($"Errors in target {this.Index} of robot {this.Group}:");
errors.AddRange(kinematics.Errors);
}
if (warnings != null && prevTarget != null && prevTarget.Kinematics.Configuration != kinematics.Configuration)
{
this.ChangesConfiguration = true;
warnings.Add($"Configuration changed to \"{kinematics.Configuration.ToString()}\" on target {this.Index} of robot {this.Group}");
}
else
{
this.ChangesConfiguration = false;
}
}
void FixTargetAttributes(List <CellTarget> cellTargets)
{
// Fix externals
int resizeCount = 0;
ProgramTarget resizeTarget = null;
foreach (var cellTarget in cellTargets)
{
foreach (var programTarget in cellTarget.ProgramTargets)
{
int externalCount = 0;
if (robotSystem is RobotCell cell)
{
externalCount = cell.MechanicalGroups[programTarget.Group].Joints.Count - 6;
}
if (programTarget.Target.External.Length != externalCount)
{
double[] external = programTarget.Target.External;
Array.Resize <double>(ref external, externalCount);
programTarget.Target.External = external;
resizeCount++;
if (resizeTarget == null)
{
resizeTarget = programTarget;
}
}
}
}
if (resizeCount > 0)
{
program.Warnings.Add($"{resizeCount} targets had wrong number of external axes configured, the first one being target {resizeTarget.Index} of robot {resizeTarget.Group}.");
}
// Warn about defaults
var defaultTools = cellTargets.SelectMany(x => x.ProgramTargets).Where(x => x.Target.Tool == Tool.Default);
if (defaultTools.Count() > 0)
{
program.Warnings.Add($" {defaultTools.Count()} targets have their tool set to default, the first one being target {defaultTools.First().Index} in robot {defaultTools.First().Group}");
}
var defaultSpeeds = cellTargets.SelectMany(x => x.ProgramTargets).Where(x => x.Target.Speed == Speed.Default);
if (defaultSpeeds.Count() > 0)
{
program.Warnings.Add($" {defaultSpeeds.Count()} targets have their speed set to default, the first one being target {defaultSpeeds.First().Index} in robot {defaultSpeeds.First().Group}");
}
var linearForced = cellTargets.SelectMany(x => x.ProgramTargets).Where(x => x.Target is CartesianTarget).Where(x => (x.Target as CartesianTarget).Motion == Target.Motions.Linear && (x.Target as CartesianTarget).Configuration != null);
if (linearForced.Count() > 0)
{
program.Warnings.Add($" {linearForced.Count()} targets are set to linear with a forced configuration, the first one being target {linearForced.First().Index} in robot {linearForced.First().Group}");
}
foreach (var target in linearForced)
{
var cartesian = target.Target as CartesianTarget;
cartesian.Configuration = null;
}
// Check max payload
var tools = cellTargets.SelectMany(x => x.ProgramTargets.Select(y => new { Tool = y.Target.Tool, Group = y.Group })).Distinct();
foreach (var tool in tools)
{
double payload = robotSystem.Payload(tool.Group);
if (tool.Tool.Weight > payload)
{
program.Warnings.Add($"Weight of tool {tool.Tool.Name} exceeds the robot {tool.Group} rated payload of {payload} kg");
}
}
// Get unique attributes
program.Attributes.AddRange(tools.Select(x => x.Tool).Distinct());
program.Attributes.AddRange(cellTargets.SelectMany(x => x.ProgramTargets.Select(y => y.Target.Frame)).Distinct());
program.Attributes.AddRange(cellTargets.SelectMany(x => x.ProgramTargets.Select(y => y.Target.Speed)).Distinct());
program.Attributes.AddRange(cellTargets.SelectMany(x => x.ProgramTargets.Select(y => y.Target.Zone)).Distinct());
var commands = new List <Command>();
commands.AddRange(program.InitCommands);
commands.AddRange(cellTargets.SelectMany(x => x.ProgramTargets.SelectMany(y => y.Commands)));
program.Attributes.AddRange(commands.Distinct());
// Name attributes with no name
{
var types = new List <Type>();
foreach (var attribute in program.Attributes.ToList())
{
if (attribute.Name == null)
{
var type = attribute.GetType();
types.Add(type);
int i = types.FindAll(x => x == type).Count();
string name = $"{type.Name}{i - 1:000}";
SetAttributeName(attribute, cellTargets.SelectMany(x => x.ProgramTargets), name);
}
}
}
// Rename attributes with duplicate names
{
var duplicates = program.Attributes.GroupBy(x => x.Name).Where(x => x.Count() > 1);
foreach (var group in duplicates)
{
program.Warnings.Add($"Multiple target attributes named \"{group.Key}\" found");
int i = 0;
foreach (var attribute in group)
{
string name = $"{attribute.Name}{i++:000}";
SetAttributeName(attribute, cellTargets.SelectMany(x => x.ProgramTargets), name);
}
}
}
// Fix frames
{
foreach (var frame in program.Attributes.OfType <Frame>())
{
if (frame.CoupledMechanicalGroup == -1 && frame.CoupledMechanism != -1)
{
throw new Exception($" Frame {frame.Name} has a coupled mechanism set but no mechanical group.");
}
if (frame.CoupledMechanicalGroup == 0 && frame.CoupledMechanism == -1)
{
throw new Exception($" Frame {frame.Name} is set to couple the robot rather than a mechanism.");
}
if (frame.IsCoupled)
{
var cell = robotSystem as RobotCell;
if (frame.CoupledMechanicalGroup > cell.MechanicalGroups.Count - 1)
{
throw new Exception($" Frame {frame.Name} is set to couple an inexistant mechanical group.");
}
if (frame.CoupledMechanism > cell.MechanicalGroups[frame.CoupledMechanicalGroup].Externals.Count - 1)
{
throw new Exception($" Frame {frame.Name} is set to couple an inexistant mechanism.");
}
frame.CoupledPlaneIndex = (robotSystem as RobotCell).GetPlaneIndex(frame);
}
}
}
}
Tuple <double, double, int> GetSpeeds(ProgramTarget target, ProgramTarget prevTarget)
{
Plane prevPlane = target.GetPrevPlane(prevTarget);
var joints = robotSystem.GetJoints(target.Group).ToArray();
double deltaTime = 0;
// Axis
double deltaAxisTime = 0;
int leadingJoint = -1;
for (int i = 0; i < target.Kinematics.Joints.Length; i++)
{
double deltaCurrentAxisTime = Abs(target.Kinematics.Joints[i] - prevTarget.Kinematics.Joints[i]) / joints[i].MaxSpeed;
if (deltaCurrentAxisTime > deltaAxisTime)
{
deltaAxisTime = deltaCurrentAxisTime;
leadingJoint = i;
}
}
// External
double deltaExternalTime = 0;
int externalLeadingJoint = -1;
for (int i = 0; i < target.Target.External.Length; i++)
{
var joint = joints[i + 6];
double jointSpeed = joint.MaxSpeed;
if (joint is PrismaticJoint)
{
jointSpeed = Min(jointSpeed, target.Target.Speed.TranslationExternal);
}
else if (joint is RevoluteJoint)
{
jointSpeed = Min(jointSpeed, target.Target.Speed.RotationExternal);
}
double deltaCurrentExternalTime = Abs(target.Kinematics.Joints[i + 6] - prevTarget.Kinematics.Joints[i + 6]) / jointSpeed;
if (deltaCurrentExternalTime > deltaExternalTime)
{
deltaExternalTime = deltaCurrentExternalTime;
externalLeadingJoint = i + 6;
}
}
if (target.Target.Speed.Time == 0)
{
// Translation
double distance = prevPlane.Origin.DistanceTo(target.Plane.Origin);
double deltaLinearTime = distance / target.Target.Speed.TranslationSpeed;
// Rotation
double angleSwivel = Vector3d.VectorAngle(prevPlane.Normal, target.Plane.Normal);
double angleRotation = Vector3d.VectorAngle(prevPlane.XAxis, target.Plane.XAxis);
double angle = Max(angleSwivel, angleRotation);
double deltaRotationTime = angle / target.Target.Speed.RotationSpeed;
// Get slowest
double[] deltaTimes = new double[] { deltaLinearTime, deltaRotationTime, deltaAxisTime, deltaExternalTime };
int deltaIndex = -1;
for (int i = 0; i < deltaTimes.Length; i++)
{
if (deltaTimes[i] > deltaTime)
{
deltaTime = deltaTimes[i];
deltaIndex = i;
}
}
{
if (deltaTime < TimeTol)
{
program.Warnings.Add($"Position and orientation don't change for {target.Index}");
}
else if (deltaIndex == 1)
{
if (target.Index != lastIndex)
{
program.Warnings.Add($"Rotation speed limit reached in target {target.Index}");
}
lastIndex = target.Index;
}
else if (deltaIndex == 2)
{
if (target.Index != lastIndex)
{
program.Warnings.Add($"Axis {leadingJoint + 1} speed limit reached in target {target.Index}");
}
lastIndex = target.Index;
}
else if (deltaIndex == 3)
{
if (target.Index != lastIndex)
{
program.Warnings.Add($"External axis {externalLeadingJoint + 1} speed limit reached in target {target.Index}");
}
leadingJoint = externalLeadingJoint;
lastIndex = target.Index;
}
}
}
else
{
// Get slowest by time
double deltaTimeTime = target.Target.Speed.Time;
double[] deltaTimes = new double[] { deltaTimeTime, deltaAxisTime, deltaExternalTime };
int deltaIndex = -1;
for (int i = 0; i < deltaTimes.Length; i++)
{
if (deltaTimes[i] > deltaTime)
{
deltaTime = deltaTimes[i];
deltaIndex = i;
}
}
}
return(Tuple.Create(deltaTime, deltaAxisTime, leadingJoint));
}
Tuple<double, double, int> GetSpeeds(ProgramTarget target, ProgramTarget prevTarget)
{
Plane prevPlane = target.GetPrevPlane(prevTarget);
var joints = robotSystem.GetJoints(target.Group).ToArray();
double deltaTime = 0;
// Axis
double deltaAxisTime = 0;
int leadingJoint = -1;
for (int i = 0; i < target.Kinematics.Joints.Length; i++)
{
double deltaCurrentAxisTime = Abs(target.Kinematics.Joints[i] - prevTarget.Kinematics.Joints[i]) / joints[i].MaxSpeed;
if (deltaCurrentAxisTime > deltaAxisTime)
{
deltaAxisTime = deltaCurrentAxisTime;
leadingJoint = i;
}
}
// External
double deltaExternalTime = 0;
int externalLeadingJoint = -1;
for (int i = 0; i < target.Target.External.Length; i++)
{
var joint = joints[i + 6];
double jointSpeed = joint.MaxSpeed;
if (joint is PrismaticJoint) jointSpeed = Min(jointSpeed, target.Target.Speed.TranslationExternal);
else if (joint is RevoluteJoint) jointSpeed = Min(jointSpeed, target.Target.Speed.RotationExternal);
double deltaCurrentExternalTime = Abs(target.Kinematics.Joints[i + 6] - prevTarget.Kinematics.Joints[i + 6]) / jointSpeed;
if (deltaCurrentExternalTime > deltaExternalTime)
{
deltaExternalTime = deltaCurrentExternalTime;
externalLeadingJoint = i + 6;
}
}
if (target.Target.Speed.Time == 0)
{
// Translation
double distance = prevPlane.Origin.DistanceTo(target.Plane.Origin);
double deltaLinearTime = distance / target.Target.Speed.TranslationSpeed;
// Rotation
double angleSwivel = Vector3d.VectorAngle(prevPlane.Normal, target.Plane.Normal);
double angleRotation = Vector3d.VectorAngle(prevPlane.XAxis, target.Plane.XAxis);
double angle = Max(angleSwivel, angleRotation);
double deltaRotationTime = angle / target.Target.Speed.RotationSpeed;
// Get slowest
double[] deltaTimes = new double[] { deltaLinearTime, deltaRotationTime, deltaAxisTime, deltaExternalTime };
int deltaIndex = -1;
for (int i = 0; i < deltaTimes.Length; i++)
{
if (deltaTimes[i] > deltaTime)
{
deltaTime = deltaTimes[i];
deltaIndex = i;
}
}
{
if (deltaTime < TimeTol)
{
program.Warnings.Add($"Position and orientation don't change for {target.Index}");
}
else if (deltaIndex == 1)
{
if (target.Index != lastIndex) program.Warnings.Add($"Rotation speed limit reached in target {target.Index}");
lastIndex = target.Index;
}
else if (deltaIndex == 2)
{
if (target.Index != lastIndex) program.Warnings.Add($"Axis {leadingJoint + 1} speed limit reached in target {target.Index}");
lastIndex = target.Index;
}
else if (deltaIndex == 3)
{
if (target.Index != lastIndex) program.Warnings.Add($"External axis {externalLeadingJoint + 1} speed limit reached in target {target.Index}");
leadingJoint = externalLeadingJoint;
lastIndex = target.Index;
}
}
}
else
{
// Get slowest by time
double deltaTimeTime = target.Target.Speed.Time;
double[] deltaTimes = new double[] { deltaTimeTime, deltaAxisTime, deltaExternalTime };
int deltaIndex = -1;
for (int i = 0; i < deltaTimes.Length; i++)
{
if (deltaTimes[i] > deltaTime)
{
deltaTime = deltaTimes[i];
deltaIndex = i;
}
}
}
return Tuple.Create(deltaTime, deltaAxisTime, leadingJoint);
}
string ExternalSpeed(ProgramTarget target)
{
string externalSpeedCode = "";
var joints = cell.GetJoints(target.Group);
// int externalJointsCount = target.External.Length;
// for (int i = 0; i < externalJointsCount; i++)
{
var joint = joints[target.LeadingJoint];
double percentSpeed = 0;
if (joint is PrismaticJoint) percentSpeed = target.Target.Speed.TranslationExternal / joint.MaxSpeed;
if (joint is RevoluteJoint) percentSpeed = target.Target.Speed.RotationExternal / joint.MaxSpeed;
percentSpeed = Clamp(percentSpeed, 0.0, 1.0);
externalSpeedCode += $"BAS(#VEL_PTP, 100)" + "\r\n";
externalSpeedCode += $"$VEL_EXTAX[{target.LeadingJoint + 1 - 6}] = {percentSpeed * 100:0.000}";
// if (i < externalJointsCount - 1) externalSpeedCode += "\r\n";
}
return externalSpeedCode;
}