예제 #1
0
  def setup(task, table_bounds):
    if Globals.pr2 is None: 
      Globals.pr2 = PR2.PR2.create(rave_only=True)
      load_table(table_bounds)

    if Globals.rviz is None:
      Globals.rviz = ros_utils.RvizWrapper.create()
      # Globals.rviz.start_publisher_thread(Globals.handles)

    #Globals.table_height = rospy.get_param("table_height")

    if Globals.demos is None:
      data_dir = osp.join(osp.dirname(lfd.__file__), "data")
      with open(osp.join(data_dir, "knot_demos.yaml"),"r") as fh: 
        task_info = yaml.load(fh)
      H5FILE = osp.join(data_dir, task_info[task]["db_file"])
      demos_file = h5py.File(H5FILE,"r")
      rospy.loginfo("loading demos into memory")
      Globals.demos = warping.group_to_dict(demos_file)    
      demos_file.close()
      rospy.loginfo("preprocessing demo point clouds...")
      for (_,demo) in Globals.demos.items():
        demo["cloud_xyz_ds"], ds_inds = downsample(demo["cloud_xyz"])
        #demo["cloud_xyz_ds"], ds_inds = demo["cloud_xyz"].reshape(-1, 3), np.arange(len(demo['cloud_xyz'])).reshape(-1, 1)
        demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"])
        demo["geodesic_dists"] = recognition.calc_geodesic_distances_downsampled_old(demo["cloud_xyz"], demo["cloud_xyz_ds"], ds_inds)
      rospy.loginfo("done")
예제 #2
0
    def __init__(self):
        smach.State.__init__(self,
            outcomes = ["done", "not_done","failure", "move_base"],
            input_keys = ["points"],
            output_keys = ["trajectory", "base_offset"])

        rospy.loginfo("preprocessing demo point clouds...")
        for (_,demo) in demos.items():
            demo["cloud_xyz_ds"], ds_inds = downsample(demo["cloud_xyz"])
            demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"])
            demo["geodesic_dists"] = recognition.calc_geodesic_distances_downsampled_old(demo["cloud_xyz"], demo["cloud_xyz_ds"], ds_inds)

        if args.count_steps:
            self.count2segnames = defaultdict(list)
            for (name, demo) in demos.items():
                self.count2segnames[int(demo["seg_index"])].append(name)

        rospy.loginfo("done")
예제 #3
0
    def __init__(self):
        smach.State.__init__(self,
                             outcomes=["done", "not_done", "failure"],
                             input_keys=["points"],
                             output_keys=["trajectory"])

        rospy.loginfo("preprocessing demo point clouds...")
        for (_, demo) in demos.items():
            demo["cloud_xyz_ds"], ds_inds = downsample(demo["cloud_xyz"])
            demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"])
            demo[
                "geodesic_dists"] = recognition.calc_geodesic_distances_downsampled_old(
                    demo["cloud_xyz"], demo["cloud_xyz_ds"], ds_inds)

        if args.count_steps:
            self.count2segnames = defaultdict(list)
            for (name, demo) in demos.items():
                self.count2segnames[int(demo["seg_index"])].append(name)

        rospy.loginfo("done")
예제 #4
0
    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"
예제 #5
0
    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"
예제 #6
0
    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"
