def do_single(demo_name, stage_num, prev_demo_index, verb_data_accessor, prev_and_cur_pc2): if stage_num == 0: do_stage(demo_name, stage_num, None, None, prev_and_cur_pc2[1], verb_data_accessor) else: prev_stage_num = stage_num - 1 prev_demo_name = "%s%i" % (demo_base_name, prev_demo_index) prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num) call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller") print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True) yes_or_no("type y to continue") call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller") do_stage(demo_name, stage_num, prev_stage_info, prev_and_cur_pc2[0], prev_and_cur_pc2[1], verb_data_accessor)
def exec_traj_do_work(l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles, traj_ik_func, obj_cloud_xyz, obj_name, can_move_lower): del Globals.handles[1:] grab_obj_kinbody = setup_obj_rave(obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None body_traj = {} for (lr, gripper_poses, gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses], [l_gripper_angles, r_gripper_angles]): if len(gripper_poses) == 0: continue gripper_poses, gripper_angles = np.array(gripper_poses), np.array(gripper_angles) gripper_angles_grabbing = process_gripper_angles_for_grabbing(lr, gripper_angles) Globals.pr2.update_rave() current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform() if can_move_lower: gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing else: gripper_poses_remove, gripper_angles_remove = remove_lower_poses(current_pos, gripper_poses, gripper_angles_grabbing) gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start(current_pos, gripper_poses_remove, gripper_angles_remove, 15) final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend #do ik joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses) if len(joint_positions) == 0: return ExecTrajectoryResponse(success=False) unwrapped_joint_positions = unwrap_angles(joint_positions) final_joint_positions = unwrapped_joint_positions body_traj["%s_arm"%lr] = final_joint_positions body_traj["%s_gripper"%lr] = final_gripper_angles yn = yes_or_no("continue?") if yn: lt.follow_trajectory_with_grabs(Globals.pr2, body_traj) while not yes_or_no("continue to next stage?"): lt.follow_trajectory_with_grabs(Globals.pr2, body_traj) if grab_obj_kinbody is not None: handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles) raw_input("Press enter when done viewing trajectory") Globals.pr2.join_all() return ExecTrajectoryResponse(success=True) else: return ExecTrajectoryResponse(success=False)
def do_single(demo_base_name, demo_index, stage_num, prev_demo_index): if stage_num == 0: do_stage(demo_base_name, demo_index, stage_num, None, None) else: prev_stage_num = stage_num - 1 prev_demo_name = demo_base_name + str(prev_demo_index) prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num) prev_exp_pc = do_segmentation(prev_stage_info.item) call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller") print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True) yes_or_no("type y to continue") call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller") do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, np.array([prev_exp_pc]))
def execute_dual_arm_trajectory(pr2, lquat, lpos, lgrip, left_used, rquat, rpos, rgrip, right_used, rope): rviz.draw_curve(lpos, RVIZ_LPOS, width=.005) rviz.draw_curve(rpos, RVIZ_RPOS, width=.005) ok = yes_or_no("trajectory ok?") if not ok: print "canceling trajectory" return for i in xrange(len(lquat)): if left_used: brett.larm.set_cart_target(normalize(quatswap(lquat[i])), lpos[i], "/base_footprint") if lgrip[i] < .03: lgrip[i] = 0 brett.lgrip.set_angle_target(lgrip[i]) if right_used: brett.rarm.set_cart_target(normalize(quatswap(rquat[i])), rpos[i], "/base_footprint") if rgrip[i] < .03: rgrip[i] = 0 brett.rgrip.set_angle_target(rgrip[i]) rviz.draw_curve(rope[i], RVIZ_ORIG_ROPE, width=.01, rgba = (1,0,0,1)) sleep(.04)
def erase_all_data(group): consent = yes_or_no("erase all data in %s?" % group.name) if consent: for name in group.keys(): erase_dataset(group, name) else: raise IOError
def execute_dual_arm_trajectory(pr2, lquat, lpos, lgrip, left_used, rquat, rpos, rgrip, right_used, rope): rviz.draw_curve(lpos, RVIZ_LPOS, width=.005) rviz.draw_curve(rpos, RVIZ_RPOS, width=.005) ok = yes_or_no("trajectory ok?") if not ok: print "canceling trajectory" return for i in xrange(len(lquat)): if left_used: brett.larm.set_cart_target(normalize(quatswap(lquat[i])), lpos[i], "/base_footprint") if lgrip[i] < .03: lgrip[i] = 0 brett.lgrip.set_angle_target(lgrip[i]) if right_used: brett.rarm.set_cart_target(normalize(quatswap(rquat[i])), rpos[i], "/base_footprint") if rgrip[i] < .03: rgrip[i] = 0 brett.rgrip.set_angle_target(rgrip[i]) rviz.draw_curve(rope[i], RVIZ_ORIG_ROPE, width=.01, rgba=(1, 0, 0, 1)) sleep(.04)
def make_and_execute_verb_traj(verb, arg_clouds): if "expansion" in verb_info[verb]: prim_verbs = verb_info[verb]["expansion"] print "expanding into primitive actions: %s" % " ".join(prim_verbs) for i in xrange(len(prim_verbs)): exec_success = make_and_execute_verb_traj(prim_verbs[i], arg_clouds[i:i + 1]) if exec_success == False: rospy.logerr("stopping complex verb %s", verb) return False else: if len(F[verb]["demos"]) == 0: rospy.logerr("verb %s has no demonstrations!", verb) return False make_req = MakeTrajectoryRequest() make_req.object_clouds = [ xyz2pc(cloud, "base_footprint") for cloud in arg_clouds ] make_req.verb = verb make_resp = make_svc.call(make_req) approved = yes_or_no("execute trajectory?") if approved: exec_req = ExecTrajectoryRequest(make_resp.traj) exec_resp = exec_svc.call(exec_req) if exec_resp.success == False: rospy.logerr("%s failed :(", verb) return False else: rospy.loginfo("%s succeeded!", verb)
def make_and_execute_verb_traj(verb, arg_clouds): if "expansion" in verb_info[verb]: prim_verbs = verb_info[verb]["expansion"] print "expanding into primitive actions: %s"%" ".join(prim_verbs) for i in xrange(len(prim_verbs)): exec_success = make_and_execute_verb_traj(prim_verbs[i], arg_clouds[i:i+1]) if exec_success == False: rospy.logerr("stopping complex verb %s", verb) return False else: if len(F[verb]["demos"]) == 0: rospy.logerr("verb %s has no demonstrations!", verb) return False make_req = MakeTrajectoryRequest() make_req.object_clouds = [xyz2pc(cloud, "base_footprint") for cloud in arg_clouds] make_req.verb = verb make_resp = make_svc.call(make_req) approved = yes_or_no("execute trajectory?") if approved: exec_req = ExecTrajectoryRequest(make_resp.traj) exec_resp = exec_svc.call(exec_req) if exec_resp.success == False: rospy.logerr("%s failed :(", verb) return False else: rospy.loginfo("%s succeeded!", verb)
def get_all_clouds_pc2(num_objs): clouds = [] for obj_num in xrange(num_objs): next_cloud = filter_pc2(do_segmentation("object%i" % obj_num)) while not yes_or_no("Continue?"): next_cloud = filter_pc2(do_segmentation("object%i" % obj_num)) clouds.append(next_cloud) return clouds
def do_empty_move(demo_name, exp_name, test_dir_name): empty_move_init() verb_data_accessor = multi_item_verbs.VerbDataAccessor(test_info_dir=osp.join("test", TEST_DATA_DIR, test_dir_name)) for stage_num in xrange(verb_data_accessor.get_num_stages(demo_name)): # info and data for previous stage if stage_num > 0: prev_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num-1) prev_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num-1) prev_exp_data = verb_data_accessor.get_demo_stage_data(prev_exp_info.stage_name) prev_exp_pc = prev_exp_data["object_cloud"][prev_exp_info.item]["xyz"] else: # first stage has no previous items prev_demo_info, prev_exp_info, prev_exp_data, prev_exp_pc = None, None, None, None # info and data for current stage cur_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num) cur_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num) cur_exp_data = verb_data_accessor.get_demo_stage_data(cur_exp_info.stage_name) cur_exp_pc = cur_exp_data["object_cloud"][cur_exp_info.item]["xyz"] if stage_num == 0: world_to_grip_transform_func = None else: world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_tf("%s_gripper_tool_frame" % cur_exp_info.arms_used) warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(demo_name, cur_exp_pc, "base_footprint", stage_num, prev_demo_info, prev_exp_pc, verb_data_accessor, world_to_grip_transform_func, transform_type="tps_zrot") yn = yes_or_no("continue?") if yn: can_move_lower = (stage_num == 0) traj = warped_traj_resp.traj exec_verb_traj.exec_traj_do_work(traj.l_gripper_poses.poses, traj.l_gripper_angles, traj.r_gripper_poses.poses, traj.r_gripper_angles, ik_functions.do_traj_ik_graph_search, cur_exp_pc, cur_demo_info.item, can_move_lower) if stage_num < verb_data_accessor.get_num_stages(demo_name)-1: yn = yes_or_no("continue to next stage?") if not yn: break
def mkdir_ask(path,make_path=False): if os.path.exists(path): consent = yes_or_no("%s already exists. Delete it?"%path) if consent: shutil.rmtree(path) else: raise IOError if make_path: os.makedirs(path) else: os.mkdir(path)
def mkdir_ask(path, make_path=False): if os.path.exists(path): consent = yes_or_no("%s already exists. Delete it?" % path) if consent: shutil.rmtree(path) else: raise IOError if make_path: os.makedirs(path) else: os.mkdir(path)
def record_demo_stage(name_for_motion, item_name, data_dir): os.chdir(data_dir + "/images") get_point_cloud(name_for_motion, item_name) while not yes_or_no("Ready to continue?"): get_point_cloud(name_for_motion, item_name) os.chdir(data_dir + "/bags") record_trajectory(args.dry_run, name_for_motion) n_tries = 20 if not args.dry_run: rename_bag_file(name_for_motion, n_tries)
def get_all_clouds_pc2(num_objs, save_file=None): clouds_pc2 = [] for obj_num in xrange(num_objs): next_cloud = filter_pc2(do_segmentation("object%i" % obj_num)) while not yes_or_no("Continue?"): next_cloud = filter_pc2(do_segmentation("object%i" % obj_num)) clouds_pc2.append(next_cloud) if save_file is not None: save_pcs(clouds_pc2, save_file) return clouds_pc2
def record_demonstration_for_motion(name_for_motion, item_name): # get the point cloud for the first item os.chdir(data_dir + "/images") call_and_print("get_point_cloud.py %s" % (name_for_motion)) print colorize("now, select the %s object" % (item_name), color="red", bold=True) # two point clouds need to be stored, so give first point cloud a suffix of '-1' call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s" % (name_for_motion, item_name)) yes_or_no("Ready to continue?") # do the demonstration for the first motion print colorize("now, human, demonstrate the next action for %s on the robot" % (name_for_motion), color="red", bold=True) os.chdir(data_dir + "/bags") call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller") # record the demonstration for the first motion try: old_tmp_bags = glob("tmpname*.bag") if not args.dry_run: for bag in old_tmp_bags: os.remove(bag) call_and_print("rosbag record /tf /joint_states /joy -o tmpname") except KeyboardInterrupt: pass # rename bag file that was just created n_tries = 20 if not args.dry_run: success = False for i in xrange(n_tries): try: bagname = glob("tmpname*.bag")[0] os.rename(bagname, "%s.bag" % (name_for_motion)) success = True except IndexError: time.sleep(.1) if not success: raise Exception("couldn't get bag file") call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
def execute(self, userdata): #if not args.test: draw_table() Globals.pr2.update_rave() if yes_or_no('about to execute trajectory. save?'): import pickle fname = '/tmp/trajectory_' + str(np.random.randint(9999999999)) + '.pkl' with open(fname, 'w') as f: pickle.dump(userdata.trajectory, f) print 'saved to', fname success = lfd_traj.follow_trajectory_with_grabs(Globals.pr2, userdata.trajectory) raw_input('done executing segment. press enter to continue') if success: if args.count_steps: Globals.stage += 1 return "success" else: return "failure"
def do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, prev_exp_clouds): demo_name = demo_base_name + str(demo_index) stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num) pc_sel = do_segmentation(stage_info.item) make_req = get_trajectory_request(stage_info.verb, pc_sel) make_resp = make_verb_traj.make_traj_multi_stage(make_req, stage_info, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor) yn = yes_or_no("continue?") if yn: exec_req = ExecTrajectoryRequest() exec_req.traj = make_resp.traj exec_verb_traj.exec_traj(exec_req, traj_ik_func=exec_verb_traj.do_traj_ik_graph_search) # return stage info and object clouds so they can be saved for use in the next stage if necessary return (stage_info, make_req.object_clouds)
def execute(self, userdata): #if not args.test: draw_table() Globals.pr2.update_rave() if yes_or_no('about to execute trajectory. save?'): import pickle fname = '/tmp/trajectory_' + str( np.random.randint(9999999999)) + '.pkl' with open(fname, 'w') as f: pickle.dump(userdata.trajectory, f) print 'saved to', fname success = lfd_traj.follow_trajectory_with_grabs( Globals.pr2, userdata.trajectory) raw_input('done executing segment. press enter to continue') if success: if args.count_steps: Globals.stage += 1 return "success" else: return "failure"
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor): stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num) make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2) make_resp = multi_item_make_verb_traj.make_traj_multi_stage( make_req, demo_name, stage_num, prev_stage_info, prev_exp_pc2, verb_data_accessor, "tps_zrot" ) can_move_lower = stage_num == 0 yn = yes_or_no("continue?") if yn: exec_req = ExecTrajectoryRequest() exec_req.traj = make_resp.traj exec_verb_traj.exec_traj( exec_req, traj_ik_func=ik_functions.do_traj_ik_graph_search, obj_pc=cur_exp_pc2, obj_name=stage_info.item, can_move_lower=can_move_lower, )
def exec_traj(req, traj_ik_func=do_traj_ik_default): assert isinstance(req, ExecTrajectoryRequest) del Globals.handles[1:] traj = req.traj body_traj = {} for (lr,gripper_poses, gripper_angles) in zip("lr",[traj.l_gripper_poses.poses,traj.r_gripper_poses.poses], [traj.l_gripper_angles,traj.r_gripper_angles]): if len(gripper_poses) == 0: continue gripper_angles = np.array(gripper_angles) rospy.logwarn("warning! gripper angle hacks") gripper_angles[gripper_angles < .04] = gripper_angles[gripper_angles < .04] - .02 gripper_angles = get_proper_gripper_angles(lr, gripper_angles) plot_gripper_xyzs_from_poses(lr, gripper_poses) #do ik joint_positions = traj_ik_func(lr, gripper_poses) if len(joint_positions) == 0: return ExecTrajectoryResponse(success=False) joint_positions = ku.smooth_positions(joint_positions, .15) body_traj["%s_arm"%lr] = joint_positions body_traj["%s_gripper"%lr] = gripper_angles pose_array = gm.PoseArray() pose_array.header.frame_id = "base_footprint" pose_array.header.stamp = rospy.Time.now() pose_array.poses = traj.l_gripper_poses.poses if lr == 'l' else traj.r_gripper_poses.poses Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1))) yn = yes_or_no("continue?") if yn: lt.go_to_start(Globals.pr2, body_traj) lt.follow_trajectory_with_grabs(Globals.pr2, body_traj) Globals.pr2.join_all() return ExecTrajectoryResponse(success=True) else: return ExecTrajectoryResponse(success=False)
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor): stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num) make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2) if stage_num == 0: grip_to_world_transform_func = None else: grip_to_world_transform_func = multi_item_make_verb_traj.make_grip_to_world_transform_tf("%s_gripper_tool_frame" % verb_data_accessor.get_stage_info(demo_name, stage_num).arms_used) make_resp = multi_item_make_verb_traj.make_traj_multi_stage(make_req, demo_name, stage_num, prev_stage_info, prev_exp_pc2, verb_data_accessor, "tps_zrot") can_move_lower = (stage_num == 0) yn = yes_or_no("continue?") if yn: exec_req = ExecTrajectoryRequest() exec_req.traj = make_resp.traj exec_verb_traj.exec_traj(exec_req, traj_ik_func=ik_functions.do_traj_ik_graph_search, obj_pc=cur_exp_pc2, obj_name=stage_info.item, can_move_lower=can_move_lower)
os.rename(bagname, "%s.bag"%demo_name) success = True except IndexError: time.sleep(.1) if not success: raise Exception("couldn't get bag file") call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller") new_entry_text = """ # AUTOMATICALLY ADDED BY teach_verb.py ON %(datestr)s %(demo_name)s: bag_file: bags/%(demo_name)s.bag seg_file: images/%(demo_name)s.seg.h5 verb: %(verb)s args: [%(target)s] arms_used: %(arms_used)s """%dict( datestr = datetime.date.today(), demo_name = demo_name, verb = verb, target = target, arms_used = arms_used) yn = yes_or_no("save demonstration?") if yn: with open(data_dir + "/verb_demos2.yaml", "a") as demofile: if not args.dry_run: demofile.write(new_entry_text) call_and_print("make_verb_library2.py") else: print "exiting without saving"
assert args.order is not None assert args.reset is not None from comm.vision_pipeline import make_towel_pipeline, make_robot_rope_pipeline2, make_human_rope_pipeline from comm import pipeline from comm import comm import os, subprocess from jds_utils.yes_or_no import yes_or_no comm.initComm() os.chdir(comm.DATA_ROOT) if args.pipeline == "robot_rope": if args.reset == "full": yn = yes_or_no("are you sure you want to delete everything?") if yn: subprocess.check_call("rm -r *", shell=True) else: exit(0) elif args.reset == "partial": subprocess.check_call( "rm -rf kinect labels images rope_pts towel_pts rope_init rope_model logs/* joint_states base_pose once/table_corners.txt", shell=True) elif args.reset == "no": pass PIPELINE = make_robot_rope_pipeline2( classifier= "/home/joschu/python/jds_image_proc/pixel_classifiers/rope_sdh_light_afternoon/classifier.pkl", downsample=5) if args.pipeline == "human_rope":
def execute(self,userdata): """ - lookup closest trajectory from database - if it's a terminal state, we're done - warp it based on the current rope returns: done, not_done, failure """ xyz_new = np.squeeze(np.asarray(userdata.points)) #if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025) xyz_new_ds, ds_inds = downsample(xyz_new) dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds) ELOG.log('SelectTrajectory', 'xyz_new', xyz_new) ELOG.log('SelectTrajectory', 'xyz_new_ds', xyz_new_ds) ELOG.log('SelectTrajectory', 'dists_new', dists_new) if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage] else: candidate_demo_names = demos.keys() global last_selected_segment print 'Last selected segment:', last_selected_segment if args.human_select_demo: best_name = None while best_name not in demos: print 'Select demo from', demos.keys() best_name = raw_input('> ') else: from joblib import parallel costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names) #costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names] #costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names] ELOG.log('SelectTrajectory', 'costs_names', costs_names) _, best_name = min(costs_names) ELOG.log('SelectTrajectory', 'best_name', best_name) best_demo = demos[best_name] if best_demo["done"]: rospy.loginfo("best demo was a 'done' state") return "done" best_demo = demos[best_name] rospy.loginfo("best segment name: %s", best_name) last_selected_segment = best_name xyz_demo_ds = best_demo["cloud_xyz_ds"] ELOG.log('SelectTrajectory', 'xyz_demo_ds', xyz_demo_ds) if args.test: n_iter = 21 else: n_iter = 101 if args.use_rigid: self.f = registration.Translation2d() self.f.fit(xyz_demo_ds, xyz_new_ds) ELOG.log('SelectTrajectory', 'f', self.f) else: self.f, info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, return_full=True)#, interactive=True) ELOG.log('SelectTrajectory', 'f', self.f) ELOG.log('SelectTrajectory', 'f_info', info) if args.use_nr: rospy.loginfo('Using nonrigidity costs') from lfd import tps import scipy.spatial.distance as ssd pts_grip = [] for lr in "lr": if best_demo["arms_used"] in ["b", lr]: pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"]) pts_grip = np.array(pts_grip) dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1) pts_grip_near_rope = pts_grip[dist_to_rope < .04,:] pts_rigid = voxel_downsample(pts_grip_near_rope, .01) self.f.lin_ag, self.f.trans_g, self.f.w_ng, self.f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5) # print 'correspondences', self.f.corr_nm #################### Generate new trajectory ################## #### Plot original and warped point clouds ####### # orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint") # warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint") # Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST)) # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST)) #### Plot grid ######## mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0) maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0) mins[2] -= .1 maxes[2] += .1 grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint') Globals.handles.append(grid_handle) #### Actually generate the trajectory ########### warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo) # if yes_or_no('dump warped demo?'): # import pickle # fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl' # with open(fname, 'w') as f: # pickle.dump(warped_demo, f) # print 'saved to', fname ELOG.log('SelectTrajectory', 'warped_demo', warped_demo) def make_traj(warped_demo, inds=None, xyz_offset=0, feas_check_only=False): traj = {} total_feas_inds = 0 total_inds = 0 for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if args.hard_table: clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) pos = warped_demo["%s_gripper_tool_frame"%lr]["position"] + xyz_offset ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"] if inds is not None: pos, ori = pos[inds], ori[inds] if feas_check_only: feas_inds = lfd_traj.compute_feas_inds( pos, ori, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True) traj["%s_arm_feas_inds"%lr] = feas_inds else: arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search( pos, ori, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True) traj["%s_arm"%lr] = arm_traj traj["%s_arm_feas_inds"%lr] = feas_inds total_feas_inds += len(feas_inds) total_inds += len(pos) rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(pos)) return traj, total_feas_inds, total_inds # Check if we need to move the base for reachability base_offset = np.array([0, 0, 0]) if args.use_base: # First figure out how much we need to move the base to maximize feasible points OFFSET = 0.1 XYZ_OFFSETS = np.array([[0., 0., 0.], [-OFFSET, 0, 0], [OFFSET, 0, 0], [0, -OFFSET, 0], [0, OFFSET, 0]]) inds_to_check = lfd_traj.where_near_rope(best_demo, xyz_demo_ds, add_other_points=30) need_to_move_base = False best_feas_inds, best_xyz_offset = -1, None for xyz_offset in XYZ_OFFSETS: _, n_feas_inds, n_total_inds = make_traj(warped_demo, inds=inds_to_check, xyz_offset=xyz_offset, feas_check_only=True) rospy.loginfo('Cloud offset %s has feas inds %d', str(xyz_offset), n_feas_inds) if n_feas_inds > best_feas_inds: best_feas_inds, best_xyz_offset = n_feas_inds, xyz_offset if n_feas_inds >= 0.99*n_total_inds: break if np.linalg.norm(best_xyz_offset) > 0.01: need_to_move_base = True base_offset = -best_xyz_offset rospy.loginfo('Best base offset: %s, with %d feas inds', str(base_offset), best_feas_inds) # Move the base if need_to_move_base: rospy.loginfo('Will move base.') userdata.base_offset = base_offset return 'move_base' else: rospy.loginfo('Will not move base.') Globals.pr2.update_rave() # calculate joint trajectory using IK trajectory = make_traj(warped_demo)[0] # fill in gripper/grab stuff for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if len(trajectory["%s_arm_feas_inds"%lr]) == 0: return "failure" trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07 trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr] trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0 # smooth any discontinuities in the arm traj for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts( trajectory["%s_arm"%lr], Globals.pr2.env, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, ignore_inds=[1] # ignore the 0--1 discontinuity, which is usually just moving from rest to the traj starting pose ) # after smoothing the arm traj, we need to fill in all other trajectories (in both arms) other_lr = 'r' if lr == 'l' else 'l' if best_demo["arms_used"] in [other_lr, "b"]: trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps) for tmp_lr in 'lr': if best_demo["arms_used"] in [tmp_lr, "b"]: trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0 # plotting for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: # plot warped trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST, ns='warped_finger_traj' )) # plot original trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST, ns='demo_finger_traj' )) ELOG.log('SelectTrajectory', 'trajectory', trajectory) userdata.trajectory = trajectory if args.prompt_before_motion: consent = yes_or_no("trajectory ok?") else: consent = True if consent: return "not_done" else: return "failure"
from comm.vision_pipeline import make_towel_pipeline, make_robot_rope_pipeline2, make_human_rope_pipeline from comm import pipeline from comm import comm import os, subprocess from jds_utils.yes_or_no import yes_or_no comm.initComm() os.chdir(comm.DATA_ROOT) if args.pipeline == "robot_rope": if args.reset == "full": yn = yes_or_no("are you sure you want to delete everything?") if yn: subprocess.check_call("rm -r *", shell=True) else: exit(0) elif args.reset == "partial": subprocess.check_call("rm -rf kinect labels images rope_pts towel_pts rope_init rope_model logs/* joint_states base_pose once/table_corners.txt",shell=True) elif args.reset == "no": pass PIPELINE = make_robot_rope_pipeline2( classifier="/home/joschu/python/jds_image_proc/pixel_classifiers/rope_sdh_light_afternoon/classifier.pkl", downsample=5) if args.pipeline == "human_rope": if args.reset == "full": yn = yes_or_no("are you sure you want to delete everything?")
success = True except IndexError: time.sleep(.1) if not success: raise Exception("couldn't get bag file") call_and_print( "rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller" ) new_entry_text = """ # AUTOMATICALLY ADDED BY teach_verb.py ON %(datestr)s %(demo_name)s: bag_file: bags/%(demo_name)s.bag seg_file: images/%(demo_name)s.seg.h5 verb: %(verb)s args: [%(target)s] arms_used: %(arms_used)s """ % dict(datestr=datetime.date.today(), demo_name=demo_name, verb=verb, target=target, arms_used=arms_used) yn = yes_or_no("save demonstration?") if yn: with open(data_dir + "/verb_demos2.yaml", "a") as demofile: if not args.dry_run: demofile.write(new_entry_text) call_and_print("make_verb_library2.py") else: print "exiting without saving"
def execute(self, userdata): """ - lookup closest trajectory from database - if it's a terminal state, we're done - warp it based on the current rope returns: done, not_done, failure """ xyz_new = np.squeeze(np.asarray(userdata.points)) #if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025) xyz_new_ds, ds_inds = downsample(xyz_new) dists_new = recognition.calc_geodesic_distances_downsampled_old( xyz_new, xyz_new_ds, ds_inds) if args.human_select_demo: raise NotImplementedError seg_name = trajectory_library.interactive_select_demo(demos) best_demo = demos[seg_name] pts0, _ = best_demo["cloud_xyz_ds"] pts1, _ = downsample(xyz_new) self.f = registration.tps_rpm(pts0, pts1, plotting=4, reg_init=1, reg_final=args.reg_final, n_iter=40) else: if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage] else: candidate_demo_names = demos.keys() from joblib import parallel costs_names = parallel.Parallel(n_jobs=-2)( parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names) #costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names] #costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names] _, best_name = min(costs_names) best_demo = demos[best_name] if best_demo["done"]: rospy.loginfo("best demo was a 'done' state") return "done" best_demo = demos[best_name] rospy.loginfo("best segment name: %s", best_name) xyz_demo_ds = best_demo["cloud_xyz_ds"] if args.test: n_iter = 21 else: n_iter = 101 if args.use_rigid: self.f = registration.Translation2d() self.f.fit(xyz_demo_ds, xyz_new_ds) else: self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting=20, reg_init=1, reg_final=.01, n_iter=n_iter, verbose=False) #, interactive=True) np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds) # print 'correspondences', self.f.corr_nm #################### Generate new trajectory ################## #### Plot original and warped point clouds ####### # orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint") # warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint") # Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST)) # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST)) #### Plot grid ######## mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0) maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0) mins[2] -= .1 maxes[2] += .1 grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint') Globals.handles.append(grid_handle) #### Actually generate the trajectory ########### warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo) if yes_or_no('dump warped demo?'): import pickle fname = '/tmp/warped_demo_' + str( np.random.randint(9999999999)) + '.pkl' with open(fname, 'w') as f: pickle.dump(warped_demo, f) print 'saved to', fname Globals.pr2.update_rave() trajectory = {} # calculate joint trajectory using IK for lr in "lr": leftright = {"l": "left", "r": "right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if args.hard_table: clipinplace( warped_demo["l_gripper_tool_frame"]["position"][:, 2], Globals.table_height + .032, np.inf) clipinplace( warped_demo["r_gripper_tool_frame"]["position"][:, 2], Globals.table_height + .032, np.inf) arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search( warped_demo["%s_gripper_tool_frame" % lr]["position"], warped_demo["%s_gripper_tool_frame" % lr]["orientation"], Globals.pr2.robot.GetManipulator("%sarm" % leftright), "%s_gripper_tool_frame" % lr, check_collisions=True) if len(feas_inds) == 0: return "failure" trajectory["%s_arm" % lr] = arm_traj trajectory["%s_grab" % lr] = best_demo["%s_gripper_joint" % lr] < .07 trajectory["%s_gripper" % lr] = warped_demo["%s_gripper_joint" % lr] trajectory["%s_gripper" % lr][trajectory["%s_grab" % lr]] = 0 # smooth any discontinuities in the arm traj for lr in "lr": leftright = {"l": "left", "r": "right"}[lr] if best_demo["arms_used"] in [lr, "b"]: trajectory[ "%s_arm" % lr], discont_times, n_steps = lfd_traj.smooth_disconts( trajectory["%s_arm" % lr], Globals.pr2.env, Globals.pr2.robot.GetManipulator("%sarm" % leftright), "%s_gripper_tool_frame" % lr) # after smoothing the arm traj, we need to fill in all other trajectories (in both arms) other_lr = 'r' if lr == 'l' else 'l' if best_demo["arms_used"] in [other_lr, "b"]: trajectory["%s_arm" % other_lr] = lfd_traj.fill_stationary( trajectory["%s_arm" % other_lr], discont_times, n_steps) for tmp_lr in 'lr': if best_demo["arms_used"] in [tmp_lr, "b"]: trajectory["%s_grab" % tmp_lr] = lfd_traj.fill_stationary( trajectory["%s_grab" % tmp_lr], discont_times, n_steps) trajectory["%s_gripper" % tmp_lr] = lfd_traj.fill_stationary( trajectory["%s_gripper" % tmp_lr], discont_times, n_steps) trajectory["%s_gripper" % tmp_lr][trajectory["%s_grab" % tmp_lr]] = 0 # plotting for lr in "lr": leftright = {"l": "left", "r": "right"}[lr] if best_demo["arms_used"] in [lr, "b"]: # plot warped trajectory Globals.handles.append( Globals.rviz.draw_curve(conversions.array_to_pose_array( alternate( warped_demo["%s_gripper_l_finger_tip_link" % lr]["position"], warped_demo["%s_gripper_r_finger_tip_link" % lr]["position"]), "base_footprint"), width=.001, rgba=(1, 0, 1, .4), type=Marker.LINE_LIST, ns='warped_finger_traj')) # plot original trajectory Globals.handles.append( Globals.rviz.draw_curve(conversions.array_to_pose_array( alternate( best_demo["%s_gripper_l_finger_tip_link" % lr]["position"], best_demo["%s_gripper_r_finger_tip_link" % lr]["position"]), "base_footprint"), width=.001, rgba=(0, 1, 1, .4), type=Marker.LINE_LIST, ns='demo_finger_traj')) userdata.trajectory = trajectory if args.prompt_before_motion: consent = yes_or_no("trajectory ok?") else: consent = True if consent: return "not_done" else: return "failure"
def create_segments(bag, link_names): """extract a bunch of dictionaries from bag file, each one corresponding to one segment of trajectory. each segment corresponds to 'look', 'start', and 'stop' button presses""" button_presses = get_button_presses(bag) all_times = read_button_presses(button_presses) print 'Events in bag file:', actions_to_str(all_times) if yes_or_no('edit events?'): s = raw_input( 'new action sequence (must be the same length as the original): ') #print all_times all_times = str_to_actions(s, [t for t, _ in all_times]) #print all_times start_times, stop_times, look_times, l_close_times, l_open_times, r_close_times, r_open_times, done_times = \ correct_bag_errors(all_times) assert len(start_times) == len(stop_times) == len(look_times) assert len(done_times) == 1 kinematics_info = extract_kinematics_from_bag(bag, link_names) N = len(kinematics_info["time"]) l_open = np.zeros(N, bool) l_close = np.zeros(N, bool) r_open = np.zeros(N, bool) r_close = np.zeros(N, bool) times = kinematics_info["time"] l_open[np.searchsorted(times, l_open_times)] = True l_close[np.searchsorted(times, l_close_times)] = True r_open[np.searchsorted(times, r_open_times)] = True r_close[np.searchsorted(times, r_close_times)] = True kinematics_info["l_open"] = l_open kinematics_info["l_close"] = l_close kinematics_info["r_open"] = r_open kinematics_info["r_close"] = r_close start_inds = np.searchsorted(times, start_times) stop_inds = np.searchsorted(times, stop_times) look_clouds = get_transformed_clouds(bag, look_times) seg_infos = [] for (start_ind, stop_ind, (xyz, bgr)) in zip(start_inds, stop_inds, look_clouds): seg_info = extract_segment(kinematics_info, start_ind, stop_ind) seg_info["arms_used"] = determine_arms_used(seg_info) seg_info["cloud_xyz"] = xyz seg_info["cloud_bgr"] = bgr seg_info["done"] = False seg_infos.append(seg_info) done_clouds = get_transformed_clouds(bag, done_times) for (xyz, bgr) in done_clouds: seg_info = {} seg_info["cloud_xyz"] = xyz seg_info["cloud_bgr"] = bgr seg_info["done"] = True seg_infos.append(seg_info) return seg_infos
def execute(self,userdata): """ - lookup closest trajectory from database - if it's a terminal state, we're done - warp it based on the current rope returns: done, not_done, failure """ xyz_new = np.squeeze(np.asarray(userdata.points)) #if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025) xyz_new_ds, ds_inds = downsample(xyz_new) dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds) if args.human_select_demo: raise NotImplementedError seg_name = trajectory_library.interactive_select_demo(demos) best_demo = demos[seg_name] pts0,_ = best_demo["cloud_xyz_ds"] pts1,_ = downsample(xyz_new) self.f = registration.tps_rpm(pts0, pts1, plotting = 4, reg_init=1,reg_final=args.reg_final,n_iter=40) else: if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage] else: candidate_demo_names = demos.keys() from joblib import parallel costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names) #costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names] #costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names] _, best_name = min(costs_names) best_demo = demos[best_name] if best_demo["done"]: rospy.loginfo("best demo was a 'done' state") return "done" best_demo = demos[best_name] rospy.loginfo("best segment name: %s", best_name) xyz_demo_ds = best_demo["cloud_xyz_ds"] if args.test: n_iter = 21 else: n_iter = 101 if args.use_rigid: self.f = registration.Translation2d() self.f.fit(xyz_demo_ds, xyz_new_ds) else: self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False)#, interactive=True) np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds) # print 'correspondences', self.f.corr_nm #################### Generate new trajectory ################## #### Plot original and warped point clouds ####### # orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint") # warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint") # Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST)) # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST)) #### Plot grid ######## mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0) maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0) mins[2] -= .1 maxes[2] += .1 grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint') Globals.handles.append(grid_handle) #### Actually generate the trajectory ########### warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo) if yes_or_no('dump warped demo?'): import pickle fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl' with open(fname, 'w') as f: pickle.dump(warped_demo, f) print 'saved to', fname Globals.pr2.update_rave() trajectory = {} # calculate joint trajectory using IK for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if args.hard_table: clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search( warped_demo["%s_gripper_tool_frame"%lr]["position"], warped_demo["%s_gripper_tool_frame"%lr]["orientation"], Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True ) if len(feas_inds) == 0: return "failure" trajectory["%s_arm"%lr] = arm_traj trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07 trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr] trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0 # smooth any discontinuities in the arm traj for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts( trajectory["%s_arm"%lr], Globals.pr2.env, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr ) # after smoothing the arm traj, we need to fill in all other trajectories (in both arms) other_lr = 'r' if lr == 'l' else 'l' if best_demo["arms_used"] in [other_lr, "b"]: trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps) for tmp_lr in 'lr': if best_demo["arms_used"] in [tmp_lr, "b"]: trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0 # plotting for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: # plot warped trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST, ns='warped_finger_traj' )) # plot original trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST, ns='demo_finger_traj' )) userdata.trajectory = trajectory if args.prompt_before_motion: consent = yes_or_no("trajectory ok?") else: consent = True if consent: return "not_done" else: return "failure"
def exec_traj_do_work(l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles, traj_ik_func, obj_cloud_xyz, obj_name, can_move_lower=True): del Globals.handles[1:] grab_obj_kinbody = setup_obj_rave( obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None body_traj = {} for (lr, gripper_poses, gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses], [l_gripper_angles, r_gripper_angles]): if len(gripper_poses) == 0: continue gripper_poses, gripper_angles = np.array(gripper_poses), np.array( gripper_angles) gripper_angles_grabbing = process_gripper_angles_for_grabbing( lr, gripper_angles) Globals.pr2.update_rave() current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame" % lr).GetTransform() if can_move_lower: gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing else: gripper_poses_remove, gripper_angles_remove = remove_lower_poses( current_pos, gripper_poses, gripper_angles_grabbing) gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start( current_pos, gripper_poses_remove, gripper_angles_remove, 15) final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend #do ik joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses) if len(joint_positions) == 0: return ExecTrajectoryResponse(success=False) unwrapped_joint_positions = unwrap_angles(joint_positions) final_joint_positions = unwrapped_joint_positions body_traj["%s_arm" % lr] = final_joint_positions body_traj["%s_gripper" % lr] = final_gripper_angles yn = yes_or_no("continue?") if yn: lt.follow_trajectory_with_grabs(Globals.pr2, body_traj) if grab_obj_kinbody is not None: handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles) raw_input("Press enter when done viewing trajectory") Globals.pr2.join_all() return ExecTrajectoryResponse(success=True) else: return ExecTrajectoryResponse(success=False)
for obj_name in demo_info["args"]: print "select the %s" % obj_name pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc_tf)).cloud_out make_req.object_clouds.append(pc_sel) else: object_clouds = [ demo_data["object_clouds"][obj_name]["xyz"] for obj_name in demo_data["object_clouds"].keys() ] for i in xrange(len(object_clouds)): cloud = object_clouds[i].reshape(-1, 3) #translation = np.random.randn(1,3) * np.array([[.1,.1,0]]) #cloud += translation center = cloud.mean(axis=0) from numpy import cos, sin, pi rotation = np.array([[cos(pi / 4), sin(pi / 4), 0], [-sin(pi / 4), cos(pi / 4), 0], [0, 0, 1]]) cloud = center[None, :] + np.dot(cloud - center[None, :], rotation.T) make_req.object_clouds.append(ru.xyz2pc(cloud, 'base_footprint')) make_resp = make_verb_traj.make_traj(make_req) yn = yes_or_no("continue?") if yn: #exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5 #exec_verb_traj.Globals.pr2.larm.vel_limits *= .5 exec_req = ExecTrajectoryRequest() exec_req.traj = make_resp.traj exec_verb_traj.exec_traj(exec_req)
import os.path as osp from jds_utils.yes_or_no import yes_or_no import os, sys import yaml import h5py data_dir = osp.join(osp.dirname(lfd.__file__), "data") task_file = osp.join(data_dir, "knot_demos.yaml") with open(osp.join(data_dir,task_file),"r") as fh: task_info = yaml.load(fh) db_file = osp.join(data_dir, task_info[args.task]["db_file"]) print 'Writing to', db_file if osp.exists(db_file): if yes_or_no(db_file + ' already exists. Overwrite?'): os.remove(db_file) else: print 'Aborting.' sys.exit(1) task_lib = h5py.File(db_file, mode="w") for (i_demo,bag_file) in enumerate(task_info[args.task]["demo_bags"]): bag = rosbag.Bag(bag_file) demo_segs = bag_proc.create_segments(bag, ["r_gripper_tool_frame", "r_gripper_l_finger_tip_link", "r_gripper_r_finger_tip_link", "l_gripper_tool_frame", "l_gripper_l_finger_tip_link", "l_gripper_r_finger_tip_link"]) for (i_seg,demo_seg) in enumerate(demo_segs): if demo_seg["done"]:
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2) pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id) for obj_name in demo_info["args"]: print "select the %s"%obj_name pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out make_req.object_clouds.append(pc_sel) else: object_clouds = [demo_data["object_clouds"][obj_name]["xyz"] for obj_name in demo_data["object_clouds"].keys()] for i in xrange(len(object_clouds)): cloud = object_clouds[i].reshape(-1,3) #translation = np.random.randn(1,3) * np.array([[.1,.1,0]]) #cloud += translation center = cloud.mean(axis=0) from numpy import cos,sin,pi rotation = np.array([[cos(pi/4), sin(pi/4), 0], [-sin(pi/4), cos(pi/4), 0], [0, 0, 1]]) cloud = center[None,:] + np.dot(cloud - center[None,:], rotation.T) make_req.object_clouds.append(ru.xyz2pc(cloud,'base_footprint')) make_resp = make_verb_traj.make_traj(make_req) yn = yes_or_no("continue?") if yn: #exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5 #exec_verb_traj.Globals.pr2.larm.vel_limits *= .5 exec_req = ExecTrajectoryRequest() exec_req.traj = make_resp.traj exec_verb_traj.exec_traj(exec_req)
def create_segments(bag, link_names): """extract a bunch of dictionaries from bag file, each one corresponding to one segment of trajectory. each segment corresponds to 'look', 'start', and 'stop' button presses""" button_presses = get_button_presses(bag) all_times = read_button_presses(button_presses) print 'Events in bag file:', actions_to_str(all_times) if yes_or_no('edit events?'): s = raw_input('new action sequence (must be the same length as the original): ') #print all_times all_times = str_to_actions(s, [t for t, _ in all_times]) #print all_times start_times, stop_times, look_times, l_close_times, l_open_times, r_close_times, r_open_times, done_times = \ correct_bag_errors(all_times) assert len(start_times) == len(stop_times) == len(look_times) assert len(done_times) == 1 kinematics_info = extract_kinematics_from_bag(bag, link_names) N = len(kinematics_info["time"]) l_open = np.zeros(N, bool) l_close = np.zeros(N, bool) r_open = np.zeros(N, bool) r_close = np.zeros(N, bool) times = kinematics_info["time"] l_open[np.searchsorted(times, l_open_times)] = True l_close[np.searchsorted(times, l_close_times)] = True r_open[np.searchsorted(times, r_open_times)] = True r_close[np.searchsorted(times, r_close_times)] = True kinematics_info["l_open"] = l_open kinematics_info["l_close"] = l_close kinematics_info["r_open"] = r_open kinematics_info["r_close"] = r_close start_inds = np.searchsorted(times, start_times) stop_inds = np.searchsorted(times, stop_times) look_clouds = get_transformed_clouds(bag, look_times) seg_infos = [] for (start_ind, stop_ind, (xyz, bgr)) in zip(start_inds, stop_inds, look_clouds): seg_info = extract_segment(kinematics_info, start_ind, stop_ind) seg_info["arms_used"] = determine_arms_used(seg_info) seg_info["cloud_xyz"] = xyz seg_info["cloud_bgr"] = bgr seg_info["done"] = False seg_infos.append(seg_info) done_clouds = get_transformed_clouds(bag, done_times) for (xyz, bgr) in done_clouds: seg_info = {} seg_info["cloud_xyz"] = xyz seg_info["cloud_bgr"] = bgr seg_info["done"] = True seg_infos.append(seg_info) return seg_infos
def erase_all_data(group): consent = yes_or_no("erase all data in %s?"%group.name) if consent: for name in group.keys(): erase_dataset(group, name) else: raise IOError