public static void specified_pos(Needle needle)
{
/*
Needle needle = new Needle(); // for sake of have a valid head position and initial joint values for the following optimizer
needle.kinematics.joint.UpperBevel = 60;
needle.kinematics.joint.LowerBevel = 60;
needle.kinematics.joint.Elbow = 150; // no need for twist, twist will be given in loop
needle.kinematics.joint.twist=0;
needle.update_needle();
*/
Optimizer optimizer = new Optimizer();
optimizer.point_target.X = NeedleKinematics.get_translation(needle.head).X;
optimizer.point_target.Y = NeedleKinematics.get_translation(needle.head).Y;
optimizer.point_target.Z = NeedleKinematics.get_translation(needle.head).Z;
optimizer.x = new double[3] { needle.kinematics.joint.UpperBevel, needle.kinematics.joint.LowerBevel, needle.kinematics.joint.Elbow };
int n = 20;
for (int i = 0; i < n; i++)
{
optimizer.point_target.W = i * 360 / n;
Joints optimized_joints = optimizer.minimize_postionNtwist();
needle.kinematics.joint=optimized_joints;
needle.update_needle();
Print.PrintJointOnFile(optimized_joints);
Print.PrintMatrixOnFile(needle.center);
optimizer.x = new double[3] { optimized_joints.UpperBevel, optimized_joints.LowerBevel, optimized_joints.Elbow };
}
}
public static void specified_pos(Needle needle)
{
/*
* Needle needle = new Needle(); // for sake of have a valid head position and initial joint values for the following optimizer
* needle.kinematics.joint.UpperBevel = 60;
* needle.kinematics.joint.LowerBevel = 60;
* needle.kinematics.joint.Elbow = 150; // no need for twist, twist will be given in loop
* needle.kinematics.joint.twist=0;
* needle.update_needle();
*/
Optimizer optimizer = new Optimizer();
optimizer.point_target.X = NeedleKinematics.get_translation(needle.head).X;
optimizer.point_target.Y = NeedleKinematics.get_translation(needle.head).Y;
optimizer.point_target.Z = NeedleKinematics.get_translation(needle.head).Z;
optimizer.x = new double[3] {
needle.kinematics.joint.UpperBevel, needle.kinematics.joint.LowerBevel, needle.kinematics.joint.Elbow
};
int n = 20;
for (int i = 0; i < n; i++)
{
optimizer.point_target.W = i * 360 / n;
Joints optimized_joints = optimizer.minimize_postionNtwist();
needle.kinematics.joint = optimized_joints;
needle.update_needle();
Print.PrintJointOnFile(optimized_joints);
Print.PrintMatrixOnFile(needle.center);
optimizer.x = new double[3] {
optimized_joints.UpperBevel, optimized_joints.LowerBevel, optimized_joints.Elbow
};
}
}
public bool DO_SUTURING()
{
switch(mode)
{
case 1: // mode.one_point_suturing
needle.update_needle(needle.moved_head);
Print.PrintMatrixOnFile(needle.center);
break;
case 2: //mode.two_point_suturing
//needle = trajectory.needle_entry; //for test
needle = trajectory.update_trajectory();
Print.PrintMatrixOnFile(needle.center);
break;
}
t = t + Math.PI / (needle.n - 1);
// for test and can be deleted
//Test.specified_pos(needle);
//
if (t >= 1 * Math.PI)
{
END_SUTURING();
Console.Write("\nAutomatically ended\n");
return true;
}
return false;
}
public void START_SUTURING()
{
Console.Write("\nSuturing satrted.\n");
needle = new Needle();
trajectory = new Trajectory(); // mode 2
t = 0;
state = 1; // state initialization: state 1 indicates entry, state 2 exit and state 3 the suturing
}
public Trajectory()
{
t = 0;
head = new Matrix3D();
needle_entry = new Needle();
needle_exit = new Needle();
needle_mid = new Needle();
interpolation = new Interpolation(); // might be deleted
optimizer = new Optimizer();
}
public Trajectory()
{
t = 0;
head = new Matrix3D();
needle_entry = new Needle();
needle_exit = new Needle();
needle_mid = new Needle();
interpolation = new Interpolation(); // might be deleted
optimizer = new Optimizer();
}
public System.Windows.Media.Media3D.Vector3D interpolate_center(Needle needle_entry, Needle needle_exit)
{
Vector3 v1 = new Vector3(needle_entry.center.M14, needle_entry.center.M24, needle_entry.center.M34);
Vector3 v2 = new Vector3(needle_exit.center.M14, needle_exit.center.M24, needle_exit.center.M34);
Vector3 v_mid = Vector3.Lerp(v1, v2, t);
t = t + .1;
if (t > 1)
t = 1;
System.Windows.Media.Media3D.Vector3D output = new System.Windows.Media.Media3D.Vector3D(v_mid.X, v_mid.Y, v_mid.Z);
return output;
}
public System.Windows.Media.Media3D.Vector3D interpolate_center(Needle needle_entry, Needle needle_exit)
{
Vector3 v1 = new Vector3(needle_entry.center.M14, needle_entry.center.M24, needle_entry.center.M34);
Vector3 v2 = new Vector3(needle_exit.center.M14, needle_exit.center.M24, needle_exit.center.M34);
Vector3 v_mid = Vector3.Lerp(v1, v2, t);
t = t + .1;
if (t > 1)
{
t = 1;
}
System.Windows.Media.Media3D.Vector3D output = new System.Windows.Media.Media3D.Vector3D(v_mid.X, v_mid.Y, v_mid.Z);
return(output);
}
public static void score_accuracy(Needle needle_old, Needle needle_new)
{
if (needle_old != null && needle_new != null)
{
Vector3D[] points_old = needle_old.real_half;
Vector3D[] points_new = needle_new.real_half;
int inx = find_closest(points_old, points_new[0]);
int n = needle_old.real_half.Length;
double score = 0;
for (int i = inx; i < n; i++)
{
score = score + Math.Pow((points_old[i] - points_new[i - inx]).Length, 2);
}
score = Math.Sqrt(score);
}
}
public static void optimization_test() // given a matrix find the possible one
{
Optimizer optimizaer = new Optimizer();
Matrix3D T_target = new Matrix3D(0.0630, 0.0303, 0.9976, 129.7036,
-0.2570, 0.9663, -0.0131, -43.1047,
-0.9643, -0.2556, 0.0686, 13.1218,
0, 0, 0, 1.0000);
optimizaer.T_taget = T_target;
optimizaer.x = new double[4] {
-50.0282200688546, -75.3754525301847, 57.2766149563357, 162
};
Joints optimized_joints;
optimized_joints = optimizaer.minimize_error();
Needle needle = new Needle();
needle.kinematics.joint = optimized_joints;
needle.update_needle();
}
public bool DO_SUTURING()
{
needle_old = needle_new;
switch (mode)
{
case 1: // mode.one_point_suturing
needle.update_needle(needle.moved_head);
//Print.PrintMatrixOnFile(needle.center);
Print.PrintMatrixOnFile(needle.center, "_center.txt");
Print.PrintMatrixOnFile(needle.head, "_head.txt");
Print.PrintMatrixOnFile(needle.tail, "_tail.txt");
Print.PrintJointOnFile(needle.kinematics.joint, "_jointsforNeedles.txt");
break;
case 2: //mode.two_point_suturing
//needle = trajectory.needle_entry; //for test
needle = trajectory.update_trajectory();
Print.PrintMatrixOnFile(needle.center);
break;
}
needle_new = needle;
t = t + Math.PI / (needle.n - 1);
// for test and can be deleted
//Test.specified_pos(needle);
//Test.optimization_test();
//Test.score_accuracy(needle_old, needle_new);
//
if (t >= 1 * Math.PI)
{
END_SUTURING();
Console.Write("\nAutomatically ended\n");
return(true);
}
return(false);
}
public void START_SUTURING()
{
Console.Write("\nSuturing satrted.\n");
needle = new Needle();
trajectory = new Trajectory(); // mode 2
t = 0;
state = 1; // state initialization: state 1 indicates entry, state 2 exit and state 3 the suturing
}