double[] GetClosestSolution(Transform transform, double[] prevJoints, out Target.RobotConfigurations configuration, out List <string> errors, out double difference)
{
var solutions = new double[8][];
var solutionsErrors = new List <List <string> >(8);
for (int i = 0; i < 8; i++)
{
solutions[i] = InverseKinematics(transform, (Target.RobotConfigurations)i, out var solutionErrors);
solutions[i] = JointTarget.GetAbsoluteJoints(solutions[i], prevJoints);
solutionsErrors.Add(solutionErrors);
}
int closestSolutionIndex = 0;
double closestDifference = double.MaxValue;
for (int i = 0; i < 8; i++)
{
double currentDifference = prevJoints.Zip(solutions[i], (x, y) => SquaredDifference(x, y)).Sum();
if (currentDifference < closestDifference)
{
closestSolutionIndex = i;
closestDifference = currentDifference;
}
}
difference = closestDifference;
configuration = (Target.RobotConfigurations)closestSolutionIndex;
errors = solutionsErrors[closestSolutionIndex];
return(solutions[closestSolutionIndex]);
}
List <string> SubModule(int file, int group)
{
bool multiProgram = program.MultiFileIndices.Count > 1;
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>();
if (multiProgram)
{
code.Add($"MODULE {program.Name}_{groupName}_{file:000}");
code.Add($"PROC Main()");
code.Add("ConfL \\Off;");
}
for (int j = start; j < end; j++)
{
var programTarget = program.Targets[j].ProgramTargets[group];
var target = programTarget.Target;
string moveText = null;
string zone = target.Zone.IsFlyBy ? target.Zone.Name : "fine";
string id = (cell.MechanicalGroups.Count > 1) ? id = $@"\ID:={programTarget.Index}" : "";
string external = "extj";
if (cell.MechanicalGroups[group].Externals.Count > 0)
{
var externals = new string[6];
for (int i = 0; i < 6; i++)
{
externals[i] = (i < target.External.Length) ? $"{target.External[i].ToDegrees():0.00}" : "9E9";
}
external = $"[{externals[0]},{externals[1]},{externals[2]},{externals[3]},{externals[4]},{externals[5]}]";
}
if (programTarget.IsJointTarget)
{
var jointTarget = programTarget.Target as JointTarget;
double[] joints = jointTarget.Joints;
joints = joints.Select((x, i) => cell.MechanicalGroups[group].RadianToDegree(x, i)).ToArray();
moveText = $"MoveAbsJ [[{joints[0]:0.000},{joints[1]:0.000},{joints[2]:0.000},{joints[3]:0.000},{joints[4]:0.000},{joints[5]:0.000}],{external}]{id},{target.Speed.Name},{zone},{target.Tool.Name};";
}
else
{
var cartesian = programTarget.Target as CartesianTarget;
var plane = cartesian.Plane;
var quaternion = Quaternion.Rotation(Plane.WorldXY, plane);
switch (cartesian.Motion)
{
case Target.Motions.Joint:
{
string pos = $"[{plane.OriginX:0.00},{plane.OriginY:0.00},{plane.OriginZ:0.00}]";
string orient = $"[{quaternion.A:0.0000},{quaternion.B:0.0000},{quaternion.C:0.0000},{quaternion.D:0.0000}]";
int cf1 = (int)Floor(programTarget.Kinematics.Joints[0] / (PI / 2));
int cf4 = (int)Floor(programTarget.Kinematics.Joints[3] / (PI / 2));
int cf6 = (int)Floor(programTarget.Kinematics.Joints[5] / (PI / 2));
if (cf1 < 0)
{
cf1--;
}
if (cf4 < 0)
{
cf4--;
}
if (cf6 < 0)
{
cf6--;
}
Target.RobotConfigurations configuration = (Target.RobotConfigurations)programTarget.Kinematics.Configuration;
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = configuration.HasFlag(Target.RobotConfigurations.Wrist);
int cfx = 0;
if (wrist)
{
cfx += 1;
}
if (elbow)
{
cfx += 2;
}
if (shoulder)
{
cfx += 4;
}
string conf = $"[{cf1},{cf4},{cf6},{cfx}]";
string robtarget = $"[{pos},{orient},{conf},{external}]";
moveText = $@"MoveJ {robtarget}{id},{target.Speed.Name},{zone},{target.Tool.Name} \WObj:={target.Frame.Name};";
break;
}
case Target.Motions.Linear:
{
string pos = $"[{plane.OriginX:0.00},{plane.OriginY:0.00},{plane.OriginZ:0.00}]";
string orient = $"[{quaternion.A:0.0000},{quaternion.B:0.0000},{quaternion.C:0.0000},{quaternion.D:0.0000}]";
string robtarget = $"[{pos},{orient},conf,{external}]";
moveText = $@"MoveL {robtarget}{id},{target.Speed.Name},{zone},{target.Tool.Name} \WObj:={target.Frame.Name};";
break;
}
}
}
code.Add(moveText);
foreach (var command in programTarget.Commands)
{
code.Add(command.Code(cell, target));
}
}
code.Add("ENDPROC");
code.Add("ENDMODULE");
return(code);
}
/// <summary>
/// Inverse kinematics for a offset wrist 6 axis robot.
/// Code adapted from https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_kinematics/src/ur_kin.cpp
/// </summary>
/// <param name="target">Cartesian target</param>
/// <returns>Returns the 6 rotation values in radians.</returns>
override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);
if (shoulder)
{
wrist = !wrist;
}
double[] joints = new double[6];
bool isUnreachable = false;
transform *= Transform.Rotation(PI / 2, Point3d.Origin);
double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();
// shoulder
{
double A = d[5] * transform[1, 2] - transform[1, 3];
double B = d[5] * transform[0, 2] - transform[0, 3];
double R = A * A + B * B;
double arccos = Acos(d[3] / Sqrt(R));
if (double.IsNaN(arccos))
{
errors.Add($"Overhead singularity.");
arccos = 0;
}
double arctan = Atan2(-B, A);
if (!shoulder)
{
joints[0] = arccos + arctan;
}
else
{
joints[0] = -arccos + arctan;
}
}
// wrist 2
{
double numer = (transform[0, 3] * Sin(joints[0]) - transform[1, 3] * Cos(joints[0]) - d[3]);
double div = numer / d[5];
double arccos = Acos(div);
if (double.IsNaN(arccos))
{
errors.Add($"Overhead singularity 2.");
arccos = PI;
isUnreachable = true;
}
if (!wrist)
{
joints[4] = arccos;
}
else
{
joints[4] = 2.0 * PI - arccos;
}
}
// rest
{
double c1 = Cos(joints[0]);
double s1 = Sin(joints[0]);
double c5 = Cos(joints[4]);
double s5 = Sin(joints[4]);
joints[5] = Atan2(Sign(s5) * -(transform[0, 1] * s1 - transform[1, 1] * c1), Sign(s5) * (transform[0, 0] * s1 - transform[1, 0] * c1));
double c6 = Cos(joints[5]), s6 = Sin(joints[5]);
double x04x = -s5 * (transform[0, 2] * c1 + transform[1, 2] * s1) - c5 * (s6 * (transform[0, 1] * c1 + transform[1, 1] * s1) - c6 * (transform[0, 0] * c1 + transform[1, 0] * s1));
double x04y = c5 * (transform[2, 0] * c6 - transform[2, 1] * s6) - transform[2, 2] * s5;
double p13x = d[4] * (s6 * (transform[0, 0] * c1 + transform[1, 0] * s1) + c6 * (transform[0, 1] * c1 + transform[1, 1] * s1)) - d[5] * (transform[0, 2] * c1 + transform[1, 2] * s1) + transform[0, 3] * c1 + transform[1, 3] * s1;
double p13y = transform[2, 3] - d[0] - d[5] * transform[2, 2] + d[4] * (transform[2, 1] * c6 + transform[2, 0] * s6);
double c3 = (p13x * p13x + p13y * p13y - a[1] * a[1] - a[2] * a[2]) / (2.0 * a[1] * a[2]);
double arccos = Acos(c3);
if (double.IsNaN(arccos))
{
arccos = 0;
isUnreachable = true;
}
if (!elbow)
{
joints[2] = arccos;
}
else
{
joints[2] = 2.0 * PI - arccos;
}
double denom = a[1] * a[1] + a[2] * a[2] + 2 * a[1] * a[2] * c3;
double s3 = Sin(arccos);
double A = (a[1] + a[2] * c3);
double B = a[2] * s3;
if (!elbow)
{
joints[1] = Atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom);
}
else
{
joints[1] = Atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom);
}
double c23_0 = Cos(joints[1] + joints[2]);
double s23_0 = Sin(joints[1] + joints[2]);
joints[3] = Atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0);
}
if (isUnreachable)
{
errors.Add($"Target out of reach.");
}
// joints[5] += PI / 2;
for (int i = 0; i < 6; i++)
{
if (joints[i] > PI)
{
joints[i] -= 2.0 * PI;
}
if (joints[i] < -PI)
{
joints[i] += 2.0 * PI;
}
}
return(joints);
}
/// <summary>
/// Inverse kinematics for a spherical wrist 6 axis robot.
/// Code adapted from https://github.com/whitegreen/KinematikJava
/// </summary>
/// <param name="target">Cartesian target</param>
/// <returns>Returns the 6 rotation values in radians.</returns>
override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);
bool isUnreachable = false;
double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();
Plane flange = Plane.WorldXY;
flange.Transform(transform);
double[] joints = new double[6];
double l2 = Sqrt(a[2] * a[2] + d[3] * d[3]);
double ad2 = Atan2(a[2], d[3]);
Point3d center = flange.Origin - flange.Normal * d[5];
joints[0] = Atan2(center.Y, center.X);
double ll = Sqrt(center.X * center.X + center.Y * center.Y);
Point3d p1 = new Point3d(a[0] * center.X / ll, a[0] * center.Y / ll, d[0]);
if (shoulder)
{
joints[0] += PI;
var rotate = Transform.Rotation(PI, new Point3d(0, 0, 0));
center.Transform(rotate);
}
double l3 = (center - p1).Length;
double l1 = a[1];
double beta = Acos((l1 * l1 + l3 * l3 - l2 * l2) / (2 * l1 * l3));
if (double.IsNaN(beta))
{
beta = 0;
isUnreachable = true;
}
if (elbow)
{
beta *= -1;
}
double ttl = new Vector3d(center.X - p1.X, center.Y - p1.Y, 0).Length;
// if (p1.X * (center.X - p1.X) < 0)
if (shoulder)
{
ttl = -ttl;
}
double al = Atan2(center.Z - p1.Z, ttl);
joints[1] = beta + al;
double gama = Acos((l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2));
if (double.IsNaN(gama))
{
gama = PI;
isUnreachable = true;
}
if (elbow)
{
gama *= -1;
}
joints[2] = gama - ad2 - PI / 2;
double[] c = new double[3];
double[] s = new double[3];
for (int i = 0; i < 3; i++)
{
c[i] = Cos(joints[i]);
s[i] = Sin(joints[i]);
}
var arr = new Transform();
arr[0, 0] = c[0] * (c[1] * c[2] - s[1] * s[2]); arr[0, 1] = s[0]; arr[0, 2] = c[0] * (c[1] * s[2] + s[1] * c[2]); arr[0, 3] = c[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * c[0];
arr[1, 0] = s[0] * (c[1] * c[2] - s[1] * s[2]); arr[1, 1] = -c[0]; arr[1, 2] = s[0] * (c[1] * s[2] + s[1] * c[2]); arr[1, 3] = s[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * s[0];
arr[2, 0] = s[1] * c[2] + c[1] * s[2]; arr[2, 1] = 0; arr[2, 2] = s[1] * s[2] - c[1] * c[2]; arr[2, 3] = a[2] * (s[1] * c[2] + c[1] * s[2]) + a[1] * s[1] + d[0];
arr[3, 0] = 0; arr[3, 1] = 0; arr[3, 2] = 0; arr[3, 3] = 1;
arr.TryGetInverse(out var in123);
var mr = Transform.Multiply(in123, transform);
joints[3] = Atan2(mr[1, 2], mr[0, 2]);
joints[4] = Acos(mr[2, 2]);
joints[5] = Atan2(mr[2, 1], -mr[2, 0]);
if (wrist)
{
joints[3] += PI;
joints[4] *= -1;
joints[5] -= PI;
}
for (int i = 0; i < 6; i++)
{
if (joints[i] > PI)
{
joints[i] -= 2 * PI;
}
if (joints[i] < -PI)
{
joints[i] += 2 * PI;
}
}
if (isUnreachable)
{
errors.Add($"Target out of reach");
}
if (Abs(1 - mr[2, 2]) < 0.0001)
{
errors.Add($"Near wrist singularity");
}
if (new Vector3d(center.X, center.Y, 0).Length < a[0] + SingularityTol)
{
errors.Add($"Near overhead singularity");
}
for (int i = 0; i < 6; i++)
{
if (double.IsNaN(joints[i]))
{
joints[i] = 0;
}
}
return(joints);
}
protected abstract double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List <string> errors);
