Пример #1
0
def vis_graspable_grasp(graspable, grasp):

    graspable.mesh.visualize()
    mv.axes()

    x, v = graspable.sdf.surface_points(grid_basis = False)
    mv.points3d(x[:,0], x[:,1], x[:,2], scale_factor = 0.01)

    g1, g2 = grasp.endpoints()
    mv.points3d(g1[0], g1[1], g1[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
    mv.points3d(g2[0], g2[1], g2[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
    mv.points3d(grasp.center[0], grasp.center[1], grasp.center[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
    mv.show()
Пример #2
0
def vis_graspable_grasp(graspable, grasp):

    graspable.mesh.visualize()
    mv.axes()

    x, v = graspable.sdf.surface_points(grid_basis=False)
    mv.points3d(x[:, 0], x[:, 1], x[:, 2], scale_factor=0.01)

    g1, g2 = grasp.endpoints()
    mv.points3d(g1[0], g1[1], g1[2], scale_factor=0.01, color=(0.5, 0.5, 0.5))
    mv.points3d(g2[0], g2[1], g2[2], scale_factor=0.01, color=(0.5, 0.5, 0.5))
    mv.points3d(grasp.center[0],
                grasp.center[1],
                grasp.center[2],
                scale_factor=0.01,
                color=(0.5, 0.5, 0.5))
    mv.show()
Пример #3
0
    def _generate_grasps(self,
                         graspable,
                         num_grasps,
                         check_collisions=False,
                         vis=False):
        """Returns a list of candidate grasps for graspable object.
        Params:
            graspable - (GraspableObject3D) the object to grasp
            num_grasps - currently unused TODO
        Returns:
            list of ParallelJawPtGrasp3D objects
        """
        # get surface points
        ap_grasps = []
        surface_points, _ = graspable.sdf.surface_points(grid_basis=False)

        for x_surf in surface_points:
            start_time = time.clock()

            # perturb grasp for num samples
            for i in range(self.num_samples):
                # perturb contact (TODO: sample in tangent plane to surface)
                x1 = self.perturb_point(x_surf, graspable.sdf.resolution)

                # compute friction cone faces
                c1 = contacts.Contact3D(graspable, x1, in_direction=None)
                cone_succeeded, cone1, n1 = c1.friction_cone(
                    self.num_cone_faces, self.friction_coef)
                if not cone_succeeded:
                    continue
                cone_time = time.clock()

                # sample grasp axes from friction cone
                v_samples = self.sample_from_cone(cone1, num_samples=1)
                sample_time = time.clock()

                for v in v_samples:
                    if vis:
                        x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1)
                        cone1_grid = graspable.sdf.transform_pt_obj_to_grid(
                            cone1, direction=True)
                        plt.clf()
                        h = plt.gcf()
                        plt.ion()
                        ax = plt.gca(projection='3d')
                        for i in range(cone1.shape[1]):
                            ax.scatter(x1_grid[0] - cone1_grid[0],
                                       x1_grid[1] - cone1_grid[1],
                                       x1_grid[2] - cone1_grid[2],
                                       s=50,
                                       c=u'm')

                    # start searching for contacts
                    grasp, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(
                        graspable, x1, v, self.grasp_width, vis=vis)
                    if grasp is None or c2 is None:
                        continue

                    # make sure grasp is wide enough
                    x2 = c2.point
                    if np.linalg.norm(x1 - x2) < self.min_contact_dist:
                        continue

                    v_true = grasp.axis
                    # compute friction cone for contact 2
                    cone_succeeded, cone2, n2 = c2.friction_cone(
                        self.num_cone_faces, self.friction_coef)
                    if not cone_succeeded:
                        continue

                    if vis:
                        plt.figure()
                        ax = plt.gca(projection='3d')
                        c1_proxy = c1.plot_friction_cone(color='m')
                        c2_proxy = c2.plot_friction_cone(color='y')
                        ax.view_init(elev=5.0, azim=0)
                        plt.show(block=False)
                        time.sleep(0.5)
                        plt.close()  # lol

                    # check friction cone
                    in_cone1, alpha1 = self.within_cone(cone1, n1, v_true.T)
                    in_cone2, alpha2 = self.within_cone(cone2, n2, -v_true.T)
                    within_cone_time = time.clock()

                    # add points if within friction cone
                    if in_cone1 and in_cone2:
                        # get moment arms
                        x1_world, x2_world = grasp.endpoints()
                        rho1 = np.linalg.norm(graspable.moment_arm(x1_world))
                        rho2 = np.linalg.norm(graspable.moment_arm(x2_world))

                        antipodal_grasp = AntipodalGraspParams(
                            graspable, grasp, alpha1, alpha2, rho1, rho2)
                        ap_grasps.append(antipodal_grasp)

        # randomly sample max num grasps from total list
        max_grasp_index = min(len(ap_grasps), self.max_num_grasps)
        random.shuffle(ap_grasps)
        ap_grasps = ap_grasps[:max_grasp_index]

        # load openrave
        if check_collisions:
            rave.raveSetDebugLevel(rave.DebugLevel.Error)
            grasp_checker = pgc.OpenRaveGraspChecker(view=vis)

        # go back through grasps and threshold
        grasps = []
        pr2_grasps = []
        alpha_thresh = self.alpha_thresh
        rho_thresh = self.rho_thresh * graspable.sdf.max_dim()
        while len(ap_grasps) > 0 and len(grasps) < self.min_num_grasps and len(pr2_grasps) < self.min_num_collision_free and \
                alpha_thresh < np.pi / 2:
            # prune grasps above thresholds
            next_ap_grasps = []
            for ap_grasp in ap_grasps:
                if max(ap_grasp.alpha1, ap_grasp.alpha2) < alpha_thresh and \
                        max(ap_grasp.rho1, ap_grasp.rho2) < rho_thresh:
                    # convert grasps to PR2 gripper poses
                    rotated_grasps = ap_grasp.grasp.transform(
                        graspable.tf, self.theta_res)

                    # prune collision grasps if necessary
                    if check_collisions:
                        rotated_grasps = grasp_checker.prune_grasps_in_collision(
                            graspable,
                            rotated_grasps,
                            auto_step=True,
                            delay=0.0)

                    # only add grasp if at least 1 is collision free
                    if len(rotated_grasps) > 0:
                        grasps.append(ap_grasp.grasp)
                        pr2_grasps.extend(rotated_grasps)
                else:
                    next_ap_grasps.append(ap_grasp)

            # update alpha and rho thresholds
            alpha_thresh = alpha_thresh + self.alpha_inc  #np.arctan(friction_coef)
            rho_thresh = rho_thresh + self.rho_inc
            ap_grasps = list(next_ap_grasps)

        return grasps
Пример #4
0
    def _generate_grasps(self, graspable, num_grasps,
                         check_collisions=False, vis=False):
        """Returns a list of candidate grasps for graspable object.
        Params:
            graspable - (GraspableObject3D) the object to grasp
            num_grasps - currently unused TODO
        Returns:
            list of ParallelJawPtGrasp3D objects
        """
        # get surface points
        ap_grasps = []
        surface_points, _ = graspable.sdf.surface_points(grid_basis=False)

        for x_surf in surface_points:
            start_time = time.clock()

            # perturb grasp for num samples
            for i in range(self.num_samples):
                # perturb contact (TODO: sample in tangent plane to surface)
                x1 = self.perturb_point(x_surf, graspable.sdf.resolution)

                # compute friction cone faces
                c1 = contacts.Contact3D(graspable, x1, in_direction=None)
                cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef)
                if not cone_succeeded:
                    continue
                cone_time = time.clock()

                # sample grasp axes from friction cone
                v_samples = self.sample_from_cone(cone1, num_samples=1)
                sample_time = time.clock()

                for v in v_samples:
                    if vis:
                        x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1)
                        cone1_grid = graspable.sdf.transform_pt_obj_to_grid(cone1, direction=True)
                        plt.clf()
                        h = plt.gcf()
                        plt.ion()
                        ax = plt.gca(projection = '3d')
                        for i in range(cone1.shape[1]):
                            ax.scatter(x1_grid[0] - cone1_grid[0], x1_grid[1] - cone1_grid[1], x1_grid[2] - cone1_grid[2], s = 50, c = u'm')

                    # start searching for contacts
                    grasp, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, self.grasp_width, vis = vis)
                    if grasp is None or c2 is None:
                        continue

                    # make sure grasp is wide enough
                    x2 = c2.point
                    if np.linalg.norm(x1 - x2) < self.min_contact_dist:
                        continue

                    v_true = grasp.axis
                    # compute friction cone for contact 2
                    cone_succeeded, cone2, n2 = c2.friction_cone(self.num_cone_faces, self.friction_coef)
                    if not cone_succeeded:
                        continue

                    if vis:
                        plt.figure()
                        ax = plt.gca(projection='3d')
                        c1_proxy = c1.plot_friction_cone(color='m')
                        c2_proxy = c2.plot_friction_cone(color='y')
                        ax.view_init(elev=5.0, azim=0)
                        plt.show(block=False)
                        time.sleep(0.5)
                        plt.close() # lol

                    # check friction cone
                    in_cone1, alpha1 = self.within_cone(cone1, n1, v_true.T)
                    in_cone2, alpha2 = self.within_cone(cone2, n2, -v_true.T)
                    within_cone_time = time.clock()

                    # add points if within friction cone
                    if in_cone1 and in_cone2:
                        # get moment arms
                        x1_world, x2_world = grasp.endpoints()
                        rho1 = np.linalg.norm(graspable.moment_arm(x1_world))
                        rho2 = np.linalg.norm(graspable.moment_arm(x2_world))

                        antipodal_grasp = AntipodalGraspParams(graspable, grasp, alpha1, alpha2, rho1, rho2)
                        ap_grasps.append(antipodal_grasp)

        # randomly sample max num grasps from total list
        max_grasp_index = min(len(ap_grasps), self.max_num_grasps)
        random.shuffle(ap_grasps)
        ap_grasps = ap_grasps[:max_grasp_index]

        # load openrave
        if check_collisions:
            rave.raveSetDebugLevel(rave.DebugLevel.Error)
            grasp_checker = pgc.OpenRaveGraspChecker(view=vis)

        # go back through grasps and threshold
        grasps = []
        pr2_grasps = []
        alpha_thresh = self.alpha_thresh
        rho_thresh = self.rho_thresh * graspable.sdf.max_dim()
        while len(ap_grasps) > 0 and len(grasps) < self.min_num_grasps and len(pr2_grasps) < self.min_num_collision_free and \
                alpha_thresh < np.pi / 2:
            # prune grasps above thresholds
            next_ap_grasps = []
            for ap_grasp in ap_grasps:
                if max(ap_grasp.alpha1, ap_grasp.alpha2) < alpha_thresh and \
                        max(ap_grasp.rho1, ap_grasp.rho2) < rho_thresh:
                    # convert grasps to PR2 gripper poses
                    rotated_grasps = ap_grasp.grasp.transform(graspable.tf, self.theta_res)

                    # prune collision grasps if necessary
                    if check_collisions:
                        rotated_grasps = grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=True, delay=0.0)

                    # only add grasp if at least 1 is collision free
                    if len(rotated_grasps) > 0:
                        grasps.append(ap_grasp.grasp)
                        pr2_grasps.extend(rotated_grasps)
                else:
                    next_ap_grasps.append(ap_grasp)

            # update alpha and rho thresholds
            alpha_thresh = alpha_thresh + self.alpha_inc #np.arctan(friction_coef)
            rho_thresh = rho_thresh + self.rho_inc
            ap_grasps = list(next_ap_grasps)

        return grasps