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))