unit5_pp/scripts/gradient_descent_solution.py
def gradient_descent(start_index, goal_index, width, height, potential_field_data, descent_viz):
  ''' 
  Performs gradient descent on an artificial potential field 
  with a given start, goal node and a total potential field
  '''
  
  # Iterations limit
  max_iterations = 1000
  # Iteration counter
  current_iteration = 0
  # Tolerance margin allowed around goal position (in grid cells)
  goal_tolerance = 4
  # Boolean flag indicating whether the goal was reached or not
  path_found = False
  # Index corresponding to the last grid cell added to the path
  current = start_index
  # A list to keep all path waypoints
  path = []

  rospy.loginfo('Gradient descent: Done with initialization')

  # Loop that iterates until the maximum allowed number of loops is met
  while (current_iteration < max_iterations):

    # Optional: Visualize gradient descent path in Rviz
    descent_viz.draw(current, potential_field_data[current])

    # Check if goal was reached using tolerance value
    if (goal_tolerance > euclidean_distance(current, goal_index, width)):
      path_found = True
      rospy.loginfo('Gradient descent: Goal reached')
      # Optional: Visualize gradient descent path in Rviz
      descent_viz.draw(goal_index, 0)
      break

    # Neighbor cell with the lowest potential force value
    neighbours_and_gradients = get_neighbours_and_gradients(current, width, height, potential_field_data)
    new_waypoint = min(neighbours_and_gradients)[1]

    # Add it to the path
    path.append(new_waypoint)
    # Set new waypoint as the new grid cell located at the tip of the path (for the next algorithm iteration)
    current = new_waypoint
    # Increase the number of iteration by one
    current_iteration += 1
  
  # Once the main loop exits
  if not path_found:
    rospy.logwarn('Gradient descent probably stuck at critical point!!')
    return
  else:
    return path
 
  Performs gradient descent on an artificial potential field 
  with a given start, goal node and a total potential field
  
Performs gradient descent on an artificial potential field
with a given start, goal node and a total potential field
Performs gradient descent on an artificial potential field with a given start goal node and a total potential field
def gradient_descent(start_index, goal_index, width, height, potential_field_data, descent_viz):
  max_iterations = 1000
  current_iteration = 0
  goal_tolerance = 4
  path_found = False
  current = start_index
  path = []
  rospy.loginfo('Gradient descent: Done with initialization')
  while (current_iteration < max_iterations):
    descent_viz.draw(current, potential_field_data[current])
    if (goal_tolerance > euclidean_distance(current, goal_index, width)):
      path_found = True
      rospy.loginfo('Gradient descent: Goal reached')
      descent_viz.draw(goal_index, 0)
      break
    neighbours_and_gradients = get_neighbours_and_gradients(current, width, height, potential_field_data)
    new_waypoint = min(neighbours_and_gradients)[1]
    path.append(new_waypoint)
    current = new_waypoint
    current_iteration += 1
  if not path_found:
    rospy.logwarn('Gradient descent probably stuck at critical point!!')
    return
  else:
    return path
def gradient_descent ( start_index , goal_index , width , height , potential_field_data , descent_viz ) : max_iterations = 1000 current_iteration = 0 goal_tolerance = 4 path_found = False current = start_index path = [ ] rospy . loginfo ( 'Gradient descent: Done with initialization' ) while ( current_iteration < max_iterations ) : descent_viz . draw ( current , potential_field_data [ current ] ) if ( goal_tolerance > euclidean_distance ( current , goal_index , width ) ) : path_found = True rospy . loginfo ( 'Gradient descent: Goal reached' ) descent_viz . draw ( goal_index , 0 ) break neighbours_and_gradients = get_neighbours_and_gradients ( current , width , height , potential_field_data ) new_waypoint = min ( neighbours_and_gradients ) [ 1 ] path . append ( new_waypoint ) current = new_waypoint current_iteration += 1 if not path_found : rospy . logwarn ( 'Gradient descent probably stuck at critical point!!' ) return else : return path
Performs gradient descent on an artificial potential field
with a given start, goal node and a total potential field
Performs gradient descent on an artificial potential field with a given start goal node and a total potential field
''' 
  Performs gradient descent on an artificial potential field 
  with a given start, goal node and a total potential field
  ''' # Iterations limit # Iteration counter # Tolerance margin allowed around goal position (in grid cells) # Boolean flag indicating whether the goal was reached or not # Index corresponding to the last grid cell added to the path # A list to keep all path waypoints # Loop that iterates until the maximum allowed number of loops is met # Optional: Visualize gradient descent path in Rviz # Check if goal was reached using tolerance value # Optional: Visualize gradient descent path in Rviz # Neighbor cell with the lowest potential force value # Add it to the path # Set new waypoint as the new grid cell located at the tip of the path (for the next algorithm iteration) # Increase the number of iteration by one # Once the main loop exits
start_index
goal_index
width
height
potential_field_data
descent_viz