├── LICENSE
├── .gitignore
├── invkin.py
├── three_link.py
├── driver.py
├── genetic_algorithm.py
├── plotter.py
└── trajectory_generation.py


/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2017 Fauzan Zaid
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
  1 | # Byte-compiled / optimized / DLL files
  2 | __pycache__/
  3 | *.py[cod]
  4 | *$py.class
  5 | 
  6 | # C extensions
  7 | *.so
  8 | 
  9 | # Distribution / packaging
 10 | .Python
 11 | env/
 12 | build/
 13 | develop-eggs/
 14 | dist/
 15 | downloads/
 16 | eggs/
 17 | .eggs/
 18 | lib/
 19 | lib64/
 20 | parts/
 21 | sdist/
 22 | var/
 23 | wheels/
 24 | *.egg-info/
 25 | .installed.cfg
 26 | *.egg
 27 | 
 28 | # PyInstaller
 29 | #  Usually these files are written by a python script from a template
 30 | #  before PyInstaller builds the exe, so as to inject date/other infos into it.
 31 | *.manifest
 32 | *.spec
 33 | 
 34 | # Installer logs
 35 | pip-log.txt
 36 | pip-delete-this-directory.txt
 37 | 
 38 | # Unit test / coverage reports
 39 | htmlcov/
 40 | .tox/
 41 | .coverage
 42 | .coverage.*
 43 | .cache
 44 | nosetests.xml
 45 | coverage.xml
 46 | *.cover
 47 | .hypothesis/
 48 | 
 49 | # Translations
 50 | *.mo
 51 | *.pot
 52 | 
 53 | # Django stuff:
 54 | *.log
 55 | local_settings.py
 56 | 
 57 | # Flask stuff:
 58 | instance/
 59 | .webassets-cache
 60 | 
 61 | # Scrapy stuff:
 62 | .scrapy
 63 | 
 64 | # Sphinx documentation
 65 | docs/_build/
 66 | 
 67 | # PyBuilder
 68 | target/
 69 | 
 70 | # Jupyter Notebook
 71 | .ipynb_checkpoints
 72 | 
 73 | # pyenv
 74 | .python-version
 75 | 
 76 | # celery beat schedule file
 77 | celerybeat-schedule
 78 | 
 79 | # SageMath parsed files
 80 | *.sage.py
 81 | 
 82 | # dotenv
 83 | .env
 84 | 
 85 | # virtualenv
 86 | .venv
 87 | venv/
 88 | ENV/
 89 | 
 90 | # Spyder project settings
 91 | .spyderproject
 92 | .spyproject
 93 | 
 94 | # Rope project settings
 95 | .ropeproject
 96 | 
 97 | # mkdocs documentation
 98 | /site
 99 | 
100 | # mypy
101 | .mypy_cache/
102 | 


--------------------------------------------------------------------------------
/invkin.py:
--------------------------------------------------------------------------------
 1 | import math
 2 | import numpy as np
 3 | 
 4 | def div(a,b):
 5 |         return a/b if b is not 0 else math.inf if a>0 else -math.inf
 6 | 
 7 | class Arm :
 8 |     '''
 9 |     Class to define arm parameters
10 |     Input: arm lenghts , initial position of end arm(angles of each arm) , final (x,y) coordinates of end effector
11 |     Output: inv_kin() returns the angles(in radians) for a given (x,y) position , get_position(angles(radians)) returns the (x,y) coordinates
12 |             for a given set of angles
13 |     '''
14 |     def __init__(self,arm_lens):
15 |         """
16 |         Set the default parameters of the arm
17 |         input
18 |             arm_lens: array of the order [[link1(lowest link), link2]
19 |         """
20 |                     
21 |         self.arm_lens = arm_lens
22 |         #self.intial_position = intial_position
23 |         #self.final_position = final_position
24 | 
25 |     def get_position(self,angles):
26 |         """
27 |         Returns the (x,y) coordinates of the arm, given its link angles
28 |         input
29 |             angles: array of the order [link1,link2]
30 |         """
31 | 
32 |         x = self.arm_lens[0]*np.cos(angles[0]) + self.arm_lens[1]* np.cos(angles[0]+angles[1])
33 |         y = self.arm_lens[0]*np.sin(angles[0]) + self.arm_lens[1]* np.sin(angles[0]+angles[1])
34 |         return x,y
35 | 
36 |     def inv_kin(self,final_coords):
37 |         #if final_coords == None:
38 |         #    print('Please pass final coordinates')
39 |         """
40 |         Returns the angles for a given (x,y) coordinate
41 |         The angle of link-2 is constrained to move between [0,pi]
42 |         input:
43 |             final_coords : array
44 |                             array of current joint angles
45 |         """
46 |             
47 |         # definition of a division to allow for the case division with 0
48 |         D = div( (final_coords[0]**2 + final_coords[1]**2 - self.arm_lens[0]**2 - self.arm_lens[1]**2), 2*self.arm_lens[0]*self.arm_lens[1] )
49 |         #final angles
50 |         angles = [0,0]
51 |         #use of atan to use the range(-pi,pi) rather than (0,pi/2)
52 |         temp = math.atan2( (1-(D**2))**(1/2),D  )
53 |         #print(temp,D)
54 |         angles[1] = temp if temp>=0 else -temp
55 |         angles[0] = math.atan2(final_coords[1],final_coords[0]) - math.atan2( self.arm_lens[1]*np.sin(angles[1]), self.arm_lens[0]+ self.arm_lens[1]*np.cos(angles[1]) )
56 |         return np.array(angles)
57 | 
58 |     def time_series(self,coordinate_series):
59 |         """
60 |             Recursively calls  function inv_kin to calculate joint angles for every (x,y) coordinate
61 |             input:
62 |                 coordinate_series: array of the order [[x1 y1],
63 |                                                        [x2 y2],..
64 |                                                         ...
65 |                                                        [xn yn]]
66 |         """
67 |         angle_series=[]
68 |         for i in range(len(coordinate_series)):
69 |             angle_series.append(self.inv_kin(coordinate_series[i]))
70 |         return angle_series
71 | def test():
72 | 
73 |     initialangles = [.349 , .349]
74 |     lengths = [4,4]
75 |     initialPos = [0,0]
76 |     Arm1 = Arm(lengths)
77 | 
78 |     #print(Arm1.inv_kin())
79 |     #print(Arm1.get_position(Arm1.inv_kin()))
80 |     series = [[2,2],[3,3],[4,4]]
81 |     print(Arm1.time_series(series))
82 | 
83 | #test()
84 | 


