diff --git a/hakoniwa/python/adjust_pid_params.py b/hakoniwa/python/adjust_pid_params.py new file mode 100644 index 00000000..9b3ac53d --- /dev/null +++ b/hakoniwa/python/adjust_pid_params.py @@ -0,0 +1,156 @@ +import sys +import random + +# パラメータの範囲 +Kp_range = (0, 10) +Ki_range = (0, 2) +Kd_range = (0, 10) + +# 初期集団サイズ +population_size = 10 + +def read_evaluation_results(filename): + results = { + 'steady_state': 0.0, + 'steady_state_ok': False, + 'rise_time': 0.0, + 'rise_time_ok': False, + 'delay_time': 0.0, + 'delay_time_ok': False, + 'overshoot': 0.0, + 'overshoot_ok': False, + 'settling_time': 0.0, + 'settling_time_ok': False + } + + try: + with open(filename, 'r') as file: + lines = file.readlines() + for line in lines: + if 'Steady state value' in line: + results['steady_state'] = float(line.split(':')[1].split(' ')[1]) + results['steady_state_ok'] = 'OK' in line + elif 'Rise time' in line: + results['rise_time'] = float(line.split(':')[1].split(' ')[1]) + results['rise_time_ok'] = 'OK' in line + elif 'Delay time' in line: + results['delay_time'] = float(line.split(':')[1].split(' ')[1]) + results['delay_time_ok'] = 'OK' in line + elif 'Maximum overshoot' in line: + results['overshoot'] = float(line.split(':')[1].split(' ')[1]) + results['overshoot_ok'] = 'OK' in line + elif 'settling time' in line: + results['settling_time'] = float(line.split(':')[1].split(' ')[1]) + results['settling_time_ok'] = 'OK' in line + except Exception as e: + print(f"Error reading evaluation results: {e}") + + return results + +def read_pid_params(filename): + params = { + 'Kp': 0.0, + 'Ki': 0.0, + 'Kd': 0.0 + } + + try: + with open(filename, 'r') as file: + lines = file.readlines() + for line in lines: + if 'DroneAvator_Kp' in line: + params['Kp'] = float(line.split('=')[1]) + elif 'DroneAvator_Ki' in line: + params['Ki'] = float(line.split('=')[1]) + elif 'DroneAvator_Kd' in line: + params['Kd'] = float(line.split('=')[1]) + except Exception as e: + print(f"Error reading PID params: {e}") + + return params + +def fitness(results): + # フィットネス関数を定義 + fitness_value = 0 + if results['steady_state_ok']: + fitness_value += 1 + if results['rise_time_ok']: + fitness_value += 1 + if results['delay_time_ok']: + fitness_value += 1 + if results['overshoot_ok']: + fitness_value += 1 + if results['settling_time_ok']: + fitness_value += 1 + return fitness_value + +def generate_initial_population(size): + population = [] + for _ in range(size): + individual = { + 'Kp': random.uniform(*Kp_range), + 'Ki': random.uniform(*Ki_range), + 'Kd': random.uniform(*Kd_range) + } + population.append(individual) + return population + +def select_parents(population, fitnesses): + total_fitness = sum(fitnesses) + if total_fitness == 0: + # フィットネス値がすべてゼロの場合、ランダムに親を選択 + parents = random.sample(population, 2) + else: + parents = random.choices(population, weights=fitnesses, k=2) + return parents + +def crossover(parent1, parent2): + child = { + 'Kp': (parent1['Kp'] + parent2['Kp']) / 2, + 'Ki': (parent1['Ki'] + parent2['Ki']) / 2, + 'Kd': (parent1['Kd'] + parent2['Kd']) / 2 + } + return child + +def mutate(individual): + mutation_rate = 0.1 + if random.random() < mutation_rate: + individual['Kp'] += random.uniform(-0.1, 0.1) + individual['Ki'] += random.uniform(-0.05, 0.05) + individual['Kd'] += random.uniform(-0.1, 0.1) + + individual['Kp'] = max(min(individual['Kp'], Kp_range[1]), Kp_range[0]) + individual['Ki'] = max(min(individual['Ki'], Ki_range[1]), Ki_range[0]) + individual['Kd'] = max(min(individual['Kd'], Kd_range[1]), Kd_range[0]) + return individual + +def adjust_pid_params(results, params, population): + fitnesses = [fitness(results) for _ in population] + new_population = [] + for _ in range(len(population)): + parent1, parent2 = select_parents(population, fitnesses) + child = crossover(parent1, parent2) + child = mutate(child) + new_population.append(child) + return new_population + +if __name__ == "__main__": + if len(sys.argv) == 2 and sys.argv[1] == '--generate_initial_population': + initial_population = generate_initial_population(population_size) + for individual in initial_population: + print(individual['Kp'], individual['Ki'], individual['Kd']) + elif len(sys.argv) == 3: + evaluation_file = sys.argv[1] + pid_param_file = sys.argv[2] + + results = read_evaluation_results(evaluation_file) + params = read_pid_params(pid_param_file) + initial_population = generate_initial_population(population_size) + new_population = adjust_pid_params(results, params, initial_population) + + for individual in new_population: + print(individual['Kp'], individual['Ki'], individual['Kd']) + else: + print("Usage: python adjust_pid_params.py ") + print(" python adjust_pid_params.py --generate_initial_population") + sys.exit(1) diff --git a/hakoniwa/run_pid_sim.bash b/hakoniwa/run_pid_sim.bash new file mode 100644 index 00000000..43db2c5d --- /dev/null +++ b/hakoniwa/run_pid_sim.bash @@ -0,0 +1,50 @@ +#!/bin/bash + +# シミュレーション実行時間 +SIMULATION_TIME=10 + +# 初期集団を生成 +python python/adjust_pid_params.py --generate_initial_population > population.txt + +# シミュレーションと評価のループ +for i in {1..10}; do + echo "Generation $i" + + # 現在の集団から各個体を取り出しシミュレーションを実行 + while IFS= read -r line; do + Kp=$(echo $line | awk '{print $1}') + Ki=$(echo $line | awk '{print $2}') + Kd=$(echo $line | awk '{print $3}') + echo "Individual: Kp=$Kp, Ki=$Ki, Kd=$Kd" + + # PIDパラメータファイルの更新 + echo "DroneAvator_Kp=$Kp" > drone_control_params.txt + echo "DroneAvator_Ki=$Ki" >> drone_control_params.txt + echo "DroneAvator_Kd=$Kd" >> drone_control_params.txt + + sleep 1 + + # シミュレーションの開始 + hako-cmd start + + # 指定時間待機 + sleep $SIMULATION_TIME + + # シミュレーションの停止 + hako-cmd stop + + # シミュレーション結果の評価 + python python/control_evaluate.py ./drone_log0/drone_dynamics.csv > evaluation_result.txt + cat evaluation_result.txt + + # 評価結果に基づき新しい集団を生成 + python python/adjust_pid_params.py evaluation_result.txt drone_control_params.txt > new_population.txt + + # 集団を更新 + mv new_population.txt population.txt + + # シミュレーションのリセット + hako-cmd reset + + done < population.txt +done