Khepera-II-IAR/odometry_algorithm.py at master · AngusP/Khepera-II-IAR · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#!/usr/bin/env python

#
#     - - - -  I A R  - - - -
#
#  s1311631         Angus Pearson
#  s1346981    Jevgenij Zubovskij
#


from odometry_state import Odometry_State
import constants


import math
import sys
import time

class Odometry_Algorithm:
        def __init__(self):
                pass


        #normalizes angle in degrees to -180 : 180 degrees
    	def normalize_angle(self, angle):
		if angle < 0:
		    angle = angle % -360
		    if angle < -180:
				angle = 360 + angle
		else:
		    angle = angle % 360
		    if angle > 180:
				angle = -(360 - angle)
		return angle

	#calculate differences in distance driven by different wheels
	def delta_s(self, delta_odo):

		result 		= 0
		result 		= ((delta_odo[0] + delta_odo[1]) / float(2) ) / constants.TICKS_PER_MM

		return result # mm

	#calculate the orientation angle
	def delta_theta(self, delta_odo):

		result 		= 0
		result 		= ((delta_odo[1] - delta_odo[0]) / constants.TICKS_PER_MM) / constants.WHEEL_BASE_MM


		return result #radians

	#calculate the change in angle, X and Y
	def delta_x_y_angle(self, curr_theta, delta_odo):
		result 	    = [0]*3

		delta_dist  = self.delta_s(delta_odo)
		delta_angle = self.delta_theta(delta_odo)


		new_angle = curr_theta + delta_angle / float(2) # this is the alternative


		delta_x     = delta_dist*math.cos(new_angle) # in mm
	    	delta_y     = delta_dist*math.sin(new_angle) # in mm


		result[0] = delta_x
		result[1] = delta_y
		result[2] = delta_angle


		return result


	#get the new state
	def new_state(self, prev_state, new_odo):

		delta_odo = [new_odo[0] - prev_state.odo[0], new_odo[1] - prev_state.odo[1] ]
		state_change = self.delta_x_y_angle(prev_state.theta, delta_odo)

 		#update the variables
		prev_x = prev_state.x
		prev_y = prev_state.y
		prev_theta = prev_state.theta
		#t = constants.MEASUREMENT_PERIOD_S

		#calculate the new values
		x_n 	= prev_x     + state_change[0]
		y_n 	= prev_y     + state_change[1]
		theta_n	= prev_theta + state_change[2]


		#return new state
		result 	     = Odometry_State()
		#result.time  = prev_state.time + t
		result.time  = time.time()
                result.x     = x_n
		result.y     = y_n
		result.odo   = new_odo
		result.theta = theta_n


		return result