def geometry_pose_component_get(pose, component_name): """ Get an XYZ-RPY component of a pose. Parameters: * pose (Pose): The pose * component_name (str): The component to get. May be `X`, `Y`, `Z`, `R_R`, `R_P`, or `R_Y` Return (float): The pose value """ geom_util = GeometryUtil(node = PyriSandboxContext.node) xyz,rpy = geom_util.pose_to_xyz_rpy(_convert_to_pose(pose)) rpy = np.rad2deg(rpy) if component_name == "X": return float(xyz[0]) if component_name == "Y": return float(xyz[1]) if component_name == "Z": return float(xyz[2]) if component_name == "R_R": return float(rpy[0]) if component_name == "R_P": return float(rpy[1]) if component_name == "R_Y": return float(rpy[2]) assert False, "Invalid pose component"
def __init__(self, device_manager, device_info=None, node: RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self._pose2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Pose2D") self._point2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Point2D") self._geom_util = GeometryUtil(node=self._node) self._robot_util = RobotUtil(node=self._node) self.device_manager = device_manager self.device_manager.connect_device_type( "com.robotraconteur.robotics.robot.Robot") self.device_manager.connect_device_type( "com.robotraconteur.robotics.tool.Tool") self.device_manager.connect_device_type( "tech.pyri.variable_storage.VariableStorage") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5)
def __init__(self, device_manager, device_info=None, node: RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self._detected_marker = self._node.GetStructureType( "tech.pyri.vision.aruco_detection.DetectedMarker") self._aruco_detection_result = self._node.GetStructureType( "tech.pyri.vision.aruco_detection.ArucoDetectionResult") self._pose2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Pose2D") self._point2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Point2D") self._image_util = ImageUtil(node=self._node) self._geom_util = GeometryUtil(node=self._node) self.device_manager = device_manager self.device_manager.connect_device_type( "tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type( "com.robotraconteur.imaging.Camera") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5)
def _calibrate_camera_intrinsic(images, calibration_target): ret, mtx, dist, rvecs, tvecs, mean_error, imgs = _calibrate_camera_intrinsic2( images, calibration_target) if not ret: raise RR.OperationFailedException( "Camera intrinsic calibration failed") geom_util = GeometryUtil() calib = RRN.NewStructure( "com.robotraconteur.imaging.camerainfo.CameraCalibration") calib.image_size = geom_util.wh_to_size2d( [images[0].image_info.width, images[0].image_info.height], dtype=np.int32) calib.K = mtx dist_rr = RRN.NewStructure( "com.robotraconteur.imaging.camerainfo.PlumbBobDistortionInfo") dist_rr.k1 = dist[0] dist_rr.k2 = dist[1] dist_rr.p1 = dist[2] dist_rr.p2 = dist[3] dist_rr.k3 = dist[4] calib.distortion_info = RR.VarValue( dist_rr, "com.robotraconteur.imaging.camerainfo.PlumbBobDistortionInfo") imgs2 = [] for img in imgs: imgs2.append(_cv_img_to_rr_display_img(img)) return calib, imgs2, mean_error
def __init__(self, device_manager, device_info = None, node: RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self._matched_template_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate2D") self._template_matching_result_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult2D") self._template_matching_result_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult3D") self._matched_template_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate3D") self._named_pose_with_covariance_type = self._node.GetStructureType("com.robotraconteur.geometry.NamedPoseWithCovariance") self._pose2d_dtype = self._node.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D") self._image_util = ImageUtil(node=self._node) self._geom_util = GeometryUtil(node=self._node) self.device_manager = device_manager self.device_manager.connect_device_type("tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type("com.robotraconteur.imaging.Camera") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5)
def handle_create(self, *args): try: robot_local_device_name = self.vue["$data"].robot_selected intrinsic_calib = self.vue["$data"].camera_intrinsic_selected extrinsic_calib = self.vue["$data"].camera_extrinsic_selected image_sequence_global_name = self.vue[ "$data"].image_sequence_selected aruco_dict = self.vue["$data"].aruco_dict_selected aruco_tag_id = int(self.vue["$data"].aruco_tag_id) aruco_tag_size = float(self.vue["$data"].aruco_tag_size) xyz = np.zeros((3, ), dtype=np.float64) rpy = np.zeros((3, ), dtype=np.float64) xyz[0] = float(self.vue["$data"].marker_pose_x) xyz[1] = float(self.vue["$data"].marker_pose_y) xyz[2] = float(self.vue["$data"].marker_pose_z) rpy[0] = float(self.vue["$data"].marker_pose_r_r) rpy[1] = float(self.vue["$data"].marker_pose_r_p) rpy[2] = float(self.vue["$data"].marker_pose_r_y) rpy = np.deg2rad(rpy) robot_calib = self.core.device_manager.get_device_subscription( "vision_robot_calibration").GetDefaultClient() geom_util = GeometryUtil(client_obj=robot_calib) marker_pose = geom_util.xyz_rpy_to_pose(xyz, rpy) self.core.create_task(do_calibration(robot_local_device_name,intrinsic_calib,extrinsic_calib,\ image_sequence_global_name,aruco_dict,aruco_tag_id, aruco_tag_size, marker_pose, self.new_name,self.core)) except: traceback.print_exc()
def geometry_pose_component_set(pose, component_name, value): """ Set an XYZ-RPY component of a pose. This function does not modify in place. It returns a new pose. Parameters: * pose (Pose): The pose * component_name (str): The component to get. May be `X`, `Y`, `Z`, `R_R`, `R_P`, or `R_Y` * value (float): The new pose component value in meters or degrees Return (Pose): The new pose with updated value """ geom_util = GeometryUtil(node = PyriSandboxContext.node) xyz,rpy = geom_util.pose_to_xyz_rpy(_convert_to_pose(pose)) rpy = np.rad2deg(rpy) if component_name == "X": xyz[0] = value elif component_name == "Y": xyz[1] = value elif component_name == "Z": xyz[2] = value elif component_name == "R_R": rpy[0] = value elif component_name == "R_P": rpy[1] = value elif component_name == "R_Y": rpy[2] = value else: assert False, "Invalid pose component" rpy = np.deg2rad(rpy) return geom_util.xyz_rpy_to_pose(xyz,rpy)
def _calibrate_camera_extrinsic(intrinsic_calib, image, board, camera_local_device_name): # TODO: verify calibration data mtx = intrinsic_calib.K dist_rr = intrinsic_calib.distortion_info.data dist = np.array( [dist_rr.k1, dist_rr.k2, dist_rr.p1, dist_rr.p2, dist_rr.k3], dtype=np.float64) image_util = ImageUtil() frame = image_util.compressed_image_to_array(image) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) if board == "chessboard": width = 7 height = 6 square_size = 0.03 else: raise RR.InvalidOperationException( f"Invalid calibration board {board}") ret, corners = cv2.findChessboardCorners(gray, (width, height), None) assert ret, "Could not find calibration target" objp = np.zeros((height * width, 3), np.float32) objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(-1, 2) objp = objp * square_size criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) ret, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist) R = cv2.Rodrigues(rvecs.flatten())[0] R_landmark = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]], dtype=np.float64) R_cam1 = R.transpose() p_cam1 = -R.transpose() @ tvecs R_cam = R_landmark.transpose() @ R_cam1 p_cam = R_landmark.transpose() @ p_cam1 cv_image2 = cv2.aruco.drawAxis(frame, mtx, dist, cv2.Rodrigues(R_cam.transpose())[0], -R_cam.transpose() @ p_cam, 0.1) T = rox.Transform(R_cam, p_cam, "world", camera_local_device_name) geom_util = GeometryUtil() cov = np.eye(6) * 1e-5 return geom_util.rox_transform_to_named_pose( T), cov, image_util.array_to_compressed_image_jpg(cv_image2), 0.0
def do_show_new_camera_calibration_extrinsic_dialog2(new_name: str, camera_pose, display_image, core: "PyriWebUIBrowser"): try: camera_calib = core.device_manager.get_device_subscription("vision_camera_calibration").GetDefaultClient() dialog2_html = importlib_resources.read_text(__package__,"new_calibrate_extrinsic_dialog2.html") el = js.document.createElement('div') el.id = "new_calibrate_extrinsic_dialog2_wrapper" js.document.getElementById("wrapper").appendChild(el) def handle_hidden(*args): try: el.parentElement.removeChild(el) except: traceback.print_exc() geom_util = GeometryUtil(client_obj=camera_calib) xyz, rpy1, _, _ = geom_util.named_pose_to_xyz_rpy(camera_pose.pose) rpy = np.rad2deg(rpy1) x = f"{xyz[0]:4e}" y = f"{xyz[1]:4e}" z = f"{xyz[2]:4e}" r_r = f"{rpy[0]:4e}" r_p = f"{rpy[1]:4e}" r_y = f"{rpy[2]:4e}" i=0 d_encoded = str(base64.b64encode(display_image.data))[2:-1] disp_img_src = "data:image/jpeg;base64," + d_encoded # TODO: check for png? dialog = js.Vue.new(js.python_to_js({ "el": "#new_calibrate_extrinsic_dialog2_wrapper", "template": dialog2_html, "data": { "x": x, "y": y, "z": z, "r_r": r_r, "r_p": r_p, "r_y": r_y, "disp_img": disp_img_src }, "methods": { "handle_hidden": handle_hidden } })) dialog["$bvModal"].show("new_vision_camera_calibrate_extrinsic2") except: traceback.print_exc()
def geometry_pose_inv(pose): """ Invert a pose Parameters: pose (Pose): The pose to invert Return (Pose): The inverted pose """ geom_util = GeometryUtil(node = PyriSandboxContext.node) T_rox = geom_util.pose_to_rox_transform(_convert_to_pose(pose)) T_res = T_rox.inv() return geom_util.rox_transform_to_pose(T_res)
def test_geometry_util_transform_types(): node = RR.RobotRaconteurNode() node.SetLogLevelFromString("DEBUG") node.Init() try: RRC.RegisterStdRobDefServiceTypes(node) geom_util = GeometryUtil(node) _do_transform_test(geom_util.rox_transform_to_transform, geom_util.transform_to_rox_transform, "Transform", node) _do_transform_test(geom_util.rox_transform_to_pose, geom_util.pose_to_rox_transform, "Pose", node) _do_named_transform_test(geom_util.rox_transform_to_named_transform, geom_util.named_transform_to_rox_transform, "NamedTransform", node) _do_named_transform_test(geom_util.rox_transform_to_named_pose, geom_util.named_pose_to_rox_transform, "NamedPose", node) _do_transform_test_xyz_rpy(geom_util.xyz_rpy_to_transform, geom_util.transform_to_xyz_rpy, "Transform", node) _do_transform_test_xyz_rpy(geom_util.xyz_rpy_to_pose, geom_util.pose_to_xyz_rpy, "Pose", node) _do_named_transform_test_xyz_rpy(geom_util.xyz_rpy_to_named_transform, geom_util.named_transform_to_xyz_rpy, "NamedTransform", node) _do_named_transform_test_xyz_rpy(geom_util.xyz_rpy_to_named_pose, geom_util.named_pose_to_xyz_rpy, "NamedPose", node) finally: node.Shutdown()
def geometry_pose_multiply(pose_a, pose_b): """ Multiply one pose with another Parameters: * pose_a (Pose): The first pose * pose_b (Pose): The second pose Return (Pose): The result of the multiplication """ geom_util = GeometryUtil(node = PyriSandboxContext.node) T_a = geom_util.pose_to_rox_transform(_convert_to_pose(pose_a)) T_b = geom_util.pose_to_rox_transform(_convert_to_pose(pose_b)) T_res = T_a * T_b return geom_util.rox_transform_to_pose(T_res)
def __init__(self, device_manager, device_info = None, node : RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self.device_manager = device_manager self.device_manager.connect_device_type("tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type("com.robotraconteur.robotics.robot.Robot") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5) self.robot_util = RobotUtil(self._node) self.geom_util = GeometryUtil(self._node) self.image_util = ImageUtil(self._node)
def geometry_pose_new(x,y,z,r_r,r_p,r_y): """ Create a new pose using XYZ-RPY format. Units are meters and degrees Parameters: * x (float): X position in meters * y (float): Y position in meters * z (float): Z position in meters * r_r (float): Roll in degrees * r_p (float): Pitch in degrees * r_y (float): Yaw in degrees Return (Pose): Pose named array """ xyz = np.array([x,y,z],dtype=np.float64) rpy = np.deg2rad(np.array([r_r,r_p,r_y],dtype=np.float64)) geom_util = GeometryUtil(node = PyriSandboxContext.node) return geom_util.xyz_rpy_to_pose(xyz,rpy)
def jog_joints_to_pose(self, pose, speed_perc): print("Jog Joints to Pose is called") # Similar to jog_joints_with_limits. But, # Moves the robot to the specified joint angles with max speed robot = self.robot if robot is not None: robot_state, _ = self.robot.robot_state.PeekInValue() q_current = robot_state.joint_position geom_util = GeometryUtil(client_obj=robot) T_des = geom_util.pose_to_rox_transform(pose) q_des, res = invkin.update_ik_info3(self.robot_rox, T_des, q_current) assert res, "Inverse kinematics failed" self.jog_joints_with_limits( q_des, float(speed_perc) * 0.01 * self.joint_vel_limits, True) else: # Give an error message to show that the robot is not connected print("Robot is not connected to RoboticsJog service yet!")
def robot_get_end_pose(frame): """ Returns the end pose of a robot. This end pose is reported by the robot driver. It is typically defined as the flange of the robot, but may be the tool if the driver is configured to report the tool pose. Parameters: * frame (str): The frame to return the pose in. May be `robot` or `world`. Return (Pose): The robot end pose in the requested frame """ robot_name = _get_active_robot_name() device_manager = PyriSandboxContext.device_manager robot = device_manager.get_device_client(robot_name,1) robot_state, _ = robot.robot_state.PeekInValue() robot_util = RobotUtil(client_obj = robot) geom_util = GeometryUtil(client_obj = robot) # TODO: cache robot_info rox_robot = robot_util.robot_info_to_rox_robot(robot.robot_info,0) T1 = rox.fwdkin(rox_robot,robot_state.joint_position) if frame =="ROBOT": return geom_util.rox_transform_to_pose(T1) elif frame == "WORLD": var_storage = device_manager.get_device_client("variable_storage") # TODO: don't hard code robot origin robot_origin_pose = var_storage.getf_variable_value("globals",_get_robot_origin_calibration()).data T_rob = geom_util.named_pose_to_rox_transform(robot_origin_pose.pose) T2 = T_rob * T1 return geom_util.rox_transform_to_pose(T2) else: assert False, "Invalid frame"
def test_geometry_util_array_types(): node = RR.RobotRaconteurNode() node.SetLogLevelFromString("DEBUG") node.Init() try: RRC.RegisterStdRobDefServiceTypes(node) geom_util = GeometryUtil(node) _do_array_test(geom_util.xy_to_vector2, geom_util.vector2_to_xy, (2, ), "Vector2", node) _do_array_test(geom_util.xyz_to_vector3, geom_util.vector3_to_xyz, (3, ), "Vector3", node) _do_array_test(geom_util.abgxyz_to_vector6, geom_util.vector6_to_abgxyz, (6, ), "Vector6", node) _do_array_test(geom_util.xy_to_point2d, geom_util.point2d_to_xy, (2, ), "Point2D", node) _do_array_test(geom_util.xyz_to_point, geom_util.point_to_xyz, (3, ), "Point", node) _do_array_test(geom_util.wh_to_size2d, geom_util.size2d_to_wh, (2, ), "Size2D", node) _do_array_test(geom_util.whd_to_size, geom_util.size_to_whd, (3, ), "Size", node) _do_array_test(geom_util.q_to_quaternion, geom_util.quaternion_to_q, (3, ), "Quaternion", node, lambda a: rox.R2q(rox.rpy2R(a))) _do_array_test(geom_util.R_to_quaternion, geom_util.quaternion_to_R, (3, ), "Quaternion", node, lambda a: rox.rpy2R(a)) _do_array_test(geom_util.rpy_to_quaternion, geom_util.quaternion_to_rpy, (3, ), "Quaternion", node) _do_array_test(geom_util.array_to_spatial_velocity, geom_util.spatial_velocity_to_array, (6, ), "SpatialVelocity", node) _do_array_test(geom_util.array_to_spatial_acceleration, geom_util.spatial_acceleration_to_array, (6, ), "SpatialAcceleration", node) _do_array_test(geom_util.array_to_wrench, geom_util.wrench_to_array, (6, ), "Wrench", node) finally: node.Shutdown()
def movel(self, robot_local_device_name, pose_final, frame, robot_origin_calib_global_name, speed_perc, final_seed=None): robot = self.device_manager.get_device_client(robot_local_device_name) geom_util = GeometryUtil(client_obj=robot) if frame.lower() == "world": var_storage = self.device_manager.get_device_client( "variable_storage") robot_origin_pose = var_storage.getf_variable_value( "globals", robot_origin_calib_global_name).data T_rob = geom_util.named_pose_to_rox_transform( robot_origin_pose.pose) T_des1 = geom_util.pose_to_rox_transform(pose_final) T_des = T_rob.inv() * T_des1 pose_final = geom_util.rox_transform_to_pose(T_des) elif frame.lower() == "robot": T_des = geom_util.pose_to_rox_transform(pose_final) else: assert False, "Unknown parent frame for movel" robot_info = robot.robot_info rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0) robot_state = robot.robot_state.PeekInValue()[0] q_initial = robot_state.joint_position traj = self._generate_movel_trajectory(robot, rox_robot, q_initial, T_des, speed_perc, final_seed) if traj is None: return EmptyGenerator() return TrajectoryMoveGenerator(robot, rox_robot, traj, self._node)
d.connect_device("robot") c = d.get_device_client("robotics_motion", 1) #p_target = np.array([-np.random.uniform(0.4,0.8),np.random.uniform(-0.1,0.1),np.random.uniform(0.0,0.4)]) p_target = np.array([ -np.random.uniform(-0.1, 0.1), np.random.uniform(-0.1, 0.1), np.random.uniform(0.0, 0.4) ]) rpy_target = np.random.randn(3) * 0.5 rpy_target[0] += np.pi R_target = rox.rpy2R(rpy_target) # p_target = np.array([-0.6, 0.0, 0.1]) # R_target = np.array([[0,1,0],[1,0,0],[0,0,-1]]) T_target = rox.Transform(R_target, p_target) r = d.get_device_client("robot", 1) geom_util = GeometryUtil(client_obj=r) p_target = geom_util.rox_transform_to_pose(T_target) print("Begin movel") gen = c.movel("robot", p_target, "world", "robot_origin_calibration", 50) while True: try: gen.Next() except RR.StopIterationException: break print("End movel")
class VisionTemplateMatching_impl(object): def __init__(self, device_manager, device_info = None, node: RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self._matched_template_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate2D") self._template_matching_result_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult2D") self._template_matching_result_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult3D") self._matched_template_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate3D") self._named_pose_with_covariance_type = self._node.GetStructureType("com.robotraconteur.geometry.NamedPoseWithCovariance") self._pose2d_dtype = self._node.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D") self._image_util = ImageUtil(node=self._node) self._geom_util = GeometryUtil(node=self._node) self.device_manager = device_manager self.device_manager.connect_device_type("tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type("com.robotraconteur.imaging.Camera") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5) def RRServiceObjectInit(self, ctx, service_path): self.service_path = service_path self.ctx = ctx def _device_added(self, local_device_name): pass def _device_removed(self, local_device_name): pass def match_template_stored_image(self, image_global_name, template_global_name, roi): var_storage = self.device_manager.get_device_client("variable_storage",1) image_var = var_storage.getf_variable_value("globals", image_global_name) image = self._image_util.compressed_image_to_array(image_var.data) template_var = var_storage.getf_variable_value("globals", template_global_name) template = self._image_util.compressed_image_to_array(template_var.data) return self._do_template_match(template, image, roi) def match_template_camera_capture(self, camera_local_device_name, template_global_name, roi): var_storage = self.device_manager.get_device_client("variable_storage",1) template_var = var_storage.getf_variable_value("globals", template_global_name) template = self._image_util.compressed_image_to_array(template_var.data) camera_device = self.device_manager.get_device_client(camera_local_device_name) image_compressed = camera_device.capture_frame_compressed() image = self._image_util.compressed_image_to_array(image_compressed) return self._do_template_match(template, image, roi) def _do_template_match(self, template, image, roi): matcher_roi = None if roi is not None: roi_x = roi.center.pose[0]["position"]["x"] roi_y = roi.center.pose[0]["position"]["y"] roi_theta = roi.center.pose[0]["orientation"] roi_w = roi.size[0]["width"] roi_h = roi.size[0]["height"] matcher_roi = (roi_x, roi_y, roi_w, roi_h, -roi_theta) # execute the image detection using opencv matcher = TemplateMatchingMultiAngleWithROI(template,image,matcher_roi) # return_result_image = True match_center, template_wh, match_angle, detection_result_img = matcher.detect_object(True) # return the pose of the object print("the object is found..:") print("center coordinates in img frame(x,y): " + str(match_center)) print("(w,h): " + str(template_wh)) print("angle: " + str(match_angle)) match_result = self._template_matching_result_2d_type() matched_template_result = self._matched_template_2d_type() centroid = np.zeros((1,),dtype=self._pose2d_dtype) centroid[0]["position"]["x"] = match_center[0] centroid[0]["position"]["y"] = match_center[1] centroid[0]["orientation"] = match_angle matched_template_result.match_centroid = centroid matched_template_result.template_size = self._geom_util.wh_to_size2d(template_wh,dtype=np.int32) matched_template_result.confidence = 0 match_result.template_matches =[matched_template_result] match_result.display_image = self._image_util.array_to_compressed_image_jpg(detection_result_img, 70) return match_result def _do_match_with_pose(self,image, template, intrinsic_calib, extrinsic_calib, object_z, roi): matcher_roi = None if roi is not None: roi_x = roi.center.pose[0]["position"]["x"] roi_y = roi.center.pose[0]["position"]["y"] roi_theta = roi.center.pose[0]["orientation"] roi_w = roi.size[0]["width"] roi_h = roi.size[0]["height"] matcher_roi = (roi_x, roi_y, roi_w, roi_h, -roi_theta) # execute the image detection using opencv matcher = TemplateMatchingMultiAngleWithROI(template,image,matcher_roi) # return_result_image = True match_center, template_wh, match_angle, detection_result_img = matcher.detect_object(True) # detection_result.width # detection_result.height x = match_center[0] y = match_center[1] theta = match_angle src = np.asarray([x,y], dtype=np.float32) src = np.reshape(src,(-1,1,2)) # Rehsape as opencv requires (N,1,2) # print(src) # Now the detection results in image frame is found, # Hence, find the detected pose in camera frame with given z distance to camera # To do that first we need to get the camera parameters # Load the camera matrix and distortion coefficients from the calibration result. mtx = intrinsic_calib.K d = intrinsic_calib.distortion_info.data dist = np.array([d.k1,d.k2,d.p1,d.p2,d.k3]) T_cam = self._geom_util.named_pose_to_rox_transform(extrinsic_calib.pose) #TODO: Figure out a better value for this object_z_cam_dist = abs(T_cam.p[2]) - object_z # Find the corresponding world pose of the detected pose in camera frame dst = cv2.undistortPoints(src,mtx,dist) # dst is Xc/Zc and Yc/Zc in the same shape of src dst = dst * float(object_z_cam_dist) * 1000.0 # Multiply by given Zc distance to find all cordinates, multiply by 1000 is because of Zc is given in meters but others are in millimeters dst = np.squeeze(dst) * 0.001 # Xc and Yc as vector # Finally the translation between the detected object center and the camera frame represented in camera frame is T = [Xc,Yc,Zc] Xc = dst[0] Yc = dst[1] Zc = float(object_z_cam_dist) T = np.asarray([Xc,Yc,Zc]) # Now lets find the orientation of the detected object with respect to camera # We are assuming +z axis is looking towards the camera and xy axes of the both object and camera are parallel planes # So the rotation matrix would be theta = np.deg2rad(theta) #convert theta from degrees to radian R_co = np.asarray([[math.cos(theta),-math.sin(theta),0],[-math.sin(theta),-math.cos(theta),0],[0,0,-1]]) T_obj_cam_frame = rox.Transform(R_co, T, "camera", "object") T_obj = T_cam * T_obj_cam_frame # TODO: Better adjustment of Z height? T_obj.p[2] = object_z ret1 = self._matched_template_3d_type() ret1.pose = self._named_pose_with_covariance_type() ret1.pose.pose = self._geom_util.rox_transform_to_named_pose(T_obj) ret1.pose.covariance= np.zeros((6,6)) ret1.confidence = 0 ret = self._template_matching_result_3d_type() ret.template_matches = [ret1] ret.display_image = self._image_util.array_to_compressed_image_jpg(detection_result_img,70) return ret def _do_match_template_world_pose(self, image, template_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi, var_storage): template_var = var_storage.getf_variable_value("globals", template_global_name) template = self._image_util.compressed_image_to_array(template_var.data) intrinsic_calib = var_storage.getf_variable_value("globals", camera_calibration_intrinsic).data extrinsic_calib = var_storage.getf_variable_value("globals", camera_calibration_extrinsic).data return self._do_match_with_pose(image,template,intrinsic_calib,extrinsic_calib,object_z_height,roi) def match_template_world_pose_stored_image(self, image_global_name, template_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi): var_storage = self.device_manager.get_device_client("variable_storage",1) image_var = var_storage.getf_variable_value("globals", image_global_name) image = self._image_util.compressed_image_to_array(image_var.data) return self._do_match_template_world_pose(image, template_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi, var_storage) def match_template_world_pose_camera_capture(self, camera_local_device_name, template_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi): var_storage = self.device_manager.get_device_client("variable_storage",1) camera_device = self.device_manager.get_device_client(camera_local_device_name,1) img_rr = camera_device.capture_frame_compressed() image = self._image_util.compressed_image_to_array(img_rr) return self._do_match_template_world_pose(image, template_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi, var_storage)
class CameraRobotCalibrationService_impl(object): def __init__(self, device_manager, device_info = None, node : RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self.device_manager = device_manager self.device_manager.connect_device_type("tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type("com.robotraconteur.robotics.robot.Robot") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5) self.robot_util = RobotUtil(self._node) self.geom_util = GeometryUtil(self._node) self.image_util = ImageUtil(self._node) def RRServiceObjectInit(self, ctx, service_path): self.service_path = service_path self.ctx = ctx def _device_added(self, local_device_name): pass def _device_removed(self, local_device_name): pass def calibrate_robot_origin(self, robot_local_device_name, camera_intrinsic_calibration_global_name, camera_extrinsic_calibration_global_name, \ image_sequence_global_name, aruco_dict, aruco_id, aruco_markersize, flange_to_marker, output_global_name): var_storage = self.device_manager.get_device_client("variable_storage",1) var_consts = var_storage.RRGetNode().GetConstants('tech.pyri.variable_storage', var_storage) variable_persistence = var_consts["VariablePersistence"] variable_protection_level = var_consts["VariableProtectionLevel"] if len(output_global_name) > 0: if len(var_storage.filter_variables("globals",output_global_name,[])) > 0: raise RR.InvalidOperationException(f"Global {output_global_name} already exists") image_sequence = [] joint_pos_sequence = [] image_sequence_vars = var_storage.getf_variable_value("globals",image_sequence_global_name) for image_var in image_sequence_vars.data.splitlines(): var2 = var_storage.getf_variable_value("globals",image_var) image_sequence.append(self.image_util.compressed_image_to_array(var2.data)) var2_tags = var_storage.getf_variable_attributes("globals", image_var) var2_state_var_name = var2_tags["system_state"] var2_state = var_storage.getf_variable_value("globals", var2_state_var_name) joint_pos = None for s in var2_state.data.devices_states[robot_local_device_name].state: if s.type == "com.robotraconteur.robotics.robot.RobotState": joint_pos = s.state_data.data.joint_position assert joint_pos is not None, "Could not find joint position in state sequence" joint_pos_sequence.append(joint_pos) cam_intrinsic_calib = var_storage.getf_variable_value("globals",camera_intrinsic_calibration_global_name).data cam_extrinsic_calib = var_storage.getf_variable_value("globals",camera_extrinsic_calibration_global_name).data mtx = cam_intrinsic_calib.K dist_rr = cam_intrinsic_calib.distortion_info.data dist = np.array([dist_rr.k1, dist_rr.k2, dist_rr.p1, dist_rr.p2, dist_rr.k3],dtype=np.float64) cam_pose = self.geom_util.named_pose_to_rox_transform(cam_extrinsic_calib.pose) robot = self.device_manager.get_device_client(robot_local_device_name,1) robot_info = robot.robot_info rox_robot = self.robot_util.robot_info_to_rox_robot(robot_info,0) # Calibrate robot_pose1, robot_pose_cov, img, calibration_error = calibrator.calibrate(image_sequence, joint_pos_sequence, aruco_dict, aruco_id, aruco_markersize, flange_to_marker, mtx, dist, cam_pose, rox_robot, robot_local_device_name) robot_pose = self._node.NewStructure("com.robotraconteur.geometry.NamedPoseWithCovariance") robot_pose.pose = robot_pose1 robot_pose.covariance = robot_pose_cov if len(output_global_name) > 0: var_storage.add_variable2("globals",output_global_name,"com.robotraconteur.geometry.NamedPoseWithCovariance", \ RR.VarValue(robot_pose,"com.robotraconteur.geometry.NamedPoseWithCovariance"), ["robot_origin_pose_calibration"], {"device": robot_local_device_name}, variable_persistence["const"], None, variable_protection_level["read_write"], \ [], f"Robot \"{robot_local_device_name}\" origin pose calibration", False) ret = RRN.NewStructure("tech.pyri.vision.robot_calibration.CameraRobotBaseCalibrateResult") ret.robot_pose = robot_pose ret.display_images = img ret.calibration_error = calibration_error return ret
import time from RobotRaconteurCompanion.Util.ImageUtil import ImageUtil from RobotRaconteurCompanion.Util.GeometryUtil import GeometryUtil import numpy as np import cv2 d = DeviceManagerClient('rr+tcp://localhost:59902?service=device_manager', autoconnect=False) d.refresh_devices(1) d.connect_device("vision_robot_calibration") calibration_service = d.get_device_client("vision_robot_calibration", 1) geom_util = GeometryUtil(client_obj=calibration_service) marker_pose = geom_util.xyz_rpy_to_pose( np.array([0.0393, -0.0091, 0.055]), np.array(np.deg2rad([90.0, 0.0, 180.0]))) ret = calibration_service.calibrate_robot_origin( "robot", "camera_intrinsic_calibration", "camera_extrinsic_calibration", "robot_calib0", "DICT_6X6_250", 150, 0.0316, marker_pose, "") # "robot_origin_calibration0" image_util = ImageUtil(client_obj=calibration_service) geom_util = GeometryUtil(client_obj=calibration_service) T = geom_util.named_pose_to_rox_transform(ret.robot_pose.pose) print(T) print(ret)
import time from RobotRaconteurCompanion.Util.ImageUtil import ImageUtil from RobotRaconteurCompanion.Util.GeometryUtil import GeometryUtil import cv2 import numpy as np d = DeviceManagerClient('rr+tcp://localhost:59902?service=device_manager', autoconnect=False) d.refresh_devices(1) d.connect_device("robotics_motion") c = d.get_device_client("robotics_motion", 1) geom_util = GeometryUtil(client_obj=c) def _run_grab(gen): while True: try: res = gen.Next() print(res) except RR.StopIterationException: break for i in range(5): pose2d_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D", c) obj_pose = np.zeros((1, ), dtype=pose2d_dtype)
print(jog_service.device_info.device.name) jog = jog_service.get_jog("robot") jog.setf_jog_mode() #for x in range(100): #jog.jog_joints3(1,1) #jog.setf_halt_mode() robot = d.get_device_client("robot", 1) robot_state, _ = robot.robot_state.PeekInValue() q_current = robot_state.joint_position robot_util = RobotUtil(client_obj=robot) rox_robot = robot_util.robot_info_to_rox_robot(robot.robot_info, 0) geom_util = GeometryUtil(client_obj=jog_service) T = rox.fwdkin(rox_robot, q_current) print(f"Current xyz = {T.p}, rpy = {np.rad2deg(rox.R2rpy(T.R))}") T2 = copy.deepcopy(T) T2.p[1] += 0.1 T3 = copy.deepcopy(T) T3.p[1] -= 0.1 pose_des = geom_util.rox_transform_to_pose(T2) pose_des2 = geom_util.rox_transform_to_pose(T3) for i in range(10): jog.jog_joints_to_pose(pose_des, 50) jog.jog_joints_to_pose(pose_des2, 50)
import time from RobotRaconteurCompanion.Util.ImageUtil import ImageUtil from RobotRaconteurCompanion.Util.GeometryUtil import GeometryUtil import cv2 d = DeviceManagerClient('rr+tcp://localhost:59902?service=device_manager', autoconnect=False) d.refresh_devices(1) d.connect_device("vision_camera_calibration") calibration_service = d.get_device_client("vision_camera_calibration", 1) ret = calibration_service.calibrate_camera_extrinsic( "camera", "camera_intrinsic_calibration", "extrinsic_image0", "chessboard", "camera_extrinsic_calibration0") # "camera_extrinsic_calibration1") image_util = ImageUtil(client_obj=calibration_service) geom_util = GeometryUtil(client_obj=calibration_service) T = geom_util.named_pose_to_rox_transform(ret.camera_pose.pose) print(T) print(ret) img = image_util.compressed_image_to_array(ret.display_image) cv2.imshow(f"img", img) cv2.waitKey() cv2.destroyAllWindows()
def do_show_new_robot_origin_calibration_dialog2(new_name: str, robot_pose, display_images, core: "PyriWebUIBrowser"): try: dialog2_html = importlib_resources.read_text( __package__, "new_calibrate_robot_origin_dialog2.html") robot_calib = core.device_manager.get_device_subscription( "vision_robot_calibration").GetDefaultClient() geom_util = GeometryUtil(client_obj=robot_calib) marker_xyz, marker_rpy, _, _ = geom_util.named_pose_to_xyz_rpy( robot_pose.pose) el = js.document.createElement('div') el.id = "new_calibrate_robot_origin_dialog2_wrapper" js.document.getElementById("wrapper").appendChild(el) def handle_hidden(*args): try: el.parentElement.removeChild(el) except: traceback.print_exc() x = f"{marker_xyz[0]:4e}" y = f"{marker_xyz[1]:4e}" z = f"{marker_xyz[2]:4e}" r_r = f"{marker_rpy[0]:4e}" r_p = f"{marker_rpy[1]:4e}" r_y = f"{marker_rpy[2]:4e}" imgs = [] i = 0 for d in display_images: d_encoded = str(base64.b64encode(d.data))[2:-1] d2 = { "id": i, "caption": f"Calibration result {i+1}", "img": "data:image/jpeg;base64," + d_encoded } del d_encoded imgs.append(d2) i += 1 #TODO: check for png? dialog = js.Vue.new( js.python_to_js({ "el": "#new_calibrate_robot_origin_dialog2_wrapper", "template": dialog2_html, "data": { "x": x, "y": y, "z": z, "r_r": r_r, "r_p": r_p, "r_y": r_y, "display_images": imgs }, "methods": { "handle_hidden": handle_hidden } })) dialog["$bvModal"].show("new_vision_camera_calibrate_robot_origin2") except: traceback.print_exc()
c = d.get_device_client("vision_template_matching", 1) bounding_box2d_type = RRN.GetStructureType( 'com.robotraconteur.geometry.BoundingBox2D', c) named_pose2d_type = RRN.GetStructureType( 'com.robotraconteur.geometry.NamedPose2D', c) pose2d_dtype = RRN.GetNamedArrayDType('com.robotraconteur.geometry.Pose2D', c) var_storage = d.get_device_client("variable_storage", 1) roi = var_storage.getf_variable_value("globals", roi_name).data #res = c.match_template_stored_image("extrinsic_image0", "test10", None) res = c.match_template_world_pose_camera_capture( "camera", template_name, "camera_calibration_intrinsic", "camera_calibration_extrinsic", 0, None) img_util = ImageUtil(client_obj=c) res_img = img_util.compressed_image_to_array(res.display_image) geom_util = GeometryUtil(client_obj=c) xyz, rpy, _, _ = geom_util.named_pose_to_xyz_rpy( res.template_matches[0].pose.pose) cv2.imshow("", res_img) cv2.waitKey() cv2.destroyAllWindows() print(res)
class VisionArucoDetection_impl(object): def __init__(self, device_manager, device_info=None, node: RR.RobotRaconteurNode = None): if node is None: self._node = RR.RobotRaconteurNode.s else: self._node = node self.device_info = device_info self.service_path = None self.ctx = None self._detected_marker = self._node.GetStructureType( "tech.pyri.vision.aruco_detection.DetectedMarker") self._aruco_detection_result = self._node.GetStructureType( "tech.pyri.vision.aruco_detection.ArucoDetectionResult") self._pose2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Pose2D") self._point2d_dtype = self._node.GetNamedArrayDType( "com.robotraconteur.geometry.Point2D") self._image_util = ImageUtil(node=self._node) self._geom_util = GeometryUtil(node=self._node) self.device_manager = device_manager self.device_manager.connect_device_type( "tech.pyri.variable_storage.VariableStorage") self.device_manager.connect_device_type( "com.robotraconteur.imaging.Camera") self.device_manager.device_added += self._device_added self.device_manager.device_removed += self._device_removed self.device_manager.refresh_devices(5) def RRServiceObjectInit(self, ctx, service_path): self.service_path = service_path self.ctx = ctx def _device_added(self, local_device_name): pass def _device_removed(self, local_device_name): pass def _do_aruco_detection(self, img, intrinsic_global_name, extrinsic_global_name, aruco_dict_str, aruco_id, aruco_markersize, roi): intrinsic_calib = None extrinsic_calib = None if intrinsic_global_name is not None and extrinsic_global_name is not None: var_storage = self.device_manager.get_device_client( "variable_storage", 0.1) intrinsic_calib = var_storage.getf_variable_value( "globals", intrinsic_global_name).data extrinsic_calib = var_storage.getf_variable_value( "globals", extrinsic_global_name).data display_img = img.copy() assert aruco_dict_str.startswith( "DICT_"), "Invalid aruco dictionary name" aruco_dict_i = getattr(aruco, aruco_dict_str) # convert string to python aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_i) aruco_params = cv2.aruco.DetectorParameters_create() aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX corners1, ids1, rejected = cv2.aruco.detectMarkers( img, aruco_dict, parameters=aruco_params) if corners1 is None: corners1 = [] if ids1 is None: ids1 = [] if aruco_id < 0: corners2 = corners1 ids2 = ids1 else: corners2 = [] ids2 = [] for id2, corner2 in zip(ids1, corners1): if id2 == aruco_id: corners2.append(corner2) ids2.append(id2) ids2 = np.array(ids2) if roi is None: corners = corners2 ids = ids2 else: roi_x = roi.center.pose[0]["position"]["x"] roi_y = roi.center.pose[0]["position"]["y"] roi_theta = roi.center.pose[0]["orientation"] roi_w = roi.size[0]["width"] roi_h = roi.size[0]["height"] geom_roi1 = shapely.geometry.box(-roi_w / 2., -roi_h / 2., roi_w / 2., roi_h / 2., ccw=True) geom_roi2 = shapely.affinity.translate(geom_roi1, xoff=roi_x, yoff=roi_y) geom_roi = shapely.affinity.rotate(geom_roi2, roi_theta, origin='centroid', use_radians=True) corners = [] ids = [] for id3, corner3 in zip(ids2, corners2): centroid = shapely.geometry.Polygon([ shapely.geometry.Point(corner3[0, i, 0], corner3[0, i, 1]) for i in range(4) ]) if geom_roi.contains(centroid): corners.append(corner3) ids.append(id3) ids = np.array(ids) roi_outline = np.array([geom_roi.exterior.coords], dtype=np.int32) display_img = cv2.polylines(display_img, roi_outline, True, color=(255, 255, 0)) if len(ids) > 0: display_img = aruco.drawDetectedMarkers(display_img, corners, ids) poses = None if intrinsic_calib is not None and extrinsic_calib is not None: poses = [] mtx = intrinsic_calib.K d = intrinsic_calib.distortion_info.data dist = np.array([d.k1, d.k2, d.p1, d.p2, d.k3]) T_cam = self._geom_util.named_pose_to_rox_transform( extrinsic_calib.pose) for id4, corner4 in zip(ids, corners): rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers( corner4, aruco_markersize, mtx, dist) display_img = cv2.aruco.drawAxis(display_img, mtx, dist, rvec, tvec, 0.05) # R_marker1 = cv2.Rodrigues(rvec.flatten())[0] # TODO: 3D pose estimation from rvec is very innaccurate. Use a simple trigonometry # to estimate the Z rotation of the tag # compute vectors from opposite corners v1 = corner4[0, 2, :] - corner4[0, 0, :] v2 = corner4[0, 3, :] - corner4[0, 1, :] # Use atan2 on each vector and average theta1 = (np.arctan2(v1[1], v1[0]) - np.deg2rad(45)) % (2. * np.pi) theta2 = (np.arctan2(v2[1], v2[0]) - np.deg2rad(135)) % (2. * np.pi) theta = (theta1 + theta2) / 2. R_marker1 = rox.rot([1., 0., 0.], np.pi) @ rox.rot( [0., 0., -1.], theta) p_marker1 = tvec.flatten() T_marker1 = rox.Transform(R_marker1, p_marker1, "camera", f"aruco{int(id4)}") T_marker = T_cam * T_marker1 rr_marker_pose = self._geom_util.rox_transform_to_named_pose( T_marker) poses.append(rr_marker_pose) ret_markers = [] if poses is None: poses = [None] * len(ids) for id_, corner, pose in zip(ids, corners, poses): m = self._detected_marker() m.marker_id = int(id_) m.marker_corners = np.zeros((4, ), dtype=self._point2d_dtype) for i in range(4): m.marker_corners[i] = self._geom_util.xy_to_point2d( corner[0, i, :]) m.marker_pose = pose ret_markers.append(m) ret = self._aruco_detection_result() ret.detected_markers = ret_markers ret.display_image = self._image_util.array_to_compressed_image_jpg( display_img, 70) return ret def detect_aruco_stored_image(self, image_global_name, camera_calibration_intrinsic, camera_calibration_extrinsic, aruco_dict, aruco_id, aruco_markersize, roi): if len(camera_calibration_intrinsic) == 0: camera_calibration_intrinsic = None if len(camera_calibration_extrinsic) == 0: camera_calibration_extrinsic = None var_storage = self.device_manager.get_device_client( "variable_storage", 0.1) img_rr = var_storage.getf_variable_value("globals", image_global_name) img = self._image_util.compressed_image_to_array(img_rr.data) return self._do_aruco_detection(img, camera_calibration_intrinsic, camera_calibration_extrinsic, aruco_dict, aruco_id, aruco_markersize, roi) def detect_aruco_camera_capture(self, camera_local_device_name, camera_calibration_intrinsic, camera_calibration_extrinsic, aruco_dict, aruco_id, aruco_markersize, roi): if len(camera_calibration_intrinsic) == 0: camera_calibration_intrinsic = None if len(camera_calibration_extrinsic) == 0: camera_calibration_extrinsic = None camera_device = self.device_manager.get_device_client( camera_local_device_name, 1) img_rr = camera_device.capture_frame_compressed() img = self._image_util.compressed_image_to_array(img_rr) return self._do_aruco_detection(img, camera_calibration_intrinsic, camera_calibration_extrinsic, aruco_dict, aruco_id, aruco_markersize, roi)
def calibrate(images, joint_poses, aruco_dict, aruco_id, aruco_markersize, flange_to_marker, mtx, dist, cam_pose, rox_robot, robot_local_device_name): """ Apply extrinsic camera calibration operation for images in the given directory path using opencv aruco marker detection, the extrinsic marker poses given in a json file, and the given intrinsic camera parameters.""" assert aruco_dict.startswith("DICT_"), "Invalid aruco dictionary name" aruco_dict = getattr(aruco, aruco_dict) # convert string to python aruco_dict = cv2.aruco.Dictionary_get(aruco_dict) aruco_params = cv2.aruco.DetectorParameters_create() i = 0 imgs_out = [] geom_util = GeometryUtil() image_util = ImageUtil() object_points = [] image_points = [] for img, joints in zip(images, joint_poses): # Find the aruco tag corners # corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=aruco_params,cameraMatrix=mtx, distCoeff=dist) corners, ids, rejected = cv2.aruco.detectMarkers( img, aruco_dict, parameters=aruco_params) # #debug # print(str(type(corners))) # <class 'list'> # print(str(corners)) # list of numpy arrays of corners # print(str(type(ids))) # <class 'numpy.ndarray'> # print(str(ids)) if len(corners) > 0: # Only find the id that we desire indexes = np.flatnonzero(ids.flatten() == aruco_id).tolist() corners = [corners[index] for index in indexes] ids = np.asarray([ids[index] for index in indexes]) # #debug # print(str(type(corners))) # <class 'list'> # print(str(corners)) # list of numpy arrays of corners # print(str(type(ids))) # <class 'numpy.ndarray'> # print(str(ids)) if len(ids) > 0: # if there exist at least one id that we desire # Select the first detected one, discard the others corners = corners[0] # now corners is 1 by 4 # # extract the marker corners (which are always returned # # in top-left, top-right, bottom-right, and bottom-left # # order) # corners = corners.reshape((4, 2)) # (topLeft, topRight, bottomRight, bottomLeft) = corners # Estimate the pose of the detected marker in camera frame rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers( corners, aruco_markersize, mtx, dist) # # Debug: Show the detected tag and axis in the image # # # cv2.aruco.drawDetectedMarkers(img, corners) # Draw A square around the markers (Does not work) img1 = img.copy() img_out = cv2.aruco.drawAxis(img1, mtx, dist, rvec, tvec, aruco_markersize * 0.75) # Draw Axis imgs_out.append(img_out) transform_base_2_flange = rox.fwdkin(rox_robot, joints) transform_flange_2_marker = geom_util.pose_to_rox_transform( flange_to_marker) transform_base_2_marker = transform_base_2_flange * transform_flange_2_marker transform_base_2_marker_corners = _marker_corner_poses( transform_base_2_marker, aruco_markersize) # Structure of this disctionary is "filename":[[R_base2marker],[T_base2marker],[R_cam2marker],[T_cam2marker]] for j in range(4): object_points.append(transform_base_2_marker_corners[j].p) image_points.append(corners[0, j]) #pose_pairs_dict[i] = (transform_base_2_marker_corners, corners) i += 1 object_points_np = np.array(object_points, dtype=np.float64) image_points_np = np.array(image_points, dtype=np.float32) # Finally execute the calibration retval, rvec, tvec = cv2.solvePnP(object_points_np, image_points_np, mtx, dist) R_cam2base = cv2.Rodrigues(rvec)[0] T_cam2base = tvec # Add another display image of marker at robot base img_out = cv2.aruco.drawAxis(img, mtx, dist, cv2.Rodrigues(R_cam2base)[0], T_cam2base, aruco_markersize * 0.75) # Draw Axis imgs_out.append(img_out) rox_transform_cam2base = rox.Transform(R_cam2base, T_cam2base, cam_pose.parent_frame_id, robot_local_device_name) rox_transform_world2base = cam_pose * rox_transform_cam2base #R_base2cam = R_cam2base.T #T_base2cam = - R_base2cam @ T_cam2base R_base2cam = rox_transform_world2base.inv().R T_base2cam = rox_transform_world2base.inv().p #debug print("FINAL RESULTS: ") print("str(R_cam2base)") # print(str(type(R_cam2base))) print(str(R_cam2base)) print("str(T_cam2base)") # print(str(type(T_cam2base.flatten()))) print(str(T_cam2base)) print("str(R_base2cam)") # print(str(type(R_base2cam))) print(str(R_base2cam)) print("str(T_base2cam)") # print(str(type(T_base2cam.flatten()))) print(str(T_base2cam)) pose_res = geom_util.rox_transform_to_named_pose(rox_transform_world2base) cov = np.eye(6) * 1e-5 imgs_out2 = [ image_util.array_to_compressed_image_jpg(i, 70) for i in imgs_out ] return pose_res, cov, imgs_out2, 0.0
from RobotRaconteur.Client import * import time from RobotRaconteurCompanion.Util.ImageUtil import ImageUtil from RobotRaconteurCompanion.Util.GeometryUtil import GeometryUtil import cv2 import numpy as np d = DeviceManagerClient('rr+tcp://localhost:59902?service=device_manager', autoconnect=False) d.refresh_devices(1) d.connect_device("vision_aruco_detection") c = d.get_device_client("vision_aruco_detection",1) geom_util = GeometryUtil(client_obj = c) b = RRN.NewStructure("com.robotraconteur.geometry.BoundingBox2D", c) center = RRN.NewStructure("com.robotraconteur.geometry.NamedPose2D", c) pose2d_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D", c) size2d_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Size2D", c) center.pose = np.zeros((1,),dtype=pose2d_dtype) center.pose[0]["position"]["x"] = 990 center.pose[0]["position"]["y"] = 64 center.pose[0]["orientation"] = np.deg2rad(10) b.center = center size = np.zeros((1,),dtype=size2d_dtype) size[0]["width"] = 100 size[0]["height"] = 100 b.size = size