The goal of this section is to solve the inverse kinematics problem, i.e. given a target in the environment, what set of joint angles will achieve that target, if any? This is a tough problem, and in general analytical solutions for arbitrary robot arms may not exist, which is why we need to search for them numerically.

Implement the method ik_grid_search, which does the following. It uses the input intervals to split up the interval [0, 2pi] into the specified number of endpoints, e.g. if intervals = 4, then it picks 0, 0.5pi, pi, and 1.5pi. It then tries every possible combination of those thetas for the joint angles and returns two values: The thetas which resulted in the closest distance to the target, as well as the distance from the target. It should exclude any combinations which are in collision with any walls. Note that this means the number of points searched will be intervals^num_links, e.g. a grid search of 4 for a robot with 3 links will result in 4^3 = 64 points being searched.Hint: You may be interested in the product generator from the itertools package. You can use product(grid_coordinates, repeat=n) to get the desired grid points (where grid_coordinates is something like [0, pi/2, pi, 3*pi/2]).

Implement the method ik_fmin_search, which searches for an IK solution by utilizing scipys fmin function, where the cost function we are trying to minimize is the distance from the target. It should initialize the estimate to thetas_guess, and when calling fmin, it should set maxfun to max_calls, to limit the number of function evaluations. It should return three values: the thetas which resulted in the closest distance to the target, the distance from the target, and the actual number of function iterations it took for convergence. Note: fmin cannot take constraints into account, so it may end up with an IK solution which collides with a wall. This is OK. Note: For the purposes of autograding, we will track how many times that RobotArm.get_links() gets called, which should match up with the count fmin returns. Because of this, you should only call RobotArm.get_ee_location once per fmin evaluation.

Analysis: For the 3-arm robot RobotArm(2,1,2) and a target of [-1.5, 1.5], for max_calls = 0, 5, 10, , run ik_fmin_search with an initial guess of [0, 0, 0] until the produced result converges and fmin no longer utilizes all of the allotted calls. Produce a scatter plot showing the distance from the target as the y-axis against the number of function calls on the x-axis. Paste the scatter plot below, as well as a plot from plot_robot_state verifying that the chosen IK solution actually reaches the target.

Code:

class RobotArm: def __init__(self, *arm_lengths, obstacles=None): """  Represents an N-link arm with the arm lengths given.  Example of initializing a 3-link robot with a single obstacle:   my_arm = RobotArm(0.58, 0.14, 0.43, obstacles=[VerticalWall(0.45)])   :param arm_lengths: Float values representing arm lengths of the robot.  :param obstacles:  """  self.arm_lengths = np.array(arm_lengths) if np.any(self.arm_lengths < 0): raise ValueError("Cannot have negative arm length!") self.obstacles = [] if obstacles is not None: self.obstacles = obstacles def __repr__(self): msg = '"""  Returns all of the link locations of the robot as Link objects.  :param thetas: A list or array of scalars matching the number of arms.  :return: A list of Link objects.  """   cum_theta = np.cumsum(thetas) results = np.zeros((self.arm_lengths.shape[0] + 1, 2)) results[1:, 0] = np.cumsum(self.arm_lengths * np.cos(cum_theta)) results[1:, 1] = np.cumsum(self.arm_lengths * np.sin(cum_theta)) links = [Link(start, end) for start, end in zip(results[:-1], results[1:])] return links def get_ee_location(self, thetas): """  Returns the location of the end effector as a length 2 Numpy array.  :param thetas: A list or array of scalars matching the number of arms.  :return: A length 2 Numpy array of the x,y coordinate.  """  return self.get_links(thetas)[-1].end def ik_grid_search(self, target, intervals):  ***Put Answer Here*** def ik_fmin_search(self, target, thetas_guess, max_calls=100): ***Put Answer Here*** def get_collision_score(self, thetas): """ This function checks to see how many times the robot arm hits the wall  Inputs:  thetas = list or array of scalars matching the number of arms   Outputs:  count = number of times arm hits the wall"""  # Initialize count and get links count = 0 interfere_links = self.get_links(thetas) # Check for each link to see if it is in the way of each obstacle if self.obstacles is not None: for numb in range(len(self.obstacles)): for link in range(len(interfere_links)): if interfere_links[link].check_wall_collision((self.obstacles[numb])) is True: count = count - 1 return count def ik_constrained_search(self, target, thetas_guess, max_iters=100): raise NotImplementedError def plot_robot_state(self, thetas, target=None, filename='robot_arm_state.png'): """ This function plots the robot arm and its environment  Input:  thetas = list or array of scalars matching the number of arms  target = target point in environment   Output:  plot of robot arm saved  """   # Plot if target is None if target is not None: plt.plot(target[0], target[1], 'gx') # Grab links link = self.get_links(thetas) # Plot for obstacles if self.obstacles is not None: for i in self.obstacles: plt.axvline(i.loc, color='b') for l in range(len(link)): x = [link[l].start[0], link[l].end[0]] y = [link[l].start[1], link[l].end[1]] plt.plot(link[l].end[0], link[l].end[1], 'ko') # Change link color for obstacle if link[l].check_wall_collision(i) is True: plt.plot(x, y, '--', color='r') else: plt.plot(x, y, color='k') # Set limits plt.ylim(-sum(self.arm_lengths), sum(self.arm_lengths)) plt.xlim(-sum(self.arm_lengths), sum(self.arm_lengths)) # Show and save plt.show() return plt.savefig(filename) class Link: def __init__(self, start, end): """  Represents a finite line segment in the XY plane, with start and ends given as 2-vectors  :param start: A length 2 Numpy array  :param end: A length 2 Numpy array  """  self.start = start self.end = end def __repr__(self): return ''.format(self.start[0], self.start[1], self.end[0], self.end[1]) def __str__(self): return self.__repr__() def check_wall_collision(self, wall): """ This function checks to see if the links hit the vertical wall  Inputs:  wall = wall x location   Outputs:  True if wall hits  False if it doesn't"""  if not isinstance(wall, VerticalWall): raise ValueError('Please input a valid Wall object to check for collision.') # Initialize values start_x = self.start[0] end_x = self.end[0] # Check to see if start > end. If so, swap values if start_x > end_x: start_x = self.end[0] end_x = self.start[0] # Check to see if wall hits if start_x <= wall.loc <= end_x: return True else: return False class VerticalWall: def __init__(self, loc): """  A VerticalWall represents a vertical line in space in the XY plane, of the form x = loc.  :param loc: A scalar value  """  self.loc = loc def __repr__(self): return ''.format(self.loc)