protected Matrix <float> CreateJacobian(ForwardKinematicsResult fkRes, int joint = -1)
{
// use gripper if default joint
if (joint == -1)
{
joint = fkRes.JointPositions.Length - 1;
}
Vector3 jointPos = fkRes.JointPositions[joint];
float[][] data = new float[joint + 1][];
for (int i = 0; i <= joint; i++)
{
var elem = Vector3.Cross(fkRes.JointAxes[i], jointPos - fkRes.JointPositions[i]);
//if (elem != Vector3.Zero)
// elem = Vector3.Normalize(elem); // TODO: any use of normalization?
data[i] = new float[]
{
elem.X,
elem.Y,
elem.Z,
fkRes.JointAxes[i].X,
fkRes.JointAxes[i].Y,
fkRes.JointAxes[i].Z
};
}
return(Matrix <float> .Build.DenseOfColumnArrays(data));
}
protected bool ErrorExceedsThreshold(Manipulator manipulator, VectorFloat configuration, Vector3 goal, int joint,
out ForwardKinematicsResult fkRes, out VectorFloat error)
{
fkRes = manipulator.ForwardKinematics(configuration);
var errorPos = goal - fkRes.JointPositions[joint];
// TODO: integrate orientation goal somehow?
error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 });
return(errorPos.Length() > _threshold);
}
