def __init__(self, robot_filename, serial_num, rotation, radius): """ :param robot_filename: {string} -- the location of the Klampt's robot files :param serial_num: {string} -- the serial number of the calibration :param rotation: {int} -- the rotation angle of the needle :param radius: {float} -- the radius of the needle :return: None """ self.world_file = robot_filename + "/worlds/flatworld.xml" self.robot_file = robot_filename + "/robots/irb120_icp.rob" self.world = get_world(self.world_file, self.robot_file, visualize_robot=True) self.robot = self.world.robot(0) self.suture_center = np.array(pd.read_csv("../../data/suture_experiment/suture_center_files/" + serial_num + "_suture_center.csv", header=None)) print(self.suture_center) self.calibration_res = np.load("../../data/suture_experiment/calibration_result_files/" + serial_num + "/calibration_result.npy") self.Toct = (so3.from_rpy(self.calibration_res[0:3]), self.calibration_res[3:6]) self.Tneedle = (so3.from_rpy(self.calibration_res[6:9]), self.calibration_res[9:12]) self.rotation = rotation self.radius = radius self.dim_arr = np.array(pd.read_csv("../../data/oct_volume_calibration/" + serial_num + "/dimension.csv", header=None)) seed_config = np.multiply([-92.1441, 60.1883, 22.6847, 1.83303, -82.2083, 59.7906], np.pi / 180.).tolist() self.robot.setConfig([0.] * 7 + seed_config + [0.]) self.robot.link('tool0').setParentTransform(self.Tneedle[0], self.Tneedle[1])
def opt_error_fun(est_input, *args): """ :param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle :param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix :return: error: {float} -- the error between the true transformation and estimated transformation """ Roct = so3.from_rpy(est_input[0:3]) toct = est_input[3:6] Rn = so3.from_rpy(est_input[6:9]) tn = est_input[9:12] Tlink_set = args[0] Tneedle2oct_icp = args[1] fun_list = np.array([]) for i in range(len(Tlink_set)): fun_list = np.append( fun_list, np.multiply( so3.error(so3.inv(Tneedle2oct_icp[i][0]), so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]), Rn)), 1)) fun_list = np.append( fun_list, np.multiply( vectorops.sub( vectorops.sub([0., 0., 0.], so3.apply(so3.inv(Tneedle2oct_icp[i][0]), Tneedle2oct_icp[i][1])), so3.apply( so3.inv(Roct), vectorops.add(vectorops.sub(Tlink_set[i][1], toct), so3.apply(Tlink_set[i][0], tn)))), 1000)) return fun_list
def opt_error_fun_penalize(est_input, *args): """ :param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle :param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix :return: error: {float} -- the error between the true transformation and estimated transformation """ est_Toct = (so3.from_rpy(est_input[0:3]), est_input[3:6]) est_Tneedle = (so3.from_rpy(est_input[6:9]), est_input[9:12]) Tlink_set = args[0] Tneedle2oct_icp = args[1] fun_list = np.array([]) for i in range(len(Tlink_set)): Toct2needle_est = se3.mul(se3.mul(se3.inv(est_Toct), Tlink_set[i]), est_Tneedle) fun_list = np.append( fun_list, np.absolute( np.multiply( se3.error(Tneedle2oct_icp[i], se3.inv(Toct2needle_est))[0:3], [1, args[2], 1]))) fun_list = np.append( fun_list, np.absolute( np.multiply( se3.error(Tneedle2oct_icp[i], se3.inv(Toct2needle_est))[3:6], 1000))) return fun_list
def error_analysis(self): """ :return: None """ Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6]) Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12]) trans_error_list = [] trans_error_mat = [] rot_error_list = [] rot_error_mat = [] redraw_list = [] for i in range(len(self.Tlink_set)): Toct_needle_est = se3.mul( se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle) trans_error = vectorops.sub( se3.inv(self.trans_list[i])[1], Toct_needle_est[1]) rot_error = so3.error( se3.inv(self.trans_list[i])[0], Toct_needle_est[0]) trans_error_list.append(vectorops.normSquared(trans_error)) trans_error_mat.append(np.absolute(trans_error)) rot_error_list.append(vectorops.normSquared(rot_error)) rot_error_mat.append(np.absolute(rot_error)) redraw_list.append( redraw_pcd(self.pcd_list[i], Toct_needle_est)[0]) redraw_list.append( redraw_pcd(self.pcd_list[i], Toct_needle_est)[1]) redraw_list.append( create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0])) draw_geometries(redraw_list) trans_error_list = np.array(trans_error_list) trans_error_mat = np.array(trans_error_mat) rot_error_list = np.array(rot_error_list) rot_error_mat = np.array(rot_error_mat) rmse_trans = np.sqrt(np.mean(trans_error_list)) rmse_rot = np.sqrt(np.mean(rot_error_list)) print("The squared translation error list is:\n ", np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans) print("The mean error in XYZ directions is: ", np.mean(trans_error_mat, axis=0)) print("The squared rotation error list is:\n ", np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot) print("The mean error in three rotation vectors is: ", np.mean(rot_error_mat, axis=0)) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list), # np.sqrt(rot_error_list)]), allow_pickle=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0), # np.mean(rot_error_mat, axis=0)]), allow_pickle=True) trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def draw_part_circle_3d(radius, center, start_Z, rot_angle, start_angle, rot_x, pix_x, pix_h, pix_z): """ :param radius: {float} -- the radius of suture path :param center: {list} -- the suture center in the OCT frame :param start_Z: {float} -- the suture path Z axis value in the OCT frame :param rot_angle: {float} -- the rotation angle of the suture path :param start_angle: {float} -- the start suture angle :param rot_x: {float} -- the suture path rotation on the OCT frame X axis :param pix_x: {float} -- the length in pixels x length :param pix_h: {float} -- the length in pixels y length :param pix_z: {float} -- the length in pixels z length :return: suture_path: {list} -- the suture milestones in the world frame """ suture_path = [] rot_mat = (so3.from_rpy([rot_x, 0.0, 0.0]), [0.0] * 3) for i in range(0, rot_angle * 10): current_angle = start_angle + i * 0.1 pos_x_0 = center[0] - radius * np.cos((current_angle * np.pi) / 180.) pos_y_0 = center[1] + radius * np.sin((current_angle * np.pi) / 180.) milestone = np.divide(se3.apply(rot_mat, [pos_x_0, pos_y_0, start_Z]), [pix_x, pix_h, pix_z]) suture_path.append(milestone) return suture_path
def cal_est_config_list(robot, serial_num): est_config_list = [] local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]] Ticp_list = np.load("../../data/suture_experiment/standard_trans_list/trans_array.npy") est_calibration = np.load("../../data/suture_experiment/calibration_result_files/" + serial_num + "B/calibration_result.npy") Toct = (so3.from_rpy(est_calibration[0:3]), est_calibration[3:6]) Tneedle = (so3.from_rpy(est_calibration[6:9]), est_calibration[9:12]) for i in range(0, 9): Tee = se3.mul(se3.mul(Toct, se3.inv(Ticp_list[i])), se3.inv(Tneedle)) world_pos = [se3.apply(Tee, local_pos[0]), se3.apply(Tee, local_pos[1]), se3.apply(Tee, local_pos[2])] q = solve_ik(robot.link('link_6'), local_pos, world_pos, i) est_config_list.append(q) return est_config_list
def _do_rotation_change(index, element): rpy[index] = element value[0][:] = so3.from_rpy(rpy) if klampt_widget: klampt_widget.setTransform(name=xform_name, R=value[0], t=value[1]) if callback: callback(value)
def dalk_path_generation(path_filename, calib_filename): data = np.loadtxt(path_filename) oct2robot = np.loadtxt(calib_filename)[0, :].reshape((4, 4)) robot2oct = np.linalg.inv(oct2robot) robot_target = [] xyzrpy = [] # dalk_frame = (so3.from_rpy([0, 0, -np.pi/2]), [52.5 * 0.0254, 6.5 * 0.0251, 0.75 * 0.0254]) dalk_frame = (so3.from_rpy([0, np.pi, 0]), [0.0, 0.0, 0.0]) for row in data[::1]: x_target = row[12 + 6:12 + 6 + 6] # xyzrpy.append(x_target) T = np.eye(4) T = rotation_matrix(x_target[3], [1, 0, 0]).dot( rotation_matrix(x_target[4], [0, 1, 0])).dot( rotation_matrix(x_target[5], [0, 0, 1])) T[:3, 3] = x_target[0:3] T = robot2oct.dot(T) T[:3, 3] = T[:3, 3] * 1 T = oct2robot.dot(T) robot_target.append(T) T2rpyxyz = list(se3.from_homogeneous(T)[1]) + list( so3.rpy(se3.from_homogeneous(T)[0])) print(T2rpyxyz) xyzrpy.append(T2rpyxyz) # T_m = se3.mul(se3.inv(dalk_frame), se3.from_homogeneous(T)) # T_h = se3.homogeneous(T_m) # T2rpyxyz = list(T_m[1]) + list(so3.rpy(T_m[0])) # xyzrpy.append(T2rpyxyz) # robot_target.append(T_h) return robot_target, xyzrpy
def solve_placement(self, goal_bounds): """Implemented for you: come up with a collision-free target placement""" xmin, ymin, zmin = goal_bounds[0] xmax, ymax, zmax = goal_bounds[1] #center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)] center_sampling_range = [(xmin, xmax), (ymin, ymax), (zmin, zmax)] Tobj_feasible = [] original_obj_tarnsform = self.object.getTransform() for iters in range(20): crand = [random.uniform(b[0], b[1]) for b in center_sampling_range] if self.robot0: Robj = so3.from_rpy((0, np.pi / 2, 0)) else: Robj = so3.identity() tobj = [crand[0], crand[1], crand[2]] self.object.setTransform(Robj, tobj) if not self.check_feasible(): continue Tobj_feasible.append((Robj, tobj)) print("Found", len(Tobj_feasible), "valid object placements") self.object.setTransform(original_obj_tarnsform[0], original_obj_tarnsform[1]) print("Tobj_feasible", Tobj_feasible) return Tobj_feasible
def _do_rotation_change(index,element): rpy[index] = element value[0][:] = so3.from_rpy(rpy) if klampt_widget: klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1]) if callback: callback(value)
def addTable(self, x=0, y=0): """ Add a table to the world Parameters: -------------- x,y: floats, the x,y position of the center of the table """ table_top = Geometry3D() table_leg_1 = Geometry3D() table_leg_2 = Geometry3D() table_leg_3 = Geometry3D() table_leg_4 = Geometry3D() table_top.loadFile(model_path + "cube.off") table_leg_1.loadFile(model_path + "cube.off") table_leg_2.loadFile(model_path + "cube.off") table_leg_3.loadFile(model_path + "cube.off") table_leg_4.loadFile(model_path + "cube.off") table_top.transform([self.table_length,0,0,0,self.table_width,0,0,0,\ self.table_top_thickness],[0,0,self.table_height - self.table_top_thickness]) table_leg_1.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height\ - self.table_top_thickness],[0,0,0]) table_leg_2.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height - \ self.table_top_thickness],[self.table_length-self.leg_width,0,0]) table_leg_3.transform([ self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0, self.table_height - self.table_top_thickness ], [ self.table_length - self.leg_width, self.table_width - self.leg_width, 0 ]) table_leg_4.transform([ self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0, self.table_height - self.table_top_thickness ], [0, self.table_width - self.leg_width, 0]) table_geom = Geometry3D() table_geom.setGroup() for i, elem in enumerate( [table_top, table_leg_1, table_leg_2, table_leg_3, table_leg_4]): g = Geometry3D(elem) table_geom.setElement(i, g) #table_geom.transform([1,0,0,0,1,0,0,0,1],[x-self.table_length/2.0,y-self.table_width/2.0,0]) table_geom.transform( so3.from_rpy([0, 0, math.pi / 2]), [x - self.table_length / 2.0, y - self.table_width / 2.0, 0]) table = self.w.makeTerrain("table") table.geometry().set(table_geom) table.appearance().setColor(self.light_blue[0], self.light_blue[1], self.light_blue[2], self.light_blue[3])
def test_from_rpy(self): R = so3.from_rpy((0, 0, 0))
builder.addRobot("./Anthrax.urdf", None) w = builder.getWorld() robot = w.robot(0) reset_arms(robot) sim = klampt.Simulator(w) # setup simulated camera cam = klampt.SimRobotSensor(sim.controller(0), "rgbd_camera", "CameraSensor") # mount camera in place cam.setSetting("link", "4") #cam.setSetting("Tsensor","0.0 -0.866 -0.5 1.0 0.0 0.0 0 -0.5 0.866 0.2 0.005 1.2") cam.setSetting( "Tsensor", transform_to_string(so3.from_rpy((math.pi, radians(-30), radians(270))), [0.2, 0.005, 1.2])) # minimum range cam.setSetting("zmin", "0.1") # x field of view cam.setSetting("xfov", "1.5") # add to visualization vis.add("world", w) vis.add("cam", cam) # Take a picture! cam.kinematicSimulate(w, 0.01) rgb, depth = sensing.camera_to_images(cam) #plt.imshow(rgb) #plt.show()
def solve_qplace(self, Tplacement): if self.robot0: v = list(np.array(Tplacement[1]) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=Tplacement[0], t=v) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('qplace') return None qpreplace = self.robot.getConfig() # add a waypoint to insert swab into human face face_trans = [0.34, -0.3, 1.05] # [0.5, -0.35, 1.35] objective = ik.objective(self.robot.link(9), R=so3.from_rpy((np.pi, -np.pi / 2, 0)), t=face_trans) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab near mouth') return None self.qmouth = self.robot.getConfig() self.qmouth = self.gripper.set_finger_config( self.qmouth, self.gripper.partway_open_config(self.close_amount)) face_trans = [0.34, -0.4, 1.05] # [0.5, -0.35, 1.35] objective = ik.objective(self.robot.link(9), R=so3.from_rpy((np.pi, -np.pi / 2, 0)), t=face_trans) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7]) if not solved: print('cannot insert swab into mouth') return None self.qmouth_in = self.robot.getConfig() self.qmouth_in = self.gripper.set_finger_config( self.qmouth_in, self.gripper.partway_open_config(self.close_amount)) # add a waypoint to lower down the gripper v1 = list(np.array(v) - np.array([0, 0, 0.1])) # vis.add('v1', v1, color=[0, 0, 1, 1]) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, 0, -np.pi / 2)), t=v1) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('qlower') return None self.qlower = self.robot.getConfig() self.qlower = self.gripper.set_finger_config( self.qlower, self.gripper.partway_open_config(self.close_amount)) # add waypoints to dip the swab into the plate goal = (0.7, 0.35, 0.9) t = list(np.array(goal) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplaceplate = self.robot.getConfig() self.qplaceplate = self.gripper.set_finger_config( self.qplaceplate, self.gripper.partway_open_config(self.close_amount)) t1 = list(np.array(t) - np.array([0, 0, 0.06])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t1) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplaceplate_lower = self.robot.getConfig() self.qplaceplate_lower = self.gripper.set_finger_config( self.qplaceplate_lower, self.gripper.partway_open_config(self.close_amount)) # add waypoints to throw the swab into the trash can goal = (0.1, -0.4, 0.8) t = list(np.array(goal) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplace_trash = self.robot.getConfig() self.qplace_trash = self.gripper.set_finger_config( self.qplace_trash, self.gripper.partway_open_config(self.close_amount)) self.qplace = self.gripper.set_finger_config( self.qplace_trash, self.gripper.partway_open_config(1)) else: Tplacement_x = se3.mul(Tplacement, se3.inv(self.Tobject_gripper)) objective = ik.objective(self.robot.link(9), R=Tplacement_x[0], t=Tplacement_x[1]) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: return None qpreplace = self.robot.getConfig() qplace = self.gripper.set_finger_config( qpreplace, self.gripper.partway_open_config(1)) return qplace
def optimization(self): """ :return: None """ time_stamp = time.time() min_error = 10e5 for k in range(25): random.seed(time.time()) est_input = [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.8, 0.8) for i in range(3)] + \ [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.2, 0.2) for i in range(3)] print("We will start " + str(k + 1) + "th minimize!") res = root(opt_error_fun, np.array(est_input), args=(self.Tlink_set, self.trans_list), method='lm', options={}) print("The reason for termination: \n", res.message, "\nAnd the number of the iterations are: ", res.nfev) error = np.sum(np.absolute(res.fun)) print("The minimize is end, and the error is: ", error) if error <= min_error: min_error = error self.min_res = res print("The optimization uses: ", time.time() - time_stamp, "seconds") print("The error is: ", np.sum(np.absolute(self.min_res.fun))) print("The optimized T_oct is: ", (self.min_res.x[0:3], self.min_res.x[3:6])) print( "And the matrix form is: \n", np.array( se3.homogeneous( (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])))) print("The optimized T_needle_end is: ", (self.min_res.x[6:9], self.min_res.x[9:12])) print( "And the matrix form is: \n", np.array( se3.homogeneous((so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])))) # os.makedirs("../../data/suture_experiment/calibration_result_files/" + self.serial_num + "/", exist_ok=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_result.npy", self.min_res.x, allow_pickle=True) vis.spin(float('inf')) vis.clear() self.robot.setConfig(self.qstart) vis.add("world", self.world) World_base = model.coordinates.Frame("Base Frame", worldCoordinates=(so3.from_rpy( [0, 0, 0]), [0, 0, 0])) vis.add('World_base', World_base) est_OCT_coordinate = model.coordinates.Frame( "est OCT Frame", worldCoordinates=(so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])) vis.add("est OCT Frame", est_OCT_coordinate) Tlink_6 = model.coordinates.Frame( "Link 6 Frame", worldCoordinates=self.robot.link('link_6').getTransform()) vis.add('Link 6 Frame', Tlink_6) est_Tneedle_world = se3.mul( self.robot.link('link_6').getTransform(), (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])) est_needle_coordinate = model.coordinates.Frame( "est needle Frame", worldCoordinates=est_Tneedle_world) vis.add("est needle Frame", est_needle_coordinate) vis.spin(float('inf'))
def needle2oct_icp(self, thres_t, thres_s): """ :return: None """ threshold = 50 for i in range(0, self.num_pcd): pcd_s = read_point_cloud( "../../data/point_clouds/source_point_clouds/" + self.serial_num + "/cropped_OCT_config_" + str(i + 1) + ".ply") pcd_s.points = Vector3dVector( np.multiply(np.asarray(pcd_s.points), 1e-3)) if i < thres_t: pcd_t = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1-cut.ply") else: pcd_t = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1.ply") pcd_t.points = Vector3dVector( np.multiply(np.asarray(pcd_t.points), 1e-3)) temp_rmse = 1. count = 0 termination_threshold = 2.0e-5 while temp_rmse > termination_threshold: print("ICP implementation is in " + str(i + 1) + "th point clouds.") random.seed(count * 5 + 7 - count / 3) if i > thres_s: trans_init = np.asarray( se3.homogeneous((so3.from_rpy([ random.uniform(-0.5 * np.pi, 0.5 * np.pi) for kk in range(3) ]), [ random.uniform(-5. * 1e-3, 5. * 1e-3) for kk in range(3) ]))) else: trans_init = np.asarray( se3.homogeneous((so3.from_rpy([ random.uniform(-0.5 * np.pi, 0.5 * np.pi) + np.pi ] + [ random.uniform(-0.5 * np.pi, 0.5 * np.pi) for kk in range(2) ]), [random.uniform(-5. * 1e-3, 5. * 1e-3)] + [ random.uniform(-5. * 1e-3, 5. * 1e-3) + 15. * 1e-3 ] + [random.uniform(-5. * 1e-3, 5. * 1e-3)]))) reg_p2p = registration_icp( pcd_s, pcd_t, threshold, trans_init, TransformationEstimationPointToPoint(), ICPConvergenceCriteria(max_iteration=300, relative_rmse=1e-15, relative_fitness=1e-15)) # ***DEBUG*** # # draw_registration_result(pcd_s, pcd_t, reg_p2p.transformation) modified_pcd_s = duplicate_part_pcd(reg_p2p.transformation, pcd_s, -0.2 * 1e-3, 0.0 * 1e-3, 30) reg_p2p_2 = registration_icp( modified_pcd_s, pcd_t, threshold, reg_p2p.transformation, TransformationEstimationPointToPoint(), ICPConvergenceCriteria(max_iteration=100, relative_rmse=1e-15, relative_fitness=1e-15)) # ***DEBUG*** # # draw_registration_result(modified_pcd_s, pcd_t, reg_p2p_2.transformation) print("The RMSE is: ", reg_p2p_2.inlier_rmse) temp_rmse = reg_p2p_2.inlier_rmse count += 1 if count == 15: termination_threshold = 2.5e-5 trans = se3.from_homogeneous(reg_p2p_2.transformation) self.trans_list.append(trans) point_oct = [] pcd_s_copy = PointCloud() pcd_p = PointCloud() pcd_s_copy.points = Vector3dVector( np.multiply(np.asarray(modified_pcd_s.points), 1)) pcd_t_copy = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1.ply") pcd_t_copy.points = Vector3dVector( np.multiply(np.asarray(pcd_t_copy.points), 1e-3)) pcd_p.points = Vector3dVector(point_oct) # ***DEBUG*** # # draw_registration_result(pcd_s_copy, pcd_t_copy, se3.homogeneous(trans)) self.pcd_list.append([pcd_s_copy, pcd_t_copy])
def test_from_rpy(self): R = so3.from_rpy((0,0,0))
def integrate_velocities(controller, sim, dt): """The make() function returns a 1-argument function that takes a SimRobotController and performs whatever processing is needed when it is invoked.""" global start_time global now_dur global syn_curr global syn_next global entered_once global got_syn if not entered_once: syn_vec = controller.getCommandedConfig() if not syn_vec: got_syn = False entered_once = False else: got_syn = True entered_once = True syn_curr = syn_vec[34] start_time = sim.getTime() else: syn_curr = syn_next now_dur = sim.getTime() - start_time # print 'The current simulation duration is', now_dur rob = sim.controller(0).model() palm_curr = mv_el.get_moving_base_xform(rob) R = palm_curr[0] t = palm_curr[1] if DEBUG: print 'The current palm rotation is ', R print 'The current palm translation is ', t print 'The simulation dt is ', dt # Converting to rpy euler = so3.rpy(R) # Checking if list empty and returning false if not got_syn: return False, None, None if DEBUG: print 'The integration time is ', dt print 'The adaptive velocity of hand is ', global_vars.hand_command print 'The adaptive twist of palm is \n', global_vars.arm_command # print 'The commanded position of the hand encoder (in memory) is ', syn_curr # print 'The present position of the hand encoder (sensed) is ', controller.getCommandedConfig()[34] v = rob.getVelocity() print 'The actual velocity of the robot is ', v # print 'The present pose of the palm is \n', palm_curr # print 'The present position of the palm is ', t, 'and its orientation is', euler # Getting linear and angular velocities lin_vel_vec = global_vars.arm_command.linear ang_vel_vec = global_vars.arm_command.angular lin_vel = [lin_vel_vec.x, lin_vel_vec.y, lin_vel_vec.z] ang_vel = [ang_vel_vec.x, ang_vel_vec.y, ang_vel_vec.z] ang_vel_loc = so3.apply(so3.inv(R), ang_vel) # rotating ang vel to local frame # Transform ang vel into rpy vel euler_vel = transform_ang_vel(euler, ang_vel_loc) palm_vel = [] palm_vel.extend(lin_vel) palm_vel.extend([euler_vel[0], euler_vel[1], euler_vel[2]]) # appending syn_vel = global_vars.hand_command # Integrating syn_next = syn_curr + syn_vel * int_const_syn * dt t_next = vectorops.madd(t, lin_vel, int_const_t * dt) euler_next = vectorops.madd(euler, euler_vel, int_const_eul * dt) # Saturating synergy if syn_next >= 1.0: syn_next = 1.0 # Convert back for send xform palm_R_next = so3.from_rpy(euler_next) palm_t_next = t_next palm_next = (palm_R_next, palm_t_next) if DEBUG or True: print 'euler is ', euler, ' and is of type ', type(euler) print 'euler_vel is ', euler_vel, ' and is of type ', type(euler_vel) print 'euler_next is ', euler_next, ' and is of type ', type( euler_next) # # print 't is ', t, ' and is of type ', type(t) # print 't_next is ', t_next, ' and is of type ', type(t_next) # # print 'R is ', R, ' and is of type ', type(R) # print 'palm_R_next is ', palm_R_next, ' and is of type ', type(palm_R_next) # # print 'palm_curr is ', palm_curr, ' and is of type ', type(palm_curr) # print 'palm_next is ', palm_next, ' and is of type ', type(palm_next) return True, syn_next, palm_next, syn_vel, palm_vel