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