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)
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"
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)
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) for img1 in ret.display_images: img = image_util.compressed_image_to_array(img1) cv2.imshow(f"img", img) cv2.waitKey() cv2.destroyAllWindows()
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()
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)
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
class RoboticsMotion_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._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 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 _generate_movej_trajectory(self, robot_client, rox_robot, q_initial, q_final, speed_perc): JointTrajectoryWaypoint = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint", robot_client) JointTrajectory = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectory", robot_client) dof = len(rox_robot.joint_type) N_samples = math.ceil( np.max(np.abs(q_final - q_initial)) * 0.2 * 180 / np.pi) N_samples = np.max((N_samples, 20)) ss = np.linspace(0, 1, N_samples) ss = np.linspace(0, 1, N_samples) way_pts = np.zeros((N_samples, dof), dtype=np.float64) for i in range(dof): way_pts[:, i] = np.linspace(q_initial[i], q_final[i], N_samples) vlims = rox_robot.joint_vel_limit alims = rox_robot.joint_acc_limit path = ta.SplineInterpolator(ss, way_pts) pc_vel = constraint.JointVelocityConstraint(vlims * 0.95 * (speed_perc / 100.0)) pc_acc = constraint.JointAccelerationConstraint(alims) instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel") jnt_traj = instance.compute_trajectory() if jnt_traj is None: return None ts_sample = np.linspace(0, jnt_traj.duration, N_samples) qs_sample = jnt_traj(ts_sample) waypoints = [] for i in range(N_samples): wp = JointTrajectoryWaypoint() wp.joint_position = qs_sample[i, :] wp.time_from_start = ts_sample[i] waypoints.append(wp) traj = JointTrajectory() traj.joint_names = rox_robot.joint_names traj.waypoints = waypoints return traj def movej(self, robot_local_device_name, q_final, speed_perc): # Convert to radians q_final = np.deg2rad(q_final) robot = self.device_manager.get_device_client(robot_local_device_name) 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_movej_trajectory(robot, rox_robot, q_initial, q_final, speed_perc) if traj is None: return EmptyGenerator() return TrajectoryMoveGenerator(robot, rox_robot, traj, self._node) def _generate_movel_trajectory(self, robot_client, rox_robot, initial_q_or_T, T_final, speed_perc, q_final_seed): JointTrajectoryWaypoint = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint", robot_client) JointTrajectory = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectory", robot_client) dof = len(rox_robot.joint_type) if isinstance(initial_q_or_T, rox.Robot): T_initial = initial_q_or_T q_initial = invkin.update_ik_info3(rox_robot, initial_q_or_T, np.random.randn((dof, ))) else: q_initial = initial_q_or_T T_initial = rox.fwdkin(rox_robot, q_initial) p_diff = T_final.p - T_initial.p R_diff = np.transpose(T_initial.R) @ T_final.R k, theta = rox.R2rot(R_diff) N_samples_translate = np.linalg.norm(p_diff) * 100.0 N_samples_rot = np.abs(theta) * 0.25 * 180 / np.pi N_samples_translate = np.linalg.norm(p_diff) * 100.0 N_samples_rot = np.abs(theta) * 0.2 * 180 / np.pi N_samples = math.ceil(np.max((N_samples_translate, N_samples_rot, 20))) ss = np.linspace(0, 1, N_samples) if q_final_seed is None: q_final, res = invkin.update_ik_info3(rox_robot, T_final, q_initial) else: q_final, res = invkin.update_ik_info3(rox_robot, T_final, q_final_seed) #assert res, "Inverse kinematics failed" way_pts = np.zeros((N_samples, dof), dtype=np.float64) way_pts[0, :] = q_initial for i in range(1, N_samples): s = float(i) / (N_samples - 1) R_i = T_initial.R @ rox.rot(k, theta * s) p_i = (p_diff * s) + T_initial.p T_i = rox.Transform(R_i, p_i) #try: # q, res = invkin.update_ik_info3(rox_robot, T_i, way_pts[i-1,:]) #except AssertionError: ik_seed = (1.0 - s) * q_initial + s * q_final q, res = invkin.update_ik_info3(rox_robot, T_i, ik_seed) assert res, "Inverse kinematics failed" way_pts[i, :] = q vlims = rox_robot.joint_vel_limit alims = rox_robot.joint_acc_limit path = ta.SplineInterpolator(ss, way_pts) pc_vel = constraint.JointVelocityConstraint(vlims * 0.95 * speed_perc / 100.0) pc_acc = constraint.JointAccelerationConstraint(alims) instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel") jnt_traj = instance.compute_trajectory() if jnt_traj is None: return None ts_sample = np.linspace(0, jnt_traj.duration, N_samples) qs_sample = jnt_traj(ts_sample) waypoints = [] for i in range(N_samples): wp = JointTrajectoryWaypoint() wp.joint_position = qs_sample[i, :] wp.time_from_start = ts_sample[i] waypoints.append(wp) traj = JointTrajectory() traj.joint_names = rox_robot.joint_names traj.waypoints = waypoints return traj 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) def _generate_movel_trajectory_tool_j_range(self, robot_client, rox_robot, initial_q_or_T, T_final, speed_perc, final_seed): # Rotate joint 6 initial position to try to converge try: return self._generate_movel_trajectory(robot_client, rox_robot, initial_q_or_T, T_final, speed_perc, final_seed) except AssertionError: if not isinstance(initial_q_or_T, np.ndarray): raise #raise p = final_seed p1 = float(p[-1]) p2 = float(p[-1]) while True: if p1 > rox_robot.joint_upper_limit[ -1] and p2 < rox_robot.joint_lower_limit[-1]: assert False, "Could not solve inverse kinematics for grab or place operation" p1 = p1 + np.deg2rad(30) if p1 <= rox_robot.joint_upper_limit[-1]: p[-1] = p1 try: res = self._generate_movel_trajectory( robot_client, rox_robot, initial_q_or_T, T_final, speed_perc, p) return res except AssertionError: pass p2 = p2 - np.deg2rad(30) if p2 >= rox_robot.joint_lower_limit[-1]: p[-1] = p2 try: res = self._generate_movel_trajectory( robot_client, rox_robot, initial_q_or_T, T_final, speed_perc, p) return res except AssertionError: pass def _do_grab_place_object_planar(self, robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_x, object_y, object_theta, z_offset_before, z_offset_grab, speed_perc, grab): var_storage = self.device_manager.get_device_client( "variable_storage", 0.1) robot = self.device_manager.get_device_client(robot_local_device_name) tool = self.device_manager.get_device_client(tool_local_device_name) reference_pose = var_storage.getf_variable_value( "globals", reference_pose_global_name) robot_origin_calib = var_storage.getf_variable_value( "globals", robot_origin_calib_global_name) T_robot = self._geom_util.named_pose_to_rox_transform( robot_origin_calib.data.pose) robot_info = robot.robot_info rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0) # The object's pose in world coordinates T_object = rox.Transform(rox.rot([0., 0., 1.], object_theta), [object_x, object_y, 0.]) # The robot's reference pose in its own frame T_reference_pose_r = rox.fwdkin(rox_robot, np.deg2rad(reference_pose.data)) # The robot's reference pose in world coordinates T_reference_pose = T_robot * T_reference_pose_r # The nominal grab pose copy T_grab_pose_n = copy.deepcopy(T_reference_pose) # Translate the reference pose in x and y to the nominal grab point T_grab_pose_n.p[0] = T_object.p[0] T_grab_pose_n.p[1] = T_object.p[1] # Rotate the reference pose to align with the object T_grab_pose_n.R = T_object.R @ T_grab_pose_n.R # Before pose T_grab_pose_before = copy.deepcopy(T_grab_pose_n) T_grab_pose_before.p[2] += z_offset_before # Transform to robot frame T_grab_pose_before_r = T_robot.inv() * T_grab_pose_before # Grab pose T_grab_pose = copy.deepcopy(T_grab_pose_n) T_grab_pose.p[2] += z_offset_grab # Transform to robot frame T_grab_pose_r = T_robot.inv() * T_grab_pose robot_state, _ = robot.robot_state.PeekInValue() q_current = robot_state.joint_position ## Compute inverse kinematics # q_grab_before, res = invkin.update_ik_info3(rox_robot, T_grab_pose_before_r, q_current) # assert res, "Invalid setpoint: invkin did not converge" # q_grab, res = invkin.update_ik_info3(rox_robot, T_grab_pose_r, q_current) # assert res, "Invalid setpoint: invkin did not converge" # print(f"q_grab_before: {q_grab_before}") # print(f"q_grab: {q_grab}") # print() final_seed = np.deg2rad(reference_pose.data) traj_before = self._generate_movel_trajectory_tool_j_range( robot, rox_robot, q_current, T_grab_pose_before_r, speed_perc, final_seed) q_init_grab = traj_before.waypoints[-1].joint_position traj_grab = self._generate_movel_trajectory_tool_j_range( robot, rox_robot, q_init_grab, T_grab_pose_r, speed_perc, q_init_grab) q_init_after = traj_grab.waypoints[-1].joint_position traj_after = self._generate_movel_trajectory_tool_j_range( robot, rox_robot, q_init_after, T_grab_pose_before_r, speed_perc, q_init_grab) gen = PickPlaceMotionGenerator(robot, rox_robot, tool, traj_before, traj_grab, traj_after, grab, self._node) # robot.jog_freespace(q_grab_before,max_velocity,True) # time.sleep(0.5) # robot.jog_freespace(q_grab,max_velocity*.25,True) # time.sleep(0.5) # robot.jog_freespace(q_grab_before,max_velocity*.25,True) return gen def grab_object_planar(self, robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_world_pose, z_offset_before, z_offset_grab, speed_perc): object_x = object_world_pose["position"]["x"] object_y = object_world_pose["position"]["y"] object_theta = object_world_pose["orientation"] return self._do_grab_place_object_planar( robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_x, object_y, object_theta, z_offset_before, z_offset_grab, speed_perc, True) def grab_object_planar2(self, robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_world_pose, z_offset_before, z_offset_grab, speed_perc): object_x = object_world_pose["position"]["x"] object_y = object_world_pose["position"]["y"] object_rpy = self._geom_util.quaternion_to_rpy( object_world_pose["orientation"]) assert np.abs( object_rpy[0]) < np.deg2rad(15), "Object is not flat on surface!" assert np.abs( object_rpy[1]) < np.deg2rad(15), "Object is not flat on surface!" object_theta = object_rpy[2] return self._do_grab_place_object_planar( robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_x, object_y, object_theta, z_offset_before, z_offset_grab, speed_perc, True) def place_object_planar(self, robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_world_pose, z_offset_before, z_offset_grab, speed_perc): object_x = object_world_pose["position"]["x"] object_y = object_world_pose["position"]["y"] object_theta = object_world_pose["orientation"] return self._do_grab_place_object_planar( robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_x, object_y, object_theta, z_offset_before, z_offset_grab, speed_perc, False) def place_object_planar2(self, robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_world_pose, z_offset_before, z_offset_grab, speed_perc): object_x = object_world_pose["position"]["x"] object_y = object_world_pose["position"]["y"] object_rpy = self._geom_util.quaternion_to_rpy( object_world_pose["orientation"]) assert np.abs(object_rpy[0]) < np.deg2rad( 5), "Target location is not flat on surface!" assert np.abs(object_rpy[1]) < np.deg2rad( 5), "Target location is not flat on surface!" object_theta = object_rpy[2] return self._do_grab_place_object_planar( robot_local_device_name, tool_local_device_name, robot_origin_calib_global_name, reference_pose_global_name, object_x, object_y, object_theta, z_offset_before, z_offset_grab, speed_perc, False) def move_joint_trajectory(self, robot_local_device_name, trajectory, speed_perc): if speed_perc != 100.0: t_scale = 100.0 / speed_perc traj2 = copy.deepcopy(trajectory) for wp in traj2.waypoints: wp.time_from_start *= t_scale trajectory = traj2 robot = self.device_manager.get_device_client(robot_local_device_name) robot_info = robot.robot_info rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0) return TrajectoryMoveGenerator(robot, rox_robot, trajectory, self._node)
intrinsic_var = var_storage.getf_variable_value( "globals", "camera_calibration_extrinsic") robot_var = var_storage.getf_variable_value("globals", "robot_origin_calibration") calib = extrinsic_var.data print("Camera Matrix") print(calib.K) d = calib.distortion_info.data dist = np.array([d.k1, d.k2, d.p1, d.p2, d.k3]) print("Camera distortion") print(dist) geom_util = GeometryUtil(client_obj=var_storage) T_cam = geom_util.named_pose_to_rox_transform(intrinsic_var.data.pose) T_rob = geom_util.named_pose_to_rox_transform(robot_var.data.pose) print("T_cam") print(T_cam.R) print(T_cam.p) print("T_rob") print(T_rob.R) print(T_rob.p) print("T") T = (T_cam.inv() * T_rob).inv() print(T.R) print(T.p)