--------------------------------------------------------------------------------
/three_link.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import scipy.optimize
  3 | 
  4 | 
  5 | class Arm3Link:
  6 | 
  7 |     def __init__(self,Len=None):
  8 | 
  9 |         """Set up the basic parameters of the arm.
 10 |         order [link1(lowest link), link2, link3].
 11 |         input
 12 |             L : np.array
 13 |                 the arm segment lengths
 14 |         """
 15 |         # initial joint angles set randomly, they will be updated later
 16 |         self.angles = [.3, .3, 0] 
 17 |         # A default arm position(set randomly, any angles would work)
 18 |         self.default = np.array([np.pi/4, np.pi/4, np.pi/4]) 
 19 |         # arm lengths
 20 |         self.Len = np.array([1, 1, 1]) if Len is None else Len
 21 | 
 22 |     def inv_kin(self, xy):
 23 |         """
 24 |         Given an (x,y) coords of the hand, return a set of joint angles (q)
 25 |         using the scipy.optimize package
 26 |         input
 27 |             xy : tuple
 28 |                 the desired xy position of the arm
 29 |         returns : list
 30 |             the optimal [link 1-shoulder, link 2- elbow, link 3-wrist] angle configuration
 31 |         """
 32 | 
 33 |         def distance_to_default(q, *args):
 34 |             """Objective function to minimize
 35 |             Calculates the euclidean distance of arms.The weights allow us to penalise 
 36 |             an arm for moving too far from the default position
 37 |             input
 38 |                 q : array
 39 |                     the list of current joint angles
 40 |             returns : 
 41 |                 euclidean distance to the default arm position
 42 |             """
 43 |             weight = [1, 1, 1.3]
 44 |             return np.sqrt(np.sum([(qi - q0i)**2 * wi for qi, q0i, wi in zip(q, self.default, weight)]))
 45 |             
 46 | 
 47 |         def x_constraint(q, xy):
 48 |             """Function required as a parameter in the scipy fucntion
 49 |             Input
 50 |                 q : array
 51 |                     the list of current joint angles
 52 |                 xy : array
 53 |                     current xy position (not used)
 54 |             returns : array
 55 |                 the difference between current and desired x position
 56 |             """
 57 |             x = (self.Len[0]*np.cos(q[0]) + self.Len[1]*np.cos(q[0]+q[1]) +self.Len[2]*np.cos(np.sum(q))) - xy[0]
 58 |             return x
 59 | 
 60 |         def y_constraint(q, xy):
 61 | 
 62 |             """Function required as a parameter in the scipy fucntion
 63 |             Input
 64 |                 q : array
 65 |                     the list of current joint angles
 66 |                 xy : array
 67 |                     current xy position (not used)
 68 |             returns : array
 69 |                 the difference between current and desired x position
 70 |             """
 71 |             y = (self.Len[0]*np.sin(q[0]) + self.Len[1]*np.sin(q[0]+q[1]) +self.Len[2]*np.sin(np.sum(q))) - xy[1]
 72 |             return y
 73 | 
 74 | 
 75 |         return scipy.optimize.fmin_slsqp(func=distance_to_default,x0=self.angles,eqcons=[x_constraint,y_constraint],args=(xy,),iprint=0) 
 76 | 
 77 |     def time_series(self,coordinate_series):
 78 |         """
 79 |         Given a series of coordinates in the form [[x0,y0],
 80 |                                                    [x1,y1],
 81 |                                                    ...
 82 |                                                    [xn,yn]]
 83 |         we get the series of link angles [theta1,theta2,theta3]
 84 |         input
 85 |             coordinate_series: array
 86 |         returns : array
 87 |             the series of angles for every coordinate provided
 88 |         """
 89 |         angle_series=[]
 90 |         self.angles = self.inv_kin(coordinate_series[0])
 91 | 
 92 |         for i in range(len(coordinate_series)):
 93 |             angle_series.append(self.inv_kin(coordinate_series[i]))
 94 |             self.angles = self.inv_kin(coordinate_series[i])
 95 | 
 96 |         return angle_series
 97 | 
 98 | def test():
 99 | 
100 |     arm = Arm3Link()
101 |     
102 |     x = [-.75,.5]
103 |     y= [.25,.40]
104 | 
105 |     coordinate_series = list(zip(x,y))
106 |     arm.time_series(coordinate_series)
107 |     print(arm.time_series(coordinate_series))
108 |     
109 | 
110 | #test()
111 | 
112 | 
113 | 
114 | 
115 | 
116 | 


--------------------------------------------------------------------------------
/driver.py:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/python3
  2 | 
  3 | 
  4 | 
  5 | import os
  6 | 
  7 | import numpy as np
  8 | import matplotlib.pyplot as plt
  9 | 
 10 | from plotter import Plotter
 11 | from genetic_algorithm import GeneticAlgorithm
 12 | import trajectory_generation as tg
 13 | from invkin import Arm
 14 | from three_link import Arm3Link
 15 | 
 16 | 
 17 | 
 18 | class ProblemParams:
 19 | 	"""A struct like class to store problem parameters"""
 20 | 	def __init__(self, description=None, link_lengths=None, start_cood=None, end_cood=None, obs_coods=None):
 21 | 		self.description = description
 22 | 		self.link_lengths = link_lengths
 23 | 		self.start_cood = start_cood
 24 | 		self.end_cood = end_cood
 25 | 		self.obs_coods = obs_coods
 26 | 
 27 | 
 28 | preset_params = [
 29 | 	ProblemParams("Two links, two obstacles", [4,4], [6.5,2.8], [-3.3,5.1], [[-0,5.3],[5.4,3.2]]),
 30 | 	ProblemParams("Two links, three obstacles", [4,4], [7,2.6], [-5,2.8], [[5,3.8],[1.7,5.8],[-2.5,5.8]]),
 31 | 	ProblemParams("Three links, two obstacles", [4,4,3], [8.1,4.9], [-7,5.8], [[5.5,7.7],[2.8,8.5]]),
 32 | 	ProblemParams("Three links, three obstacles", [4,4,3], [8.1,4.9], [-7,5.8], [[5.5,7.7],[2,9],[-2.8,8.5]]),
 33 | ]
 34 | 
 35 | 
 36 | def select_param_method():
 37 | 	# Clear terminal on win, linux
 38 | 	os.system('cls' if os.name == 'nt' else 'clear')
 39 | 	print("Robotic arm trajectory using Genetic Algorithm\n")
 40 | 	print("Select problem parameters:")
 41 | 	print("  1: Choose from presets")
 42 | 	# print("  2: Custom")
 43 | 	print("\n  q: Quit")
 44 | 
 45 | 	choice = None
 46 | 	while choice == None:
 47 | 		choice = input("Choice: ")
 48 | 		if choice == 'q':
 49 | 			return 'q'
 50 | 		elif choice.isdigit() and 1 <= int(choice) <= 2:
 51 | 			choice = int(choice)
 52 | 		else:
 53 | 			print("Invalid input!")
 54 | 			choice = None
 55 | 	return choice
 56 | 
 57 | 
 58 | def select_preset_param(preset_params):
 59 | 	# Clear terminal on win, linux
 60 | 	os.system('cls' if os.name == 'nt' else 'clear')
 61 | 	print("Robotic arm trajectory using Genetic Algorithm\n")
 62 | 	print("Select a preset problem:")
 63 | 	for i,param in enumerate(preset_params):
 64 | 		print("  {0}: {1}".format(i+1, param.description))
 65 | 	print("\n  q: Quit")
 66 | 	
 67 | 	choice = None
 68 | 	while choice == None:
 69 | 		choice = input("Choice: ")
 70 | 		if choice == 'q':
 71 | 			return 'q'
 72 | 		elif choice.isdigit() and 1 <= int(choice) <= len(preset_params):
 73 | 			choice = int(choice)
 74 | 		else:
 75 | 			print("Invalid input!")
 76 | 			choice = None
 77 | 	return choice-1
 78 | 
 79 | 
 80 | def select_link_lengths():
 81 | 	# Clear terminal on win, linux
 82 | 	os.system('cls' if os.name == 'nt' else 'clear')
 83 | 	print("Robotic arm trajectory using Genetic Algorithm\n")
 84 | 	
 85 | 	links_num = None
 86 | 	link_lengths = None
 87 | 	
 88 | 	while links_num == None:
 89 | 		links_num = input("Number of links:  ")
 90 | 		if links_num == 'q':
 91 | 			return 'q'
 92 | 		elif links_num.isdigit():
 93 | 			links_num = int(links_num)
 94 | 			if links_num < 2 or links_num > 3:
 95 | 				print("Invalid input! Enter two or three")
 96 | 				links_num = None
 97 | 		else:
 98 | 			print("Invalid input!")
 99 | 			links_num = None
