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 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); }