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 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(); * */ // }
void get_p1() { Vector3D u0 = NeedleKinematics.get_ez(M_initial); //u0 orientation of needle Vector3D u2 = NeedleKinematics.get_ez(M_target); // might be inverse Vector3D p1 = intersect(u0, p0, u2, p2); this.p1 = p1; }
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]); } }
public Needle() { kinematics = new NeedleKinematics(); // initializing the center, head and tail center = new Matrix3D(); center.SetIdentity(); head0 = new Matrix3D(); head0.SetIdentity(); head0.M14 = -radius; tail0 = new Matrix3D(); tail0.SetIdentity(); tail0.M14 = radius; real_half = new Vector3D[n]; imag_half = new Vector3D[n]; // initializing points for (int i = 0; i < n; i++) { real_half[i].X = radius * Math.Cos((double)i / (n - 1) * Math.PI); real_half[i].Y = -radius *Math.Sin((double)i / (n - 1) *Math.PI); real_half[i].Z = 0; imag_half[i].X = -radius *Math.Cos((double)i / (n - 1) *Math.PI); imag_half[i].Y = radius * Math.Sin((double)i / (n - 1) * Math.PI); imag_half[i].Z = 0; } // initializing the moved head double angle = -Math.PI / (n - 1); Matrix3D rotZ = new Matrix3D(Math.Cos(angle), -Math.Sin(angle), 0, 0, Math.Sin(angle), Math.Cos(angle), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); moved_head0 = Matrix3D.Multiply(rotZ, head0); }
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; } }
public Needle() { kinematics = new NeedleKinematics(); // initializing the center, head and tail center = new Matrix3D(); center.SetIdentity(); head0 = new Matrix3D(); head0.SetIdentity(); head0.M14 = -radius; tail0 = new Matrix3D(); tail0.SetIdentity(); tail0.M14 = radius; real_half = new Vector3D[n]; imag_half = new Vector3D[n]; // initializing points for (int i = 0; i < n; i++) { real_half[i].X = radius * Math.Cos((double)i / (n - 1) * Math.PI); real_half[i].Y = -radius * Math.Sin((double)i / (n - 1) * Math.PI); real_half[i].Z = 0; imag_half[i].X = -radius * Math.Cos((double)i / (n - 1) * Math.PI); imag_half[i].Y = radius * Math.Sin((double)i / (n - 1) * Math.PI); imag_half[i].Z = 0; } // initializing the moved head double angle = -Math.PI / (n - 1); Matrix3D rotZ = new Matrix3D(Math.Cos(angle), -Math.Sin(angle), 0, 0, Math.Sin(angle), Math.Cos(angle), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); moved_head0 = Matrix3D.Multiply(rotZ, head0); }
void get_p2() { this.p2 = NeedleKinematics.get_translation(M_target); }
void get_p0() { this.p0 = NeedleKinematics.get_translation(M_initial); }
public Optimizer() { kinematics = new NeedleKinematics(); //x = new double[] { 0, 0, 0, 0 }; }