def _set_all_visitor_position(self, position):
visitorNum = len(self.visitorHandles)
for i in range(visitorNum):
vrep.simxSetObjectPosition(
self.clientID, self.visitorHandles[i], -1,
[position[i * 2], position[i * 2 + 1], 0],
self._set_visitor_op_mode)
def _set_single_visitor_position(self, targetPositionName, position):
visitorIndex = np.where(self.visitorNames == targetPositionName)
if len(visitorIndex[0]) == 0:
print("Not found visitor: {}".format(targetPositionName))
else:
vrep.simxSetObjectPosition(self.clientID,
self.visitorHandles[visitorIndex], -1,
[position[0], position[1], 0],
self._set_visitor_op_mode)
def load_model(clientID,
model_name='wall_brick',
location_file='wall_brick_location.csv'):
"""
Parameters
----------
clientID: int
the V-REP clientID
model_name: string
the name of model you are going to load:
1. wall_brick
2. floor_tile
3. hallway
4. goal
5. action_down
6. action_up
7. action_left
8. action_right
9.
location_file: string
the file given by this string contains the location of model.
Each entry in the file has the format:
name, x, y, z
Returns
-------
object_handles: list
a list of object handles
object_positions: list
a list of positions of objects
"""
object_handles = []
object_positions = []
with open(location_file, newline='') as csvfile:
csv_reader = csv.reader(csvfile, delimiter=',')
next(csv_reader) # remove first row
for i, row in enumerate(csv_reader):
object_name = row[0]
x, y, z = float(row[1]), float(row[2]), float(row[3])
#print('{}th {} location: ({}, {}, {})'.format(i, model_name, x, y, z))
model_path = os.path.join(os.path.abspath('.'),
'4room_world_models',
model_name + '.ttm')
res, handle = vrep.simxLoadModel(clientID, model_path, 0,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise Exception('vrep.simxLoadModel does not work!')
else:
object_handles.append(handle)
#print('handle: {}'.format(handle))
vrep.simxSetObjectPosition(clientID, handle, -1, [x, y, z],
vrep.simx_opmode_oneshot)
res, objPosition = vrep.simxGetObjectPosition(
clientID, handle, -1, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise Exception('vrep.simxGetObjectPosition does not work.')
else:
object_positions.append(objPosition)
#print("{} handle: {}, position: {}".format(object_name, handle, objPosition))
return object_handles, object_positions
def loadModel(clientID, grp_name, grp_coordinate):
num_group = len(grp_name)
for i in range(0, num_group):
single_grp_name = grp_name[i]
single_grp_coord = grp_coordinate[i]
# find center of A and B
centerA = [0, 0, 0]
centerB = [0, 0, 0]
for j in range(0, len(single_grp_name)):
if single_grp_name[j] == 'A':
centerA = single_grp_coord[j]
elif single_grp_name[j] == 'B':
centerB = single_grp_coord[j]
# loop through all groups
for j in range(0, len(single_grp_name)):
if single_grp_name[j] == 'A' or single_grp_name[j] == 'B':
res, handle = vrep.simxLoadModel(
clientID, 'models/LAS_Model/center.ttm', 0,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
print("Load model failed, " + str(res))
print('handle = ', handle)
# position = [single_grp_coord[j][0] - x_offset, single_grp_coord[j][1] - y_offset, single_grp_coord[j][2]]
vrep.simxSetObjectPosition(clientID, handle, -1,
single_grp_coord[j],
vrep.simx_opmode_oneshot)
# vrep.simxGetObject
# for index in range(0,2):
# res, handle = vrep.simxGetObjectChild(clientID,handle,index,vrep.simx_opmode_blocking)
else:
sma_number = (int(single_grp_name[j]) - 1) % 6 + 1
filename = 'models/LAS_Model/edge' + str(sma_number) + '.ttm'
res, handle = vrep.simxLoadModel(clientID, filename, 0,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
print("Load model failed, " + str(res))
print('handle = ', handle)
if int(single_grp_name[j]) <= 6:
# move sma closer to center to avoid collision
position = (np.array(single_grp_coord[j]) +
np.array(centerA)) / 2
vector = np.array(single_grp_coord[j][0:2]) - np.array(
centerA[0:2])
gamma = np.arccos(vector[0] / np.linalg.norm(vector))
# as the range of arccos() is [0,Pi], we need to check the case where angle is negative (y < 0)
if vector[1] < 0:
gamma = -gamma
# print("gamma = ", gamma*180/math.pi)
else:
# move sma closer to center to avoid collision
position = (np.array(single_grp_coord[j]) +
np.array(centerB)) / 2
vector = np.array(single_grp_coord[j][0:2]) - np.array(
centerB[0:2])
gamma = np.arccos(vector[0] / np.linalg.norm(vector))
if vector[1] < 0:
gamma = -gamma
# print("gamma = ", gamma * 180 / math.pi)
vrep.simxSetObjectPosition(clientID, handle, -1,
position.tolist(),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, handle, -1,
[0, 0, gamma],
vrep.simx_opmode_oneshot)