Simple Position

[1]:

%matplotlib notebook
from notebook_quick_setup import *

Beginning notebook setup...
        Added /home/jhewers/Documents/projects/jdrones/src to path
        Imported gymnasium version 0.27.1

pybullet build time: Feb  2 2023 13:13:41
/home/jhewers/.local/lib/python3.10/site-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=1.16.5 and <1.23.0 is required for this version of SciPy (detected version 1.23.5
  warnings.warn(f"A NumPy version >={np_minversion} and <{np_maxversion}"

        Imported jdrones version unknown
        Imported scipy==1.7.3, numpy==1.23.5, pandas==1.3.5
        Imported functools, collections and itertools
        Imported tqdm (standard and trange)
        Imported seaborn==0.11.2, matplotlib==3.5.1
End of notebook setup

[2]:

dt = 1/240

[3]:

initial_state = State()
initial_state.pos = (0,0,1)

[4]:

def run_sim(env, wps):
    observations = collections.deque()
    obs, info = env.reset(seed=1337)

    for setpoint in tqdm(wps):
        obs, reward, term, trunc, info = env.step(setpoint)

        observations.append(np.copy(obs))

        if trunc:
            print(f"{trunc=} {term=} {info=}")
            break

    return observations

[5]:

def plot_states(states):
    df = States(np.concatenate(states)).to_df(tag='Observations',dt=dt)
    jplot.plot_standard(df)

[6]:

wps = [(10,0,10),(10,10,10),(0,0,10), (5,5,1)]

Straight Line Trajectory

[7]:

first_order_env = gymnasium.make("FirstOrderPolyPositionDroneEnv-v0", dt=dt, initial_state=initial_state)

/home/jhewers/.local/lib/python3.10/site-packages/gymnasium/spaces/box.py:129: UserWarning: WARN: Box bound precision lowered by casting to float32
  gym.logger.warn(f"Box bound precision lowered by casting to {self.dtype}")

[8]:

first_order_obs = run_sim(first_order_env, wps)

[9]:

plot_states(first_order_obs)

Polynomial Trajectory

[10]:

fifth_order_env = gymnasium.make("FifthOrderPolyPositionDroneEnv-v0", dt=dt, initial_state=initial_state)

/home/jhewers/.local/lib/python3.10/site-packages/gymnasium/spaces/box.py:129: UserWarning: WARN: Box bound precision lowered by casting to float32
  gym.logger.warn(f"Box bound precision lowered by casting to {self.dtype}")

[11]:

fifth_order_obs = run_sim(fifth_order_env, wps)

[12]:

plot_states(fifth_order_obs)

Plot XYZ Trajectories

[13]:

from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")

first_order_df = States(np.concatenate(first_order_obs)).to_df(tag='LQR+5O-Poly', dt=dt)
fifth_order_df = States(np.concatenate(fifth_order_obs)).to_df(tag='LQR+1O-Poly', dt=dt)

jplot.plot_3d_path(fifth_order_df, ax, "LQR+50-Poly")
jplot.plot_3d_path(first_order_df, ax, "LQR+10-Poly")

ax.legend()

plt.show()