working phase 2 with low collision rate

Author: reiniscimurs

robot_nav/sim2.py

@@ -286,7 +286,7 @@ def get_reward(goal, collision, action, closest_robots, distance):
             r_dist = 1.5 / distance
             cl_pen = 0
             for rob in closest_robots:
-                add = 2.5 - rob if rob < 2.5 else 0
+                add = (3 - rob)**2 if rob < 3 else 0
                 cl_pen += add
 
             return -0.5 * abs(action[1]) - cl_pen