double[] GetClosestSolution(Transform transform, double[] prevJoints, out 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, (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 = (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)
{
double[] values = cell.MechanicalGroups[group].RadiansToDegreesExternal(target);
var externals = new string[6];
for (int i = 0; i < 6; i++)
{
externals[i] = "9E9";
}
if (target.ExternalCustom == null)
{
for (int i = 0; i < target.External.Length; i++)
{
externals[i] = $"{values[i]:0.####}";
}
}
else
{
for (int i = 0; i < target.External.Length; i++)
{
string e = target.ExternalCustom[i];
if (!string.IsNullOrEmpty(e))
{
externals[i] = e;
}
}
}
external = $"[{string.Join(",", externals)}]";
}
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.####},{joints[1]:0.####},{joints[2]:0.####},{joints[3]:0.####},{joints[4]:0.####},{joints[5]:0.####}],{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);
Quaternion quaternion = GetRotation(plane);
switch (cartesian.Motion)
{
case Motions.Joint:
{
string pos = $"[{plane.OriginX:0.###},{plane.OriginY:0.###},{plane.OriginZ:0.###}]";
string orient = $"[{quaternion.A:0.#####},{quaternion.B:0.#####},{quaternion.C:0.#####},{quaternion.D:0.#####}]";
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--;
}
RobotConfigurations configuration = programTarget.Kinematics.Configuration;
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = configuration.HasFlag(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 Motions.Linear:
{
string pos = $"[{plane.OriginX:0.###},{plane.OriginY:0.###},{plane.OriginZ:0.###}]";
string orient = $"[{quaternion.A:0.#####},{quaternion.B:0.#####},{quaternion.C:0.#####},{quaternion.D:0.#####}]";
string robtarget = $"[{pos},{orient},conf,{external}]";
moveText = $@"MoveL {robtarget}{id},{target.Speed.Name},{zone},{target.Tool.Name} \WObj:={target.Frame.Name};";
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));
}
}
if (!multiProgram)
{
if (cell.MechanicalGroups.Count > 1)
{
code.Add($"SyncMoveOff sync2;");
}
}
code.Add("ENDPROC");
code.Add("ENDMODULE");
return(code);
}
public CartesianTarget(Plane plane, Target target, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, IEnumerable<double> external = null) : this(plane, configuration, motion, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
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;
}
/// <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, RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(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, RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(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, RobotConfigurations configuration, out List <string> errors);
/// <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>
protected override double[] InverseKinematics(Transform transform, RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(RobotConfigurations.Wrist);
if (shoulder)
{
wrist = !wrist;
}
double[] joints = new double[6];
bool isUnreachable = false;
transform *= Transform.Rotation(HalfPI, Point3d.Origin);
var a = Vector6d.Map(_mechanism.Joints, j => j.A);
var d = Vector6d.Map(_mechanism.Joints, j => j.D);
// shoulder
{
double A = d[5] * transform.M12 - transform.M13;
double B = d[5] * transform.M02 - transform.M03;
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);
joints[0] = !shoulder ? arccos + arctan : -arccos + arctan;
}
// wrist 2
{
double numer = (transform.M03 * Sin(joints[0]) - transform.M13 * 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;
}
joints[4] = !wrist ? arccos : 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.M01 * s1 - transform.M11 * c1), Sign(s5) * (transform.M00 * s1 - transform.M10 * c1));
double c6 = Cos(joints[5]), s6 = Sin(joints[5]);
double x04x = -s5 * (transform.M02 * c1 + transform.M12 * s1) - c5 * (s6 * (transform.M01 * c1 + transform.M11 * s1) - c6 * (transform.M00 * c1 + transform.M10 * s1));
double x04y = c5 * (transform.M20 * c6 - transform.M21 * s6) - transform.M22 * s5;
double p13x = d[4] * (s6 * (transform.M00 * c1 + transform.M10 * s1) + c6 * (transform.M01 * c1 + transform.M11 * s1)) - d[5] * (transform.M02 * c1 + transform.M12 * s1) + transform.M03 * c1 + transform.M13 * s1;
double p13y = transform.M23 - d[0] - d[5] * transform.M22 + d[4] * (transform.M21 * c6 + transform.M20 * 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;
}
joints[2] = !elbow ? arccos : PI2 - 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;
joints[1] = !elbow
? Atan2((A *p13y - B *p13x) / denom, (A *p13x + B *p13y) / denom)
: 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] -= PI2;
}
if (joints[i] < -PI)
{
joints[i] += PI2;
}
}
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>
protected override double[] InverseKinematics(Transform transform, RobotConfigurations configuration, out List <string> errors)
{
errors = new List <string>();
bool shoulder = configuration.HasFlag(RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(RobotConfigurations.Elbow);
if (shoulder)
{
elbow = !elbow;
}
bool wrist = !configuration.HasFlag(RobotConfigurations.Wrist);
bool isUnreachable = false;
var a = Vector6d.Map(_mechanism.Joints, joint => joint.A);
var d = Vector6d.Map(_mechanism.Joints, joint => joint.D);
var flange = transform.ToPlane();
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);
var 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;
Vector3d c = default;
Vector3d s = default;
for (int i = 0; i < 3; i++)
{
c[i] = Cos(joints[i]);
s[i] = Sin(joints[i]);
}
Transform arr = default;
arr.Set(
c[0] * (c[1] * c[2] - s[1] * s[2]), s[0], c[0] * (c[1] * s[2] + s[1] * c[2]), c[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * c[0],
s[0] * (c[1] * c[2] - s[1] * s[2]), -c[0], s[0] * (c[1] * s[2] + s[1] * c[2]), s[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * s[0],
s[1] * c[2] + c[1] * s[2], 0, s[1] * s[2] - c[1] * c[2], a[2] * (s[1] * c[2] + c[1] * s[2]) + a[1] * s[1] + d[0]
);
arr.TryGetInverse(out var in123);
var mr = in123 * transform;
joints[3] = Atan2(mr.M12, mr.M02);
joints[4] = Acos(mr.M22);
joints[5] = Atan2(mr.M21, -mr.M20);
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.M22) < 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);
}
