Example #1
0
    def Test10(self
               ):  # Description: Add noise and interpolate (take image at end)
        # do STLs with noise after this.

        V = Vis()
        Hand1 = HandVis(V)
        Hand1.loadHand()
        Hand2 = HandVis(V)
        Hand2.loadHand()
        Obj1 = ObjectVis(V)
        Obj1.loadObject(4)

        Data = ParseGraspData()
        grasps = Data.findGrasp(objnum=4,
                                subnum=4,
                                graspnum=11,
                                list2check=Data.all_transforms)
        grasp = grasps[1]
        HandT, ObjT, Arm_JA, Hand_JA = Data.matricesFromGrasp(grasp)
        Hand1.orientHandtoObj(HandT, ObjT, Obj1)
        Hand1.setJointAngles(Hand_JA)
        Hand1.getPalmPoint()
        contact_points1, contact_links1 = Hand1.retractFingers(Obj1)
        Contact_JA = Hand1.obj.GetDOFValues()

        filename_base = "obj%s_sub%s_graspnum%s_grasptype%s" % (4, 4, 11,
                                                                'extreme0')
        if False:
            T_noise, JA_noise = Hand2.addNoiseToGrasp(
                Obj1,
                T_zero=Hand1.obj.GetTransform(),
                Contact_JA=Contact_JA,
                TL_n=0.01,
                R_n=0.1,
                JA_n=0.1)
            np.savetxt(filename_base + "_%s.txt" % 'T_noise', T_noise)
            np.savetxt(filename_base + "_%s.txt" % 'JA_noise', JA_noise)
        else:
            JA_noise = np.genfromtxt(filename_base + "_%s.txt" % 'JA_noise')
            T_noise = np.genfromtxt(filename_base + "_%s.txt" % 'T_noise')
        Hand2.setJointAngles(JA_noise)
        Hand2.obj.SetTransform(T_noise)
        contact_points2, contact_links2 = Hand2.retractFingers(Obj1)
        alpha = np.linspace(0, 1, 6)[1:-1]
        start_axis_angle = np.array([0, 0, 0])
        end_axis_angle = np.array([0, np.pi, 0])
        Hand1.hide()
        for a in alpha:
            filename = filename_base + "_alpha%s.png" % (int(10 * a))
            Hand2.ZSLERP(start_axis_angle, end_axis_angle, a, T_zero=T_noise)
            V.clearPoints()
            V.takeImage(filename)
            time.sleep(1)

        pdb.set_trace()
Example #2
0
    def Test11(self):  # Description: Generating Images for Interpolated Grasps
        # Oriignal is grey
        # 180 + noise grasp is pink
        # interpolated is violet

        # do STLs with noise after this.

        V = Vis()
        Hand1 = HandVis(V)
        Hand1.loadHand()
        Hand2 = HandVis(V)
        Hand2.loadHand()
        Obj1 = ObjectVis(V)
        Obj1.loadObject(4)
        Obj1.changeColor('green')

        Data = ParseGraspData()
        Data.parseOutputData()
        Data.parseAllTransforms()
        # obj4_cluster13_sub5_grasp2_optimal0_prime.jpg obj4_cluster13_sub5_grasp3_extreme1_target.jpg -- don't have the data.  Tried without class but extreme grasp was too far!
        # grasp1 = Data.findGrasp(objnum = 4, subnum = 5, graspnum = 2, grasptype = 'optimal0', list2check = Data.all_transforms)
        # grasp2 = Data.findGrasp(objnum = 4, subnum = 5, graspnum = 3, grasptype = 'extreme1', list2check = Data.all_transforms)
        # obj4_cluster8_sub4_grasp14_extreme1_prime.jpg obj4_cluster8_sub4_grasp14_optimal0_target.jpg -- don't have the data
        # grasp1 = Data.findGrasp(objnum = 4, subnum = 4, graspnum = 14, grasptype = 'extreme1', list2check = Data.all_transforms)
        # grasp2 = Data.findGrasp(objnum = 4, subnum = 4, graspnum = 14, grasptype = 'optimal0', list2check = Data.all_transforms)
        # obj4_cluster8_sub4_grasp14_extreme1_prime.jpg obj4_cluster8_sub4_grasp9_optimal0_target.jpg -- don't have the data

        case = 4

        #ones that I think look good
        if case == 1:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=11,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=11,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, 11, 'optimal0', 'optimal0')
        elif case == 2:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=5,
                                    graspnum=2,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=5,
                                    graspnum=3,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 5, '2&3', 'optimal0', 'extreme1')
        elif case == 3:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, 14, 'extreme1', 'optimal0')
        elif case == 4:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=9,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, '14&9', 'extreme1', 'optimal0')

        HandT1, ObjT1, Arm_JA1, Hand_JA1 = Data.matricesFromGrasp(grasp1[0])
        HandT2, ObjT2, Arm_JA2, Hand_JA2 = Data.matricesFromGrasp(grasp2[0])
        Hand1.orientHandtoObj(HandT1, ObjT1, Obj1)
        Hand1.setJointAngles(Hand_JA1)
        Hand1.changeColor('greyI')
        Hand2.orientHandtoObj(HandT2, ObjT2, Obj1)
        Hand2.setJointAngles(Hand_JA2)
        Hand2.changeColor('pinkI')
        Hand1.getPalmPoint()
        contact_points1, contact_links1 = Hand1.retractFingers(Obj1)
        contact_points2, contact_links2 = Hand2.retractFingers(Obj1)

        start_axis_angle = np.array([0, 0, 0])
        end_axis_angle = np.array([0, np.pi, 0])
        Hand2.ZSLERP(start_axis_angle,
                     end_axis_angle,
                     1,
                     T_zero=Hand2.obj.GetTransform())
        Hand3 = HandVis(V)
        Hand3.loadHand()
        Hand3.makeEqual(Hand1)
        Hand3.changeColor('blueI')
        alpha = np.linspace(0, 1, 6)[1:-1]

        if case == 1:
            V.setCamera(60, 0, 5 / 4 * np.pi - 0.75)
        elif case == 2:
            V.setCamera(60, np.pi / 4 - 1.5, -np.pi - 1)
        elif case == 3:
            V.setCamera(40, np.pi / 3 + 0.2, -np.pi - 0.5)
        elif case == 4:
            V.setCamera(40, np.pi / 3 - np.pi, -np.pi - 0.75)
            # pdb.set_trace()

        # function to get out dist, az, el of camera from current view
        for a in alpha:
            filename = filename_base + "_alpha%s.png" % (int(10 * a))
            Hand3.ZSLERP(start_axis_angle,
                         end_axis_angle,
                         a,
                         T_zero=Hand1.obj.GetTransform())
            V.clearPoints()
            V.takeImage(filename)
            time.sleep(1)