deep_mpc_trainer_v2.py

# -*- coding: utf-8 -*-
"""deep_mpc_trainer_v2.ipynb

Automatically generated by Colaboratory.

Original file is located at
    https://colab.research.google.com/drive/1KV4UQLP0ZZQoMy-7MduhHvuHqkaxoj4s
"""

import numpy as np
import pandas as pd
import random

import tensorflow as tf
from tensorflow import keras
from keras.models import Model, load_model
from keras.layers import Dense, LSTM, Dropout, Input, Concatenate, Normalization, Conv2D, Flatten

FOLDER_INDEX = 8
PLAN_LENGTH = 50
DATASET_ADDRESS = f'../../bag/log_{FOLDER_INDEX}'

cmdVel = pd.read_csv(
    f'{DATASET_ADDRESS}/cmd_vel_log.csv', delimiter=',').values
heading = pd.read_csv(f'{DATASET_ADDRESS}/heading.csv', delimiter=',').values
costmap = pd.read_csv(f'{DATASET_ADDRESS}/costmap.csv',
                      delimiter=',').values.reshape((-1, 20, 20))
planX = pd.read_csv(f'{DATASET_ADDRESS}/global_plan_x_log.csv',
                    delimiter=',', names=list(range(110)), header=None).values
planY = pd.read_csv(f'{DATASET_ADDRESS}/global_plan_y_log.csv',
                    delimiter=',', names=list(range(110)), header=None).values

print("================================")
print("current data size: \n")
print(cmdVel.shape, planX.shape, planY.shape, heading.shape, costmap.shape)
len = np.amin([cmdVel.shape[0], planX.shape[0], planY.shape[0],
              heading.shape[0], costmap.shape[0]])
cmdVel = cmdVel[:len]
heading = heading[:len]
planX = planX[:len]
planY = planY[:len]
costmap = costmap[:len]
print("reduced to: \n")
print(cmdVel.shape, planX.shape, planY.shape, heading.shape, costmap.shape)

planX = planX[:, :PLAN_LENGTH]
planY = planY[:, :PLAN_LENGTH]


def emptyRows(arr):
    delList = []
    for r, row in enumerate(arr):
        if np.any(np.isnan(row)):
            delList += [r]
    return delList


delList1 = emptyRows(planX)
delList2 = emptyRows(planY)
assert delList1 == delList2

planX = np.delete(planX, delList1, axis=0)
planY = np.delete(planY, delList1, axis=0)
cmdVel = np.delete(cmdVel, delList1, axis=0)
heading = np.delete(heading, delList1, axis=0)
costmap = np.delete(costmap, delList1, axis=0)

print("================================")
print(f'deleting NaNs; size reduced to:\n {planX.shape[0]}')

assert not np.any(np.isnan(planX))
assert not np.any(np.isnan(planY))
assert not np.any(np.isnan(cmdVel))
assert not np.any(np.isnan(heading))
assert not np.any(np.isnan(costmap))

print("================================")
print("data range:\n")
print(planX.min().round(5), planX.max().round(5))
print(planY.min().round(5), planY.max().round(5))
print(costmap.min().round(5), costmap.max().round(5))
for i in range(cmdVel.shape[1]):
    print(cmdVel[:, i].min().round(5), cmdVel[:, i].max().round(5))
for i in range(heading.shape[1]):
    print(heading[:, i].min().round(5), heading[:, i].max().round(5))

plan = np.stack([planX, planY], axis=2)

tf.convert_to_tensor(heading)
tf.convert_to_tensor(cmdVel)
tf.convert_to_tensor(plan)
tf.convert_to_tensor(costmap)

print("================================")
print(f"input shape:\n {plan.shape, heading.shape, costmap.shape}\noutput shape:\n {cmdVel.shape}")

validationLength = int(plan.shape[0]*0.05)
print(f'validation length: {validationLength}\n')

planNormalizer = Normalization()
headingNormalizer = Normalization()
costmapNormalizer = Normalization()

planNormalizer.adapt(plan)
headingNormalizer.adapt(heading)
costmapNormalizer.adapt(costmap)

planInput = Input((None, 2))
headingInput = Input((2,))
costmapInput = Input((20, 20, 1))

planNormalized = planNormalizer(planInput)
headingNormalized = headingNormalizer(headingInput)
costmapNormalized = costmapNormalizer(costmapInput)

planLatent = LSTM(32, input_shape=(None, 2), activation='tanh')(planNormalized)
planLatent = Dense(16, activation='tanh')(planLatent)

costmapLatent = Conv2D(16, (3, 3), activation="relu",
                       padding="same", strides=(2, 2))(costmapNormalized)
costmapLatent = Dropout(0.1)(costmapLatent)
costmapLatent = Flatten()(costmapLatent)
costmapLatent = Dense(8, activation="relu")(costmapLatent)

concatLayer = Concatenate()([planLatent, headingNormalized, costmapLatent])

mergedLayer = Dense(16, activation='tanh')(concatLayer)
mergedLayer = Dropout(0.1)(mergedLayer)
mergedLayer = Dense(8, activation='tanh')(mergedLayer)
output = Dense(2, activation='tanh')(mergedLayer)

model = Model(inputs=[planInput, headingInput, costmapInput], outputs=output)

model.compile(loss='mse',
              optimizer=tf.keras.optimizers.SGD(
                  learning_rate=1e-2, decay=1e-6, momentum=0.9, nesterov=True),
              metrics=['accuracy'])
print("================================")
print("================================")
print("================================")

model.summary()
tf.keras.utils.plot_model(model, to_file='model.png', show_shapes=True)

model_checkpoint_callback = tf.keras.callbacks.ModelCheckpoint(
    filepath=f'{DATASET_ADDRESS}/checkpoint',
    save_weights_only=False,
    monitor='val_loss',
    mode='min',
    save_best_only=True)
print("================================")
print("================================")
print("================================")
model.fit(
    x=[plan[:-validationLength],
      heading[:-validationLength],
      costmap[:-validationLength]],
    y=cmdVel[:-validationLength],
    epochs=3,
    batch_size=256,
    validation_split=0.2,
    callbacks=[model_checkpoint_callback]
)
print("================================")
print("================================")
print("================================")
q = model.evaluate(x=[plan[-validationLength:], heading[-validationLength:], costmap[-validationLength:]],
                   y=cmdVel[-validationLength:],
                   batch_size=128)
q = int(np.array(q).round(2)[1]*100)
print("================================")
print(f"evaluation accurecy: {q}%")

address = f"{DATASET_ADDRESS}/deepMPCModel{q}"


model.save(filepath=address,
           overwrite=True, include_optimizer=True)
print("================================\n")
print(f'model saved to: {DATASET_ADDRESS}')

model = load_model(address)

i = random.randint(0, plan.shape[0]-1)
vel = model.predict([plan[None, i], heading[None, i], costmap[None, i]])
print(f"sample predicted output: {vel}")