From a5c1367efb05aa2782114dda4f107a01160d12cc Mon Sep 17 00:00:00 2001 From: Ruthrash Date: Tue, 4 Apr 2023 19:10:42 -0400 Subject: [PATCH 1/3] moving robot automatically with both abs poses and relative poses to collect data for calibration --- .gitignore | 3 + calibration/ROSCameraRobotCalibration.py | 35 ++++++++--- calibration/base_robot_camera_calibration.py | 6 +- calibration/calibration_utils.py | 61 ++++++++++++-------- 4 files changed, 68 insertions(+), 37 deletions(-) diff --git a/.gitignore b/.gitignore index 5e2bc4d..b4428c9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,12 @@ franka_control_suite/build/ .vscode/ #calibration/data/ +calibration/data/fg/*.csv +calibration/data/fg/*.jpg !calibration/calibration_board/target_A4.pdf !calibration/calibration_board/target_US_letter.pdf *.png *.pdf *.pyc *.jpg +*.csv \ No newline at end of file diff --git a/calibration/ROSCameraRobotCalibration.py b/calibration/ROSCameraRobotCalibration.py index 45fc241..04cc129 100644 --- a/calibration/ROSCameraRobotCalibration.py +++ b/calibration/ROSCameraRobotCalibration.py @@ -47,10 +47,15 @@ def __init__(self, args, self.processed_image = 0 self.bool_kill_thread = False if self.args.move_robot_automatically: - self.abs_poses = get_absolute_poses(movement_pose_file)#gets command poses for frankapy to move robot automatically + if not self.delta_poses: + self.abs_poses = get_absolute_poses(movement_pose_file)#gets command poses for frankapy to move robot automatically + else: + self.initial_joints = self.fa_object.get_joints() + self.initial_pose = self.fa_object.get_pose() + self.delta_poses = get_delta_poses(file_name=movement_pose_file, initial_pose_fa=self.initial_pose) else: self.abs_poses = None - + def subscribe_image_cb(self, img_msg): if self.camera_matrix is None: return @@ -76,13 +81,23 @@ def user_robot_move_function(self,): R_tag2cam = []; t_tag2cam = [] prev_tag_pose = None while(True): - if(self.abs_poses is not None and len(self.abs_poses)!=0 ): - self.fa_object.reset_joints() - self.fa_object.goto_pose(self.abs_poses.pop(0),ignore_virtual_walls=True, use_impedance=False) - time.sleep(1.0) - elif (self.abs_poses is not None and len(self.abs_poses)==0): - rospy.core.signal_shutdown('keyboard interrupt') - break + # moves robot to viewposes recorded in file + if(self.move_robot_automatically and not self.delta_poses): + if(self.abs_poses is not None and len(self.abs_poses)!=0 ): + self.fa_object.reset_joints() + self.fa_object.goto_pose(self.abs_poses.pop(0),ignore_virtual_walls=True, use_impedance=False) + time.sleep(1.0) + elif (self.abs_poses is not None and len(self.abs_poses)==0): + rospy.core.signal_shutdown('keyboard interrupt') + break + elif(self.move_robot_automatically and self.delta_poses): + if(self.delta_poses is not None and len(self.delta_poses)!=0 ): + self.fa_object.goto_joints(self.initial_joints) + self.fa_object.goto_pose_delta(self.delta_poses.pop(0),ignore_virtual_walls=True, use_impedance=False) + time.sleep(1.0) + elif (self.delta_poses is not None and len(self.delta_poses)==0): + rospy.core.signal_shutdown('keyboard interrupt') + break if(args.move_robot_automatically): ip = "" @@ -169,6 +184,8 @@ def main(args): 'manually moved to an initial pose and the script controls EE to predefined '\ 'relative poses. If false, the EE should be moved manually(white status LED '\ 'and press enter to collect data') + parser.add_argument("--use_delta_poses", default=True, nargs='?', type=str2bool, help='should we use '\ + 'delta end-effector poses to move the robot to collect data for calibration') parser.add_argument("--only_calibration", default=False, nargs='?',type=str2bool, help='if true, values '\ 'stored in the data folder are used for calibration if false, data is first collected, '\ 'stored in /data folder and then calibration routine is run') diff --git a/calibration/base_robot_camera_calibration.py b/calibration/base_robot_camera_calibration.py index ab5c5fc..f9c63e9 100644 --- a/calibration/base_robot_camera_calibration.py +++ b/calibration/base_robot_camera_calibration.py @@ -124,10 +124,10 @@ def process_image_for_aruco(self, image_gray, prev_tag_pose=None): # print(objPoints, imgPoints) retval, rvec, tvec = cv2.solvePnP(objPoints, imgPoints, self.camera_matrix, rvec, tvec) if(self.args.debug_image): - cv2.aruco.drawDetectedMarkers(color_im, corners, borderColor=(0, 0, 255)) - cv2.drawFrameAxes(color_im, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.1) + cv2.aruco.drawDetectedMarkers(image_gray, corners, borderColor=(0, 0, 255)) + cv2.drawFrameAxes(image_gray, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.1) img_file_name = "data/image/image_"+str(self.detections_count-1)+".jpg" - cv2.imwrite(img_file_name, color_im) + cv2.imwrite(img_file_name, image_gray) reproj_error = reprojection_error(corners, ids, rvec, tvec, diff --git a/calibration/calibration_utils.py b/calibration/calibration_utils.py index 4df746c..10ac839 100755 --- a/calibration/calibration_utils.py +++ b/calibration/calibration_utils.py @@ -34,11 +34,11 @@ def write_to_file(line_list, file_name): f.write(line) f.write('\n') -def sample_view_poses(initial_pose): - delta_position = [[],[],[],[],[],[],[],[],[],[]] - return +# def sample_view_poses(initial_pose): +# delta_position = [[],[],[],[],[],[],[],[],[],[]] +# return -def get_delta_poses(file_name): +def get_delta_poses(file_name, initial_pose_fa): with open(file_name, 'r') as fp: lines = fp.readlines() ee_poses_tf = [] @@ -50,36 +50,47 @@ def get_delta_poses(file_name): (float(data[3]), float(data[4]), float(data[5])))) - tag_pose = tuple(((float(data[6]), - float(data[7]), - float(data[8])), - (float(data[9]), - float(data[10]), - float(data[11])))) + # tag_pose = tuple(((float(data[6]), + # float(data[7]), + # float(data[8])), + # (float(data[9]), + # float(data[10]), + # float(data[11])))) ee_pose_tf = np.eye(4) - tag_pose_tf = np.eye(4) + # tag_pose_tf = np.eye(4) ee_pose_tf[0:3, 0:3] = cv2.Rodrigues(ee_pose[0])[0]; ee_pose_tf[0:3, -1] = ee_pose[1] - tag_pose_tf[0:3, 0:3] = cv2.Rodrigues(tag_pose[0])[0]; tag_pose_tf[0:3, -1] = tag_pose[1] + ee_pose_tf[0:3, -1] = ee_pose_tf[0:3, -1]/1000.0 + # tag_pose_tf[0:3, 0:3] = cv2.Rodrigues(tag_pose[0])[0]; tag_pose_tf[0:3, -1] = tag_pose[1] ee_poses_tf.append(ee_pose_tf) - prev = None + delta_poses = [] + initial_pose = np.eye(4) + initial_pose[0:3, 0:3] = initial_pose_fa.rotation + initial_pose[0:3, -1] = initial_pose_fa.translation for abs_ee_pose_tf in ee_poses_tf: - if prev is None: - delta_pose = RigidTransform(from_frame='franka_tool', - to_frame='franka_tool', - rotation=np.eye(3), - translation=np.array([0.0, 0.0,0.0])) - prev = abs_ee_pose_tf - else: + # if prev is None: + # delta_pose = RigidTransform(from_frame='franka_tool', + # to_frame='franka_tool', + # rotation=np.eye(3), + # translation=np.array([0.0, 0.0,0.0])) + # prev = abs_ee_pose_tf + # else: # difference = np.matmul(np.linalg.inv(abs_ee_pose_tf), prev) # print(prev) # print(abs_ee_pose_tf) - difference = np.matmul(np.linalg.inv(prev), abs_ee_pose_tf) - delta_pose = RigidTransform(from_frame='franka_tool', - to_frame='franka_tool', - rotation=difference[0:3,0:3], - translation=difference[0:3,-1]) + # difference = np.matmul(np.linalg.inv(abs_ee_pose_tf), initial_pose) + difference = np.matmul(np.linalg.inv(initial_pose), abs_ee_pose_tf ) + + delta_pose = RigidTransform(from_frame='franka_tool', + to_frame='franka_tool', + rotation=difference[0:3,0:3], + translation=difference[0:3,-1]) delta_poses.append(delta_pose) + first_delta_pose = RigidTransform(from_frame='franka_tool', + to_frame='franka_tool', + rotation=np.eye(3), + translation=np.array([0.0, 0.0, 0.0])) + delta_poses = [first_delta_pose] + delta_poses return delta_poses def get_absolute_poses(file_name): From 0963bdba77f609c28519072adc93445e74c84e83 Mon Sep 17 00:00:00 2001 From: Ruthrash Date: Wed, 5 Apr 2023 11:54:59 -0400 Subject: [PATCH 2/3] tested automated moving with both relative and absolute ee poses --- calibration/ROSCameraRobotCalibration.py | 7 ++++--- calibration/calibration_utils.py | 12 ------------ calibration/docs/USAGE.md | 3 +++ 3 files changed, 7 insertions(+), 15 deletions(-) diff --git a/calibration/ROSCameraRobotCalibration.py b/calibration/ROSCameraRobotCalibration.py index 04cc129..da052e0 100644 --- a/calibration/ROSCameraRobotCalibration.py +++ b/calibration/ROSCameraRobotCalibration.py @@ -32,6 +32,7 @@ def __init__(self, args, aruco_board_n_cols=aruco_board_n_cols, data_file_name=file_name) self.args = args + if(self.args.move_robot_automatically): self.fa_object = FrankaArm(with_gripper=self.args.with_franka_gripper) if(not args.move_robot_automatically):# as frankapy inits its own node @@ -47,7 +48,7 @@ def __init__(self, args, self.processed_image = 0 self.bool_kill_thread = False if self.args.move_robot_automatically: - if not self.delta_poses: + if not self.args.use_delta_poses: self.abs_poses = get_absolute_poses(movement_pose_file)#gets command poses for frankapy to move robot automatically else: self.initial_joints = self.fa_object.get_joints() @@ -82,7 +83,7 @@ def user_robot_move_function(self,): prev_tag_pose = None while(True): # moves robot to viewposes recorded in file - if(self.move_robot_automatically and not self.delta_poses): + if(self.args.move_robot_automatically and not self.args.use_delta_poses): if(self.abs_poses is not None and len(self.abs_poses)!=0 ): self.fa_object.reset_joints() self.fa_object.goto_pose(self.abs_poses.pop(0),ignore_virtual_walls=True, use_impedance=False) @@ -90,7 +91,7 @@ def user_robot_move_function(self,): elif (self.abs_poses is not None and len(self.abs_poses)==0): rospy.core.signal_shutdown('keyboard interrupt') break - elif(self.move_robot_automatically and self.delta_poses): + elif(self.args.move_robot_automatically and self.args.use_delta_poses): if(self.delta_poses is not None and len(self.delta_poses)!=0 ): self.fa_object.goto_joints(self.initial_joints) self.fa_object.goto_pose_delta(self.delta_poses.pop(0),ignore_virtual_walls=True, use_impedance=False) diff --git a/calibration/calibration_utils.py b/calibration/calibration_utils.py index 10ac839..907142c 100755 --- a/calibration/calibration_utils.py +++ b/calibration/calibration_utils.py @@ -68,19 +68,7 @@ def get_delta_poses(file_name, initial_pose_fa): initial_pose[0:3, 0:3] = initial_pose_fa.rotation initial_pose[0:3, -1] = initial_pose_fa.translation for abs_ee_pose_tf in ee_poses_tf: - # if prev is None: - # delta_pose = RigidTransform(from_frame='franka_tool', - # to_frame='franka_tool', - # rotation=np.eye(3), - # translation=np.array([0.0, 0.0,0.0])) - # prev = abs_ee_pose_tf - # else: - # difference = np.matmul(np.linalg.inv(abs_ee_pose_tf), prev) - # print(prev) - # print(abs_ee_pose_tf) - # difference = np.matmul(np.linalg.inv(abs_ee_pose_tf), initial_pose) difference = np.matmul(np.linalg.inv(initial_pose), abs_ee_pose_tf ) - delta_pose = RigidTransform(from_frame='franka_tool', to_frame='franka_tool', rotation=difference[0:3,0:3], diff --git a/calibration/docs/USAGE.md b/calibration/docs/USAGE.md index adec82d..3f0f566 100644 --- a/calibration/docs/USAGE.md +++ b/calibration/docs/USAGE.md @@ -187,6 +187,9 @@ optional arguments: is first manually moved to an initial pose and the script controls EE to predefined relative poses. If false, the EE should be moved manually(white status LED) and press enter to collect data + --use_delta_poses [USE_DELTA_POSES]:True(default)/False + should we use delta end-effector poses to move the robot to collect + data for calibration --only_calibration [ONLY_CALIBRATION]: True/False(default) if true, values stored in the data folder are used for calibration if false, data is first collected, stored in /data folder and then From 0d51f7337ee28c3c69415e6d72babffedb623547 Mon Sep 17 00:00:00 2001 From: Ruthrash Date: Wed, 26 Apr 2023 10:18:16 -0400 Subject: [PATCH 3/3] updates documentation about automatic data collection with relative poses for robot-camera calibraiton --- calibration/docs/USAGE.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/calibration/docs/USAGE.md b/calibration/docs/USAGE.md index 3f0f566..b4b0b91 100644 --- a/calibration/docs/USAGE.md +++ b/calibration/docs/USAGE.md @@ -39,7 +39,7 @@ For the camera-in-hand case, the calibration tag is fixed to the environment rig -For the camera-in-environment case, the calibration tag needs to be **rigidly** attached to the robot's End-Effector. You could use the (to be) provided CAD files for the finger tips and gripping points. The finger tips are to be attached to Franka End-effector and the gripping points are drilled/screwed onto the calibration tag. Now make the Franka End-effector with custom finger tips grasp the calibration tag with the attached custom gripping points, this ensures that the tag remains rigid with respect to the End-effector. +For the camera-in-environment case, the calibration tag needs to be **rigidly** attached to the robot's End-Effector. You could use the provided CAD files for the finger tips and gripping points in [models_4_3d_printing](../models_4_3d_printing). The [finger tips](../models_4_3d_printing/franka_custom_finger_tips.stl) are to be attached to Franka End-effector and the [gripping points](../models_4_3d_printing/finger_grasp_points.stl) or [handle plate](../models_4_3d_printing/finger_handle_plate.stl) are drilled/screwed onto the calibration tag. Now make the Franka End-effector with custom finger tips(figure 1) grasp the calibration tag(as show in figure 3) with the attached custom gripping points(figure 2), this ensures that the tag remains rigid with respect to the End-effector. @@ -137,7 +137,10 @@ run the readstates server that send end effector poses when requested, ### c. Run Pose Data Collection + Calibration Script - +There are currently two ways to collect data for calibration- +1. move the robot end-effector manually and then hit "enter" to collect the pose data samples(works for both ROS and non-ROS scripts) +2. move the robot automatically with absolute/relative poses present in [../data/file_moves.txt](../data/file_moves.txt). + 2.a. If you are using relative poses, the robot will move relative to the initial end-effector configuration when you start the script, therefore, before starting frankapy, point the camera(camera in hand case) or the calibration tag(camera in environment case) to the calibration tag or the camera respectively. Ensure, the calibration tag's normal is close to perpendicular to the camera and there's around ~30cm between the tag and the camera. ### In workstation computer,