class CollectData():
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
# if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
# self.tt = TableTop()
# self.tt.find_table(self.robot)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.collision_world = hsrb_interface.collision_world.CollisionWorld(
"global_collision_world")
self.collision_world.remove_all()
self.collision_world.add_box(x=.8,
y=.9,
z=0.5,
pose=geometry.pose(y=1.4, z=0.15),
frame_id='map')
print "after thread"
def collect_data(self):
self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
i = 0
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
cv2.imwrite("debug_imgs/data_chris/test" + str(i) + ".png", c_img)
#can also save d_img
IPython.embed() #re-arrange setup here
i += 1
def __init__(self):
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.web = Web_Labeler()
print "after thread"
def __init__(self):
'''
Initialization class for a Policy
Parameters
----------
yumi : An instianted yumi robot
com : The common class for the robot
cam : An open bincam class
debug : bool
A bool to indicate whether or not to display a training set point for
debuging.
'''
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
self.tt.find_table(self.robot)
self.wl = Python_Labeler(cam=self.cam)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.ds = data_saver('tpc_rollouts/rollouts')
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, cam, options,
robot.get('gripper'))
self.gm = GraspManipulator(self.gp, self.gripper, self.whole_body,
self.omni_base, self.tt)
print "after thread"
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
# if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
# self.tt = TableTop()
# self.tt.find_table(self.robot)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.collision_world = hsrb_interface.collision_world.CollisionWorld(
"global_collision_world")
self.collision_world.remove_all()
self.collision_world.add_box(x=.8,
y=.9,
z=0.5,
pose=geometry.pose(y=1.4, z=0.15),
frame_id='map')
print "after thread"
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.helper = Helper(cfg)
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.dl = DataLogger("stats_data/model_base", cfg.EVALUATE)
self.web = Web_Labeler(cfg.NUM_ROBOTS_ON_NETWORK)
model_path = 'main/output_inference_graph.pb'
label_map_path = 'main/object-detection.pbtxt'
self.det = Detector(model_path, label_map_path)
print "after thread"
class LabelDemo():
def __init__(self):
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.web = Web_Labeler()
print "after thread"
def bbox_to_fg(self, bbox, c_img, col_img):
obj_mask = crop_img(c_img,
bycoords=[bbox[1], bbox[3], bbox[0], bbox[2]])
obj_workspace_img = col_img.mask_binary(obj_mask)
fg = obj_workspace_img.foreground_mask(cfg.COLOR_TOL,
ignore_black=True)
return fg, obj_workspace_img
def test_bbox_overlap(self, box_a, box_b):
#bbox has format [xmin, ymin, xmax, ymax]
if box_a[0] > box_b[2] or box_a[2] < box_b[0]:
return False
if box_a[1] > box_b[3] or box_a[3] < box_b[1]:
return False
return True
def find_isolated_objects(self, bboxes, c_img):
valid_bboxes = []
for curr_ind in range(len(bboxes)):
curr_bbox = bboxes[curr_ind]
overlap = False
for test_ind in range(curr_ind + 1, len(bboxes)):
test_bbox = bboxes[test_ind]
if self.test_bbox_overlap(curr_bbox, test_bbox):
overlap = True
break
if not overlap:
valid_bboxes.append(curr_bbox)
return valid_bboxes
def label_demo(self):
self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
while not (c_img is None or d_img is None):
path = "/home/autolab/Workspaces/michael_working/siemens_challenge/debug_imgs/web.png"
cv2.imwrite(path, c_img)
time.sleep(2) #make sure new image is written before being read
# print "\n new iteration"
main_mask = crop_img(c_img, simple=True)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
labels = self.web.label_image(path)
obj = labels['objects'][0]
bbox = obj['box']
class_label = obj['class']
#bbox has format [xmin, ymin, xmax, ymax]
fg, obj_w = self.bbox_to_fg(bbox, c_img, col_img)
cv2.imwrite("debug_imgs/test.png", obj_w.data)
groups = get_cluster_info(fg)
display_grasps(workspace_img, groups)
group = groups[0]
pose, rot = self.gm.compute_grasp(group.cm, group.dir, d_img)
grasp_pose = self.gripper.get_grasp_pose(pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data)
self.gm.execute_grasp(grasp_pose, class_num=class_label)
#reset to start
self.whole_body.move_to_go()
self.gm.move_to_home()
self.gm.position_head()
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
class LabelDemo():
def __init__(self):
self.robot = hsrb_interface.Robot()
self.br = tf.TransformBroadcaster()
self.rgbd_map = RGBD2Map(self.br)
# IPython.embed()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction, self.whole_body, self.omni_base, self.tl)
self.web = Web_Labeler()
thread.start_new_thread(self.broadcast_temp_bin,())
time.sleep(3)
print "after thread"
def broadcast_temp_bin(self):
while True:
self.br.sendTransform((1, 1, 0.6),
tf.transformations.quaternion_from_euler(ai=0.0,aj=1.57,ak=0.0),
rospy.Time.now(),
'temp_bin',
# 'head_rgbd_sensor_link')
'map')
rospy.sleep(1)
def bbox_to_fg(self, bbox, c_img, col_img):
obj_mask = crop_img(c_img, bycoords = [bbox[1], bbox[3], bbox[0], bbox[2]])
obj_workspace_img = col_img.mask_binary(obj_mask)
fg = obj_workspace_img.foreground_mask(cfg.COLOR_TOL, ignore_black=True)
return fg, obj_workspace_img
def test_bbox_overlap(self, box_a, box_b):
#bbox has format [xmin, ymin, xmax, ymax]
if box_a[0] > box_b[2] or box_a[2] < box_b[0]:
return False
if box_a[1] > box_b[3] or box_a[3] < box_b[1]:
return False
return True
def find_isolated_objects(self, bboxes, c_img):
valid_bboxes = []
for curr_ind in range(len(bboxes)):
curr_bbox = bboxes[curr_ind]
overlap = False
for test_ind in range(curr_ind + 1, len(bboxes)):
test_bbox = bboxes[test_ind]
if self.test_bbox_overlap(curr_bbox, test_bbox):
overlap = True
break
if not overlap:
valid_bboxes.append(curr_bbox)
return valid_bboxes
def label_demo(self):
""" Main method which executes the stuff we're interested in.
Should apply to both the physical and simulated HSRs. Call as `python
main/test_labeling.py`.
"""
self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
path = "/home/ron/siemens_sim/siemens_challenge/debug_imgs/web.png"
cv2.imwrite(path, c_img)
time.sleep(2) #make sure new image is written before being read
# print "\n new iteration"
main_mask = crop_img(c_img, simple=True)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
labels = self.web.label_image(path)
obj = labels['objects'][0]
bbox = obj['box']
class_label = obj['class']
#bbox has format [xmin, ymin, xmax, ymax]
fg, obj_w = self.bbox_to_fg(bbox, c_img, col_img)
cv2.imwrite("debug_imgs/test.png", obj_w.data)
groups = get_cluster_info(fg)
display_grasps(workspace_img, groups)
group = groups[0]
print(d_img)
pose,rot = self.gm.compute_grasp(group.cm, group.dir, d_img)
pose = find_pose(pose)
if pose == None:
print("unable to find corresponding item.")
sys.exit()
a = tf.transformations.quaternion_from_euler(ai=-2.355,aj=-3.14,ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a,b)
print("now about to get grasp pose, w/pose: {}, rot: {}".format(pose, rot))
thread.start_new_thread(self.gripper.loop_broadcast,(pose,base_rot,rot))
time.sleep(5)
print("now calling execute_grasp w/grasp_pose: {}".format(grasp_pose))
# IPython.embed()
self.gm.execute_grasp("grasp_0")
self.whole_body.move_end_effector_pose(geometry.pose(),"temp_bin")
self.gripper.open_gripper()
#reset to start
self.whole_body.move_to_go()
# self.gm.move_to_home()
self.gm.position_head()
time.sleep(3)
class LegoDemo():
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
# if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
# self.tt = TableTop()
# self.tt.find_table(self.robot)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base)
self.collision_world = hsrb_interface.collision_world.CollisionWorld(
"global_collision_world")
self.collision_world.remove_all()
self.collision_world.add_box(x=.8,
y=.9,
z=0.5,
pose=geometry.pose(y=1.4, z=0.15),
frame_id='map')
print "after thread"
def find_mean_depth(self, d_img):
"""
Evaluates the current policy and then executes the motion
specified in the the common class
"""
indx = np.nonzero(d_img)
mean = np.mean(d_img[indx])
return
def get_success(self, action):
"""
Parameters
----------
action : str
action to query success of
Returns
-------
:str: y or n
"""
if cfg.COLLECT_DATA and cfg.QUERY:
print("Was " + action + " successful? (y or n)")
succ = raw_input()
while not (succ == "y" or succ == "n"):
print("Enter only y or n to indicate success of " + action)
succ = raw_input()
return succ
else:
return "no data queried"
def get_int(self):
"""
Returns
-------
int
"""
if cfg.COLLECT_DATA and cfg.QUERY:
print("How many legos are on the table?")
inp = raw_input()
while not inp.isdigit():
print("Enter an integer.")
inp = raw_input()
return inp
else:
return "no data queried"
def get_str(self):
"""
Returns
-------
:str: arbitary value
"""
if cfg.COLLECT_DATA and cfg.QUERY:
print(
"Any notes? (pushed legos off table, ran into table, etc.) (no if none)"
)
inp = raw_input()
return inp
else:
return "no data queried"
def run_singulation(self, col_img, main_mask, d_img, to_singulate):
"""
Parameters
----------
col_img : `ColorImage`
main_mask : `BinaryImage`
d_img : 'DepthImage'
to_singulate : list of `Group`
"""
print("SINGULATING")
self.dm.update_traj("action", "singulate")
a = time.time()
singulator = Singulation(col_img, main_mask,
[g.mask for g in to_singulate])
waypoints, rot, free_pix = singulator.get_singulation()
self.dm.update_traj("compute_singulate_time", time.time() - a)
singulator.display_singulation()
self.dm.update_traj("singulate_info", (waypoints, rot, free_pix))
self.dm.update_traj("singulate_successes", "crashed")
self.dm.update_traj("execute_singulate_time", "crashed")
self.dm.overwrite_traj()
a = time.time()
self.gm.singulate(waypoints, rot, col_img.data, d_img, expand=True)
self.dm.update_traj("execute_singulate_time", time.time() - a)
self.dm.update_traj("singulate_success",
self.get_success("singulation"))
self.dm.overwrite_traj()
def run_grasps(self, workspace_img, to_grasp):
"""
Parameters
----------
workspace_img : `ColorImage`
to_grasp : list of tuple of:
(`Group`, grasp_pose, suction_pose, class_num)
"""
print("GRASPING")
#impose ordering on grasps (by closest/highest y first)
to_grasp.sort(key=lambda g: -1 * g[0].cm[0])
self.dm.update_traj("action", "grasp")
self.dm.update_traj("all_grasps_info",
[(c[0].cm, c[0].dir, c[0].mask.data, c[3])
for c in to_grasp])
if not cfg.CHAIN_GRASPS:
to_grasp = to_grasp[0:1]
self.dm.update_traj("run_grasps_info",
[(c[0].cm, c[0].dir, c[0].mask.data, c[3])
for c in to_grasp])
display_grasps(workspace_img, [g[0] for g in to_grasp])
successes = ["not attempted" for i in range(len(to_grasp))]
correct_colors = ["not attempted" for i in range(len(to_grasp))]
times = ["not attempted" for i in range(len(to_grasp))]
for i in range(len(to_grasp)):
print "grasping", to_grasp[i][1]
successes[i] = "crashed"
correct_colors[i] = "crashed"
times[i] = "crashed"
self.dm.update_traj("grasp_successes", successes)
self.dm.update_traj("grasp_colors", correct_colors)
self.dm.update_traj("execute_grasp_times", times)
self.dm.overwrite_traj()
a = time.time()
self.gm.execute_grasp(to_grasp[i][1], class_num=to_grasp[i][3])
# self.gm.execute_suction(to_grasp[i][2], to_grasp[i][3])
times[i] = time.time() - a
successes[i] = self.get_success("grasp")
correct_colors[i] = self.get_success("correct color")
self.dm.update_traj("grasp_successes", successes)
self.dm.update_traj("grasp_colors", correct_colors)
self.dm.update_traj("execute_grasp_times", times)
self.dm.overwrite_traj()
def clusters_to_actions(self, groups, col_img, d_img, workspace_img):
"""
Parameters
----------
groups : list of `Group`
col_img : `ColorImage`
d_img : `DepthImage`
workspace_img : `ColorImage`
Returns
-------
list of tuples of form:
(`Group`, grasp_pose, suction_pose, class_num)
list of `Group`
"""
find_grasps_times = []
compute_grasps_times = []
find_hsv_times = []
to_singulate = []
to_grasp = []
for group in groups:
a = time.time()
inner_groups = grasps_within_pile(col_img.mask_binary(group.mask))
find_grasps_times.append(time.time() - a)
if len(inner_groups) == 0:
to_singulate.append(group)
else:
for in_group in inner_groups:
a = time.time()
pose, rot = self.gm.compute_grasp(in_group.cm,
in_group.dir, d_img)
grasp_pose = self.gripper.get_grasp_pose(
pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data)
suction_pose = self.suction.get_grasp_pose(
pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data)
compute_grasps_times.append(time.time() - a)
a = time.time()
class_num = hsv_classify(col_img.mask_binary(
in_group.mask))
find_hsv_times.append(time.time() - a)
to_grasp.append(
(in_group, grasp_pose, suction_pose, class_num))
self.dm.update_traj("compute_grasps_times", compute_grasps_times)
self.dm.update_traj("find_grasps_times", find_grasps_times)
self.dm.update_traj("find_hsv_times", find_hsv_times)
return to_grasp, to_singulate
def lego_demo(self):
if not cfg.COLLECT_DATA:
print("WARNING: NO DATA IS BEING COLLECTED")
print("TO COLLECT DATA, CHANGE COLLECT_DATA IN config_tpc")
self.dm = DataManager()
self.get_new_grasp = True
# DEBUG = False
# if not DEBUG:
# self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
while not (c_img is None or d_img is None):
print "\n new iteration"
main_mask = crop_img(c_img, use_preset=True, arc=False)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
self.dm.clear_traj()
self.dm.update_traj("c_img", c_img)
self.dm.update_traj("d_img", d_img)
self.dm.update_traj("stop_condition", "crash")
cv2.imwrite("debug_imgs/c_img.png", c_img)
self.dm.update_traj("crop", workspace_img.data)
a = time.time()
groups = run_connected_components(workspace_img, viz=True)
self.dm.update_traj("connected_components_time", time.time() - a)
self.dm.update_traj("num_legos", self.get_int())
self.dm.append_traj()
print "num masses", len(groups)
if len(groups) == 0:
print("cleared workspace")
self.dm.update_traj("stop_condition",
self.get_success("clearing table"))
self.dm.overwrite_traj()
time.sleep(5)
break
to_grasp, to_singulate = self.clusters_to_actions(
groups, col_img, d_img, workspace_img)
self.whole_body.collision_world = self.collision_world
if len(to_grasp) > 0:
self.run_grasps(workspace_img, to_grasp)
else:
self.run_singulation(col_img, main_mask, d_img, to_singulate)
self.dm.update_traj("notes", self.get_str())
#reset to start
self.whole_body.move_to_go()
self.gm.move_to_home()
self.gm.position_head()
time.sleep(8) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
self.dm.update_traj("stop_condition", "none")
self.dm.update_traj("c_img_result", c_img)
self.dm.update_traj("d_img_result", d_img)
self.dm.overwrite_traj()
class CollectData():
def __init__(self):
""" Class to run HSR lego task """
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
# if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
# self.tt = TableTop()
# self.tt.find_table(self.robot)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base)
self.collision_world = hsrb_interface.collision_world.CollisionWorld(
"global_collision_world")
self.collision_world.remove_all()
self.collision_world.add_box(x=.8,
y=.9,
z=0.5,
pose=geometry.pose(y=1.4, z=0.15),
frame_id='map')
print("finished initializing collect data class")
def collect_data(self):
"""
Run this a few times to check that the rgb images are reasonable.
If not, rearrange the setup and try again. Delete any images saved after
that, the run this "for real".
"""
self.gm.position_head()
IMDIR_RGB = 'image_rgb/'
IMDIR_DEPTH = 'image_depth/'
time.sleep(5) #making sure the robot is finished moving
print("after calling gm.position_head() w/several second delay")
print("\nwhole body joint positions:\n{}".format(
self.whole_body.joint_positions))
while True:
num = len([x for x in os.listdir(IMDIR_RGB) if 'png' in x])
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
cv2.imshow('rgb/image_raw', c_img)
cv2.imshow('depth/image_raw', d_img)
fname1 = IMDIR_RGB + 'rgb_raw_{}.png'.format(str(num).zfill(4))
fname2 = IMDIR_DEPTH + 'depth_raw_{}.png'.format(str(num).zfill(4))
cv2.imwrite(fname1, c_img)
cv2.imwrite(fname2, d_img)
print("just saved {} and {}. NOW REARRANGE SETUP!!".format(
fname1, fname2))
IPython.embed() #re-arrange setup here
com = COM()
com.go_to_initial_state(whole_body)
tt = TableTop()
tt.find_table(robot)
grasp_count = 0
br = tf.TransformBroadcaster()
tl = TransformListener()
gp = GraspPlanner()
gripper = Crane_Gripper(gp, cam, com.Options, robot.get('gripper'))
suction = Suction_Gripper(gp, cam, com.Options, robot.get('suction'))
gm = GraspManipulator(gp, gripper, suction, whole_body, omni_base, tt)
gm.position_head()
print "after thread"
curr_offsets = np.array([0, 0, -0.5])
curr_rot = np.array([0.0,0.0,1.57])
while True:
label = id_generator()
tt.make_new_pose(curr_offsets,label,rot = curr_rot)
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), label)
delta = raw_input()
while not (delta in ["+x", "-x", "+y", "-y", "+z", "-z"]):
print("not a valid delta")
delta = raw_input()
class BedMaker():
def __init__(self):
'''
Initialization class for a Policy
Parameters
----------
yumi : An instianted yumi robot
com : The common class for the robot
cam : An open bincam class
debug : bool
A bool to indicate whether or not to display a training set point for
debuging.
'''
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
if not DEBUG:
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
self.tt.find_table(self.robot)
self.wl = Python_Labeler(cam=self.cam)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.ds = data_saver('tpc_rollouts/rollouts')
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, cam, options,
robot.get('gripper'))
self.gm = GraspManipulator(self.gp, self.gripper, self.whole_body,
self.omni_base, self.tt)
print "after thread"
def find_mean_depth(self, d_img):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
indx = np.nonzero(d_img)
mean = np.mean(d_img[indx])
return
def lego_demo(self):
self.rollout_data = []
self.get_new_grasp = True
if not DEBUG:
self.gm.position_head()
while True:
time.sleep(1) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
cv2.imwrite("debug_imgs/c_img.png", c_img)
print "\n new iteration"
if (not c_img == None and not d_img == None):
#label image
data = self.wl.label_image(c_img)
c_img = self.cam.read_color_data()
grasp_boxes = []
suction_boxes = []
singulate_boxes = []
for i in range(data['num_labels']):
bbox = data['objects'][i]['box']
classnum = data['objects'][i]['class']
classname = ['grasp', 'singulate', 'suction',
'quit'][classnum]
if classname == "grasp":
grasp_boxes.append(bbox)
elif classname == "suction":
suction_boxes.append(bbox)
elif classname == "singulate":
singulate_boxes.append(bbox)
elif classname == "quit":
self.ds.save_rollout()
self.gm.move_to_home()
self.ds.append_data_to_rollout(c_img, data)
main_mask = crop_img(c_img)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
grasps = []
viz_info = []
for i in range(len(grasp_boxes)):
bbox = grasp_boxes[i]
center_mass, direction = bbox_to_grasp(bbox, c_img, d_img)
viz_info.append([center_mass, direction])
pose, rot = self.gm.compute_grasp(center_mass, direction,
d_img)
grasps.append(
self.gripper.get_grasp_pose(pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data))
suctions = []
for i in range(len(suction_boxes)):
suctions.append("compute_suction?")
if len(grasps) > 0 or len(suctions) > 0:
cv2.imwrite(
"grasps.png",
visualize(workspace_img, [v[0] for v in viz_info],
[v[1] for v in viz_info]))
print "running grasps"
for grasp in grasps:
print "grasping", grasp
self.gm.execute_grasp(grasp)
print "running suctions"
for suction in suctions:
print "suctioning", suction
#execute suction
elif len(singulate_boxes) > 0:
print("singulating")
bbox = singulate_boxes[0]
obj_mask = bbox_to_mask(bbox, c_img)
start, end = find_singulation(col_img, main_mask, obj_mask)
display_singulation(start, end, workspace_img)
self.gm.singulate(start, end, c_img, d_img)
else:
print("cleared workspace")
self.whole_body.move_to_go()
self.gm.position_head()
class FullWebDemo():
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.web = Web_Labeler()
print "after thread"
def run_grasp(self, bbox, c_img, col_img, workspace_img, d_img):
print("grasping a " + cfg.labels[bbox.label])
#bbox has format [xmin, ymin, xmax, ymax]
fg, obj_w = bbox.to_mask(c_img, col_img)
# cv2.imwrite("debug_imgs/test.png", obj_w.data)
# cv2.imwrite("debug_imgs/test2.png", fg.data)
groups = get_cluster_info(fg)
curr_tol = cfg.COLOR_TOL
while len(groups) == 0 and curr_tol > 10:
curr_tol -= 5
#retry with lower tolerance- probably white object
fg, obj_w = bbox.to_mask(c_img, col_img, tol=curr_tol)
groups = get_cluster_info(fg)
if len(groups) == 0:
print("No object within bounding box")
return False
display_grasps(workspace_img, groups)
group = groups[0]
pose, rot = self.gm.compute_grasp(group.cm, group.dir, d_img)
grasp_pose = self.gripper.get_grasp_pose(pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data)
self.gm.execute_grasp(grasp_pose, class_num=bbox.label)
def run_singulate(self, col_img, main_mask, to_singulate, d_img):
print("singulating")
singulator = Singulation(col_img, main_mask,
[g.mask for g in to_singulate])
waypoints, rot, free_pix = singulator.get_singulation()
singulator.display_singulation()
self.gm.singulate(waypoints, rot, col_img.data, d_img, expand=True)
def full_web_demo(self):
self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
while not (c_img is None or d_img is None):
path = "/home/autolab/Workspaces/michael_working/siemens_challenge/debug_imgs/web.png"
cv2.imwrite(path, c_img)
time.sleep(2) #make sure new image is written before being read
# print "\n new iteration"
main_mask = crop_img(c_img, simple=True)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
labels = self.web.label_image(path)
objs = labels['objects']
bboxes = [Bbox(obj['box'], obj['class']) for obj in objs]
single_objs = find_isolated_objects(bboxes)
if len(single_objs) > 0:
to_grasp = select_first_obj(single_objs)
self.run_grasp(to_grasp, c_img, col_img, workspace_img, d_img)
else:
#for accurate singulation should have bboxes for all
fg_imgs = [box.to_mask(c_img, col_img) for box in bboxes]
groups = [get_cluster_info(fg[0])[0] for fg in fg_imgs]
groups = merge_groups(groups, cfg.DIST_TOL)
self.run_singulate(col_img, main_mask, groups, d_img)
#reset to start
self.whole_body.move_to_go()
self.gm.move_to_home()
self.gm.position_head()
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
class SiemensDemo():
def __init__(self):
"""
Class to run HSR lego task
"""
self.robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.side = 'BOTTOM'
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.grasp_count = 0
self.helper = Helper(cfg)
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('suction'))
self.gm = GraspManipulator(self.gp, self.gripper, self.suction,
self.whole_body, self.omni_base, self.tl)
self.dl = DataLogger("stats_data/model_base", cfg.EVALUATE)
self.web = Web_Labeler(cfg.NUM_ROBOTS_ON_NETWORK)
model_path = 'main/output_inference_graph.pb'
label_map_path = 'main/object-detection.pbtxt'
self.det = Detector(model_path, label_map_path)
print "after thread"
def run_grasp(self, bbox, c_img, col_img, workspace_img, d_img):
print("grasping a " + cfg.labels[bbox.label])
group = bbox.to_group(c_img, col_img)
display_grasps(workspace_img, [group])
pose, rot = self.gm.compute_grasp(group.cm, group.dir, d_img)
grasp_pose = self.gripper.get_grasp_pose(pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data)
self.gm.execute_grasp(grasp_pose, class_num=bbox.label)
def run_singulate(self, col_img, main_mask, to_singulate, d_img):
print("singulating")
singulator = Singulation(col_img, main_mask,
[g.mask for g in to_singulate])
waypoints, rot, free_pix = singulator.get_singulation()
singulator.display_singulation()
self.gm.singulate(waypoints, rot, col_img.data, d_img, expand=True)
def get_bboxes_from_net(self, path):
output_dict, vis_util_image = self.det.predict(path)
plt.savefig('debug_imgs/predictions.png')
plt.close()
plt.clf()
plt.cla()
img = cv2.imread(path)
boxes = format_net_bboxes(output_dict, img.shape)
return boxes, vis_util_image
def get_bboxes_from_web(self, path):
labels = self.web.label_image(path)
objs = labels['objects']
bboxes = [Bbox(obj['box'], obj['class']) for obj in objs]
return bboxes
def determine_to_ask_for_help(self, bboxes, col_img):
bboxes = deepcopy(bboxes)
col_img = deepcopy(col_img)
single_objs = find_isolated_objects_by_overlap(bboxes)
if len(single_objs) > 0:
return False
else:
isolated_exist = find_isolated_objects_by_distance(bboxes, col_img)
return isolated_exist
def get_bboxes(self, path, col_img):
boxes, vis_util_image = self.get_bboxes_from_net(path)
#low confidence or no objects
if self.determine_to_ask_for_help(boxes, col_img):
self.helper.asked = True
self.helper.start_timer()
boxes = self.get_bboxes_from_web(path)
self.helper.stop_timer()
self.dl.save_stat("duration", self.helper.duration)
self.dl.save_stat("num_online", cfg.NUM_ROBOTS_ON_NETWORK)
return boxes, vis_util_image
def siemens_demo(self):
self.gm.position_head()
time.sleep(3) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
while not (c_img is None or d_img is None):
path = "/home/autolab/Workspaces/michael_working/siemens_challenge/debug_imgs/web.png"
cv2.imwrite(path, c_img)
time.sleep(2) #make sure new image is written before being read
# print "\n new iteration"
main_mask = crop_img(c_img, simple=True)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
bboxes, vis_util_image = self.get_bboxes(path, col_img)
if len(bboxes) == 0:
print("Cleared the workspace")
print("Add more objects, then resume")
IPython.embed()
else:
box_viz = draw_boxes(bboxes, c_img)
cv2.imwrite("debug_imgs/box.png", box_viz)
single_objs = find_isolated_objects_by_overlap(bboxes)
grasp_sucess = 1.0
if len(single_objs) == 0:
single_objs = find_isolated_objects_by_distance(
bboxes, col_img)
if len(single_objs) > 0:
to_grasp = select_first_obj(single_objs)
singulation_time = 0.0
self.run_grasp(to_grasp, c_img, col_img, workspace_img,
d_img)
grasp_sucess = self.dl.record_success(
"grasp", other_data=[c_img, vis_util_image, d_img])
else:
#for accurate singulation should have bboxes for all
fg_imgs = [box.to_mask(c_img, col_img) for box in bboxes]
groups = [get_cluster_info(fg[0])[0] for fg in fg_imgs]
groups = merge_groups(groups, cfg.DIST_TOL)
self.run_singulate(col_img, main_mask, groups, d_img)
sing_start = time.time()
singulation_success = self.dl.record_success(
"singulation",
other_data=[c_img, vis_util_image, d_img])
singulation_time = time.time() - sing_start
if cfg.EVALUATE:
reward = self.helper.get_reward(grasp_sucess,
singulation_time)
self.dl.record_reward(reward)
#reset to start
self.whole_body.move_to_go()
self.gm.move_to_home()
self.gm.position_head()
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()