1 * 
  2 * Prior to executing this example, the example
  3 * 'handeye_stationarycam_calibration.hdev' has to be executed.
  4 * Given the transformations computed by the hand-eye calibration
  5 * this example computes the pose of the robot tool in robot base
  6 * coordinates for grasping the nut with the gripper.
  7 dev_update_off ()
  8 dev_close_window ()
  9 * Directories with calibration images and data files
 10 ImageNameStart := '3d_machine_vision/handeye/stationarycam_'
 11 DataNameStart := 'handeye/stationarycam_'
 12 read_image (Image, ImageNameStart + 'nut12_square')
 13 dev_close_window ()
 14 get_image_size (Image, Width, Height)
 15 dev_open_window_fit_image (Image, 0, 0, Width, Height, WindowHandle)
 16 dev_set_draw ('margin')
 17 dev_set_line_width (2)
 18 set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
 19 dev_display (Image)
 20 disp_message (WindowHandle, 'Object to grasp', 'window', 12, 12, 'black', 'true')
 21 * Read internal camera parameters and calibrated poses
 22 read_cam_par (DataNameStart + 'final_campar.dat', CamParam)
 23 read_pose (DataNameStart + 'final_pose_cam_base.dat', BaseInCamPose)
 24 pose_to_hom_mat3d (BaseInCamPose, cam_H_base)
 25 read_pose (DataNameStart + 'final_pose_tool_calplate.dat', CalplateInToolPose)
 26 pose_to_hom_mat3d (CalplateInToolPose, tool_H_calplate)
 27 * Read pose of gripper in tool coordinates
 28 read_pose (DataNameStart + 'pose_tool_gripper.dat', GripperInToolPose)
 29 pose_to_hom_mat3d (GripperInToolPose, tool_H_gripper)
 30 stop ()
 31 * Define reference coordinate system and display it
 32 CalplateFile := 'caltab_30mm.descr'
 33 define_reference_coord_system (ImageNameStart + 'calib3cm_00', CamParam, CalplateFile, WindowHandle, PoseRef)
 34 pose_to_hom_mat3d (PoseRef, cam_H_ref)
 35 Message := 'Defining a reference coordinate system'
 36 Message[1] := 'based on a calibration image'
 37 disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
 38 disp_continue_message (WindowHandle, 'black', 'true')
 39 stop ()
 40 dev_display (Image)
 41 disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)
 42 * Find parallel sides of the nut
 43 dev_set_color ('yellow')
 44 threshold (Image, BrightRegion, 60, 255)
 45 connection (BrightRegion, BrightRegions)
 46 select_shape (BrightRegions, Nut, 'area', 'and', 500, 99999)
 47 fill_up (Nut, NutFilled)
 48 gen_contour_region_xld (NutFilled, NutContours, 'border')
 49 segment_contours_xld (NutContours, LineSegments, 'lines', 5, 4, 2)
 50 fit_line_contour_xld (LineSegments, 'tukey', -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)
 51 gen_empty_obj (Lines)
 52 for I := 0 to |RowBegin| - 1 by 1
 53 gen_contour_polygon_xld (Contour, [RowBegin[I],RowEnd[I]], [ColBegin[I],ColEnd[I]])
 54 concat_obj (Lines, Contour, Lines)
 55 endfor
 56 gen_polygons_xld (Lines, Polygon, 'ramer', 2)
 57 gen_parallels_xld (Polygon, ParallelLines, 50, 100, rad(10), 'true')
 58 dev_display (ParallelLines)
 59 * Accumulate corner points
 60 get_parallels_xld (ParallelLines, Row1, Col1, Length1, Phi1, Row2, Col2, Length2, Phi2)
 61 CornersRow := [Row1[0],Row1[1],Row2[0],Row2[1]]
 62 CornersCol := [Col1[0],Col1[1],Col2[0],Col2[1]]
 63 * Method 1: transform corners into reference coordinate system and determine grasp
 64 image_points_to_world_plane (CamParam, PoseRef, CornersRow, CornersCol, 'm', CornersX_ref, CornersY_ref)
 65 * Determine center and orientation of the grasp
 66 CenterPointX_ref := sum(CornersX_ref) * 0.25
 67 CenterPointY_ref := sum(CornersY_ref) * 0.25
 68 GraspPointsX_ref := [(CornersX_ref[0] + CornersX_ref[1]) * 0.5,(CornersX_ref[2] + CornersX_ref[3]) * 0.5]
 69 GraspPointsY_ref := [(CornersY_ref[0] + CornersY_ref[1]) * 0.5,(CornersY_ref[2] + CornersY_ref[3]) * 0.5]
 70 GraspPhiZ_ref := atan((GraspPointsY_ref[1] - GraspPointsY_ref[0]) / (GraspPointsX_ref[1] - GraspPointsX_ref[0]))
 71 * Display grasping points after projecting them into the image
 72 affine_trans_point_3d (cam_H_ref, GraspPointsX_ref, GraspPointsY_ref, [0,0], GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam)
 73 project_3d_point (GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam, CamParam, GraspPointsRow, GraspPointsCol)
 74 display_grasping_points (GraspPointsRow, GraspPointsCol, WindowHandle)
 75 disp_message (WindowHandle, 'Finding grasping points', 'window', -1, -1, 'black', 'true')
 76 disp_continue_message (WindowHandle, 'black', 'true')
 77 stop ()
 78 * Transform it into a homogeneous transformation matrix
 79 hom_mat3d_identity (HomMat3DIdentity)
 80 hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, 'z', 0, 0, 0, HomMat3D_RZ_Phi)
 81 hom_mat3d_translate (HomMat3D_RZ_Phi, CenterPointX_ref, CenterPointY_ref, 0, ref_H_grasp)
 82 * Display coordinate system of the gripper
 83 hom_mat3d_compose (cam_H_ref, ref_H_grasp, cam_H_grasp)
 84 hom_mat3d_to_pose (cam_H_grasp, GripperInCamPose)
 85 dev_set_colored (3)
 86 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
 87 Message := 'Determining the gripper pose'
 88 Message[1] := 'via the reference coordinate system'
 89 disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
 90 disp_continue_message (WindowHandle, 'black', 'true')
 91 stop ()
 92 * Method 2: pose estimation using the four corner points of the nut
 93 NX := [0.009,-0.009,-0.009,0.009]
 94 NY := [0.009,0.009,-0.009,-0.009]
 95 NZ := [0,0,0,0]
 96 sort_corner_points (CornersRow, CornersCol, WindowHandle, NRow, NCol)
 97 vector_to_pose (NX, NY, NZ, NRow, NCol, CamParam, 'iterative', 'error', PoseCamNut, Quality)
 98 dev_set_colored (3)
 99 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
