public void Execute()
{
NelderMeadSimplex solver = new NelderMeadSimplex(jobConvergenceTolerance, jobMaxIterations);
var V = Vector <double> .Build;
var meJob = this;
var f1 = new Func <Vector <double>, double>(angleVec => RobotController.ErrorFunction(meJob.targetPos, meJob.targetRot, angleVec.ToArray(), meJob.jobAngleMins.ToArray(), meJob.jobAngleMaxes.ToArray(), meJob.jobBasePos, meJob.jobBaseRot, meJob.jobRotAxes.ToArray(), meJob.jobTransAxes.ToArray(), meJob.jobStartOffsets.ToArray()));
var obj = ObjectiveFunction.Value(f1);
MinimizationResult result;
try
{
result = solver.FindMinimum(obj, V.DenseOfArray(jobAngles.ToArray()));
for (int i = 0; i < 5; i++)
{
if (result.FunctionInfoAtMinimum.Value > jobDistanceThreshold)
{
f1 = new Func <Vector <double>, double>(angleVec => RobotController.ErrorFunction(meJob.targetPos, meJob.targetRot, angleVec.ToArray(), meJob.jobAngleMins.ToArray(), meJob.jobAngleMaxes.ToArray(), meJob.jobBasePos, meJob.jobBaseRot, meJob.jobRotAxes.ToArray(), meJob.jobTransAxes.ToArray(), meJob.jobStartOffsets.ToArray()));
obj = ObjectiveFunction.Value(f1);
MinimizationResult result2;
try
{
result2 = solver.FindMinimum(obj, V.DenseOfArray(jobAngles.ToArray()) + (V.Random(jobAngles.Length) * (i + 1) * (i + 1) * (i + 1)));
if (result2 != null && result2.FunctionInfoAtMinimum.Value < result.FunctionInfoAtMinimum.Value)
{
result = result2;
}
}
catch (MaximumIterationsException e)
{
Debug.Log(e);
}
}
else
{
break;
}
}
jobAngles.CopyFrom(result.MinimizingPoint.ToArray());
}
catch (MaximumIterationsException e)
{
Debug.Log(e);
}
}
