public override InverseKinematicsResult Execute(Manipulator manipulator, Vector3 goal, int joint = -1)
{
// use gripper if default joint
if (joint == -1)
{
joint = manipulator.Joints.Length - 1;
}
VectorFloat configuration = manipulator.q, dq;
VectorFloat error;
// TODO: check for oscillations (the error starts increasing) and break if they appear
int iterations = 0;
while (ErrorExceedsThreshold(manipulator, configuration, goal, joint,
out ForwardKinematicsResult fkRes, out error) && iterations < _maxIterations)
{
errorMod.Add(error.L2Norm());
configs.Add(VectorFloat.Build.DenseOfVector(configuration));
// get generalized coordinates' offset
dq = GetCoordinateOffset(CreateJacobian(fkRes, joint), error);
// update configuration
configuration.AddSubVector(dq);
//for (int i = 0; i < manipulator.Joints.Length; i++)
//{
// manipulator.Joints[i].InitialCoordinate = configuration[i];
//}
iterations++;
}
return(new InverseKinematicsResult
{
Converged = iterations < _maxIterations && !JointLimitsExceeded(manipulator, configuration),
Iterations = iterations,
Configuration = configuration,
Error = error
});
}
