public static void PrintJointOnFile(Joints J) { string path = System.IO.Directory.GetCurrentDirectory(); string OutputFileName = "dataJ.txt"; string fullPath = path + OutputFileName; if (!File.Exists(fullPath)) { // Create a file to write to. using (StreamWriter sw = File.CreateText(fullPath)) { sw.Write(OutputFileName); sw.Write("\t"); DateTime millisTime = DateTime.Now; sw.WriteLine(millisTime); } } using (StreamWriter sw = File.AppendText(fullPath)) { sw.Write("\t"); sw.Write(J.UpperBevel.ToString()); sw.Write("\t"); sw.Write(J.LowerBevel.ToString()); sw.Write("\t"); sw.Write(J.Elbow.ToString()); sw.Write("\t"); sw.WriteLine(J.twist.ToString()); } }
public Needle update_trajectory() { /* * //interpolation.M_initial = needle_entry.head; * //interpolation.M_target = needle_exit.head; * interpolation.initialize(needle_entry.head, needle_exit.head);// might not needed * Matrix3D head = interpolation.update(needle_mid.moved_head); //might not needed// head of next needle (mid needle) * needle_mid.update_needle(head); */ Vector3D center_mid = interpolation.interpolate_center(needle_entry, needle_exit); Matrix3D M_target = needle_mid.moved_head; M_target.M14 = center_mid.X; M_target.M24 = center_mid.Y; M_target.M34 = center_mid.Z; optimizer.T_taget = M_target; optimizer.x = new double[4] { needle_mid.kinematics.joint.UpperBevel, needle_mid.kinematics.joint.LowerBevel, needle_mid.kinematics.joint.Elbow, needle_mid.kinematics.joint.twist }; Joints optimized = optimizer.minimize_center(); needle_mid.kinematics.joint = optimized; needle_mid.update_needle(); //t = t + .1; // needed? return(needle_mid); }
public void SELECT_EXIT(Joints joints) { needle.kinematics.joint.UpperBevel = joints.UpperBevel; needle.kinematics.joint.LowerBevel = joints.LowerBevel; needle.kinematics.joint.Elbow = joints.Elbow; needle.kinematics.joint.twist = joints.twist; needle.update_needle(); trajectory.exit_point = NeedleKinematics.get_translation(needle.head);//mode.two_point_suturing trajectory.set_entry_needle(joints); trajectory.set_exit_needle(joints); state++; // delete late /* * Joints traj_joint = trajectory.update_trajectory(joints); * needle.kinematics.joint.UpperBevel = traj_joint.UpperBevel; * needle.kinematics.joint.LowerBevel = traj_joint.LowerBevel; * needle.kinematics.joint.Elbow = traj_joint.Elbow; * needle.kinematics.joint.twist = traj_joint.twist; * needle.update_needle(); * */ // }
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 PrintJointOnFile(Joints J, string fileName) { string path = System.IO.Directory.GetCurrentDirectory(); string OutputFileName = fileName;// "dataJ.txt"; string fullPath = path + OutputFileName; if (!File.Exists(fullPath)) { // Create a file to write to. using (StreamWriter sw = File.CreateText(fullPath)) { sw.Write(OutputFileName); sw.Write("\t"); DateTime millisTime = DateTime.Now; sw.WriteLine(millisTime); } } using (StreamWriter sw = File.AppendText(fullPath)) { sw.Write("\t"); sw.Write(J.UpperBevel.ToString()); sw.Write("\t"); sw.Write(J.LowerBevel.ToString()); sw.Write("\t"); sw.Write(J.Elbow.ToString()); sw.Write("\t"); sw.WriteLine(J.twist.ToString()); } }
public Joints update_trajectory(Joints joint) // needle center for the needle frame and a point { // not used double central_angle = get_central_angle(); Optimizer optimizer = new Optimizer(); Point4D target = new Point4D(entry_point.X, entry_point.Y, entry_point.Z, central_angle); optimizer.point_target = target; optimizer.x = new double[] { joint.UpperBevel, joint.LowerBevel, joint.Elbow, joint.twist }; Joints optimized = optimizer.minimize_angle(); return(optimized); //Vector3D projected_exit_point = project_point(exit_point, needle_normal, needle_point); //Matrix3D head = calculate_needle_tip(entry_point, projected_exit_point, needle_normal); }
public void update_needle(Matrix3D desired) //based on head frame { Optimizer optimizer = new Optimizer(); optimizer.T_taget = desired; optimizer.x = new double[4] { kinematics.joint.UpperBevel, kinematics.joint.LowerBevel, kinematics.joint.Elbow, kinematics.joint.twist }; Joints optimized_joints = optimizer.minimize_error(); kinematics.joint.UpperBevel = optimized_joints.UpperBevel; kinematics.joint.LowerBevel = optimized_joints.LowerBevel; kinematics.joint.Elbow = optimized_joints.Elbow; kinematics.joint.twist = optimized_joints.twist; update_needle(); }
public void set_exit_needle(Joints joint) { double central_angle = 180 - get_central_angle(); Optimizer optimizer = new Optimizer(); Point4D target = new Point4D(exit_point.X, exit_point.Y, exit_point.Z, central_angle); Console.Write("\noTarget exit: {0}, {1}, {2}, {3}", exit_point.X, exit_point.Y, exit_point.Z, central_angle); optimizer.point_target = target; optimizer.x = new double[] { joint.UpperBevel, joint.LowerBevel, joint.Elbow, joint.twist }; Joints optimized = optimizer.minimize_angle(); needle_exit.kinematics.joint.UpperBevel = optimized.UpperBevel; needle_exit.kinematics.joint.LowerBevel = optimized.LowerBevel; needle_exit.kinematics.joint.Elbow = optimized.Elbow; needle_exit.kinematics.joint.twist = optimized.twist; needle_exit.update_needle(); }
public void set_entry_needle(Joints joint) { double central_angle = get_central_angle(); Optimizer optimizer = new Optimizer(); Point4D target = new Point4D(entry_point.X, entry_point.Y, entry_point.Z, central_angle); Console.Write("\noTarget entry: {0}, {1}, {2}, {3}", entry_point.X, entry_point.Y, entry_point.Z, central_angle); optimizer.point_target = target; optimizer.x = new double[] { joint.UpperBevel, joint.LowerBevel, joint.Elbow, joint.twist }; Joints optimized = optimizer.minimize_angle(); needle_entry.kinematics.joint.UpperBevel = optimized.UpperBevel; needle_entry.kinematics.joint.LowerBevel = optimized.LowerBevel; needle_entry.kinematics.joint.Elbow = optimized.Elbow; needle_entry.kinematics.joint.twist = optimized.twist; needle_entry.update_needle(); // setup needle_mid needle_mid.kinematics.joint.UpperBevel = needle_entry.kinematics.joint.UpperBevel; needle_mid.kinematics.joint.LowerBevel = needle_entry.kinematics.joint.LowerBevel; needle_mid.kinematics.joint.Elbow = needle_entry.kinematics.joint.Elbow; needle_mid.kinematics.joint.twist = needle_entry.kinematics.joint.twist; needle_mid.update_needle(); }
public void SELECT_ENTRY(Joints joints) { needle.kinematics.joint.UpperBevel = joints.UpperBevel; //-60; //joints.UpperBevel; needle.kinematics.joint.LowerBevel = joints.LowerBevel; //-50;// joints.LowerBevel; needle.kinematics.joint.Elbow = joints.Elbow; //80;// joints.Elbow; needle.kinematics.joint.twist = joints.twist; //0;// joints.twist; needle.update_needle(); // one-point suturing Print.PrintJointOnFile(joints); // print joints at the entry time switch (mode) { case 1: // mode.one_point_suturing state++; state++; break; case 2: //mode.two_point_suturing trajectory.entry_point = NeedleKinematics.get_translation(needle.head); //trajectory.set_entry_needle(joints); state++; break; } }
// needle center for the needle frame and a point public Joints update_trajectory(Joints joint) { // not used double central_angle = get_central_angle(); Optimizer optimizer = new Optimizer(); Point4D target = new Point4D(entry_point.X, entry_point.Y, entry_point.Z, central_angle); optimizer.point_target = target; optimizer.x = new double[] { joint.UpperBevel, joint.LowerBevel, joint.Elbow, joint.twist }; Joints optimized = optimizer.minimize_angle(); return optimized; //Vector3D projected_exit_point = project_point(exit_point, needle_normal, needle_point); //Matrix3D head = calculate_needle_tip(entry_point, projected_exit_point, needle_normal); }
public void SELECT_ENTRY(Joints joints) { needle.kinematics.joint.UpperBevel = joints.UpperBevel; needle.kinematics.joint.LowerBevel = joints.LowerBevel; needle.kinematics.joint.Elbow = joints.Elbow; needle.kinematics.joint.twist = joints.twist; needle.update_needle(); // one-point suturing switch (mode) { case 1: // mode.one_point_suturing state++; state++; break; case 2: //mode.two_point_suturing trajectory.entry_point = NeedleKinematics.get_translation(needle.head); //trajectory.set_entry_needle(joints); state++; break; } }
public void SELECT_EXIT(Joints joints) { needle.kinematics.joint.UpperBevel = joints.UpperBevel; needle.kinematics.joint.LowerBevel = joints.LowerBevel; needle.kinematics.joint.Elbow = joints.Elbow; needle.kinematics.joint.twist = joints.twist; needle.update_needle(); trajectory.exit_point = NeedleKinematics.get_translation(needle.head);//mode.two_point_suturing trajectory.set_entry_needle(joints); trajectory.set_exit_needle(joints); state++; // delete late /* Joints traj_joint = trajectory.update_trajectory(joints); needle.kinematics.joint.UpperBevel = traj_joint.UpperBevel; needle.kinematics.joint.LowerBevel = traj_joint.LowerBevel; needle.kinematics.joint.Elbow = traj_joint.Elbow; needle.kinematics.joint.twist = traj_joint.twist; needle.update_needle(); * */ // }