100 | 
101 | 	link_lengths = []
102 | 	for i in range(links_num):
103 | 		link_length = None
104 | 
105 | 		while link_length == None:
106 | 			link_length = input("Length of link {0}: ".format(i+1))
107 | 			if link_length == 'q':
108 | 				return 'q'
109 | 			elif link_length.isdigit():
110 | 				link_length = int(link_length)
111 | 				link_lengths.append(link_length)
112 | 			else:
113 | 				print("Invalid input!")
114 | 				link_length = None
115 | 
116 | 	return link_lengths
117 | 
118 | 
119 | 
120 | while True:
121 | 
122 | 	
123 | 	plotter = Plotter()
124 | 
125 | 	link_lengths = None
126 | 	start_cood = None
127 | 	end_cood = None
128 | 	obs_coods = None
129 | 
130 | 	ga_genr_2 = 300
131 | 	ga_genr_3 = 20
132 | 	ga_genr = 10
133 | 	ga_pop_sz = 40
134 | 	ga_mut_ratio = 0.05
135 | 	ga_xov_ratio = 0.30
136 | 	ga_mu_2 = [0.5,0.5]
137 | 	ga_mu_3 = [0.4,0.3,0.3]
138 | 	ga_mu = None
139 | 	ga_eps = 0.1
140 | 
141 | 	param_method = select_param_method()
142 | 
143 | 	if param_method == 'q':
144 | 		break
145 | 
146 | 	elif param_method == 1:
147 | 		param_idx = select_preset_param(preset_params)
148 | 		
149 | 		if param_idx == 'q':
150 | 			break
151 | 
152 | 		else:
153 | 			link_lengths = preset_params[param_idx].link_lengths
154 | 			start_cood = preset_params[param_idx].start_cood
155 | 			end_cood = preset_params[param_idx].end_cood
156 | 			obs_coods = preset_params[param_idx].obs_coods
157 | 
158 | 			plotter.link_lengths = link_lengths
159 | 			plotter.start_cood = start_cood
160 | 			plotter.end_cood = end_cood
161 | 			plotter.obs_coods = obs_coods
162 | 
163 | 			plotter.static_show()
164 | 
165 | 
166 | 	elif param_method == 2:
167 | 		link_lengths = select_link_lengths()
168 | 
169 | 		if link_lengths == 'q':
170 | 			break
171 | 
172 | 		plotter.link_lengths = link_lengths
173 | 		
174 | 		plotter.picker_show()
175 | 		
176 | 		start_cood = plotter.start_cood
177 | 		end_cood = plotter.end_cood
178 | 		obs_coods = plotter.obs_coods
179 | 
180 | 		if len(obs_coods) < 2:
181 | 			print("Select atleast two obstacles!")
182 | 			continue
183 | 
184 | 
185 | 	os.system('cls' if os.name == 'nt' else 'clear')
186 | 	print("Robotic arm trajectory using Genetic Algorithm\n")
187 | 	print("Running genetic algorithm... ")
188 | 
189 | 	ga_mu = ga_mu_2 if len(link_lengths) == 2 else ga_mu_3
190 | 	ga_genr = ga_genr_2 if len(link_lengths) == 2 else ga_genr_3
191 | 
192 | 	ga = GeneticAlgorithm(link_lengths, start_cood, end_cood, obs_coods, tg.fitness_population, ga_mu, ga_eps, ga_pop_sz, ga_mut_ratio, ga_xov_ratio, ga_genr)
193 | 	output_chr = ga.run()
194 | 	print("Done")
195 | 
196 | 	output_path = tg.chrome_traj(output_chr, start_cood, end_cood)
197 | 
198 | 	arm = Arm(link_lengths) if len(link_lengths) == 2 else Arm3Link(np.array(link_lengths))
199 | 	link_angles_series = np.degrees(arm.time_series(output_path))
200 | 
201 | 	# plt.plot(ga.fitness_stats)
202 | 	# plt.show()
203 | 	
204 | 	plotter.transition_show(link_angles_series)
205 | 
206 | 	usr_input = input("\nTry again? [y/n] ")
207 | 	if usr_input == 'y':
208 | 		continue
209 | 	else:
210 | 		break
211 | 


