isaac_sim_grasping/tests.py at main · IRVLUTD/isaac_sim_grasping · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
from utils_local import te_batch, re_batch
import numpy as np
from sim_utils import GenericTest

class SimpleGravity(GenericTest):
    def __init__(self, objects, gripper_dict, total_test_time):
        super().__init__()
        self.label = "gravity"
        self.objects = objects
        self.contact_th = gripper_dict['contact_th']
        self.test_time = total_test_time
        return

    def failure_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the test has failed

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        current_positions, current_rotations = self.objects.get_world_poses(indices)
        t_error = abs(te_batch(init_pos[indices], current_positions))

        finish_ind = indices[t_error>0.3]
        return finish_ind

    def setup_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the objects advance from the setup phase

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        # Fix velocities and check for set up criteria
        self.objects.set_velocities([0,0,0,0,0,0],indices)
        self.objects.set_world_poses(init_pos[indices], init_rot[indices],indices)

        # Setup Condition
        tmp = np.count_nonzero(np.sum(self.objects.get_contact_force_matrix(indices),axis =2),axis=1)

        # Update grasp_setup for the ones that fulfill criteria
        setup_ind = indices[tmp>=self.contact_th]

        return setup_ind

    def test_step(self, current_times):
        gravity = np.tile(np.array([0,0,-9.81]), (len(current_times),1))*self.objects.get_masses()[:, np.newaxis]
        self.objects.apply_forces(gravity)
        return

class AxesForces(GenericTest):
    def __init__(self, objects, gripper_dict, total_test_time):
        super().__init__()
        self.objects = objects
        self.label = 'axes_forces'
        self.contact_th = gripper_dict['contact_th']
        self.test_time = total_test_time
        return

    def failure_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the test has failed

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        current_positions, current_rotations = self.objects.get_world_poses(indices)
        t_error = abs(te_batch(init_pos[indices], current_positions))

        finish_ind = indices[t_error>0.02]
        return finish_ind

    def setup_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the objects advance from the setup phase

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        # Fix velocities and check for set up criteria
        return indices

    def test_step(self, current_times):
        acc = np.zeros((len(current_times),3))
        masses = self.objects.get_masses()[:, np.newaxis]

        # Axis forces
        acc[:] = [0, 0, 0.5]
        tmp_ind = np.argwhere(current_times<5*self.test_time/6)
        acc[tmp_ind] = [0, 0, -0.5]
        tmp_ind = np.argwhere(current_times<4*self.test_time/6)
        acc[tmp_ind] = [0, 0.5, 0]
        tmp_ind = np.argwhere(current_times<3*self.test_time/6)
        acc[tmp_ind] = [0, -0.5, 0]
        tmp_ind = np.argwhere(current_times<2*self.test_time/6)
        acc[tmp_ind] = [0.5, 0, 0]
        tmp_ind = np.argwhere(current_times<self.test_time/6)
        acc[tmp_ind] = [-0.5, 0, 0]

        forces = acc*masses
        self.objects.apply_forces(forces)
        return

empty_array = np.array([],dtype=int)
class Controller_tester(GenericTest):
    def __init__(self, objects, gripper_dict, total_test_time):
        super().__init__()
        self.label = "control_test"
        self.objects = objects
        self.test_time = total_test_time
        pos, self.o = self.objects.get_world_poses()
        self.pos = pos - np.array([0,0,5])
        return
    def failure_condition(self, init_pos, init_rot, indices):
        return empty_array
    def setup_condition(self, init_pos, init_rot, indices):
        return empty_array

    def test_step(self, current_times):
        self.objects.set_world_poses(self.pos,self.o)
        self.objects.set_velocities([0,0,0,0,0,0])
        return

class GradualGravity(GenericTest):
    def __init__(self, objects, gripper_dict, total_test_time):
        super().__init__()
        self.label = "gradual_gravity"
        self.objects = objects
        self.contact_th = gripper_dict['contact_th']
        self.test_time = total_test_time
        self.gravity_rampup_ratio = 0.70
        return

    def failure_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the test has failed

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        current_positions, current_rotations = self.objects.get_world_poses(indices)
        t_error = abs(te_batch(init_pos[indices], current_positions))

        finish_ind = indices[t_error>0.1]
        return finish_ind

    def setup_condition(self, init_pos, init_rot, indices):
        ''' Function to test if the objects advance from the setup phase

        Arguments:
        - init_pos: Initial positions of objects
        - init_rot: Initial rotation of objects
        - indices: array with indices of objects to check
        '''
        # Fix velocities and check for set up criteria
        self.objects.set_velocities([0,0,0,0,0,0],indices)
        self.objects.set_world_poses(init_pos[indices], init_rot[indices],indices)

        # Setup Condition
        tmp = np.count_nonzero(np.sum(self.objects.get_contact_force_matrix(indices),axis =2),axis=1)

        # Update grasp_setup for the ones that fulfill criteria
        setup_ind = indices[tmp>=self.contact_th]

        return setup_ind

    def test_step(self, current_times):
        ratio = current_times / (self.test_time*self.gravity_rampup_ratio)
        ratio = np.clip(ratio,0,1.0)
        gravity = np.tile(np.array([0,0,-9.81]), (len(current_times),1))*self.objects.get_masses()[:, np.newaxis]
        gravity = gravity*ratio
        self.objects.apply_forces(gravity)
        return

""" LIST OF Tests to Perform:
They are the references used in command line to determine the controller to use
"""
test_dict = {
    'gravity' : SimpleGravity,
    'axes_forces': AxesForces,
    'control_test': Controller_tester,
    'gradual_gravity': GradualGravity,
}