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))
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) # 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. if fully_conv: # TODO(vsatish): We should really be doing this in some factory policy. 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))
# Create publisher to publish pose of final grasp. grasp_pose_publisher = rospy.Publisher("/gqcnn_grasp/pose", PoseStamped, queue_size=10) # Create a grasping policy. rospy.loginfo("Creating Grasping Policy") if fully_conv: # TODO(vsatish): We should really be doing this in some factory policy. if policy_cfg["type"] == "fully_conv_suction": grasping_policy = \ 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))
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))