private void function_fvec(double[] x, double[] fi, object obj) // to reach a point with closest position and orientation
{
// updating joints
kinematics.joint.UpperBevel = x[0];
kinematics.joint.LowerBevel = x[1];
kinematics.joint.Elbow = x[2];
kinematics.joint.twist = x[3];
// calculating the forward kinematics for the given joints value
T_FK = kinematics.transformation_matrix(55);
// calculating errors fi s
fi[0] = Math.Pow(T_taget.M11 - T_FK.M11, 2);
fi[1] = Math.Pow(T_taget.M12 - T_FK.M12, 2);
fi[2] = Math.Pow(T_taget.M13 - T_FK.M13, 2);
fi[3] = Math.Pow(T_taget.M14 - T_FK.M14, 2);
fi[4] = Math.Pow(T_taget.M21 - T_FK.M21, 2);
fi[5] = Math.Pow(T_taget.M22 - T_FK.M22, 2);
fi[6] = Math.Pow(T_taget.M23 - T_FK.M23, 2);
fi[7] = Math.Pow(T_taget.M24 - T_FK.M24, 2);
fi[8] = Math.Pow(T_taget.M31 - T_FK.M31, 2);
fi[9] = Math.Pow(T_taget.M32 - T_FK.M32, 2);
fi[10] = Math.Pow(T_taget.M33 - T_FK.M33, 2);
fi[11] = Math.Pow(T_taget.M34 - T_FK.M34, 2);
}
public void update_needle() // joints of kinematic must be upadated beforehand
{
// update the center, head, tail and moved head
this.center = kinematics.transformation_matrix(5);
this.head = Matrix3D.Multiply(center, this.head0);
this.moved_head = Matrix3D.Multiply(center, this.moved_head0);
this.tail = Matrix3D.Multiply(center, this.tail0);
// update the points
for (int i = 0; i < n; i++)
{
real_half[i] = NeedleKinematics.transform(center, this.real_half[i]);
imag_half[i] = NeedleKinematics.transform(center, this.imag_half[i]);
}
}
