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; }