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]);
}
public JointTarget GetPosition()
{
if (!Connected)
{
Connect(); // return Target.Default as JointTarget;
}
var joints = _controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
var values = new double[] { joints.RobAx.Rax_1, joints.RobAx.Rax_2, joints.RobAx.Rax_3, joints.RobAx.Rax_4, joints.RobAx.Rax_5, joints.RobAx.Rax_6 };
for (int i = 0; i < 6; i++)
{
values[i] = RobotAbb.ABBDegreeToRadian(values[i], i);
}
var target = new JointTarget(values);
return(target);
}
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);
}
public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double start, double end)
{
double[] allJoints = JointTarget.Lerp(prevTarget.Kinematics.Joints, Kinematics.Joints, t, start, end);
var external = allJoints.RangeSubset(6, Target.External.Length);
if (IsJointMotion)
{
var joints = allJoints.RangeSubset(0, 6);
return(new JointTarget(joints, Target, external));
}
else
{
Plane prevPlane = GetPrevPlane(prevTarget);
Plane plane = robot.CartesianLerp(prevPlane, Plane, t, start, end);
// Plane plane = CartesianTarget.Lerp(prevTarget.WorldPlane, this.WorldPlane, t, start, end);
// Target.RobotConfigurations? configuration = (Abs(prevTarget.cellTarget.TotalTime - t) < TimeTol) ? prevTarget.Kinematics.Configuration : this.Kinematics.Configuration;
var target = new CartesianTarget(plane, Target, prevTarget.Kinematics.Configuration, Motions.Linear, external);
// target.Frame = Frame.Default;
return(target);
}
}
static Target()
{
Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 });
}
static Target()
{
Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 }, Tool.Default, Speed.Default, Zone.Default, Command.Default, Frame.Default, null);
}
