fixed singularity weighting in coordinate optimization for nonholonomic systems

Author: Ross Hatton

ProgramFiles/sys_calcsystem_fcns/weight_away_from_singularities.m

@@ -21,9 +21,13 @@
 		min_singular_distance_gridded = reshape(min_singular_distance,size(grid{1}));
 
 		% Set the weight to the distance from the singularity
-		weight = (min_singular_distance_gridded);%.^2;
+		%weight = (min_singular_distance_gridded).^2;
+         weight = abs(atan(min_singular_distance_gridded));
 
 		% Add in a fudge factor to avoid zero-values at singularities
 		weight = weight+0.01*max(weight(:));
+        
+       
+       
 
 end