training_initialize

README.md

@@ -1,42 +1,54 @@
-# DPtraj
-A double-polynomial discription for trajectory interfaced with learning-based front end.
+# Stable-Time Path Planning
 
-This work is presented in the paper: Hierarchically Depicting Vehicle Trajectory with Stability in Complex Environments, published in Science Robotics.
+## 1. Installation
 
-The backend trajectory optimizer improvements build upon our previous work (available at https://github.com/ZJU-FAST-Lab/Dftpav), where singularity issues were addressed. 
-
-Moreover, the approach has recently been extended and applied to more complex multi-joint robotic platforms (see https://github.com/Tracailer/Tracailer).
-
-
-If you find this repository helpful, please consider citing at least one of the following papers:
-
-```bibtex
-@article{han2025hierarchically,
-  title={Hierarchically depicting vehicle trajectory with stability in complex environments},
-  author={Han, Zhichao and Tian, Mengze and Gongye, Zaitian and Xue, Donglai and Xing, Jiaxi and Wang, Qianhao and Gao, Yuman and Wang, Jingping and Xu, Chao and Gao, Fei},
-  journal={Science Robotics},
-  volume={10},
-  number={103},
-  pages={eads4551},
-  year={2025},
-  publisher={American Association for the Advancement of Science}
-}
-@article{han2023efficient,
-  title={An efficient spatial-temporal trajectory planner for autonomous vehicles in unstructured environments},
-  author={Han, Zhichao and Wu, Yuwei and Li, Tong and Zhang, Lu and Pei, Liuao and Xu, Long and Li, Chengyang and Ma, Changjia and Xu, Chao and Shen, Shaojie and others},
-  journal={IEEE Transactions on Intelligent Transportation Systems},
-  volume={25},
-  number={2},
-  pages={1797--1814},
-  year={2023},
-  publisher={IEEE}
-}
+To reproduce our ablation results, please install [Conda](https://docs.conda.io/projects/conda/en/stable/user-guide/install/linux.html#) environment on a Linux machine with Nvidia GPU.<br>
+You may need to install the following apt packages:
+```bash
+sudo apt-get install libboost-dev
+```
+Please config the conda environment:
+```bash
+cd ~/DPtraj/deepPathPlan
+conda create -n <your_env_name> python=3.8
+conda activate <your_env_name>
+pip install -r requirements.txt
 ```
 
-The code will be divided into several modules and gradually open-sourced in different branches. You can check out the following branches to try them out:
 
-*   **`backend`**: 
-    - Efficient singularity-free backend optimization.
+## 2. Reproducing the Model
+### 2.1 Download Training Data
+Please download, unzip and place the [Data](https://drive.google.com/file/d/1uuQsWTBYMzHI0RcXgpFg6Ft-3lf6-fRD/view?usp=drive_link) as the directory `~/totalData`.
 
-*   **`frontend_deploy`**:
-    - Reproducing of learning-enhanced stable path planning.
+### 2.2 Training
+You can easily retrain the model by running `PathNet/train_ours.py`:
+
+```bash
+cd ~/DPtraj/deepPathPlan
+python PathNet/train_ours.py
+```
+
+### 2.3 Visualizing the Results
+Once the model converges, you can visualize it:
+
+```bash
+cd ~/DPtraj/deepPathPlan
+python PathNet/visualizer_tojit.py
+```
+
+> **Note:** This script will utilize `torch.jit.trace` to generate a model file that can be directly invoked by LibTorch, allowing you to seamlessly integrate it into our ROS program.
+
+## 3. Checkout our experiment logs
+To check similar results in table Table.S1 and Fig.S2 of Supplementary Materials, we provide:<br>
+1. Detailed eval log [`model.pkl.txt`](deepPathPlan/models/model.pkl.txt).
+2. Detailed training log [`model.pklStep.txt`](deepPathPlan/models/model.pklStep.txt).
+3. Reproduced model [`model.pkl`](deepPathPlan/models/model.pkl).
+
+> **Note:**  
+> Please note that slight differences in the results compared to the paper are normal, due to variations in training configuration and package versions.<br>
+> For example, the batch size (`bz`) is set to 32 in this repo for easier reproduction on GPUs with 16 GB memory (the bz used in the paper is 64 based on 32 GB memory).  
+
+
+## 4. Contact
+
+If you have any questions, please feel free to contact Zhichao HAN (<zhichaohan@zju.edu.cn>) or Mengze TIAN(<mengze.tian@epfl.ch>).

deepPathPlan/PathNet/Layers.py

@@ -0,0 +1,80 @@
+'''Define the Layers
+Derived from - https://github.com/jadore801120/attention-is-all-you-need-pytorch/blob/132907dd272e2cc92e3c10e6c4e783a87ff8893d/transformer/Layers.py
+'''
+
+import torch.nn as nn
+import torch
+import torch.utils.checkpoint
+from SubLayers import MultiHeadAttention, PositionwiseFeedForward
+
+class EncoderLayer(nn.Module):
+    ''' Single Encoder layer, that consists of a MHA layers and positiion-wise
+    feedforward layer.
+    '''
+
+    def __init__(self, d_model, d_inner, n_head, d_k, d_v):
+        '''
+        Initialize the module.
+        :param d_model: Dimension of input/output of this layer
+        :param d_inner: Dimension of the hidden layer of hte position-wise feedforward layer
+        :param n_head: Number of self-attention modules
+        :param d_k: Dimension of each Key
+        :param d_v: Dimension of each Value
+        :param dropout: Argument to the dropout layer.
+        '''
+        super(EncoderLayer, self).__init__()
+        self.slf_attn = MultiHeadAttention(n_head, d_model, d_k, d_v)
+        self.pos_ffn = PositionwiseFeedForward(d_model, d_inner)
+
+    def forward(self, enc_input, slf_attn_mask=None):
+        '''
+        The forward module:
+        :param enc_input: The input to the encoder.
+        :param slf_attn_mask: TODO ......
+        '''
+        # # Without gradient Checking
+        # enc_output = self.slf_attn(
+        #     enc_input, enc_input, enc_input, mask=slf_attn_mask)
+
+        # With Gradient Checking
+        # enc_output = torch.utils.checkpoint.checkpoint(self.slf_attn, 
+        # enc_input, enc_input, enc_input, slf_attn_mask)
+        enc_output = self.slf_attn(enc_input, enc_input, enc_input, slf_attn_mask)
+
+        # enc_output, enc_slf_attn = self.slf_attn(
+        #     enc_input, enc_input, enc_input, mask=slf_attn_mask)
+
+        enc_output = self.pos_ffn(enc_output)
+        return enc_output
+
+
+class DecoderLayer(nn.Module):
+    ''' Compose with three layers '''
+
+    def __init__(self, d_model, d_inner, n_head, d_k, d_v, dropout=0.1):
+        '''
+        Initialize the Layer
+        :param d_model: Dimension of input/output this layer.
+        :param d_inner: Dimension of hidden layer of the position wise FFN
+        :param n_head: Number of self-attention modules.
+        :param d_k: Dimension of each Key.
+        :param d_v: Dimension of each Value.
+        :param dropout: Argument to the dropout layer.
+        '''
+        super(DecoderLayer, self).__init__()
+        # self.slf_attn = MultiHeadAttention(n_head, d_model, d_k, d_v, dropout=dropout)
+        self.enc_attn = MultiHeadAttention(n_head, d_model, d_k, d_v, dropout=dropout)
+        self.pos_ffn = PositionwiseFeedForward(d_model, d_inner, dropout=dropout)
+
+    def forward(self, dec_input, enc_output, slf_attn_mask=None, dec_enc_attn_mask=None):
+        '''
+        Callback function
+        :param dec_input:
+        :param enc_output:
+        :param slf_attn_mask:
+        :param dec_enc_attn_mask:
+        '''
+        # dec_output, dec_slf_attn = self.slf_attn(dec_input, dec_input, dec_input, mask=slf_attn_mask)
+        dec_output, dec_enc_attn = self.enc_attn(dec_input, enc_output, enc_output, mask=dec_enc_attn_mask)
+        dec_output = self.pos_ffn(dec_output)
+        return dec_output, dec_enc_attn

deepPathPlan/PathNet/SubLayers.py

@@ -0,0 +1,140 @@
+''' Define the sublayers in encoder/decoder layer 
+Derived From : https://github.com/jadore801120/attention-is-all-you-need-pytorch/blob/master/transformer/SubLayers.py
+'''
+
+import numpy as np
+import torch
+import torch.nn as nn
+import torch.nn.functional as F
+
+class ScaledDotProductAttention(nn.Module):
+    '''Scaled Dot-Product Attention 
+    '''
+
+    def __init__(self, temperature):
+        '''
+        Initialize the model.
+        :param temperature: TODO ....
+        :param attn_dropout: Argument to dropout after softmax(QK)
+        '''
+        super().__init__()
+        self.temperature = temperature
+
+    def forward(self, q, k, v, mask=None):
+        '''
+        Callback Function:
+        :param q: The Query matrix.
+        :param k: The Key matrix.
+        :param v: The value matrix.
+        :param mask: The mask of the input.
+        :returns (output, attention): A tuple consisting of softmax(QK^T)V and softmax(QK^T)
+        '''
+        attn = torch.matmul(q / self.temperature, k.transpose(2, 3))
+
+        if mask is not None:
+            attn = attn.masked_fill(mask == 0, -1e9)
+
+        attn = F.softmax(attn, dim=-1)
+        output = torch.matmul(attn, v)
+
+        return output
+
+        
+class MultiHeadAttention(nn.Module):
+    ''' Multi-Head Attention module '''
+
+    def __init__(self, n_head, d_model, d_k, d_v):
+        '''
+        Intialize the model.
+        :param n_head: Number of self-attention modules
+        :param d_model: Dimension of input/output of this layer
+        :param d_k: Dimension of each Key
+        :param d_v: Dimension of each Value
+        :param dropout: 
+        '''
+        super().__init__()
+
+        self.n_head = n_head
+        self.d_k = d_k
+        self.d_v = d_v
+
+        self.w_qs = nn.Linear(d_model, n_head * d_k, bias=False)
+        self.w_ks = nn.Linear(d_model, n_head * d_k, bias=False)
+        self.w_vs = nn.Linear(d_model, n_head * d_v, bias=False)
+        self.fc = nn.Linear(n_head * d_v, d_model, bias=False)
+
+        self.attention = ScaledDotProductAttention(temperature=d_k ** 0.5)
+
+        self.layer_norm = nn.LayerNorm(d_model, eps=1e-6)
+
+
+    def forward(self, q, k, v, mask=None):
+        '''
+        Callback function.
+        :param q: The Query matrix.
+        :param k: The Key matrix.
+        :param v: The value matrix.
+        :param mask: The mask to use.
+        :returns (output, attention): A tuple consisting of network output and softmax(QK^T)
+        '''
+        d_k, d_v, n_head = self.d_k, self.d_v, self.n_head
+        sz_b_q = q.size(0)
+        sz_b, len_q, len_k, len_v = q.size(0), q.size(1), k.size(1), v.size(1)
+
+        residual = q
+
+        # Pass through the pre-attention projection: b x lq x (n*dv)
+        # Separate different heads: b x lq x n x dv
+        q = self.w_qs(q).view(sz_b_q, len_q, n_head, d_k)
+        k = self.w_ks(k).view(sz_b, len_k, n_head, d_k)
+        v = self.w_vs(v).view(sz_b, len_v, n_head, d_v)
+
+        # Transpose for attention dot product: b x n x lq x dv
+        q, k, v = q.transpose(1, 2), k.transpose(1, 2), v.transpose(1, 2)
+
+        if mask is not None:
+            mask = mask.unsqueeze(1)   # For head axis broadcasting.
+
+        q = self.attention(q, k, v, mask=mask)
+
+        # Transpose to move the head dimension back: b x lq x n x dv
+        # Combine the last two dimensions to concatenate all the heads together: b x lq x (n*dv)
+        q = q.transpose(1, 2).contiguous().view(sz_b, len_q, -1)
+        q = self.fc(q)
+        q += residual
+
+        q = self.layer_norm(q)
+
+        return q
+
+
+class PositionwiseFeedForward(nn.Module):
+    ''' A simple 2 layer with fully connected layer.
+    '''
+
+    def __init__(self, d_in, d_hid):
+        '''
+        Initialize the model.
+        :param d_in: Dimension of the input/output of the model.
+        :param d_hid: Dimension of the hidden layer.
+        :param dropout: Argument to the dropout layer.
+        '''
+        super().__init__()
+        self.w_1 = nn.Linear(d_in, d_hid) # position-wise
+        self.w_2 = nn.Linear(d_hid, d_in) # position-wise
+        self.layer_norm = nn.LayerNorm(d_in, eps=1e-6)
+
+    def forward(self, x):
+        '''
+        Callback function.
+        :param x: The input to the function.
+        :returns torch.array: An output of the same dimension as the input.
+        '''
+        residual = x
+
+        x = self.w_2(F.relu(self.w_1(x)))
+        x += residual
+
+        x = self.layer_norm(x)
+
+        return x

deepPathPlan/PathNet/course.py

@@ -0,0 +1,53 @@
+import torch
+def read_wei_course(epoch):
+    if(epoch >=0):
+        wei_anchors = torch.tensor(1.0)
+        wei_arc = torch.tensor(0.0)
+        wei_uni = torch.tensor(0.0)
+        wei_pos = torch.tensor(5.0)
+        wei_hol = torch.tensor(0.0)
+        wei_rot = torch.tensor(5.0)
+        wei_cur = torch.tensor(0.0)
+        wei_safety = torch.tensor(0.0)
+        wei_rsm = torch.tensor(0.0)
+    if(epoch >=1):
+        wei_anchors = torch.tensor(1.0)
+        wei_arc = torch.tensor(0.05)
+        wei_uni = torch.tensor(10.0)
+        wei_pos = torch.tensor(5.0)
+        wei_hol = torch.tensor(10.0)
+        wei_rot = torch.tensor(5.0)
+        wei_cur = torch.tensor(0.0)
+        wei_safety = torch.tensor(0.0)
+        wei_rsm = torch.tensor(0.0)
+    if(epoch >=2):
+        wei_anchors = torch.tensor(1.0)
+        wei_arc = torch.tensor(0.5)
+        wei_uni = torch.tensor(100.0)
+        wei_pos = torch.tensor(5.0)
+        wei_hol = torch.tensor(100.0)
+        wei_rot = torch.tensor(5.0)
+        wei_cur = torch.tensor(5.0)
+        wei_safety = torch.tensor(0.0)
+        wei_rsm = torch.tensor(0.5)
+    if(epoch >=3):
+        wei_anchors = torch.tensor(1.0)
+        wei_arc = torch.tensor(0.5)
+        wei_uni = torch.tensor(100.0)
+        wei_pos = torch.tensor(5.0)
+        wei_hol = torch.tensor(200.0)
+        wei_rot = torch.tensor(5.0)
+        wei_cur = torch.tensor(500.0)
+        wei_safety = torch.tensor(0.0)
+        wei_rsm = torch.tensor(0.25)
+    if(epoch >=4):
+        wei_anchors = torch.tensor(1.0)
+        wei_arc = torch.tensor(0.5)
+        wei_uni = torch.tensor(100.0)
+        wei_pos = torch.tensor(5.0)
+        wei_hol = torch.tensor(500.0)
+        wei_rot = torch.tensor(5.0)
+        wei_cur = torch.tensor(500.0)
+        wei_safety = torch.tensor(500.0)
+        wei_rsm = torch.tensor(0.5)
+    return wei_anchors, wei_arc, wei_uni, wei_pos, wei_hol, wei_rot, wei_cur, wei_safety, wei_rsm

deepPathPlan/PathNet/data_loader.py

@@ -0,0 +1,144 @@
+import torch
+import numpy as np
+import math
+from torch.utils.data import Dataset
+import cv2
+import os
+
+class  pDataset(Dataset):
+    def __init__(self):
+        self.indexDict = []
+        self.startidx = 1
+        self.endidx = 29000
+        self.pstart = 1
+        self.pend = 51
+        self.envlist = []
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        self.dpath = os.path.dirname(os.path.dirname(current_dir)) +"/totalData/"
+        self.max_count = 500000
+    
+        self.envlist = ['forest1', 'office1', 'ruins1']
+        for env in self.envlist:
+            count = 0 
+            for eidx in range(self.startidx,self.endidx):
+                if(count > self.max_count):
+                    break
+                for pathidx in range(self.pstart,self.pend):
+                    self.path = self.dpath + env
+                    if(os.path.exists(self.path+'/e'+str(eidx)+'/path'+str(pathidx)+'.dat')):
+                        self.indexDict.append((eidx, pathidx, self.path))
+                        count += 1
+                        
+        self.envlist = ['forest2', 'office2', 'ruins2']
+        self.max_count = int(self.max_count/5)
+        for env in self.envlist:
+            count = 0 
+            for eidx in range(self.startidx,self.endidx):
+                if(count > self.max_count):
+                    break
+                for pathidx in range(self.pstart,self.pend):
+                    self.path = self.dpath + env
+                    if(os.path.exists(self.path+'/e'+str(eidx)+'/path'+str(pathidx)+'.dat')):
+                        self.indexDict.append((eidx, pathidx, self.path))
+                        count += 1
+            
+        self.length_ = len(self.indexDict)
+        
+    def __len__(self):
+        return self.length_
+
+    def __getitem__(self, idx):
+        (eidx, pathidx, datapath) = self.indexDict[idx]
+        fs = cv2.FileStorage(datapath+'/esdfmaps/'+str(eidx)+'.xml', cv2.FILE_STORAGE_READ)
+        path = np.fromfile(datapath+'/e'+str(eidx)+'/path'+str(pathidx)+'.dat')
+        
+        fn = fs.getNode("instance")
+        image = fn.mat()
+        raw_env = image #H*W
+        raw_env = np.expand_dims(raw_env, 0) #H*W
+        raw_env = np.where(raw_env > 0.2, 1, 0)
+
+        label = path[10:].reshape(200, 6)
+        env = np.expand_dims(image, 0) #H*W
+        
+        startpos = geom2pix(label[0][1:3])
+        goalpos = geom2pix(label[-1][1:3])
+        
+        data = get_encoder_input(env, goalpos, label[-1][3], startpos, label[0][3])
+        opState = label[:, 1:3]
+        labelRot = np.zeros((200,2))
+        labelRot[:,0] = np.cos(label[:, 3])
+        labelRot[:,1] = np.sin(label[:, 3])
+
+        grid = np.floor((opState+10.0)/1.0).astype(int)
+        anchors = np.zeros((200,20,20))
+        index = [i for i in range(200)]
+        anchors[index, grid[:,0], grid[:,1]] = 1
+        return  torch.as_tensor(data.copy()).float().contiguous(),torch.as_tensor(raw_env.copy()).float().contiguous(),\
+             torch.as_tensor(opState.copy()).float().contiguous(), torch.as_tensor(labelRot.copy()).float().contiguous(),\
+             torch.as_tensor(anchors.copy()).float().contiguous()
+    
+def geom2pix(pos, res=0.1, size=(200, 200)):
+    """
+    Convert geometrical position to pixel co-ordinates. The origin 
+    is assumed to be at [image_size[0]-1, 0].
+    :param pos: The (x,y) geometric co-ordinates.
+    :param res: The distance represented by each pixel.
+    :param size: The size of the map image
+    :returns (int, int): The associated pixel co-ordinates.
+    """
+
+    return (int(np.floor((pos[0] + 10.0) / res)), int(np.floor((pos[1] + 10.0) / res)))
+def pix2geom(grid, res=0.1, size=(200, 200)):
+    """
+    Convert geometrical position to pixel co-ordinates. The origin 
+    is assumed to be at [image_size[0]-1, 0].
+    :param pos: The (x,y) geometric co-ordinates.
+    :param res: The distance represented by each pixel.
+    :param size: The size of the map image
+    :returns (int, int): The associated pixel co-ordinates.
+    """
+
+    return (np.float((grid[0]+0.5)*res-10.0), np.float((grid[1]+0.5)*res-10.0))
+
+def get_encoder_input(InputMap, goal_pos, goal_yaw, start_pos, start_yaw):
+    '''
+    Returns the input map appended with the goal, and start position encoded.
+    :param InputMap: The grayscale map
+    :param goal_pos: The goal pos of the robot on the costmap.
+    :param start_pos: The start pos of the robot on the costmap.
+    :returns np.array: The map concatentated with the encoded start and goal pose.
+    '''
+
+    receptive_field = 16
+    map_size = (200,200)
+    context_map = np.zeros(map_size)
+    context_cos_map = np.zeros(map_size)
+    context_sin_map = np.zeros(map_size)
+
+    goal_start_x = max(0, goal_pos[0]- receptive_field//2)
+    goal_start_y = max(0, goal_pos[1]- receptive_field//2)
+
+    goal_end_x = min( map_size[0], goal_pos[0]+ receptive_field//2)
+    goal_end_y = min( map_size[1], goal_pos[1]+ receptive_field//2)
+
+    context_map[goal_start_x:goal_end_x, goal_start_y:goal_end_y] = 1.0
+
+    context_cos_map[goal_start_x:goal_end_x, goal_start_y:goal_end_y] = math.cos(goal_yaw)
+    context_sin_map[goal_start_x:goal_end_x, goal_start_y:goal_end_y] = math.sin(goal_yaw)
+
+    # Mark start region
+    start_start_x = max(0, start_pos[0]- receptive_field//2)
+    start_start_y = max(0, start_pos[1]- receptive_field//2)
+    start_end_x = min( map_size[0], start_pos[0]+ receptive_field//2)
+    start_end_y = min( map_size[1], start_pos[1]+ receptive_field//2)
+
+    context_map[start_start_x:start_end_x, start_start_y:start_end_y] = -1.0
+    context_cos_map[start_start_x:start_end_x, start_start_y:start_end_y] = math.cos(start_yaw)
+    context_sin_map[start_start_x:start_end_x, start_start_y:start_end_y] = math.sin(start_yaw)
+
+
+    context_map = np.expand_dims(context_map, 0)
+    context_cos_map = np.expand_dims(context_cos_map, 0)
+    context_sin_map = np.expand_dims(context_sin_map, 0)
+    return np.concatenate((InputMap, context_map, context_cos_map, context_sin_map), axis = 0)

deepPathPlan/PathNet/loss.py

@@ -0,0 +1,345 @@
+
+import torch
+import torch.nn as nn
+import torch.nn.functional as F
+from torch.autograd import Variable
+from einops import rearrange
+    
+def positiveSmoothedL1(x):
+    #x:B*99
+    pe = 1.0e-4
+    half = 0.5 * pe
+    f3c = 1.0 / (pe * pe)
+    f4c = -0.5 * f3c / pe
+
+    b1 = x <= 0.0
+    b2 = (x>0.0) & (x < pe)
+    b3 = x >= pe
+
+    a1 = 0.0
+    a2 = (f4c * x + f3c) * x * x * x
+    a3 = x - half
+    loss = a1 * b1 + a2 * b2 + a3 * b3
+
+    return loss
+
+def positiveSmoothedL2(x):
+    #x:B*99
+    f = nn.ReLU()
+    x = f(x)
+    loss  = x * x
+    return loss
+
+def positiveSmoothedL3(x):
+    #x:B*99
+    f = nn.ReLU()
+    x = f(x)
+    loss  = x * x *x
+    return loss
+
+def floatToInt(pos, res=0.1):
+    #pos B*C*2
+    gridIdx = Variable(((pos+10.0)/res).floor().long())
+    return gridIdx
+
+def intToFloat(grid, res=0.1):
+    pos = Variable(((grid+0.5)*res-10.0).float())
+    return pos
+
+def getDistGrad(inputpos, esdfMap, res=0.1):
+    #pos B*C*2
+    #esdfMap B*3*200*200
+    outRange=  (inputpos < -9.9) | (inputpos > 9.9)
+    outRange = outRange[:,:,0] | outRange[:,:,1]
+    # print(outRange)
+    pos = torch.clamp(inputpos,-9.9,9.9)
+    # pos = inputpos
+    pos_m = pos - 0.5 * res
+    idx = floatToInt(pos_m) #B*C*2
+    idx_pos = intToFloat(idx)
+    diff = (pos - idx_pos) / res #B*C*2
+    
+    Bs = pos.shape[0]
+    channels = pos.shape[1]
+    values = torch.zeros(Bs,channels, 2,2).cuda()
+    for x in range(0,2):
+        for y in range(0,2):
+            offset = Variable(torch.tensor([x,y]).long().cuda())
+            current_idx = idx + offset#B*C*2
+            for i in range(Bs):
+                values[i,:,x,y] = esdfMap[i,0,current_idx[i,:,0], current_idx[i,:,1]]
+    values = Variable(values)
+    v00 = (1-diff[:,:,0]) * values[:,:,0,0] + diff[:,:,0] * values[:,:,1,0]
+    v10 = (1-diff[:,:,0]) * values[:,:,0,1] + diff[:,:,0] * values[:,:,1,1]
+    v0 = (1-diff[:,:,1]) * v00 + diff[:,:,1] * v10
+    v0 = torch.where(outRange, -1.0, v0)
+    return v0
+
+def getSqureArc(inputs):
+    pts1 = inputs[:,:-1,:]
+    pts2 = inputs[:,1:,:]
+    dif = pts1 - pts2
+    dif = dif * dif
+    arc = torch.sum(dif, dim=2)
+    arc = torch.sum(arc, dim=1)
+    return arc
+    
+class FocalLoss(nn.Module):
+    def __init__(self, weight=None, size_average=True):
+        super(FocalLoss, self).__init__()
+
+    def forward(self, inputs, targets, alpha=0.95, gamma=1.0, smooth=1):       
+        
+        #flatten label and prediction tensors
+        inputs = inputs.view(-1)
+        targets = targets.view(-1)
+        
+        #first compute binary cross-entropy 
+        ce_loss = F.binary_cross_entropy(inputs, targets, reduction='mean')
+        p_t = inputs * targets + (1 - inputs) * (1 - targets)
+        loss = ce_loss * ((1 - p_t) ** gamma)
+        if alpha >= 0:
+            alpha_t = alpha * targets + (1 - alpha) * (1 - targets)
+            loss = alpha_t * loss
+             
+        return loss.mean()
+    
+class ArcLoss(nn.Module):
+    def __init__(self):
+        super(ArcLoss, self).__init__()
+
+    def forward(self, inputs):
+        pts1 = inputs[:,:-1,:]
+        pts2 = inputs[:,1:,:]
+        arcs = pts1 - pts2
+        arcs = torch.norm(arcs, dim=2)
+        loss = torch.sum(arcs, dim=1)
+        return loss
+    
+class NormalizeArcLoss(nn.Module):
+    def __init__(self):
+        super(NormalizeArcLoss, self).__init__()
+        self.arcloss = ArcLoss()
+
+    def forward(self, inputs, labels):
+        arc1 = self.arcloss(inputs)
+        arc2 = self.arcloss(labels)
+        vioarc = arc1-arc2
+        arcloss = positiveSmoothedL1(vioarc)
+        return torch.mean(arcloss)
+
+class NormalizeRotLoss(nn.Module):
+    def __init__(self):
+        super(NormalizeRotLoss, self).__init__()
+
+    def forward(self, inputs, labels):
+        dif1 = getSqureArc(inputs)
+        dif2 = getSqureArc(labels)
+        normlizeArc = dif1 / dif2 #B
+        return torch.mean(normlizeArc)
+
+class SmoothTrajLoss(nn.Module):
+    def __init__(self):
+        super(SmoothTrajLoss, self).__init__()
+    def forward(self, opState):
+        #B*C*2
+        pts1 = opState[:,:-1,:]  
+        pts2 = opState[:,1:,:]
+        v = pts2-pts1
+        normV = torch.nn.functional.normalize(v, dim=2)#B 99 2
+        v1 = normV[:,:-1,:]
+        v2 = normV[:,1: ,:]
+        c = torch.zeros_like(v2)#B 98 2
+        c[:,:,0] = -v2[:,:,1]
+        c[:,:,1] =  v2[:,:,0]
+        cross = torch.sum(v1 * c,dim=2)#B*98
+        cross = torch.pow(cross,2)#b 98
+        loss = torch.sum(cross, dim=1)#B 
+        return loss
+
+class NormalizeSmoothTrajLoss(nn.Module):
+    def __init__(self):
+        super(NormalizeSmoothTrajLoss, self).__init__()
+        self.s = SmoothTrajLoss()
+    def forward(self, inputs, labels):
+        loss1 = self.s(inputs)
+        loss2 = self.s(labels)
+        vios = loss1-loss2
+        sloss = positiveSmoothedL1(vios) #B
+        return torch.mean(sloss)
+
+class GearTrajLoss(nn.Module):
+    def __init__(self):
+        super(GearTrajLoss, self).__init__()
+    def forward(self, opState):
+        #B*C*2
+        pts1 = opState[:,:-1,:]  
+        pts2 = opState[:,1:,:]
+        v = pts2-pts1
+        normV = torch.nn.functional.normalize(v, dim=2)#B 99 2
+        v1 = normV[:,:-1,:]
+        v2 = normV[:,1: ,:]
+        dif = v2-v1 #B*98*2
+        dfi2 = torch.pow(dif,2) # B*98*2
+        loss = torch.sum(dfi2, dim=2)#B*98
+        loss = torch.sum(loss, dim=1)#B
+        return loss
+
+class NormalizeGearTrajLoss(nn.Module):
+    def __init__(self):
+        super(NormalizeGearTrajLoss, self).__init__()
+        self.s = GearTrajLoss()
+    def forward(self, inputs, labels):
+        loss1 = self.s(inputs)
+        loss2 = self.s(labels)
+        loss = loss1/loss2
+        
+        vios = 8.0*(loss-1.0)#B
+        sloss = positiveSmoothedL1(vios) #B
+        return torch.mean(sloss)
+
+class UniforArcLoss(nn.Module):
+    def __init__(self):
+        super(UniforArcLoss, self).__init__()
+        self.arcloss = ArcLoss()
+
+    def forward(self, inputs, labels):
+        pts1 = inputs[:,:-1,:]  
+        pts2 = inputs[:,1:,:]
+        arcs = pts1 - pts2 #B*99*2
+        arcs = torch.norm(arcs, dim=2)#B*99
+        varloss = torch.std(arcs, dim=1, unbiased=False) #B
+        labelArc = self.arcloss(labels) #B
+        normalizeLoss = varloss / labelArc
+        loss = torch.mean(normalizeLoss)
+        return loss
+
+class NonholoLoss(nn.Module):
+    def __init__(self):
+        super(NonholoLoss, self).__init__()
+
+    def forward(self, inputs, labels):
+        #labels cos sin
+        pts1 = inputs[:,:-1,:]  
+        pts2 = inputs[:,1:,:]
+        arcs = pts2 - pts1
+        arcs = torch.nn.functional.normalize(arcs, dim=2)
+        
+        labeldir = torch.zeros_like(labels)
+        labeldir[:,:,0] = -labels[:,:,1]
+        labeldir[:,:,1] =  labels[:,:,0]
+        
+        cross = torch.sum(arcs * labeldir,dim=2)
+        cross = torch.pow(cross,2)
+        
+        
+        vioh = (cross-0.067)
+        hloss = positiveSmoothedL1(vioh)
+        return torch.mean(hloss)
+     
+class CurvatureLoss(nn.Module):
+    def __init__(self):
+        super(CurvatureLoss, self).__init__()
+        self.kmax = 1.67
+    def forward(self, opstate, rot):
+        rot1 = rot[:, :-1, :]
+        rot2 = rot[:, 1:, :]
+        rotarc = rot2-rot1
+        deltaAngles = torch.norm(rotarc,dim=2)
+        
+        pts1 = opstate[:,:-1,:]  
+        pts2 = opstate[:,1:,:]
+        arcs = pts2 - pts1
+        arcs = torch.norm(arcs, dim=2)
+        viok = (deltaAngles * deltaAngles - self.kmax * self.kmax * (arcs + 1.0e-3) * (arcs + 1.0e-3))
+        kloss = positiveSmoothedL1(viok)
+        return torch.mean(kloss)
+    
+class TurnLoss(nn.Module):
+    def __init__(self):
+        super(TurnLoss, self).__init__()
+    def forward(self, opState):
+        pts1 = opState[:,:-1,:]  
+        pts2 = opState[:,1:,:]
+        v = pts2-pts1
+        normV = torch.nn.functional.normalize(v, dim=2)
+        v1 = normV[:,:-1,:]
+        v2 = normV[:,1: ,:]
+        c = torch.zeros_like(v2)
+        c[:,:,0] = -v2[:,:,1]
+        c[:,:,1] =  v2[:,:,0]
+        cross = torch.sum(v1 * c,dim=2)
+
+        cross = torch.pow(cross,2)
+        vio = (cross - 0.35)
+        sloss = positiveSmoothedL1(vio)
+        return torch.mean(sloss)
+
+class CollisionLoss(nn.Module):
+    def __init__(self):
+        super(CollisionLoss, self).__init__()
+    def forward(self, opState, envs):
+        #inputs:B*C*2       B*3*200*200
+        dists = getDistGrad(opState, envs)
+        viod = 10.0*(0.3-dists)
+        penalty = positiveSmoothedL2(viod)
+        return torch.mean(penalty)
+    
+class FullShapeCollisionLoss(nn.Module):
+    def __init__(self):
+        super(FullShapeCollisionLoss, self).__init__()
+        self.conpts = torch.tensor([[0.15, 0.0], [0.45, -0.00]]).cuda()#10*2
+    def forward(self, opState, opRot, envs):
+        #inputs:B*C*2       B*3*200*200
+        B = opState.shape[0]
+        C = opState.shape[1]
+        N = self.conpts.shape[0]
+        rotR = torch.zeros(B,C,2,2).cuda()
+        cos = opRot[:,:,0]
+        sin = opRot[:,:,1]
+        rotR[:,:,0,0] = cos
+        rotR[:,:,0,1] = -sin
+        rotR[:,:,1,0] = sin
+        rotR[:,:,1,1] = cos
+        offset = torch.einsum('bcij,nj->bcni',rotR, self.conpts)
+        absPt = opState.unsqueeze(dim=2) # B C 1 2
+        absPt = absPt.repeat(1,1,N,1)
+        absPt = absPt + offset #B C N 2
+        absPt = rearrange(absPt, 'b c n i-> b (c n) i') #B*CN*2
+        dists = getDistGrad(absPt, envs)
+        viod = 10.0*(0.3-dists)
+        penalty = positiveSmoothedL1(viod)
+        return torch.mean(penalty)
+    
+class TotalLoss(nn.Module):
+    def __init__(self):
+        super(TotalLoss, self).__init__()
+        self.wei_arc = torch.tensor(0.5)
+        self.wei_uni = torch.tensor(100.0)
+        self.wei_hol = torch.tensor(500.0)
+        self.wei_cur = torch.tensor(500.0)
+        self.wei_safety = torch.tensor(500.0)
+        self.wei_rsm = torch.tensor(0.5)
+        self.wei_traj = torch.tensor(0.3)
+        self.wei_turn  = torch.tensor(100.0)
+        
+        self.smooLoss = NormalizeArcLoss()
+        self.rotsLoss = NormalizeArcLoss()
+        self.holoLoss = NonholoLoss()
+        self.uniLoss = UniforArcLoss()
+        self.curLoss = CurvatureLoss()
+        self.safeLoss = FullShapeCollisionLoss()
+        self.trajLoss = NormalizeSmoothTrajLoss()
+        self.turnLoss = TurnLoss()
+
+    def forward(self, opState, label_opState, opRot, label_opRot,envs):
+        arcloss = self.wei_arc * self.smooLoss(opState, label_opState)
+        holloss = self.wei_hol * self.holoLoss(opState,opRot[:,1:,:])
+        unilos = self.wei_uni * self.uniLoss(opState,label_opState)
+        curloss = self.wei_cur * self.curLoss(opState, opRot)
+        rsmloss = self.wei_rsm * self.rotsLoss(opRot, label_opRot)
+        safetyloss = self.wei_safety * self.safeLoss(opState, opRot, envs)
+        trajloss = self.wei_traj * self.trajLoss(opState, label_opState)
+        turnloss = self.wei_turn * self.turnLoss(opState)
+        loss = arcloss + holloss + unilos  + curloss + rsmloss + safetyloss+trajloss+turnloss
+        return loss

deepPathPlan/PathNet/network.py

@@ -0,0 +1,509 @@
+import torch
+import torch.nn as nn
+import time
+import torch.nn.functional as F
+from torch.autograd import Variable
+from einops import rearrange
+import numpy as np
+from Layers import EncoderLayer
+from einops.layers.torch import Rearrange
+
+def filter(opState, kernelsize=5):
+    Bs = opState.shape[0]
+    ches = opState.shape[1]
+    recL = int((kernelsize-3)/2)
+    labelTable = torch.zeros(Bs, int(ches+2*recL),opState.shape[2]).cuda()
+    labelTable[:,:recL,:] =   opState[:,0,:].unsqueeze(dim=1)
+    labelTable[:,-recL:,:] =   opState[:,-1,:].unsqueeze(dim=1)
+    labelTable[:,recL:-recL,:] = opState
+
+    newOpState = torch.zeros_like(opState)
+
+    tmpT = labelTable.unfold(1, kernelsize, 1)
+    tmpMeanT = torch.mean(tmpT, dim=-1)
+    newOpState[:,1:-1,:] = tmpMeanT
+
+
+    newOpState[:,0,:] = opState[:,0,:]
+    newOpState[:,-1,:] = opState[:,-1,:]
+    
+    return newOpState
+
+class Down(nn.Module):
+    """Downscaling with maxpool then double conv"""
+
+    def __init__(self, in_channels, out_channels):
+        super().__init__()
+        self.maxpool_conv = nn.Sequential(
+            nn.MaxPool2d(2),
+            DoubleConv(in_channels, out_channels)
+        )
+
+    def forward(self, x):
+        return self.maxpool_conv(x)
+
+
+class Up(nn.Module):
+    """Upscaling then double conv"""
+
+    def __init__(self, in_channels, out_channels):
+        super().__init__()
+
+
+        self.up = nn.ConvTranspose2d(in_channels, in_channels // 2, kernel_size=2, stride=2)
+        self.conv = DoubleConv(in_channels, out_channels)
+
+    def forward(self, x1, x2):
+        x1 = self.up(x1)
+        # input is CHW
+        diffY = x2.size()[2] - x1.size()[2]
+        diffX = x2.size()[3] - x1.size()[3]
+
+        x1 = F.pad(x1, [diffX // 2, diffX - diffX // 2,
+                        diffY // 2, diffY - diffY // 2])
+        # if you have padding issues, see
+        # https://github.com/HaiyongJiang/U-Net-Pytorch-Unstructured-Buggy/commit/0e854509c2cea854e247a9c615f175f76fbb2e3a
+        # https://github.com/xiaopeng-liao/Pytorch-UNet/commit/8ebac70e633bac59fc22bb5195e513d5832fb3bd
+        x = torch.cat([x2, x1], dim=1)
+        return self.conv(x)
+    
+    
+    
+class block(nn.Module):
+    def __init__(
+        self, in_channels, intermediate_channels, identity_downsample=None, stride=1
+    ):
+        super().__init__()
+        self.expansion = 4
+        self.conv1 = nn.Conv2d(
+            in_channels,
+            intermediate_channels,
+            kernel_size=1,
+            stride=1,
+            padding=0,
+            bias=False,
+        )
+        self.bn1 = nn.BatchNorm2d(intermediate_channels)
+        self.conv2 = nn.Conv2d(
+            intermediate_channels,
+            intermediate_channels,
+            kernel_size=3,
+            stride=stride,
+            padding=1,
+            bias=False,
+        )
+        self.bn2 = nn.BatchNorm2d(intermediate_channels)
+        self.conv3 = nn.Conv2d(
+            intermediate_channels,
+            intermediate_channels * self.expansion,
+            kernel_size=1,
+            stride=1,
+            padding=0,
+            bias=False,
+        )
+        self.bn3 = nn.BatchNorm2d(intermediate_channels * self.expansion)
+        self.relu = nn.ReLU()
+        self.identity_downsample = identity_downsample
+        self.stride = stride
+
+    def forward(self, x):
+        
+        identity = x.clone()
+        x = self.conv1(x)
+        x = self.bn1(x)
+        x = self.relu(x)
+        x = self.conv2(x)
+        x = self.bn2(x)
+        x = self.relu(x)
+        x = self.conv3(x)
+        x = self.bn3(x)
+        if self.identity_downsample is not None:
+            identity = self.identity_downsample(identity)
+        x += identity
+        x = self.relu(x)
+        return x
+
+class DoubleConv(nn.Module):
+    """(convolution => [BN] => ReLU) * 2"""
+
+    def __init__(self, in_channels, out_channels):
+        super().__init__()
+        self.double_conv = nn.Sequential(
+            nn.Conv2d(in_channels, out_channels, kernel_size=3, padding=1, bias=False),
+            nn.BatchNorm2d(out_channels),
+            nn.ReLU(inplace=True),
+            nn.Conv2d(out_channels, out_channels, kernel_size=3, padding=1, bias=False),
+            nn.BatchNorm2d(out_channels),
+            nn.ReLU(inplace=True)
+        )
+
+    def forward(self, x):
+        return self.double_conv(x)
+class OutConv(nn.Module):
+    def __init__(self, in_channels, out_channels):
+        super(OutConv, self).__init__()
+        self.conv = nn.Conv2d(in_channels, out_channels, kernel_size=1)
+
+    def forward(self, x):
+        return self.conv(x)
+
+
+class PositionalEncoding(nn.Module):
+    '''Positional encoding
+    '''
+    def __init__(self, d_hid, n_position, train_shape):
+        '''
+        Intialize the Encoder.
+        :param d_hid: Dimesion of the attention features.
+        :param n_position: Number of positions to consider.
+        :param train_shape: The 2D shape of the training model.
+        '''
+        super(PositionalEncoding, self).__init__()
+        self.n_pos_sqrt = int(np.sqrt(n_position))
+        self.train_shape = train_shape
+        # Not a parameter
+        self.register_buffer('hashIndex', self._get_hash_table(n_position))
+        self.register_buffer('pos_table', self._get_sinusoid_encoding_table(n_position, d_hid))
+        self.register_buffer('pos_table_train', self._get_sinusoid_encoding_table_train(n_position, train_shape))
+
+    def _get_hash_table(self, n_position):
+        '''
+        A simple table converting 1D indexes to 2D grid.
+        :param n_position: The number of positions on the grid.
+        ''' 
+
+        return rearrange(torch.arange(n_position), '(h w) -> h w', h=int(np.sqrt(n_position)), w=int(np.sqrt(n_position))) # 40 * 40
+
+    def _get_sinusoid_encoding_table(self, n_position, d_hid):
+        '''
+        Sinusoid position encoding table.
+        :param n_position:
+        :param d_hid:
+        :returns 
+        '''
+        # TODO: make it with torch instead of numpy
+
+        def get_position_angle_vec(position):
+            return [position / np.power(10000, 2 * (hid_j // 2) / d_hid) for hid_j in range(d_hid)]
+
+        sinusoid_table = np.array([get_position_angle_vec(pos_i) for pos_i in range(n_position)])
+        sinusoid_table[:, 0::2] = np.sin(sinusoid_table[:, 0::2])  # dim 2i
+        sinusoid_table[:, 1::2] = np.cos(sinusoid_table[:, 1::2])  # dim 2i+1
+        return torch.FloatTensor(sinusoid_table[None,:])
+    
+    def _get_sinusoid_encoding_table_train(self, n_position, train_shape):
+        '''
+        The encoding table to use for training.
+        NOTE: It is assumed that all training data comes from a fixed map.
+        NOTE: Another assumption that is made is that the training maps are square.
+        :param n_position: The maximum number of positions on the table.
+        :param train_shape: The 2D dimension of the training maps.
+        '''
+        selectIndex = rearrange(self.hashIndex[:train_shape[0], :train_shape[1]], 'h w -> (h w)') # 24 * 24
+        return torch.index_select(self.pos_table, dim=1, index=selectIndex)
+
+    def forward(self, x, conv_shape=None):
+        '''
+        Callback function
+        :param x:
+        '''
+        if conv_shape is None:
+            startH, startW = torch.randint(0, self.n_pos_sqrt-self.train_shape[0], (2,))
+            selectIndex = rearrange(
+                self.hashIndex[startH:startH+self.train_shape[0], startW:startW+self.train_shape[1]],
+                'h w -> (h w)'
+                )
+            return x + torch.index_select(self.pos_table, dim=1, index=selectIndex).clone().detach()
+
+        # assert x.shape[0]==1, "Only valid for testing single image sizes"
+        selectIndex = rearrange(self.hashIndex[:conv_shape[0], :conv_shape[1]], 'h w -> (h w)')
+        return x + self.pos_table[:, selectIndex.long(), :]
+
+
+class Encoder(nn.Module):
+    ''' The encoder of the planner.
+    '''
+
+    def __init__(self, n_layers, n_heads, d_k, d_v, d_model, d_inner, pad_idx, n_position, train_shape):
+        '''
+        Intialize the encoder.
+        :param n_layers: Number of layers of attention and fully connected layer.
+        :param n_heads: Number of self attention modules.
+        :param d_k: Dimension of each Key.
+        :param d_v: Dimension of each Value.
+        :param d_model: Dimension of input/output of encoder layer.
+        :param d_inner: Dimension of the hidden layers of position wise FFN
+        :param pad_idx: TODO ....
+        :param dropout: The value to the dropout argument.
+        :param n_position: Total number of patches the model can handle.
+        :param train_shape: The shape of the output of the patch encodings.
+        '''
+        super().__init__()
+        self.to_patch_embedding = nn.Sequential(
+
+            
+            (DoubleConv(4, 64)),
+            nn.MaxPool2d(2),
+            nn.Conv2d(64, 128, kernel_size=3, padding=0, bias=False),
+            nn.BatchNorm2d(128),
+            nn.ReLU(inplace=True),
+            nn.Conv2d(128, 128, kernel_size=3, padding=0, bias=False),
+            nn.BatchNorm2d(128),
+            nn.ReLU(inplace=True),
+            nn.MaxPool2d(2),
+            nn.Conv2d(128, 256, kernel_size=3, padding=0, bias=False),
+            nn.BatchNorm2d(256),
+            nn.ReLU(inplace=True),
+            nn.Conv2d(256, 256, kernel_size=3, padding=0, bias=False),
+            nn.BatchNorm2d(256),
+            nn.ReLU(inplace=True),
+            nn.MaxPool2d(2),
+            nn.Conv2d(256, 512, kernel_size=3, padding=0, bias=False),
+            nn.BatchNorm2d(512),
+            nn.ReLU(inplace=True),
+            nn.Conv2d(512, 512, kernel_size=3, padding=1, bias=False),
+            nn.BatchNorm2d(512),
+            nn.ReLU(inplace=True)
+        )
+
+        self.reorder_dims = Rearrange('b c h w -> b (h w) c')
+        # Position Encoding.
+        # NOTE: Current setup for adding position encoding after patch Embedding.
+        self.position_enc = PositionalEncoding(d_model, n_position=n_position, train_shape=train_shape)
+
+        self.layer_stack = nn.ModuleList([
+            EncoderLayer(d_model, d_inner, n_heads, d_k, d_v)
+            for _ in range(n_layers)
+        ])
+
+        self.layer_norm = nn.LayerNorm(d_model, eps=1e-6)
+        
+
+    def forward(self, input_map, returns_attns=False):
+        '''
+        The input of the Encoder should be of dim (b, c, h, w).
+        :param input_map: The input map for planning.
+        :param returns_attns: If True, the model returns slf_attns at each layer
+        '''
+        enc_slf_attn_list = []
+        enc_output = self.to_patch_embedding(input_map)
+        conv_map_shape = enc_output.shape[-2:]
+        enc_output = self.reorder_dims(enc_output)
+
+        if self.training:
+            enc_output = self.position_enc(enc_output)
+        else:
+            enc_output = self.position_enc(enc_output, conv_map_shape)
+        
+        enc_output = self.layer_norm(enc_output)
+
+        for enc_layer in self.layer_stack:
+            enc_output = enc_layer(enc_output, slf_attn_mask=None)
+        
+        if returns_attns:
+            return enc_output, enc_slf_attn_list
+        return enc_output, 
+    
+    
+class Transformer(nn.Module):
+    ''' A Transformer module
+    '''
+    def __init__(self, n_layers, n_heads, d_k, d_v, d_model, d_inner, pad_idx, n_position, train_shape):
+        '''
+        Initialize the Transformer model.
+        :param n_layers: Number of layers of attention and fully connected layers
+        :param n_heads: Number of self attention modules.
+        :param d_k: Dimension of each Key.
+        :param d_v: Dimension of each Value.
+        :param d_model: Dimension of input/output of decoder layer.
+        :param d_inner: Dimension of the hidden layers of position wise FFN1
+        :param pad_idx: TODO ......
+        :param dropout: The value of the dropout argument.
+        :param n_position: Dim*dim of the maximum map size.
+        :param train_shape: The shape of the output of the patch encodings. 
+        '''
+        super().__init__()
+
+        self.encoder = Encoder(
+            n_layers=n_layers, # num of sublayer
+            n_heads=n_heads,  # a dimension in query, key, value
+            d_k=d_k, # dimension of key
+            d_v=d_v, # dimension of value
+            d_model=d_model, # channel of conv as a first part
+            d_inner=d_inner, # channel of inner part in the model
+            pad_idx=pad_idx, 
+            n_position=n_position, # max table size for position encoding
+            train_shape=train_shape # image size in meters
+        )
+
+    def forward(self, input_map):
+        '''
+        The callback function.
+        :param input_map:
+        :param goal: A 2D torch array representing the goal.
+        :param start: A 2D torch array representing the start.
+        :param cur_index: The current anchor point of patch.
+        '''
+        enc_output, *_ = self.encoder(input_map)
+        enc_output = rearrange(enc_output, 'b c d -> b d c')
+        enc_output = rearrange(enc_output, 'b c (h w) -> b c h w', h = 20)
+        return enc_output
+
+class AnchorNet25(nn.Module):
+    def __init__(self, n_channels, out_channels=1):
+        super(AnchorNet25, self).__init__()
+        model_args = dict(
+            n_layers=6, 
+            n_heads=3, 
+            d_k=512, 
+            d_v=256, 
+            d_model=512, 
+            d_inner=1024, 
+            pad_idx=None,
+            n_position=40*40, 
+            # train_shape=[25, 25],
+            train_shape=[20, 20]
+        )
+        self.transformer = Transformer(**model_args)
+
+
+        self.outc = nn.Sequential(
+            nn.Conv2d(512, 1024, kernel_size=3, padding=1,stride=1),
+            nn.BatchNorm2d(1024),
+            nn.ReLU(inplace=True),
+            OutConv(1024, out_channels)
+        )
+
+    def forward(self, x):
+        x = self.transformer(x)
+        result = self.outc(x)
+        result_1 = rearrange(result, 'b c h w-> b c (h w)')
+        result_2 = rearrange(result_1, 'b c l-> (b c) l')
+        result = rearrange(result, 'b c h w-> b c (h w)')
+        result = torch.softmax(result, dim=2)
+        result = rearrange(result, 'b c (h w)-> b c h w', h = 20)
+        return x,result,result_2
+
+
+class trajFCNet(nn.Module):
+    def __init__(self, image_channels=4, pt_num=100, filter = 7, l = 1.2, use_groundTruth = True):
+        super(trajFCNet, self).__init__()
+        self.a = AnchorNet25(image_channels, pt_num)
+    
+        self.fcntrajout = nn.Sequential(
+            nn.Conv2d(512+pt_num, 1024, kernel_size=3, padding=1,stride=1),
+            nn.BatchNorm2d(1024),
+            nn.ReLU(inplace=True),
+            OutConv(1024, pt_num*3)
+        )
+     
+
+        self.pt_num = pt_num
+        self.filter = filter
+        self.l = l
+        self.w = (self.l -1.0)/2.0
+
+        self.use_groundTruth = use_groundTruth
+        self.sig = nn.Sigmoid()#0~1
+        xylim = torch.arange(0,20).unsqueeze(dim=1)
+        xylim = xylim.repeat(1,20)
+        yxlim = torch.arange(0,20).unsqueeze(dim=0)
+        yxlim = yxlim.repeat(20,1)
+        self.register_buffer('xylim', xylim)
+        self.register_buffer('yxlim', yxlim)
+
+
+
+
+    def forward(self, x, labelState, labelRot, anchors):
+        ft, result_1, result_2 = self.a(x)
+        
+        
+        prbmap = torch.zeros_like(result_1)
+        prbmap[:,0,:,:] = anchors[:,0,:,:]
+        prbmap[:,-1,:,:] = anchors[:,-1,:,:]
+        prbmap[:,1:-1,:,:] = result_1[:,1:-1,:,:]
+
+
+        resFeature= ft
+        if(self.use_groundTruth):
+            anchorsFeature = anchors
+        else:
+            anchorsFeature = prbmap
+
+        resInput = torch.cat((anchorsFeature, resFeature), dim=1)
+
+        resOutput = self.fcntrajout(resInput)
+        if(self.use_groundTruth):
+            if(self.l>0):
+                #hzchzc
+                px = (self.l*self.sig(resOutput[:,0::3,:,:])-self.w)* anchors 
+                py = (self.l*self.sig(resOutput[:,1::3,:,:])-self.w)* anchors
+            else:
+                px = resOutput[:,0::3,:,:]* anchors
+                py = resOutput[:,1::3,:,:]* anchors
+            yw =               resOutput[:,2::3,:,:]  * anchors
+        else:
+            if(self.l>0):
+                px = (self.l*self.sig(resOutput[:,0::3,:,:])-self.w)* prbmap
+                py = (self.l*self.sig(resOutput[:,1::3,:,:])-self.w)* prbmap
+            else:
+                px = resOutput[:,0::3,:,:]* prbmap 
+                py = resOutput[:,1::3,:,:]* prbmap
+            yw =               resOutput[:,2::3,:,:]  * prbmap
+        
+        # bias
+        px = torch.sum(torch.sum(px, dim = 3), dim=2).unsqueeze(dim=2)
+        py = torch.sum(torch.sum(py, dim = 3), dim=2).unsqueeze(dim=2)
+        yw = torch.sum(torch.sum(yw, dim = 3), dim=2)
+
+        gridx = Variable(self.xylim, requires_grad = False)
+        gridy = Variable(self.yxlim, requires_grad = False)
+        if(self.use_groundTruth):
+            xmap = anchors * gridx
+            ymap = anchors * gridy
+        else:    
+            xmap = prbmap * gridx
+            ymap = prbmap * gridy
+        aveGirdx = torch.sum(torch.sum(xmap, dim=3), dim=2).unsqueeze(dim=2)
+        aveGirdy = torch.sum(torch.sum(ymap, dim=3), dim=2).unsqueeze(dim=2)
+        # local origin
+        lo = torch.cat((aveGirdx, aveGirdy), dim=2)*1.0-10.0
+        opState = torch.cat((px, py), dim=2) + lo
+        cosyaw = torch.cos(yw).unsqueeze(dim=2)
+        sinyaw = torch.sin(yw).unsqueeze(dim=2)
+        rotOutput = torch.cat((cosyaw,sinyaw), dim=2)
+        opState[:,0,:] = labelState[:,0,:]
+        opState[:,-1,:] = labelState[:,-1,:]
+        rotOutput[:,0,:] = labelRot[:,0,:]
+        rotOutput[:,-1,:] = labelRot[:,-1,:]
+        if(self.filter >=3):
+            opState = filter(opState, self.filter)
+            rotOutput = filter(rotOutput, self.filter)
+            rotOutput = torch.nn.functional.normalize(rotOutput, dim=2)
+
+
+        return opState, rotOutput, prbmap, result_2
+
+if __name__ == "__main__":
+    pt = 200
+    model = trajFCNet(pt_num=pt, filter=7).cuda().half()
+    totalt = 0.0
+    count = 0
+    model.eval()
+    input  = torch.rand(1,4,200,200).cuda().half()
+    labelState = torch.rand(1,pt,2).cuda().half()
+    labelRot = torch.rand(1,pt,2).cuda().half()
+    anchors = torch.rand(1,pt,20,20).cuda().half()
+    out = model(input, labelState, labelRot, anchors)
+    with torch.no_grad():
+        for i in range(200):
+            torch.cuda.synchronize()
+            start = time.time()
+            out = model(input, labelState, labelRot, anchors)
+            torch.cuda.synchronize()
+            end = time.time()
+            if i>=20:
+                totalt += 1000.0*(end-start)
+                count +=1
+    print("model time: ", totalt / count, " ms")

deepPathPlan/PathNet/resnet.py

@@ -0,0 +1,274 @@
+
+import torch
+import torch.nn as nn
+import torch.nn.functional as F
+import time
+import timm
+def filter(opState, kernelsize=5):
+    #B*100*2
+
+    Bs = opState.shape[0]
+    ches = opState.shape[1]
+    recL = int((kernelsize-3)/2)
+    labelTable = torch.zeros(Bs, int(ches+2*recL),opState.shape[2]).cuda()
+    labelTable[:,:recL,:] =   opState[:,0,:].unsqueeze(dim=1)
+    labelTable[:,-recL:,:] =   opState[:,-1,:].unsqueeze(dim=1)
+    labelTable[:,recL:-recL,:] = opState
+
+    newOpState = torch.zeros_like(opState)
+
+    tmpT = labelTable.unfold(1, kernelsize, 1)
+    tmpMeanT = torch.mean(tmpT, dim=-1)
+    newOpState[:,1:-1,:] = tmpMeanT
+
+
+    newOpState[:,0,:] = opState[:,0,:]
+    newOpState[:,-1,:] = opState[:,-1,:]
+    
+
+    return newOpState
+class BasicBlock(nn.Module):
+    expansion = 1
+
+    def __init__(self, in_planes, planes, stride=1):
+        super(BasicBlock, self).__init__()
+        self.conv1 = nn.Conv2d(
+            in_planes, planes, kernel_size=3, stride=stride, padding=1, bias=False)
+        self.bn1 = nn.BatchNorm2d(planes)
+        self.conv2 = nn.Conv2d(planes, planes, kernel_size=3,
+                               stride=1, padding=1, bias=False)
+        self.bn2 = nn.BatchNorm2d(planes)
+
+        self.shortcut = nn.Sequential()
+        if stride != 1 or in_planes != self.expansion*planes:
+            self.shortcut = nn.Sequential(
+                nn.Conv2d(in_planes, self.expansion*planes,
+                          kernel_size=1, stride=stride, bias=False),
+                nn.BatchNorm2d(self.expansion*planes)
+            )
+
+    def forward(self, x):
+        out = F.relu(self.bn1(self.conv1(x)))
+        out = self.bn2(self.conv2(out))
+        out += self.shortcut(x)
+        out = F.relu(out)
+        return out
+
+
+class Bottleneck(nn.Module):
+    expansion = 4
+
+    def __init__(self, in_planes, planes, stride=1):
+        super(Bottleneck, self).__init__()
+        self.conv1 = nn.Conv2d(in_planes, planes, kernel_size=1, bias=False)
+        self.bn1 = nn.BatchNorm2d(planes)
+        self.conv2 = nn.Conv2d(planes, planes, kernel_size=3,
+                               stride=stride, padding=1, bias=False)
+        self.bn2 = nn.BatchNorm2d(planes)
+        self.conv3 = nn.Conv2d(planes, self.expansion *
+                               planes, kernel_size=1, bias=False)
+        self.bn3 = nn.BatchNorm2d(self.expansion*planes)
+
+        self.shortcut = nn.Sequential()
+        if stride != 1 or in_planes != self.expansion*planes:
+            self.shortcut = nn.Sequential(
+                nn.Conv2d(in_planes, self.expansion*planes,
+                          kernel_size=1, stride=stride, bias=False),
+                nn.BatchNorm2d(self.expansion*planes)
+            )
+
+    def forward(self, x):
+        out = F.relu(self.bn1(self.conv1(x)))
+        out = F.relu(self.bn2(self.conv2(out)))
+        out = self.bn3(self.conv3(out))
+        out += self.shortcut(x)
+        out = F.relu(out)
+        return out
+
+
+class ResNet(nn.Module):
+    def __init__(self, block, num_blocks, num_classes=600):
+        super(ResNet, self).__init__()
+        self.in_planes = 64
+
+        self.conv1 = nn.Conv2d(4, 64, kernel_size=3,
+                               stride=1, padding=1, bias=False)
+        self.bn1 = nn.BatchNorm2d(64)
+        self.layer1 = self._make_layer(block, 64, num_blocks[0], stride=1)
+        self.layer2 = self._make_layer(block, 128, num_blocks[1], stride=2)
+        self.layer3 = self._make_layer(block, 256, num_blocks[2], stride=2)
+        self.layer4 = self._make_layer(block, 512, num_blocks[3], stride=2)
+        self.linear = nn.Linear(512*block.expansion, num_classes)
+
+    def _make_layer(self, block, planes, num_blocks, stride):
+        strides = [stride] + [1]*(num_blocks-1)
+        layers = []
+        for stride in strides:
+            layers.append(block(self.in_planes, planes, stride))
+            self.in_planes = planes * block.expansion
+        return nn.Sequential(*layers)
+
+    def forward(self, x):
+        out = F.relu(self.bn1(self.conv1(x)))
+        out = self.layer1(out)
+        out = self.layer2(out)
+        out = self.layer3(out)
+        out = self.layer4(out)
+        out = F.avg_pool2d(out, 4)
+        out = out.view(out.size(0), -1)
+        out = self.linear(out)
+        return out
+
+
+def ResNet18():
+    return ResNet(BasicBlock, [2, 2, 2, 2])
+
+
+def ResNet34():
+    return ResNet(BasicBlock, [3, 4, 6, 3])
+
+
+def ResNet50():
+    return ResNet(Bottleneck, [3, 4, 6, 3])
+
+
+def ResNet101():
+    return ResNet(Bottleneck, [3, 4, 23, 3])
+
+
+def ResNet152():
+    return ResNet(Bottleneck, [3, 8, 36, 3])
+
+
+def test():
+    net = ResNet18()
+    y = net(torch.randn(1, 3, 32, 32))
+    print(y.size())
+
+
+class resnet101(nn.Module):
+    def __init__(self,  filter = 7):
+        super(resnet101, self).__init__()
+        self.image = timm.create_model('resnet101',num_classes=600)
+        self.image._modules['conv1'] = nn.Conv2d(4, 64, kernel_size=(7, 7), stride=(2, 2), padding=(3, 3), bias=False)
+        # print(self.image)
+        self.filter = filter
+
+    def forward(self, x, labelState, labelRot):
+        Bs = x.shape[0]
+        ft = torch.reshape(self.image(x),(Bs,200,3))
+        # print("lo.shape", lo.shape)
+        opState = torch.zeros_like(labelState) 
+        yw = ft[:,:,2]
+        cosyaw = torch.cos(yw).unsqueeze(dim=2) #b*100*1
+        sinyaw = torch.sin(yw).unsqueeze(dim=2) #b*100*1
+        rotOutput = torch.cat((cosyaw,sinyaw), dim=2) #b*100*2
+        opState[:,1:-1,:] = ft[:,1:-1,0:2]
+        opState[:,0,:] = labelState[:,0,:]
+        opState[:,-1,:] = labelState[:,-1,:]
+        # print("rotOutput.shape", rotOutput.shape)
+        # print("labelRot.shape", labelRot.shape)
+        rotOutput[:,0,:] = labelRot[:,0,:]
+        rotOutput[:,-1,:] = labelRot[:,-1,:]
+        if(self.filter >=3):
+            opState = filter(opState, self.filter)
+            rotOutput = filter(rotOutput, self.filter)#B*99*2
+            rotOutput = torch.nn.functional.normalize(rotOutput, dim=2)
+
+
+        return opState, rotOutput
+    
+class resnet50(nn.Module):
+    def __init__(self,  filter = 7):
+        super(resnet50, self).__init__()
+        self.image = timm.create_model('resnet50',num_classes=600)
+        self.image._modules['conv1'] = nn.Conv2d(4, 64, kernel_size=(7, 7), stride=(2, 2), padding=(3, 3), bias=False)
+        # print(self.image)
+        self.filter = filter
+
+    def forward(self, x, labelState, labelRot):
+        Bs = x.shape[0]
+        ft = torch.reshape(self.image(x),(Bs,200,3))
+        # print("lo.shape", lo.shape)
+        opState = torch.zeros_like(labelState) 
+        yw = ft[:,:,2]
+        cosyaw = torch.cos(yw).unsqueeze(dim=2) #b*100*1
+        sinyaw = torch.sin(yw).unsqueeze(dim=2) #b*100*1
+        rotOutput = torch.cat((cosyaw,sinyaw), dim=2) #b*100*2
+        opState[:,1:-1,:] = ft[:,1:-1,0:2]
+        opState[:,0,:] = labelState[:,0,:]
+        opState[:,-1,:] = labelState[:,-1,:]
+        # print("rotOutput.shape", rotOutput.shape)
+        # print("labelRot.shape", labelRot.shape)
+        rotOutput[:,0,:] = labelRot[:,0,:]
+        rotOutput[:,-1,:] = labelRot[:,-1,:]
+        if(self.filter >=3):
+            opState = filter(opState, self.filter)
+            rotOutput = filter(rotOutput, self.filter)#B*99*2
+            rotOutput = torch.nn.functional.normalize(rotOutput, dim=2)
+
+
+        return opState, rotOutput
+    
+class resnet152(nn.Module):
+    def __init__(self, filter = 7):
+        super(resnet152, self).__init__()
+        self.image = timm.create_model('resnet152',num_classes=600)
+        self.image._modules['conv1'] = nn.Conv2d(4, 64, kernel_size=(7, 7), stride=(2, 2), padding=(3, 3), bias=False)
+        # print(self.image)
+        self.filter = filter
+
+    def forward(self, x, labelState, labelRot):
+        Bs = x.shape[0]
+        ft = torch.reshape(self.image(x),(Bs,200,3))
+        # print("lo.shape", lo.shape)
+        opState = torch.zeros_like(labelState) 
+        yw = ft[:,:,2]
+        cosyaw = torch.cos(yw).unsqueeze(dim=2) #b*100*1
+        sinyaw = torch.sin(yw).unsqueeze(dim=2) #b*100*1
+        rotOutput = torch.cat((cosyaw,sinyaw), dim=2) #b*100*2
+        opState[:,1:-1,:] = ft[:,1:-1,0:2]
+        opState[:,0,:] = labelState[:,0,:]
+        opState[:,-1,:] = labelState[:,-1,:]
+        # print("rotOutput.shape", rotOutput.shape)
+        # print("labelRot.shape", labelRot.shape)
+        rotOutput[:,0,:] = labelRot[:,0,:]
+        rotOutput[:,-1,:] = labelRot[:,-1,:]
+        if(self.filter >=3):
+            opState = filter(opState, self.filter)
+            rotOutput = filter(rotOutput, self.filter)#B*99*2
+            rotOutput = torch.nn.functional.normalize(rotOutput, dim=2)
+
+
+        return opState, rotOutput
+    
+  
+
+
+
+# test()
+if __name__ == "__main__":
+    # test()
+    pt = 200
+    model = resnet152(pt_num=pt, filter=7).cuda().half()
+    totalt = 0.0
+    count = 0
+    model.eval()
+    # out = model(input)
+    input  = torch.rand(1,4,200,200).cuda().half()
+    labelState = torch.rand(1,pt,2).cuda().half()
+    labelRot = torch.rand(1,pt,2).cuda().half()
+    anchors = torch.rand(1,pt,20,20).cuda().half()
+    with torch.no_grad():
+        for i in range(100):
+            torch.cuda.synchronize()
+            start = time.time()
+            out = model(input, labelState, labelRot, anchors)
+            torch.cuda.synchronize()
+            end = time.time()
+            if i>=10:
+                totalt += 1000.0*(end-start)
+                count +=1
+    print("model time: ", totalt / count, " ms")
+    
+    

deepPathPlan/PathNet/resshep.py

@@ -0,0 +1,69 @@
+import numpy as np
+import reeds_shepp
+import math
+rho = 1 / 1.67 # turning radius
+step_size = 0.1
+def geom2pix(pos, res=0.1, size=(200, 200)):
+    """
+    Convert geometrical position to pixel co-ordinates. The origin 
+    is assumed to be at [image_size[0]-1, 0].
+    :param pos: The (x,y) geometric co-ordinates.
+    :param res: The distance represented by each pixel.
+    :param size: The size of the map image
+    :returns (int, int): The associated pixel co-ordinates.
+    """
+
+    return (int(np.floor((pos[0] + 10.0) / res)), int(np.floor((pos[1] + 10.0) / res)))
+def pix2geom(grid, res=0.1, size=(200, 200)):
+    """
+    Convert geometrical position to pixel co-ordinates. The origin 
+    is assumed to be at [image_size[0]-1, 0].
+    :param pos: The (x,y) geometric co-ordinates.
+    :param res: The distance represented by each pixel.
+    :param size: The size of the map image
+    :returns (int, int): The associated pixel co-ordinates.
+    """
+
+    return (np.float((grid[0]+0.5)*res-10.0), np.float((grid[1]+0.5)*res-10.0))
+def reedshep_process(opState, opRot, env):
+    #opState 100*2 env 200*200
+    endPx = opState[-1,0]
+    endPy = opState[-1,1]
+    endCos = opRot[-1,0]
+    endSin = opRot[-1,1]
+    endYaw = math.atan2(endSin, endCos)
+    endQ = (endPx, endPy, endYaw)
+    for i in range(170,opState.shape[0]-1):
+        px = opState[i,0]
+        py = opState[i,1]
+        cos = opRot[i,0]
+        sin = opRot[i,1]
+        yaw = math.atan2(sin, cos)
+        q0 = (px, py, yaw)
+        qs = reeds_shepp.path_sample(q0, endQ, rho, step_size)
+        xs = [q[0] for q in qs]
+        ys = [q[1] for q in qs]
+        angles = [q[2] for q in qs]
+        collision = False
+        for j in range(len(xs)):
+            gridx = int((xs[j] + 10.0) / 0.1)
+            gridy = int((ys[j] + 10.0) / 0.1)
+
+            if env[gridx][gridy] < 0.2:
+                collision = True
+                break
+        if(collision==False):
+            newOpState=  np.zeros((i+len(xs),2))
+            newOpRot=  np.zeros((i+len(xs),2))
+            newOpState[:i,:] = opState[:i,:]
+            newOpRot[:i,:] = opRot[:i,:]
+            for j in range(len(xs)):
+                rpx = xs[j]
+                rpy = ys[j]
+                ryaw = angles[j]
+                newOpState[i+j][0] = rpx
+                newOpState[i+j][1] = rpy
+                newOpRot[i+j][0] = math.cos(ryaw)
+                newOpRot[i+j][1] = math.sin(ryaw)
+            return newOpState, newOpRot
+    return opState, opRot

deepPathPlan/PathNet/train_ours.py

@@ -0,0 +1,273 @@
+import os
+import argparse
+import torch
+import torch.nn as nn
+import os
+from data_loader import pDataset 
+from network import   trajFCNet
+from einops import rearrange
+from tqdm import tqdm
+from torch.utils.tensorboard import SummaryWriter  
+from torch.utils.data import DataLoader, random_split
+from loss import  NormalizeArcLoss,NonholoLoss,UniforArcLoss,CurvatureLoss,FullShapeCollisionLoss
+from course import read_wei_course
+
+def test_net(mpnet, val_loader, output_logfile, wei_arc, smooLoss, wei_pos, mseLoss, wei_rot,
+             wei_hol, holoLoss, wei_uni, uniLoss,wei_cur,curLoss, wei_rsm,rotsLoss,wei_safety,safeLoss,
+             wei_anchors,crossentropyloss,compt_count):
+	mpnet.eval()
+	avg_loss=0
+	avg_posloss = 0
+	avg_rotloss = 0
+	avg_arcloss = 0
+	avg_holloss = 0
+	avg_uniloss = 0
+	avg_curloss = 0
+	avg_safeloss = 0
+	avg_rsmloss = 0
+	avg_anchorsloss = 0
+	step_num = 0
+	for batch in val_loader:
+		with torch.no_grad():
+			input = batch[0].cuda()
+			labelops = batch[2].cuda()
+			labelrot = batch[3].cuda()
+			anchors = batch[4].cuda()
+			
+			labelgrids= rearrange(anchors, 'b c h w-> b c (h w)')
+			labelgrids = rearrange(labelgrids, 'b c l-> (b c) l')
+			labelgrids = torch.argmax(labelgrids, dim=1)
+
+			opState, opRot,prbmap, predicted_anchors = mpnet(input, labelops,labelrot, anchors)
+
+
+
+
+			arcloss = wei_arc * smooLoss(opState, labelops)
+			posloss = wei_pos * torch.sqrt(mseLoss(opState, labelops))
+			rotloss = wei_rot * torch.sqrt(mseLoss(opRot, labelrot))
+			holloss = wei_hol * holoLoss(opState,opRot[:,1:,:])
+			unilos = wei_uni * uniLoss(opState,labelops)
+			curloss = wei_cur * curLoss(opState, opRot)
+
+			rsmloss = wei_rsm * rotsLoss(opRot, labelrot)
+			safetyloss = wei_safety * safeLoss(opState, opRot, input)
+			
+			gridloss = wei_anchors *  crossentropyloss(predicted_anchors,labelgrids)
+
+
+			loss = posloss + arcloss + holloss + unilos + rotloss + curloss + rsmloss + safetyloss+gridloss
+			avg_posloss = avg_posloss + posloss.item()
+			avg_arcloss = avg_arcloss + arcloss.item()
+			avg_holloss = avg_holloss + holloss.item()
+			avg_uniloss = avg_uniloss + unilos.item()
+			avg_rotloss = avg_rotloss + rotloss.item()
+			avg_curloss = avg_curloss + curloss.item()
+			avg_rsmloss = avg_rsmloss + rsmloss.item()
+			avg_safeloss = avg_safeloss + safetyloss.item()
+			avg_anchorsloss = avg_anchorsloss + gridloss.item()
+			avg_loss=avg_loss+loss.item()
+			step_num += 1
+
+
+	output_logfile.write(f"--step: {compt_count}\n")
+	output_logfile.write(f"--average loss: {avg_loss/step_num}\n")
+	output_logfile.write(f"--average arcloss: {avg_arcloss/step_num}\n")
+	output_logfile.write(f"--average holloss: {avg_holloss/step_num}\n")
+	output_logfile.write(f"--average uniloss: {avg_uniloss/step_num}\n")
+	output_logfile.write(f"--average posloss: {avg_posloss/step_num}\n")
+	output_logfile.write(f"--average rotloss: {avg_rotloss/step_num}\n")
+	output_logfile.write(f"--average curloss: {avg_curloss/step_num}\n")
+	output_logfile.write(f"--average rsmloss: {avg_rsmloss/step_num}\n")
+	output_logfile.write(f"--average safeloss: {avg_safeloss/step_num}\n")
+	output_logfile.write(f"--average anchors: {avg_anchorsloss/step_num}\n")
+
+def main(args):
+	# Create model directory
+	if not os.path.exists(args.model_path):
+		os.makedirs(args.model_path)
+  
+	# 0. Build the models
+	mpnet = trajFCNet(4,200,7,l=1.2,use_groundTruth=False)
+	model_path='model.pkl'
+	# mpnet.load_state_dict(torch.load("./models/"+model_path))
+	if torch.cuda.is_available():
+		mpnet.cuda()
+	
+	# 1. Loss and Optimizer
+	mseLoss = nn.MSELoss()
+	smooLoss = NormalizeArcLoss()
+	rotsLoss = NormalizeArcLoss()
+	holoLoss = NonholoLoss()
+	uniLoss = UniforArcLoss()
+	curLoss = CurvatureLoss()
+	safeLoss = FullShapeCollisionLoss()
+	crossentropyloss=nn.CrossEntropyLoss()
+	optimizer = torch.optim.Adam(mpnet.parameters(), lr=args.learning_rate)	
+	dataset = pDataset()
+	writer = SummaryWriter('./path/to/log')
+	# 2. Split into train / validation partitions
+	n_val = int(len(dataset) * 0.001)
+	n_train = len(dataset) - n_val
+	train_set, val_set = random_split(dataset, [n_train, n_val], generator=torch.Generator().manual_seed(1))
+	# 3. Create data loaders
+	loader_args = dict(batch_size=args.batch_size, num_workers=8, pin_memory=True)
+	train_loader = DataLoader(train_set, shuffle=True, **loader_args)
+	val_loader = DataLoader(val_set, shuffle=False, drop_last=True, **loader_args)
+	# Train the Models
+	print("num_train:", n_train)
+	
+
+
+	step_count, compt_count = 0, 0
+	wei_arc = torch.tensor(0.0)
+	wei_uni = torch.tensor(0.0)
+	wei_pos = torch.tensor(0.0)
+	wei_hol = torch.tensor(0.0)
+	wei_rot = torch.tensor(0.0)
+	wei_cur = torch.tensor(0.0)
+	wei_safety = torch.tensor(0.0)
+	wei_rsm = torch.tensor(0.0)
+	wei_anchors = torch.tensor(0.0)
+
+
+	
+	output_logfile = open("./models/"+model_path+'.txt', 'w')  
+	output_logfile2 = open("./models/"+model_path+'Step.txt', 'w')  
+	for epoch in range(0, args.num_epochs):
+		if(epoch < 2):
+			for param_group in optimizer.param_groups:
+				param_group['lr'] = 1.0e-4
+		else:
+			for param_group in optimizer.param_groups:
+				param_group['lr'] = 1.0e-5
+		print("epoch" + str(epoch) + ' lr: ' + str(optimizer.state_dict()['param_groups'][0]['lr']))
+		output_logfile.write("epoch" + str(epoch) + ' lr: ' + str(optimizer.state_dict()['param_groups'][0]['lr'])+'\n')
+		wei_anchors, wei_arc, wei_uni, wei_pos, wei_hol, wei_rot, wei_cur, wei_safety, wei_rsm = read_wei_course(epoch)
+		print('wei_arc: ', wei_arc)
+		print('wei_uni: ', wei_uni)
+		print('wei_pos: ', wei_pos)
+		print('wei_hol: ', wei_hol)
+		print('wei_rot: ', wei_rot)
+		print('wei_cur: ', wei_cur)
+		print('wei_safety: ',wei_safety)
+		print('wei_rsm: ', wei_rsm)
+		print('wei aors: ',	wei_anchors)	
+		output_logfile.write('wei_arc: ' + str(wei_arc) + '\n')
+		output_logfile.write('wei_uni: ' + str(wei_uni) + '\n')
+		output_logfile.write('wei_pos: ' + str(wei_pos) + '\n')
+		output_logfile.write('wei_hol: ' + str(wei_hol) + '\n')
+		output_logfile.write('wei_rot: ' + str(wei_rot) + '\n')
+		output_logfile.write('wei_cur: ' + str(wei_cur) + '\n')
+		output_logfile.write('wei_safety: ' + str(wei_safety) + '\n')
+		output_logfile.write('wei_rsm: ' + str(wei_rsm) + '\n')
+		output_logfile.write('wei_aors: ' + str(wei_anchors) + '\n')
+		avg_loss=0
+		avg_posloss = 0
+		avg_rotloss = 0
+		avg_arcloss = 0
+		avg_holloss = 0
+		avg_uniloss = 0
+		avg_curloss = 0
+		avg_safeloss = 0
+		avg_rsmloss = 0
+		avg_anchorsloss = 0.0
+		step_num = 0
+		mpnet.train()
+
+
+		with tqdm(total=n_train, desc=f'Epoch {epoch}/{args.num_epochs}', unit='data') as pbar:
+			for batch in train_loader:
+				mpnet.train()
+				input = batch[0].cuda()
+				raw_env = batch[1].cuda()
+				labelops = batch[2].cuda()
+				labelrot = batch[3].cuda()
+				anchors = batch[4].cuda()
+				labelgrids= rearrange(anchors, 'b c h w-> b c (h w)')
+				labelgrids = rearrange(labelgrids, 'b c l-> (b c) l')
+				labelgrids = torch.argmax(labelgrids, dim=1)
+				optimizer.zero_grad()
+				opState, opRot,prbmap, predicted_anchors = mpnet(input, labelops,labelrot, anchors)
+
+				arcloss = wei_arc * smooLoss(opState, labelops)
+				posloss = wei_pos * torch.sqrt(mseLoss(opState, labelops))
+				rotloss = wei_rot * torch.sqrt(mseLoss(opRot, labelrot))
+				holloss = wei_hol * holoLoss(opState,opRot[:,1:,:])
+				unilos = wei_uni * uniLoss(opState,labelops)
+				curloss = wei_cur * curLoss(opState, opRot)
+				rsmloss = wei_rsm * rotsLoss(opRot, labelrot)
+				safetyloss = wei_safety * safeLoss(opState, opRot, input)
+
+				gridloss = wei_anchors *  crossentropyloss(predicted_anchors,labelgrids)
+				loss = posloss + arcloss + holloss + unilos + rotloss + curloss + rsmloss + safetyloss+gridloss
+				avg_posloss = avg_posloss + posloss.item()
+				avg_arcloss = avg_arcloss + arcloss.item()
+				avg_holloss = avg_holloss + holloss.item()
+				avg_uniloss = avg_uniloss + unilos.item()
+				avg_rotloss = avg_rotloss + rotloss.item()
+				avg_curloss = avg_curloss + curloss.item()
+				avg_rsmloss = avg_rsmloss + rsmloss.item()
+				avg_safeloss = avg_safeloss + safetyloss.item()
+				avg_anchorsloss = avg_anchorsloss + gridloss.item()
+				avg_loss=avg_loss+loss.item()
+				step_num += 1
+				loss.backward()
+				optimizer.step()
+				pbar.update(input.shape[0])
+				pbar.set_postfix(**{'loss (batch)': loss.item()})
+				writer.add_scalar('mseloss', loss.item(), step_count)
+				step_count += 1
+    
+				if(epoch>=4):
+					compt_count+=1
+					if compt_count%100 == 0:
+						test_net(mpnet, val_loader, output_logfile2, wei_arc, smooLoss, wei_pos, mseLoss, wei_rot,
+             wei_hol, holoLoss, wei_uni, uniLoss,wei_cur,curLoss, wei_rsm,rotsLoss,wei_safety,safeLoss,
+             wei_anchors,crossentropyloss,compt_count)
+
+		
+
+		
+		
+
+
+
+		print ("--average loss: ", avg_loss/step_num)
+		print ("--average arcloss: ", avg_arcloss/step_num)
+		print ("--average holloss: ", avg_holloss/step_num)
+		print ("--average uniloss: ", avg_uniloss/step_num)
+		print ("--average posloss: ", avg_posloss/step_num)
+		print ("--average rotloss: ", avg_rotloss/step_num)
+		print ("--average curloss: ", avg_curloss/step_num)
+		print ("--average rsmloss: ", avg_rsmloss/step_num)
+		print ("--average safeoss: ", avg_safeloss/step_num)
+		print ("--average anchors: ", avg_anchorsloss/step_num)
+  
+		output_logfile.write('--average loss: ' + str(avg_loss/step_num) + '\n')
+		output_logfile.write('--average arcloss: ' + str(avg_arcloss/step_num) + '\n')
+		output_logfile.write('--average holloss: ' + str(avg_holloss/step_num) + '\n')
+		output_logfile.write('--average uniloss: ' + str(avg_uniloss/step_num) + '\n')
+		output_logfile.write('--average posloss: ' + str(avg_posloss/step_num) + '\n')
+		output_logfile.write('--average rotloss: ' + str(avg_rotloss/step_num) + '\n')
+		output_logfile.write('--average curloss: ' + str(avg_curloss/step_num) + '\n')
+		output_logfile.write('--average rsmloss: ' + str(avg_rsmloss/step_num) + '\n')
+		output_logfile.write('--average safeloss: ' + str(avg_safeloss/step_num) + '\n')
+		output_logfile.write('--average anchors: ' + str(avg_anchorsloss/step_num) + '\n')
+
+
+		torch.save(mpnet.state_dict(),os.path.join(args.model_path,model_path))
+	
+if __name__ == '__main__':
+	parser = argparse.ArgumentParser()
+	parser.add_argument('--model_path', type=str, default='./models/',help='path for saving trained models')
+	# Model parameters
+	parser.add_argument('--num_epochs', '-e', type=int, default=14)
+	parser.add_argument('--batch_size','-b', type=int, default=32)
+	parser.add_argument('--learning_rate','-l', type=float, default=1e-4)
+	args = parser.parse_args()
+	print(args)
+	main(args)
+
+
+

deepPathPlan/PathNet/train_resnet.py

@@ -0,0 +1,262 @@
+import os
+import argparse
+import torch
+import torch.nn as nn
+import os
+from data_loader import pDataset 
+from einops import rearrange
+from tqdm import tqdm
+from torch.utils.tensorboard import SummaryWriter  
+from torch.utils.data import DataLoader, random_split
+from loss import NormalizeArcLoss,NonholoLoss,UniforArcLoss,CurvatureLoss,FullShapeCollisionLoss
+from course import read_wei_course
+from resnet import resnet152
+
+def test_net(mpnet, val_loader, output_logfile, wei_arc, smooLoss, wei_pos, mseLoss, wei_rot,
+			wei_hol, holoLoss, wei_uni, uniLoss,wei_cur,curLoss, wei_rsm,rotsLoss,wei_safety,safeLoss,compt_count):
+		mpnet.eval()
+		avg_loss=0
+		avg_posloss = 0
+		avg_rotloss = 0
+		avg_arcloss = 0
+		avg_holloss = 0
+		avg_uniloss = 0
+		avg_curloss = 0
+		avg_safeloss = 0
+		avg_rsmloss = 0
+		step_num = 0
+  
+		for batch in val_loader:
+			with torch.no_grad():
+				input = batch[0].cuda()
+				raw_env = batch[1].cuda()
+				labelops = batch[2].cuda()
+				labelrot = batch[3].cuda()
+				
+    
+				opState, opRot = mpnet(input, labelops,labelrot)
+
+    
+
+				arcloss = wei_arc * smooLoss(opState, labelops)
+				posloss = wei_pos * torch.sqrt(mseLoss(opState, labelops))
+				rotloss = wei_rot * torch.sqrt(mseLoss(opRot, labelrot))
+				holloss = wei_hol * holoLoss(opState,opRot[:,1:,:])
+				unilos = wei_uni * uniLoss(opState,labelops)
+				curloss = wei_cur * curLoss(opState, opRot)
+    
+				rsmloss = wei_rsm * rotsLoss(opRot, labelrot)
+				safetyloss = wei_safety * safeLoss(opState, opRot, input)
+				
+
+
+				loss = posloss + arcloss + holloss + unilos + rotloss + curloss + rsmloss + safetyloss
+				avg_posloss = avg_posloss + posloss.item()
+				avg_arcloss = avg_arcloss + arcloss.item()
+				avg_holloss = avg_holloss + holloss.item()
+				avg_uniloss = avg_uniloss + unilos.item()
+				avg_rotloss = avg_rotloss + rotloss.item()
+				avg_curloss = avg_curloss + curloss.item()
+				avg_rsmloss = avg_rsmloss + rsmloss.item()
+				avg_safeloss = avg_safeloss + safetyloss.item()
+				avg_loss=avg_loss+loss.item()
+				step_num += 1
+
+  
+		output_logfile.write(f"--step: {compt_count}\n")
+		output_logfile.write(f"--average loss: {avg_loss/step_num}\n")
+		output_logfile.write(f"--average arcloss: {avg_arcloss/step_num}\n")
+		output_logfile.write(f"--average holloss: {avg_holloss/step_num}\n")
+		output_logfile.write(f"--average uniloss: {avg_uniloss/step_num}\n")
+		output_logfile.write(f"--average posloss: {avg_posloss/step_num}\n")
+		output_logfile.write(f"--average rotloss: {avg_rotloss/step_num}\n")
+		output_logfile.write(f"--average curloss: {avg_curloss/step_num}\n")
+		output_logfile.write(f"--average rsmloss: {avg_rsmloss/step_num}\n")
+		output_logfile.write(f"--average safeloss: {avg_safeloss/step_num}\n")
+
+def main(args):
+	# Create model directory
+	if not os.path.exists(args.model_path):
+		os.makedirs(args.model_path)
+  
+	# 0. Build the models
+	mpnet = resnet152(7)
+	model_path='resnet152.pkl'
+	# mpnet.load_state_dict(torch.load("./models/"+model_path))
+	if torch.cuda.is_available():
+		mpnet.cuda()
+
+	# 1. Loss and Optimizer
+	mseLoss = nn.MSELoss()
+	smooLoss = NormalizeArcLoss()
+	rotsLoss = NormalizeArcLoss()
+	holoLoss = NonholoLoss()
+	uniLoss = UniforArcLoss()
+	curLoss = CurvatureLoss()
+	safeLoss = FullShapeCollisionLoss()
+	optimizer = torch.optim.Adam(mpnet.parameters(), lr=args.learning_rate)	
+	dataset = pDataset()
+	writer = SummaryWriter('./path/to/log')
+	# 2. Split into train / validation partitions
+	n_val = int(len(dataset) * 0.001)
+	n_train = len(dataset) - n_val
+	train_set, val_set = random_split(dataset, [n_train, n_val], generator=torch.Generator().manual_seed(1))
+	# 3. Create data loaders
+	loader_args = dict(batch_size=args.batch_size, num_workers=8, pin_memory=True)
+	train_loader = DataLoader(train_set, shuffle=True, **loader_args)
+	val_loader = DataLoader(val_set, shuffle=False, drop_last=True, **loader_args)
+	# Train the Models
+	print(n_train)
+	
+
+
+	step_count, compt_count = 0, 0
+	wei_arc = torch.tensor(0.0)
+	wei_uni = torch.tensor(0.0)
+	wei_pos = torch.tensor(0.0)
+	wei_hol = torch.tensor(0.0)
+	wei_rot = torch.tensor(0.0)
+	wei_cur = torch.tensor(0.0)
+	wei_safety = torch.tensor(0.0)
+	wei_rsm = torch.tensor(0.0)
+
+
+	
+	output_logfile = open("./models/"+model_path+'.txt', 'w')
+	output_logfile2 = open("./models/"+model_path+'Step.txt', 'w')
+	for epoch in range(0, args.num_epochs):
+		if(epoch < 2):
+			for param_group in optimizer.param_groups:
+				param_group['lr'] = 1.0e-4
+		else:
+			for param_group in optimizer.param_groups:
+				param_group['lr'] = 1.0e-5
+		print("epoch" + str(epoch) + ' lr: ' + str(optimizer.state_dict()['param_groups'][0]['lr']))
+		output_logfile.write("epoch" + str(epoch) + ' lr: ' + str(optimizer.state_dict()['param_groups'][0]['lr'])+'\n')
+		_, wei_arc, wei_uni, wei_pos, wei_hol, wei_rot, wei_cur, wei_safety, wei_rsm = read_wei_course(epoch)
+		
+		
+
+
+  
+  
+  
+  
+		print('wei_arc: ', wei_arc)
+		print('wei_uni: ', wei_uni)
+		print('wei_pos: ', wei_pos)
+		print('wei_hol: ', wei_hol)
+		print('wei_rot: ', wei_rot)
+		print('wei_cur: ', wei_cur)
+		print('wei_safety: ',wei_safety)
+		print('wei_rsm: ', wei_rsm)
+		output_logfile.write('wei_arc: ' + str(wei_arc) + '\n')
+		output_logfile.write('wei_uni: ' + str(wei_uni) + '\n')
+		output_logfile.write('wei_pos: ' + str(wei_pos) + '\n')
+		output_logfile.write('wei_hol: ' + str(wei_hol) + '\n')
+		output_logfile.write('wei_rot: ' + str(wei_rot) + '\n')
+		output_logfile.write('wei_cur: ' + str(wei_cur) + '\n')
+		output_logfile.write('wei_safety: ' + str(wei_safety) + '\n')
+		output_logfile.write('wei_rsm: ' + str(wei_rsm) + '\n')
+
+
+		avg_loss=0
+		avg_posloss = 0
+		avg_rotloss = 0
+		avg_arcloss = 0
+		avg_holloss = 0
+		avg_uniloss = 0
+		avg_curloss = 0
+		avg_safeloss = 0
+		avg_rsmloss = 0
+		step_num = 0
+		mpnet.train()
+
+
+		with tqdm(total=n_train, desc=f'Epoch {epoch}/{args.num_epochs}', unit='data') as pbar:
+			for batch in train_loader:
+				mpnet.train()
+				input = batch[0].cuda()
+				raw_env = batch[1].cuda()
+				labelops = batch[2].cuda()
+				labelrot = batch[3].cuda()
+    
+				optimizer.zero_grad()
+				opState, opRot= mpnet(input, labelops,labelrot)
+
+    
+    
+
+				arcloss = wei_arc * smooLoss(opState, labelops)
+				posloss = wei_pos * torch.sqrt(mseLoss(opState, labelops))
+				rotloss = wei_rot * torch.sqrt(mseLoss(opRot, labelrot))
+				holloss = wei_hol * holoLoss(opState,opRot[:,1:,:])
+				unilos = wei_uni * uniLoss(opState,labelops)
+				curloss = wei_cur * curLoss(opState, opRot)
+    
+				rsmloss = wei_rsm * rotsLoss(opRot, labelrot)
+				safetyloss = wei_safety * safeLoss(opState, opRot, input)
+    
+    
+
+				loss = posloss + arcloss + holloss + unilos + rotloss + curloss + rsmloss + safetyloss
+				avg_posloss = avg_posloss + posloss.item()
+				avg_arcloss = avg_arcloss + arcloss.item()
+				avg_holloss = avg_holloss + holloss.item()
+				avg_uniloss = avg_uniloss + unilos.item()
+				avg_rotloss = avg_rotloss + rotloss.item()
+				avg_curloss = avg_curloss + curloss.item()
+				avg_rsmloss = avg_rsmloss + rsmloss.item()
+				avg_safeloss = avg_safeloss + safetyloss.item()
+				avg_loss=avg_loss+loss.item()
+				
+
+				step_num += 1
+				loss.backward()
+
+				optimizer.step()
+				pbar.update(input.shape[0])
+				pbar.set_postfix(**{'loss (batch)': loss.item()})
+				writer.add_scalar('mseloss', loss.item(), step_count)
+				step_count += 1
+				if(epoch>=4):
+					compt_count+=1
+					if compt_count%100 == 0:
+						test_net(mpnet, val_loader, output_logfile2, wei_arc, smooLoss, wei_pos, mseLoss, wei_rot,
+			wei_hol, holoLoss, wei_uni, uniLoss,wei_cur,curLoss, wei_rsm,rotsLoss,wei_safety,safeLoss,compt_count)
+		print ("--average loss: ", avg_loss/step_num)
+		print ("--average arcloss: ", avg_arcloss/step_num)
+		print ("--average holloss: ", avg_holloss/step_num)
+		print ("--average uniloss: ", avg_uniloss/step_num)
+		print ("--average posloss: ", avg_posloss/step_num)
+		print ("--average rotloss: ", avg_rotloss/step_num)
+		print ("--average curloss: ", avg_curloss/step_num)
+		print ("--average rsmloss: ", avg_rsmloss/step_num)
+		print ("--average safeoss: ", avg_safeloss/step_num)
+  
+		output_logfile.write('--average loss: ' + str(avg_loss/step_num) + '\n')
+		output_logfile.write('--average arcloss: ' + str(avg_arcloss/step_num) + '\n')
+		output_logfile.write('--average holloss: ' + str(avg_holloss/step_num) + '\n')
+		output_logfile.write('--average uniloss: ' + str(avg_uniloss/step_num) + '\n')
+		output_logfile.write('--average posloss: ' + str(avg_posloss/step_num) + '\n')
+		output_logfile.write('--average rotloss: ' + str(avg_rotloss/step_num) + '\n')
+		output_logfile.write('--average curloss: ' + str(avg_curloss/step_num) + '\n')
+		output_logfile.write('--average rsmloss: ' + str(avg_rsmloss/step_num) + '\n')
+		output_logfile.write('--average safeloss: ' + str(avg_safeloss/step_num) + '\n')
+  
+
+		torch.save(mpnet.state_dict (),os.path.join(args.model_path,model_path))
+	
+if __name__ == '__main__':
+	parser = argparse.ArgumentParser()
+	parser.add_argument('--model_path', type=str, default='./models/',help='path for saving trained models')
+	# Model parameters
+	parser.add_argument('--num_epochs', '-e', type=int, default=14)
+	parser.add_argument('--batch_size','-b', type=int, default=64)
+	parser.add_argument('--learning_rate','-l', type=float, default=1e-5)
+	args = parser.parse_args()
+	print(args)
+	main(args)
+
+
+

deepPathPlan/PathNet/visualizer_tojit.py

@@ -0,0 +1,136 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import argparse
+import torch
+from resshep import reedshep_process
+import numpy as np
+from network import  trajFCNet
+from data_loader import geom2pix,get_encoder_input,geom2pix
+import cv2
+import os
+    
+
+
+def main(args):
+
+
+    
+    mpnet = trajFCNet(4,200,7,l=1.2,use_groundTruth=False)
+    model_path='model.pkl'
+    mpnet.load_state_dict(torch.load("./models/"+model_path))
+
+    if torch.cuda.is_available():
+        mpnet.cuda()
+
+
+    mpnet.eval()
+    
+    current_dir = os.path.dirname(os.path.abspath(__file__))
+    filepath = os.path.join(os.path.dirname(os.path.dirname(current_dir)), "totalData/ruins1")
+    for eidx in range(29000,30001):
+        if(not os.path.exists(filepath+'/e'+str(eidx)+'/path1.dat')):
+            continue
+        temp=np.fromfile(filepath+'/obcs/obc'+str(eidx)+'.dat')
+        env=temp.reshape(15000,2)
+        path = np.fromfile(filepath+'/e'+str(eidx)+'/path1.dat')
+    
+        plt.figure(figsize=(19.2, 10.8))
+
+        plt.scatter(env[:,0], env[:,1], c='black', marker='o', label="ground truth")
+        ax = plt.gca()
+
+
+        raw_path = np.zeros([200,2], float)
+        raw_theta = np.zeros(200, float)
+        raw_t = np.zeros(200, float)
+        raw_v = np.zeros(200, float)
+        raw_c = np.zeros(200, float)
+        
+
+        for j in range(200):
+            raw_t[j] = path[10+j*6]
+            raw_path[j,0] = path[10+j*6+1]
+            raw_path[j,1] = path[10+j*6+2]
+            raw_theta[j] = path[10+j*6+3]
+            raw_v[j] = path[10+j*6+4]
+            raw_c[j] = path[10+j*6+5] 
+        # plt.quiver(raw_path[:,0], raw_path[:,1], 0.1 * np.cos(raw_theta), 0.1*np.sin(raw_theta),color='b',width=0.001, scale=5.0)
+        plt.scatter(raw_path[:,0], raw_path[:,1], c='blue', marker='o',s=5, label="ground truth")
+        # plt.plot(raw_path[:,0], raw_path[:,1],color='blue', marker='o', linestyle=':', linewidth=2, markersize=1)
+        
+        fs = cv2.FileStorage(filepath+'/esdfmaps/'+str(eidx)+'.xml', cv2.FILE_STORAGE_READ)
+        fn = fs.getNode("instance")
+        image = fn.mat()
+        raw_env = image#H*W
+        raw_env = np.expand_dims(raw_env, 0)#H*W
+        raw_env = np.where(raw_env > 0.2, 1, 0)
+        # #free is 1, obs = 0
+        path = np.fromfile(filepath+'/e'+str(eidx)+'/path1.dat')
+        input = path[:10].reshape(2,5)
+        label = path[10:].reshape(200, 6)
+        env = np.expand_dims(image, 0)#H*W
+
+        
+        goalpos = geom2pix(input[1][0:2])
+        startpos = geom2pix(input[0][0:2])
+        data = get_encoder_input(env, goalpos, input[1][2], startpos, input[0][2])
+
+        label_opDir = np.zeros((200,2))
+        label_opDir[:,0] = -np.sin(label[:, 3])
+        label_opDir[:,1] = np.cos(label[:, 3])
+        label_opState = label[:, 1:3]
+        label_Rot = np.zeros((200,2))
+        label_Rot[:,0] = np.cos(label[:, 3])
+        label_Rot[:,1] = np.sin(label[:, 3])
+
+        label_grid = np.floor((label_opState+10.0)/1.0).astype(int)#200*2
+        labelanchors = np.zeros((200,20,20))
+        index = [i for i in range(200)]
+        labelanchors[index, label_grid[:,0], label_grid[:,1]] = 1
+
+        data = torch.as_tensor(data.copy()).float().unsqueeze(0).contiguous().cuda()
+        label_opState = torch.as_tensor(label_opState.copy()).float().contiguous().unsqueeze(0).cuda()
+        label_Rot = torch.as_tensor(label_Rot.copy()).float().unsqueeze(0).contiguous().cuda()
+        labelanchors = torch.as_tensor(labelanchors.copy()).float().unsqueeze(0).contiguous().cuda()
+        
+        
+        #save model
+        mpnet.half()
+        data = data.half()
+        label_opState = label_opState.half()
+        label_Rot = label_Rot.half()
+        labelanchors = labelanchors.half()
+        opState, opRot,anchors,_= mpnet(data, label_opState,label_Rot,  labelanchors)
+        CAB_traced_script_module = torch.jit.trace(mpnet, (data, label_opState,label_Rot, labelanchors))
+        CAB_traced_script_module.save("./models/model.pt")
+    
+
+
+
+        opState=opState.data.cpu().numpy()[0]
+        opRot = opRot.data.cpu().numpy()[0]
+        env = env[0]
+        opState, opRot = reedshep_process(opState, opRot, env)
+
+
+
+
+
+        for i in range(opState.shape[0]):
+            plt.arrow(opState[i,0], opState[i,1],0.05*opRot[i,0] ,0.05*opRot[i,1], width=0.001, color='red')
+        plt.plot(opState[:,0], opState[:,1],color='green', marker='o', linestyle=':', linewidth=2, markersize=1)
+        plt.scatter(opState[:,0], opState[:,1], c='red', marker='o',s=5, label="planner")
+
+        plt.xlim((-11, 11))
+        plt.ylim((-11, 11))
+    
+        plt.show()
+        plt.close()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--path_file','-p', type=str, default='./data/freeEnv/path1002.dat')
+    args = parser.parse_args()
+    print(args)
+    main(args)

deepPathPlan/models/model.pkl.txt

@@ -0,0 +1,280 @@
+epoch0 lr: 0.0001
+wei_arc: tensor(0.)
+wei_uni: tensor(0.)
+wei_pos: tensor(5.)
+wei_hol: tensor(0.)
+wei_rot: tensor(5.)
+wei_cur: tensor(0.)
+wei_safety: tensor(0.)
+wei_rsm: tensor(0.)
+wei_aors: tensor(1.)
+--average loss: 8.06198415877395
+--average arcloss: 0.0
+--average holloss: 0.0
+--average uniloss: 0.0
+--average posloss: 4.5134706898332615
+--average rotloss: 1.5049493015374618
+--average curloss: 0.0
+--average rsmloss: 0.0
+--average safeloss: 0.0
+--average anchors: 2.043564165656325
+epoch1 lr: 0.0001
+wei_arc: tensor(0.0500)
+wei_uni: tensor(10.)
+wei_pos: tensor(5.)
+wei_hol: tensor(10.)
+wei_rot: tensor(5.)
+wei_cur: tensor(0.)
+wei_safety: tensor(0.)
+wei_rsm: tensor(0.)
+wei_aors: tensor(1.)
+--average loss: 5.019177352205206
+--average arcloss: 0.0038839441434178044
+--average holloss: 0.05458394393475797
+--average uniloss: 0.010759122569141613
+--average posloss: 2.6638446151785975
+--average rotloss: 1.0360660213038153
+--average curloss: 0.0
+--average rsmloss: 0.0
+--average safeloss: 0.0
+--average anchors: 1.2500397054523207
+epoch2 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(100.)
+wei_rot: tensor(5.)
+wei_cur: tensor(5.)
+wei_safety: tensor(0.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 4.68895891328937
+--average arcloss: 0.008183894988604497
+--average holloss: 0.13029613115596253
+--average uniloss: 0.08588701316743295
+--average posloss: 1.8579194024380217
+--average rotloss: 0.9269288579124396
+--average curloss: 0.012219247905045872
+--average rsmloss: 0.7044051234239879
+--average safeloss: 0.0
+--average anchors: 0.9631192421936906
+epoch3 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(200.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(0.)
+wei_rsm: tensor(0.2500)
+wei_aors: tensor(1.)
+--average loss: 4.745125634601187
+--average arcloss: 0.026877454563553654
+--average holloss: 0.18418890316184103
+--average uniloss: 0.10297594526055419
+--average posloss: 1.884187303124454
+--average rotloss: 0.9198017363007683
+--average curloss: 0.3988196368315505
+--average rsmloss: 0.25973377930722596
+--average safeloss: 0.0
+--average anchors: 0.9685408772915417
+epoch4 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 19.731002670851915
+--average arcloss: 0.20979579106313714
+--average holloss: 1.5253499871130065
+--average uniloss: 0.15299846975775713
+--average posloss: 3.273296459084839
+--average rotloss: 1.174490636924856
+--average curloss: 0.9095049503192606
+--average rsmloss: 1.3547295072078238
+--average safeloss: 9.375466897855645
+--average anchors: 1.755369973167617
+epoch5 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 17.35771486355118
+--average arcloss: 0.24915402417057045
+--average holloss: 1.4544789069733575
+--average uniloss: 0.16270333445734447
+--average posloss: 3.7460585601266674
+--average rotloss: 1.2475770550933594
+--average curloss: 0.8845004071528924
+--average rsmloss: 1.265900521613464
+--average safeloss: 6.43820409340887
+--average anchors: 1.909137958277162
+epoch6 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 17.764582541745977
+--average arcloss: 0.25872511512643176
+--average holloss: 1.4364092676743947
+--average uniloss: 0.1724707286891623
+--average posloss: 4.201128017491688
+--average rotloss: 1.3279298406702333
+--average curloss: 0.8454181791530256
+--average rsmloss: 1.226171281884782
+--average safeloss: 6.226113324041587
+--average anchors: 2.0702167910009495
+epoch7 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 15.785695028971995
+--average arcloss: 0.2375121480664727
+--average holloss: 1.2241923052933459
+--average uniloss: 0.16861708735277775
+--average posloss: 3.8096637190050124
+--average rotloss: 1.2856888858784017
+--average curloss: 0.8123444865545019
+--average rsmloss: 1.1663268719570383
+--average safeloss: 5.160833306864611
+--average anchors: 1.9205162199513073
+epoch8 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 15.425916258546668
+--average arcloss: 0.22056228557643467
+--average holloss: 1.132496457373267
+--average uniloss: 0.17018246243297694
+--average posloss: 3.880802043723639
+--average rotloss: 1.3023101880088956
+--average curloss: 0.7612168637050438
+--average rsmloss: 1.1109563653564885
+--average safeloss: 4.892489986450564
+--average anchors: 1.9548996071075917
+epoch9 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 14.73867193576967
+--average arcloss: 0.2298927892499185
+--average holloss: 1.0485475392665728
+--average uniloss: 0.16941489148385178
+--average posloss: 3.8030924390932785
+--average rotloss: 1.295587372976916
+--average curloss: 0.7343074503939482
+--average rsmloss: 1.085112362489114
+--average safeloss: 4.461452936496554
+--average anchors: 1.9112641553122238
+epoch10 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 14.350036520212603
+--average arcloss: 0.24931309405859187
+--average holloss: 1.0164944709404795
+--average uniloss: 0.17092974116608498
+--average posloss: 3.7293495974198585
+--average rotloss: 1.2872617204054895
+--average curloss: 0.6995111909340379
+--average rsmloss: 1.0526712644850285
+--average safeloss: 4.251338293904919
+--average anchors: 1.8931671491686801
+epoch11 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 14.183135204845005
+--average arcloss: 0.2666033199641007
+--average holloss: 0.9918856180740813
+--average uniloss: 0.17218418529271629
+--average posloss: 3.803230342014174
+--average rotloss: 1.2902717933210412
+--average curloss: 0.6645574028382917
+--average rsmloss: 1.032610342568397
+--average safeloss: 4.078076377394617
+--average anchors: 1.8837158215861363
+epoch12 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 14.16942340013811
+--average arcloss: 0.2577934196032968
+--average holloss: 0.9688295220033568
+--average uniloss: 0.1727902776355446
+--average posloss: 3.8484853654346804
+--average rotloss: 1.2992724725367106
+--average curloss: 0.6492055289991424
+--average rsmloss: 0.9965988802249274
+--average safeloss: 4.0513577184188625
+--average anchors: 1.9250902141703516
+epoch13 lr: 1e-05
+wei_arc: tensor(0.5000)
+wei_uni: tensor(100.)
+wei_pos: tensor(5.)
+wei_hol: tensor(500.)
+wei_rot: tensor(5.)
+wei_cur: tensor(500.)
+wei_safety: tensor(500.)
+wei_rsm: tensor(0.5000)
+wei_aors: tensor(1.)
+--average loss: 14.366591612820406
+--average arcloss: 0.2929780938732128
+--average holloss: 0.9725297783308605
+--average uniloss: 0.1773672202894107
+--average posloss: 3.9593081795852783
+--average rotloss: 1.3238227908304048
+--average curloss: 0.6377065901636771
+--average rsmloss: 0.993676309513786
+--average safeloss: 4.036436664704354
+--average anchors: 1.9727659895225933

deepPathPlan/requirements.txt

@@ -0,0 +1,13 @@
+einops==0.8.1
+Markdown==3.7
+matplotlib==3.7.5
+numpy==1.24.4
+opencv-python==4.10.0.84
+pillow==10.4.0
+reeds_shepp==1.0.7
+timm==1.0.16
+torch==2.0.1
+torchaudio==2.0.2
+torchvision==0.15.2
+tqdm==4.66.4
+tensorboard==2.14.0