# Parallelised DiffeomorphicSampling-based Motion Planning

## External dependencies

- Eigen (the math library)

- EigenRand (For generating random numbers in Eigen)

- OpenMP

## Code Folder

The structure of this supplementary code is as follow:

- `DiffeomorphicStateSampler.h`
  
  - The file `DiffeomorphicStateSampler.h` is a c++ header only file that can be included directly without linker issue. This is the main file for the diffeomorphic sampler where it performs all the necessary dispatching of threads, performs link Jacobian 

- `concurrentqueue.h`
  
  - External dependency. A concurrent queue implementation available [here](https://github.com/cameron314/concurrentqueue). The necessary file is extracted and included.

- `TorchOccMap.hpp`
  
  - This is the file that acts as an interface between C++ and the saved PyTorch model file.

- `occmap_divider_torchmodel.pt`
  
  - This is the weight file for the divider environment, which is selected by `DiffeomorphicStateSampler.h` in line

- `map_neural_network.zip
  
  `
  
  - An example file to train up the weights for the continuous occupancy map.

### DiffeomorphicStateSampler.h

This file contain template class that are specialised on the state space given by the ompl state space. There are two implemented, the `RealVectorStateSpace` and the `ompl_interface::JointModelStateSpace`. 



The `class DiffeomorphicStateSampler<ompl_interface::JointModelStateSpace, NumDim>` is of interest here as it is the one presented in the paper.

- `sampleUniform_Diffeomorphic` is the main function which tries to draw from the concurrent queue bucket. When the bucket is empty, it will then draw from the prior uniform.

- `_start_sampling` is the main function to start sampling, which dispatch to worker thread by omp. Each omp thread will have their own cuda stream of which will perform forward pass using the torch api.

## How to use DiffeomorphicStateSampler.h in ompl planner

To utilise the diffeomorphic state sampler in ompl planner, we inject some code directly in the ompl planner. For example, for RRTstar we include the following include lines in the beginning

```cpp
#define DIFFEOMORPHIC_SAMPLER_FOR_MOVEIT yes
#define CSPACE_NUM_DIM 6
#include "DiffeomorphicStateSampler.h"
using DiffeomorphicSamplerType = DiffeomorphicStateSampler<ompl_interface::JointModelStateSpace, CSPACE_NUM_DIM>;
```

Then, we replace the member variable of `_sampler` by changing the `void ompl::geometric::RRTstar::allocSampler()` function:

```cpp
// store si in a global storage and then retireve it inside the state sampler
sxs::g::storage::store("si", si_);

// replace the sampler to the diffeomorphic one
auto diff_sampler = std::make_shared<DiffeomorphicSamplerType>(
    si_->getStateSpace().get(), diff__rand_batch_sample_size, diff__epsilon, diff__num_drift);

diff_sampler->use_diff = diff__use_diff;
diff_sampler->start_sampling();
sampler_ = diff_sampler;

return;
```

Lastly, we put the following line at the end of the planner main loop to stop sampling

```cpp
std::static_pointer_cast<DiffeomorphicSamplerType>(sampler_)->finish_sampling();
```

By doing so, we can replace the original uniform sampler with the diffeomorphic sampler.


