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 };
}
public Optimizer()
{
kinematics = new NeedleKinematics();
//x = new double[] { 0, 0, 0, 0 };
}
