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();
* */
//
}
