Now that the simulator and its dependencies have been installed, you can import the modules you'll need for this assignment:

$[4]$ import gymnasium

import numpy as np

import gym$\_$neu$\_$racing

from gymnasium import spaces

from gym$\_$neu$\_$racing.envs.wrappers import StateFeedbackWrapper

import matplotlib.pyplot as plt

from typing import Callable

import

matplotlib.cm as $cmx$

import matplotlib.colors as colors

from gym$\_$neu$\_$racing import motion$\_$models

import cvxpy as $cp$

$1$a$)$ MPPI to move toward a goal coordinate

In this problem, you'll implement a basic version of MPPI and show that it outputs a good rollout to move a robot toward a given goal position.

This part uses the Unicycle kinematic model that's built into the simulator. You should make sure that your get$\_$action method considers the

control limits $($otherwise it may command things that the robot can't execute$),$ which may require passing those as arguments to the $\_$

method.

You will probably need to experiment with different numbers of rollouts, cost functions, $$ values, numbers of iterations, etc. to get good

performance.

Keeping this code relatively organized and clean will help for later parts in the assignment, where you build on this implementation. For

example, you are encouraged to define helper methods in your MPPI class to help keep your code organized $($e$.$g$.,$ you may want a

score$\_$rollouts and$/$or plot$\_$rollouts method that get called inside the get$\_$next$\_$action method$).$

Deliverables:

Implement the MPPI class, in particular the get$\_$next$\_$action method, so that the chosen rollout drives the robot toward the goal

position.

Print the best control sequence your MPPI algorithm came up with $($the first row$/$element of this sequence should be the action that your

get$\_$action returns$)$

Include a plot that shows the rollouts, start position, goal position, and highlights the best rollout in that iteration, for at least a few

iterations. We expect that the later iterations will give much better rollouts than the first iteration. You should make your axes have the

same scale $($e$.$g$.,$ using plt$.$axis$(\text{'}$equal$\text{'}\text{'})).$

$[]$ # Create an instance of the mobile robot simulator we'll use this semester

env $=$ gymnasium.make$("$gym$\_$neu$\_$racing$/$NEUEmptyWorld$-$v$0")$

# Tell the simulator to directly provide the current state vector $($no sensors yet$)$

env $=$ StateFeedbackWrapper$($env$)$

$[2]$ class MPPI:

def $($

self,

motion$\_$model$=$motion$\_$models.Unicycle$(),$

:

$"""."$ Your implementation here $"""."$

self.motion$\_$model $=$ motion$\_$model

raise NotImplementedError

def get$\_$action$($self$,$ initial$\_$state: np$.$ndarray, goal$\_$pos: np$.$ndarray$)$:

$""$u$"$ Your implementation here $""".$

raise NotImplementedError

return action

You can use the following code to check whether your MPPI implementation is working. After tuning your algorithm, it should be able to come

up with a rollout that ends close to the goal $($within $0\mathrm{.}1m$ in $12$ distance is close enough$)$:

$[]$ # Initialize the environment $($and set random seed so any randomness is repeatable$)$

np$.$ random. seed $(0)$

obs, $-=$ env. reset $()$

# Set the starting state $,$ theta$)$ and goal position $(x,y).$

initial$\_$state $=np.$array$([0\mathrm{.}0,0\mathrm{.}0,0\mathrm{.}0])$

goal$\_$pos $=$ np$.$array $([0\mathrm{.}5,0\mathrm{.}5])$

# Instantiate your contoller class

controller $=$ MPPI $()$

# Run your control algorithm for $1$ step. We'll worry about running your

# algorithm in closed$-$loop in later parts of the assignment.

action $=$ controller.get$\_$action$($initial$\_$state, goal$\_$pos$)$