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]); }
protected override void SetJoints(Target target, double[] prevJoints) { for (int i = 0; i < mechanism.Joints.Length; i++) { int externalNum = mechanism.Joints[i].Number - 6; if (target.External.Length - 1 < externalNum) { Errors.Add($"Positioner external axis not configured on this target."); } else { Joints[i] = target.External[externalNum]; } } if (prevJoints != null) { Joints = JointTarget.GetAbsoluteJoints(Joints, prevJoints); } }
protected override void SetJoints(Target target, double[] prevJoints) { if (target is JointTarget) { Joints = (target as JointTarget).Joints; } else if (target is CartesianTarget) { double[] joints = null; var cartesianTarget = target as CartesianTarget; Plane tcp = target.Tool.Tcp; tcp.Rotate(PI, Vector3d.ZAxis, Point3d.Origin); Plane targetPlane = cartesianTarget.Plane; targetPlane.Transform(target.Frame.Plane.ToTransform()); var transform = Transform.PlaneToPlane(Planes[0], Plane.WorldXY) * Transform.PlaneToPlane(tcp, targetPlane); List <string> errors; if (cartesianTarget.Configuration != null || prevJoints == null) { Configuration = cartesianTarget.Configuration ?? Target.RobotConfigurations.None; joints = InverseKinematics(transform, Configuration, out errors); } else { joints = GetClosestSolution(transform, prevJoints, out var configuration, out errors, out var difference); Configuration = configuration; } if (prevJoints != null) { Joints = JointTarget.GetAbsoluteJoints(joints, prevJoints); } else { Joints = joints; } Errors.AddRange(errors); } }
public override Plane CartesianLerp(Plane a, Plane b, double t, double min, double max) { t = (t - min) / (max - min); if (double.IsNaN(t)) { t = 0; } var anglesA = RobotCellKuka.PlaneToEuler(a); var anglesB = RobotCellKuka.PlaneToEuler(b); var result = new double[anglesA.Length]; for (int i = 0; i < anglesA.Length; i++) { if (i < 3) { result[i] = anglesA[i] * (1.0 - t) + anglesB[i] * t; } else { double angleA = anglesA[i].ToRadians(); double angleB = anglesB[i].ToRadians(); double delta = angleB - angleA; double absDelta = Abs(delta); if (absDelta > PI) { delta = Sign(delta) * (absDelta - PI * 2); } angleB = angleA + delta; double angle = angleA * (1.0 - t) + angleB * t; // if (i == 4) angle = Abs(angle); var results = JointTarget.GetAbsoluteJoints(new double[] { angle }, new double[] { angleB }); result[i] = results[0].ToDegrees(); } } return(RobotCellKuka.EulerToPlane(result[0], result[1], result[2], result[3], result[4], result[5])); // return base.CartesianLerp(a, b, t, min, max); }