public Target(Tool tool, Speed speed, Zone zone, Command command, Frame frame = null, IEnumerable<double> external = null)
{
this.Tool = tool ?? Tool.Default;
this.Speed = speed ?? Speed.Default;
this.Zone = zone ?? Zone.Default;
this.Frame = frame ?? Frame.Default;
this.Command = command;
this.External = (external != null) ? external.ToArray() : new double[0];
}
string Tool(Tool tool)
{
string pos = $" set_tcp({tool.Name}Tcp)";
string mass = $" set_payload({tool.Name}Weight, {tool.Name}Cog)";
/*
Plane tcp = tool.Tcp;
Plane originPlane = new Plane(Point3d.Origin, Vector3d.YAxis, -Vector3d.XAxis);
tcp.Transform(Transform.PlaneToPlane(Plane.WorldXY, originPlane));
Point3d tcpPoint = tcp.Origin / 1000;
double[] axisAngle = AxisAngle(tcp, Plane.WorldXY);
Point3d cog = tool.Centroid;
cog.Transform(Transform.PlaneToPlane(Plane.WorldXY, originPlane));
cog /= 1000;
string tcpString = $"p[{tcpPoint.X:0.00000}, {tcpPoint.Y:0.00000}, {tcpPoint.Z:0.00000}, {axisAngle[0]:0.0000}, {axisAngle[1]:0.0000}, {axisAngle[2]:0.0000}]";
string cogString = $"[{cog.X:0.00000}, {cog.Y:0.00000}, {cog.Z:0.00000}]";
string pos = $" set_tcp({tcpString})";
string mass = $" set_payload({tool.Weight:0.000}, {cogString})";
*/
return $"{pos}\n{mass}";
}
public JointTarget(double[] joints, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable<double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Joints = joints;
}
public CartesianTarget(Plane plane, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable<double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Plane = plane;
this.Motion = motion;
this.Configuration = configuration;
}
static Tool()
{
Default = new Tool(Plane.WorldXY, "DefaultTool");
}
string Tool(Tool tool)
{
Quaternion quaternion = Quaternion.Rotation(Plane.WorldXY, tool.Tcp);
double weight = (tool.Weight > 0.001) ? tool.Weight : 0.001;
Point3d centroid = tool.Centroid;
if (centroid.DistanceTo(Point3d.Origin) < 0.001)
centroid = new Point3d(0, 0, 0.001);
string pos = $"[{tool.Tcp.OriginX:0.000},{tool.Tcp.OriginY:0.000},{tool.Tcp.OriginZ:0.000}]";
string orient = $"[{quaternion.A:0.0000},{quaternion.B:0.0000},{quaternion.C:0.0000},{quaternion.D:0.0000}]";
string loaddata = $"[{weight:0.000},[{centroid.X:0.000},{centroid.Y:0.000},{centroid.Z:0.000}],[1,0,0,0],0,0,0]";
return $"PERS tooldata {tool.Name}:=[TRUE,[{pos},{orient}],{loaddata}];";
}
public CartesianTarget(Plane plane, RobotConfigurations?configuration = null, Motions motion = Motions.Joint, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable <double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Plane = plane;
this.Motion = motion;
this.Configuration = configuration;
}
List <string> Program()
{
string indent = " ";
var code = new List <string>
{
"def Program():"
};
// Attribute declarations
foreach (var tool in program.Attributes.OfType <Tool>())
{
Plane tcp = tool.Tcp;
Plane originPlane = new Plane(Point3d.Origin, Vector3d.YAxis, -Vector3d.XAxis);
tcp.Transform(Transform.PlaneToPlane(Plane.WorldXY, originPlane));
Point3d tcpPoint = tcp.Origin / 1000;
tcp.Origin = tcpPoint;
double[] axisAngle = PlaneToAxisAngle(tcp, Plane.WorldXY);
Point3d cog = tool.Centroid;
cog.Transform(Transform.PlaneToPlane(Plane.WorldXY, originPlane));
cog /= 1000;
code.Add(indent + $"{tool.Name}Tcp = p[{axisAngle[0]:0.00000}, {axisAngle[1]:0.00000}, {axisAngle[2]:0.00000}, {axisAngle[3]:0.0000}, {axisAngle[4]:0.0000}, {axisAngle[5]:0.0000}]");
code.Add(indent + $"{tool.Name}Weight = {tool.Weight:0.000}");
code.Add(indent + $"{tool.Name}Cog = [{cog.X:0.00000}, {cog.Y:0.00000}, {cog.Z:0.00000}]");
}
foreach (var speed in program.Attributes.OfType <Speed>())
{
double linearSpeed = speed.TranslationSpeed / 1000;
code.Add(indent + $"{speed.Name} = {linearSpeed:0.00000}");
}
foreach (var zone in program.Attributes.OfType <Zone>())
{
double zoneDistance = zone.Distance / 1000;
code.Add(indent + $"{zone.Name} = {zoneDistance:0.00000}");
}
foreach (var command in program.Attributes.OfType <Command>())
{
string declaration = command.Declaration(cell);
if (declaration != null && declaration.Length > 0)
{
declaration = indent + declaration;
// declaration = indent + declaration.Replace("\n", "\n" + indent);
code.Add(declaration);
}
}
// Init commands
foreach (var command in program.InitCommands)
{
code.Add(command.Code(cell, Target.Default));
}
Tool currentTool = null;
// Targets
foreach (var cellTarget in program.Targets)
{
var programTarget = cellTarget.ProgramTargets[0];
var target = programTarget.Target;
if (currentTool == null || target.Tool != currentTool)
{
code.Add(Tool(target.Tool));
currentTool = target.Tool;
}
string moveText = null;
string zoneDistance = $"{target.Zone.Name:0.00000}";
// double zoneDistance = target.Zone.Distance / 1000;
if (programTarget.IsJointTarget || (programTarget.IsJointMotion && programTarget.ForcedConfiguration))
{
double[] joints = programTarget.IsJointTarget ? (programTarget.Target as JointTarget).Joints : programTarget.Kinematics.Joints;
double maxAxisSpeed = robot.Joints.Max(x => x.MaxSpeed);
double percentage = (cellTarget.DeltaTime > 0) ? cellTarget.MinTime / cellTarget.DeltaTime : 0.1;
double axisSpeed = percentage * maxAxisSpeed;
double axisAccel = target.Speed.AxisAccel;
string speed = null;
if (target.Speed.Time == 0)
{
speed = $"v={axisSpeed: 0.000}";
}
else
{
speed = $"t={target.Speed.Time: 0.000}";
}
moveText = $" movej([{joints[0]:0.0000}, {joints[1]:0.0000}, {joints[2]:0.0000}, {joints[3]:0.0000}, {joints[4]:0.0000}, {joints[5]:0.0000}], a={axisAccel:0.0000}, {speed}, r={zoneDistance})";
}
else
{
var cartesian = target as CartesianTarget;
var plane = cartesian.Plane;
plane.Transform(Transform.PlaneToPlane(Plane.WorldXY, target.Frame.Plane));
plane.Transform(Transform.PlaneToPlane(cell.BasePlane, Plane.WorldXY));
var axisAngle = cell.PlaneToNumbers(plane);
switch (cartesian.Motion)
{
case Target.Motions.Joint:
{
double maxAxisSpeed = robot.Joints.Min(x => x.MaxSpeed);
double percentage = (cellTarget.DeltaTime > 0) ? cellTarget.MinTime / cellTarget.DeltaTime : 0.1;
double axisSpeed = percentage * maxAxisSpeed;
double axisAccel = target.Speed.AxisAccel;
string speed = null;
if (target.Speed.Time == 0)
{
speed = $"v={axisSpeed: 0.000}";
}
else
{
speed = $"t={target.Speed.Time: 0.000}";
}
moveText = $" movej(p[{axisAngle[0]:0.00000}, {axisAngle[1]:0.00000}, {axisAngle[2]:0.00000}, {axisAngle[3]:0.0000}, {axisAngle[4]:0.0000}, {axisAngle[5]:0.0000}],a={axisAccel:0.00000},{speed},r={zoneDistance})";
break;
}
case Target.Motions.Linear:
{
double linearSpeed = target.Speed.TranslationSpeed / 1000;
double linearAccel = target.Speed.TranslationAccel / 1000;
string speed = null;
if (target.Speed.Time == 0)
{
// speed = $"v={linearSpeed: 0.000}";
speed = $"v={target.Speed.Name}";
}
else
{
speed = $"t={target.Speed.Time: 0.000}";
}
moveText = $" movel(p[{axisAngle[0]:0.00000}, {axisAngle[1]:0.00000}, {axisAngle[2]:0.00000}, {axisAngle[3]:0.0000}, {axisAngle[4]:0.0000}, {axisAngle[5]:0.0000}],a={linearAccel:0.00000},{speed},r={zoneDistance})";
break;
}
}
}
code.Add(moveText);
foreach (var command in programTarget.Commands)
{
string commands = command.Code(cell, target);
commands = indent + commands;
// commands = indent + commands.Replace("\n", "\n" + indent);
code.Add(commands);
}
}
code.Add("end");
return(code);
}
public JointTarget(double[] joints, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable <double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Joints = joints;
}
static Tool()
{
Default = new Tool(Plane.WorldXY, "DefaultTool");
}
void SetMeshes(Tool tool)
{
{
Meshes[0] = mechanism.BaseMesh.DuplicateMesh();
Meshes[0].Transform(Planes[0].ToTransform());
}
for (int i = 0; i < mechanism.Joints.Length; i++)
{
var jointMesh = mechanism.Joints[i].Mesh.DuplicateMesh();
jointMesh.Transform(Transform.PlaneToPlane(mechanism.Joints[i].Plane, Planes[i + 1]));
Meshes[i + 1] = jointMesh;
}
}
string Tool(Tool tool)
{
double[] euler = PlaneToEuler(tool.Tcp);
return($"DECL GLOBAL FRAME {tool.Name}={{FRAME: X {euler[0]:0.###},Y {euler[1]:0.###},Z {euler[2]:0.###},A {euler[3]:0.####},B {euler[4]:0.####},C {euler[5]:0.####}}}");
}
List <string> SrcFile(int file, int group)
{
string groupName = cell.MechanicalGroups[group].Name;
int start = program.MultiFileIndices[file];
int end = (file == program.MultiFileIndices.Count - 1) ? program.Targets.Count : program.MultiFileIndices[file + 1];
var code = new List <string>
{
$@"&ACCESS RVP
&REL 1
DEF {program.Name}_{groupName}_{file:000}()
"
};
Tool currentTool = null;
Frame currentFrame = null;
Speed currentSpeed = null;
double currentPercentSpeed = 0;
Zone currentZone = null;
for (int j = start; j < end; j++)
{
var cellTarget = program.Targets[j];
var programTarget = cellTarget.ProgramTargets[group];
var target = programTarget.Target;
if (currentTool == null || target.Tool != currentTool)
{
code.Add(SetTool(target.Tool));
currentTool = target.Tool;
}
if (currentFrame == null || target.Frame != currentFrame)
{
if (target.Frame.IsCoupled)
{
int mech = target.Frame.CoupledMechanism + 2;
code.Add($"$BASE = EK(MACHINE_DEF[{mech}].ROOT, MACHINE_DEF[{mech}].MECH_TYPE, {target.Frame.Name})");
code.Add($"$ACT_EX_AX = 2");
}
else
{
code.Add($"$BASE={target.Frame.Name}");
}
currentFrame = target.Frame;
}
if (target.Zone.IsFlyBy && (currentZone == null || target.Zone != currentZone))
{
code.Add($"$APO.CDIS={target.Zone.Name}");
currentZone = target.Zone;
}
if (programTarget.Index > 0)
{
if (programTarget.LeadingJoint > 5)
{
code.Add(ExternalSpeed(programTarget));
}
else
{
if (currentSpeed == null || target.Speed != currentSpeed)
{
if (!programTarget.IsJointMotion)
{
double rotation = target.Speed.RotationSpeed.ToDegrees();
// code.Add($"$VEL={{CP {target.Speed.Name}, ORI1 {rotation:0.000}, ORI2 {rotation:0.000}}}");
code.Add($"$VEL.CP = {target.Speed.Name}\r\n$VEL.ORI1 = {rotation:0.###}\r\n$VEL.ORI2 = {rotation:0.####}");
currentSpeed = target.Speed;
}
}
if (programTarget.IsJointMotion)
{
double percentSpeed = cellTarget.MinTime / cellTarget.DeltaTime;
if (Abs(currentPercentSpeed - percentSpeed) > UnitTol)
{
code.Add("BAS(#VEL_PTP, 100)");
if (cellTarget.DeltaTime > UnitTol)
{
code.Add($"$VEL_AXIS[{programTarget.LeadingJoint + 1}] = {percentSpeed * 100:0.000}");
}
currentPercentSpeed = percentSpeed;
}
}
}
}
// external axes
string external = string.Empty;
double[] values = cell.MechanicalGroups[group].RadiansToDegreesExternal(target);
for (int i = 0; i < target.External.Length; i++)
{
int num = i + 1;
external += $", E{num} {values[i]:0.####}";
}
// motion command
string moveText = null;
if (programTarget.IsJointTarget)
{
var jointTarget = target as JointTarget;
double[] jointDegrees = jointTarget.Joints.Select((x, i) => cell.MechanicalGroups[group].Robot.RadianToDegree(x, i)).ToArray();
moveText = $"PTP {{A1 {jointDegrees[0]:0.####},A2 {jointDegrees[1]:0.####},A3 {jointDegrees[2]:0.####},A4 {jointDegrees[3]:0.####},A5 {jointDegrees[4]:0.####},A6 {jointDegrees[5]:0.####}{external}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_PTP";
}
}
else
{
var cartesian = target as CartesianTarget;
var plane = cartesian.Plane;
var euler = PlaneToEuler(plane);
switch (cartesian.Motion)
{
case Motions.Joint:
{
string bits = string.Empty;
// if (target.ChangesConfiguration)
{
double[] jointDegrees = programTarget.Kinematics.Joints.Select((x, i) => cell.MechanicalGroups[group].Robot.RadianToDegree(x, i)).ToArray();
int turnNum = 0;
for (int i = 0; i < 6; i++)
{
if (jointDegrees[i] < 0)
{
turnNum += (int)Pow(2, i);
}
}
var configuration = programTarget.Kinematics.Configuration;
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
elbow = !elbow;
bool wrist = configuration.HasFlag(RobotConfigurations.Wrist);
int configNum = 0;
if (shoulder)
{
configNum += 1;
}
if (elbow)
{
configNum += 2;
}
if (wrist)
{
configNum += 4;
}
string status = Convert.ToString(configNum, 2);
string turn = Convert.ToString(turnNum, 2);
bits = $", S'B{status:000}',T'B{turn:000000}'";
}
moveText = $"PTP {{X {euler[0]:0.###},Y {euler[1]:0.###},Z {euler[2]:0.###},A {euler[3]:0.####},B {euler[4]:0.####},C {euler[5]:0.####}{external}{bits}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_PTP";
}
break;
}
case Motions.Linear:
{
moveText = $"LIN {{X {euler[0]:0.###},Y {euler[1]:0.###},Z {euler[2]:0.###},A {euler[3]:0.####},B {euler[4]:0.####},C {euler[5]:0.####}{external}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_DIS";
}
break;
}
}
}
foreach (var command in programTarget.Commands.Where(c => c.RunBefore))
{
code.Add(command.Code(program, target));
}
code.Add(moveText);
foreach (var command in programTarget.Commands.Where(c => !c.RunBefore))
{
code.Add(command.Code(program, target));
}
}
code.Add("END");
return(code);
}
string Tool(Tool tool)
{
double[] euler = PlaneToEuler(tool.Tcp);
return $"DECL GLOBAL FRAME {tool.Name}={{FRAME: X {euler[0]:0.000},Y {euler[1]:0.000},Z {euler[2]:0.000},A {euler[3]:0.000},B {euler[4]:0.000},C {euler[5]:0.000}}}";
}
string SetTool(Tool tool)
{
string toolTxt = $"$TOOL={tool.Name}";
string load = $"$LOAD.M={tool.Weight}";
Point3d centroid = tool.Centroid;
string centroidTxt = $"$LOAD.CM={{X {centroid.X:0.000},Y {centroid.Y:0.000},Z {centroid.Z:0.000},A 0,B 0,C 0}}";
return $"{toolTxt}\r\n{load}\r\n{centroidTxt}";
}
