internal int GetPlaneIndex(Frame frame)
{
if (frame.CoupledMechanism != -1)
{
int index = -1;
for (int i = 0; i < frame.CoupledMechanicalGroup; i++)
{
index += this.MechanicalGroups[i].Joints.Count + 2;
}
for (int i = 0; i <= frame.CoupledMechanism; i++)
{
index += this.MechanicalGroups[frame.CoupledMechanicalGroup].Externals[i].Joints.Length + 1;
}
return index;
}
else
{
int index = -1;
for (int i = 0; i <= frame.CoupledMechanicalGroup; i++)
{
index += this.MechanicalGroups[i].Joints.Count + 2;
}
return index;
}
}
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];
}
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 Frame()
{
Default = new Frame(Plane.WorldXY, -1, -1, "DefaultFrame");
}
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;
}
string Frame(Frame frame)
{
Plane plane = frame.Plane;
plane.Transform(Transform.PlaneToPlane(cell.BasePlane, Plane.WorldXY));
Quaternion quaternion = Quaternion.Rotation(Plane.WorldXY, plane);
string pos = $"[{plane.OriginX:0.000},{plane.OriginY:0.000},{plane.OriginZ:0.000}]";
string orient = $"[{quaternion.A:0.0000},{quaternion.B:0.0000},{quaternion.C:0.0000},{quaternion.D:0.0000}]";
string coupledMech = "";
string coupledBool = frame.IsCoupled ? "FALSE" : "TRUE";
if (frame.IsCoupled)
{
if (frame.CoupledMechanism == -1)
coupledMech = $"ROB_{frame.CoupledMechanicalGroup + 1}";
else
coupledMech = $"STN_{frame.CoupledMechanism + 1}";
}
return $@"TASK PERS wobjdata {frame.Name}:=[FALSE,{coupledBool},""{coupledMech}"",[{pos},{orient}],[[0,0,0],[1,0,0,0]]];";
}
string Frame(Frame frame)
{
Plane plane = frame.Plane;
plane.Transform(Transform.PlaneToPlane(cell.BasePlane, Plane.WorldXY));
double[] euler = PlaneToEuler(plane);
return $"DECL GLOBAL FRAME {frame.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}}}";
}