-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathtests.py
136 lines (113 loc) · 4.82 KB
/
tests.py
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
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
""" 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,
}