--------------------------------------------------------------------------------
/genetic_algorithm.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | np.random.seed(1)
  3 | 
  4 | 
  5 | 
  6 | class GeneticAlgorithm:
  7 |     
  8 |     def __init__(self, link_lengths, start_cood, end_cood, obs_coods, fitness, mu=[0.4,0.2], epsilon=0.1, population_size=120, mutation_percent=0.05, crossover_percent=0.30, generations=500):
  9 |         self.L1 = link_lengths[0]
 10 |         self.L2 = link_lengths[1]
 11 |         
 12 |         self.start_cood = start_cood
 13 |         self.end_cood = end_cood
 14 |         self.obs_coods = obs_coods
 15 |         self.fitness = fitness # fitenss function, takes population as input
 16 |         self.mu = mu
 17 |         self.epsilon = epsilon
 18 | 
 19 |         self.fitness_params = (link_lengths, start_cood, end_cood, obs_coods, epsilon, mu)
 20 | 
 21 |         self.L = 12
 22 |         self.x_min = 0                   
 23 |         self.x_max = pow(2,self.L)-1          
 24 |         self.y_min = 0                   
 25 |         self.y_max = pow(2,self.L)-1          
 26 |         
 27 |         self.R1 = sum(link_lengths)
 28 |         self.R2 = self.L1
 29 |         if self.R2 < 0: self.R2 = 0
 30 | 
 31 |         self.population_size = population_size
 32 |         self.mutation_percent = mutation_percent
 33 |         self.crossover_percent = crossover_percent
 34 |         self.generations = generations
 35 |         self.k = self.n_obstacles_interior() + 1
 36 | 
 37 |         self.fitness_stats = []
 38 | 
 39 | 
 40 |     def n_obstacles_interior(self):
 41 |         if len(self.obs_coods) == 0:
 42 |             return 0
 43 |         obs_coods = np.array(self.obs_coods)
 44 |         x_interior = obs_coods[:,0]
 45 |         y_interior = obs_coods[:,1]
 46 |         distance = np.sqrt(x_interior**2+y_interior**2) #distance of interior point from centre
 47 |         #taking only valid interior points (i.e. points between R1 and R2)
 48 |         
 49 |         distance = (distance<self.R1)
 50 |         return len(distance[distance==True])
 51 | 
 52 | 
 53 |     def chromosome_to_points(self, chromosome):
 54 |         return (chromosome)*(2*self.R1)/(2**self.L-1) - self.R1
 55 | 
 56 | 
 57 |     def chromosome_init(self):
 58 |         chromosome = np.zeros((self.population_size,2*self.k))
 59 |         # print(chromosome)
 60 | 
 61 |         centre_cood = [2**(self.L-1)-1, 2**(self.L-1)-1]
 62 |         distance_max = (self.x_max+1)/2
 63 |         distance_min = self.R2/self.R1*distance_max
 64 |         
 65 |         for i,chrom in enumerate(chromosome):
 66 |             chrom_valid = False
 67 |             
 68 |             while chrom_valid == False:
 69 |                 # random_chrom = np.random.randint(2**self.L,size=[self.k,2])
 70 |                 random_chrom_x = np.random.randint(2**self.L,size=[self.k])
 71 |                 random_chrom_y = np.random.randint(2**(self.L-1),size=[self.k]) + 2**(self.L-1)
 72 |                 random_chrom = np.column_stack((random_chrom_x,random_chrom_y))
 73 | 
 74 |                 distance = np.sqrt((random_chrom[:,0]-centre_cood[0])**2+(random_chrom[:,1]-centre_cood[1])**2)
 75 | 
 76 |                 if np.all(distance_min < distance) and np.all(distance_max > distance):
 77 |                     chrom_valid = True
 78 |                     chromosome[i] = np.ravel(random_chrom)
 79 | 
 80 |         return chromosome
 81 | 
 82 | 
 83 |     def fitness_mod(self,chromosome):
 84 |         fitness_row, _ = self.fitness(self.chromosome_to_points(chromosome), *self.fitness_params)
 85 |         for i,v in enumerate(fitness_row):
 86 |             if np.isnan(v):
 87 |                 fitness_row[i] = 0
 88 |             else:
 89 |                 fitness_row[i] = abs(v)
 90 |         return fitness_row
 91 | 
 92 | 
 93 |     def run(self):
 94 |         chromosome = self.chromosome_init()    #getting initial random chromosome
 95 |         # print(chromosome)
 96 | 
 97 |         # fitness_row = self.fitness(self.chromosome_to_points(chromosome), *self.fitness_params)    #return a matrix which has fitness of respective input chromosomes
 98 |         fitness_row = self.fitness_mod(chromosome)
 99 |         # fitness_row = np.random.rand(self.population_size)  #remove it later on
100 |         # print(fitness_row)
101 | 
102 |         # s = 0
103 |         # while(s<self.generations):
104 |         for genr in range(self.generations):
105 |             
106 |             print("*", end="", flush=True)
107 |             
108 |             roulette_wheel_cdf = np.cumsum(fitness_row/np.sum(fitness_row))    #cdf 
109 |             crossover_point = np.random.randint(self.k-1) if self.k != 1 else 0                      #random crossover point 
110 |             index = np.zeros((2))
111 |             new_chromosome = np.zeros((self.population_size,2*self.k))
112 | 
113 |             for i in range(int(self.population_size/2)):                  #crossover
114 | 
115 |                 a = np.random.rand(2)
116 |                 index = np.searchsorted(roulette_wheel_cdf, a)  
117 |                 parent = np.array([chromosome[index[0]], chromosome[index[1]]])
118 | 
119 |                 if np.random.rand() < self.crossover_percent:
120 |                     new_chromosome[2*i+0,0:2*crossover_point+1] = parent[0,0:2*crossover_point+1]
121 |                     new_chromosome[2*i+0,2*crossover_point+1:2*self.k] = parent[1,2*crossover_point+1:2*self.k]
122 |                     new_chromosome[2*i+1,0:2*crossover_point+1] = parent[1,0:2*crossover_point+1]
123 |                     new_chromosome[2*i+1,2*crossover_point+1:2*self.k] = parent[0,2*crossover_point+1:2*self.k]
124 |                 else:
125 |                     new_chromosome[2*i+0] = parent[0]
126 |                     new_chromosome[2*i+1] = parent[1]
127 | 
128 |             # print(new_chromosome)
129 | 
130 |             for i in range(self.population_size):                                 #mutation
131 |                 if (np.random.rand() < self.mutation_percent):
132 |                     p = np.random.randint(12*2*self.k)
133 |                     q = int(np.floor(p/12))
134 |                     p = int(p - 12*q)
135 | 
136 |                     binary = list(np.binary_repr(int(new_chromosome[i,q]),12))
137 |                     #flipping the pth binary place
138 |                     if (binary[p] == '0'):
139 |                         binary[p] = '1'
140 |                     elif (binary[p] == '1'):
141 |                         binary[p] = '0'
142 |                     binary_string = "".join(binary)            
143 |                     new_chromosome[i,q] = int(binary_string,2)
144 | 
145 |             chromosome = new_chromosome
146 |             # s = s+1               #incrementing generation
147 | 
148 |             # fitness_row = self.fitness(self.chromosome_to_points(chromosome), *self.fitness_params)    #return a matrix which has fitness of respective input chromosomes
149 |             fitness_row = self.fitness_mod(chromosome)
150 |             # fitness_row = np.random.rand(self.population_size)  #remove it later on
151 |             self.fitness_stats.append(max(fitness_row))
152 |             # print(fitness_row)
153 |         
154 |         print()
155 | 
156 |         # print(chromosome)
157 |         # fitness_row = self.fitness(chromosome, *self.fitness_params)
158 |         fitness_row = self.fitness_mod(chromosome)
159 |         max_idx = np.argmax(fitness_row)
160 |         return (self.chromosome_to_points(chromosome))[max_idx]
161 |     
162 | 


