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 get_pos(self, handle): res, pos = vrep.simxGetObjectPosition(self.client_id, handle, -1, OP_MODE) if res not in VALID_ERROR_CODES: raise HandleLoadingError(handle) return pos
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 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()