예제 #7
0
def select_trajectory(points, curr_robot_joint_vals, curr_step):
  """
  - 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(points))
  #if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
  xyz_new_ds, ds_inds = downsample(xyz_new)
#  xyz_new_ds, ds_inds = xyz_new.reshape(-1,3), np.arange(0, len(xyz_new)).reshape(-1, 1)
  dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
  candidate_demo_names = Globals.demos.keys()

  #from joblib import parallel
  #costs_names = parallel.Parallel(n_jobs = 4)(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 sorted(candidate_demo_names)]
  _, best_name = min(costs_names)
  print "choices: ", candidate_demo_names

  best_name = None
  while best_name not in Globals.demos:
    best_name = raw_input("type name of trajectory you want to use\n")
    rospy.loginfo('costs_names %s', costs_names)

  #matcher = recognition.CombinedNNMatcher(recognition.DataSet.LoadFromDict(Globals.demos), [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1])
  #best_name, best_cost = matcher.match(xyz_new)

  best_demo = Globals.demos[best_name]
  if best_demo["done"]:
    rospy.loginfo("best demo was a 'done' state")
    return {'status': 'success'}
  rospy.loginfo("best segment name: %s", best_name)
  xyz_demo_ds = best_demo["cloud_xyz_ds"]

#  print 'arms used', best_demo['arms_used']
#  overlap_ctl_pts = []
#  grabbing_pts = []
#  for lr in 'lr':
#    # look at points around gripper when grabbing
#    grabbing = map(bool, list(best_demo["%s_gripper_joint"%lr] < .07))
#    grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])])
#    grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])])
#  overlap_ctl_pts = [p for p in xyz_demo_ds if any(np.linalg.norm(p - g) < 0.1 for g in grabbing_pts)]
#  overlap_ctl_pts = xyz_demo_ds
  #rviz_draw_points(overlap_ctl_pts,rgba=(1,1,1,1),type=Marker.CUBE_LIST)
#  rviz_draw_points(grabbing_pts,rgba=(.5,.5,.5,1),type=Marker.CUBE_LIST)
  n_iter = 101
  #warping_map = registration.tps_rpm_with_overlap_control(xyz_demo_ds, xyz_new_ds, overlap_ctl_pts, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20)
  warping_map,info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20,return_full=True)  

  from lfd import tps
  import scipy.spatial.distance as ssd
  f = warping_map
  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)

  Globals.handles = []
  registration.Globals.handles = []
  f.lin_ag, f.trans_g, f.w_ng, f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5)
  
  #if plotting:
    #plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md)   
    #targ_pose_array = conversions.array_to_pose_array(targ_Nd, 'base_footprint')
    #Globals.handles.append(Globals.rviz.draw_curve(targ_pose_array,rgba=(1,1,0,1),type=Marker.CUBE_LIST))

  #raw_input('Press enter to continue:')
  #################### 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(warping_map.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, ns='demo_cloud'))
  # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST, ns='warped_cloud'))

  #### 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, warping_map.transform_points, mins, maxes, 'base_footprint')
  Globals.handles.append(grid_handle)

  #### Actually generate the trajectory ###########
  warped_demo = warping.transform_demo_with_fingertips(warping_map, best_demo)

  Globals.pr2.update_rave_without_ros(curr_robot_joint_vals)
  trajectory = {}
  trajectory['seg_name'] = best_name
  trajectory['demo'] = best_demo
  if 'tracked_states' in best_demo:
    trajectory['orig_tracked_states'] = best_demo['tracked_states']
    trajectory['tracked_states'], Globals.offset_trans = warping.transform_tracked_states(warping_map, best_demo, Globals.offset_trans)

  steps = 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)

      rospy.loginfo("calculating joint trajectory...")
      #arm_traj, feas_inds = lfd_traj.make_joint_traj(
      #  warped_demo["%s_gripper_tool_frame"%lr]["position"],
      #  warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
      #  best_demo["%sarm"%leftright],
      #  Globals.pr2.robot.GetManipulator("%sarm"%leftright),
      #  "base_footprint","%s_gripper_tool_frame"%lr,
      #  1+2+16
      #)
      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 {'status': "failure"}
      trajectory["%s_arm"%lr] = arm_traj
      trajectory["%s_steps"%lr] = steps = len(arm_traj)
      rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(arm_traj))
      trajectory["%s_grab"%lr] = map(bool, list(best_demo["%s_gripper_joint"%lr] < .02))
      trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
      trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
      # 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'
      ))
  assert 'l_steps' not in trajectory or steps == trajectory['l_steps']
  assert 'r_steps' not in trajectory or steps == trajectory['r_steps']
  trajectory['steps'] = steps
  #raw_input('Press enter to continue:')

  return {'status': 'not_done', 'trajectory': trajectory}