optimization class for pygmo

optimization.py

class optimization:

	def __init__(self, enc_vector, gps_vector, ratio_t2ml, ratio_t2mr,
		abs_zero_initial, abs_first_coef, abs_second_coef, abs_third_coef,
		abs_first_exp, abs_second_exp, abs_third_exp, orientation_start):

		self.enc_vector = enc_vector
		self.gps_vector = gps_vector
		self.ratio_t2ml = ratio_t2ml
		self.ratio_t2mr = ratio_t2mr
		self.abs_zero_initial = abs_zero_initial
		self.abs_first_coef = abs_first_coef
		self.abs_second_coef = abs_second_coef
		self.abs_third_coef = abs_third_coef
		self.abs_first_exp = abs_first_exp
		self.abs_second_exp = abs_second_exp
		self.abs_third_exp = abs_third_exp
		self.orientation_start = orientation_start


	def fitness(self, x):

		offset = x[0]	
		delta_orentation = x[1]
		ratio_left = x[2]
		ratio_right = x[3]
		global_pose_theta = (self.orientation_start + delta_orentation) - math.pi
		global_pose_x = 0.0
		global_pose_y = 0.0

		error = 0.0
		i = 1
		for index, enc in self.enc_vector.iterrows():
			distance = ((enc['left_wheel_ticks'] * (self.ratio_t2ml + ratio_left)) + (enc['right_wheel_ticks'] * (self.ratio_t2mr + ratio_right))) / 2.0


			orentation = distance * (math.tan(-((enc['abs_enc_ticks'] - (self.abs_zero_initial + offset)) * self.abs_first_coef * pow(10.0, self.abs_first_exp) + self.abs_second_coef * pow(10.0, self.abs_second_exp) * pow((enc['abs_enc_ticks'] - (self.abs_zero_initial + offset)), 2.0) + self.abs_third_coef * pow(10.0, self.abs_third_exp) * pow((enc['abs_enc_ticks'] - (self.abs_zero_initial + offset)), 3.0))))/1.70
			pose_r_x = distance * math.cos(orentation)
			pose_r_y = distance * math.sin(orentation)
			pose_r_theta = orentation

			global_pose_x = global_pose_x + pose_r_x * math.cos(global_pose_theta) - pose_r_y * math.sin(global_pose_theta)
			global_pose_y = global_pose_y + pose_r_x * math.sin(global_pose_theta) + pose_r_y * math.cos(global_pose_theta)
			global_pose_theta = global_pose_theta + pose_r_theta
			if(math.fabs((enc['stamp'] - self.gps_vector.loc[i, 'stamp'])) < 0.0001):
				error = error + (math.pow(self.gps_vector.loc[i, 'x'] - global_pose_x, 2) + math.pow(self.gps_vector.loc[i, 'y'] - global_pose_y, 2))
				i = i + 1

		return [error]


	def get_bounds(self):
		return ([-200,-0.5,-0.002,-0.002],[200,0.5,0.002,0.002])