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 cfg.USE_WEB_INTERFACE:
self.wl = Web_Labeler()
else:
self.wl = Python_Labeler(self.cam)
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
self.tt.find_table(self.robot)
self.ins = InitialSampler(self.cam)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.g_detector = Analytic_Grasp()
self.sn = Success_Net(self.whole_body, self.tt, self.cam,
self.omni_base)
c_img = self.cam.read_color_data()
#self.test_current_point()
time.sleep(4)
#thread.start_new_thread(self.ql.run,())
print "after thread"
def __init__(self, args):
"""For data collection of bed-making, NOT the deployment.
Assumes we roll out the robot's policy via code (not via human touch).
This is the 'slower' way where we have the python interface that the
human clicks on to indicate grasping points. Good news is, our deployment
code is probably going to be similar to this.
For joystick: you only need it plugged in for the initial state sampler,
which (at the moment) we are not even using.
"""
self.robot = robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = robot.get('omni_base')
self.whole_body = robot.get('whole_body')
self.cam = RGBD()
self.com = COM()
self.wl = Python_Labeler(cam=self.cam)
# View mode: STANDARD (the way I was doing earlier), CLOSE (the way they want).
self.view_mode = cfg.VIEW_MODE
# Set up initial state, table, etc.
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
# For now, a workaround. Ugly but it should do the job ...
#self.tt.find_table(robot)
self.tt.make_fake_ar()
self.tt.find_table_workaround(robot)
#self.ins = InitialSampler(self.cam)
self.side = 'BOTTOM'
self.grasp_count = 0
# Bells and whistles; note the 'success check' to check if transitioning
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
robot.get('gripper'))
self.sc = Success_Check(self.whole_body, self.tt, self.cam,
self.omni_base)
time.sleep(4)
print(
"Finished creating BedMaker()! Get the bed set up and run bed-making!"
)
if cfg.INS_SAMPLE:
print("TODO: we don't have sampling code here.")
# When we start, spin this so we can check the frames. Then un-comment,
# etc. It's the current hack we have to get around crummy AR marker detection.
if args.phase == 1:
print("Now doing rospy.spin() because phase = 1.")
rospy.spin()
def __init__(self, args):
self.args = args
self.robot = robot = hsrb_interface.Robot()
self.omni_base = robot.get('omni_base')
self.whole_body = robot.get('whole_body')
self.cam = RGBD()
self.data = dict() # list of processed depth images and actions taken (I_t, a_t)
# We don't use directly, but it makes a frame that we need for pixels -> world grasp poses.
self.rgbd_map = RGBD2Map() # makes frame we need but we don't use it otherwise
# TODO: eventually we need to remove this. Doing this to let us go from
# camera coordinates to world frame, but we need HSR_CORE to support it.
# But, should be easy because the grasp planner is pretty simple and we
# only use it to compute the average depth values in a region.
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, options=None, gripper=robot.get('gripper'))
# Also this is a bit hacky. We want the HSR to rotate so that it's
# _facing_ the bed now, whereas it started facing 'sideways'. Makes a
# target pose for the robot so it goes there, before grasp executiuon.
# TODO: make pose here. I think we can get away with rotating wrt the
# map but in general we want to create our own poses.
print("Initialized the data collector! Resting for 2 seconds...")
time.sleep(2)
# Hande the part about loading the network and pretrained model.
HEAD = '/nfs/diskstation/seita/bedmake_ssl'
MODEL = 'resnet18_2018-11-18-09-50_000'
PATH = join(HEAD, MODEL, 'act_predictor.pt')
# Get old args we used, and put into a newer Namespace object.
with open(join(HEAD, MODEL, 'args.json'), 'r') as fh:
saved_args = json.load(fh)
self.netargs = opt._json_to_args(jsonfile=saved_args)
# Load the pretrained model.
model = opt.get_pretrained_model(self.netargs)
self.act_predictor = ActPredictorNet(model, self.netargs)
self.act_predictor.load_state_dict(torch.load(PATH))
self.act_predictor.eval()
self.transforms_valid = transforms.Compose([
CT.Rescale((256,256)),
CT.CenterCrop((224,224)),
CT.ToTensor(),
CT.Normalize(opt.MEAN, opt.STD),
])
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.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 cfg.USE_WEB_INTERFACE:
self.wl = Web_Labeler()
else:
self.wl = Python_Labeler(self.cam)
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 = Bed_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.sc = Success_Check(self.whole_body, self.tt, self.cam,
self.omni_base)
#self.test_current_point()
time.sleep(4)
#thread.start_new_thread(self.ql.run,())
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 bed_pick(self):
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img == None and not d_img == None):
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
data = self.wl.label_image(c_img)
self.gripper.find_pick_region_labeler(data, c_img, d_img,
self.grasp_count)
pick_found, bed_pick = self.check_card_found()
self.grasp_count += 1
if (pick_found):
if (self.side == 'BOTTOM'):
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
success = self.sc.check_bottom_success(self.wl)
print "WAS SUCCESFUL: "
print success
if (success):
self.move_to_top_side()
self.side = "TOP"
elif (self.side == "TOP"):
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
success = self.sc.check_top_success(self.wl)
print "WAS SUCCESFUL: "
print success
if (success):
self.side == "PILLOW"
def test_current_point(self):
self.gripper.tension.force_pull(self.whole_body, (0, 1, 0))
self.gripper.com.grip_open(self.gripper)
self.move_to_top_side()
def move_to_top_side(self):
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'right_mid')
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'top_mid')
def check_bottom_success(self):
self.tt.move_to_pose(self.omni_base, 'lower_mid')
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
def check_card_found(self):
# try:
transforms = self.tl.getFrameStrings()
cards = []
try:
for transform in transforms:
print transform
current_grasp = 'bed_' + str(self.grasp_count)
if current_grasp in transform:
print 'got here'
f_p = self.tl.lookupTransform('head_rgbd_sensor_rgb_frame',
transform, rospy.Time(0))
cards.append(transform)
except:
rospy.logerr('bed pick not found yet')
return True, cards
def __init__(self, args):
"""For deploying the bed-making policy, not for data collection.
We use all three variants (analytic, human, networks) here due to
similarities in code structure.
"""
self.args = args
DEBUG = True
# Set up the robot.
self.robot = robot = hsrb_interface.Robot()
if DEBUG:
print("finished: hsrb_interface.Robot()...")
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
if DEBUG:
print("finished: robot.get(omni_base)...")
self.whole_body = self.robot.get('whole_body')
if DEBUG:
print("finished: robot.get(whole_body)...")
self.cam = RGBD()
self.com = COM()
self.wl = Python_Labeler(cam=self.cam)
# Set up initial state, table, etc. Don't forget view mode!
self.view_mode = BED_CFG.VIEW_MODE
self.com.go_to_initial_state(self.whole_body)
if DEBUG:
print("finished: go_to_initial_state() ...")
self.tt = TableTop()
if DEBUG:
print("finished: TableTop()...")
# For now, a workaround. Ugly but it should do the job ...
#self.tt.find_table(robot)
self.tt.make_fake_ar()
self.tt.find_table_workaround(robot)
#self.ins = InitialSampler(self.cam)
self.side = 'BOTTOM'
self.grasp_count = 0
self.b_grasp_count = 0
self.t_grasp_count = 0
# AH, build the YOLO network beforehand.
g_cfg = BED_CFG.GRASP_CONFIG
s_cfg = BED_CFG.SUCC_CONFIG
self.yc = YOLO_CONV(options=s_cfg)
self.yc.load_network()
# Policy for grasp detection, using Deep Imitation Learning.
# Or, actually, sometimes we will use humans or an analytic version.
if DEBUG:
self._test_variables()
print("\nnow forming the GDetector with type {}".format(args.g_type))
if args.g_type == 'network':
self.g_detector = GDetector(g_cfg, BED_CFG, yc=self.yc)
elif args.g_type == 'analytic':
self.g_detector = Analytic_Grasp() # TODO not implemented!
elif args.g_type == 'human':
print("Using a human, don't need to have a `g_detector`. :-)")
if DEBUG:
self._test_variables()
print("\nnow making success net")
self.sn = Success_Net(self.whole_body,
self.tt,
self.cam,
self.omni_base,
fg_cfg=s_cfg,
bed_cfg=BED_CFG,
yc=self.yc)
# Bells and whistles.
self.br = TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
robot.get('gripper'))
self.dp = DrawPrediction()
# When we start, do rospy.spin() to check the frames (phase 1). Then re-run.
# The current hack we have to get around crummy AR marker detection. :-(
if DEBUG:
self._test_variables()
print("Finished with init method")
time.sleep(4)
if args.phase == 1:
print("Now doing rospy.spin() because phase = 1.")
rospy.spin()
# For evaluating coverage.
self.img_start = None
self.img_final = None
self.img_start2 = None
self.img_final2 = None
# For grasp offsets.
self.apply_offset = False
class BedMaker():
def __init__(self, args):
"""For deploying the bed-making policy, not for data collection.
We use all three variants (analytic, human, networks) here due to
similarities in code structure.
"""
self.args = args
DEBUG = True
# Set up the robot.
self.robot = robot = hsrb_interface.Robot()
if DEBUG:
print("finished: hsrb_interface.Robot()...")
self.rgbd_map = RGBD2Map()
self.omni_base = self.robot.get('omni_base')
if DEBUG:
print("finished: robot.get(omni_base)...")
self.whole_body = self.robot.get('whole_body')
if DEBUG:
print("finished: robot.get(whole_body)...")
self.cam = RGBD()
self.com = COM()
self.wl = Python_Labeler(cam=self.cam)
# Set up initial state, table, etc. Don't forget view mode!
self.view_mode = BED_CFG.VIEW_MODE
self.com.go_to_initial_state(self.whole_body)
if DEBUG:
print("finished: go_to_initial_state() ...")
self.tt = TableTop()
if DEBUG:
print("finished: TableTop()...")
# For now, a workaround. Ugly but it should do the job ...
#self.tt.find_table(robot)
self.tt.make_fake_ar()
self.tt.find_table_workaround(robot)
#self.ins = InitialSampler(self.cam)
self.side = 'BOTTOM'
self.grasp_count = 0
self.b_grasp_count = 0
self.t_grasp_count = 0
# AH, build the YOLO network beforehand.
g_cfg = BED_CFG.GRASP_CONFIG
s_cfg = BED_CFG.SUCC_CONFIG
self.yc = YOLO_CONV(options=s_cfg)
self.yc.load_network()
# Policy for grasp detection, using Deep Imitation Learning.
# Or, actually, sometimes we will use humans or an analytic version.
if DEBUG:
self._test_variables()
print("\nnow forming the GDetector with type {}".format(args.g_type))
if args.g_type == 'network':
self.g_detector = GDetector(g_cfg, BED_CFG, yc=self.yc)
elif args.g_type == 'analytic':
self.g_detector = Analytic_Grasp() # TODO not implemented!
elif args.g_type == 'human':
print("Using a human, don't need to have a `g_detector`. :-)")
if DEBUG:
self._test_variables()
print("\nnow making success net")
self.sn = Success_Net(self.whole_body,
self.tt,
self.cam,
self.omni_base,
fg_cfg=s_cfg,
bed_cfg=BED_CFG,
yc=self.yc)
# Bells and whistles.
self.br = TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
robot.get('gripper'))
self.dp = DrawPrediction()
# When we start, do rospy.spin() to check the frames (phase 1). Then re-run.
# The current hack we have to get around crummy AR marker detection. :-(
if DEBUG:
self._test_variables()
print("Finished with init method")
time.sleep(4)
if args.phase == 1:
print("Now doing rospy.spin() because phase = 1.")
rospy.spin()
# For evaluating coverage.
self.img_start = None
self.img_final = None
self.img_start2 = None
self.img_final2 = None
# For grasp offsets.
self.apply_offset = False
def bed_make(self):
"""Runs the pipeline for deployment, testing out bed-making.
"""
# Get the starting image (from USB webcam). Try a second as well.
cap = cv2.VideoCapture(0)
frame = None
while frame is None:
ret, frame = cap.read()
cv2.waitKey(50)
self.image_start = frame
cv2.imwrite('image_start.png', self.image_start)
_, frame = cap.read()
self.image_start2 = frame
cv2.imwrite('image_start2.png', self.image_start2)
cap.release()
print(
"NOTE! Recorded `image_start` for coverage evaluation. Was it set up?"
)
def get_pose(data_all):
# See `find_pick_region_labeler` in `p_pi/bed_making/gripper.py`.
# It's because from the web labeler, we get a bunch of objects.
# So we have to compute the pose (x,y) from it.
res = data_all['objects'][0]
x_min = float(res['box'][0])
y_min = float(res['box'][1])
x_max = float(res['box'][2])
y_max = float(res['box'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
return (x, y)
args = self.args
use_d = BED_CFG.GRASP_CONFIG.USE_DEPTH
self.get_new_grasp = True
self.new_grasp = True
self.rollout_stats = [] # What we actually save for analysis later
# Add to self.rollout_stats at the end for more timing info
self.g_time_stats = [] # for _execution_ of a grasp
self.move_time_stats = [] # for moving to the other side
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img.all() == None and not d_img.all() == None):
if self.new_grasp:
self.position_head()
else:
self.new_grasp = True
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
d_img_raw = np.copy(d_img) # Needed for determining grasp pose
# --------------------------------------------------------------
# Process depth images! Helps network, human, and (presumably) analytic.
# Obviously human can see the c_img as well ... hard to compare fairly.
# --------------------------------------------------------------
if use_d:
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
policy_input = np.copy(d_img)
else:
policy_input = np.copy(c_img)
# --------------------------------------------------------------
# Run grasp detector to get data=(x,y) point for target, record stats.
# Note that the web labeler returns a dictionary like this:
# {'objects': [{'box': (155, 187, 165, 194), 'class': 0}], 'num_labels': 1}
# but we really want just the 2D grasping point. So use `get_pose()`.
# Also, for the analytic one, we'll pick the highest point ourselves.
# --------------------------------------------------------------
sgraspt = time.time()
if args.g_type == 'network':
data = self.g_detector.predict(policy_input)
elif args.g_type == 'analytic':
data_all = self.wl.label_image(policy_input)
data = get_pose(data_all)
elif args.g_type == 'human':
data_all = self.wl.label_image(policy_input)
data = get_pose(data_all)
egraspt = time.time()
g_predict_t = egraspt - sgraspt
print("Grasp predict time: {:.2f}".format(g_predict_t))
self.record_stats(c_img, d_img_raw, data, self.side,
g_predict_t, 'grasp')
# For safety, we can check image and abort as needed before execution.
if use_d:
img = self.dp.draw_prediction(d_img, data)
else:
img = self.dp.draw_prediction(c_img, data)
caption = 'G Predicted: {} (ESC to abort, other key to proceed)'.format(
data)
call_wait_key(cv2.imshow(caption, img))
# --------------------------------------------------------------
# Broadcast grasp pose, execute the grasp, check for success.
# We'll use the `find_pick_region_net` since the `data` is the
# (x,y) pose, and not `find_pick_region_labeler`.
# --------------------------------------------------------------
self.gripper.find_pick_region_net(
pose=data,
c_img=c_img,
d_img=d_img_raw,
count=self.grasp_count,
side=self.side,
apply_offset=self.apply_offset)
pick_found, bed_pick = self.check_card_found()
if self.side == "BOTTOM":
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'lower_start')
tic = time.time()
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
toc = time.time()
else:
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'top_mid')
tic = time.time()
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
toc = time.time()
self.g_time_stats.append(toc - tic)
self.check_success_state(policy_input)
def check_success_state(self, old_grasp_image):
"""
Checks whether a single grasp in a bed-making trajectory succeeded.
Depends on which side of the bed the HSR is at. Invokes the learned
success network policy and transitions the HSR if successful.
When we record the data, c_img and d_img should be what success net saw.
UPDATE: now we can pass in the previous `d_img` from the grasping to
compare the difference. Well, technically the `policy_input` so it can
handle either case.
"""
use_d = BED_CFG.SUCC_CONFIG.USE_DEPTH
if self.side == "BOTTOM":
result = self.sn.check_bottom_success(use_d, old_grasp_image)
self.b_grasp_count += 1
else:
result = self.sn.check_top_success(use_d, old_grasp_image)
self.t_grasp_count += 1
self.grasp_count += 1
assert self.grasp_count == self.b_grasp_count + self.t_grasp_count
success = result['success']
data = result['data']
c_img = result['c_img']
d_img = result['d_img']
d_img_raw = result['d_img_raw']
s_predict_t = result['s_predict_t']
img_diff = result['diff_l2']
img_ssim = result['diff_ssim']
self.record_stats(c_img, d_img_raw, data, self.side, s_predict_t,
'success')
# We really need a better metric, such as 'structural similarity'.
# Edit: well, it's probably marginally better, I think.
# I use an L2 threshold of 98k, and an SSIM threshold of 0.88.
if BED_CFG.GRASP_CONFIG.USE_DEPTH != BED_CFG.SUCC_CONFIG.USE_DEPTH:
print("grasp vs success for using depth differ")
print("for now we'll ignore the offset issue.")
else:
print("L2 and SSIM btwn grasp & next image: {:.1f} and {:.3f}".
format(img_diff, img_ssim))
if img_ssim >= 0.875 or img_diff < 85000:
print("APPLYING OFFSET! (self.apply_offset = True)")
self.apply_offset = True
else:
print("no offset applied (self.apply_offset = False)")
self.apply_offset = False
# Have user confirm that this makes sense.
caption = "Success net saw this and thought: {}. Press any key".format(
success)
if use_d:
call_wait_key(cv2.imshow(caption, d_img))
else:
call_wait_key(cv2.imshow(caption, c_img))
# Limit amount of grasp attempts per side, pretend 'success' anyway.
lim = BED_CFG.GRASP_ATTEMPTS_PER_SIDE
if (self.side == 'BOTTOM' and self.b_grasp_count >= lim) or \
(self.side == 'TOP' and self.t_grasp_count >= lim):
print("We've hit {} for this side so set success=True".format(lim))
success = True
# Handle transitioning to different side
if success:
if self.side == "BOTTOM":
self.transition_to_top()
self.side = 'TOP'
else:
self.transition_to_start()
print(
"We're moving to another side so revert self.apply_offset = False."
)
self.apply_offset = False
else:
self.new_grasp = False
def transition_to_top(self):
"""Transition to top (not bottom)."""
transition_time = self.move_to_top_side()
self.move_time_stats.append(transition_time)
def transition_to_start(self):
"""Transition to start=bottom, SAVE ROLLOUT STATS, exit program.
The `rollout_stats` is a list with a bunch of stats recorded via the
class method `record_stats`. We save with a top-down webcam and save
before moving back, since the HSR could disconnect.
"""
# Record the final image for evaluation later (from USB webcam).
cap = cv2.VideoCapture(0)
frame = None
while frame is None:
ret, frame = cap.read()
self.image_final = frame
cv2.imwrite('image_final.png', self.image_final)
_, frame = cap.read()
self.image_final2 = frame
cv2.imwrite('image_final2.png', self.image_final2)
cap.release()
print("NOTE! Recorded `image_final` for coverage evaluation.")
# Append some last-minute stuff to `self.rollout_stats` for saving.
final_stuff = {
'image_start': self.image_start,
'image_final': self.image_final,
'image_start2': self.image_start2,
'image_final2': self.image_final2,
'grasp_times': self.g_time_stats,
'move_times': self.move_time_stats,
'args': self.args, # ADDING THIS! Now we can 'retrace' our steps.
}
self.rollout_stats.append(final_stuff)
# SAVE, move to start, then exit.
self.com.save_stat(self.rollout_stats, target_path=self.args.save_path)
self.move_to_start()
sys.exit()
def record_stats(self, c_img, d_img, data, side, time, typ):
"""Adds a dictionary to the `rollout_stats` list.
We can tell it's a 'net' thing due to 'net_pose' and 'net_succ' keys.
EDIT: argh wish I hadn't done that since this script also handles the
human and analytic cases. Oh well, too late for that now.
"""
assert side in ['BOTTOM', 'TOP']
grasp_point = {}
grasp_point['c_img'] = c_img
grasp_point['d_img'] = d_img
if typ == "grasp":
grasp_point['net_pose'] = data
grasp_point['g_net_time'] = time
elif typ == "success":
grasp_point['net_succ'] = data
grasp_point['s_net_time'] = time
else:
raise ValueError(typ)
grasp_point['side'] = side
grasp_point['type'] = typ
self.rollout_stats.append(grasp_point)
def position_head(self):
"""Position head for a grasp.
Use lower_start_tmp so HSR looks 'sideways'; thus, hand is not in the way.
"""
self.whole_body.move_to_go()
if self.side == "BOTTOM":
self.tt.move_to_pose(self.omni_base, 'lower_start_tmp')
self.whole_body.move_to_joint_positions(
{'arm_flex_joint': -np.pi / 16.0})
self.whole_body.move_to_joint_positions(
{'head_pan_joint': np.pi / 2.0})
self.whole_body.move_to_joint_positions({'arm_lift_joint': 0.120})
self.whole_body.move_to_joint_positions(
{'head_tilt_joint': -np.pi / 4.0})
def move_to_top_side(self):
"""Assumes we're at the bottom and want to go to the top."""
self.whole_body.move_to_go()
tic = time.time()
self.tt.move_to_pose(self.omni_base, 'right_down_1')
self.tt.move_to_pose(self.omni_base, 'right_mid_1')
self.tt.move_to_pose(self.omni_base, 'right_up_1')
self.tt.move_to_pose(self.omni_base, 'top_mid_tmp')
toc = time.time()
return toc - tic
def move_to_start(self):
"""Assumes we're at the top and we go back to the start.
Go to lower_start_tmp to be at the same view as we started with, so that
we take a c_img and compare coverage.
"""
self.whole_body.move_to_go()
tic = time.time()
self.tt.move_to_pose(self.omni_base, 'right_up_2')
self.tt.move_to_pose(self.omni_base, 'right_mid_2')
self.tt.move_to_pose(self.omni_base, 'right_down_2')
self.tt.move_to_pose(self.omni_base, 'lower_start_tmp')
toc = time.time()
return toc - tic
def check_card_found(self):
"""Looks up the pose for where the HSR's hand should go to."""
transforms = self.tl.getFrameStrings()
cards = []
try:
for transform in transforms:
current_grasp = 'bed_' + str(self.grasp_count)
if current_grasp in transform:
print('found {}'.format(current_grasp))
f_p = self.tl.lookupTransform('map', transform,
rospy.Time(0))
cards.append(transform)
except:
rospy.logerr('bed pick not found yet')
return True, cards
def _test_grasp(self):
"""Simple tests for grasping. Don't forget to process depth images.
Do this independently of any rollout ...
"""
print("\nNow in `test_grasp` to check grasping net...")
self.position_head()
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
pred = self.g_detector.predict(np.copy(d_img))
img = self.dp.draw_prediction(d_img, pred)
print("prediction: {}".format(pred))
caption = 'G Predicted: {} (ESC to abort, other key to proceed)'.format(
pred)
cv2.imshow(caption, img)
key = cv2.waitKey(0)
if key in ESC_KEYS:
print("Pressed ESC key. Terminating program...")
sys.exit()
def _test_success(self):
"""Simple tests for success net. Don't forget to process depth images.
Should be done after a grasp test since I don't re-position... Note: we
have access to `self.sn` but that isn't the actual net which has a
`predict`, but it's a wrapper (explained above), but we can access the
true network via `self.sn.sdect` and from there call `predict`.
"""
print("\nNow in `test_success` to check success net...")
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
result = self.sn.sdect.predict(np.copy(d_img))
result = np.squeeze(result)
print("s-net pred: {} (if [0]<[1] failure, else success...)".format(
result))
caption = 'S Predicted: {} (ESC to abort, other key to proceed)'.format(
result)
cv2.imshow(caption, d_img)
key = cv2.waitKey(0)
if key in ESC_KEYS:
print("Pressed ESC key. Terminating program...")
sys.exit()
def _test_variables(self):
"""Test to see if TF variables were loaded correctly.
"""
vars = tf.trainable_variables()
print("\ntf.trainable_variables:")
for vv in vars:
print(" {}".format(vv))
print("done\n")
class DataCollector:
def __init__(self, args):
self.args = args
self.robot = robot = hsrb_interface.Robot()
self.omni_base = robot.get('omni_base')
self.whole_body = robot.get('whole_body')
self.cam = RGBD()
self.data = dict() # list of processed depth images and actions taken (I_t, a_t)
# We don't use directly, but it makes a frame that we need for pixels -> world grasp poses.
self.rgbd_map = RGBD2Map() # makes frame we need but we don't use it otherwise
# TODO: eventually we need to remove this. Doing this to let us go from
# camera coordinates to world frame, but we need HSR_CORE to support it.
# But, should be easy because the grasp planner is pretty simple and we
# only use it to compute the average depth values in a region.
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, options=None, gripper=robot.get('gripper'))
# Also this is a bit hacky. We want the HSR to rotate so that it's
# _facing_ the bed now, whereas it started facing 'sideways'. Makes a
# target pose for the robot so it goes there, before grasp executiuon.
# TODO: make pose here. I think we can get away with rotating wrt the
# map but in general we want to create our own poses.
print("Initialized the data collector! Resting for 2 seconds...")
time.sleep(2)
# Hande the part about loading the network and pretrained model.
HEAD = '/nfs/diskstation/seita/bedmake_ssl'
MODEL = 'resnet18_2018-11-18-09-50_000'
PATH = join(HEAD, MODEL, 'act_predictor.pt')
# Get old args we used, and put into a newer Namespace object.
with open(join(HEAD, MODEL, 'args.json'), 'r') as fh:
saved_args = json.load(fh)
self.netargs = opt._json_to_args(jsonfile=saved_args)
# Load the pretrained model.
model = opt.get_pretrained_model(self.netargs)
self.act_predictor = ActPredictorNet(model, self.netargs)
self.act_predictor.load_state_dict(torch.load(PATH))
self.act_predictor.eval()
self.transforms_valid = transforms.Compose([
CT.Rescale((256,256)),
CT.CenterCrop((224,224)),
CT.ToTensor(),
CT.Normalize(opt.MEAN, opt.STD),
])
def orient_robot(self):
"""Orients the robot so that it is in a good position to take pictures.
Make sure the HSR is starting about a foot away from the long side of the bed,
facing parallel to the bed towards the top.
"""
self.whole_body.move_to_go()
self.whole_body.move_to_joint_positions({'arm_flex_joint': -np.pi/16.0})
self.whole_body.move_to_joint_positions({'head_pan_joint': np.pi/2.0})
self.whole_body.move_to_joint_positions({'head_tilt_joint': -np.pi/4.0})
self.whole_body.move_to_joint_positions({'arm_lift_joint': 0.120})
def get_images(self):
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
d_img_proc = process_depth(d_img)
return (c_img, d_img, d_img_proc)
def get_target_images(self):
"""Get a series of target images, stopping only when user wishes to
abort (via pressing ESC key).
Note, in this phase, we add onto existing indices in saved target dir.
We assume we can sort these, then extract the index from the last item.
"""
args = self.args
if not os.path.exists(TARGET_DIR):
os.makedirs(TARGET_DIR)
digits = 3
# Get the index we should start with for saving imgs.
saved_imgs = sorted([x for x in os.listdir(TARGET_DIR)])
if len(saved_imgs) == 0:
idx = 0
else:
saved_img_noext = (saved_imgs[0]).replace('.png','')
idx = int( (saved_img_noext.split('_'))[-1] )
while True:
c_img, d_img, d_img_proc = self.get_images()
if (c_img.all() is not None) and (d_img.all() is not None):
# Once we get here, we've saved the images. BUT, since the code
# stops here, we should change the bed making setup. THEN we
# press any key. This way, next time step has the new images.
caption = 'Here is data (ESC to abort). NOTE: CHANGE THE IMAGE for next time step'
opt.call_wait_key( cv2.imshow(caption, d_img_proc) )
sidx = str(idx).zfill(digits)
pth1 = join(TARGET_DIR, 'c_img_{}.png'.format(sidx))
pth2 = join(TARGET_DIR, 'd_img_{}.png'.format(sidx))
pth3 = join(TARGET_DIR, 'd_img_proc_{}.png'.format(sidx))
cv2.imwrite(pth1, c_img)
cv2.imwrite(pth2, d_img)
cv2.imwrite(pth3, d_img_proc)
print("Saved:")
print(" {}".format(pth1))
print(" {}".format(pth2))
print(" {}".format(pth3))
idx += 1
def deploy(self):
"""Deploy!!
Careful, did you run the first phases beforehand to check that (a) there
exist target images you can use, and (b) that the poses in rviz look
reasonable.
"""
idx = 0
# Pick images to try from the saved file, and then current image.
pth_end = 'tmp_physical_targs/d_img_proc_000.png'
img_end = cv2.imread(pth_end)
c_img, d_img, d_img_proc = self.get_images()
assert c_img.shape == d_img_proc.shape == (480,640,3), c_img.shape
assert d_img.shape == (480,640), d_img.shape
# Visualize current and target image initially.
caption1 = 'Full size images. ESC to abort, other key to proceed.'
img_t = d_img_proc
hstack1 = np.concatenate((img_t, img_end), axis=1)
opt.call_wait_key( cv2.imshow(caption1, hstack1) )
# Next, network predictions.
t_input = transform_imgs(img_t, img_end, self.transforms_valid)
t_img_t = t_input['img_t'].unsqueeze(0) # after unsqueezes, both
t_img_end = t_input['img_tp1'].unsqueeze(0) # w/shape: (1,3,224,224)
out_pos, out_ang = self.act_predictor(t_img_t, t_img_end)
assert t_img_t.shape == t_img_end.shape == (1,3,224,224)
out_pos = out_pos.cpu().detach().squeeze()
out_ang = out_ang.cpu().detach().squeeze()
# Post-process to visualize for humans. See `deploy_test` for some
# earlier tests I did. We should eventually make it less dependent on
# hard-coded values, but only if we're changing the transforms.
w, h = 224, 224
pred_pos_proc = int(out_pos[0]*w), int(out_pos[1]*h)
# Get predicted position w.r.t. the original (480,640)-sized image.
pred_pos = (pred_pos_proc[0] + 16, pred_pos_proc[1] + 16)
widthf = 640.0 / 256.0
heightf = 480.0 / 256.0
pred_pos = ( int(pred_pos[0]*widthf), int(pred_pos[1]*heightf) )
# Show and visualize to the user.
caption2 = 'Predicted Pos on full image: {}'.format(pred_pos)
img = np.copy(img_t)
assert img.shape == (480,640,3), img.shape
cv2.circle(img, center=pred_pos, radius=4, color=opt.BLUE, thickness=-1)
cv2.circle(img, center=pred_pos, radius=6, color=opt.GREEN, thickness=1)
hstack2 = np.concatenate((img, img_end), axis=1)
opt.call_wait_key( cv2.imshow(caption2, hstack2) )
# Some debugging/logging.
print("pred_pos for 224x224 img: {} (scaled {})".format(out_pos, pred_pos_proc))
print("pred_pos for 480x640 img: {}".format(pred_pos))
print("out angle (logits): {}".format(out_ang))
print("current idx for grasp pose: {}".format(idx))
# TODO: later, save in a principled manner.
# Broadcast the target grasp onto world space. See `main/deploy.py` for details.
self.gripper.find_pick_region_net(pred_pos, c_img, d_img, idx, 'BOTTOM')
print("We have now broadcasted the bed_0 and bed_i_0 poses !!! Now execute grasp")
time.sleep(1)
# Rotate the robot 90 degrees so it now faces the bed, w.r.t. the map frame.
self.omni_base.move(geometry.pose(ek=1.57), 500.0, ref_frame_id='map')
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 cfg.USE_WEB_INTERFACE:
self.wl = Web_Labeler()
else:
self.wl = Python_Labeler(self.cam)
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
self.tt.find_table(self.robot)
self.ins = InitialSampler(self.cam)
self.grasp_count = 0
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.g_detector = Analytic_Grasp()
self.sn = Success_Net(self.whole_body, self.tt, self.cam,
self.omni_base)
c_img = self.cam.read_color_data()
#self.test_current_point()
time.sleep(4)
#thread.start_new_thread(self.ql.run,())
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 bed_make(self):
self.rollout_stats = []
self.get_new_grasp = True
if cfg.INS_SAMPLE:
u_c, d_c = self.ins.sample_initial_state()
self.rollout_stats.append([u_c, d_c])
self.new_grasp = True
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img == None and not d_img == None):
if self.new_grasp:
self.position_head()
else:
self.new_grasp = True
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
#CHANGE HERE
grasp_factor = 3
img_small = cv2.resize(np.copy(c_img), (640 / 3, 480 / 3))
sgraspt = time.time()
data = self.g_detector.get_grasp(img_small, grasp_factor)
egraspt = time.time()
print("Grasp predict time: " + str(egraspt - sgraspt))
data = 3 * data
IPython.embed()
self.record_stats(c_img, d_img, data, self.side, 'grasp')
self.gripper.find_pick_region_net(data, c_img, d_img,
self.grasp_count)
pick_found, bed_pick = self.check_card_found()
if self.side == "BOTTOM":
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
else:
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
self.check_success_state(c_img, d_img)
def check_success_state(self, c_img, d_img):
if self.side == "BOTTOM":
success, data, c_img = self.sn.check_bottom_success(self.wl)
else:
success, data, c_img = self.sn.check_top_success(self.wl)
self.record_stats(c_img, d_img, data, self.side, 'success')
print "WAS SUCCESFUL: "
print success
if (success):
if self.side == "BOTTOM":
self.transition_to_top()
else:
self.transition_to_start()
self.update_side()
else:
self.new_grasp = False
self.grasp_count += 1
if self.grasp_count > cfg.GRASP_OUT:
self.transition_to_start()
def update_side(self):
if self.side == "BOTTOM":
self.side = "TOP"
def transition_to_top(self):
if cfg.DEBUG_MODE:
self.com.save_stat(self.rollout_stats)
self.tt.move_to_pose(self.omni_base, 'lower_mid')
sys.exit()
else:
self.move_to_top_side()
def transition_to_start(self):
self.com.save_stat(self.rollout_stats)
self.move_to_start()
sys.exit()
def record_stats(self, c_img, d_img, data, side, typ):
grasp_point = {}
grasp_point['c_img'] = c_img
grasp_point['d_img'] = d_img
if typ == "grasp":
grasp_point['net_pose'] = data
else:
grasp_point['net_trans'] = data
grasp_point['side'] = side
grasp_point['type'] = typ
self.rollout_stats.append(grasp_point)
def position_head(self):
if self.side == "TOP":
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
elif self.side == "BOTTOM":
self.tt.move_to_pose(self.omni_base, 'lower_start')
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
def move_to_top_side(self):
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'top_mid')
def move_to_start(self):
if self.side == "BOTTOM":
self.tt.move_to_pose(self.omni_base, 'lower_mid')
else:
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'lower_mid')
def check_bottom_success(self):
self.tt.move_to_pose(self.omni_base, 'lower_mid')
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
def check_card_found(self):
time.sleep(1)
# try:
transforms = self.tl.getFrameStrings()
cards = []
try:
for transform in transforms:
print transform
current_grasp = 'bed_' + str(self.grasp_count)
if current_grasp in transform:
print 'got here'
f_p = self.tl.lookupTransform('map', transform,
rospy.Time(0))
cards.append(transform)
except:
rospy.logerr('bed pick not found yet')
return True, cards
class BedMaker():
def __init__(self, args):
"""For data collection of bed-making, NOT the deployment.
Assumes we roll out the robot's policy via code (not via human touch).
This is the 'slower' way where we have the python interface that the
human clicks on to indicate grasping points. Good news is, our deployment
code is probably going to be similar to this.
For joystick: you only need it plugged in for the initial state sampler,
which (at the moment) we are not even using.
"""
self.robot = robot = hsrb_interface.Robot()
self.rgbd_map = RGBD2Map()
self.omni_base = robot.get('omni_base')
self.whole_body = robot.get('whole_body')
self.cam = RGBD()
self.com = COM()
self.wl = Python_Labeler(cam=self.cam)
# View mode: STANDARD (the way I was doing earlier), CLOSE (the way they want).
self.view_mode = cfg.VIEW_MODE
# Set up initial state, table, etc.
self.com.go_to_initial_state(self.whole_body)
self.tt = TableTop()
# For now, a workaround. Ugly but it should do the job ...
#self.tt.find_table(robot)
self.tt.make_fake_ar()
self.tt.find_table_workaround(robot)
#self.ins = InitialSampler(self.cam)
self.side = 'BOTTOM'
self.grasp_count = 0
# Bells and whistles; note the 'success check' to check if transitioning
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
robot.get('gripper'))
self.sc = Success_Check(self.whole_body, self.tt, self.cam,
self.omni_base)
time.sleep(4)
print(
"Finished creating BedMaker()! Get the bed set up and run bed-making!"
)
if cfg.INS_SAMPLE:
print("TODO: we don't have sampling code here.")
# When we start, spin this so we can check the frames. Then un-comment,
# etc. It's the current hack we have to get around crummy AR marker detection.
if args.phase == 1:
print("Now doing rospy.spin() because phase = 1.")
rospy.spin()
def bed_make(self):
"""Runs the pipeline for data collection.
You can run this for multiple bed-making trajectories.
For now, though, assume one call to this means one trajectory.
"""
self.rollout_data = []
self.get_new_grasp = True
# I think, creates red line in GUI where we adjust the bed to match it.
# But in general we better fix our sampler before doing this for real.
# Don't forget to press 'B' on the joystick to get past this screen.
if cfg.INS_SAMPLE:
u_c, d_c = self.ins.sample_initial_state()
self.rollout_data.append([u_c, d_c])
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img.all() == None and not d_img.all() == None):
if self.get_new_grasp:
self.position_head()
# Human supervisor labels. data = dictionary of relevant info
data = self.wl.label_image(c_img)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
self.add_data_point(c_img, d_img, data, self.side, 'grasp')
# Broadcasts grasp pose
self.gripper.find_pick_region_labeler(
data, c_img, d_img, self.grasp_count)
# Execute the grasp and check for success. But if VIEW_MODE is
# close, better to reset to a 'nicer' position for base movement.
pick_found, bed_pick = self.check_card_found()
if self.side == "BOTTOM":
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'lower_start')
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
else:
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'top_mid')
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
self.check_success_state()
def check_success_state(self):
"""
Checks whether a single grasp in a bed-making trajectory succeeded.
Depends on which side of the bed the HSR is at. Invokes human supervisor
and transitions the HSR if successful.
"""
if self.side == "BOTTOM":
success, data = self.sc.check_bottom_success(self.wl)
else:
success, data = self.sc.check_top_success(self.wl)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
self.add_data_point(c_img, d_img, data, self.side, 'success')
print("WAS SUCCESFUL: {}".format(success))
# Handle transitioning to different side
if success:
if self.side == "BOTTOM":
self.transition_to_top()
else:
self.transition_to_start()
self.update_side()
self.grasp_count += 1
self.get_new_grasp = True
else:
self.grasp_count += 1
# If grasp failure, invokes finding region again and add new data
self.gripper.find_pick_region_labeler(data, c_img, d_img,
self.grasp_count)
self.add_data_point(c_img, d_img, data, self.side, 'grasp')
self.get_new_grasp = False
def update_side(self):
"""TODO: extend to multiple side switches?"""
if self.side == "BOTTOM":
self.side = "TOP"
def transition_to_top(self):
"""Transition to top (not bottom)."""
self.move_to_top_side()
def transition_to_start(self):
"""Transition to start=bottom, save rollout data, exit program.
Saves to a supervisor's directory since we're using a supervisor.
"""
self.com.save_rollout(self.rollout_data)
self.move_to_start()
sys.exit()
def add_data_point(self, c_img, d_img, data, side, typ, pose=None):
"""Adds a dictionary to the `rollout_data` list."""
grasp_point = {}
grasp_point['c_img'] = c_img
grasp_point['d_img'] = d_img
if pose == None:
label = data['objects'][0]['box']
pose = [(label[2] - label[0]) / 2.0 + label[0],
(label[3] - label[1]) / 2.0 + label[1]]
grasp_point['pose'] = pose
grasp_point['class'] = data['objects'][0]['class']
grasp_point['side'] = side
grasp_point['type'] = typ
self.rollout_data.append(grasp_point)
def position_head(self):
"""Position the head for a grasp attempt.
After playing around a bit, I think `head_tilt_joint` should be set last.
"""
self.whole_body.move_to_go()
if self.side == "BOTTOM":
self.tt.move_to_pose(self.omni_base, 'lower_start_tmp')
self.whole_body.move_to_joint_positions(
{'arm_flex_joint': -np.pi / 16.0})
self.whole_body.move_to_joint_positions(
{'head_pan_joint': np.pi / 2.0})
self.whole_body.move_to_joint_positions({'arm_lift_joint': 0.120})
self.whole_body.move_to_joint_positions(
{'head_tilt_joint': -np.pi / 4.0})
def move_to_top_side(self):
"""Assumes we're at the bottom and want to go to the top."""
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'right_mid')
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'top_mid_tmp')
def move_to_start(self):
"""Assumes we're at the top and we go back to the start."""
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'right_mid')
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'lower_mid')
def check_card_found(self):
"""Looks up the pose for where the HSR's hand should go to."""
transforms = self.tl.getFrameStrings()
cards = []
try:
for transform in transforms:
current_grasp = 'bed_' + str(self.grasp_count)
if current_grasp in transform:
print('found {}'.format(current_grasp))
f_p = self.tl.lookupTransform('map', transform,
rospy.Time(0))
cards.append(transform)
except:
rospy.logerr('bed pick not found yet')
return True, cards
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 cfg.USE_WEB_INTERFACE:
self.wl = Web_Labeler()
else:
self.wl = Python_Labeler(cam=self.cam)
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.ins = InitialSampler(self.cam)
self.gp = GraspPlanner()
self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options,
self.robot.get('gripper'))
self.sc = Success_Check(self.whole_body, self.tt, self.cam,
self.omni_base)
self.ss = Self_Supervised(self.cam)
#self.test_current_point()
time.sleep(4)
#thread.start_new_thread(self.ql.run,())
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 bed_make(self):
self.rollout_data = []
self.get_new_grasp = True
if cfg.INS_SAMPLE:
u_c, d_c = self.ins.sample_initial_state()
self.rollout_data.append([u_c, d_c])
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img == None and not d_img == None):
if self.get_new_grasp:
self.position_head()
data = self.wl.label_image(c_img)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
self.add_data_point(c_img, d_img, data, self.side, 'grasp')
self.gripper.find_pick_region_labeler(
data, c_img, d_img, self.grasp_count)
if cfg.SS_LEARN:
grasp_points = self.ss.learn(self.whole_body,
self.grasp_count)
self.add_ss_data(grasp_points, data, self.side,
'grasp')
pick_found, bed_pick = self.check_card_found()
if self.side == "BOTTOM":
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
else:
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
self.check_success_state()
def check_success_state(self):
if self.side == "BOTTOM":
success, data = self.sc.check_bottom_success(self.wl)
else:
success, data = self.sc.check_top_success(self.wl)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
self.add_data_point(c_img, d_img, data, self.side, 'success')
print "WAS SUCCESFUL: "
print success
if (success):
if cfg.SS_LEARN:
grasp_points = self.ss.learn(self.whole_body, self.grasp_count)
self.add_ss_data(grasp_points, data, self.side, 'success')
if self.side == "BOTTOM":
self.transition_to_top()
else:
self.transition_to_start()
self.update_side()
self.grasp_count += 1
self.get_new_grasp = True
else:
self.grasp_count += 1
self.gripper.find_pick_region_labeler(data, c_img, d_img,
self.grasp_count)
self.add_data_point(c_img, d_img, data, self.side, 'grasp')
self.get_new_grasp = False
if cfg.SS_LEARN:
grasp_points = self.ss.learn(self.whole_body, self.grasp_count)
self.add_ss_data(grasp_points, data, self.side, 'success')
def update_side(self):
if self.side == "BOTTOM":
self.side = "TOP"
def transition_to_top(self):
if cfg.DEBUG_MODE:
self.com.save_rollout(self.rollout_data)
self.tt.move_to_pose(self.omni_base, 'lower_mid')
sys.exit()
else:
self.move_to_top_side()
def transition_to_start(self):
self.com.save_rollout(self.rollout_data)
self.move_to_start()
sys.exit()
def add_data_point(self, c_img, d_img, data, side, typ, pose=None):
grasp_point = {}
grasp_point['c_img'] = c_img
grasp_point['d_img'] = d_img
if pose == None:
label = data['objects'][0]['box']
pose = [(label[2] - label[0]) / 2.0 + label[0],
(label[3] - label[1]) / 2.0 + label[1]]
grasp_point['pose'] = pose
grasp_point['class'] = data['objects'][0]['class']
grasp_point['side'] = side
grasp_point['type'] = typ
self.rollout_data.append(grasp_point)
def position_head(self):
if self.side == "TOP":
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
elif self.side == "BOTTOM":
self.tt.move_to_pose(self.omni_base, 'lower_start')
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
def add_ss_data(self, g_points, data, side, typ):
for g_point in g_points:
self.add_data_point(g_point['c_img'],
g_point['d_img'],
data,
side,
typ,
pose=g_point['pose'])
def move_to_top_side(self):
self.tt.move_to_pose(self.omni_base, 'right_down')
#self.tt.move_to_pose(self.omni_base,'right_mid')
self.tt.move_to_pose(self.omni_base, 'right_up')
self.tt.move_to_pose(self.omni_base, 'top_mid')
def move_to_start(self):
self.tt.move_to_pose(self.omni_base, 'right_up')
#self.tt.move_to_pose(self.omni_base,'right_mid')
self.tt.move_to_pose(self.omni_base, 'right_down')
self.tt.move_to_pose(self.omni_base, 'lower_mid')
def check_bottom_success(self):
self.tt.move_to_pose(self.omni_base, 'lower_mid')
self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8})
def check_card_found(self):
# try:
transforms = self.tl.getFrameStrings()
cards = []
try:
for transform in transforms:
print transform
current_grasp = 'bed_' + str(self.grasp_count)
if current_grasp in transform:
print 'got here'
f_p = self.tl.lookupTransform('map', transform,
rospy.Time(0))
cards.append(transform)
except:
rospy.logerr('bed pick not found yet')
return True, cards