def create_annotations(stamps, meanings, bagfile, video_dir): # initialize with basic information of start, stop, look times and description seg_infos = bp.joy_to_annotations(stamps, meanings) frame_stamps = [t["look"] for t in seg_infos] print frame_stamps key_rgb_imgs, key_depth_imgs = bp.get_video_frames(video_dir, frame_stamps) bag = rosbag.Bag(bagfile) jnames, jstamps, traj = bp.extract_joints(bag) key_joint_inds = np.searchsorted(jstamps, frame_stamps) key_joints = [traj[i] for i in key_joint_inds] for i,t in enumerate(frame_stamps): print 'Looking for key-points in segment %i.'%i keypoint_info = {} Twk = svi.find_Twk(key_joints[i], jnames) while True: svi.show_pointclouds([key_rgb_imgs[i]]) kp = raw_input('Which key points are important for this segment?\nChoices are: ' + str(svi.KEYPOINTS) + '.\n Please only enter one key point at a time: ') kp = kp.strip() if kp not in svi.KEYPOINTS: print 'Invalid keypoint: %s'%kp elif svi.KEYPOINTS_FULL[kp] in keypoint_info.keys(): if yes_or_no('Already have information for %s. Overwrite?'%kp): kp_loc = svi.find_kp_processing(kp, frame_stamps[i], Twk, video_dir) if kp_loc is None: print 'Something went wrong with keypoint selection.' else: keypoint_info[svi.KEYPOINTS_FULL[kp]] = kp_loc print 'Key point %s saved for this segment.'%kp else: kp_loc = svi.find_kp_processing(kp, frame_stamps[i], Twk, video_dir) if kp_loc is None: print 'Something went wrong with keypoint selection.' else: keypoint_info[svi.KEYPOINTS_FULL[kp]] = kp_loc print 'Key point %s saved for this segment.'%kp if not yes_or_no('Enter another key point for this segment?'): break seg_infos[i]['key_points'] = keypoint_info seg_infos[i]['ar_marker_poses'] = get_ar_marker_poses(key_rgb_imgs[i], key_depth_imgs[i], Twk) seg_infos[i]['extra_information'] = [] if yes_or_no('Is the needle-tip the relevant end effector for this segment?'): if yes_or_no('Is the needle in the left gripper?'): seg_infos[i]['extra_information'].append("l_grab") else: seg_infos[i]['extra_information'].append("r_grab") if not seg_infos[i]["extra_information"]: seg_infos[i]["extra_information"].append("nothing") return seg_infos
def store_in_yaml_kp (name, kpl): import yaml, rospy rospy.init_node("store_points", disable_signals = True) #subprocess.call("killall XnSensorServer", shell=True) pr2 = PR2.PR2() grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() point_sets = {} d = 0 while True: print "Demo %i"%(d+1) #xyz_tf, rgb = svi.get_kp_clouds(grabber, 1, T_w_k) while True: key_points = {} T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot) for kp in kpl: key_points[svi.KEYPOINTS_FULL[kp]] = svi.find_kp_execution(kp, grabber, T_w_k) if yes_or_no.yes_or_no("Satisfied?"): break point_sets[d] = fk.key_points_to_points(key_points, True) if not yes_or_no.yes_or_no("Do you want to save another situation?"): break d += 1 print point_sets with open(name, 'w') as fl: yaml.dump(point_sets, fl)
def exec_traj_maybesim(bodypart2traj): if args.animation: dof_inds = [] trajs = [] for (part_name, traj) in bodypart2traj.items(): manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name] dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices()) trajs.append(traj) full_traj = np.concatenate(trajs, axis=1) Globals.robot.SetActiveDOFs(dof_inds) animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True) if args.execution: if not args.prompt or yes_or_no("execute?"): pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj) else: return False return True
def store_normals_in_yaml (name): import yaml, rospy #subprocess.call("killall XnSensorServer", shell=True) rospy.init_node("store_normals", disable_signals = True) pr2 = PR2.PR2() grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() point_sets = {} d = 0 dist = 0.1 while True: print "Demo %i"%(d+1) point_sets[d] = [] T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot) for i in [1,2]: print "Save hole normal %i:"%i kp_loc = svi.find_kp_execution('hn%i'%i, grabber, T_w_k) p = kp_loc[0] n = kp_loc[1] point_sets[d].append([p[0], p[1], p[2]]) point_sets[d].append([p[0]+dist*n[0]/2, p[1]+dist*n[1]/2, p[2]+dist*n[2]/2]) point_sets[d].append([p[0]+dist*n[0], p[1]+dist*n[1], p[2]+dist*n[2]]) if not yes_or_no.yes_or_no("Do you want to save another situation?"): break d += 1 print point_sets with open(name, 'w') as fl: yaml.dump(point_sets, fl)
def exec_traj_maybesim(bodypart2traj): if args.animation: dof_inds = [] trajs = [] for (part_name, traj) in bodypart2traj.items(): manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name] dof_inds.extend( Globals.robot.GetManipulator(manip_name).GetArmIndices()) trajs.append(traj) full_traj = np.concatenate(trajs, axis=1) Globals.robot.SetActiveDOFs(dof_inds) animate_traj.animate_traj(full_traj, Globals.robot, restore=False, pause=True) if args.execution: if not args.prompt or yes_or_no("execute?"): pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj) else: return False return True
def store_in_yaml (name): import yaml, rospy rospy.init_node("store_points", disable_signals = True) #subprocess.call("killall XnSensorServer", shell=True) pr2 = PR2.PR2() grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() n = input("Enter the number of points you are saving: ") point_sets = {} d = 0 while True: print "Demo %i"%(d+1) point_sets[d] = [] T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot) xyz_tf, rgb = svi.get_kp_clouds(grabber, 1, T_w_k) for _ in xrange(n): print "Save point set (make sure you click on them in the same order every situation)." point = svi.find_kp_single_cloud("nt", xyz_tf, rgb) point_sets[d].append([float(point[0]), float(point[1]), float(point[2])]) if not yes_or_no.yes_or_no("Do you want to save another situation?"): break d += 1 print point_sets with open(name, 'w') as fl: yaml.dump(point_sets, fl)
started_video = True time.sleep(9999) except KeyboardInterrupt: print colorize("got control-c", "green") finally: if started_bag: bag_handle.send_signal(signal.SIGINT) bag_handle.wait() if started_video: video_handle.send_signal(signal.SIGINT) video_handle.wait() bagfilename = demo_name+".bag" if yes_or_no("save demo?"): annfilename = demo_name+".ann.yaml" call_and_print("generate_annotations.py %s %s"%(bagfilename, annfilename)) with open(args.master_file,"a") as fh: fh.write("\n" "- bag_file: %(bagfilename)s\n" " annotation_file: %(annfilename)s\n" " video_dir: %(videodir)s" " demo_name: %(demoname)s"%dict(bagfilename=bagfilename, annfilename=annfilename, videodir=demo_name, demoname=demo_name)) else: shutil.rmtree(demo_name) #video dir os.unlink(bagfilename)
from rapprentice.call_and_print import call_and_print from rapprentice.yes_or_no import yes_or_no import os.path as osp import itertools import yaml started_bag = False started_video = False localtime = time.localtime() time_string = time.strftime("%Y-%m-%d-%H-%M-%S", localtime) os.chdir(osp.dirname(args.master_file)) if not osp.exists(args.master_file): yn = yes_or_no("master file does not exist. create?") basename = raw_input("what is the base name?\n").strip() if yn: with open(args.master_file,"w") as fh: fh.write(""" name: %s h5path: %s bags: """%(basename, basename+".h5")) else: print "exiting." exit(0) with open(args.master_file, "r") as fh: master_info = yaml.load(fh) if master_info["bags"] is None: master_info["bags"] = [] for suffix in itertools.chain("", (str(i) for i in itertools.count())): demo_name = args.demo_prefix + suffix if not any(bag["demo_name"] == demo_name for bag in master_info["bags"]):
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.execution: rospy.init_node("exec_task",disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### started_bag = False started_video = False localtime = time.localtime() time_string = time.strftime("%Y-%m-%d-%H-%M-%S", localtime) os.chdir(osp.dirname(args.new_demo)) if not osp.exists(args.new_demo): yn = yes_or_no("master file does not exist. create?") basename = raw_input("what is the base name?\n").strip() if yn: with open(args.new_demo,'w') as fh: fh.write(""" name: %s h5path: %s bags: """%(basename, basename+".h5")) else: print "exiting." exit(0) with open(args.new_demo, "r") as fh: master_info = yaml.load(fh) if master_info["bags"] is None: master_info["bags"] = [] for suffix in itertools.chain("", (str(i) for i in itertools.count())): demo_name = args.demo_prefix + suffix if not any(bag["demo_name"] == demo_name for bag in master_info["bags"]): break print demo_name timestampfile = demo_name+"timestamps.txt" fht = open(timestampfile,"w") try: bag_cmd = "rosbag record /joint_states /joy -O %s"%demo_name #print colorize(bag_cmd, "green") bag_handle = subprocess.Popen(bag_cmd, shell=True) time.sleep(1) poll_result = bag_handle.poll() print "poll result", poll_result if poll_result is not None: raise Exception("problem starting video recording") else: started_bag = True video_cmd = "record_rgbd_video --out=%s --downsample=%i"%(demo_name, args.downsample) #print colorize(video_cmd, "green") video_handle = subprocess.Popen(video_cmd, shell=True) started_video = True #grab_end(new_xyz) fht.write('look:' + str(rospy.get_rostime().secs)) ################################ redprint("Finding closest demonstration") seg_name = find_closest_manual(demofile, None) seg_info = demofile[seg_name] redprint("Generating end-effector trajectory") old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution=="real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'lr': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end lr2oldtraj[lr] = old_joint_traj ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] #old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) ee_link_name = "%s_gripper_tool_frame"%lr #new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] #new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) if args.execution: Globals.pr2.update_rave() #new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, # Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = old_joint_traj traj = {} for lr in 'lr': part_name = {"l":"larm", "r":"rarm"}[lr] traj[lr] = bodypart2traj[part_name] if i_miniseg ==0: redprint("Press enter to use current position as starting point") raw_input() arm_positions = {} (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.r_arm_joint_names) arm_positions['r'] = arm_position diff_r = np.array(arm_position - traj['r'][0,:]) maximum_r = max(abs(diff_r)) (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.l_arm_joint_names) arm_positions['l'] = arm_position diff_l = np.array(arm_position - traj['l'][0,:]) maximum_l = max(abs(diff_l)) maximum = max(maximum_l, maximum_r) speed = (20.0/360.0*2*(np.pi)) time_needed = maximum / speed initial_pos_traj = {} for lr in 'lr': part_name = {"l":"larm", "r":"rarm"}[lr] initial_pos_traj[part_name] = mu.interp2d(np.arange(0, time_needed, 0.01), np.array([0,time_needed]), np.array([arm_positions[lr], traj[lr][0,:]])) #initial_traj_length = initial_pos_traj.shape[0] #traj[lr] = np.concatenate((initial_pos_traj, traj_part[lr]), axis=0) #=======================send to controller====================== length_total = initial_pos_traj["larm"].shape[0] qs_l = np.resize(initial_pos_traj["larm"], (1, initial_pos_traj["larm"].shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps) qs_r = np.resize(initial_pos_traj["rarm"], (1, initial_pos_traj["rarm"].shape[0]*7))[0] #F = np.array(np.matrix(F).T) # 6 x n F = np.zeros((length_total,6)) F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x3n gripper = np.zeros((length_total,1)) gripper = np.resize(gripper, (1, gripper.shape[0]*1))[0] # Controller code in joint space pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0]) dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0]) # Gains as Kps and Kvs for testing Kps = [] Kvs = [] for i in range(length_total): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) #toAddJkvJ = np.diag(np.asarray([0, 0, 0, 0, 0, 0, 0])) #length = complete_force_traj.shape[0] JKpJ = np.asarray([toAddJkpJ for i in range(length_total)]) JKpJ = np.resize(JKpJ, (1, 49*length_total))[0] JKvJ = np.asarray([toAddJkvJ for i in range(length_total)]) JKvJ = np.resize(JKvJ, (1, 49*length_total))[0] # [traj, Kp, Kv, F, use_force, seconds] Kps = np.resize(Kps, (1, 36 * length_total))[0] Kvs = np.resize(Kvs, (1, 36 * length_total))[0] #JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0] #JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0] stamps = asarray(seg_info['stamps']) data = np.zeros((1, length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)+2)) data[0][0:length_total*7] = qs_r data[0][length_total*7:length_total*(7+49)] = JKpJ data[0][length_total*(7+49):length_total*(7+49+49)] = JKvJ data[0][length_total*(7+49+49):length_total*(7+49+49+6)] = F data[0][length_total*(7+49+49+6):length_total*(7+49+49+6+36)] = Kps data[0][length_total*(7+49+49+6+36):length_total*(7+49+49+6+36+36)] = Kvs data[0][length_total*(7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7)] = qs_l data[0][length_total*(7+49+49+6+36+36+7):length_total*(7+49+49+6+36+36+7+49)] = JKpJ data[0][length_total*(7+49+49+6+36+36+7+49):length_total*(7+49+49+6+36+36+7+49+49)] = JKvJ data[0][length_total*(7+49+49+6+36+36+7+49+49):length_total*(7+49+49+6+36+36+7+49+49+6)] = F data[0][length_total*(7+49+49+6+36+36+7+49+49+6):length_total*(7+49+49+6+36+36+7+49+49+6+36)] = Kps data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36)] = Kvs data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)] = gripper data[0][-2] = 0 data[0][-1] = stamps[i_end] - stamps[i_start] msg = Float64MultiArray() msg.data = data[0].tolist() pub = rospy.Publisher("/controller_data", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) #===================end send to controller======================= raw_input("CAME TO START POSITION") time.sleep(1) fht.write('\nstart:' + str(rospy.get_rostime().secs)) ################################ redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) if not args.useHenry: for lr in 'lr': success &= set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) # Doesn't actually check if grab occurred, unfortunately print('1') fht.write('\nstart:' + str(rospy.get_rostime().secs)) print('2') if len(bodypart2traj) > 0: success &= exec_traj_maybesim(bodypart2traj) print('3') fht.write('\nstop:' + str(rospy.get_rostime().secs)) print('4') else: for lr in 'lr': redprint("Press enter to set gripper") raw_input() set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) # Doesn't actually check if grab occurred, unfortunately if bodypart2traj!={}: length = i_end - i_start + 1 length_total = length traj_r = traj['r'] qs_r = np.resize(traj_r, (1, traj_r.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps) traj_l = traj['l'] qs_l = np.resize(traj_l, (1, traj_l.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps) #F = np.array(np.matrix(F).T) # 6 x n F = np.zeros((length_total,6)) F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x3n gripper = np.zeros((length_total,1)) gripper = np.resize(gripper, (1, gripper.shape[0]*1))[0] # Controller code in joint space pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0]) dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0]) # Gains as Kps and Kvs for testing Kps = [] Kvs = [] for i in range(length_total): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) #toAddJkvJ = np.diag(np.asarray([0, 0, 0, 0, 0, 0, 0])) #length = complete_force_traj.shape[0] JKpJ = np.asarray([toAddJkpJ for i in range(length_total)]) JKpJ = np.resize(JKpJ, (1, 49*length_total))[0] JKvJ = np.asarray([toAddJkvJ for i in range(length_total)]) JKvJ = np.resize(JKvJ, (1, 49*length_total))[0] # [traj, Kp, Kv, F, use_force, seconds] Kps = np.resize(Kps, (1, 36 * length_total))[0] Kvs = np.resize(Kvs, (1, 36 * length_total))[0] #JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0] #JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0] stamps = asarray(seg_info['stamps']) data = np.zeros((1, length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)+2)) data[0][0:length_total*7] = qs_r data[0][length_total*7:length_total*(7+49)] = JKpJ data[0][length_total*(7+49):length_total*(7+49+49)] = JKvJ data[0][length_total*(7+49+49):length_total*(7+49+49+6)] = F data[0][length_total*(7+49+49+6):length_total*(7+49+49+6+36)] = Kps data[0][length_total*(7+49+49+6+36):length_total*(7+49+49+6+36+36)] = Kvs data[0][length_total*(7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7)] = qs_l data[0][length_total*(7+49+49+6+36+36+7):length_total*(7+49+49+6+36+36+7+49)] = JKpJ data[0][length_total*(7+49+49+6+36+36+7+49):length_total*(7+49+49+6+36+36+7+49+49)] = JKvJ data[0][length_total*(7+49+49+6+36+36+7+49+49):length_total*(7+49+49+6+36+36+7+49+49+6)] = F data[0][length_total*(7+49+49+6+36+36+7+49+49+6):length_total*(7+49+49+6+36+36+7+49+49+6+36)] = Kps data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36)] = Kvs data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)] = gripper data[0][-2] = 0 data[0][-1] = stamps[i_end] - stamps[i_start] msg = Float64MultiArray() msg.data = data[0].tolist() pub = rospy.Publisher("/controller_data", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) #if not success: break print("Segment %s result: %s"%(seg_name, success)) print("exit loop") for i in range(100): time.sleep(1) except KeyboardInterrupt: redprint("=================================DONE================================") raw_input() raw_input() raw_input() fht.write('\nstop:' + str(rospy.get_rostime().secs)) fht.write('\ndone:' + str(rospy.get_rostime().secs)) fht.close() time.sleep(3) if started_bag: print "stopping bag" bag_handle.send_signal(signal.SIGINT) #bag_handle.wait() started_bag = False if started_video: print "stopping video" video_handle.send_signal(signal.SIGINT) #video_handle.wait() started_video = False bagfilename = demo_name+".bag" annfilename = demo_name+".ann.yaml" call_and_print("generate_ann.py %s %s %s"%(bagfilename, annfilename, timestampfile),check=False) with open(args.new_demo,"a") as fh1: fh1.write("\n" "- bag_file: %(bagfilename)s\n" " annotation_file: %(annfilename)s\n" " video_dir: %(videodir)s\n" " demo_name: %(demoname)s"%dict(bagfilename=bagfilename, annfilename=annfilename, videodir=demo_name, demoname=demo_name)) return
import itertools import yaml if "localhost" in os.getenv("ROS_MASTER_URI"): raise Exception("on localhost!") started_bag = False started_video = False localtime = time.localtime() time_string = time.strftime("%Y-%m-%d-%H-%M-%S", localtime) os.chdir(osp.dirname(args.master_file)) if not osp.exists(args.master_file): yn = yes_or_no("master file does not exist. create?") basename = raw_input("what is the base name?\n").strip() if yn: with open(args.master_file, "w") as fh: fh.write(""" name: %s h5path: %s bags: """ % (basename, basename + ".h5")) else: print "exiting." exit(0) with open(args.master_file, "r") as fh: master_info = yaml.load(fh) if master_info["bags"] is None: master_info["bags"] = [] for suffix in itertools.chain("", (str(i) for i in itertools.count())):
parser = argparse.ArgumentParser() parser.add_argument( "path", help="script will create a sampledata directory in this location") parser.add_argument("--use_rsync", action="store_true") args = parser.parse_args() import os, urllib2, shutil from rapprentice.yes_or_no import yes_or_no from rapprentice.call_and_print import call_and_print if args.use_rsync: call_and_print( "rsync -azvu --delete [email protected]:/var/www/rapprentice/sampledata %s" % args.path) else: os.chdir(args.path) if os.path.exists("sampledata"): yn = yes_or_no("delete old directory") if yn: shutil.rmtree("sampledata") else: raise IOError os.mkdir("sampledata") print "downloading archive file" urlinfo = urllib2.urlopen( "http://rll.berkeley.edu/rapprentice/sampledata.tar") with open("sampledata.tar", "w") as fh: fh.write(urlinfo.read()) print "unpacking file" call_and_print("tar xvf sampledata.tar") os.unlink("sampledata.tar")
import argparse parser = argparse.ArgumentParser() parser.add_argument("path", help="script will create a sampledata directory in this location") parser.add_argument("--use_rsync", action="store_true") args = parser.parse_args() import os, urllib2, subprocess, shutil import os.path as osp import rapprentice from rapprentice.yes_or_no import yes_or_no from rapprentice.call_and_print import call_and_print if args.use_rsync: call_and_print("rsync -azvu --delete --exclude='*.tar' [email protected]:/var/www/rapprentice/sampledata %s"%args.path) else: os.chdir(args.path) if os.path.exists("sampledata"): yn = yes_or_no("delete old directory") if yn: shutil.rmtree("sampledata") else: raise IOError os.mkdir("sampledata") os.chdir("sampledata") print "downloading zip file" urlinfo = urllib2.urlopen("http://rll.berkeley.edu/rapprentice/sampledata/all.tar") with open("all.tar","w") as fh: fh.write(urlinfo.read()) print "unpacking file" call_and_print("tar xvf all.tar") os.unlink("all.tar")
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) rospy.init_node("exec_task",disable_signals=True) global filename if args.logging: name = raw_input ('Enter identifier of this experiment (without spaces): ') name = name + '.yaml' import os.path as osp filename = osp.join('/home/sibi/sandbox/rapprentice/experiments/', name) if not osp.exists(filename): with open(filename,'w') as fh: fh.write("experiment: %s\ninfo: \n"%name) #thc.start() if args.execution: Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] import trajoptpy.make_kinbodies as mk #Globals.env.Load("/home/sibi/sandbox/rapprentice/objects/table.xml") addBoxToRave(Globals.env, "table") Globals.sponge = addBoxToRave(Globals.env, "sponge") # Not sure about this Globals.needle_tip = mk.create_spheres(Globals.env, [(0,0,0)], radii=0.02, name="needle_tip") Globals.demo_env = Globals.env.CloneSelf(1) Globals.demo_env.StopSimulation() Globals.demo_robot = Globals.demo_env.GetRobot("pr2") Globals.demo_needle_tip = Globals.demo_env.GetKinBody("needle_tip") if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() thc.grabber = grabber #Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### #Globals.env.SetViewer('qtcoin') #threadClass().start() while True: redprint("Acquire point cloud") ################################ redprint("Finding closest demonstration") if args.fake_data_segment: seg_name = args.fake_data_segment else: seg_name = find_closest(demofile) seg_info = demofile[seg_name] # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"])) ################################ redprint("Generating end-effector trajectory") handles = [] if seg_info.get('ar_marker_poses') is None: use_markers = False else: use_markers = True if use_markers: old_marker_poses_str = seg_info['ar_marker_poses'] old_marker_poses = {} for i in old_marker_poses_str: old_marker_poses[int(i)] = old_marker_poses_str[i] # TODO: Have a check for only transformations -> rigid transformations if args.fake_data_segment: new_keypoints = demofile[args.fake_data_segment]["key_points"] if use_markers: new_marker_poses_str = demofile[args.fake_data_segment]["ar_marker_poses"] new_marker_poses = {} for i in new_marker_poses_str: new_marker_poses[int(i)] = new_marker_poses_str[i] if seg_info['key_points'].keys().sort() != new_keypoints.keys().sort(): print "Keypoints don't match." exit(1) else: if args.execution: Globals.pr2.update_rave() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_keypoints, new_marker_poses = fk.get_keypoints_execution(grabber, seg_info['key_points'].keys(), T_w_k, use_markers) print "Warping the points for the new situation." if "l_grab" in seg_info['extra_information'] or "r_grab" in seg_info['extra_information']: use_ntt_kp = False else: use_ntt_kp = True # Points from keypoints old_xyz, rigid, kp_mapping = fk.key_points_to_points(seg_info['key_points'], use_ntt_kp) new_xyz, _, _ = fk.key_points_to_points(new_keypoints, use_ntt_kp) # Points from markers if use_markers: old_common_poses, new_common_poses = find_common_marker_poses(old_marker_poses, new_marker_poses, new_keypoints.keys()) old_m_xyz, rigid_m, _ = fk.key_points_to_points(old_common_poses, False) new_m_xyz, _, _ = fk.key_points_to_points(new_common_poses, False) if len(old_m_xyz) > 0 and rigid: rigid = False elif rigid_m and not old_xyz.any(): rigid = True # concatenate points if old_xyz.any() and old_m_xyz.any(): old_xyz = np.r_[old_xyz, old_m_xyz] elif old_m_xyz.any(): old_xyz = old_m_xyz if new_xyz.any() and new_m_xyz.any(): new_xyz = np.r_[new_xyz, new_m_xyz] elif new_m_xyz.any(): new_xyz = new_m_xyz if new_xyz.any() and new_xyz.shape != (4,4): #handles.append(Globals.env.plot3(old_xyz,5, (1,0,0,1))) #handles.append(Globals.env.plot3(old_xyz,5, np.array([(1,0,0) for _ in xrange(old_xyz.shape[0])]))) handles.append(Globals.env.plot3(new_xyz,5, np.array([(0,0,1) for _ in xrange(old_xyz.shape[0])]))) print 'Old points:', old_xyz print 'New points:', new_xyz #if not yes_or_no.yes_or_no("Use identity?"): if rigid: f = registration.ThinPlateSpline() rel_tfm = new_xyz.dot(np.linalg.inv(old_xyz)) f.init_rigid_tfm(rel_tfm) bend_c = 0 rot_c = 0 scale_c = 0 max_error = None elif len(new_xyz) > 0: #f.fit(demopoints_m3, newpoints_m3, 10,10) # TODO - check if this regularization on bending is correct #if "right_hole_normal" in new_keypoints or "left_hole_normal" in new_keypoints: # bend_c = 0.1 # rot_c = [1e-5,1e-5,0.1] #wt = 5 #wt_n = np.ones(len(old_xyz)) #if kp_mapping.get("right_hole_normal"): # wt_n[kp_mapping["right_hole_normal"][0]] = wt #if kp_mapping.get("left_hole_normal"): # wt_n[kp_mapping["left_hole_normal"][0]] = wt #else: # bend_c = 0.1 # rot_c = 1e-5#[0,0,1e-5] # wt_n = None # f = registration.fit_ThinPlateSpline(old_xyz, new_xyz, bend_coef = bend_c,rot_coef = rot_c) bend_c = 0.05 rot_c = [1e-3, 1e-3, 1e-3] scale_c = 0.1 f = registration.fit_ThinPlateSpline_RotReg(old_xyz, new_xyz, bend_c, rot_c, scale_c) np.set_printoptions(precision=3) max_error = np.max(np.abs(f.transform_points(old_xyz) - new_xyz)) print "nonlinear part", f.w_ng print "affine part", f.lin_ag print "translation part", f.trans_g print "residual", f.transform_points(old_xyz) - new_xyz print "max error ", max_error else: f = registration.ThinPlateSpline() bend_c = 0 rot_c = 0 scale_c = 0 max_error = None # else: # f = registration.ThinPlateSpline() # bend_c = 0 # rot_c = 0 # if old_xyz.any() and old_xyz.shape != (4,4): tfm_xyz = f.transform_points(old_xyz) handles.append(Globals.env.plot3(tfm_xyz,5, np.array([(0,1,0) for _ in xrange(tfm_xyz.shape[0])]))) #f = registration.ThinPlateSpline() XXX XXX if new_xyz.any() and new_xyz.shape != (4,4): handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0), old_xyz.max(axis=0), xres = .1, yres = .1, zres = .04)) if args.ask: # if new_xyz.any() and new_xyz.shape != (4,4): # import visualize # visualize.plot_mesh_points(f.transform_points, old_xyz, new_xyz) #lines = plotting_openrave.gen_grid(f.transform_points, np.array([0,-1,0]), np.array([1,1,1])) #plotting_openrave.plot_lines(lines) if not yes_or_no.yes_or_no("Continue?"): continue miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends # Assuming only lgrab or rgrab use_needle = None for lr in 'lr': if "%s_grab"%lr in seg_info['extra_information']: if "needle_tip_transform" in seg_info['key_points']: demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform']) ntfm = np.array(new_keypoints['needle_tip_transform']) use_needle = lr elif Globals.rel_ntfm is not None: T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform() T_g_n = Globals.rel_ntfm ntfm = T_w_g.dot(T_g_n) T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform() T_g_n_demo = Globals.demo_rel_ntfm demo_ntfm = T_w_g_demo.dot(T_g_n_demo) use_needle = lr if use_needle is not None: setup_needle_grabs(use_needle, seg_info["joint_states"]["name"], seg_info["joint_states"]["look_position"], demo_ntfm, ntfm) if args.logging: with open(filename,'a') as fh: fh.write("- seg_name: %s\n"%seg_name) fh.write(" tps_info: \n") if max_error is not None: res_cost, bend_cost, tot_cost = f.fitting_cost(new_xyz, bend_c) fh.write(" res_cost: %f\n bend_cost: %f\n tot_cost: %f\n"%(res_cost, bend_cost, tot_cost)) fh.write(" max_tps_error: %f\n"%max_error) else: fh.write(" res_cost: -1\n bend_cost: -1\n tot_cost: -1\n max_tps_error: -1\n") fh.write(" trajopt_info: \n") for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution: Globals.pr2.update_rave() # Changing the trajectory according to the end-effector full_traj = np.c_[seg_info["leftarm"][i_start:i_end+1], seg_info["rightarm"][i_start:i_end+1]] full_traj = mu.remove_duplicate_rows(full_traj) _, ds_traj = resampling.adaptive_resample(full_traj, tol=.01, max_change=.1) # about 2.5 degrees, 10 degrees Globals.robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()]) Globals.demo_robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()]) Globals.demo_robot.SetDOFValues(Globals.robot.GetDOFValues()) ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) bodypart2traj = {} arms_used = "" if args.logging: with open(filename, 'a') as fh: fh.write(" mini_seg%i: \n"%i_miniseg) for lr in 'lr': # if "%s_grab"%lr in seg_info['extra_information']: # if "needle_tip_transform" in seg_info['key_points']: # demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform']) # ntfm = np.array(new_keypoints['needle_tip_transform']) # use_needle = True # elif Globals.rel_ntfm is not None: # T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform() # T_g_n = Globals.rel_ntfm # ntfm = T_w_g.dot(T_g_n) # T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform() # T_g_n_demo = Globals.demo_rel_ntfm # demo_ntfm = T_w_g_demo.dot(T_g_n_demo) # use_needle = True # else: # use_needle = False # else: # use_needle = False if use_needle == lr: Globals.sponge.Enable(False) arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr] old_ee_traj = setup_needle_traj (lr, arm_traj) link = Globals.needle_tip.GetLinks()[0] redprint("Using needle for trajectory execution...") else: arm = {"l":"leftarm", "r":"rightarm"}[lr] Globals.demo_robot.SetActiveDOFs(Globals.robot.GetManipulator(arm).GetArmIndices()) if seg_name == "8_knot_tie_next0_seg00": Globals.sponge.Enable(False) else: Globals.sponge.Enable(True) ee_link_name = "%s_gripper_tool_frame"%lr link = Globals.robot.GetLink(ee_link_name) #old_ee_traj = np.asarray(seg_info[ee_link_name]["hmat"]) demo_link = Globals.demo_robot.GetLink(ee_link_name) old_ee_traj = [] arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr] for row in arm_traj: Globals.demo_robot.SetActiveDOFValues(row) old_ee_traj.append(demo_link.GetTransform()) ############ CAN YOU PLOT THIS OLD_EE_TRAJ? old_ee_traj = np.asarray(old_ee_traj) old_joint_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr] if lr == 'l':#arm_moved(old_joint_traj): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] new_ee_traj = f.transform_hmats(old_ee_traj) #new_ee_traj = old_ee_traj #################### CAN YOU PLOT THIS NEW_EE_TRAJ? handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1))) #old_poses = [conv.hmat_to_pose(hmat) for hmat in old_ee_traj] #print new_ee_traj #new_poses = [conv.hmat_to_pose(hmat) for hmat in new_ee_traj] update_markers(new_ee_traj, old_ee_traj) #thc.run() if args.execution: Globals.pr2.update_rave() new_joint_traj, costs, cnts, pos_err = plan_follow_traj(Globals.robot, manip_name, link, new_ee_traj, old_joint_traj, True) new_joint_traj = unwrap_joint_traj (lr, new_joint_traj) # (robot, manip_name, ee_link, new_hmats, old_traj): part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj arms_used += lr if args.logging: with open(filename, 'a') as fh: fh.write(" - %s: \n"%lr) fh.write(" costs: \n") tot_cost = 0 for c_name, c_val in costs: fh.write(" - %s: %f\n"%(c_name, c_val)) tot_cost += c_val fh.write(" - tot_cost: %f\n"%tot_cost) fh.write(" constraints: \n") for cnt in cnts: fh.write(" - %s\n"%str(cnt)) fh.write(" max_pos_error: %f\n"%pos_err) ################################ redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, arms_used)) for lr in 'lr': set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) #trajoptpy.GetViewer(Globals.env).Idle() if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj, speed_factor=1) # TODO measure failure condtions if not success: break Globals.robot.ReleaseAllGrabbed() Globals.demo_robot.ReleaseAllGrabbed() redprint("Segment %s result: %s"%(seg_name, success)) if args.fake_data_segment: break