--------------------------------------------------------------------------------
/plotter.py:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/python3
  2 | 
  3 | 
  4 | 
  5 | import math
  6 | 
  7 | import numpy as np
  8 | import matplotlib.pyplot as plt
  9 | from matplotlib.widgets import Slider, Button, RadioButtons
 10 | 
 11 | 
 12 | 
 13 | class Plotter():
 14 | 	"""docstring for Plotter"""
 15 | 
 16 | 	"""
 17 | 	x_range: A tuple or a list (x_min, x_max) to set graph limits
 18 | 	y_range: Similar to x_range, for y axis
 19 | 	link_lengths: A list containing lengths of each link, starting
 20 | 		from the base.
 21 | 	link_angles_init: Initial link angles. Length of list must be
 22 | 		equal to link_lengths
 23 | 	start_cood: A tuple or list with initial end effector coordinates
 24 | 	end_cood: A tuple or list with desired end effector coordinates
 25 | 		The above two coordinates are for plotting points only. No
 26 | 		caculation of link angles takes place in this class
 27 | 	"""
 28 | 	def __init__(self):
 29 | 		self.link_lengths = None
 30 | 		self.link_angles = None
 31 | 		self.start_cood = None
 32 | 		self.end_cood = None
 33 | 		self.obs_coods = None
 34 | 
 35 | 
 36 | 	def plot_grids(self, ax):
 37 | 		# X and Y axis
 38 | 		ax.axhline(c="0.9")
 39 | 		ax.axvline(c="0.9")
 40 | 
 41 | 		# Two circles
 42 | 		circle1 = plt.Circle((0,0), self.link_lengths[0], color='0.9', fill=False)
 43 | 		circle2 = plt.Circle((0,0), sum(self.link_lengths), color='0.9', fill=False)
 44 | 		ax.add_artist(circle1)
 45 | 		ax.add_artist(circle2)
 46 | 
 47 | 
 48 | 	def plot_terminals(self, ax):
 49 | 		ax.plot(*self.start_cood, 'bo')
 50 | 		ax.plot(*self.end_cood, 'go')
 51 | 		
 52 | 		ax.annotate(r'$P_s$', xy=self.start_cood, xytext=(5,-10), textcoords='offset points')
 53 | 		ax.annotate(r'$P_t$', xy=self.end_cood, xytext=(5,-10), textcoords='offset points')
 54 | 
 55 | 
 56 | 	def plot_obstacles(self, ax):
 57 | 		for i,cood in enumerate(self.obs_coods):
 58 | 			ax.plot(*cood, 'r^')
 59 | 			ax.annotate(r'$P_{o'+str(i)+r'}$', xy=cood, xytext=(5,-10), textcoords='offset points')
 60 | 
 61 | 	def plot_start_point(self, ax):
 62 | 		point = ax.plot(*self.start_cood, 'bo')
 63 | 		label = ax.annotate(r'$P_s$', xy=self.start_cood, xytext=(5,-10), textcoords='offset points')
 64 | 		return point, label
 65 | 
 66 | 
 67 | 	def plot_end_point(self, ax):
 68 | 		point = ax.plot(*self.end_cood, 'go')
 69 | 		label = ax.annotate(r'$P_t$', xy=self.end_cood, xytext=(5,-10), textcoords='offset points')
 70 | 		return point, label
 71 | 
 72 | 
 73 | 	def plot_obs_point(self, ax, cood=None, label_idx=""):
 74 | 		if cood == None:
 75 | 			cood = self.obs_coods[-1]
 76 | 		point = ax.plot(*cood, 'r^')
 77 | 		label = ax.annotate(r'$P_{o'+str(label_idx)+r'}$', xy=cood, xytext=(5,-10), textcoords='offset points')
 78 | 		return point, label
 79 | 
 80 | 
 81 | 	def plot_links(self, ax, *args):
 82 | 		if args:
 83 | 			coods_x, coods_y = args[0]
 84 | 		else:
 85 | 			coods_x, coods_y = self.get_coods_from_link_angles()
 86 | 
 87 | 		[link_object] = ax.plot(coods_x, coods_y, '-mo')
 88 | 		return link_object
 89 | 
 90 | 
 91 | 	def plot_links_by_time(self, ax, coods_series, t):
 92 | 		steps = len(coods_series[0])
 93 | 		coods_x = coods_series[0][int(t*(steps-1))]
 94 | 		coods_y = coods_series[1][int(t*(steps-1))]
 95 | 
 96 | 		return self.plot_links(ax, [coods_x, coods_y])
 97 | 
 98 | 
 99 | 	def plot_update_links_by_time(self, link, coods_series, t):
