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 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))
# 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"],
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))
# 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)
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))