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):
"""
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 __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 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()
cam = RGBD()
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()