100 | 		steps = len(coods_series[0])
101 | 		coods_x = coods_series[0][int(t*(steps-1))]
102 | 		coods_y = coods_series[1][int(t*(steps-1))]
103 | 
104 | 		link.set_xdata(coods_x)
105 | 		link.set_ydata(coods_y)
106 | 
107 | 
108 | 	def plot_set_lims(self, ax):
109 | 		ax.axis('scaled')
110 | 		axis_limit = 1.2*sum(self.link_lengths)
111 | 		ax.set_xlim(-axis_limit, axis_limit)
112 | 		ax.set_ylim(-axis_limit, axis_limit)
113 | 
114 | 
115 | 	def plot_end_path(self, ax, xs, ys):
116 | 		ax.plot(xs, ys, '--', c="y")
117 | 
118 | 
119 | 	def plot_joint_path(self, ax, xs, ys):
120 | 		ax.plot(xs, ys, '--', c="0.8")
121 | 
122 | 
123 | 	def static_plot(self, ax):
124 | 		self.plot_grids(ax)
125 | 		self.plot_terminals(ax)
126 | 		self.plot_obstacles(ax)
127 | 		self.plot_set_lims(ax)
128 | 		self.plot_set_lims(ax)
129 | 
130 | 
131 | 	def static_show(self):
132 | 		fig,ax = plt.subplots()
133 | 		self.static_plot(ax)
134 | 		plt.show()
135 | 
136 | 	def static_show(self):
137 | 		fig,ax = plt.subplots()
138 | 		self.static_plot(ax)
139 | 		plt.show()
140 | 
141 | 
142 | 	"""
143 | 	link_angles_series: A time series 2d numpy array. Each element
144 | 		is a link_angles aray, indexed by time
145 | 	"""
146 | 	def transition_plot_base(self, ax, coods_series):
147 | 		self.plot_grids(ax)
148 | 		self.plot_terminals(ax)
149 | 		self.plot_obstacles(ax)
150 | 		self.plot_set_lims(ax)
151 | 
152 | 		coods_x_series, coods_y_series = coods_series
153 | 		self.plot_end_path(ax, coods_x_series.T[-1], coods_y_series.T[-1])
154 | 		for xs, ys in zip(coods_x_series.T[:-1], coods_y_series.T[:-1]):
155 | 			self.plot_joint_path(ax, xs, ys)
156 | 
157 | 
158 | 	def transition_show(self, link_angles_series):		
159 | 		fig = plt.figure()
160 | 
161 | 		ax_main = fig.add_axes([0.1,0.2,0.8,0.7])
162 | 		ax_slider = fig.add_axes([0.1,0.03,0.8,0.03])
163 | 
164 | 		coods_series = self.get_coods_series_from_link_angles_series(link_angles_series)
165 | 
166 | 		self.transition_plot_base(ax_main, coods_series)
167 | 		link = self.plot_links_by_time(ax_main, coods_series, 0)
168 | 
169 | 
170 | 		slider = Slider(ax_slider, "Time", 0, 1, valinit=0)
171 | 		def on_slider_upd(val):
172 | 			self.plot_update_links_by_time(link, coods_series, val)
173 | 			fig.canvas.draw()
174 | 		slider.on_changed(on_slider_upd)
175 | 
176 | 		plt.show()
177 | 
178 | 
179 | 	def picker_plot_base(self, ax):
180 | 		self.plot_grids(ax)
181 | 		self.plot_set_lims(ax)
182 | 
183 | 
184 | 	def picker_show(self):
185 | 		fig,ax = plt.subplots()
186 | 		self.picker_plot_base(ax)
187 | 
188 | 
189 | 		# Remove any previous bindings to start afresh
190 | 		self.start_cood = None
191 | 		self.end_cood = None
192 | 		self.obs_coods = []
193 | 
194 | 		ax_title = ax.set_title("Pick start point")
195 | 
196 | 		# Store the artists for possible future use
197 | 		start_point = None
198 | 		end_point = None
199 | 		obs_points = []
200 | 
201 | 		start_label = None
202 | 		end_label = None
203 | 		obs_labels = []
204 | 
205 | 		def on_click(event):
206 | 
207 | 			# Ensure left mouse click, within axes bounds
208 | 			if event.button != 1 or event.xdata == None or event.ydata == None:
209 | 				return
210 | 
211 | 			nonlocal ax_title
212 | 
213 | 			nonlocal start_point
214 | 			nonlocal end_point
215 | 			nonlocal obs_points
216 | 
217 | 			nonlocal start_label
218 | 			nonlocal end_label
219 | 			nonlocal obs_labels
220 | 
221 | 			cood = [event.xdata, event.ydata]
222 | 
223 | 			if self.start_cood == None:
224 | 				self.start_cood = cood
225 | 				start_point, start_label = self.plot_start_point(ax)
226 | 				ax_title.set_text("Pick end point")
227 | 
228 | 			elif self.end_cood == None:
229 | 				self.end_cood = cood
230 | 				end_point, end_label = self.plot_end_point(ax)
231 | 				ax_title.set_text("Pick obstacle points")
232 | 
233 | 			else:
234 | 				self.obs_coods.append(cood)
235 | 				obs_point, obs_label = self.plot_obs_point(ax, label_idx=len(self.obs_coods)-1)
236 | 				obs_points.append(obs_point)
237 | 				obs_labels.append(obs_labels)
238 | 
239 | 			fig.canvas.draw()
240 | 
241 | 		cid = fig.canvas.mpl_connect("button_release_event", on_click)
242 | 		plt.show()
243 | 		fig.canvas.mpl_disconnect(cid)
244 | 
245 | 
246 | 
247 | 	def get_coods_from_link_angles(self, *args):
248 | 		coods_y = [0]
249 | 		coods_x = [0]
250 | 
251 | 		if args:
252 | 			link_angles = args[0]
253 | 		else:
254 | 			link_angles = self.link_angles
255 | 
256 | 		angle = 0
257 | 		for l,a in zip(self.link_lengths, link_angles):
258 | 			angle += a 
259 | 			cood_y = coods_y[-1] + l*math.sin(math.radians(angle))
260 | 			cood_x = coods_x[-1] + l*math.cos(math.radians(angle))
261 | 
262 | 			coods_y.append(cood_y)
263 | 			coods_x.append(cood_x)
264 | 
265 | 		return coods_x, coods_y
266 | 
267 | 
268 | 	def get_coods_series_from_link_angles_series(self, link_angles_series):
269 | 		coods_y_series = []
270 | 		coods_x_series = []
271 | 		
272 | 		for link_angles in link_angles_series:
273 | 			coods_x, coods_y = self.get_coods_from_link_angles(link_angles)
274 | 			coods_x_series.append(coods_x)
275 | 			coods_y_series.append(coods_y)
276 | 
277 | 		coods_x_series = np.array(coods_x_series)
278 | 		coods_y_series = np.array(coods_y_series)
279 | 		
280 | 		return coods_x_series, coods_y_series
281 | 


