def init_vrep(): vrep.simxFinish(-1) client_id = vrep.simxStart('127.0.0.1',19997,True,True,5000,5) assert client_id != -1, 'Failed to connect to remote API server' vrep.simxSynchronous(client_id, 1) ENV_VAR['client_id'] = client_id print('Initialized simulation environment')
def start(self): vrep.simxFinish(-1) self.client_id = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.client_id == 1: raise ConnectionError vrep.simxSynchronous(self.client_id, 1) print('Initialized simulation environment with client id {}'.format( self.client_id))
def incline_obj_f(x): print('\nParameters: ' + str(x)) try: CLIENTID = ENV_VAR['client_id'] load_scene('scenes/inclines/' + str(int(x[3])) + 'deg.ttt') walker = ENV_VAR['walker'] vrep.simxSynchronous(CLIENTID, 1) vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking) errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) assert errorCode in VALID_ERROR_CODES, 'Error getting object position' gait = DualTripod gait.f = x[0] gait.phase_offset = x[1] gait.coupling_phase_biases = gait.coupling_phase_biases = \ gait.generate_coupling(gait.phase_groupings) cpg = CpgController(gait) for _ in range(1000): cpg.update(plot=False) print('Running trial...') for _ in range(100): output = cpg.output() for i in range(6): walker.legs[i].extendZ(output[i][0] * x[2]) walker.legs[i].extendX(output[i][1] * x[2]) vrep.simxSynchronousTrigger(CLIENTID) cpg.update() errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking) vrep.simxGetPingTime(CLIENTID) vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking) # to be maximized print('Objective: ' + str(start[0] - end[0])) return np.array([start[0] - end[0]]) except Exception: print('Encountered an exception, disconnecting from remote API server') vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot) vrep.simxGetPingTime(CLIENTID) vrep.simxFinish(CLIENTID) traceback.print_exc() exit()
def objective(x): x = np.asarray(x)[0] print('\nParameters: ' + str(x)) try: # Clean up VREP self.run() # Get starting position start = self.get_pos(self.walker.base_handle) # Assign parameters self.walker.set_left_bias(x[2]) self.walker.set_right_bias(x[3]) self.set_cpg(gait, x[0], x[1]) for _ in range(1000): self.cpg.update(plot=False) # Run the simulation print('Running trial...') self.walk(max_steps) # Calculate how far the robot walked end = self.get_pos(self.walker.base_handle) dist = self.calc_dist(start, end) print('Distance traveled: ' + str(dist)) # Clean up VREP self.stop() return np.array([dist]) except (ConnectionError, SceneLoadingError, WalkerLoadingError, MotorLoadingError, HandleLoadingError) as e: print("Encountered an exception: {} " "disconnecting from remote API server".format(e)) vrep.simxFinish(self.client_id) traceback.print_exc() self.stop() raise e
def objective(x): try: walker.reset() vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking) errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) assert errorCode in VALID_ERROR_CODES, 'Walker not found' x = np.asarray(x)[0] print('\nParameters: ' + str(x)) cpg_gait = gait if parameter_mode is 'normal': cpg_gait.f = x[0] cpg_gait.phase_offset = x[1] walker.set_left_bias(x[2]) walker.set_right_bias(x[3]) elif parameter_mode is 'contextual': cpg_gait.f = x[0] cpg_gait.phase_offset = x[1] walker.set_left_bias(x[2]) walker.set_right_bias(x[2]) elif parameter_mode is 'discovery': cpg_gait.f = x[0] cpg_gait.phase_offset = x[1] cpg_gait.phase_groupings = { 0: x[2], 1: x[3], 2: x[4], 3: x[5], 4: x[6], 5: x[7] } else: raise Exception('Invalid parameter mode') cpg_gait.coupling_phase_biases = \ cpg_gait.generate_coupling(cpg_gait.phase_groupings) cpg = CpgController(cpg_gait) for _ in range(1000): cpg.update(plot=False) print('Running trial...') for _ in range(steps): output = cpg.output() if objective_mode is not 'normal': walker.update_energy() for i in range(6): walker.legs[i].extendZ(output[i][0]) walker.legs[i].extendX(output[i][1]) wait(1) cpg.update() errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) if objective_mode is not 'single': total_energy = walker.calculate_energy() / 5 vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking) vrep.simxGetPingTime(CLIENTID) vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking) if objective_mode is 'target': target = x[-2], x[-1] return np.array([distance(end, target)]) else: distance = start[0] - end[0] if penalize_offset: distance += (.1 * np.abs(end[1] - start[1])) if objective_mode is 'single': print('Distance traveled: ' + str(distance)) return np.array([distance]) else: print('Distance traveled: ' + str(distance)) print('Total power: ' + str(total_energy) + 'W') return np.array([distance, total_energy]) except Exception as e: print('Encountered an exception, disconnecting from remote API server') vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot) vrep.simxGetPingTime(CLIENTID) vrep.simxFinish(CLIENTID) traceback.print_exc() exit()
def exit_vrep(): vrep.simxFinish(ENV_VAR['client_id']) print('Exited simulation environment')
def turning_obj_f(x): print('\nParameters: ' + str(x)) try: CLIENTID = ENV_VAR['client_id'] walker = ENV_VAR['walker'] errorCode,baseCollectionHandle=\ vrep.simxGetCollectionHandle(CLIENTID, 'Base', \ vrep.simx_opmode_blocking) vrep.simxSynchronous(CLIENTID, 1) vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking) walker.reset() errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) assert errorCode in VALID_ERROR_CODES, 'Error getting object position' q1 = vrep.simxGetObjectGroupData(CLIENTID, baseCollectionHandle, \ 7, vrep.simx_opmode_blocking) o1 = quatToDirection(q1[3]) + np.pi x[2] = 15 + (3 * x[2]) x[3] = x[3] * 2 * np.pi / 10 gait = DualTripod gait.f = x[2] gait.phase_offset = x[3] gait.coupling_phase_biases=gait.generate_coupling(gait.phase_groupings) walker.set_left_bias(x[0] / 10) walker.set_right_bias(x[1] / 10) cpg = CpgController(gait) for _ in range(1000): cpg.update(plot=False) print('Running trial...') for _ in range(100): output = cpg.output() walker.update_energy() for i in range(6): walker.legs[i].extendZ(output[i][0]) walker.legs[i].extendX(output[i][1]) vrep.simxSynchronousTrigger(CLIENTID) cpg.update() total_energy = walker.calculate_energy() / 5 errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \ walker.base_handle, -1, vrep.simx_opmode_blocking) q2 = vrep.simxGetObjectGroupData(CLIENTID, baseCollectionHandle, \ 7, vrep.simx_opmode_blocking) o2 = quatToDirection(q2[3]) + np.pi vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking) vrep.simxGetPingTime(CLIENTID) vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking) displacement = np.array(end) - np.array(start) orientation = o2 - o1 info = np.array([total_energy, displacement[0], displacement[1], \ orientation]) objective = distance(displacement, (x[4], x[5])) print('Orientation change: ' + str(orientation)) print('Total power: ' + str(total_energy) + 'W') print('Target offset: ' + str(objective)) return np.array([objective]), info except Exception: print('Encountered an exception, disconnecting from remote API server') vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot) vrep.simxGetPingTime(CLIENTID) vrep.simxFinish(CLIENTID) traceback.print_exc() exit()
def objective(x, cache_walker=True): """ x[0] = cpg frequency [1, 45] x[1] = cpg phase offset [-np.pi, np.pi] x[2] = left bias of walker [0, 1] x[3] = right bias of walker [0, 1] x[4] = front pair leg scaling [.8, 1.2] x[5] = middle pair leg scaling [.8, 1.2] x[6] = rear pair leg scaling [.8, 1.2] :return: """ self.start() x = np.asarray(x)[0] print('\nParameters: ' + str(x)) try: # Clean up VREP if cache_walker and \ self.last_walker_params is not None and \ np.array_equal(x[4:], self.last_walker_params): self.run() self.load_prev_walker() else: self.exit() self.start() self.load_scene() self.load_walker() self.run() # Assign parameters self.scale_walker(x[4:]) # Cache walker self.walker_filename = "logs/models/{}.ttm" \ .format(time.strftime("%Y.%m.%d-%H.%M.%S")) self.last_walker_params = x[4:] self.save_walker(self.walker_filename) # Set the other parameters self.set_cpg(gait, x[0], x[1]) self.walker.set_left_bias(x[2]) self.walker.set_right_bias(x[3]) # Get starting position start = self.get_pos(self.walker.base_handle) # Run the simulation print('Running trial...') self.walk(max_steps) # Calculate how far the robot walked end = self.get_pos(self.walker.base_handle) dist = self.calc_dist(start, end) print("Distance traveled: {}".format(dist)) # Clean up VREP self.remove_walker() self.close_scene() self.stop() self.exit() return np.array([dist]) except SceneLoadingError as e: print(str(e)) self.stop() self.close_scene() except (ConnectionError, WalkerLoadingError, MotorLoadingError, HandleLoadingError) as e: print("Encountered an exception: {} " "disconnecting from remote API server".format(e)) vrep.simxFinish(self.client_id) traceback.print_exc() self.stop() self.close_scene() raise e
def exit(self): print("Exiting VREP") vrep.simxFinish(self.client_id)
def objective(x, cache_walker=True): """ 8 sw, 6 hw x[0] = f # Frequency [1, 45] x[1] = phase_offset # phase difference btwn leg and foot. [-pi, pi] x[2] = R_l # Sequence of leg extension length. [0, .04] x[3] = R_f # Sequence of foot extension lengths. [0, .04] x[4] = X_l # Sequence of leg offset values. [0, .04] x[5] = X_f # Sequence of foot offset values. [0, .04] x[6] = left bias of walker [.5, 1] x[7] = right bias of walker [.5, 1] x[8] = front left leg scaling [.8, 1.2] x[9] = front right leg scaling [.8, 1.2] x[11] = middle left leg scaling [.8, 1.2] x[12] = middle right leg scaling [.8, 1.2] x[13] = rear left pair leg scaling [.8, 1.2] x[14] = rear right pair leg scaling [.8, 1.2] walker params = x[8:] :return: """ if not is_1D(x): x = np.asarray(x)[0] else: x = np.asarray(x) self.start() gait_params = x[:6] left_bias = x[6] right_bias = x[7] walker_params = x[8:] print("walker params", walker_params) print('\nParameters: ' + str(x)) try: # Clean up VREP if cache_walker and \ self.last_walker_params is not None and \ np.array_equal(walker_params, self.last_walker_params): self.run() self.load_prev_walker() else: self.exit() self.start() self.load_scene() self.load_walker() self.run() # Assign parameters self.scale_walker(walker_params) # Cache walker self.walker_filename = "logs/models/{}.ttm" \ .format(time.strftime("%Y.%m.%d-%H.%M.%S")) self.last_walker_params = walker_params self.save_walker(self.walker_filename) # Set the other parameters self.set_cpg(build_gait(gait_params)) self.walker.set_left_bias(left_bias) self.walker.set_right_bias(right_bias) # Get starting position start = self.get_pos(self.walker.base_handle) # Run the simulation print('Running trial...') self.walk(max_steps) # Calculate how far the robot walked end = self.get_pos(self.walker.base_handle) dist = self.calc_dist(start, end) print("Distance traveled: {}".format(dist)) # Clean up VREP self.remove_walker() self.close_scene() self.stop() self.exit() return np.array([dist]) except SceneLoadingError as e: print(str(e)) self.stop() self.close_scene() except (ConnectionError, WalkerLoadingError, MotorLoadingError, HandleLoadingError) as e: print("Encountered an exception: {} " "disconnecting from remote API server".format(e)) vrep.simxFinish(self.client_id) traceback.print_exc() self.stop() self.close_scene() raise e