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 static void optimization_test() // given a matrix find the possible one
{
Optimizer optimizaer = new Optimizer();
Matrix3D T_target = new Matrix3D(0.0630, 0.0303, 0.9976, 129.7036,
-0.2570, 0.9663, -0.0131, -43.1047,
-0.9643, -0.2556, 0.0686, 13.1218,
0, 0, 0, 1.0000);
optimizaer.T_taget = T_target;
optimizaer.x = new double[4] {
-50.0282200688546, -75.3754525301847, 57.2766149563357, 162
};
Joints optimized_joints;
optimized_joints = optimizaer.minimize_error();
Needle needle = new Needle();
needle.kinematics.joint = optimized_joints;
needle.update_needle();
}
//based on head frame
public void update_needle(Matrix3D desired)
{
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();
}