--------------------------------------------------------------------------------
/trajectory_generation.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import scipy.interpolate as sc
  3 | import matplotlib.pyplot as plt
  4 | import three_link
  5 | import invkin
  6 | import timeit
  7 | 
  8 | '''
  9 | generate_trajectories(sorted_population, start, end) : 
 10 |     population - matrix of (interlaced x and y coordinates of internal points) of each chromosome of the population. 
 11 |                  Each row contains 1 chromosome
 12 |     start - x and y coordinates of start point
 13 |     end - x and y coordinates of end point
 14 |     outputs - list of interpolated functions for each chromosome (list of PchipInterpolator objects)
 15 |             - points are also returned.
 16 | format(population):
 17 |     takes a population matrix andconverts into a 3D matrix for better use for generate_trajectories function.
 18 | 
 19 | check_point_validity(sorted_population, link1, link2)    
 20 | '''
 21 | 
 22 | 
 23 | def generate_trajectories(formatted_population, start, end, fitness_calculated):
 24 |     '''
 25 |     :param sorted_population: (P x K x 2) array of formatted population
 26 |     :param start: (x, y) cords of start point
 27 |     :param end: (x, y) coords of end point
 28 |     :param fitness_calculated: boolean list stating whether a chromosome's fitness has been calculated.
 29 |                                only those chromosome's values are calculated, whose points are not valid.
 30 |     :return: trajectory_points: a (P x (N+2) x 2) array of all internal and end points
 31 |              population_trajectories: list of all population trajectories. Invalid chromosomes have 'False' in their index
 32 |     '''
 33 |     # Every chromosome's points are seperated and arranged in form of x and y coordinates.
 34 |     # It is then arranged in the order of x coordinated. Start and End point coordinates are then added to the array.
 35 |     # then the trajectories are generated.
 36 |     shape = np.shape(formatted_population)
 37 |     left_end, right_end = start, start
 38 |     if start[0] < end[0]:
 39 |         right_end = end
 40 |     else:
 41 |         left_end = end
 42 | 
 43 |     population_trajectories = [False for g in range(shape[0])]
 44 |     trajectory_points = np.zeros([shape[0], shape[1] + 2, shape[2]])
 45 |     for i in range(shape[0]):
 46 |         if fitness_calculated[i]:
 47 |             continue
 48 |         ch_with_start = np.insert(formatted_population[i, :, :], 0, left_end, axis=0)
 49 |         chrome_all_pts = np.insert(ch_with_start, (shape[1] + 1), right_end, axis=0)
 50 |         population_trajectories[i] = sc.PchipInterpolator(chrome_all_pts[:, 0], chrome_all_pts[:, 1])
 51 |         trajectory_points[i, :, :] = chrome_all_pts
 52 |     return trajectory_points, population_trajectories
 53 | 
 54 | 
 55 | def chrome_traj(chrome, start, end):
 56 |     sorted_chrome = format(chrome)
 57 |     sh = np.shape(sorted_chrome)
 58 |     left_end, right_end = start, start
 59 |     if start[0] < end[0]:
 60 |         right_end = end
 61 |     else:
 62 |         left_end = end
 63 |     K = sh[1]
 64 |     ch_with_start = np.insert(sorted_chrome, 0, left_end, axis=1)
 65 |     chrome_all_pts = np.insert(ch_with_start, (K + 1), right_end, axis=1)
 66 |     ch_x, ch_y = chrome_all_pts[:, :, 0][0], chrome_all_pts[:, :, 1][0]
 67 |     trajectory = sc.PchipInterpolator(ch_x, ch_y)
 68 |     traj_points = path_points(trajectory, 0.1, start, end)
 69 |     return traj_points
 70 | 
 71 | 
 72 | def format(population) -> object:
 73 |     '''
 74 |     :param population: complete population in 2D matrix (P x 2k)
 75 |     :return: sorted_population: 3D array (P x k x 2)
 76 |     '''
 77 |     shape = np.shape(population)
 78 |     if len(shape) == 1:
 79 |         P = 1
 80 |         K = int(shape[0]/2)
 81 |     elif len(shape) == 2:
 82 |         P = shape[0]
 83 |         K = int(shape[1]/2)
 84 |     formatted_population = np.zeros([P, K, 2])
 85 |     for i in range(P):
 86 |         if P == 1:
 87 |             chrome = np.reshape(population, [K, 2])
 88 |         else:
 89 |             chrome = np.reshape(population[i, :], [K, 2])
 90 |         chrome_sorted = chrome[chrome[:, 0].argsort()].transpose()
 91 |         formatted_population[i, :, :] = chrome_sorted.transpose()
 92 |     return formatted_population
 93 | 
 94 | 
 95 | def check_point_validity(formatted_population, link_len, start, end) -> list:
 96 |     '''
 97 |     :param sorted_population: 3D array of sorted population matrix
 98 |     :param link1: length of link 1
 99 |     :param link2: length of link 2
100 |     :return: validity: list of indexed validity values. could be used for setting fitness to zero.
101 |     '''
102 |     shape = np.shape(formatted_population)
103 |     left_end, right_end = start, start
104 |     if start[0] < end[0]:
105 |         right_end = end
106 |     else:
107 |         left_end = end
108 |     validity = []
109 |     for i in range(shape[0]):
110 |         r = np.linalg.norm(formatted_population[i, :, :], axis=1)
111 |         if np.any(formatted_population[i, :, 0] < left_end[0]):
112 |             validity.append(False)
113 |         elif np.any(formatted_population[i, :, 0] > right_end[0]):
114 |             validity.append(False)
115 |         elif np.all(r > link_len[0]):
116 |             if np.all(r < (sum(link_len))):
117 |                 if np.all(formatted_population[i,:,1] > 0):
118 |                     validity.append(True)
119 |                 else:
120 |                     validity.append(False)
121 |             else:
122 |                 validity.append(False)
123 |         else:
124 |             validity.append(False)
125 | 
126 |     return validity
127 | 
128 | 
129 | def check_trajectory_validity(trajectory, obstacles):
130 |     '''
131 |     :param trajectory:
132 |     :param obstacles: (x, y) coordinates in the form of :   [x1, x2, ... xn]  (2 x N matrix)
133 |                                                             [y1, y2, ... yn]
134 |     :return: single boolean value of 'validity'
135 |     '''
136 |     obstacles = np.array(obstacles)
137 |     # print(trajectory(obstacles[:,0]), obstacles[:,1])
138 | 
139 |     if np.any(trajectory(obstacles[:,0]) > obstacles[:,1]):  # value of path at x is greater than y coord of point
140 |         validity = False
141 |     else:
142 |         validity = True
143 |     return validity
144 | 
145 | 
146 | def path_points(y, epsilon, start, end):
147 |     """
148 |     :param y: PchipInterpolator object for chromosome
149 |     :param epsilon: parameter for distance between points
150 |     :param start: (x, y) coordinates of start point
151 |     :param end: (x, y) coordinates of end point
152 |     epsilon usage: increasing it will improve resolution at the cost of more points to work on.
153 |                    decreasing it will improve computation time at the cost of resolution
154 | 
155 |     :return: (N x 2) array of (X, Y) coordinates of points, where N = no. of points
156 |     (N is variable to accomodate for equal disatnce between consecutive points)
157 |     the points are the path points as the arm travels from the start point to the end point.
158 |     """
159 |     # temporary lists to store x and y coordinates
160 |     pt_x = [start[0]]
161 |     pt_y = [start[1]]
162 |     der = y.derivative()
163 | 
164 |     # iterator point
165 |     x = start[0]
166 | 
167 |     if start[0] < end[0]:  # start point is on left side
168 |         while x < end[0]:
169 |             del_x = epsilon / np.sqrt(der(x) ** 2 + 1)
170 |             if (x + del_x) < end[0]:
171 |                 pt_x.append(x + del_x)
172 |                 pt_y.append(y(x + del_x))
173 |                 x += del_x
174 |             else:
175 |                 pt_x.append(end[0])
176 |                 pt_y.append(end[1])
177 |                 break
178 |     else:  # end point on left side
179 |         while x > end[0]:
180 |             del_x = epsilon / np.sqrt(der(x) ** 2 + 1)
181 |             if (x - del_x) > end[0]:
182 |                 pt_x.append(x - del_x)
183 |                 pt_y.append(y(x - del_x))
184 |                 x -= del_x
185 |             else:
186 |                 pt_x.append(end[0])
187 |                 pt_y.append(end[1])
188 |                 break
189 | 
190 |     points = np.zeros([2, len(pt_x)])
191 |     points[0, :] = np.array(pt_x)
192 |     points[1, :] = np.array(pt_y)
193 | 
194 |     return points.transpose()
195 | 
196 | 
197 | def fitness_population(population, link_len, start_pt, end_pt, obstacles, epsilon, mu, Single=False):
198 |     """
199 |     Envelope function for complete fitness calculation
200 |     Order of operations:
201 |     1. point checking       (set fitness to zero for invalid)
202 |     2. path interpolation
203 |     3. path discretization
204 |     4. reverse kinematics on path
205 |     5. Path checking        (check order here)
206 |     5. fitness calculation
207 |     """
208 |     if len(link_len) == 3:
209 |         arm1 = three_link.Arm3Link(link_len)
210 |     elif len(link_len) == 2:
211 |         arm1 = invkin.Arm(link_len)
212 | 
213 |     if Single == True:
214 |         pop_size = 1
215 |     else:
216 |         pop_size = np.shape(population)[0]
217 | 
218 |     cost_pop = [np.inf for i in range(pop_size)]  # stores fitness values
219 |     fitness_calculated = [False for i in range(pop_size)]  # stores fitness calculation validity
220 | 
221 |     formatted_pop = format(population)
222 |     pt_validity = check_point_validity(formatted_pop, link_len, start_pt, end_pt)
223 |     for i in range(pop_size):
224 |         if pt_validity[i] == False:
225 |             cost_pop[i] = np.inf
226 |             fitness_calculated[i] = True
227 | 
228 |     points, trajectories = generate_trajectories(formatted_pop, start_pt, end_pt, fitness_calculated)
229 |     #print(trajectories)
230 |     traj_points = None
231 |     for i in range(pop_size):
232 |         if fitness_calculated[i] == False:
233 |             traj_points = path_points(trajectories[i], epsilon, start_pt, end_pt)
234 |             # plt.plot(traj_points[:, 0], traj_points[:, 1])
235 |             # t = np.linspace(-4, 4, 100)
236 |             # plt.plot(t, np.sqrt(4 - t**2))
237 |             # plt.plot(t, np.sqrt(16 - t ** 2))
238 |             # plt.show()
239 |             theta = np.array(arm1.time_series(traj_points))
240 |             validity = check_trajectory_validity(trajectories[i], obstacles)
241 |             if validity == False:
242 |                 cost_pop[i] = np.inf
243 |             else:
244 |                 cost_pop[i] = fitness_chrome(theta, mu)
245 |             fitness_calculated[i] = True
246 | 
247 |     fitness_pop = 1/np.array(cost_pop)
248 |     #fitness_pop = np.array(cost_pop)
249 | 
250 |     return np.array(fitness_pop), traj_points
251 | 
252 | 
253 | def fitness_chrome(theta, mu):
254 |     # check for mu dependency on links
255 |     '''
256 |     :param theta: 2 x N matrix of link angles at discrete points
257 |     :param mu: fitness parameters' list. see initial note for setting mu
258 |     :return: fitness value of the chromosome
259 |     theta in format of
260 |     [ th11 th12 th13 th14 ... th1n]     link 1 angles
261 |     [ th21 th22 th23 th24 ... th2n]     link 2 angles
262 |     theta1 and theta 2 are at discrete points on the path.
263 |     internal variables:
264 |     div = no. of theta divisions, 1 dimension of theta matrix
265 |     '''
266 |     # check this while changing code for different input format
267 | 
268 |     theta = theta.T
269 |     div = np.shape(theta)[1]
270 |     theta_i = theta[:, 0:div - 2]
271 |     theta_j = theta[:, 1:div - 1]
272 |     del_theta = abs(theta_j - theta_i)
273 |     fitness = 0
274 |     for i in range(div - 2):
275 |         for j in range(len(mu)):
276 |             fitness += mu[j] * theta[j, i]
277 |     return fitness
278 | 
279 | 
280 | def testing_fitness():
281 |     test_mat = np.array([[1.1, 2.2, 1.5, 2, -1, 1.3],
282 |                          [-2, 1.5, 2, 2, 0, 0.75],
283 |                          [0.5, 0.5, 1, 0.7, -2, 0.5]])
284 |     start_pt = np.array([-3, 1])
285 |     end_pt = np.array([3, 0.5])
286 |     obst = np.array([2, 4])
287 |     link1 = 1
288 |     link2 = 3
289 |     sorted_mat = format(test_mat)
290 |     v = check_point_validity(sorted_mat, link1, link2)
291 |     points, trajectories = generate_trajectories(clean_population, start_pt, end_pt)
292 |     check_trajectory_validity(trajectories, obst)
293 |     t = np.linspace(-3, 3, 100)
294 |     for i in range(len(trajectories)):
295 |         ax = plt.plot(t, trajectories[i](t), lw=1)
296 |         ap = plt.plot(points[i, :, 0], points[i, :, 1], 'ro')
297 |     plt.show()
298 | 
299 | 
300 | def testing_fitness2():
301 |     #pop_x = np.random.rand(3, 3)*8-4*np.ones(3)
302 |     #pop_y = np.random.rand(3, 3)*4-4*np.ones(3)
303 |     #pop = np.append(pop_x, pop_y)
304 |     #print(pop)
305 |     #pop = np.array([[-2, 2, -1.8, 2, 2, 2], [-1.5, 2.5, -0.5, 3, 2.5, 1]])
306 |     pop = np.array([-1.5, 2.5, -0.5, 3, 2.5, 1])
307 |     link_len = [2,2]
308 |     start = [-4, 0]
309 |     end = [4, 0]
310 |     obst = [0, 5]
311 |     mu = [0.5]
312 |     mat = chrome_traj(pop, start, end)
313 |     fit, traj = fitness_population(pop, link_len, start, end, obst, .1, mu, Single=True)
314 |     #print(traj)
315 | 
316 | #testing_fitness2()
317 | 
318 | def test_time():
319 |     #print(timeit.timeit(testing_fitness2(),
320 |      #                   'import numpy as np import scipy.interpolate as sc import matplotlib.pyplot as plt', 10))
321 |     #a = np.ones([1, 4, 3])
322 |     #sh = a.shape
323 |     #print(len(sh), sh,  a)
324 |     testing_fitness2()
325 | 
326 | #test_time()


--------------------------------------------------------------------------------