def test_trajectory(self):
"""
Test the generate trajectory , USE SCENE 8
Tse_init --> initial position of eef
Tsc_init --> initial position of cube
Tsc_final --> final position of cube
Tce_grasp --> eef grasping position
Tce_standoff --> use it go "above position" offset
"""
Tse_init = self.allMatrix.Tse_init
Tsc_init = self.allMatrix.Tsc_init
Tsc_final =self.allMatrix.Tsc_final
Tce_grasp =self.allMatrix.Tce_grasp
Tce_standoff = self.allMatrix.Tce_standoff
trajectory = self.TrajectoryGenerator(Tse_init, Tsc_init, Tsc_final, Tce_grasp, Tce_standoff,self.N)
functions.write_csv(trajectory,"trajectory",logger)
def pick_and_place(self):
logger.info("Initializing values")
#get initial current configuration
q0 = self.init_conditions[2]
#get refference current configuration & find eef position
q_ref = self.init_conditions[3]
_,Tbe = self.calc_jacobian(q=q_ref[:8])
Tse_init = np.matmul(self.allMatrix.get_Tsb(q_ref),Tbe)
#get initial cube position and placing postion
Tsc_init = self.allMatrix.get_matrix(self.init_conditions[0])
Tsc_final = self.allMatrix.get_matrix(self.init_conditions[1])
#pregrasping and stanoff positions
Tce_grasp =self.allMatrix.Tce_grasp
Tce_standoff = self.allMatrix.Tce_standoff
#generate trajectory
trajectory = self.TrajectoryGenerator(Tse_init, Tsc_init, Tsc_final, Tce_grasp, Tce_standoff,self.N)
functions.write_csv(trajectory,"trajectory",logger)
#initialize
N = len(trajectory) -1
error_list = []
# self.Kp = np.zeros((4,4))
# self.Ki = np.zeros((4,4))
data=[q0]
logger.info("Starting simulation")
for i in range(N):
#set current configuration
q = data[-1]
#get matrices
_,Tbe = self.calc_jacobian(q=q[:8])
X = np.matmul(self.allMatrix.get_Tsb(q),Tbe)
Xd = self.allMatrix.list_to_array(trajectory[i])
Xd_next = self.allMatrix.list_to_array(trajectory[i+1])
#call feedForward Control
Vel,error = self.get_joint_vel(X,Xd,Xd_next,q=q[:8])
error_list.append(error) # keep a list with all errors
#copy gripper state
q[-1]=trajectory[i][-1]
#call next state
out = self.next_state(q,Vel)
data.append(out)
logger.info("Simulation ended")
logger.info("Saving output files")
#save output at csv format
functions.write_csv(error_list,"error",logger)
functions.write_csv(data,"pick_place",logger)
self.plotter(error_list)
def test_nextState(self):
"""
test the next state function on scene 6
create 3 csv files forward,sideways,sideways
"""
#initial values
start = np.array([0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0])
velocity_1 = np.array([0, 0,0,0,0,10,10,10,10])
velocity_2 = np.array([0, 0,0,0,0,-10,10,-10,10])
velocity_3 = np.array([0, 0,0,0,0,-10,10,10,-10])
#test of forward
data = []
print("Executing nextState forward")
logger.info("Executing nextState forward")
out = self.next_state(start,velocity_1)
for i in range(100):
out = self.next_state(out,velocity_1)
data.append(out)
functions.write_csv(data,"nextState_forward",logger)
print("last configuration -->")
print(data[-1])
logger.info("last configuration -->")
logger.info(data[-1])
data = []
#test of sideways
print("Executing nextState sideways")
logger.info("Executing nextState sideways")
out = self.next_state(start,velocity_2)
for i in range(100):
out = self.next_state(out,velocity_2)
data.append(out)
functions.write_csv(data,"nextState_sideways",logger)
print("last configuration -->")
print(data[-1])
logger.info("last configuration -->")
logger.info(data[-1])
#test of turning
data = []
print("Executing nextState turn")
logger.info("Executing nextState turn")
out = self.next_state(start,velocity_3)
for i in range(100):
out = self.next_state(out,velocity_3)
data.append(out)
functions.write_csv(data,"nextState_turn",logger)
print("last configuration -->")
print(data[-1])
logger.info("last configuration -->")
logger.info(data[-1])
solar_m_result.append(solar_multiplier)
lcoe_result.append(lcoe)
capacity_storage_result.append(capacity_storage)
gas_ratio_result.append(gas_ratio)
overhead_ratio_result.append(overhead_ratio)
print wind_multiplier, solar_multiplier, lcoe, capacity_storage, overhead_ratio
fig = plt.figure(figsize=(16, 9))
plt.scatter(solar_m_result,
wind_m_result,
s=100,
c=lcoe_result,
cmap=plt.cm.coolwarm)
plt.xlabel('solar multiplier')
plt.colorbar()
#plt.ylim(ymin=0, ymax=40)
#plt.xlim(xmin=0, xmax=40)
plt.ylabel('wind multiplier')
plt.grid()
data = [
wind_m_result, solar_m_result, lcoe_result, capacity_storage_result,
gas_ratio_result, overhead_ratio_result
]
data = map(list, zip(*data))
write_csv(data, 'b.txt')
#plt.savefig(filename)
#os.system('gwenview %s' % filename)
# 局所解とバイアスに分ける
solutions, bias = functions.divide_solutions_bias(solutions_data)
# 評価値の結果のリスト
evaluations_result = []
for num_experiment in range(1, 101):
print(num_experiment)
# 対象のデータの読み込み
data = functions.read_csv(read_filename)
del data[0]
data = functions.transform_to_float(data)
# before_index = functions.get_result(data, functions.get_evaluations_list(data, solutions, bias), num_experiment, functions.get_best_solution_index(bias), solutions)[2]
# 次の世代の作成
for num in range(num_execute):
# print('-------')
# print(functions.get_evaluation_value(data[before_index], solutions, bias))
data = next_generation_MGG_improve(data, solutions, bias, num_parents, num_children, num_elite_preservation, num_dimentions)
# print(functions.get_result(data, functions.get_evaluations_list(data, solutions, bias), num_experiment, functions.get_best_solution_index(bias), solutions))
# before_index = functions.get_result(data, functions.get_evaluations_list(data, solutions, bias), num_experiment, functions.get_best_solution_index(bias), solutions)[2]
# 新しい世代をcsvに書き込む
functions.write_csv(write_filename + '_%i' % num_experiment, data)
evaluations = functions.get_evaluations_list(data, solutions, bias)
evaluation_vector = functions.get_result(data, evaluations, num_experiment, functions.get_best_solution_index(bias), solutions)
evaluations_result.append(evaluation_vector)
final_result = functions.get_final_result(evaluations_result)
functions.write_result(result_file, evaluations_result, final_result)
parser.add_argument("--situation", "-s", required=True, help="Situation")
args = parser.parse_args()
csv_dir = './../csv_files/latent_vectors/' + args.situation + "/"
if os.path.exists(csv_dir + args.output_dir) is False:
print("対象のファイルが見つかりません。")
exit()
original_data_path = csv_dir + args.old_dir + "/latent_vectors.csv"
parents_index_path = csv_dir + args.output_dir + "/parents_index.csv"
children_index_path = csv_dir + args.output_dir + "/children_index.csv"
children_path = csv_dir + args.output_dir + "/children.csv"
original_data = functions.read_csv(original_data_path)
parents_index = functions.read_csv(parents_index_path)
parents_index = functions.transform_to_int(parents_index)
parents_index = parents_index[0]
children_index = functions.read_csv(children_index_path)
children_index = functions.transform_to_int(children_index)
children_index = children_index[0]
children = functions.read_csv(children_path)
children = functions.transform_to_float(children)
new_data = functions.replace(original_data, children, parents_index,
children_index)
# 新しい潜在ベクトル群をcsvファイルに書き込む
functions.write_csv(csv_dir + args.output_dir + "/latent_vectors.csv",
new_data)
parser.add_argument("--num_children", "-c", default=16, type=int, help="Number of children")
args = parser.parse_args()
csv_dir = './../csv_files/latent_vectors/' + args.situation + "/"
if os.path.exists(csv_dir + args.output_dir) is True:
exit()
if os.path.exists(csv_dir + args.output_dir) is False:
os.mkdir(csv_dir + args.output_dir)
input_path = csv_dir + args.input_dir + '/latent_vectors.csv'
latent_vectors = functions.read_csv(input_path)
latent_vectors = functions.transform_to_float(latent_vectors)
latent_vectors = np.array(latent_vectors)
num_vectors = len(latent_vectors)
parents_index = functions.make_random_list(num_vectors, args.num_parent)
parent_vectors = np.zeros((args.num_parent, len(latent_vectors[0])))
for index in range(len(parents_index)):
parent_vectors[index] = np.copy(latent_vectors[parents_index[index]])
children = functions.make_children(parent_vectors, args.num_children)
# 親に使ったベクトルのインデックスのcsvファイルを作成
functions.write_csv_for_vector(csv_dir + args.output_dir + "/parents_index.csv", parents_index)
# 親に使ったベクトルのcsvファイルを作成
functions.write_csv(csv_dir + args.output_dir + "/parents.csv", parent_vectors)
# 新しい子のcsvファイルの作成
functions.write_csv(csv_dir + args.output_dir + "/children.csv", children)
