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