def _get_grasp_policy(self): """ Get the grasping policy based on the model Returns ---------- grasping_policy: type `gqcnn.grasping.policy.policy.GraspingPolicy` Grasping policy to use. """ # Get grasping policy if "FC" in self.model: if self.policy_config["type"] == "fully_conv_suction": self.grasping_policy = FullyConvolutionalGraspingPolicySuction( self.policy_config) elif self.policy_config["type"] == "fully_conv_pj": self.grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw( self.policy_config) else: raise ValueError( "Invalid fully-convolutional policy type: {}".format( self.policy_config["type"])) else: policy_type = "cem" if "type" in self.policy_config: policy_type = self.policy_config["type"] if policy_type == "ranking": self.grasping_policy = RobustGraspingPolicy(self.policy_config) elif policy_type == "cem": self.grasping_policy = CrossEntropyRobustGraspingPolicy( self.policy_config) else: raise ValueError("Invalid policy type: {}".format(policy_type))
if policy_config["type"] == "fully_conv_suction": policy = FullyConvolutionalGraspingPolicySuction(policy_config) elif policy_config["type"] == "fully_conv_pj": policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_config) else: raise ValueError( "Invalid fully-convolutional policy type: {}".format( policy_config["type"])) else: policy_type = "cem" if "type" in policy_config: policy_type = policy_config["type"] if policy_type == "ranking": policy = RobustGraspingPolicy(policy_config) elif policy_type == "cem": policy = CrossEntropyRobustGraspingPolicy(policy_config) else: raise ValueError("Invalid policy type: {}".format(policy_type)) # Query policy. policy_start = time.time() action = policy(state) logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Vis final grasp. if policy_config["vis"]["final_grasp"]: vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth, vmin=policy_config["vis"]["vmin"], vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True)
FullyConvolutionalGraspingPolicySuction(policy_cfg) elif policy_cfg["type"] == "fully_conv_pj": grasping_policy = \ FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg) else: raise ValueError( "Invalid fully-convolutional policy type: {}".format( policy_cfg["type"])) else: policy_type = "cem" if "type" in policy_cfg: policy_type = policy_cfg["type"] if policy_type == "ranking": grasping_policy = RobustGraspingPolicy(policy_cfg) elif policy_type == "cem": grasping_policy = CrossEntropyRobustGraspingPolicy(policy_cfg) else: raise ValueError("Invalid policy type: {}".format(policy_type)) # Create a grasp planner. grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher) # Initialize the ROS service. grasp_planning_service = rospy.Service("grasp_planner", GQCNNGraspPlanner, grasp_planner.plan_grasp) grasp_planning_service_bb = rospy.Service("grasp_planner_bounding_box", GQCNNGraspPlannerBoundingBox, grasp_planner.plan_grasp_bb) grasp_planning_service_segmask = rospy.Service( "grasp_planner_segmask", GQCNNGraspPlannerSegmask,
#%% rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/depth/image_rect_raw/') #rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/depth/color/points') #rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/aligned_depth_to_color/image_raw/') #%% intr = rosco.get_camera_intrinsics('/camera/color/camera_info') #%% intr #rosco.threaded_synced_rgbd_cb('/camera/color/image_raw/', '/camera/depth/image_rect_raw/') #%% camera_int = CameraIntrinsics(frame='pcl', fx=intr['fx'], fy=intr['fy'], cx=intr['cx'], cy=intr['cy'], height=intr['height'], width=intr['width']) #%% cfg = YamlConfig('cfg/gqcnn_pj_dbg.yaml') #%% grasp_policy = CrossEntropyRobustGraspingPolicy(cfg['policy']) # grasp_policy = RobustGraspingPolicy(cfg['policy']) #%% img = rosco.rgb d = rosco.depth #%% plt.imshow(img) #%% plt.imshow(d) #print(img) #print(d) #%% color_im = ColorImage(img.astype(np.uint8), encoding="bgr8", frame='pcl') depth_im = DepthImage(d.astype(np.float32), frame='pcl') color_im = color_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor']) depth_im = depth_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
def configure(self, model_dir, fully_conv): """Configure model and grasping policy Args: model_config_file (str): path to model configuration file of type config.json fully_conv (bool): if fully-convolutional network grasp_offset (np.array): 3-dim array of values to adjust every pose in the eef ref frame """ # read model config.json file try: model_config = json.load(open(os.path.join(model_dir, "config.json"), "r")) except Exception: raise ValueError( "Cannot open model config file {}".format(os.path.join(model_dir, "config.json"))) # --- set gripper mode --- # if "gripper_mode" in model_config["gqcnn"]: gripper_mode = model_config["gqcnn"]["gripper_mode"] else: input_data_mode = model_config["gqcnn_config"]["input_data_mode"] if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW elif input_data_mode == "parallel_jaw": gripper_mode = GripperMode.PARALLEL_JAW else: raise ValueError( "Input data mode {} not supported!".format(input_data_mode)) if (gripper_mode != GripperMode.LEGACY_PARALLEL_JAW and gripper_mode != GripperMode.PARALLEL_JAW): raise ValueError("Gripper mode {} not supported!".format(gripper_mode)) # --- Load config --- # config_file = "cfg/fc_gqcnn_pj.yaml" if fully_conv else "cfg/gqcnn_pj.yaml" try: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), config_file) except Exception: print("cannot open configuration file {}".format( os.path.join(os.path.dirname(os.path.realpath(__file__)), config_file))) # Read config self.cfg = YamlConfig(config_filename) # --- Define the grasping policy --- # policy_cfg = self.cfg["policy"] policy_cfg["metric"]["gqcnn_model"] = model_dir policy_type = "cem" if "type" in policy_cfg: policy_type = policy_cfg["type"] if policy_type == "fully_conv_pj": self.grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg) elif policy_type == "ranking": self.grasping_policy = RobustGraspingPolicy(policy_cfg) elif policy_type == "cem": self.grasping_policy = CrossEntropyRobustGraspingPolicy(policy_cfg) else: fc = "fully-convolutional" if fully_conv else "" raise ValueError("Invalid {} policy type: {}".format(fc, policy_type)) # --- Set minimum input dimensions --- # if policy_type == "fully_conv_pj": self.min_width = self.cfg["policy"]["gqcnn_recep_w"] self.min_height = self.cfg["policy"]["gqcnn_recep_h"] else: pad = max(math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_width"]) / 2)), math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_height"]) / 2))) self.min_width = 2 * pad + self.cfg["policy"]["metric"]["crop_width"] self.min_height = 2 * pad + self.cfg["policy"]["metric"]["crop_height"]
def prepare_dexnet(self): # Get configs. model_config = json.load( open(os.path.join(self.model_path, "config.json"), "r")) # self.model_config = model_config # try: # gqcnn_config = model_config["gqcnn"] # gripper_mode = gqcnn_config["gripper_mode"] # except KeyError: # gqcnn_config = model_config["gqcnn_config"] # input_data_mode = gqcnn_config["input_data_mode"] # if input_data_mode == "tf_image": # gripper_mode = GripperMode.LEGACY_PARALLEL_JAW # elif input_data_mode == "tf_image_suction": # gripper_mode = GripperMode.LEGACY_SUCTION # elif input_data_mode == "suction": # gripper_mode = GripperMode.SUCTION # elif input_data_mode == "multi_suction": # gripper_mode = GripperMode.MULTI_SUCTION # elif input_data_mode == "parallel_jaw": # gripper_mode = GripperMode.PARALLEL_JAW # else: # raise ValueError( # "Input data mode {} not supported!".format(input_data_mode)) # Read config. config = YamlConfig(self.config_filename) self.inpaint_rescale_factor = config["inpaint_rescale_factor"] policy_config = config["policy"] self.policy_config = policy_config # Make relative paths absolute. if "gqcnn_model" in policy_config["metric"]: policy_config["metric"]["gqcnn_model"] = self.model_path if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): policy_config["metric"]["gqcnn_model"] = os.path.join( os.path.dirname(os.path.realpath(__file__)), policy_config["metric"]["gqcnn_model"]) # Setup sensor. self.camera_intr = CameraIntrinsics.load(self.camera_intr_filename) # Init policy. # Set input sizes for fully-convolutional policy. if self.fully_conv: policy_config["metric"]["fully_conv_gqcnn_config"][ "im_height"] = self.height policy_config["metric"]["fully_conv_gqcnn_config"][ "im_width"] = self.width if self.fully_conv: # TODO(vsatish): We should really be doing this in some factory policy. if policy_config["type"] == "fully_conv_suction": self.policy = FullyConvolutionalGraspingPolicySuction( policy_config) elif policy_config["type"] == "fully_conv_pj": self.policy = FullyConvolutionalGraspingPolicyParallelJaw( policy_config) else: raise ValueError( "Invalid fully-convolutional policy type: {}".format( policy_config["type"])) else: policy_type = "cem" if "type" in policy_config: policy_type = policy_config["type"] if policy_type == "ranking": self.policy = RobustGraspingPolicy(policy_config) elif policy_type == "cem": self.policy = CrossEntropyRobustGraspingPolicy(policy_config) else: raise ValueError("Invalid policy type: {}".format(policy_type))
def __init__(self, config: Config, detector=None): self.config = config self.policy_name = config.policy_name self.max_path_length = config.max_path_length self.elevation_max = config.elevation_max self.elevation_min = config.elevation_min self.roll_max = config.roll_max self.roll_min = config.roll_min self.save_video = False #True #False if self.save_video: self.imgs = [] #grasp_sampler_config = grasp_sampler.GraspSampler.Config() #self.grasp_sampler = grasp_sampler.GraspSampler(grasp_sampler_config) model_name = "gqcnn_example_pj_fish" #"GQCNN-4.0-PJ" fully_conv = False gqcnn_dir = "/home/htung/2020/gqcnn/" # Set model if provided. model_dir = os.path.join(gqcnn_dir, "models") model_path = os.path.join(model_dir, model_name) config_filename = None # Get configs. model_config = json.load( open(os.path.join(model_path, "config.json"), "r")) try: gqcnn_config = model_config["gqcnn"] gripper_mode = gqcnn_config["gripper_mode"] except KeyError: gqcnn_config = model_config["gqcnn_config"] input_data_mode = gqcnn_config["input_data_mode"] if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW elif input_data_mode == "tf_image_suction": gripper_mode = GripperMode.LEGACY_SUCTION elif input_data_mode == "suction": gripper_mode = GripperMode.SUCTION elif input_data_mode == "multi_suction": gripper_mode = GripperMode.MULTI_SUCTION elif input_data_mode == "parallel_jaw": # this is picked gripper_mode = GripperMode.PARALLEL_JAW else: raise ValueError("Input data mode {} not supported!".format( input_data_mode)) config_filename = os.path.join(gqcnn_dir, "cfg/examples/gqcnn_pj.yaml") # Read config. config = YamlConfig(config_filename) inpaint_rescale_factor = config["inpaint_rescale_factor"] policy_config = config["policy"] # original_gripper_width = 0.05 original_gripper_width = policy_config["gripper_width"] self.real_gripper_width = 0.112 #0.15 #0.112 self.rescale_factor = original_gripper_width / self.real_gripper_width #self.config.camera_fov_y = self.config.camera_fov_y * self.rescale_factor #policy_config["gripper_width"] = 0.112 # gripper distance to the grasp point # min_depth_offset = config['policy']["sampling"]["min_depth_offset"] = 0.015 # max_depth_offset = config['policy']["sampling"]["max_depth_offset"] = 0.05 # Make relative paths absolute. if "gqcnn_model" in policy_config["metric"]: policy_config["metric"]["gqcnn_model"] = model_path if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): policy_config["metric"]["gqcnn_model"] = os.path.join( os.path.dirname(os.path.realpath(__file__)), "..", policy_config["metric"]["gqcnn_model"]) # Set input sizes for fully-convolutional policy. if fully_conv: policy_config["metric"]["fully_conv_gqcnn_config"][ "im_height"] = depth_im.shape[0] policy_config["metric"]["fully_conv_gqcnn_config"][ "im_width"] = depth_im.shape[1] # Init policy. self.policy_config = policy_config self.policy_config["vis"]["vmin"] = 0 self.policy_config["vis"]["vmax"] = 0.5 if self.config.data_collection_mode: self.policy_config["num_iters"] = 0 self.policy_config["random_sample"] = True self.gqcnn_image_size = 96 import collections data_config = collections.OrderedDict() data_config['datapoints_per_file'] = 50 data_config['fields'] = collections.OrderedDict() data_config['fields']["grasp_metrics"] = dict() data_config['fields']["grasp_metrics"]['dtype'] = "float32" data_config['fields']["grasps"] = dict() data_config['fields']["grasps"]['dtype'] = "float32" data_config['fields']["grasps"]['height'] = 6 data_config['fields']["split"] = dict() data_config['fields']["split"]['dtype'] = "uint8" data_config['fields']["tf_depth_ims"] = dict() data_config['fields']["tf_depth_ims"]['dtype'] = "float32" data_config['fields']["tf_depth_ims"][ 'height'] = self.gqcnn_image_size data_config['fields']["tf_depth_ims"][ 'width'] = self.gqcnn_image_size data_config['fields']["tf_depth_ims"]['channels'] = 1 from autolab_core import TensorDataset self.tensordata = TensorDataset(self.config.save_data_name, data_config) #import ipdb; ipdb.set_trace() policy_type = "cem" if "type" in policy_config: policy_type = policy_config["type"] if policy_type == "ranking": self.policy = RobustGraspingPolicy(policy_config) elif policy_type == "cem": self.policy = CrossEntropyRobustGraspingPolicy(policy_config) else: raise ValueError("Invalid policy type: {}".format(policy_type))