100 Message := 'Alternative: Determining the gripper pose'
101 Message[1] := 'via pose estimation using the corners'
102 disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
103 disp_continue_message (WindowHandle, 'black', 'true')
104 stop ()
105 * Determine corresponding robot position (pose of the tool in base coordinates)
106 * base_H_tool = base_H_cam * cam_H_ref * ref_H_grasp * gripper_H_tool
107 * where to position the tool to grasp the nut
108 hom_mat3d_invert (cam_H_base, base_H_cam)
109 hom_mat3d_compose (base_H_cam, cam_H_grasp, base_H_grasp)
110 hom_mat3d_invert (tool_H_gripper, gripper_H_tool)
111 hom_mat3d_compose (base_H_grasp, gripper_H_tool, base_H_tool)
112 hom_mat3d_to_pose (base_H_tool, PoseRobotGrasp)
113 * Convert pose type to the one used by the robot controller (ZYX) and display it
114 convert_pose_type (PoseRobotGrasp, 'Rp+T', 'abg', 'point', PoseRobotGrasp_ZYX)
115 * Alternatively, the PoseRobotGrasp can be computed using only poses instead of
116 * matrices.
117 pose_invert (BaseInCamPose, CamInBasePose)
118 pose_compose (CamInBasePose, GripperInCamPose, GripperInBasePose)
119 pose_invert (GripperInToolPose, ToolInGripper)
120 * The computed ToolInBasePose equals PoseRobotGrasp
121 pose_compose (GripperInBasePose, ToolInGripper, ToolInBasePose)
122 dev_display (Image)
123 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
124 disp_message (WindowHandle, 'Converting the pose into robot coordinates', 'window', -1, -1, 'black', 'true')
125 dev_inspect_ctrl (PoseRobotGrasp_ZYX)
126 * This pose should then be sent to the robot
127 disp_continue_message (WindowHandle, 'black', 'true')
128 stop ()
129 dev_close_inspect_ctrl (PoseRobotGrasp_ZYX)