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)