├── viz ├── figs │ └── .gitkeep ├── KGMT_3D_single.asv ├── OMPL_3D.m ├── KGMT_3D.m ├── KGMT_3D_ExpansionColor.m ├── KGMT_3D_GPU.asv ├── KGMT_3D_MultiColor.m └── KGMT_3D_GPU.m ├── include ├── config │ ├── obstacles │ │ ├── obstacles.csv │ │ ├── empty │ │ │ └── obstacles.csv │ │ ├── test │ │ │ └── obstacles.csv │ │ ├── narrowPassage │ │ │ └── obstacles.csv │ │ ├── quadNonLinear │ │ │ └── obstacles.csv │ │ ├── quadNarrowPassage │ │ │ └── obstacles.csv │ │ ├── slot │ │ │ └── obstacles.csv │ │ ├── quadPillarsEasy │ │ │ └── obstacles.csv │ │ ├── clutter │ │ │ └── obstacles.csv │ │ ├── pillars │ │ │ └── obstacles.csv │ │ ├── house │ │ │ ├── obstacles.csv │ │ │ └── test.csv │ │ ├── quadPillars │ │ │ └── obstacles.csv │ │ ├── quadHouse │ │ │ └── obstacles.csv │ │ ├── trees │ │ │ └── obstacles.csv │ │ └── quadTrees │ │ │ └── obstacles.csv │ └── config.h ├── collisionCheck │ └── collisionCheck.cuh ├── statePropagator │ └── statePropagator.cuh ├── graphs │ └── Graph.cuh ├── planners │ ├── Planner.cuh │ └── KPAX.cuh └── helper │ └── helper.cuh ├── .gitignore ├── NSight └── command.md ├── benchmarking ├── benchmarking.py └── bench.m ├── LICENSE ├── src ├── helper │ └── helper.cu ├── collisionCheck │ └── collisionCheck.cu ├── planners │ └── Planner.cu ├── graphs │ └── Graph.cu └── statePropagator │ └── statePropagator.cu ├── .clang-format ├── examples └── gpu │ ├── main.cu │ ├── benchMain.cu │ └── executionTimeMain.cu ├── README.md └── CMakeLists.txt /viz/figs/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /include/config/obstacles/obstacles.csv: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /include/config/obstacles/empty/obstacles.csv: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /include/config/obstacles/test/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.4,0.4,0.0,0.7,0.7,1.0 -------------------------------------------------------------------------------- /include/config/obstacles/narrowPassage/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.3, 0, 0.0, 0.5, 0.49, 1.0 2 | 0.3, .51, 0.0, 0.5, 1.0, 1.0 -------------------------------------------------------------------------------- /include/config/obstacles/quadNonLinear/obstacles.csv: -------------------------------------------------------------------------------- 1 | 30.0, 0, 0.0, 50.0, 49.0, 100.0 2 | 30.0, 51.0, 0.0, 50.0, 100, 100.0 -------------------------------------------------------------------------------- /include/config/obstacles/quadNarrowPassage/obstacles.csv: -------------------------------------------------------------------------------- 1 | 30.0,0.0,0.0,50.0,49.0,100.0 2 | 30.0,51.0,0.0,50.0,100.0,100.0 3 | -------------------------------------------------------------------------------- /include/config/obstacles/slot/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.3, 0, 0.0, 0.35, 0.47, 1.0 2 | 0.3, .53, 0.0, 0.35, 1.0, 1.0 3 | 0.3, 0, 0, 0.35, 1, 0.3 4 | 0.3, 0, 0.6, 0.35, 1, 1 -------------------------------------------------------------------------------- /include/config/obstacles/quadPillarsEasy/obstacles.csv: -------------------------------------------------------------------------------- 1 | 30,40,0,100,60,100 2 | 90,0,0,100,40,100 3 | 0,40,0,30,60,60 4 | 0,40,70,30,60,100 5 | 0,60,0,30,80,40 6 | 0,60,70,30,80,100 7 | 0,80,60,30,100,100 8 | 30,60,0,35,80,100 9 | 30,80,40,35,100,100 10 | 30,80,0,35,100,30 11 | 0,10,0,70,11,100 12 | 50,70,0,52,100,100 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.i 2 | *.ii 3 | *.gpu 4 | *.ptx 5 | *.cubin 6 | *.fatbin 7 | 8 | build/* 9 | .vscode/* 10 | 11 | # figures 12 | viz/figs/* 13 | !viz/figs/.gitkeep 14 | 15 | # NSIGHT 16 | NSight/* 17 | !NSight/command.md 18 | 19 | 20 | # Benchmarking 21 | benchmarking/* 22 | !benchmarking/bench.m 23 | !benchmarking/benchmarking.py -------------------------------------------------------------------------------- /NSight/command.md: -------------------------------------------------------------------------------- 1 | Generate new report: 2 | 3 | ``` 4 | sudo nsys profile --trace=cuda,nvtx,osrt --output=/home/nicolas/dev/research/KPAX/NSight/review --force-overwrite=true /home/nicolas/dev/research/KPAX/build/executionTimeMain 5 | ``` 6 | 7 | Look at new report: 8 | nsys stats --force-export=true /home/nicolas/dev/research/KPAX/NSight/review.nsys-rep | grep -v "SKIPPED" 9 | 10 | Look at Baseline: 11 | 12 | ``` 13 | nsys stats /home/nicolas/dev/research/KPAX/NSight/baseline.nsys-rep | grep -v "SKIPPED" 14 | ``` -------------------------------------------------------------------------------- /include/config/obstacles/clutter/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.08, 0.15, 0.0, 0.12, 0.6, 1.0 2 | 0.1, 0.1, 0.0, 0.5, 0.15, 1.0 3 | 0.3, 0.3, 0.0, 0.35, 0.35, 1.0 4 | .14, 0.4, 0.0, 0.18, 0.44, 1.0 5 | .23, 0.5, 0.0, 1.0, 0.54, 1.0 6 | .6,0.03,0.0,0.65,0.07,1.0 7 | 0.5, 0.6, 0.0, 0.52, 1.0, 1.0 8 | 0.6,0.2,0.0,0.65,0.25,1.0 9 | 0.4,0.4,0.0,0.55,0.44,1.0 10 | 0.1,0.7,0.0,0.15,0.75,1.0 11 | 0.2,0.8,0.0,0.25,0.85,1.0 12 | 0.3,0.9,0.0,0.35,0.95,1.0 13 | .3,.6,0.0,.35,.65,1.0 14 | 0.7,0.5,0.0,1.0,0.7,1.0 15 | .9,0.1,0.0,0.95,0.15,1.0 16 | 0.8,0.8,0.0,0.85,0.85,1.0 17 | .7,.3,0.0,.75,.35,1.0 18 | -------------------------------------------------------------------------------- /include/collisionCheck/collisionCheck.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "config/config.h" 4 | 5 | /***************************/ 6 | /* BroadPhase CC FUNCTION */ 7 | /***************************/ 8 | // --- Bounding box around trajectory segment. Checks if bounding box overlaps with obstacle. 9 | __device__ bool isBroadPhaseValid(float *bbMin, float *bbMax, float *obs); 10 | 11 | /***************************/ 12 | /* FinePhase CC FUNCTION */ 13 | /***************************/ 14 | __device__ bool isFinePhaseValid(float *x0, float *x1, float *obs); 15 | __device__ bool faceContainsProjection(float *x0, float *x1, float lambda, int j, float *obs); 16 | 17 | /***************************/ 18 | /* Motion Validity CC FUNCTION */ 19 | /***************************/ 20 | __device__ bool isMotionValid(float *x0, float *x1, float *bbMin, float *bbMax, float *obstacles, int obstaclesCount); -------------------------------------------------------------------------------- /benchmarking/benchmarking.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import numpy as np 3 | 4 | def calculate_execution_time(file_path): 5 | with open(file_path, 'r') as file: 6 | reader = csv.reader(file) 7 | data = [float(row[0]) for row in reader] 8 | 9 | mean = np.mean(data) 10 | std_dev = np.std(data) 11 | min_val = np.min(data) 12 | max_val = np.max(data) 13 | 14 | return mean, std_dev, min_val, max_val 15 | 16 | def main(): 17 | file_path = 'build/Data/ExecutionTime/executionTime.csv' 18 | mean, std_dev, min_val, max_val = calculate_execution_time(file_path) 19 | 20 | 21 | print("/* Execution Time */") 22 | print(f"Mean : {mean}") 23 | print(f"Standard Deviation: {std_dev}") 24 | print(f"Minimum: {min_val}") 25 | print(f"Maximum: {max_val}") 26 | print("/***************************/\n") 27 | 28 | if __name__ == "__main__": 29 | main() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 ARIA Systems Group 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/helper/helper.cu: -------------------------------------------------------------------------------- 1 | #include "helper/helper.cuh" 2 | 3 | __device__ void printSample(float* x, int sampleDim) 4 | { 5 | for(int i = 0; i < sampleDim; ++i) 6 | { 7 | printf("%f ", x[i]); 8 | } 9 | printf("\n"); 10 | } 11 | 12 | // Function to read obstacles from a CSV file 13 | std::vector readObstaclesFromCSV(const std::string& filename, int& numObstacles, int workspaceDim) 14 | { 15 | std::vector obstacles; 16 | std::ifstream file(filename); 17 | 18 | if(!file.is_open()) 19 | { 20 | std::cerr << "Error opening file: " << filename << std::endl; 21 | exit(1); 22 | } 23 | 24 | std::string line; 25 | while(std::getline(file, line)) 26 | { 27 | std::stringstream ss(line); 28 | float value; 29 | while(ss >> value) 30 | { 31 | obstacles.push_back(value); 32 | if(ss.peek() == ',') ss.ignore(); 33 | } 34 | } 35 | 36 | file.close(); 37 | numObstacles = obstacles.size() / (2 * workspaceDim); 38 | return obstacles; 39 | } 40 | -------------------------------------------------------------------------------- /include/statePropagator/statePropagator.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "helper/helper.cuh" 4 | #include "config/config.h" 5 | #include "collisionCheck/collisionCheck.cuh" 6 | 7 | __device__ bool propagateAndCheck(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount); 8 | __device__ bool propagateAndCheckUnicycle(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount); 9 | __device__ bool propagateAndCheckDoubleIntRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount); 10 | __device__ bool propagateAndCheckDubinsAirplaneRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount); 11 | __device__ bool propagateAndCheckQuadRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount); 12 | __device__ void ode(float* x0dot, float* x0, float* h, float Zc, float Lc, float Mc, float Nc, int itr); 13 | 14 | typedef bool (*PropagateAndCheckFunc)(float*, float*, curandState*, float*, int); 15 | 16 | /***************************/ 17 | /* GET PROPAGATION FUNCTION */ 18 | /***************************/ 19 | // --- Determins which dynamic model to use. --- 20 | __device__ PropagateAndCheckFunc getPropagateAndCheckFunc(); -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | Language: Cpp 3 | Standard: Cpp11 4 | 5 | ColumnLimit: 140 6 | 7 | AccessModifierOffset: -4 8 | IndentWidth: 4 9 | UseTab: Never 10 | 11 | AlignEscapedNewlines: Left 12 | AllowShortFunctionsOnASingleLine: Empty 13 | AllowShortLambdasOnASingleLine: Empty 14 | AlwaysBreakTemplateDeclarations: true 15 | BreakConstructorInitializers: BeforeComma 16 | IndentPPDirectives: AfterHash 17 | 18 | BreakBeforeBraces: Custom 19 | BraceWrapping: 20 | AfterClass: true 21 | AfterControlStatement: true 22 | AfterEnum: true 23 | AfterFunction: true 24 | AfterNamespace: true 25 | AfterStruct: true 26 | AfterUnion: true 27 | AfterExternBlock: true 28 | BeforeCatch: true 29 | BeforeElse: true 30 | IndentBraces: true 31 | SplitEmptyFunction: false 32 | SplitEmptyRecord: false 33 | SplitEmptyNamespace: false 34 | 35 | ContinuationIndentWidth: 2 36 | Cpp11BracedListStyle: true 37 | KeepEmptyLinesAtTheStartOfBlocks: false 38 | MaxEmptyLinesToKeep: 1 39 | NamespaceIndentation: All 40 | PenaltyExcessCharacter: 100 41 | PenaltyReturnTypeOnItsOwnLine: 1 42 | PointerAlignment: Right 43 | SortIncludes: false 44 | SpaceBeforeParens: Never 45 | SpaceBeforeAssignmentOperators: true 46 | SpacesInContainerLiterals: false 47 | 48 | AlignConsecutiveAssignments: true 49 | -------------------------------------------------------------------------------- /examples/gpu/main.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include "planners/KPAX.cuh" 3 | 4 | int main(void) 5 | { 6 | // --- Remove Previous Bench Data --- 7 | system("rm -rf Data/*"); 8 | 9 | // --- For 6D systems --- 10 | // float h_initial[SAMPLE_DIM] = {.100, .080, .05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 11 | // h_goal[SAMPLE_DIM] = {.800, .950, .900, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 12 | 13 | // --- For 12D Nonlinear System: --- 14 | float h_initial[SAMPLE_DIM] = {10.0, 8, 5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 15 | h_goal[SAMPLE_DIM] = {80, 95.0, 90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 16 | KPAX kpax; 17 | 18 | int numObstacles; 19 | float* d_obstacles; 20 | 21 | // --- Load Workspace Obstacles --- 22 | std::vector obstacles = readObstaclesFromCSV("../include/config/obstacles/quadTrees/obstacles.csv", numObstacles, W_DIM); 23 | 24 | // --- Transfer Obstacles to device --- 25 | cudaMalloc(&d_obstacles, numObstacles * 2 * W_DIM * sizeof(float)); 26 | cudaMemcpy(d_obstacles, obstacles.data(), numObstacles * 2 * W_DIM * sizeof(float), cudaMemcpyHostToDevice); 27 | 28 | // --- Execute planner --- 29 | kpax.planBench(h_initial, h_goal, d_obstacles, numObstacles, 0); 30 | 31 | // --- Free memory --- 32 | cudaFree(d_obstacles); 33 | return 0; 34 | } -------------------------------------------------------------------------------- /examples/gpu/benchMain.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include "planners/KPAX.cuh" 3 | int main(void) 4 | { 5 | // --- Remove Previous Bench Data --- 6 | system("rm -rf Data/*"); 7 | 8 | // --- For 6D systems --- 9 | // float h_initial[SAMPLE_DIM] = {.100, .080, .05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 10 | // h_goal[SAMPLE_DIM] = {.800, .950, .900, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 11 | 12 | // --- For 12D Nonlinear System: --- 13 | float h_initial[SAMPLE_DIM] = {10.0, 8, 5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 14 | h_goal[SAMPLE_DIM] = {80, 95.0, 90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 15 | 16 | KPAX kpax; 17 | 18 | int numObstacles; 19 | float* d_obstacles; 20 | 21 | // --- Load Workspace Obstacles --- 22 | std::vector obstacles = readObstaclesFromCSV("../include/config/obstacles/quadTrees/obstacles.csv", numObstacles, W_DIM); 23 | 24 | // --- Transfer Obstacles to device --- 25 | cudaMalloc(&d_obstacles, numObstacles * 2 * W_DIM * sizeof(float)); 26 | cudaMemcpy(d_obstacles, obstacles.data(), numObstacles * 2 * W_DIM * sizeof(float), cudaMemcpyHostToDevice); 27 | 28 | // --- Execute planner N times --- 29 | int N = 50; 30 | for(int i = 0; i < N; i++) 31 | { 32 | // --- Execute planner --- 33 | kpax.planBench(h_initial, h_goal, d_obstacles, numObstacles, i); 34 | } 35 | 36 | // --- Free memory --- 37 | cudaFree(d_obstacles); 38 | return 0; 39 | } -------------------------------------------------------------------------------- /examples/gpu/executionTimeMain.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include "planners/KPAX.cuh" 3 | 4 | int main(void) 5 | { 6 | // --- Remove Previous Bench Data --- 7 | system("rm -rf Data/*"); 8 | 9 | // --- For 6D systems --- 10 | // float h_initial[SAMPLE_DIM] = {.100, .080, .05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 11 | // h_goal[SAMPLE_DIM] = {.800, .950, .900, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 12 | 13 | // --- For 12D Nonlinear System: --- 14 | float h_initial[SAMPLE_DIM] = {10.0, 8, 5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 15 | h_goal[SAMPLE_DIM] = {80, 95.0, 90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 16 | 17 | KPAX kpax; 18 | 19 | int numObstacles; 20 | float* d_obstacles; 21 | 22 | // --- Load Workspace Obstacles --- 23 | std::vector obstacles = readObstaclesFromCSV("../include/config/obstacles/quadTrees/obstacles.csv", numObstacles, W_DIM); 24 | 25 | // --- Transfer Obstacles to device --- 26 | cudaMalloc(&d_obstacles, numObstacles * 2 * W_DIM * sizeof(float)); 27 | cudaMemcpy(d_obstacles, obstacles.data(), numObstacles * 2 * W_DIM * sizeof(float), cudaMemcpyHostToDevice); 28 | 29 | // --- Execute planner N times --- 30 | int N = 50; 31 | for(int i = 0; i < N; i++) 32 | { 33 | // --- Execute planner --- 34 | kpax.plan(h_initial, h_goal, d_obstacles, numObstacles); 35 | } 36 | 37 | // --- Free memory --- 38 | cudaFree(d_obstacles); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /src/collisionCheck/collisionCheck.cu: -------------------------------------------------------------------------------- 1 | #include "collisionCheck/collisionCheck.cuh" 2 | 3 | __device__ bool isBroadPhaseValid(float *bbMin, float *bbMax, float *obs) 4 | { 5 | for(int d = 0; d < W_DIM; ++d) 6 | { 7 | if(bbMax[d] <= obs[d] || obs[W_DIM + d] <= bbMin[d]) return true; 8 | } 9 | return false; 10 | } 11 | 12 | __device__ bool isFinePhaseValid(float *x0, float *x1, float *obs) 13 | { 14 | float x0_to_x1[W_DIM]; 15 | 16 | for(int d = 0; d < W_DIM; ++d) 17 | { 18 | float lambda; 19 | x0_to_x1[d] = x1[d] - x0[d]; 20 | if(x0[d] < obs[d]) 21 | { 22 | lambda = (obs[d] - x0[d]) / x0_to_x1[d]; 23 | } 24 | else 25 | { 26 | lambda = (obs[W_DIM + d] - x0[d]) / x0_to_x1[d]; 27 | } 28 | if(faceContainsProjection(x0, x1, lambda, d, obs)) return false; 29 | } 30 | return true; 31 | } 32 | 33 | __device__ bool faceContainsProjection(float *x0, float *x1, float lambda, int j, float *obs) 34 | { 35 | for(int d = 0; d < W_DIM; ++d) 36 | { 37 | float projection = x0[d] + (x1[d] - x0[d]) * lambda; 38 | if(d != j && !(obs[d] <= projection && projection <= obs[W_DIM + d])) 39 | { 40 | if(!isFinePhaseValid(x0, x1, obs)) 41 | { 42 | return false; 43 | } 44 | } 45 | } 46 | return true; 47 | } 48 | 49 | __device__ bool isMotionValid(float *x0, float *x1, float *bbMin, float *bbMax, float *obstacles, int obstaclesCount) 50 | { 51 | for(int obsIdx = 0; obsIdx < obstaclesCount; ++obsIdx) 52 | { 53 | float obs[2 * W_DIM]; 54 | for(int d = 0; d < W_DIM; ++d) 55 | { 56 | obs[d] = obstacles[obsIdx * 2 * W_DIM + d]; 57 | obs[W_DIM + d] = obstacles[obsIdx * 2 * W_DIM + W_DIM + d]; 58 | } 59 | if(!isBroadPhaseValid(bbMin, bbMax, obs)) return false; 60 | } 61 | return true; 62 | } 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Kino-PAX 3 | Kinodynamic Parallel Accelerated eXpansion 4 | 5 | Sampling-based motion planners (SBMPs) are effective for planning with complex kinodynamic constraints in high-dimensional spaces. However, they still struggle to achieve real-time performance, primarily due to their serial computation design. 6 | 7 | We present Kinodynamic Parallel Accelerated eXpansion (Kino-PAX), a novel, highly parallel kinodynamic SBMP designed for parallel devices such as GPUs. Kino-PAX grows a tree of trajectory segments in parallel. 8 | 9 | Our key insight is in decomposing the iterative tree growth process into three massively parallel subroutines. Kino-PAX is designed to align with the parallel device execution hierarchies, ensuring that: 10 | - Threads are largely independent 11 | - Workloads are evenly distributed 12 | - Low-latency resources are utilized while minimizing high-latency data transfers and process synchronization 13 | 14 | This design results in a highly efficient GPU implementation. We prove that Kino-PAX is probabilistically complete and analyze its scalability as compute hardware improves. 15 | 16 | Empirical evaluations demonstrate solutions in the order of 10 ms on a desktop GPU and in the order of 100 ms on an embedded GPU, representing up to a 1000× improvement compared to coarse-grained CPU parallelization of state-of-the-art sequential algorithms over a range of complex environments and systems. 17 | 18 | ## Disclaimers 19 | 20 | The code in this repository is "research-grade", meaning it is largely uncommented and may not be immediately useful without modification. It serves primarily as a reference for future CUDA-related projects. 21 | 22 | ## What is included 23 | 24 | Three example executables are included: `benchMain`, `executionTimeMain`, and `main`. 25 | 26 | - `benchMain` runs N iterations of KPAX and collects data on tree growth, region exploration, and all nodes in the tree. It uses a `planBench` version of KPAX that is **NOT INTENDED FOR RUNTIME PERFORMANCE.** 27 | 28 | - `executionTimeMain` runs N iterations of the lightweight version of KPAX, where only the solution trajectory is sent back to the CPU. This version of KPAX is used to collect runtime data. 29 | 30 | - `main` runs a single iteration of `planBench` and is used to collect data for plotting. 31 | 32 | To manage the dynamic model used, edit the `config` file. 33 | 34 | ## Contact 35 | 36 | If you are interested in the code and would like to discuss further, please email me at nicolas.perrault@colorado.edu. -------------------------------------------------------------------------------- /include/graphs/Graph.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "helper/helper.cuh" 12 | 13 | class Graph 14 | { 15 | public: 16 | // --- constructor --- 17 | Graph() = default; 18 | Graph(float h_ws); 19 | 20 | // --- host fields --- 21 | int h_numPartialSums_; 22 | int h_blockSize_ = 32; 23 | 24 | // --- device fields --- 25 | thrust::device_vector d_validCounterArray_, d_counterArray_, d_activeVerticesScanIdx_, d_activeSubVertices_; 26 | thrust::device_vector d_vertexScoreArray_, d_minValueInRegion_, d_partialSums_, d_totalScore_; 27 | 28 | float *d_vertexScoreArray_ptr_, *d_minValueInRegion_ptr_, *d_partialSums_ptr_, *d_totalScore_ptr_; 29 | int *d_validCounterArray_ptr_, *d_counterArray_ptr_, *d_activeVerticesScanIdx_ptr_, *d_activeSubVertices_ptr_; 30 | 31 | /**************************** METHODS ****************************/ 32 | void updateVertices(); 33 | 34 | private: 35 | /**************************** METHODS ****************************/ 36 | void initializeRegions(); 37 | }; 38 | 39 | /**************************** DEVICE FUNCTIONS ****************************/ 40 | __host__ __device__ int getRegion(float* coord); 41 | __device__ int getSubRegion(float* coord, int r1, float* minRegion); 42 | 43 | __global__ void 44 | partialReduction_kernel(int* activeSubVertices, int* validCounterArray, int* counterArray, float* vertexScores, float* partialSums); 45 | __global__ void globalReduction_kernel(float* partialSums, float* totalScore, int numPartialSums); 46 | __global__ void updateSampleAcceptance_kernel(int* validCounterArray, float* vertexScores, float* totalScore); 47 | 48 | /***************************/ 49 | /* VERTICES UPDATE KERNEL */ 50 | /***************************/ 51 | // --- Updates Vertex Scores for device graph vectors. Determines new threshold score for future samples in expansion set. --- 52 | __global__ void updateVertices_kernel(int* activeSubVertices, int* validCounterArray, int* counterArray, float* vertexScores); 53 | 54 | /***************************/ 55 | /* INITIALIZE REGIONS KERNEL */ 56 | /***************************/ 57 | // --- Initializes min and max values for regions --- 58 | __global__ void initializeRegions_kernel(float* minValueInRegion); -------------------------------------------------------------------------------- /include/planners/Planner.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "config/config.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "helper/helper.cuh" 14 | #include "statePropagator/statePropagator.cuh" 15 | #include 16 | #include 17 | #include 18 | 19 | class Planner 20 | { 21 | public: 22 | /**************************** CONSTRUCTORS ****************************/ 23 | Planner(); 24 | 25 | /**************************** METHODS ****************************/ 26 | virtual void plan(float* h_initial, float* h_goal, float* d_obstacles_ptr, uint h_obstaclesCount) = 0; 27 | void initializeRandomSeeds(int seed); 28 | 29 | /**************************** FIELDS ****************************/ 30 | // --- host fields --- 31 | uint h_treeSize_ = 0, h_itr_ = 0, h_blockSize_ = 128, h_gridSize_; 32 | float h_costToGoal_ = 0.0; 33 | int h_pathToGoal_; 34 | float* h_controlPathToGoal_; 35 | 36 | // --- device fields --- 37 | thrust::device_vector d_treeSamples_, d_treeSampleCosts_, d_controlPathToGoal_; 38 | thrust::device_vector d_treeSamplesParentIdxs_; 39 | 40 | float *d_treeSamples_ptr_, *d_treeSampleCosts_ptr_, *d_costToGoal_ptr_, *d_controlPathToGoal_ptr_; 41 | int *d_treeSamplesParentIdxs_ptr_, *d_pathToGoal_ptr_; 42 | 43 | curandState* d_randomSeeds_ptr_; 44 | }; 45 | 46 | /**************************** DEVICE FUNCTIONS ****************************/ 47 | 48 | /***************************/ 49 | /* INIT RANDOM SEEDS KERNEL */ 50 | /***************************/ 51 | // --- used to generate random values when propagating frontier on GPU. --- 52 | __global__ void initializeRandomSeeds_kernel(curandState* randomSeeds, int numSeeds, int seed); 53 | 54 | /***************************/ 55 | /* FIND INDICES BOOL KERNEL */ 56 | /***************************/ 57 | // --- Finds active indices in a boolean array. --- 58 | __global__ void findInd(uint numSamples, bool* S, uint* scanIdx, uint* activeS); 59 | 60 | /***************************/ 61 | /* FIND INDICES INT KERNEL */ 62 | /***************************/ 63 | // --- Finds active indices in an integer array. --- 64 | __global__ void findInd(uint numSamples, uint* S, uint* scanIdx, uint* activeS); 65 | 66 | /***************************/ 67 | /* REPEAT INDICES KERNEL */ 68 | /***************************/ 69 | __global__ void repeatInd(uint numSamples, uint* activeS, uint* C, uint* prefixSum, uint* repeatedInd); -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(KPAX LANGUAGES CXX CUDA) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CUDA_STANDARD 17) 6 | 7 | find_package(CUDA REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(Boost REQUIRED) 10 | find_package(yaml-cpp REQUIRED) 11 | find_package(Python3 COMPONENTS Development NumPy REQUIRED) 12 | find_package(OpenMP REQUIRED) 13 | 14 | include_directories( 15 | include 16 | ${EIGEN3_INCLUDE_DIRS} 17 | ${BOOST_INCLUDE_DIRS} 18 | ${CMAKE_SOURCE_DIR}/src 19 | ${CUDA_INCLUDE_DIRS} 20 | ${Python3_INCLUDE_DIRS} 21 | ${Python3_NumPy_INCLUDE_DIRS} 22 | ${YAML_CPP_INCLUDE_DIR} 23 | ) 24 | 25 | set(SOURCE_FILES 26 | src/planners/KPAX.cu 27 | src/planners/Planner.cu 28 | src/collisionCheck/collisionCheck.cu 29 | src/graphs/Graph.cu 30 | src/helper/helper.cu 31 | src/statePropagator/statePropagator.cu 32 | ) 33 | add_library(KPAX_lib ${SOURCE_FILES}) 34 | 35 | 36 | if (OpenMP_CXX_FOUND) 37 | target_link_libraries(KPAX_lib PUBLIC OpenMP::OpenMP_CXX) 38 | endif() 39 | 40 | 41 | # -- GPU EXECUTABLES -- 42 | add_executable(main examples/gpu/main.cu) 43 | target_link_libraries(main 44 | PUBLIC KPAX_lib 45 | ${CUDA_LIBRARIES} 46 | ${Boost_LIBRARIES} 47 | ${EIGEN3_LIBRARIES} 48 | yaml-cpp 49 | Python3::Python 50 | Python3::NumPy 51 | ) 52 | 53 | add_executable(benchMain examples/gpu/benchMain.cu) 54 | target_link_libraries(benchMain 55 | PUBLIC KPAX_lib 56 | ${CUDA_LIBRARIES} 57 | ${Boost_LIBRARIES} 58 | ${EIGEN3_LIBRARIES} 59 | yaml-cpp 60 | Python3::Python 61 | Python3::NumPy 62 | ) 63 | 64 | add_executable(executionTimeMain examples/gpu/executionTimeMain.cu) 65 | target_link_libraries(executionTimeMain 66 | PUBLIC KPAX_lib 67 | ${CUDA_LIBRARIES} 68 | ${Boost_LIBRARIES} 69 | ${EIGEN3_LIBRARIES} 70 | yaml-cpp 71 | Python3::Python 72 | Python3::NumPy 73 | ) 74 | 75 | set_target_properties(KPAX_lib PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 76 | set_target_properties(main PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 77 | set_target_properties(benchMain PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 78 | set_target_properties(executionTimeMain PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 79 | 80 | 81 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -diag-suppress=20012,20013,20014,20015 --expt-relaxed-constexpr --ptxas-options=-v") 82 | set_property(TARGET main PROPERTY CUDA_ARCHITECTURES 86) 83 | set_property(TARGET benchMain PROPERTY CUDA_ARCHITECTURES 86) 84 | set_property(TARGET KPAX_lib PROPERTY CUDA_ARCHITECTURES 86) 85 | add_definitions(-DEIGEN_NO_DEBUG) 86 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 87 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-unused-function") 88 | endif() 89 | if(MSVC) 90 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4996 /wd4244 /wd4267") 91 | endif() 92 | add_definitions(-D_MWAITXINTRIN_H_INCLUDED) 93 | add_definitions(-D_FORCE_INLINES) -------------------------------------------------------------------------------- /include/config/obstacles/pillars/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.01, 0.02, 0.0, 0.05, 0.07, 1.0 2 | 0.08, 0.15, 0.0, 0.12, 0.18, 1.0 3 | 0.13, 0.04, 0.0, 0.17, 0.09, 1.0 4 | 0.21, 0.28, 0.0, 0.25, 0.33, 1.0 5 | 0.27, 0.19, 0.0, 0.32, 0.25, 1.0 6 | 0.34, 0.34, 0.0, 0.38, 0.39, 1.0 7 | 0.41, 0.48, 0.0, 0.46, 0.53, 1.0 8 | 0.47, 0.12, 0.0, 0.52, 0.18, 1.0 9 | 0.53, 0.57, 0.0, 0.58, 0.62, 1.0 10 | 0.59, 0.04, 0.0, 0.63, 0.08, 1.0 11 | 0.64, 0.64, 0.0, 0.69, 0.69, 1.0 12 | 0.71, 0.22, 0.0, 0.76, 0.27, 1.0 13 | 0.73, 0.73, 0.0, 0.78, 0.78, 1.0 14 | 0.82, 0.12, 0.0, 0.87, 0.18, 1.0 15 | 0.84, 0.84, 0.0, 0.89, 0.89, 1.0 16 | 0.92, 0.25, 0.0, 0.97, 0.30, 1.0 17 | 0.94, 0.94, 0.0, 0.99, 0.99, 1.0 18 | 0.06, 0.40, 0.0, 0.11, 0.45, 1.0 19 | 0.15, 0.06, 0.0, 0.19, 0.11, 1.0 20 | 0.21, 0.50, 0.0, 0.26, 0.55, 1.0 21 | 0.31, 0.07, 0.0, 0.36, 0.12, 1.0 22 | 0.33, 0.60, 0.0, 0.37, 0.65, 1.0 23 | 0.45, 0.35, 0.0, 0.50, 0.40, 1.0 24 | 0.52, 0.70, 0.0, 0.57, 0.75, 1.0 25 | 0.60, 0.15, 0.0, 0.65, 0.20, 1.0 26 | 0.63, 0.75, 0.0, 0.68, 0.80, 1.0 27 | 0.74, 0.40, 0.0, 0.79, 0.45, 1.0 28 | 0.77, 0.77, 0.0, 0.82, 0.82, 1.0 29 | 0.85, 0.55, 0.0, 0.90, 0.60, 1.0 30 | 0.90, 0.90, 0.0, 0.95, 0.95, 1.0 31 | 0.13, 0.13, 0.0, 0.18, 0.18, 1.0 32 | 0.22, 0.60, 0.0, 0.27, 0.65, 1.0 33 | 0.30, 0.30, 0.0, 0.35, 0.35, 1.0 34 | 0.38, 0.77, 0.0, 0.43, 0.82, 1.0 35 | 0.44, 0.44, 0.0, 0.49, 0.49, 1.0 36 | 0.56, 0.90, 0.0, 0.61, 0.95, 1.0 37 | 0.66, 0.55, 0.0, 0.71, 0.60, 1.0 38 | 0.72, 0.11, 0.0, 0.77, 0.16, 1.0 39 | 0.79, 0.79, 0.0, 0.84, 0.84, 1.0 40 | 0.85, 0.70, 0.0, 0.90, 0.75, 1.0 41 | 0.91, 0.18, 0.0, 0.96, 0.23, 1.0 42 | 0.02, 0.02, 0.0, 0.06, 0.06, 1.0 43 | 0.07, 0.07, 0.0, 0.11, 0.11, 1.0 44 | 0.20, 0.20, 0.0, 0.24, 0.24, 1.0 45 | 0.27, 0.27, 0.0, 0.31, 0.31, 1.0 46 | 0.36, 0.36, 0.0, 0.40, 0.40, 1.0 47 | 0.50, 0.50, 0.0, 0.54, 0.54, 1.0 48 | 0.55, 0.55, 0.0, 0.59, 0.59, 1.0 49 | 0.70, 0.70, 0.0, 0.74, 0.74, 1.0 50 | 0.75, 0.75, 0.0, 0.79, 0.79, 1.0 51 | 0.80, 0.80, 0.0, 0.84, 0.84, 1.0 52 | 0.90, 0.90, 0.0, 0.94, 0.94, 1.0 53 | 0.05, 0.30, 0.0, 0.09, 0.34, 1.0 54 | 0.14, 0.40, 0.0, 0.18, 0.44, 1.0 55 | 0.23, 0.50, 0.0, 0.27, 0.54, 1.0 56 | 0.32, 0.60, 0.0, 0.36, 0.64, 1.0 57 | 0.41, 0.70, 0.0, 0.45, 0.74, 1.0 58 | 0.50, 0.80, 0.0, 0.54, 0.84, 1.0 59 | 0.59, 0.90, 0.0, 0.63, 0.94, 1.0 60 | 0.20, 0.01, 0.0, 0.24, 0.05, 1.0 61 | 0.28, 0.11, 0.0, 0.32, 0.15, 1.0 62 | 0.36, 0.21, 0.0, 0.40, 0.25, 1.0 63 | 0.44, 0.31, 0.0, 0.48, 0.35, 1.0 64 | 0.50, 0.41, 0.0, 0.54, 0.45, 1.0 65 | 0.56, 0.51, 0.0, 0.60, 0.55, 1.0 66 | 0.62, 0.61, 0.0, 0.66, 0.65, 1.0 67 | 0.68, 0.71, 0.0, 0.72, 0.75, 1.0 68 | 0.74, 0.81, 0.0, 0.78, 0.85, 1.0 69 | 0.80, 0.91, 0.0, 0.84, 0.95, 1.0 70 | 0.90, 0.01, 0.0, 0.94, 0.05, 1.0 71 | 0.96, 0.11, 0.0, 1.0, 0.15, 1.0 72 | 0.30, 0.70, 0.0, 0.35, 0.75, 1.0 73 | 0.45, 0.85, 0.0, 0.50, 0.90, 1.0 74 | 0.55, 0.65, 0.0, 0.60, 0.70, 1.0 75 | 0.65, 0.80, 0.0, 0.70, 0.85, 1.0 76 | 0.75, 0.60, 0.0, 0.80, 0.65, 1.0 77 | 0.85, 0.75, 0.0, 0.90, 0.80, 1.0 78 | 0.95, 0.90, 0.0, 1.0, 0.95, 1.0 79 | 0.0, 0.5, 0.0, 0.05, 0.55, 1.0 80 | -0.01,0.0,0,0.0,1.01,1.0 81 | -0.01,0.0,0.0,1.01,0.01,1.0 82 | 1.0,0.0,0.0,1.01,1.0,1.0 83 | 0.0,1.0,0.0,1.01,1.01,1.0 84 | -------------------------------------------------------------------------------- /include/config/obstacles/house/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.0,0.0,0.0,1.0,1.0,0.03 2 | 0.0,0.0,0.3333,1.0,0.5,0.3633 3 | 0.0,0.5,0.3333,0.3,1.0,0.3633 4 | 0.0,0.0,0.6667,1.0,0.4,0.6967 5 | 0.3,0.4,0.6667,1.0,1.0,0.6967 6 | 0.0,0.2,0.6667,0.2,1.0,0.6967 7 | 0.2,0.4,0.6667,0.4,0.6,0.6967 8 | 0.8,0.2,0.6667,1.0,0.4,0.6967 9 | 0.29,0.5,0.01,0.3,0.8,0.6967 10 | 0.29,0.8,0.3333,0.3,1.0,0.6867 11 | 0.1,0.6,0.6667,0.3,0.61,0.7867000000000001 12 | 0.2,0.63,0.3333,0.3,0.66,0.38670000000000004 13 | 0.2,0.66,0.3333,0.3,0.69,0.42000000000000004 14 | 0.2,0.69,0.3333,0.3,0.72,0.45330000000000004 15 | 0.2,0.72,0.3333,0.3,0.75,0.4867 16 | 0.2,0.75,0.3333,0.3,0.78,0.52 17 | 0.2,0.78,0.3333,0.3,0.81,0.5533 18 | 0.2,0.81,0.3333,0.3,0.84,0.5867 19 | 0.2,0.84,0.3333,0.3,0.87,0.62 20 | 0.2,0.87,0.3333,0.3,0.9,0.6533 21 | 0.2,0.9,0.3333,0.3,0.93,0.6867 22 | 0.3,0.77,0.0,0.7,0.8,0.05330000000000001 23 | 0.3,0.74,0.0,0.7,0.77,0.08669999999999999 24 | 0.3,0.71,0.0,0.7,0.74,0.12 25 | 0.3,0.68,0.0,0.7,0.71,0.15330000000000002 26 | 0.3,0.65,0.0,0.7,0.68,0.1867 27 | 0.3,0.62,0.0,0.7,0.65,0.22000000000000003 28 | 0.3,0.59,0.0,0.7,0.62,0.2533 29 | 0.3,0.56,0.0,0.7,0.59,0.2867 30 | 0.3,0.53,0.0,0.7,0.56,0.32 31 | 0.3,0.5,0.0,0.7,0.53,0.3533 32 | 0.1,0.3,0.0,0.11,0.31,0.6867 33 | 0.1,0.5,0.0,0.11,0.51,0.6867 34 | 0.1,0.7,0.0,0.11,0.71,0.6867 35 | 0.9,0.01,0.3333,1.0,0.21,0.4033 36 | 0.975,0.01,0.3833,1.0,0.21,0.5033 37 | 0.9,0.01,0.3333,1.0,0.01,0.45330000000000004 38 | 0.9,0.21,0.3333,1.0,0.21,0.45330000000000004 39 | 0.8,0.01,0.3333,0.7,0.21,0.4033 40 | 0.725,0.01,0.3833,0.7,0.21,0.5033 41 | 0.8,0.01,0.3333,0.7,0.01,0.45330000000000004 42 | 0.8,0.21,0.3333,0.7,0.21,0.45330000000000004 43 | 0.9,0.6,0.6667,1.0,0.8,0.7367 44 | 0.975,0.6,0.7167,1.0,0.8,0.8367 45 | 0.9,0.6,0.6667,1.0,0.6,0.7867000000000001 46 | 0.9,0.8,0.6667,1.0,0.8,0.7867000000000001 47 | 0.98,0.15,0.0,0.945,0.25,0.09499999999999999 48 | 0.98,0.15,0.075,0.9683,0.25,0.17 49 | 0.75,0.15,0.0,0.785,0.25,0.09499999999999999 50 | 0.75,0.15,0.075,0.7617,0.25,0.17 51 | 0.98,0.3,0.0,0.945,0.4,0.09499999999999999 52 | 0.98,0.3,0.075,0.9683,0.4,0.17 53 | 0.75,0.3,0.0,0.785,0.4,0.09499999999999999 54 | 0.75,0.3,0.075,0.7617,0.4,0.17 55 | 0.75,0.7,0.6667,0.785,0.8,0.7617 56 | 0.75,0.7,0.7417,0.7617,0.8,0.8367 57 | 0.75,0.55,0.6667,0.785,0.65,0.7617 58 | 0.75,0.55,0.7417,0.7617,0.65,0.8367 59 | 0.01,0.7,0.3333,0.045,0.8,0.4283 60 | 0.01,0.7,0.4083,0.0217,0.8,0.5033 61 | 0.01,0.55,0.3333,0.045,0.65,0.4283 62 | 0.01,0.55,0.4083,0.0217,0.65,0.5033 63 | 0.01,0.4,0.3333,0.045,0.5,0.4283 64 | 0.01,0.4,0.4083,0.0217,0.5,0.5033 65 | 0.4,0.73,0.6667,0.6,0.8,0.8367 66 | 0.4,0.8,0.6667,0.6,0.87,0.8367 67 | 0.8,0.2,0.15,0.95,0.5,0.17500000000000002 68 | 0.9475,0.2,0.01,0.95,0.21,0.17 69 | 0.8,0.49,0.01,0.8025,0.5,0.17 70 | 0.9475,0.49,0.01,0.95,0.5,0.17 71 | 0.8,0.2,0.01,0.8025,0.21,0.17 72 | 0.825,0.05,0.4133,0.885,0.2,0.4358 73 | 0.884,0.05,0.3433,0.885,0.055,0.4333 74 | 0.825,0.195,0.3433,0.826,0.2,0.4333 75 | 0.884,0.195,0.3433,0.885,0.2,0.4333 76 | 0.825,0.05,0.3433,0.826,0.055,0.4333 77 | 0.825,0.65,0.7467,0.885,0.8,0.7692 78 | 0.884,0.65,0.6767,0.885,0.655,0.7667 79 | 0.825,0.795,0.6767,0.826,0.8,0.7667 80 | 0.884,0.795,0.6767,0.885,0.8,0.7667 81 | 0.825,0.65,0.6767,0.826,0.655,0.7667 82 | -------------------------------------------------------------------------------- /include/config/obstacles/house/test.csv: -------------------------------------------------------------------------------- 1 | 0.0,0.0,0.0,1.0,1.0,0.01 2 | 0.0,0.0,0.3333,1.0,0.5,0.3433 3 | 0.0,0.5,0.3333,0.3,1.0,0.3433 4 | 0.0,0.0,0.6667,1.0,0.2,0.6767 5 | 0.3,0.4,0.6667,2.0,2.0,0.6767 6 | 0.0,0.2,0.6667,0.2,2.0,0.6767 7 | 0.2,0.4,0.6667,0.4,0.6,0.6767 8 | 0.8,0.2,0.6667,1.0,0.4,0.6767 9 | 0.0,0.0,0.0,0.01,1.0,1.0 10 | 0.0,0.0,0.0,1.0,0.01,1.0 11 | 1.0,0.0,0.0,1.01,0.5,1.0 12 | 0.29,0.5,0.01,0.3,0.8,0.6767 13 | 0.29,0.8,0.3333,0.3,1.0,0.6667 14 | 0.29,0.6,0.6667,0.3,1.0,1.0 15 | 0.7,0.6,0.6667,0.71,1.0,1.0 16 | 0.3,0.6,0.6667,0.6,0.61,1.0 17 | 0.1,0.6,0.6667,0.3,0.61,0.7667 18 | 0.2,0.63,0.3333,0.3,0.66,0.3667 19 | 0.2,0.66,0.3333,0.3,0.69,0.4 20 | 0.2,0.69,0.3333,0.3,0.72,0.4333 21 | 0.2,0.72,0.3333,0.3,0.75,0.4667 22 | 0.2,0.75,0.3333,0.3,0.78,0.5 23 | 0.2,0.78,0.3333,0.3,0.81,0.5333 24 | 0.2,0.81,0.3333,0.3,0.84,0.5667 25 | 0.2,0.84,0.3333,0.3,0.87,0.6 26 | 0.2,0.87,0.3333,0.3,0.9,0.6333 27 | 0.2,0.9,0.3333,0.3,0.93,0.6667 28 | 0.3,0.77,0.0,0.7,0.8,0.0333 29 | 0.3,0.74,0.0,0.7,0.77,0.0667 30 | 0.3,0.71,0.0,0.7,0.74,0.1 31 | 0.3,0.68,0.0,0.7,0.71,0.1333 32 | 0.3,0.65,0.0,0.7,0.68,0.1667 33 | 0.3,0.62,0.0,0.7,0.65,0.2 34 | 0.3,0.59,0.0,0.7,0.62,0.2333 35 | 0.3,0.56,0.0,0.7,0.59,0.2667 36 | 0.3,0.53,0.0,0.7,0.56,0.3 37 | 0.3,0.5,0.0,0.7,0.53,0.3333 38 | 0.3,0.3,0.3333,0.31,0.31,1.0 39 | 0.7,0.3,0.3333,0.71,0.31,1.0 40 | 0.9,0.9,0.0,0.91,0.91,1.0 41 | 0.1,0.3,0.0,0.11,0.31,0.6667 42 | 0.1,0.5,0.0,0.11,0.51,0.6667 43 | 0.1,0.7,0.0,0.11,0.71,0.6667 44 | 0.9,0.01,0.3333,1.0,0.21,0.3833 45 | 0.975,0.01,0.3833,1.0,0.21,0.4833 46 | 0.9,0.01,0.3333,1.0,0.01,0.4333 47 | 0.9,0.21,0.3333,1.0,0.21,0.4333 48 | 0.8,0.01,0.3333,0.7,0.21,0.3833 49 | 0.725,0.01,0.3833,0.7,0.21,0.4833 50 | 0.8,0.01,0.3333,0.7,0.01,0.4333 51 | 0.8,0.21,0.3333,0.7,0.21,0.4333 52 | 0.9,0.6,0.6667,1.0,0.8,0.7167 53 | 0.975,0.6,0.7167,1.0,0.8,0.8167 54 | 0.9,0.6,0.6667,1.0,0.6,0.7667 55 | 0.9,0.8,0.6667,1.0,0.8,0.7667 56 | 0.98,0.15,0.0,0.945,0.25,0.075 57 | 0.98,0.15,0.075,0.9683,0.25,0.15 58 | 0.75,0.15,0.0,0.785,0.25,0.075 59 | 0.75,0.15,0.075,0.7617,0.25,0.15 60 | 0.98,0.3,0.0,0.945,0.4,0.075 61 | 0.98,0.3,0.075,0.9683,0.4,0.15 62 | 0.75,0.3,0.0,0.785,0.4,0.075 63 | 0.75,0.3,0.075,0.7617,0.4,0.15 64 | 0.75,0.7,0.6667,0.785,0.8,0.7417 65 | 0.75,0.7,0.7417,0.7617,0.8,0.8167 66 | 0.75,0.55,0.6667,0.785,0.65,0.7417 67 | 0.75,0.55,0.7417,0.7617,0.65,0.8167 68 | 0.01,0.7,0.3333,0.045,0.8,0.4083 69 | 0.01,0.7,0.4083,0.0217,0.8,0.4833 70 | 0.01,0.55,0.3333,0.045,0.65,0.4083 71 | 0.01,0.55,0.4083,0.0217,0.65,0.4833 72 | 0.01,0.4,0.3333,0.045,0.5,0.4083 73 | 0.01,0.4,0.4083,0.0217,0.5,0.4833 74 | 0.4,0.73,0.6667,0.6,0.8,0.8167 75 | 0.4,0.8,0.6667,0.6,0.87,0.8167 76 | 0.8,0.2,0.15,0.95,0.5,0.155 77 | 0.9475,0.2,0.01,0.95,0.21,0.15 78 | 0.8,0.49,0.01,0.8025,0.5,0.15 79 | 0.9475,0.49,0.01,0.95,0.5,0.15 80 | 0.8,0.2,0.01,0.8025,0.21,0.15 81 | 0.825,0.05,0.4133,0.885,0.2,0.4158 82 | 0.884,0.05,0.3433,0.885,0.055,0.4133 83 | 0.825,0.195,0.3433,0.826,0.2,0.4133 84 | 0.884,0.195,0.3433,0.885,0.2,0.4133 85 | 0.825,0.05,0.3433,0.826,0.055,0.4133 86 | 0.825,0.65,0.7467,0.885,0.8,0.7492 87 | 0.884,0.65,0.6767,0.885,0.655,0.7467 88 | 0.825,0.795,0.6767,0.826,0.8,0.7467 89 | 0.884,0.795,0.6767,0.885,0.8,0.7467 90 | 0.825,0.65,0.6767,0.826,0.655,0.7467 91 | -------------------------------------------------------------------------------- /include/config/obstacles/quadPillars/obstacles.csv: -------------------------------------------------------------------------------- 1 | 1.0, 2.0, 0.0, 5.0, 7.0, 100.0 2 | 8.0, 15.0, 0.0, 12.0, 18.0, 100.0 3 | 13.0, 4.0, 0.0, 17.0, 9.0, 100.0 4 | 21.0, 28.0, 0.0, 25.0, 33.0, 100.0 5 | 27.0, 19.0, 0.0, 32.0, 25.0, 100.0 6 | 34.0, 34.0, 0.0, 38.0, 39.0, 100.0 7 | 41.0, 48.0, 0.0, 46.0, 53.0, 100.0 8 | 47.0, 12.0, 0.0, 52.0, 18.0, 100.0 9 | 53.0, 57.0, 0.0, 58.0, 62.0, 100.0 10 | 59.0, 4.0, 0.0, 63.0, 8.0, 100.0 11 | 64.0, 64.0, 0.0, 69.0, 69.0, 100.0 12 | 71.0, 22.0, 0.0, 76.0, 27.0, 100.0 13 | 73.0, 73.0, 0.0, 78.0, 78.0, 100.0 14 | 82.0, 12.0, 0.0, 87.0, 18.0, 100.0 15 | 84.0, 84.0, 0.0, 89.0, 89.0, 100.0 16 | 92.0, 25.0, 0.0, 97.0, 30.0, 100.0 17 | 94.0, 94.0, 0.0, 99.0, 99.0, 100.0 18 | 6.0, 40.0, 0.0, 11.0, 45.0, 100.0 19 | 15.0, 6.0, 0.0, 19.0, 11.0, 100.0 20 | 21.0, 50.0, 0.0, 26.0, 55.0, 100.0 21 | 31.0, 7.0, 0.0, 36.0, 12.0, 100.0 22 | 33.0, 60.0, 0.0, 37.0, 65.0, 100.0 23 | 45.0, 35.0, 0.0, 50.0, 40.0, 100.0 24 | 52.0, 70.0, 0.0, 57.0, 75.0, 100.0 25 | 60.0, 15.0, 0.0, 65.0, 20.0, 100.0 26 | 63.0, 75.0, 0.0, 68.0, 80.0, 100.0 27 | 74.0, 40.0, 0.0, 79.0, 45.0, 100.0 28 | 77.0, 77.0, 0.0, 82.0, 82.0, 100.0 29 | 85.0, 55.0, 0.0, 90.0, 60.0, 100.0 30 | 90.0, 90.0, 0.0, 95.0, 95.0, 100.0 31 | 13.0, 13.0, 0.0, 18.0, 18.0, 100.0 32 | 22.0, 60.0, 0.0, 27.0, 65.0, 100.0 33 | 30.0, 30.0, 0.0, 35.0, 35.0, 100.0 34 | 38.0, 77.0, 0.0, 43.0, 82.0, 100.0 35 | 44.0, 44.0, 0.0, 49.0, 49.0, 100.0 36 | 56.0, 90.0, 0.0, 61.0, 95.0, 100.0 37 | 66.0, 55.0, 0.0, 71.0, 60.0, 100.0 38 | 72.0, 11.0, 0.0, 77.0, 16.0, 100.0 39 | 79.0, 79.0, 0.0, 84.0, 84.0, 100.0 40 | 85.0, 70.0, 0.0, 90.0, 75.0, 100.0 41 | 91.0, 18.0, 0.0, 96.0, 23.0, 100.0 42 | 2.0, 2.0, 0.0, 6.0, 6.0, 100.0 43 | 7.0, 7.0, 0.0, 11.0, 11.0, 100.0 44 | 20.0, 20.0, 0.0, 24.0, 24.0, 100.0 45 | 27.0, 27.0, 0.0, 31.0, 31.0, 100.0 46 | 36.0, 36.0, 0.0, 40.0, 40.0, 100.0 47 | 50.0, 50.0, 0.0, 54.0, 54.0, 100.0 48 | 55.0, 55.0, 0.0, 59.0, 59.0, 100.0 49 | 70.0, 70.0, 0.0, 74.0, 74.0, 100.0 50 | 75.0, 75.0, 0.0, 79.0, 79.0, 100.0 51 | 80.0, 80.0, 0.0, 84.0, 84.0, 100.0 52 | 90.0, 90.0, 0.0, 94.0, 94.0, 100.0 53 | 5.0, 30.0, 0.0, 9.0, 34.0, 100.0 54 | 14.0, 40.0, 0.0, 18.0, 44.0, 100.0 55 | 23.0, 50.0, 0.0, 27.0, 54.0, 100.0 56 | 32.0, 60.0, 0.0, 36.0, 64.0, 100.0 57 | 41.0, 70.0, 0.0, 45.0, 74.0, 100.0 58 | 50.0, 80.0, 0.0, 54.0, 84.0, 100.0 59 | 59.0, 90.0, 0.0, 63.0, 94.0, 100.0 60 | 20.0, 1.0, 0.0, 24.0, 5.0, 100.0 61 | 28.0, 11.0, 0.0, 32.0, 15.0, 100.0 62 | 36.0, 21.0, 0.0, 40.0, 25.0, 100.0 63 | 44.0, 31.0, 0.0, 48.0, 35.0, 100.0 64 | 50.0, 41.0, 0.0, 54.0, 45.0, 100.0 65 | 56.0, 51.0, 0.0, 60.0, 55.0, 100.0 66 | 62.0, 61.0, 0.0, 66.0, 65.0, 100.0 67 | 68.0, 71.0, 0.0, 72.0, 75.0, 100.0 68 | 74.0, 81.0, 0.0, 78.0, 85.0, 100.0 69 | 80.0, 91.0, 0.0, 84.0, 95.0, 100.0 70 | 90.0, 1.0, 0.0, 94.0, 5.0, 100.0 71 | 96.0, 11.0, 0.0, 100.0, 15.0, 100.0 72 | 30.0, 70.0, 0.0, 35.0, 75.0, 100.0 73 | 45.0, 85.0, 0.0, 50.0, 90.0, 100.0 74 | 55.0, 65.0, 0.0, 60.0, 70.0, 100.0 75 | 65.0, 80.0, 0.0, 70.0, 85.0, 100.0 76 | 75.0, 60.0, 0.0, 80.0, 65.0, 100.0 77 | 85.0, 75.0, 0.0, 90.0, 80.0, 100.0 78 | 95.0, 90.0, 0.0, 100.0, 95.0, 100.0 79 | 0.0, 50.0, 0.0, 5.0, 55.0, 100.0 80 | 10.0, 70.0, 0.0, 20.0, 100.0, 100.0 81 | 40.0, 40.0, 0.0, 70.0, 70.0, 100.0 82 | 70.0, 70.0, 0.0, 90.0, 90.0, 100.0 83 | 0.0, 50.0, 0.0, 25.0, 70.0, 100.0 -------------------------------------------------------------------------------- /include/config/obstacles/quadHouse/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.0,0.0,0.0,100.0,100.0,3.0 2 | 0.0,0.0,33.33,100.0,50.0,36.33 3 | 0.0,50.0,33.33,30.0,100.0,36.33 4 | 0.0,0.0,66.67,100.0,40.0,69.67 5 | 30.0,40.0,66.67,100.0,100.0,69.67 6 | 0.0,20.0,66.67,20.0,100.0,69.67 7 | 20.0,40.0,66.67,40.0,60.0,69.67 8 | 80.0,20.0,66.67,100.0,40.0,69.67 9 | 28.999999999999996,50.0,1.0,31.0,80.0,69.67 10 | 28.999999999999996,80.0,33.33,31.0,100.0,68.67 11 | 10.0,60.0,66.67,30.0,61.0,78.67 12 | 20.0,63.0,33.33,30.0,66.0,38.67 13 | 20.0,66.0,33.33,30.0,69.0,42.00000000000001 14 | 20.0,69.0,33.33,30.0,72.0,45.330000000000005 15 | 20.0,72.0,33.33,30.0,75.0,48.67 16 | 20.0,75.0,33.33,30.0,78.0,52.0 17 | 20.0,78.0,33.33,30.0,81.0,55.33 18 | 20.0,81.0,33.33,30.0,84.0,58.67 19 | 20.0,84.0,33.33,30.0,87.0,62.0 20 | 20.0,87.0,33.33,30.0,90.0,65.33 21 | 20.0,90.0,33.33,30.0,93.0,68.67 22 | 30.0,77.0,0.0,70.0,80.0,5.330000000000001 23 | 30.0,74.0,0.0,70.0,77.0,8.669999999999998 24 | 30.0,71.0,0.0,70.0,74.0,12.0 25 | 30.0,68.0,0.0,70.0,71.0,15.330000000000002 26 | 30.0,65.0,0.0,70.0,68.0,18.67 27 | 30.0,62.0,0.0,70.0,65.0,22.000000000000004 28 | 30.0,59.0,0.0,70.0,62.0,25.330000000000002 29 | 30.0,56.00000000000001,0.0,70.0,59.0,28.67 30 | 30.0,53.0,0.0,70.0,56.00000000000001,32.0 31 | 30.0,50.0,0.0,70.0,53.0,35.33 32 | 10.0,30.0,0.0,11.0,31.0,68.67 33 | 10.0,50.0,0.0,11.0,51.0,68.67 34 | 10.0,70.0,0.0,11.0,71.0,68.67 35 | 90.0,1.0,33.33,100.0,21.0,40.33 36 | 97.5,1.0,38.33,100.0,21.0,50.33 37 | 90.0,1.0,33.33,100.0,1.0,45.330000000000005 38 | 90.0,21.0,33.33,100.0,21.0,45.330000000000005 39 | 80.0,1.0,33.33,70.0,21.0,40.33 40 | 72.5,1.0,38.33,70.0,21.0,50.33 41 | 80.0,1.0,33.33,70.0,1.0,45.330000000000005 42 | 80.0,21.0,33.33,70.0,21.0,45.330000000000005 43 | 90.0,60.0,66.67,100.0,80.0,73.67 44 | 97.5,60.0,71.67,100.0,80.0,83.67 45 | 90.0,60.0,66.67,100.0,60.0,78.67 46 | 90.0,80.0,66.67,100.0,80.0,78.67 47 | 98.0,15.0,0.0,94.5,25.0,9.499999999999998 48 | 98.0,15.0,7.5,96.83,25.0,17.0 49 | 75.0,15.0,0.0,78.5,25.0,9.499999999999998 50 | 75.0,15.0,7.5,76.17,25.0,17.0 51 | 98.0,30.0,0.0,94.5,40.0,9.499999999999998 52 | 98.0,30.0,7.5,96.83,40.0,17.0 53 | 75.0,30.0,0.0,78.5,40.0,9.499999999999998 54 | 75.0,30.0,7.5,76.17,40.0,17.0 55 | 75.0,70.0,66.67,78.5,80.0,76.17 56 | 75.0,70.0,74.17,76.17,80.0,83.67 57 | 75.0,55.00000000000001,66.67,78.5,65.0,76.17 58 | 75.0,55.00000000000001,74.17,76.17,65.0,83.67 59 | 1.0,70.0,33.33,4.5,80.0,42.83 60 | 1.0,70.0,40.83,2.17,80.0,50.33 61 | 1.0,55.00000000000001,33.33,4.5,65.0,42.83 62 | 1.0,55.00000000000001,40.83,2.17,65.0,50.33 63 | 1.0,40.0,33.33,4.5,50.0,42.83 64 | 1.0,40.0,40.83,2.17,50.0,50.33 65 | 40.0,73.0,66.67,60.0,80.0,83.67 66 | 40.0,80.0,66.67,60.0,87.0,83.67 67 | 80.0,20.0,15.0,95.0,50.0,17.5 68 | 94.75,20.0,1.0,95.0,21.0,17.0 69 | 80.0,49.0,1.0,80.25,50.0,17.0 70 | 94.75,49.0,1.0,95.0,50.0,17.0 71 | 80.0,20.0,1.0,80.25,21.0,17.0 72 | 82.5,5.0,41.33,88.5,20.0,43.580000000000005 73 | 88.4,5.0,34.33,88.5,5.5,43.33 74 | 82.5,19.5,34.33,82.6,20.0,43.33 75 | 88.4,19.5,34.33,88.5,20.0,43.33 76 | 82.5,5.0,34.33,82.6,5.5,43.33 77 | 82.5,65.0,74.67,88.5,80.0,76.92 78 | 88.4,65.0,67.67,88.5,65.5,76.67 79 | 82.5,79.5,67.67,82.6,80.0,76.67 80 | 88.4,79.5,67.67,88.5,80.0,76.67 81 | 82.5,65.0,67.67,82.6,65.5,76.67 82 | -------------------------------------------------------------------------------- /src/planners/Planner.cu: -------------------------------------------------------------------------------- 1 | #include "planners/Planner.cuh" 2 | #include "config/config.h" 3 | 4 | Planner::Planner() 5 | { 6 | d_treeSamples_ = thrust::device_vector(MAX_TREE_SIZE * SAMPLE_DIM); 7 | d_treeSamplesParentIdxs_ = thrust::device_vector(MAX_TREE_SIZE); 8 | d_treeSampleCosts_ = thrust::device_vector(MAX_TREE_SIZE); 9 | d_controlPathToGoal_ = thrust::device_vector(MAX_ITER * SAMPLE_DIM); 10 | 11 | d_treeSamples_ptr_ = thrust::raw_pointer_cast(d_treeSamples_.data()); 12 | d_treeSamplesParentIdxs_ptr_ = thrust::raw_pointer_cast(d_treeSamplesParentIdxs_.data()); 13 | d_treeSampleCosts_ptr_ = thrust::raw_pointer_cast(d_treeSampleCosts_.data()); 14 | d_controlPathToGoal_ptr_ = thrust::raw_pointer_cast(d_controlPathToGoal_.data()); 15 | 16 | h_gridSize_ = iDivUp(MAX_TREE_SIZE, h_blockSize_); 17 | 18 | cudaMalloc(&d_randomSeeds_ptr_, MAX_TREE_SIZE * sizeof(curandState)); 19 | cudaMalloc(&d_costToGoal_ptr_, sizeof(float)); 20 | cudaMalloc(&d_pathToGoal_ptr_, sizeof(int)); 21 | 22 | h_controlPathToGoal_ = new float[SAMPLE_DIM * MAX_ITER]; 23 | 24 | if(VERBOSE) 25 | { 26 | printf("/***************************/\n"); 27 | printf("/* Workspace Dimension: %d */\n", W_DIM); 28 | printf("/* Workspace Size: %f */\n", W_SIZE); 29 | printf("/* Maximum discretization steps in propagation: %d */\n", MAX_PROPAGATION_DURATION); 30 | printf("/* Propagation step Size: %f */\n", STEP_SIZE); 31 | printf("/* Max Tree Size: %d */\n", MAX_TREE_SIZE); 32 | printf("/* Goal Distance Threshold: %f */\n", GOAL_THRESH); 33 | printf("/* Max Planning Iterations: %d */\n", MAX_ITER); 34 | } 35 | } 36 | 37 | __global__ void initializeRandomSeeds_kernel(curandState* randomSeeds, int numSeeds, int seed) 38 | { 39 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 40 | if(tid < numSeeds) 41 | { 42 | curand_init(seed, tid, 0, &randomSeeds[tid]); 43 | } 44 | } 45 | 46 | void Planner::initializeRandomSeeds(int seed) 47 | { 48 | int blockSize = 32; 49 | initializeRandomSeeds_kernel<<>>(d_randomSeeds_ptr_, MAX_TREE_SIZE, seed); 50 | } 51 | 52 | __global__ void findInd(uint numSamples, bool* S, uint* scanIdx, uint* activeS) 53 | { 54 | int tid = blockIdx.x * blockDim.x + threadIdx.x; 55 | if(tid >= numSamples) return; 56 | if(!S[tid]) return; 57 | activeS[scanIdx[tid]] = tid; 58 | } 59 | 60 | __global__ void findInd(uint numSamples, uint* S, uint* scanIdx, uint* activeS) 61 | { 62 | int node = blockIdx.x * blockDim.x + threadIdx.x; 63 | if(node >= numSamples) return; 64 | if(!S[node]) return; 65 | activeS[scanIdx[node]] = node; 66 | } 67 | 68 | __global__ void repeatInd(uint numSamples, uint* activeS, uint* C, uint* prefixSum, uint* repeatedInd) 69 | { 70 | int tid = blockIdx.x * blockDim.x + threadIdx.x; 71 | if(tid >= numSamples) return; 72 | 73 | uint index = activeS[tid]; 74 | uint count = C[index]; 75 | uint startPos = prefixSum[index]; 76 | for(uint i = 0; i < count; ++i) 77 | { 78 | repeatedInd[startPos + i] = index; 79 | } 80 | } -------------------------------------------------------------------------------- /include/planners/KPAX.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "planners/Planner.cuh" 3 | #include "graphs/Graph.cuh" 4 | 5 | class KPAX : public Planner 6 | { 7 | public: 8 | /**************************** CONSTRUCTORS ****************************/ 9 | KPAX(); 10 | 11 | /**************************** METHODS ****************************/ 12 | void plan(float* h_initial, float* h_goal, float* d_obstacles_ptr, uint h_obstaclesCount) override; 13 | void planBench(float* h_initial, float* h_goal, float* d_obstacles_ptr, uint h_obstaclesCount, int benchItr); 14 | void propagateFrontier(float* d_obstacles_ptr, uint h_obstaclesCount); 15 | void updateFrontier(); 16 | void writeDeviceVectorsToCSV(int itr); 17 | void writeExecutionTimeToCSV(double time); 18 | 19 | /**************************** FIELDS ****************************/ 20 | // --- host fields --- 21 | Graph graph_; 22 | uint h_frontierSize_, h_frontierNextSize_, h_activeBlockSize_, h_frontierRepeatSize_, h_propIterations_; 23 | float h_fAccept_; 24 | 25 | // --- device fields --- 26 | thrust::device_vector d_frontier_, d_frontierNext_; 27 | thrust::device_vector d_activeFrontierIdxs_, d_frontierScanIdx_, d_activeFrontierRepeatCount_, d_frontierRepeatScanIdx_, 28 | d_activeFrontierRepeatIdxs_; 29 | thrust::device_vector d_unexploredSamplesParentIdxs_; 30 | thrust::device_vector d_unexploredSamples_, d_goalSample_; 31 | float *d_unexploredSamples_ptr_, *d_goalSample_ptr_; 32 | bool *d_frontier_ptr_, *d_frontierNext_ptr_; 33 | uint *d_activeFrontierIdxs_ptr_, *d_frontierScanIdx_ptr_, *d_activeFrontierRepeatCount_ptr_, *d_frontierRepeatScanIdx_ptr_, 34 | *d_activeFrontierRepeatIdxs_ptr_; 35 | int* d_unexploredSamplesParentIdxs_ptr_; 36 | }; 37 | 38 | /**************************** DEVICE FUNCTIONS ****************************/ 39 | 40 | /***************************/ 41 | /* PROPAGATE FRONTIER KERNEL 1 */ 42 | /***************************/ 43 | // --- Propagates current frontier. Builds new frontier. --- 44 | // --- One Block Per Frontier Sample --- 45 | __global__ void propagateFrontier_kernel1(bool* frontier, uint* activeFrontierIdxs, float* treeSamples, float* unexploredSamples, 46 | uint frontierSize, curandState* randomSeeds, int* unexploredSamplesParentIdxs, float* obstacles, 47 | int obstaclesCount, int* activeSubVertices, float* vertexScores, bool* frontierNext, 48 | int* vertexCounter, int* validVertexCounter, float* minValueInRegion); 49 | 50 | __global__ void propagateFrontier_kernel2(bool* frontier, uint* activeFrontierIdxs, float* treeSamples, float* unexploredSamples, 51 | uint frontierSize, curandState* randomSeeds, int* unexploredSamplesParentIdxs, float* obstacles, 52 | int obstaclesCount, int* activeSubVertices, float* vertexScores, bool* frontierNext, 53 | int* vertexCounter, int* validVertexCounter, int iterations, float* minValueInRegion); 54 | 55 | __global__ void 56 | updateFrontier_kernel(bool* frontier, bool* frontierNext, uint* activeFrontierNextIdxs, uint frontierNextSize, float* xGoal, int treeSize, 57 | float* unexploredSamples, float* treeSamples, int* unexploredSamplesParentIdxs, int* treeSamplesParentIdxs, 58 | float* treeSampleCosts, int* pathToGoal, uint* activeFrontierRepeatCount, int* validVertexCounter, 59 | curandState* randomSeeds, float* vertexScores, float* controlPathToGoal, float fAccept); -------------------------------------------------------------------------------- /viz/KGMT_3D_single.asv: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 1; 7 | width = 20.0; 8 | height = 20.0; 9 | depth = 20.0; 10 | radius = 0.05; 11 | N = 8; 12 | n = 4; 13 | R1_width = width / N; 14 | R1_height = height / N; 15 | R1_depth = depth / N; 16 | R2_width = R1_width / n; 17 | R2_height = R1_height / n; 18 | R2_depth = R1_depth / n; 19 | numDisc = 10; 20 | sampleSize = 10; 21 | stateSize = 6; 22 | controlSize = 3; 23 | xGoal = [.0, .0, .0]; 24 | alphaValue = 0.1; 25 | 26 | % Obstacle file path 27 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles//obstacles.csv'; 28 | obstacles = readmatrix(obstacleFilePath); 29 | 30 | for i = 1:numFiles 31 | % Construct file paths 32 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/samples.csv"; 33 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/parentRelations.csv"; 34 | 35 | % Read data from files 36 | samples = readmatrix(sampleFilePath); 37 | parentRelations = readmatrix(parentFilePath); 38 | 39 | figure; 40 | hold on; 41 | axis equal; 42 | xlabel('X Position'); 43 | ylabel('Y Position'); 44 | zlabel('Z Position'); 45 | plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'k'); 46 | 47 | % Plot goal region 48 | [X, Y, Z] = sphere(20); 49 | surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 50 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 51 | 52 | % Plot obstacles 53 | for j = 1:size(obstacles, 1) 54 | x_min = obstacles(j, 1); 55 | y_min = obstacles(j, 2); 56 | z_min = obstacles(j, 3); 57 | x_max = obstacles(j, 4); 58 | y_max = obstacles(j, 5); 59 | z_max = obstacles(j, 6); 60 | vertices = [ 61 | x_min, y_min, z_min; 62 | x_max, y_min, z_min; 63 | x_max, y_max, z_min; 64 | x_min, y_max, z_min; 65 | x_min, y_min, z_max; 66 | x_max, y_min, z_max; 67 | x_max, y_max, z_max; 68 | x_min, y_max, z_max]; 69 | faces = [ 70 | 1, 2, 6, 5; 71 | 2, 3, 7, 6; 72 | 3, 4, 8, 7; 73 | 4, 1, 5, 8; 74 | 1, 2, 3, 4; 75 | 5, 6, 7, 8]; 76 | patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', 0.4); 77 | end 78 | 79 | % Add light source 80 | camlight('headlight'); 81 | lighting gouraud; 82 | 83 | % Plot paths 84 | for j = 2:size(parentRelations, 1) 85 | if parentRelations(j) == -1 86 | break; 87 | end 88 | % x0 = samples((parentRelations(j) + 1), 1:stateSize); 89 | % segmentX = [x0(1)]; 90 | % segmentY = [x0(2)]; 91 | % segmentZ = [x0(3)]; 92 | % u = samples(j, stateSize+1:sampleSize-1); 93 | % duration = samples(j, sampleSize); 94 | % dt = duration / numDisc; 95 | % x = x0(1); 96 | % y = x0(2); 97 | % z = x0(3); 98 | % vx = x0(4); 99 | % vy = x0(5); 100 | % vz = x0(6); 101 | % for k = 1:(numDisc) 102 | % x = x + vx*dt; 103 | % y = y + vy*dt; 104 | % z = z + vz*dt; 105 | % vx = vx + u(1)*dt; 106 | % vy = vy + u(2)*dt; 107 | % vz = vz + u(3)*dt; 108 | % segmentX = [segmentX, x]; 109 | % segmentY = [segmentY, y]; 110 | % segmentZ = [segmentZ, z]; 111 | % end 112 | % plot3(segmentX, segmentY, segmentZ, '-.', 'Color', 'k', 'LineWidth', 0.01); 113 | plot3(samples(j, 1), samples(j, 2), samples(j, 3), 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 2); 114 | end 115 | 116 | view(2); 117 | drawnow; 118 | saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 119 | end -------------------------------------------------------------------------------- /include/helper/helper.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "cuda.h" 12 | #include "cuda_runtime.h" 13 | #include 14 | #include 15 | #include 16 | #include "config/config.h" 17 | #define _USE_MATH_DEFINES 18 | 19 | // A macro for checking the error codes of cuda runtime calls 20 | #define CUDA_ERROR_CHECK(expr) \ 21 | { \ 22 | cudaError_t err = expr; \ 23 | if(err != cudaSuccess) \ 24 | { \ 25 | printf("CUDA call failed!\n\t%s\n", cudaGetErrorString(err)); \ 26 | exit(1); \ 27 | } \ 28 | } 29 | 30 | template 31 | void printDeviceVector(const T* d_ptr, int size); 32 | 33 | __device__ void printSample(float* x, int sampleDim); 34 | std::vector readObstaclesFromCSV(const std::string& filename, int& numObstacles, int workspaceDim); 35 | 36 | template 37 | void writeVectorToCSV(const thrust::host_vector& vec, const std::string& filename, int rows, int cols); 38 | 39 | template 40 | void copyAndWriteVectorToCSV(const thrust::device_vector& d_vec, const std::string& filename, int rows, int cols); 41 | 42 | // Implement the template functions in the header file 43 | 44 | template 45 | void printDeviceVector(const T* d_ptr, int size) 46 | { 47 | thrust::host_vector h_vec(size); 48 | cudaMemcpy(thrust::raw_pointer_cast(h_vec.data()), d_ptr, size * sizeof(T), cudaMemcpyDeviceToHost); 49 | for(int i = 0; i < size; ++i) 50 | { 51 | std::cout << h_vec[i] << " "; 52 | } 53 | std::cout << std::endl; 54 | } 55 | 56 | template 57 | void writeVectorToCSV(const thrust::host_vector& vec, const std::string& filename, int rows, int cols, bool append = false) 58 | { 59 | std::ofstream file; 60 | if(append) 61 | { 62 | file.open(filename, std::ios_base::app); // Open in append mode 63 | } 64 | else 65 | { 66 | file.open(filename); 67 | } 68 | file << std::fixed << std::setprecision(6); 69 | 70 | for(int i = 0; i < rows; i++) 71 | { 72 | for(int j = 0; j < cols; j++) 73 | { 74 | file << vec[i * cols + j]; 75 | if(j < cols - 1) 76 | { 77 | file << ","; 78 | } 79 | } 80 | file << std::endl; 81 | } 82 | 83 | file.close(); 84 | } 85 | 86 | template 87 | void copyAndWriteVectorToCSV(const thrust::device_vector& d_vec, const std::string& filename, int rows, int cols, bool append = false) 88 | { 89 | thrust::host_vector h_vec(d_vec.size()); 90 | cudaMemcpy(thrust::raw_pointer_cast(h_vec.data()), thrust::raw_pointer_cast(d_vec.data()), d_vec.size() * sizeof(T), 91 | cudaMemcpyDeviceToHost); 92 | writeVectorToCSV(h_vec, filename, rows, cols, append); 93 | } 94 | 95 | template 96 | inline void writeValueToCSV(const T& value, const std::string& filename) 97 | { 98 | std::ofstream file; 99 | file.open(filename, std::ios_base::app); // Open in append mode 100 | 101 | // Set precision for floating-point numbers 102 | if constexpr(std::is_floating_point_v>) 103 | { 104 | file << std::fixed << std::setprecision(10); 105 | } 106 | 107 | file << value << std::endl; 108 | file.close(); 109 | } 110 | 111 | __device__ __forceinline__ float atomicMinFloat(float* addr, float value) 112 | { 113 | float old; 114 | old = (value >= 0) ? __int_as_float(atomicMin((int*)addr, __float_as_int(value))) 115 | : __uint_as_float(atomicMax((unsigned int*)addr, __float_as_uint(value))); 116 | 117 | return old; 118 | } 119 | 120 | #define gpuErrchk(ans) \ 121 | { \ 122 | gpuAssert((ans), __FILE__, __LINE__); \ 123 | } 124 | inline void gpuAssert(cudaError_t code, const char* file, int line, bool abort = true) 125 | { 126 | if(code != cudaSuccess) 127 | { 128 | fprintf(stderr, "GPUassert: %s %s %d\n", cudaGetErrorString(code), file, line); 129 | if(abort) exit(code); 130 | } 131 | } 132 | 133 | inline int iDivUp(int a, int b) 134 | { 135 | return (a + b - 1) / b; 136 | } 137 | 138 | __device__ __forceinline__ float distance(float* a, float* b) 139 | { 140 | if(W_DIM == 2) 141 | { 142 | return sqrt(pow(a[0] - b[0], 2) + pow(a[1] - b[1], 2)); 143 | } 144 | else if(W_DIM == 3) 145 | { 146 | return sqrt(pow(a[0] - b[0], 2) + pow(a[1] - b[1], 2) + pow(a[2] - b[2], 2)); 147 | } 148 | else 149 | { 150 | return -1; 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /viz/OMPL_3D.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 1; 7 | radius = 5; 8 | N = 8; 9 | n = 4; 10 | sampleSize = 10; 11 | stateSize = 6; 12 | controlSize = 3; 13 | xGoal = [80, 95, 90]; 14 | alphaValue = 0.7; 15 | STEP_SIZE = .1; 16 | 17 | % Obstacle file path 18 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/quadNarrowPassage/obstacles.csv'; 19 | obstacles = readmatrix(obstacleFilePath); 20 | 21 | for i = 1:numFiles 22 | 23 | outputFilePath = "/home/nicolas/dev/research/KPAX/build/solutions/Output.csv"; 24 | samples = readmatrix(outputFilePath); 25 | 26 | figure; 27 | hold on; 28 | axis equal; 29 | xlabel('X Position'); 30 | ylabel('Y Position'); 31 | zlabel('Z Position'); 32 | plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'k'); 33 | 34 | % goal sphere plot 35 | [X, Y, Z] = sphere(20); 36 | surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 37 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 38 | 39 | % Obstacle Plot 40 | for j = 1:size(obstacles, 1) 41 | x_min = obstacles(j, 1); 42 | y_min = obstacles(j, 2); 43 | z_min = obstacles(j, 3); 44 | x_max = obstacles(j, 4); 45 | y_max = obstacles(j, 5); 46 | z_max = obstacles(j, 6); 47 | vertices = [ 48 | x_min, y_min, z_min; 49 | x_max, y_min, z_min; 50 | x_max, y_max, z_min; 51 | x_min, y_max, z_min; 52 | x_min, y_min, z_max; 53 | x_max, y_min, z_max; 54 | x_max, y_max, z_max; 55 | x_min, y_max, z_max]; 56 | faces = [ 57 | 1, 2, 6, 5; 58 | 2, 3, 7, 6; 59 | 3, 4, 8, 7; 60 | 4, 1, 5, 8; 61 | 1, 2, 3, 4; 62 | 5, 6, 7, 8]; 63 | patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', 0.2); 64 | end 65 | 66 | % Add light source 67 | camlight('headlight'); 68 | lighting gouraud; 69 | 70 | % Plot paths 71 | segmentX = []; 72 | segmentY = []; 73 | segmentZ = []; 74 | for j = 2:size(samples, 1) 75 | x0 = samples(j-1, 1:stateSize); 76 | segmentX = [x0(1)]; 77 | segmentY = [x0(2)]; 78 | segmentZ = [x0(3)]; 79 | u = samples(j, stateSize+1:sampleSize-1); 80 | duration = samples(j, sampleSize); 81 | numDisc = duration/STEP_SIZE; 82 | % x = x0(1); 83 | % y = x0(2); 84 | % z = x0(3); 85 | % vx = x0(4); 86 | % vy = x0(5); 87 | % vz = x0(6); 88 | % ax = u(1); 89 | % ay = u(2); 90 | % az = u(3); 91 | x = x0(1); 92 | y = x0(2); 93 | z = x0(3); 94 | yaw = x0(4); 95 | pitch = x0(5); 96 | v = x0(6); 97 | yawRate = u(1); 98 | pitchRate = u(2); 99 | % a = u(3); 100 | % for k = 1:(numDisc) 101 | % % x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 102 | % % y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 103 | % % z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 104 | % % vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 105 | % % vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 106 | % % vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 107 | % x = x + (STEP_SIZE / 6.0) * ... 108 | % (v * cos(pitch) * cos(yaw) + ... 109 | % 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate) + ... 110 | % (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 111 | % (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * cos(yaw + STEP_SIZE * yawRate)); 112 | % 113 | % y = y + (STEP_SIZE / 6.0) * ... 114 | % (v * cos(pitch) * sin(yaw) + ... 115 | % 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate) + ... 116 | % (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 117 | % (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * sin(yaw + STEP_SIZE * yawRate)); 118 | % 119 | % z = z + (STEP_SIZE / 6.0) * ... 120 | % (v * sin(pitch) + ... 121 | % 2.0 * ((v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate) + ... 122 | % (v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate)) + ... 123 | % (v + STEP_SIZE * a) * sin(pitch + STEP_SIZE * pitchRate)); 124 | % 125 | % yaw = yaw + STEP_SIZE * yawRate; 126 | % pitch = pitch + STEP_SIZE * pitchRate; 127 | % v = v + (STEP_SIZE / 6.0) * (a + 2.0 * (a + a) + a); 128 | % segmentX = [segmentX, x]; 129 | % segmentY = [segmentY, y]; 130 | % segmentZ = [segmentZ, z]; 131 | % end 132 | % segmentX = [segmentX, samples(j, 1)]; 133 | % segmentY = [segmentY, samples(j, 2)]; 134 | % segmentZ = [segmentZ, samples(j, 3)]; 135 | % plot3(segmentX, segmentY, segmentZ, '-.', 'Color', 'k', 'LineWidth', 1); 136 | plot3(x, y, z, 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 2); 137 | end 138 | 139 | view(3); 140 | drawnow; 141 | end 142 | -------------------------------------------------------------------------------- /include/config/obstacles/trees/obstacles.csv: -------------------------------------------------------------------------------- 1 | 0.507722,0.378436,0.0,0.5337019999999999,0.393436,0.2 2 | 0.513212,0.372946,0.0,0.5282119999999999,0.398926,0.2 3 | 0.468752,0.355936,0.2,0.572672,0.415936,0.6 4 | 0.490712,0.333976,0.2,0.550712,0.437896,0.6 5 | 0.494732,0.370936,0.6,0.546692,0.40093599999999996,1.0 6 | 0.5057119999999999,0.359956,0.6,0.535712,0.411916,1.0 7 | 0.264212,0.715526,0.0,0.277212,0.7230260000000001,0.75 8 | 0.266962,0.7127760000000001,0.0,0.274462,0.725766,0.75 9 | 0.205762,0.681776,0.5,0.335662,0.756776,1.0 10 | 0.233212,0.654316,0.5,0.308212,0.7842260000000001,1.0 11 | 0.757722,0.156216,0.0,0.7837019999999999,0.17121599999999998,0.2 12 | 0.763212,0.150727,0.0,0.7782119999999999,0.176706,0.2 13 | 0.718752,0.133717,0.2,0.822672,0.193716,0.6 14 | 0.7407119999999999,0.111756,0.2,0.800712,0.21567599999999998,0.6 15 | 0.744732,0.148717,0.6,0.796692,0.17871599999999999,1.0 16 | 0.7557119999999999,0.137736,0.6,0.785712,0.18969599999999998,1.0 17 | 0.139212,0.493296,0.0,0.15221200000000001,0.500796,0.75 18 | 0.141962,0.490556,0.0,0.149462,0.503546,0.75 19 | 0.08076,0.459546,0.5,0.21066200000000002,0.534546,1.0 20 | 0.108212,0.432096,0.5,0.183212,0.562006,1.0 21 | 0.639212,0.826636,0.0,0.6522119999999999,0.834136,0.75 22 | 0.6419619999999999,0.823886,0.0,0.649462,0.8368760000000001,0.75 23 | 0.580762,0.7928860000000001,0.5,0.7106619999999999,0.867886,1.0 24 | 0.608212,0.765436,0.5,0.6832119999999999,0.895336,1.0 25 | 0.382722,0.267326,0.0,0.408702,0.282326,0.2 26 | 0.388212,0.261836,0.0,0.403212,0.287816,0.2 27 | 0.343752,0.244826,0.2,0.447672,0.304826,0.6 28 | 0.365712,0.222866,0.2,0.42571200000000003,0.32678599999999997,0.6 29 | 0.369732,0.259826,0.6,0.421692,0.289826,1.0 30 | 0.380712,0.248846,0.6,0.410712,0.300806,1.0 31 | 0.882722,0.600666,0.0,0.9087019999999999,0.615666,0.2 32 | 0.888212,0.595176,0.0,0.9032119999999999,0.621156,0.2 33 | 0.843752,0.5781660000000001,0.2,0.947672,0.638166,0.6 34 | 0.8657119999999999,0.556196,0.2,0.925712,0.660126,0.6 35 | 0.869732,0.5931660000000001,0.6,0.921692,0.623166,1.0 36 | 0.8807119999999999,0.582176,0.6,0.910712,0.634146,1.0 37 | 0.07671700000000001,0.9377460000000001,0.0,0.08970700000000001,0.945246,0.75 38 | 0.079462,0.934996,0.0,0.08696200000000001,0.947986,0.75 39 | 0.0182601,0.903996,0.5,0.14816200000000002,0.9789960000000001,1.0 40 | 0.045712,0.876546,0.5,0.12071200000000001,1.006446,1.0 41 | 0.576712,0.085893,0.0,0.5897119999999999,0.093393,0.75 42 | 0.5794619999999999,0.083148,0.0,0.586962,0.096138,0.75 43 | 0.518262,0.05214304,0.5,0.6481619999999999,0.127143,1.0 44 | 0.545712,0.024691,0.5,0.6207119999999999,0.154596,1.0 45 | 0.320222,0.415476,0.0,0.346202,0.43047599999999997,0.2 46 | 0.325712,0.40998599999999996,0.0,0.340712,0.43596599999999996,0.2 47 | 0.281252,0.392976,0.2,0.385172,0.452976,0.6 48 | 0.303212,0.371016,0.2,0.36321200000000003,0.47493599999999997,0.6 49 | 0.307232,0.407976,0.6,0.359192,0.437976,1.0 50 | 0.318212,0.39699599999999996,0.6,0.348212,0.44895599999999997,1.0 51 | 0.826712,0.752556,0.0,0.8397119999999999,0.7600560000000001,0.75 52 | 0.8294619999999999,0.749816,0.0,0.836962,0.7628060000000001,0.75 53 | 0.768262,0.7188060000000001,0.5,0.8981619999999999,0.793806,1.0 54 | 0.795712,0.6913560000000001,0.5,0.8707119999999999,0.821266,1.0 55 | 0.195222,0.19325599999999998,0.0,0.221202,0.208256,0.2 56 | 0.200712,0.187766,0.0,0.21571200000000001,0.213746,0.2 57 | 0.156252,0.17075600000000002,0.2,0.260172,0.23075600000000002,0.6 58 | 0.178212,0.148793,0.2,0.238212,0.252716,0.6 59 | 0.182232,0.18575599999999998,0.6,0.234192,0.215756,1.0 60 | 0.193212,0.174776,0.6,0.22321200000000002,0.226736,1.0 61 | 0.695222,0.526586,0.0,0.7212019999999999,0.541586,0.2 62 | 0.700712,0.521096,0.0,0.7157119999999999,0.547076,0.2 63 | 0.656252,0.504086,0.2,0.760172,0.5640860000000001,0.6 64 | 0.6782119999999999,0.482126,0.2,0.738212,0.5860460000000001,0.6 65 | 0.682232,0.519086,0.6,0.734192,0.549086,1.0 66 | 0.6932119999999999,0.5081060000000001,0.6,0.723212,0.5600660000000001,1.0 67 | 0.445222,0.859916,0.0,0.471202,0.874916,0.2 68 | 0.450712,0.854426,0.0,0.465712,0.8804160000000001,0.2 69 | 0.406252,0.837416,0.2,0.510172,0.897416,0.6 70 | 0.428212,0.8154560000000001,0.2,0.48821200000000003,0.919386,0.6 71 | 0.432232,0.8524160000000001,0.6,0.484192,0.8824160000000001,1.0 72 | 0.443212,0.8414360000000001,0.6,0.473212,0.893406,1.0 73 | 0.945222,0.30436599999999997,0.0,0.9712019999999999,0.319366,0.2 74 | 0.950712,0.298876,0.0,0.9657119999999999,0.324856,0.2 75 | 0.906252,0.281866,0.2,1.010172,0.341866,0.6 76 | 0.9282119999999999,0.259906,0.2,0.988212,0.363826,0.6 77 | 0.932232,0.296866,0.6,0.984192,0.326866,1.0 78 | 0.9432119999999999,0.285886,0.6,0.973212,0.337846,1.0 79 | 0.038972,0.637696,0.0,0.06495200000000001,0.652696,0.2 80 | 0.044462,0.632206,0.0,0.059462,0.658186,0.2 81 | 0.0,0.6151960000000001,0.2,0.10392399999999999,0.675196,0.6 82 | 0.021962000000000002,0.5932360000000001,0.2,0.08196200000000001,0.697156,0.6 83 | 0.025981200000000003,0.6301960000000001,0.6,0.077943,0.660196,1.0 84 | 0.036962,0.619216,0.6,0.066962,0.671176,1.0 85 | 0.538972,0.971036,0.0,0.5649519999999999,0.986036,0.2 86 | 0.544462,0.965546,0.0,0.5594619999999999,0.991526,0.2 87 | 0.500002,0.948536,0.2,0.603922,1.0085359999999999,0.6 88 | 0.5219619999999999,0.926566,0.2,0.581962,1.030496,0.6 89 | 0.525982,0.9635360000000001,0.6,0.577942,0.9935360000000001,1.0 90 | 0.5369619999999999,0.9525560000000001,0.6,0.566962,1.004516,1.0 91 | 0.295462,0.12293,0.0,0.308462,0.13043,0.75 92 | 0.29821200000000003,0.120185,0.0,0.305712,0.133175,0.75 93 | 0.237012,0.08918000000000001,0.5,0.366912,0.164176,1.0 94 | 0.264462,0.0617282,0.5,0.339462,0.19163599999999997,1.0 95 | 0.788972,0.452516,0.0,0.8149519999999999,0.467516,0.2 96 | 0.794462,0.447026,0.0,0.8094619999999999,0.473006,0.2 97 | 0.750002,0.430016,0.2,0.853922,0.490016,0.6 98 | 0.7719619999999999,0.408056,0.2,0.831962,0.511976,0.6 99 | 0.775982,0.44501599999999997,0.6,0.827942,0.475016,1.0 100 | 0.7869619999999999,0.434036,0.6,0.816962,0.485996,1.0 101 | 0.163972,0.785846,0.0,0.189952,0.8008460000000001,0.2 102 | 0.169462,0.780356,0.0,0.18446200000000001,0.806336,0.2 103 | 0.125002,0.7633460000000001,0.2,0.22892200000000001,0.823346,0.6 104 | 0.146962,0.741386,0.2,0.206962,0.845306,0.6 105 | 0.150982,0.7783460000000001,0.6,0.202942,0.808346,1.0 106 | 0.161962,0.767366,0.6,0.19196200000000002,0.819326,1.0 107 | 0.670462,0.23404599999999998,0.0,0.6834619999999999,0.24154599999999998,0.75 108 | 0.6732119999999999,0.231296,0.0,0.680712,0.244286,0.75 109 | 0.612012,0.20029599999999997,0.5,0.7419119999999999,0.275296,1.0 110 | 0.639462,0.172836,0.5,0.7144619999999999,0.30274599999999996,1.0 111 | 0.413972,0.5636260000000001,0.0,0.439952,0.5786260000000001,0.2 112 | 0.419462,0.5581360000000001,0.0,0.434462,0.5841160000000001,0.2 113 | 0.375002,0.541126,0.2,0.478922,0.601126,0.6 114 | 0.396962,0.519166,0.2,0.45696200000000003,0.623086,0.6 115 | 0.400982,0.556126,0.6,0.452942,0.586126,1.0 116 | 0.411962,0.545146,0.6,0.441962,0.597106,1.0 117 | 0.913972,0.8969560000000001,0.0,0.9399519999999999,0.911956,0.2 118 | 0.919462,0.8914660000000001,0.0,0.9344619999999999,0.9174460000000001,0.2 119 | 0.875002,0.874456,0.2,0.978922,0.9344560000000001,0.6 120 | 0.8969619999999999,0.852496,0.2,0.956962,0.956416,0.6 121 | 0.900982,0.889456,0.6,0.952942,0.919456,1.0 122 | 0.9119619999999999,0.878476,0.6,0.941962,0.930436,1.0 123 | 0.101472,0.341406,0.0,0.127452,0.356406,0.2 124 | 0.106962,0.335916,0.0,0.12196200000000001,0.361896,0.2 125 | 0.0625,0.31890599999999997,0.2,0.16642200000000001,0.37890599999999997,0.6 126 | 0.08446200000000001,0.296936,0.2,0.144462,0.400866,0.6 127 | 0.088481,0.333906,0.6,0.140442,0.363906,1.0 128 | 0.099462,0.322926,0.6,0.129462,0.374886,1.0 129 | 0.601472,0.674736,0.0,0.6274519999999999,0.689736,0.2 130 | 0.606962,0.669246,0.0,0.6219619999999999,0.695226,0.2 131 | 0.562502,0.652236,0.2,0.666422,0.7122360000000001,0.6 132 | 0.5844619999999999,0.6302760000000001,0.2,0.644462,0.7341960000000001,0.6 133 | 0.588482,0.667236,0.6,0.640442,0.6972360000000001,1.0 134 | 0.5994619999999999,0.6562560000000001,0.6,0.629462,0.7082160000000001,1.0 135 | 0.351472,1.008066,0.0,0.377452,1.023066,0.2 136 | 0.356962,1.002576,0.0,0.371962,1.028556,0.2 137 | 0.312502,0.985566,0.2,0.416422,1.045566,0.6 138 | 0.334462,0.9636060000000001,0.2,0.39446200000000003,1.0675059999999998,0.6 139 | 0.338482,1.000566,0.6,0.390442,1.030566,1.0 140 | 0.349462,0.9895860000000001,0.6,0.379462,1.041546,1.0 141 | 0.857962,0.0612017,0.0,0.8709619999999999,0.068702,0.75 142 | 0.8607119999999999,0.0584565,0.0,0.868212,0.071447,0.75 143 | 0.799512,0.027452,0.5,0.9294119999999999,0.102452,1.0 144 | 0.826962,0.0,0.5,0.9019619999999999,0.12990400000000002,1.0 145 | 0.226472,0.39078599999999997,0.0,0.252452,0.405786,0.2 146 | 0.231962,0.38529599999999997,0.0,0.24696200000000001,0.411276,0.2 147 | 0.187502,0.368286,0.2,0.291422,0.428286,0.6 148 | 0.209462,0.34632599999999997,0.2,0.269462,0.450246,0.6 149 | 0.213482,0.38328599999999996,0.6,0.265442,0.413286,1.0 150 | 0.224462,0.37230599999999997,0.6,0.254462,0.424266,1.0 151 | 0.732962,0.727866,0.0,0.7459619999999999,0.7353660000000001,0.75 152 | 0.7357119999999999,0.725126,0.0,0.743212,0.738116,0.75 153 | 0.674512,0.6941160000000001,0.5,0.8044119999999999,0.769116,1.0 154 | 0.701962,0.6666660000000001,0.5,0.7769619999999999,0.796566,1.0 155 | 0.482962,0.172316,0.0,0.495962,0.17981599999999998,0.75 156 | 0.48571200000000003,0.169566,0.0,0.493212,0.182556,0.75 157 | 0.424512,0.138563,0.5,0.5544119999999999,0.21356599999999998,1.0 158 | 0.45196200000000003,0.111111,0.5,0.5269619999999999,0.241016,1.0 159 | -------------------------------------------------------------------------------- /include/config/obstacles/quadTrees/obstacles.csv: -------------------------------------------------------------------------------- 1 | 50.7722,37.8436,0.0,53.37019999999999,39.3436,20.0 2 | 51.3212,37.2946,0.0,52.82119999999999,39.8926,20.0 3 | 46.8752,35.593599999999995,20.0,57.267199999999995,41.593599999999995,60.0 4 | 49.0712,33.3976,20.0,55.0712,43.7896,60.0 5 | 49.4732,37.0936,60.0,54.6692,40.09359999999999,100.0 6 | 50.57119999999999,35.9956,60.0,53.5712,41.1916,100.0 7 | 26.4212,71.5526,0.0,27.721200000000003,72.30260000000001,75.0 8 | 26.696199999999997,71.2776,0.0,27.446199999999997,72.5766,75.0 9 | 20.5762,68.1776,50.0,33.5662,75.6776,100.0 10 | 23.3212,65.4316,50.0,30.821199999999997,78.4226,100.0 11 | 75.7722,15.621599999999999,0.0,78.37019999999998,17.12159999999999,20.0 12 | 76.3212,15.0727,0.0,77.82119999999999,17.6706,20.0 13 | 71.87519999999999,13.3717,20.0,82.2672,19.3716,60.0 14 | 74.07119999999999,11.1756,20.0,80.0712,21.567599999999988,60.0 15 | 74.47319999999999,14.871699999999999,60.0,79.66919999999999,17.87159999999999,100.0 16 | 75.57119999999999,13.7736,60.0,78.57119999999999,18.96959999999999,100.0 17 | 13.9212,49.3296,0.0,15.221199999999998,50.0796,75.0 18 | 14.196200000000001,49.0556,0.0,14.946200000000001,50.354600000000005,75.0 19 | 8.076,45.9546,50.0,21.0662,53.4546,100.0 20 | 10.821200000000001,43.209599999999995,50.0,18.3212,56.2006,100.0 21 | 63.9212,82.6636,0.0,65.2212,83.4136,75.0 22 | 64.19619999999999,82.3886,0.0,64.9462,83.6876,75.0 23 | 58.0762,79.2886,50.0,71.0662,86.7886,100.0 24 | 60.8212,76.5436,50.0,68.32119999999999,89.5336,100.0 25 | 38.2722,26.7326,0.0,40.870200000000004,28.2326,20.0 26 | 38.8212,26.183600000000002,0.0,40.321200000000005,28.7816,20.0 27 | 34.3752,24.482599999999998,20.0,44.7672,30.482599999999998,60.0 28 | 36.5712,22.2866,20.0,42.5712,32.67859999999999,60.0 29 | 36.9732,25.9826,60.0,42.169200000000004,28.982599999999998,100.0 30 | 38.0712,24.884600000000002,60.0,41.071200000000005,30.0806,100.0 31 | 88.2722,60.0666,0.0,90.8702,61.56660000000001,20.0 32 | 88.8212,59.5176,0.0,90.3212,62.1156,20.0 33 | 84.37519999999999,57.81660000000001,20.0,94.7672,63.8166,60.0 34 | 86.57119999999999,55.619600000000005,20.0,92.5712,66.01259999999999,60.0 35 | 86.97319999999999,59.31660000000001,60.0,92.16919999999999,62.3166,100.0 36 | 88.07119999999999,58.217600000000004,60.0,91.07119999999999,63.4146,100.0 37 | 7.6716999999999995,93.77459999999999,0.0,8.970699999999999,94.5246,75.0 38 | 7.9462,93.4996,0.0,8.6962,94.7986,75.0 39 | 1.8260100000000001,90.3996,50.0,14.816199999999998,97.89959999999999,100.0 40 | 4.5712,87.6546,50.0,12.0712,100.6446,100.0 41 | 57.6712,8.5893,0.0,58.97119999999999,9.3393,75.0 42 | 57.94619999999999,8.3148,0.0,58.6962,9.6138,75.0 43 | 51.8262,5.214304,50.0,64.8162,12.714300000000001,100.0 44 | 54.5712,2.4691,50.0,62.07119999999999,15.459600000000002,100.0 45 | 32.0222,41.5476,0.0,34.620200000000004,43.04759999999999,20.0 46 | 32.5712,40.99859999999999,0.0,34.071200000000005,43.59659999999999,20.0 47 | 28.1252,39.2976,20.0,38.5172,45.297599999999996,60.0 48 | 30.321199999999997,37.101600000000005,20.0,36.3212,47.493599999999994,60.0 49 | 30.723200000000002,40.7976,60.0,35.919200000000004,43.797599999999996,100.0 50 | 31.8212,39.69959999999999,60.0,34.821200000000005,44.89559999999999,100.0 51 | 82.6712,75.2556,0.0,83.9712,76.0056,75.0 52 | 82.94619999999999,74.9816,0.0,83.6962,76.2806,75.0 53 | 76.8262,71.8806,50.0,89.8162,79.3806,100.0 54 | 79.5712,69.13560000000001,50.0,87.07119999999999,82.12660000000001,100.0 55 | 19.5222,19.32559999999999,0.0,22.1202,20.8256,20.0 56 | 20.0712,18.7766,0.0,21.571199999999997,21.3746,20.0 57 | 15.6252,17.075599999999998,20.0,26.017200000000003,23.075599999999998,60.0 58 | 17.8212,14.8793,20.0,23.8212,25.2716,60.0 59 | 18.223200000000002,18.57559999999999,60.0,23.4192,21.5756,100.0 60 | 19.3212,17.4776,60.0,22.3212,22.6736,100.0 61 | 69.5222,52.6586,0.0,72.12019999999998,54.1586,20.0 62 | 70.0712,52.1096,0.0,71.57119999999999,54.7076,20.0 63 | 65.62519999999999,50.40860000000001,20.0,76.0172,56.40860000000001,60.0 64 | 67.82119999999999,48.2126,20.0,73.8212,58.604600000000005,60.0 65 | 68.22319999999999,51.90860000000001,60.0,73.41919999999999,54.90859999999999,100.0 66 | 69.32119999999999,50.81060000000001,60.0,72.32119999999999,56.006600000000006,100.0 67 | 44.5222,85.9916,0.0,47.120200000000004,87.4916,20.0 68 | 45.0712,85.4426,0.0,46.571200000000005,88.0416,20.0 69 | 40.6252,83.7416,20.0,51.017199999999995,89.7416,60.0 70 | 42.8212,81.54560000000001,20.0,48.8212,91.93860000000001,60.0 71 | 43.2232,85.2416,60.0,48.419200000000004,88.2416,100.0 72 | 44.3212,84.1436,60.0,47.321200000000005,89.34060000000001,100.0 73 | 94.5222,30.43659999999999,0.0,97.1202,31.9366,20.0 74 | 95.0712,29.8876,0.0,96.5712,32.4856,20.0 75 | 90.62519999999999,28.186600000000002,20.0,101.0172,34.1866,60.0 76 | 92.8212,25.990600000000004,20.0,98.8212,36.3826,60.0 77 | 93.22319999999999,29.686600000000002,60.0,98.41919999999999,32.6866,100.0 78 | 94.3212,28.588599999999996,60.0,97.32119999999999,33.7846,100.0 79 | 3.8971999999999998,63.769600000000004,0.0,6.4952,65.26960000000001,20.0 80 | 4.4462,63.220600000000005,0.0,5.9462,65.8186,20.0 81 | 0.0,61.51960000000001,20.0,10.392399999999991,67.5196,60.0 82 | 2.1961999999999997,59.32360000000001,20.0,8.1962,69.7156,60.0 83 | 2.5981199999999998,63.01960000000001,60.0,7.7943,66.0196,100.0 84 | 3.6962,61.9216,60.0,6.696199999999999,67.1176,100.0 85 | 53.8972,97.1036,0.0,56.49519999999999,98.6036,20.0 86 | 54.4462,96.55460000000001,0.0,55.94619999999999,99.1526,20.0 87 | 50.00019999999999,94.8536,20.0,60.392199999999995,100.85360000000001,60.0 88 | 52.19619999999999,92.6566,20.0,58.1962,103.04960000000001,60.0 89 | 52.59819999999999,96.3536,60.0,57.7942,99.3536,100.0 90 | 53.69619999999999,95.2556,60.0,56.6962,100.4516,100.0 91 | 29.5462,12.293,0.0,30.846200000000003,13.043,75.0 92 | 29.821199999999997,12.0185,0.0,30.571199999999997,13.317499999999999,75.0 93 | 23.7012,8.918,50.0,36.6912,16.4176,100.0 94 | 26.446199999999997,6.17282,50.0,33.9462,19.163599999999988,100.0 95 | 78.8972,45.251599999999996,0.0,81.49519999999998,46.751599999999996,20.0 96 | 79.4462,44.7026,0.0,80.94619999999999,47.300599999999996,20.0 97 | 75.00019999999999,43.0016,20.0,85.3922,49.0016,60.0 98 | 77.19619999999999,40.8056,20.0,83.1962,51.1976,60.0 99 | 77.59819999999999,44.50159999999999,60.0,82.79419999999999,47.501599999999996,100.0 100 | 78.69619999999999,43.4036,60.0,81.69619999999999,48.599599999999995,100.0 101 | 16.3972,78.58460000000001,0.0,18.9952,80.08460000000001,20.0 102 | 16.9462,78.0356,0.0,18.446199999999997,80.6336,20.0 103 | 12.5002,76.33460000000001,20.0,22.8922,82.33460000000001,60.0 104 | 14.696200000000001,74.1386,20.0,20.6962,84.5306,60.0 105 | 15.0982,77.83460000000001,60.0,20.2942,80.8346,100.0 106 | 16.1962,76.7366,60.0,19.1962,81.9326,100.0 107 | 67.0462,23.404599999999988,0.0,68.3462,24.15459999999999,75.0 108 | 67.32119999999999,23.1296,0.0,68.0712,24.4286,75.0 109 | 61.2012,20.029599999999988,50.0,74.1912,27.5296,100.0 110 | 63.9462,17.2836,50.0,71.44619999999999,30.27459999999999,100.0 111 | 41.3972,56.36260000000001,0.0,43.995200000000004,57.86260000000001,20.0 112 | 41.9462,55.81360000000001,0.0,43.446200000000005,58.41160000000001,20.0 113 | 37.5002,54.1126,20.0,47.8922,60.11260000000001,60.0 114 | 39.6962,51.9166,20.0,45.6962,62.308600000000006,60.0 115 | 40.0982,55.6126,60.0,45.294200000000004,58.6126,100.0 116 | 41.1962,54.5146,60.0,44.196200000000005,59.7106,100.0 117 | 91.3972,89.69560000000001,0.0,93.9952,91.1956,20.0 118 | 91.9462,89.1466,0.0,93.4462,91.74459999999999,20.0 119 | 87.50019999999999,87.4456,20.0,97.8922,93.4456,60.0 120 | 89.69619999999999,85.2496,20.0,95.6962,95.64160000000001,60.0 121 | 90.09819999999999,88.9456,60.0,95.29419999999999,91.9456,100.0 122 | 91.1962,87.8476,60.0,94.19619999999999,93.0436,100.0 123 | 10.147200000000002,34.1406,0.0,12.7452,35.6406,20.0 124 | 10.696200000000001,33.5916,0.0,12.1962,36.1896,20.0 125 | 6.25,31.890599999999992,20.0,16.6422,37.89059999999999,60.0 126 | 8.4462,29.693599999999996,20.0,14.446200000000001,40.0866,60.0 127 | 8.8481,33.3906,60.0,14.044200000000002,36.3906,100.0 128 | 9.9462,32.2926,60.0,12.9462,37.4886,100.0 129 | 60.1472,67.4736,0.0,62.74519999999999,68.9736,20.0 130 | 60.6962,66.9246,0.0,62.19619999999999,69.5226,20.0 131 | 56.25019999999999,65.2236,20.0,66.6422,71.2236,60.0 132 | 58.44619999999999,63.02760000000001,20.0,64.4462,73.4196,60.0 133 | 58.84819999999999,66.7236,60.0,64.04419999999999,69.7236,100.0 134 | 59.94619999999999,65.6256,60.0,62.9462,70.8216,100.0 135 | 35.1472,100.80659999999999,0.0,37.745200000000004,102.3066,20.0 136 | 35.6962,100.2576,0.0,37.196200000000005,102.85560000000001,20.0 137 | 31.2502,98.5566,20.0,41.6422,104.5566,60.0 138 | 33.4462,96.36059999999999,20.0,39.4462,106.75059999999999,60.0 139 | 33.8482,100.0566,60.0,39.044200000000004,103.0566,100.0 140 | 34.9462,98.95859999999999,60.0,37.946200000000005,104.1546,100.0 141 | 85.7962,6.12017,0.0,87.0962,6.8702,75.0 142 | 86.07119999999999,5.84565,0.0,86.8212,7.144699999999999,75.0 143 | 79.9512,2.7452,50.0,92.9412,10.2452,100.0 144 | 82.6962,0.0,50.0,90.1962,12.9904,100.0 145 | 22.6472,39.078599999999994,0.0,25.2452,40.578599999999994,20.0 146 | 23.1962,38.529599999999995,0.0,24.696199999999997,41.1276,20.0 147 | 18.7502,36.8286,20.0,29.142200000000003,42.8286,60.0 148 | 20.9462,34.63259999999999,20.0,26.946199999999997,45.0246,60.0 149 | 21.348200000000002,38.32859999999999,60.0,26.5442,41.3286,100.0 150 | 22.4462,37.23059999999999,60.0,25.4462,42.4266,100.0 151 | 73.2962,72.7866,0.0,74.5962,73.5366,75.0 152 | 73.57119999999999,72.5126,0.0,74.3212,73.8116,75.0 153 | 67.4512,69.4116,50.0,80.4412,76.9116,100.0 154 | 70.1962,66.6666,50.0,77.69619999999999,79.6566,100.0 155 | 48.2962,17.2316,0.0,49.5962,17.98159999999999,75.0 156 | 48.5712,16.956599999999998,0.0,49.3212,18.2556,75.0 157 | 42.4512,13.8563,50.0,55.44119999999999,21.35659999999999,100.0 158 | 45.1962,11.1111,50.0,52.69619999999999,24.1016,100.0 159 | -------------------------------------------------------------------------------- /include/config/config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /***************************/ 4 | /* 6D DOUBLE INTEGRATOR CONFIG */ 5 | /***************************/ 6 | 7 | // #define MODEL 1 8 | 9 | // #define MAX_TREE_SIZE 300000 10 | // #define MAX_ITER 300 11 | // #define STEP_SIZE 0.1f 12 | // #define MAX_PROPAGATION_DURATION 10 13 | 14 | // #define GOAL_THRESH 0.05f 15 | 16 | // #define STATE_DIM 6 17 | // #define CONTROL_DIM 3 18 | // #define SAMPLE_DIM (STATE_DIM + CONTROL_DIM + 1) 19 | 20 | // #define W_DIM 3 21 | // #define C_DIM 1 22 | // #define V_DIM 3 23 | 24 | // #define W_MIN 0.0f 25 | // #define W_MAX 1.0f 26 | // #define W_SIZE 1.0f 27 | 28 | // #define C_MIN -M_PI 29 | // #define C_MAX M_PI 30 | 31 | // #define V_MIN -0.3f 32 | // #define V_MAX 0.3f 33 | 34 | // #define A_MIN -0.2f 35 | // #define A_MAX 0.2f 36 | 37 | // #define W_R1_LENGTH 8 38 | // #define C_R1_LENGTH 1 39 | // #define V_R1_LENGTH 4 40 | 41 | // #define W_R2_LENGTH 2 42 | // #define C_R2_LENGTH 1 43 | // #define V_R2_LENGTH 2 44 | 45 | // #define W_R1_SIZE ((W_MAX - W_MIN) / W_R1_LENGTH) 46 | // #define C_R1_SIZE ((C_MAX - C_MIN) / C_R1_LENGTH) 47 | // #define V_R1_SIZE ((V_MAX - V_MIN) / V_R1_LENGTH) 48 | 49 | // #define W_R1_VOL (W_R1_SIZE * W_R1_SIZE * W_R1_SIZE) 50 | 51 | // #define NUM_R1_REGIONS (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH * V_R1_LENGTH * V_R1_LENGTH * V_R1_LENGTH) 52 | // #define NUM_R2_REGIONS (NUM_R1_REGIONS * W_R2_LENGTH * W_R2_LENGTH * W_R2_LENGTH * V_R2_LENGTH * V_R2_LENGTH * V_R2_LENGTH) 53 | // #define NUM_R2_PER_R1 W_R2_LENGTH *W_R2_LENGTH *W_R2_LENGTH *V_R2_LENGTH *V_R2_LENGTH *V_R2_LENGTH 54 | // #define NUM_R1_REGIONS_KERNEL1 1024 55 | // #define NUM_PARTIAL_SUMS 1024 56 | 57 | // #define EPSILON 1e-2f 58 | // #define VERBOSE 1 59 | 60 | // // --- UNICYCLE MODEL: MODEL 0 --- 61 | // #define UNI_MIN_STEERING -M_PI / 2 62 | // #define UNI_MAX_STEERING M_PI / 2 63 | // #define UNI_MIN_DT 0.1f 64 | // #define UNI_MAX_DT 2.0f 65 | // #define UNI_LENGTH 1.0f 66 | 67 | // // --- DUBINS AIRPLANE: MODEL 2 --- 68 | // #define DUBINS_AIRPLANE_MIN_PR (-M_PI / 4) 69 | // #define DUBINS_AIRPLANE_MAX_PR (M_PI / 4) 70 | // #define DUBINS_AIRPLANE_MIN_YR (-M_PI / 4) 71 | // #define DUBINS_AIRPLANE_MAX_YR (M_PI / 4) 72 | // #define DUBINS_AIRPLANE_MIN_YAW -M_PI 73 | // #define DUBINS_AIRPLANE_MAX_YAW M_PI 74 | // #define DUBINS_AIRPLANE_MIN_PITCH -M_PI / 3 75 | // #define DUBINS_AIRPLANE_MAX_PITCH M_PI / 3 76 | 77 | // // --- NON LINEAR QUAD: MODEL 3 --- 78 | // #define QUAD_MIN_Zc -2.0f 79 | // #define QUAD_MAX_Zc 2.0f 80 | // #define QUAD_MIN_Lc -2.0f 81 | // #define QUAD_MAX_Lc 2.0f 82 | // #define QUAD_MIN_Mc -2.0f 83 | // #define QUAD_MAX_Mc 2.0f 84 | // #define QUAD_MIN_Nc -2.0f 85 | // #define QUAD_MAX_Nc 2.0f 86 | // #define QUAD_MIN_YAW -M_PI 87 | // #define QUAD_MAX_YAW M_PI 88 | // #define QUAD_MIN_PITCH -M_PI / 3 89 | // #define QUAD_MAX_PITCH M_PI / 3 90 | // #define QUAD_MIN_ROLL -M_PI / 3 91 | // #define QUAD_MAX_ROLL M_PI / 3 92 | // #define QUAD_MIN_ANGLE_RATE -30.0f 93 | // #define QUAD_MAX_ANGLE_RATE 30.0f 94 | // #define NU 10e-3f 95 | // #define MU 2e-6f 96 | // #define KM 0.03f 97 | // #define IX 1.0f 98 | // #define IY 1.0f 99 | // #define IZ 2.0f 100 | // #define GRAVITY -9.81f 101 | // #define MASS 1.0f 102 | // #define MASS_INV 1.0f / MASS 103 | 104 | /***************************/ 105 | /* DUBINS AIRPLANE CONFIG */ 106 | /***************************/ 107 | 108 | // #define MODEL 2 109 | 110 | // #define MAX_TREE_SIZE 200000 111 | // #define MAX_ITER 300 112 | // #define STEP_SIZE 0.1f 113 | // #define MAX_PROPAGATION_DURATION 10 114 | 115 | // #define GOAL_THRESH 0.05f 116 | 117 | // #define STATE_DIM 6 118 | // #define CONTROL_DIM 3 119 | // #define SAMPLE_DIM (STATE_DIM + CONTROL_DIM + 1) 120 | 121 | // #define W_DIM 3 122 | // #define C_DIM 2 123 | // #define V_DIM 1 124 | 125 | // #define W_MIN 0.0f 126 | // #define W_MAX 1.0f 127 | // #define W_SIZE 1.0f 128 | 129 | // #define C_MIN -M_PI 130 | // #define C_MAX M_PI 131 | 132 | // #define V_MIN 0.0f 133 | // #define V_MAX 0.3f 134 | 135 | // #define A_MIN -0.3f 136 | // #define A_MAX 0.3f 137 | 138 | // #define W_R1_LENGTH 8 139 | // #define C_R1_LENGTH 8 140 | // #define V_R1_LENGTH 1 141 | 142 | // #define W_R2_LENGTH 2 143 | // #define C_R2_LENGTH 3 144 | // #define V_R2_LENGTH 1 145 | 146 | // #define W_R1_SIZE ((W_MAX - W_MIN) / W_R1_LENGTH) 147 | // #define C_R1_SIZE ((C_MAX - C_MIN) / C_R1_LENGTH) 148 | // #define V_R1_SIZE ((V_MAX - V_MIN) / V_R1_LENGTH) 149 | 150 | // #define W_R1_VOL (W_R1_SIZE * W_R1_SIZE * W_R1_SIZE) 151 | 152 | // #define NUM_R1_REGIONS (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH * C_R1_LENGTH * C_R1_LENGTH * V_R1_LENGTH) 153 | // #define NUM_R2_REGIONS (NUM_R1_REGIONS * W_R2_LENGTH * W_R2_LENGTH * W_R2_LENGTH * C_R2_LENGTH * C_R2_LENGTH * V_R2_LENGTH) 154 | // #define NUM_R2_PER_R1 W_R2_LENGTH *W_R2_LENGTH *W_R2_LENGTH *C_R2_LENGTH *C_R2_LENGTH *V_R2_LENGTH 155 | // #define NUM_R1_REGIONS_KERNEL1 1024 156 | // #define NUM_PARTIAL_SUMS 1024 157 | 158 | // #define EPSILON 1e-2f 159 | // #define VERBOSE 1 160 | 161 | // // --- UNICYCLE MODEL: MODEL 0 --- 162 | // #define UNI_MIN_STEERING -M_PI / 2 163 | // #define UNI_MAX_STEERING M_PI / 2 164 | // #define UNI_MIN_DT 0.1f 165 | // #define UNI_MAX_DT 2.0f 166 | // #define UNI_LENGTH 1.0f 167 | 168 | // // --- DUBINS AIRPLANE: MODEL 2 --- 169 | // #define DUBINS_AIRPLANE_MIN_PR (-M_PI / 4) 170 | // #define DUBINS_AIRPLANE_MAX_PR (M_PI / 4) 171 | // #define DUBINS_AIRPLANE_MIN_YR (-M_PI / 4) 172 | // #define DUBINS_AIRPLANE_MAX_YR (M_PI / 4) 173 | // #define DUBINS_AIRPLANE_MIN_YAW -M_PI 174 | // #define DUBINS_AIRPLANE_MAX_YAW M_PI 175 | // #define DUBINS_AIRPLANE_MIN_PITCH -M_PI / 3 176 | // #define DUBINS_AIRPLANE_MAX_PITCH M_PI / 3 177 | 178 | // // --- NON LINEAR QUAD: MODEL 3 --- 179 | // #define QUAD_MIN_Zc -2.0f 180 | // #define QUAD_MAX_Zc 2.0f 181 | // #define QUAD_MIN_Lc -2.0f 182 | // #define QUAD_MAX_Lc 2.0f 183 | // #define QUAD_MIN_Mc -2.0f 184 | // #define QUAD_MAX_Mc 2.0f 185 | // #define QUAD_MIN_Nc -2.0f 186 | // #define QUAD_MAX_Nc 2.0f 187 | // #define QUAD_MIN_YAW -M_PI 188 | // #define QUAD_MAX_YAW M_PI 189 | // #define QUAD_MIN_PITCH -M_PI / 3 190 | // #define QUAD_MAX_PITCH M_PI / 3 191 | // #define QUAD_MIN_ROLL -M_PI / 3 192 | // #define QUAD_MAX_ROLL M_PI / 3 193 | // #define QUAD_MIN_ANGLE_RATE -30.0f 194 | // #define QUAD_MAX_ANGLE_RATE 30.0f 195 | // #define NU 10e-3f 196 | // #define MU 2e-6f 197 | // #define KM 0.03f 198 | // #define IX 1.0f 199 | // #define IY 1.0f 200 | // #define IZ 2.0f 201 | // #define GRAVITY -9.81f 202 | // #define MASS 1.0f 203 | // #define MASS_INV 1.0f / MASS 204 | 205 | /***************************/ 206 | /* NON LINEAR QUAD CONFIG */ 207 | /***************************/ 208 | 209 | #define MODEL 3 210 | 211 | #define MAX_TREE_SIZE 400000 212 | #define MAX_ITER 300 213 | #define STEP_SIZE 0.1f 214 | #define MAX_PROPAGATION_DURATION 10 215 | 216 | #define GOAL_THRESH 5.0f 217 | 218 | #define STATE_DIM 12 219 | #define CONTROL_DIM 4 220 | #define SAMPLE_DIM (STATE_DIM + CONTROL_DIM + 1) 221 | 222 | #define W_DIM 3 223 | #define C_DIM 3 224 | #define V_DIM 3 225 | 226 | #define W_MIN 0.0f 227 | #define W_MAX 100.0f 228 | #define W_SIZE 100.0f 229 | 230 | #define C_MIN -M_PI 231 | #define C_MAX M_PI 232 | 233 | #define V_MIN -30.0f 234 | #define V_MAX 30.0f 235 | 236 | #define A_MIN -30.0f 237 | #define A_MAX 30.0f 238 | 239 | #define W_R1_LENGTH 8 240 | #define C_R1_LENGTH 2 241 | #define V_R1_LENGTH 2 242 | 243 | #define W_R2_LENGTH 2 244 | #define C_R2_LENGTH 1 245 | #define V_R2_LENGTH 2 246 | 247 | #define W_R1_SIZE ((W_MAX - W_MIN) / W_R1_LENGTH) 248 | #define C_R1_SIZE ((C_MAX - C_MIN) / C_R1_LENGTH) 249 | #define V_R1_SIZE ((V_MAX - V_MIN) / V_R1_LENGTH) 250 | 251 | #define W_R1_VOL (W_R1_SIZE * W_R1_SIZE * W_R1_SIZE) 252 | 253 | #define NUM_R1_REGIONS \ 254 | (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH * C_R1_LENGTH * C_R1_LENGTH * C_R1_LENGTH * V_R1_LENGTH * V_R1_LENGTH * V_R1_LENGTH) 255 | #define NUM_R2_REGIONS \ 256 | (NUM_R1_REGIONS * W_R2_LENGTH * W_R2_LENGTH * W_R2_LENGTH * C_R2_LENGTH * C_R2_LENGTH * C_R2_LENGTH * V_R2_LENGTH * V_R2_LENGTH * \ 257 | V_R2_LENGTH) 258 | #define NUM_R2_PER_R1 W_R2_LENGTH *W_R2_LENGTH *W_R2_LENGTH *C_R2_LENGTH *C_R2_LENGTH *C_R2_LENGTH *V_R2_LENGTH *V_R2_LENGTH *V_R2_LENGTH 259 | #define NUM_R1_REGIONS_KERNEL1 1024 260 | #define NUM_PARTIAL_SUMS 1024 261 | 262 | #define EPSILON 1e-2f 263 | #define VERBOSE 1 264 | 265 | // --- UNICYCLE MODEL: MODEL 0 --- 266 | #define UNI_MIN_STEERING -M_PI / 2 267 | #define UNI_MAX_STEERING M_PI / 2 268 | #define UNI_MIN_DT 0.1f 269 | #define UNI_MAX_DT 2.0f 270 | #define UNI_LENGTH 1.0f 271 | 272 | // --- DUBINS AIRPLANE: MODEL 2 --- 273 | #define DUBINS_AIRPLANE_MIN_PR (-M_PI / 4) 274 | #define DUBINS_AIRPLANE_MAX_PR (M_PI / 4) 275 | #define DUBINS_AIRPLANE_MIN_YR (-M_PI / 4) 276 | #define DUBINS_AIRPLANE_MAX_YR (M_PI / 4) 277 | #define DUBINS_AIRPLANE_MIN_YAW -M_PI 278 | #define DUBINS_AIRPLANE_MAX_YAW M_PI 279 | #define DUBINS_AIRPLANE_MIN_PITCH -M_PI / 3 280 | #define DUBINS_AIRPLANE_MAX_PITCH M_PI / 3 281 | 282 | // --- NON LINEAR QUAD: MODEL 3 --- 283 | #define QUAD_MIN_Zc 0.0f 284 | #define QUAD_MAX_Zc 30.0f 285 | #define QUAD_MIN_Lc -M_PI 286 | #define QUAD_MAX_Lc M_PI 287 | #define QUAD_MIN_Mc -M_PI 288 | #define QUAD_MAX_Mc M_PI 289 | #define QUAD_MIN_Nc -M_PI 290 | #define QUAD_MAX_Nc M_PI 291 | #define QUAD_MIN_YAW -M_PI 292 | #define QUAD_MAX_YAW M_PI 293 | #define QUAD_MIN_PITCH -M_PI 294 | #define QUAD_MAX_PITCH M_PI 295 | #define QUAD_MIN_ROLL -M_PI 296 | #define QUAD_MAX_ROLL M_PI 297 | #define QUAD_MIN_ANGLE_RATE -30.0f 298 | #define QUAD_MAX_ANGLE_RATE 30.0f 299 | #define NU 10e-3f 300 | #define MU 2e-6f 301 | #define KM 0.03f 302 | #define IX 1.0f 303 | #define IY 1.0f 304 | #define IZ 2.0f 305 | #define GRAVITY -9.81f 306 | #define MASS 1.0f 307 | #define MASS_INV 1.0f / MASS -------------------------------------------------------------------------------- /viz/KGMT_3D.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | try 6 | opengl hardware 7 | disp('Using hardware OpenGL.'); 8 | catch 9 | opengl software 10 | disp('Using software OpenGL.'); 11 | end 12 | 13 | % Parameters 14 | numFiles = 9; 15 | radius = 0.05; 16 | N = 8; 17 | n = 4; 18 | sampleSize = 10; 19 | stateSize = 6; 20 | controlSize = 3; 21 | xGoal = [.7, .95, .9]; 22 | alphaValue = 0.8; 23 | STEP_SIZE = .1; 24 | 25 | % Obstacle file path 26 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/pillars/obstacles.csv'; 27 | obstacles = readmatrix(obstacleFilePath); 28 | 29 | controlPath = '/home/nicolas/dev/research/KPAX/build/Data/ControlPathToGoal/ControlPathToGoal0/controlPathToGoal.csv'; 30 | controls = readmatrix(controlPath); 31 | controls = flipud(controls); 32 | 33 | % Create and save iteration 0 figure 34 | % fig = figure('Position', [100, 100, 1000, 1000]); % Set figure size 35 | % hold on; 36 | % axis equal; 37 | % title('Iteration 0'); 38 | % xlabel('X Position'); 39 | % ylabel('Y Position'); 40 | % zlabel('Z Position'); 41 | % 42 | % sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples1.csv"; 43 | % samples = readmatrix(sampleFilePath); 44 | % 45 | % 46 | % % Plot start point 47 | % plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'k'); 48 | % 49 | % % Plot goal region 50 | % [X, Y, Z] = sphere(20); 51 | % surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 52 | % 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 53 | % 54 | % % Plot obstacles 55 | % for j = 1:size(obstacles, 1) 56 | % x_min = obstacles(j, 1); 57 | % y_min = obstacles(j, 2); 58 | % z_min = obstacles(j, 3); 59 | % x_max = obstacles(j, 4); 60 | % y_max = obstacles(j, 5); 61 | % z_max = obstacles(j, 6); 62 | % vertices = [ 63 | % x_min, y_min, z_min; 64 | % x_max, y_min, z_min; 65 | % x_max, y_max, z_min; 66 | % x_min, y_max, z_min; 67 | % x_min, y_min, z_max; 68 | % x_max, y_min, z_max; 69 | % x_max, y_max, z_max; 70 | % x_min, y_max, z_max]; 71 | % faces = [ 72 | % 1, 2, 6, 5; 73 | % 2, 3, 7, 6; 74 | % 3, 4, 8, 7; 75 | % 4, 1, 5, 8; 76 | % 1, 2, 3, 4; 77 | % 5, 6, 7, 8]; 78 | % patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', 0.6); 79 | % end 80 | % 81 | % % Add light source 82 | % camlight('headlight'); 83 | % lighting gouraud; 84 | % 85 | % % Save original view with high resolution 86 | % view(3); 87 | % drawnow; 88 | % saveas(gcf, 'figs/KGMT_Iteration_0.jpg'); 89 | % print('figs/KGMT_Iteration_0.jpg', '-djpeg', '-r300'); % Save with 300 DPI 90 | % 91 | % % Save top-down view with high resolution 92 | % view(2); 93 | % drawnow; 94 | % saveas(gcf, 'figs/top_KGMT_Iteration_0.jpg'); 95 | % print('figs/top_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); % Save with 300 DPI 96 | % 97 | % % Save view looking along the x-axis from above 98 | % midY = 0.5 * xGoal(2); % Midpoint of y-axis 99 | % midZ = 0.5 * xGoal(3); % Midpoint of z-axis 100 | % campos([0, midY, xGoal(3) + 1]); % Position the camera above the max z-value 101 | % camtarget([0, midY, midZ]); % Set the target point to the middle 102 | % view([-1, .2, 0.6]); % Look along the x-axis 103 | % drawnow; 104 | % saveas(gcf, 'figs/xAxis_KGMT_Iteration_0.jpg'); 105 | % print('figs/xAxis_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); % Save with 300 DPI 106 | % 107 | % % Close the figure 108 | % close(gcf); 109 | 110 | for i = numFiles:numFiles 111 | 112 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples" + i + ".csv"; 113 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Parents/Parents0/parents" + i + ".csv"; 114 | 115 | samples = readmatrix(sampleFilePath); 116 | parentRelations = readmatrix(parentFilePath); 117 | 118 | % Create figure and set to full screen 119 | fig = figure('Position', [100, 100, 1000, 1000]); % Set figure size 120 | hold on; 121 | axis equal; 122 | title(sprintf('Iteration %d', i)); 123 | xlabel('X Position'); 124 | ylabel('Y Position'); 125 | zlabel('Z Position'); 126 | plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'k'); 127 | 128 | % Plot goal region 129 | [X, Y, Z] = sphere(20); 130 | surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 131 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 132 | 133 | % Plot obstacles 134 | for j = 1:size(obstacles, 1) 135 | x_min = obstacles(j, 1); 136 | y_min = obstacles(j, 2); 137 | z_min = obstacles(j, 3); 138 | x_max = obstacles(j, 4); 139 | y_max = obstacles(j, 5); 140 | z_max = obstacles(j, 6); 141 | vertices = [ 142 | x_min, y_min, z_min; 143 | x_max, y_min, z_min; 144 | x_max, y_max, z_min; 145 | x_min, y_max, z_min; 146 | x_min, y_min, z_max; 147 | x_max, y_min, z_max; 148 | x_max, y_max, z_max; 149 | x_min, y_max, z_max]; 150 | faces = [ 151 | 1, 2, 6, 5; 152 | 2, 3, 7, 6; 153 | 3, 4, 8, 7; 154 | 4, 1, 5, 8; 155 | 1, 2, 3, 4; 156 | 5, 6, 7, 8]; 157 | patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', 0.6); 158 | end 159 | 160 | % Add light source 161 | camlight('headlight'); 162 | lighting gouraud; 163 | 164 | % Plot paths 165 | for j = 2:size(parentRelations, 1) 166 | if parentRelations(j) == -1 167 | break; 168 | end 169 | x0 = samples((parentRelations(j) + 1), 1:stateSize); 170 | segmentX = [x0(1)]; 171 | segmentY = [x0(2)]; 172 | segmentZ = [x0(3)]; 173 | u = samples(j, stateSize+1:sampleSize-1); 174 | duration = samples(j, sampleSize); 175 | numDisc = duration/STEP_SIZE; 176 | x = x0(1); 177 | y = x0(2); 178 | z = x0(3); 179 | vx = x0(4); 180 | vy = x0(5); 181 | vz = x0(6); 182 | ax = u(1); 183 | ay = u(2); 184 | az = u(3); 185 | for k = 1:(numDisc) 186 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 187 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 188 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 189 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 190 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 191 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 192 | segmentX = [segmentX, x]; 193 | segmentY = [segmentY, y]; 194 | segmentZ = [segmentZ, z]; 195 | end 196 | segmentX = [segmentX, samples(j, 1)]; 197 | segmentY = [segmentY, samples(j, 2)]; 198 | segmentZ = [segmentZ, samples(j, 3)]; 199 | 200 | plot3(segmentX, segmentY, segmentZ, '-.', 'Color', 'k', 'LineWidth', 0.01); 201 | plot3(samples(j, 1), samples(j, 2), samples(j, 3), 'bo', 'MarkerFaceColor', 'b', 'MarkerSize', 2); 202 | end 203 | 204 | if(i == numFiles) 205 | for j = 2:size(controls, 1) 206 | x0 = controls(j-1, 1:stateSize); 207 | segmentX = [x0(1)]; 208 | segmentY = [x0(2)]; 209 | segmentZ = [x0(3)]; 210 | u = controls(j, stateSize+1:sampleSize-1); 211 | duration = controls(j, sampleSize); 212 | numDisc = duration/STEP_SIZE; 213 | x = x0(1); 214 | y = x0(2); 215 | z = x0(3); 216 | vx = x0(4); 217 | vy = x0(5); 218 | vz = x0(6); 219 | ax = u(1); 220 | ay = u(2); 221 | az = u(3); 222 | for k = 1:(numDisc) 223 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 224 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 225 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 226 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 227 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 228 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 229 | segmentX = [segmentX, x]; 230 | segmentY = [segmentY, y]; 231 | segmentZ = [segmentZ, z]; 232 | end 233 | segmentX = [segmentX, controls(j, 1)]; 234 | segmentY = [segmentY, controls(j, 2)]; 235 | segmentZ = [segmentZ, controls(j, 3)]; 236 | 237 | plot3(segmentX, segmentY, segmentZ, '-.', 'Color', 'g', 'LineWidth', 0.1); 238 | plot3(controls(j, 1), controls(j, 2), controls(j, 3), 'bo', 'MarkerFaceColor', 'g', 'MarkerSize', 3); 239 | end 240 | end 241 | 242 | % Save original view with high resolution 243 | view(3); 244 | drawnow; 245 | saveas(gcf, sprintf('figs/KGMT_Iteration_%d.jpg', i)); 246 | print(sprintf('figs/KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); % Save with 300 DPI 247 | 248 | % Save top-down view with high resolution 249 | view(2); 250 | drawnow; 251 | saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 252 | print(sprintf('figs/top_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); % Save with 300 DPI 253 | 254 | % Save view looking along the x-axis from above 255 | midY = (min(samples(:,2)) + max(samples(:,2))) / 2; 256 | midZ = (min(samples(:,3)) + max(samples(:,3))) / 2; 257 | campos([0, midY, max(samples(:,3)) + 1]); % Position the camera above the max z-value 258 | camtarget([0, midY, midZ]); % Set the target point to the middle 259 | view([-1, .2, 0.6]); % Look along the x-axis 260 | drawnow; 261 | saveas(gcf, sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i)); 262 | print(sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); % Save with 300 DPI 263 | 264 | % Close the figure 265 | close(gcf); 266 | 267 | end -------------------------------------------------------------------------------- /viz/KGMT_3D_ExpansionColor.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 23; 7 | radius = 0.05; 8 | N = 8; 9 | n = 4; 10 | sampleSize = 10; 11 | stateSize = 6; 12 | controlSize = 3; 13 | xGoal = [.7, .95, .9]; 14 | alpha = .9; 15 | STEP_SIZE = .1; 16 | model = 2; 17 | 18 | % Obstacle file path 19 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/pillars/obstacles.csv'; 20 | obstacles = readmatrix(obstacleFilePath); 21 | 22 | treeSizePath = "/home/nicolas/dev/research/KPAX/build/Data/TreeSize/TreeSize0/treeSize.csv"; 23 | treeSizes = readmatrix(treeSizePath); 24 | treeSizes = [0; treeSizes]; 25 | 26 | colors = [0 0 1; % Blue 27 | 0 .9 .2; % Green 28 | 1 0 1; % Pink 29 | .7 .7 0; % Yellow 30 | 0 .7 .7; % Turquoise 31 | 1 .5 0]; % Orange 32 | 33 | fig = figure('Position', [100, 100, 1000, 1000]); 34 | hold on; 35 | axis equal; 36 | title('Iteration 0'); 37 | 38 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples1.csv"; 39 | samples = readmatrix(sampleFilePath); 40 | 41 | controlPath = '/home/nicolas/dev/research/KPAX/build/Data/ControlPathToGoal/ControlPathToGoal0/controlPathToGoal.csv'; 42 | controls = readmatrix(controlPath); 43 | controls = flipud(controls); 44 | controls = [samples(1,1), samples(1,2), samples(1,3), samples(1,4), samples(1,5), samples(1,6), 0, 0, 0, 0; controls]; 45 | 46 | plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 47 | 48 | [X, Y, Z] = sphere(20); 49 | surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 50 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 51 | 52 | for j = 1:size(obstacles, 1) 53 | x_min = obstacles(j, 1); 54 | y_min = obstacles(j, 2); 55 | z_min = obstacles(j, 3); 56 | x_max = obstacles(j, 4); 57 | y_max = obstacles(j, 5); 58 | z_max = obstacles(j, 6); 59 | vertices = [ 60 | x_min, y_min, z_min; 61 | x_max, y_min, z_min; 62 | x_max, y_max, z_min; 63 | x_min, y_max, z_min; 64 | x_min, y_min, z_max; 65 | x_max, y_min, z_max; 66 | x_max, y_max, z_max; 67 | x_min, y_max, z_max]; 68 | faces = [ 69 | 1, 2, 6, 5; 70 | 2, 3, 7, 6; 71 | 3, 4, 8, 7; 72 | 4, 1, 5, 8; 73 | 1, 2, 3, 4; 74 | 5, 6, 7, 8]; 75 | patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 76 | end 77 | 78 | camlight('headlight'); 79 | camlight('right'); 80 | lighting phong; 81 | 82 | view(3); 83 | drawnow; 84 | saveas(gcf, 'figs/KGMT_Iteration_0.jpg'); 85 | print('figs/KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 86 | 87 | view(2); 88 | drawnow; 89 | saveas(gcf, 'figs/top_KGMT_Iteration_0.jpg'); 90 | print('figs/top_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 91 | 92 | midY = 0.5 * xGoal(2); 93 | midZ = 0.5 * xGoal(3); 94 | campos([0, midY, xGoal(3) + 1]); 95 | camtarget([0, midY, midZ]); 96 | view([-.4, -.2, 0.5]); 97 | drawnow; 98 | saveas(gcf, 'figs/xAxis_KGMT_Iteration_0.jpg'); 99 | print('figs/xAxis_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 100 | 101 | close(gcf); 102 | iteration = 1; 103 | 104 | for i = 1:numFiles 105 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples" + i + ".csv"; 106 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Parents/Parents0/parents" + i + ".csv"; 107 | 108 | samples = readmatrix(sampleFilePath); 109 | parentRelations = readmatrix(parentFilePath); 110 | 111 | fig = figure('Position', [100, 100, 1000, 1000]); 112 | hold on; 113 | axis equal; 114 | title(sprintf('Iteration %d', i)); 115 | 116 | plot3(samples(1,1), samples(1,2), samples(1,3), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 117 | 118 | [X, Y, Z] = sphere(20); 119 | surf(radius * X + xGoal(1), radius * Y + xGoal(2), radius * Z + xGoal(3), ... 120 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 121 | 122 | for j = 1:size(obstacles, 1) 123 | x_min = obstacles(j, 1); 124 | y_min = obstacles(j, 2); 125 | z_min = obstacles(j, 3); 126 | x_max = obstacles(j, 4); 127 | y_max = obstacles(j, 5); 128 | z_max = obstacles(j, 6); 129 | vertices = [ 130 | x_min, y_min, z_min; 131 | x_max, y_min, z_min; 132 | x_max, y_max, z_min; 133 | x_min, y_max, z_min; 134 | x_min, y_min, z_max; 135 | x_max, y_min, z_max; 136 | x_max, y_max, z_max; 137 | x_min, y_max, z_max]; 138 | faces = [ 139 | 1, 2, 6, 5; 140 | 2, 3, 7, 6; 141 | 3, 4, 8, 7; 142 | 4, 1, 5, 8; 143 | 1, 2, 3, 4; 144 | 5, 6, 7, 8]; 145 | patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 146 | end 147 | 148 | camlight('headlight'); 149 | camlight('right'); 150 | lighting phong; 151 | 152 | colorIndex = 1; 153 | for j = 2:size(parentRelations, 1) 154 | if j > treeSizes(iteration) 155 | colorIndex = 3; 156 | else 157 | colorIndex = 1; 158 | end 159 | if parentRelations(j) == -1 160 | iteration = iteration + 1; 161 | break; 162 | end 163 | x0 = samples((parentRelations(j) + 1), 1:stateSize); 164 | sample = samples(j, :); 165 | if model == 1 166 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 167 | elseif model == 2 168 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 169 | end 170 | plot3(segmentX, segmentY, segmentZ, '-.', 'Color', 'k', 'LineWidth', 0.01); 171 | plot3(samples(j, 1), samples(j, 2), samples(j, 3), 'o', 'Color', colors(colorIndex, :), 'MarkerFaceColor', colors(colorIndex, :), 'MarkerSize', 2); 172 | end 173 | 174 | if i == numFiles 175 | for j = 2:size(controls, 1) 176 | x0 = controls(j-1, 1:stateSize); 177 | sample = controls(j,:); 178 | if model == 1 179 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 180 | elseif model == 2 181 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 182 | end 183 | plot3(segmentX, segmentY, segmentZ, 'Color', 'g', 'LineWidth', 1); 184 | plot3(controls(j, 1), controls(j, 2), controls(j, 3), 'o', 'Color', colors(colorIndex, :), 'MarkerFaceColor', colors(colorIndex, :), 'MarkerSize', 2); 185 | end 186 | end 187 | 188 | view(3); 189 | drawnow; 190 | saveas(gcf, sprintf('figs/KGMT_Iteration_%d.jpg', i)); 191 | print(sprintf('figs/KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 192 | 193 | view(2); 194 | drawnow; 195 | saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 196 | print(sprintf('figs/top_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 197 | 198 | midY = (min(samples(:,2)) + max(samples(:,2))) / 2; 199 | midZ = (min(samples(:,3)) + max(samples(:,3))) / 2; 200 | campos([0, midY, max(samples(:,3)) + 1]); 201 | camtarget([0, midY, midZ]); 202 | view([-.4, -.2, 0.5]); 203 | drawnow; 204 | 205 | saveas(gcf, sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i)); 206 | print(sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 207 | 208 | close(gcf); 209 | end 210 | 211 | function [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize) 212 | segmentX = [x0(1)]; 213 | segmentY = [x0(2)]; 214 | segmentZ = [x0(3)]; 215 | u = sample(stateSize+1:sampleSize-1); 216 | duration = sample(sampleSize); 217 | numDisc = duration/STEP_SIZE; 218 | x = x0(1); 219 | y = x0(2); 220 | z = x0(3); 221 | vx = x0(4); 222 | vy = x0(5); 223 | vz = x0(6); 224 | ax = u(1); 225 | ay = u(2); 226 | az = u(3); 227 | for k = 1:(numDisc) 228 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 229 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 230 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 231 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 232 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 233 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 234 | segmentX = [segmentX, x]; 235 | segmentY = [segmentY, y]; 236 | segmentZ = [segmentZ, z]; 237 | end 238 | segmentX = [segmentX, sample(1)]; 239 | segmentY = [segmentY, sample(2)]; 240 | segmentZ = [segmentZ, sample(3)]; 241 | end 242 | 243 | function [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize) 244 | segmentX = [x0(1)]; 245 | segmentY = [x0(2)]; 246 | segmentZ = [x0(3)]; 247 | u = sample(stateSize+1:sampleSize-1); 248 | duration = sample(sampleSize); 249 | numDisc = duration/STEP_SIZE; 250 | x = x0(1); 251 | y = x0(2); 252 | z = x0(3); 253 | yaw = x0(4); 254 | pitch = x0(5); 255 | v = x0(6); 256 | yawRate = u(1); 257 | pitchRate = u(2); 258 | a = u(3); 259 | for k = 1:(numDisc) 260 | x = x + (STEP_SIZE / 6.0) * ... 261 | (v * cos(pitch) * cos(yaw) + ... 262 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate) + ... 263 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 264 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * cos(yaw + STEP_SIZE * yawRate)); 265 | 266 | y = y + (STEP_SIZE / 6.0) * ... 267 | (v * cos(pitch) * sin(yaw) + ... 268 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate) + ... 269 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 270 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * sin(yaw + STEP_SIZE * yawRate)); 271 | 272 | z = z + (STEP_SIZE / 6.0) * ... 273 | (v * sin(pitch) + ... 274 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate) + ... 275 | (v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate)) + ... 276 | (v + STEP_SIZE * a) * sin(pitch + STEP_SIZE * pitchRate)); 277 | 278 | yaw = yaw + STEP_SIZE * yawRate; 279 | pitch = pitch + STEP_SIZE * pitchRate; 280 | v = v + (STEP_SIZE / 6.0) * (a + 2.0 * (a + a) + a); 281 | segmentX = [segmentX, x]; 282 | segmentY = [segmentY, y]; 283 | segmentZ = [segmentZ, z]; 284 | end 285 | segmentX = [segmentX, sample(1)]; 286 | segmentY = [segmentY, sample(2)]; 287 | segmentZ = [segmentZ, sample(3)]; 288 | end -------------------------------------------------------------------------------- /benchmarking/bench.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear all; 3 | close all; 4 | 5 | %% consts: 6 | titleSize = 10; 7 | algorithmSize = 9; 8 | figureSize = [100, 100, 1000, 1000]; 9 | 10 | %% 12DQuad: 11 | 12 | % -- File Paths -- 13 | kgmtExecutionTimePath = '/home/nicolas/dev/research/KPAX/benchmarking/kgmtJetson/FlyingUni/house/executionTime.csv'; 14 | kgmtExpandedNodesPath = '/home/nicolas/dev/research/KPAX/benchmarking/kpax/DubinsAirplane/Data/ExpandedNodes/'; 15 | kgmtTreeSizePath = '/home/nicolas/dev/research/KPAX/benchmarking/kpax/DubinsAirplane/Data/TreeSize/'; 16 | 17 | kgmtStateGridExecutionTimePath = '/home/nicolas/dev/research/KPAX/benchmarking/kgmtStateGrid/FlyingUni/house/executionTime.csv'; 18 | kgmtStateGridExpandedNodesPath = '/home/nicolas/dev/research/KPAX/benchmarking/kgmtStateGrid/FlyingUni/Data/ExpandedNodes/'; 19 | kgmtStateGridTreeSizePath = '/home/nicolas/dev/research/KPAX/benchmarking/kgmtStateGrid/FlyingUni/Data/TreeSize/'; 20 | 21 | rrtParallelExecutionTimePath = '/home/nicolas/dev/research/KPAX/benchmarking/rrtParallel/FlyingUni/house/Data/ExecutionTime/executionTime.csv'; 22 | rrtParallelTreeSize = '/home/nicolas/dev/research/KPAX/benchmarking/rrtParallel/FlyingUni/Data/Vertices/vertices.csv'; 23 | rrtParallelExpandedNodesPath = '/home/nicolas/dev/research/KPAX/benchmarking/rrtParallel/FlyingUni/Data/Iterations/iterations.csv'; 24 | 25 | estParallelExecutionTimePath = '/home/nicolas/dev/research/KPAX/benchmarking/estParallel/FlyingUni/house/Data/ExecutionTime/executionTime.csv'; 26 | estParallelExpandedNodesPath = '/home/nicolas/dev/research/KPAX/benchmarking/estParallel/FlyingUni/Data/Iterations/iterations.csv'; 27 | estParallelTreeSize = '/home/nicolas/dev/research/KPAX/benchmarking/estParallel/FlyingUni/Data/Vertices/vertices.csv'; 28 | 29 | pdstParallelExecutionTimePath = '/home/nicolas/dev/research/KPAX/benchmarking/pdstParallel/FlyingUni/house/Data/ExecutionTime/executionTime.csv'; 30 | pdstParallelExpandedNodesPath = '/home/nicolas/dev/research/KPAX/benchmarking/pdstParallel/FlyingUni/Data/Iterations/iterations.csv'; 31 | pdstParallelTreeSize = '/home/nicolas/dev/research/KPAX/benchmarking/pdstParallel/FlyingUni/Data/Vertices/vertices.csv'; 32 | 33 | % -- Execution Time Data -- 34 | kgmtExecutionTime = readmatrix(kgmtExecutionTimePath) * 1000; 35 | kgmtStateGridExecutionTime = readmatrix(kgmtStateGridExecutionTimePath) * 1000; 36 | rrtParallelExecutionTime = readmatrix(rrtParallelExecutionTimePath) * 1000; 37 | estParallelExecutionTime = readmatrix(estParallelExecutionTimePath) * 1000; 38 | pdstParallelExecutionTime = readmatrix(pdstParallelExecutionTimePath) * 1000; 39 | 40 | % -- Node Expansion / Tree Size Data -- 41 | N = length(dir(kgmtExpandedNodesPath))-2; 42 | kgmtExpandedNodes = zeros(N, 1); 43 | kgmtTreeSize = zeros(N, 1); 44 | kgmtFrontierSize = zeros(N, 1); 45 | 46 | % kgmtStateGridExpandedNodes = zeros(N, 1); 47 | % kgmtStateGridTreeSize = zeros(N, 1); 48 | % kgmtStateGridFrontierSize = zeros(N, 1); 49 | % for i = 1:N 50 | % expandedNodesPath = append(kgmtExpandedNodesPath, 'ExpandedNodes', num2str(i-1), '/expandedNodes.csv'); 51 | % treeSizePath = append(kgmtTreeSizePath, 'TreeSize', num2str(i-1), '/treeSize.csv'); 52 | % kgmtExpandedNodes(i) = sum(readmatrix(expandedNodesPath)); 53 | % treeSize = readmatrix(treeSizePath); 54 | % kgmtTreeSize(i) = treeSize(end); 55 | % 56 | % expandedNodesStateGridPath = append(kgmtStateGridExpandedNodesPath, 'ExpandedNodes', num2str(i-1), '/expandedNodes.csv'); 57 | % treeSizeStateGridPath = append(kgmtStateGridTreeSizePath, 'TreeSize', num2str(i-1), '/treeSize.csv'); 58 | % kgmtStateGridExpandedNodes(i) = sum(readmatrix(expandedNodesStateGridPath)); 59 | % treeSizeStateGrid = readmatrix(treeSizeStateGridPath); 60 | % kgmtStateGridTreeSize(i) = treeSizeStateGrid(end); 61 | % end 62 | 63 | % rrtParallelExpandedNodes = readmatrix(rrtParallelExpandedNodesPath); 64 | % rrtParallelTreeSize = readmatrix(rrtParallelTreeSize); 65 | % 66 | % estParallelExpandedNodes = readmatrix(estParallelExpandedNodesPath); 67 | % estParallelTreeSize = readmatrix(estParallelTreeSize); 68 | % 69 | % pdstParallelExpandedNodes = readmatrix(pdstParallelExpandedNodesPath); 70 | % pdstParallelTreeSize = readmatrix(pdstParallelTreeSize); 71 | 72 | plotBenchmarkResultsDA(kgmtExecutionTime, kgmtStateGridExecutionTime, rrtParallelExecutionTime, estParallelExecutionTime, pdstParallelExecutionTime); 73 | 74 | function plotBenchmarkResultsDA(kgmtExecutionTime, kgmtStateGridExecutionTime, rrtParallelExecutionTime, estParallelExecutionTime, pdstParallelExecutionTime) 75 | 76 | %% Execution Time 77 | 78 | kgmtStateGrid_mean = mean(kgmtStateGridExecutionTime); 79 | kgmtStateGrid_std = std(kgmtStateGridExecutionTime); 80 | kgmtStateGrid_min = min(kgmtStateGridExecutionTime); 81 | kgmtStateGrid_max = max(kgmtStateGridExecutionTime); 82 | kgmtStateGrid_success = sum(kgmtStateGridExecutionTime < 60000) / length(kgmtStateGridExecutionTime) * 100; 83 | 84 | kgmt_mean = mean(kgmtExecutionTime); 85 | kgmt_std = std(kgmtExecutionTime); 86 | kgmt_min = min(kgmtExecutionTime); 87 | kgmt_max = max(kgmtExecutionTime); 88 | kgmt_success = sum(kgmtExecutionTime < 60000) / length(kgmtExecutionTime) * 100; 89 | kgmtJetson_ratio = kgmt_mean / kgmtStateGrid_mean; 90 | 91 | rrtParallel_mean = mean(rrtParallelExecutionTime); 92 | rrtParallel_std = std(rrtParallelExecutionTime); 93 | rrtParallel_min = min(rrtParallelExecutionTime); 94 | rrtParallel_max = max(rrtParallelExecutionTime); 95 | rrtParallel_success = sum(rrtParallelExecutionTime < 60000) / length(rrtParallelExecutionTime) * 100; 96 | rrtParallel_ratio = rrtParallel_mean / kgmtStateGrid_mean; 97 | 98 | estParallel_mean = mean(estParallelExecutionTime); 99 | estParallel_std = std(estParallelExecutionTime); 100 | estParallel_min = min(estParallelExecutionTime); 101 | estParallel_max = max(estParallelExecutionTime); 102 | estParallel_success = sum(estParallelExecutionTime < 60000) / length(estParallelExecutionTime) * 100; 103 | estParallel_ratio = estParallel_mean / kgmtStateGrid_mean; 104 | 105 | pdstParallel_mean = mean(pdstParallelExecutionTime); 106 | pdstParallel_std = std(pdstParallelExecutionTime); 107 | pdstParallel_min = min(pdstParallelExecutionTime); 108 | pdstParallel_max = max(pdstParallelExecutionTime); 109 | pdstParallel_success = sum(pdstParallelExecutionTime < 60000) / length(pdstParallelExecutionTime) * 100; 110 | pdstParallel_ratio = pdstParallel_mean / kgmtStateGrid_mean; 111 | 112 | 113 | 114 | %% Print Execution Times, Success Percentages, and Ratios 115 | fprintf('/* KPAX Jetson Execution Time */\n'); 116 | fprintf('Mean: %.2f ms\n', kgmt_mean); 117 | fprintf('Standard Deviation: %.2f ms\n', kgmt_std); 118 | fprintf('Minimum: %.2f ms\n', kgmt_min); 119 | fprintf('Maximum: %.2f ms\n', kgmt_max); 120 | fprintf('Success Percentage: %.2f%%\n', kgmt_success); 121 | fprintf('Ratio (Mean Time / KPAX State Grid Mean Time): %.2f\n', kgmtJetson_ratio); 122 | fprintf('/***************************/\n\n'); 123 | 124 | fprintf('/* KPAX State Grid Execution Time */\n'); 125 | fprintf('Mean: %.2f ms\n', kgmtStateGrid_mean); 126 | fprintf('Standard Deviation: %.2f ms\n', kgmtStateGrid_std); 127 | fprintf('Minimum: %.2f ms\n', kgmtStateGrid_min); 128 | fprintf('Maximum: %.2f ms\n', kgmtStateGrid_max); 129 | fprintf('Success Percentage: %.2f%%\n', kgmtStateGrid_success); 130 | fprintf('/***************************/\n\n'); 131 | 132 | fprintf('/* RRT-Parallel Execution Time */\n'); 133 | fprintf('Mean: %.2f ms\n', rrtParallel_mean); 134 | fprintf('Standard Deviation: %.2f ms\n', rrtParallel_std); 135 | fprintf('Minimum: %.2f ms\n', rrtParallel_min); 136 | fprintf('Maximum: %.2f ms\n', rrtParallel_max); 137 | fprintf('Success Percentage: %.2f%%\n', rrtParallel_success); 138 | fprintf('Ratio (Mean Time / KPAX State Grid Mean Time): %.2f\n', rrtParallel_ratio); 139 | fprintf('/***************************/\n\n'); 140 | 141 | fprintf('/* EST-Parallel Execution Time */\n'); 142 | fprintf('Mean: %.2f ms\n', estParallel_mean); 143 | fprintf('Standard Deviation: %.2f ms\n', estParallel_std); 144 | fprintf('Minimum: %.2f ms\n', estParallel_min); 145 | fprintf('Maximum: %.2f ms\n', estParallel_max); 146 | fprintf('Success Percentage: %.2f%%\n', estParallel_success); 147 | fprintf('Ratio (Mean Time / KPAX State Grid Mean Time): %.2f\n', estParallel_ratio); 148 | fprintf('/***************************/\n\n'); 149 | 150 | fprintf('/* PDST-Parallel Execution Time */\n'); 151 | fprintf('Mean: %.2f ms\n', pdstParallel_mean); 152 | fprintf('Standard Deviation: %.2f ms\n', pdstParallel_std); 153 | fprintf('Minimum: %.2f ms\n', pdstParallel_min); 154 | fprintf('Maximum: %.2f ms\n', pdstParallel_max); 155 | fprintf('Success Percentage: %.2f%%\n', pdstParallel_success); 156 | fprintf('Ratio (Mean Time / KPAX State Grid Mean Time): %.2f\n', pdstParallel_ratio); 157 | fprintf('/***************************/\n\n'); 158 | 159 | 160 | 161 | 162 | % %% Nodes Expanded 163 | % data = [kgmtExpandedNodes; kgmtStateGridExpandedNodes]; 164 | % group = [ones(length(kgmtExpandedNodes), 1); 2 * ones(length(kgmtStateGridExpandedNodes), 1)]; 165 | % 166 | % figure; 167 | % b = boxchart(group, data); 168 | % b.JitterOutliers = 'on'; 169 | % b.MarkerStyle = '.'; 170 | % 171 | % title(sprintf('Number Of Nodes Expanded (KPAX vs KPAX State Grid) - %s', dynamics), 'FontSize', titleSize, 'FontWeight', 'Bold'); 172 | % ylabel('Number Of Expanded Nodes', 'FontSize', 14, 'FontWeight', 'Bold'); 173 | % xticks([1, 2]); 174 | % xticklabels({'KPAX', 'KPAX State Grid'}); 175 | % set(gca, 'FontSize', algorithmSize); 176 | % set(gcf, 'Position', figureSize); 177 | % 178 | % saveas(gcf, fullfile(output_dir, 'NodesExpanded_KGMT_vs_KGMTStateGrid.jpg')); 179 | % print(fullfile(output_dir, 'NodesExpanded_KGMT_vs_KGMTStateGrid.jpg'), '-djpeg', '-r300'); 180 | % 181 | % %% Tree Size 182 | % data = [kgmtTreeSize; kgmtStateGridTreeSize]; 183 | % group = [ones(length(kgmtTreeSize), 1); 2 * ones(length(kgmtStateGridTreeSize), 1)]; 184 | % 185 | % figure; 186 | % b = boxchart(group, data); 187 | % b.JitterOutliers = 'on'; 188 | % b.MarkerStyle = '.'; 189 | % 190 | % title(sprintf('Tree Size (KPAX vs KPAX State Grid) - %s', dynamics), 'FontSize', titleSize, 'FontWeight', 'Bold'); 191 | % ylabel('Number Of Nodes In Tree', 'FontSize', 14, 'FontWeight', 'Bold'); 192 | % xticks([1, 2]); 193 | % xticklabels({'KPAX', 'KPAX State Grid'}); 194 | % set(gca, 'FontSize', algorithmSize); 195 | % set(gcf, 'Position', figureSize); 196 | % 197 | % saveas(gcf, fullfile(output_dir, 'TreeSize_KGMT_vs_KGMTStateGrid.jpg')); 198 | % print(fullfile(output_dir, 'TreeSize_KGMT_vs_KGMTStateGrid.jpg'), '-djpeg', '-r300'); 199 | 200 | %% Full Comparison (Original Code) 201 | % data = [kgmtExecutionTime; kgmtStateGridExecutionTime; rrtParallelExecutionTime; estParallelExecutionTime; pdstParallelExecutionTime]; 202 | % group = [ones(length(kgmtExecutionTime), 1); 2 * ones(length(kgmtStateGridExecutionTime), 1); 3 * ones(length(rrtParallelExecutionTime), 1); 4 * ones(length(estParallelExecutionTime), 1); 5 * ones(length(pdstParallelExecutionTime), 1)]; 203 | % 204 | % figure; 205 | % b = boxchart(group, data); 206 | % b.JitterOutliers = 'on'; 207 | % b.MarkerStyle = '.'; 208 | % 209 | % title(sprintf('Execution Time (%s)', dynamics), 'FontSize', titleSize, 'FontWeight', 'Bold'); 210 | % ylabel('Execution Time (ms)', 'FontSize', 14, 'FontWeight', 'Bold'); 211 | % xticks([1, 2, 3, 4, 5]); 212 | % xticklabels({'KPAX', 'KPAX State Grid', 'RRT (CPU Parallelization)', 'EST (CPU Parallelization)', 'PDST (CPU Parallelization)'}); 213 | % set(gca, 'FontSize', algorithmSize); 214 | % set(gcf, 'Position', figureSize); 215 | % 216 | % saveas(gcf, fullfile(output_dir, 'ExecutionTime_Comparison.jpg')); 217 | % print(fullfile(output_dir, 'ExecutionTime_Comparison.jpg'), '-djpeg', '-r300'); 218 | 219 | % %% Nodes Expanded (Original Code) 220 | % data = [kgmtExpandedNodes; kgmtStateGridExpandedNodes; rrtParallelExpandedNodes; estParallelExpandedNodes; pdstParallelExpandedNodes]; 221 | % group = [ones(length(kgmtExpandedNodes), 1); 2 * ones(length(kgmtStateGridExpandedNodes), 1); 3 * ones(length(rrtParallelExpandedNodes), 1); 4 * ones(length(estParallelExpandedNodes), 1); 5 * ones(length(pdstParallelExpandedNodes), 1)]; 222 | % 223 | % figure; 224 | % b = boxchart(group, data); 225 | % b.JitterOutliers = 'on'; 226 | % b.MarkerStyle = '.'; 227 | % 228 | % title(sprintf('Number Of Nodes Expanded (%s)', dynamics), 'FontSize', titleSize, 'FontWeight', 'Bold'); 229 | % ylabel('Number Of Expanded Nodes', 'FontSize', 14, 'FontWeight', 'Bold'); 230 | % xticks([1, 2, 3, 4, 5]); 231 | % xticklabels({'KPAX', 'KPAX State Grid', 'RRT (CPU Parallelization)', 'EST (CPU Parallelization)', 'PDST (CPU Parallelization)'}); 232 | % set(gca, 'FontSize', algorithmSize); 233 | % set(gcf, 'Position', figureSize); 234 | % 235 | % saveas(gcf, fullfile(output_dir, 'NodesExpanded_Comparison.jpg')); 236 | % print(fullfile(output_dir, 'NodesExpanded_Comparison.jpg'), '-djpeg', '-r300'); 237 | % 238 | % %% Tree Size (Original Code) 239 | % data = [kgmtTreeSize; kgmtStateGridTreeSize; rrtParallelTreeSize; estParallelTreeSize; pdstParallelTreeSize]; 240 | % group = [ones(length(kgmtTreeSize), 1); 2 * ones(length(kgmtStateGridTreeSize), 1); 3 * ones(length(rrtParallelTreeSize), 1); 4 * ones(length(estParallelTreeSize), 1); 5 * ones(length(pdstParallelTreeSize), 1)]; 241 | % 242 | % figure; 243 | % b = boxchart(group, data); 244 | % b.JitterOutliers = 'on'; 245 | % b.MarkerStyle = '.'; 246 | % 247 | % title(sprintf('Tree Size (%s)', dynamics), 'FontSize', titleSize, 'FontWeight', 'Bold'); 248 | % ylabel('Number Of Nodes In Tree', 'FontSize', 14, 'FontWeight', 'Bold'); 249 | % xticks([1, 2, 3, 4, 5]); 250 | % xticklabels({'KPAX', 'KPAX State Grid', 'RRT (CPU Parallelization)', 'EST (CPU Parallelization)', 'PDST (CPU Parallelization)'}); 251 | % set(gca, 'FontSize', algorithmSize); 252 | % set(gcf, 'Position', figureSize); 253 | % 254 | % saveas(gcf, fullfile(output_dir, 'TreeSize_Comparison.jpg')); 255 | % print(fullfile(output_dir, 'TreeSize_Comparison.jpg'), '-djpeg', '-r300'); 256 | end 257 | 258 | -------------------------------------------------------------------------------- /src/graphs/Graph.cu: -------------------------------------------------------------------------------- 1 | #include "graphs/Graph.cuh" 2 | #include "config/config.h" 3 | #include 4 | 5 | Graph::Graph(const float ws) 6 | { 7 | if(VERBOSE) 8 | { 9 | printf("/***************************/\n"); 10 | printf("/* Grid Dimension: %d */\n", W_DIM + C_DIM + V_DIM); 11 | printf("/***************************/\n"); 12 | } 13 | 14 | h_numPartialSums_ = iDivUp(NUM_R1_REGIONS, h_blockSize_); 15 | 16 | d_validCounterArray_ = thrust::device_vector(NUM_R1_REGIONS); 17 | d_counterArray_ = thrust::device_vector(NUM_R1_REGIONS); 18 | d_vertexScoreArray_ = thrust::device_vector(NUM_R1_REGIONS); 19 | d_activeVerticesScanIdx_ = thrust::device_vector(NUM_R1_REGIONS); 20 | d_activeSubVertices_ = thrust::device_vector(NUM_R2_REGIONS); 21 | d_minValueInRegion_ = thrust::device_vector(NUM_R1_REGIONS * STATE_DIM); 22 | d_partialSums_ = thrust::device_vector(h_numPartialSums_); 23 | d_totalScore_ = thrust::device_vector(1, 0.0); 24 | 25 | d_validCounterArray_ptr_ = thrust::raw_pointer_cast(d_validCounterArray_.data()); 26 | d_counterArray_ptr_ = thrust::raw_pointer_cast(d_counterArray_.data()); 27 | d_vertexScoreArray_ptr_ = thrust::raw_pointer_cast(d_vertexScoreArray_.data()); 28 | d_activeSubVertices_ptr_ = thrust::raw_pointer_cast(d_activeSubVertices_.data()); 29 | d_minValueInRegion_ptr_ = thrust::raw_pointer_cast(d_minValueInRegion_.data()); 30 | d_partialSums_ptr_ = thrust::raw_pointer_cast(d_partialSums_.data()); 31 | d_totalScore_ptr_ = thrust::raw_pointer_cast(d_totalScore_.data()); 32 | 33 | initializeRegions(); 34 | 35 | std::ostringstream filename; 36 | std::filesystem::create_directories("Data"); 37 | std::filesystem::create_directories("Data/RegionMins"); 38 | 39 | filename.str(""); 40 | filename << "Data/RegionMins/RegionMins_" << ws << ".csv"; 41 | copyAndWriteVectorToCSV(d_minValueInRegion_, filename.str(), NUM_R1_REGIONS, 1, false); 42 | } 43 | 44 | void Graph::initializeRegions() 45 | { 46 | initializeRegions_kernel<<>>(d_minValueInRegion_ptr_); 47 | } 48 | 49 | /***************************/ 50 | /* INITIALIZE REGIONS KERNEL */ 51 | /***************************/ 52 | // --- one thread per R1 region --- 53 | __global__ void initializeRegions_kernel(float* minValueInRegion) 54 | { 55 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 56 | if(tid >= NUM_R1_REGIONS) return; 57 | 58 | int wRegion = tid % (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH); 59 | int wIndex[W_DIM]; 60 | int temp = wRegion; 61 | for(int i = W_DIM - 1; i >= 0; --i) 62 | { 63 | wIndex[i] = temp % W_R1_LENGTH; 64 | temp /= W_R1_LENGTH; 65 | } 66 | 67 | for(int i = 0; i < W_DIM; ++i) 68 | { 69 | minValueInRegion[tid * STATE_DIM + i] = W_MIN + wIndex[i] * W_R1_SIZE; 70 | } 71 | 72 | int aRegion = (tid / (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH)) % (C_R1_LENGTH * C_R1_LENGTH); 73 | int aIndex[C_DIM]; 74 | temp = aRegion; 75 | for(int i = C_DIM - 1; i >= 0; --i) 76 | { 77 | aIndex[i] = temp % C_R1_LENGTH; 78 | temp /= C_R1_LENGTH; 79 | } 80 | for(int i = 0; i < C_DIM; ++i) 81 | { 82 | minValueInRegion[tid * STATE_DIM + W_DIM + i] = C_MIN + aIndex[i] * C_R1_SIZE; 83 | } 84 | 85 | int vRegion = (tid / (W_R1_LENGTH * W_R1_LENGTH * W_R1_LENGTH * C_R1_LENGTH * C_R1_LENGTH)) % V_R1_LENGTH; 86 | int vIndex[V_DIM]; 87 | temp = vRegion; 88 | for(int i = V_DIM - 1; i >= 0; --i) 89 | { 90 | vIndex[i] = temp % V_R1_LENGTH; 91 | temp /= V_R1_LENGTH; 92 | } 93 | for(int i = 0; i < V_DIM; ++i) 94 | { 95 | minValueInRegion[tid * STATE_DIM + W_DIM + C_DIM + i] = V_MIN + vIndex[i] * V_R1_SIZE; 96 | } 97 | } 98 | 99 | __host__ __device__ int getRegion(float* coord) 100 | { 101 | // --- Workspace --- 102 | int wRegion = 0; 103 | int factor = 1; 104 | int index; 105 | for(int i = W_DIM - 1; i >= 0; --i) 106 | { 107 | index = (int)(W_R1_LENGTH * (coord[i] - W_MIN) / (W_MAX - W_MIN)); 108 | if(index >= W_R1_LENGTH) index = W_R1_LENGTH - 1; 109 | if(index < 0) index = 0; 110 | 111 | wRegion += factor * index; 112 | factor *= W_R1_LENGTH; 113 | } 114 | 115 | if(V_DIM == 1 && C_DIM == 1) 116 | { 117 | return wRegion; 118 | } 119 | 120 | // --- Attitude --- 121 | int aRegion = 0; 122 | if(C_R1_LENGTH > 1) 123 | { 124 | factor = 1; 125 | for(int i = C_DIM - 1; i >= 0; --i) 126 | { 127 | index = (int)(C_R1_LENGTH * (coord[i + W_DIM] - C_MIN) / (C_MAX - C_MIN)); 128 | if(index >= C_R1_LENGTH) index = C_R1_LENGTH - 1; 129 | if(index < 0) index = 0; 130 | 131 | aRegion += factor * index; 132 | factor *= C_R1_LENGTH; 133 | } 134 | } 135 | 136 | // --- Velocity --- 137 | int vRegion = 0; 138 | if(V_R1_LENGTH > 1) 139 | { 140 | factor = 1; 141 | for(int i = V_DIM - 1; i >= 0; --i) 142 | { 143 | index = (int)(V_R1_LENGTH * (coord[i + W_DIM + C_DIM] - V_MIN) / (V_MAX - V_MIN)); 144 | if(index >= V_R1_LENGTH) index = V_R1_LENGTH - 1; 145 | if(index < 0) index = 0; 146 | 147 | vRegion += factor * index; 148 | factor *= V_R1_LENGTH; 149 | } 150 | } 151 | 152 | return wRegion * pow(C_R1_LENGTH, C_DIM) * pow(V_R1_LENGTH, V_DIM) + aRegion * pow(V_R1_LENGTH, V_DIM) + vRegion; 153 | } 154 | 155 | __device__ int getSubRegion(float* coord, int r1, float* minRegion) 156 | { 157 | // --- Workspace --- 158 | int wRegion = 0; 159 | int factor = 1; 160 | int index; 161 | 162 | for(int i = W_DIM - 1; i >= 0; --i) 163 | { 164 | index = (int)(W_R2_LENGTH * (coord[i] - minRegion[r1 * STATE_DIM + i]) / (W_R1_SIZE)); 165 | if(index >= W_R2_LENGTH) index = W_R2_LENGTH - 1; 166 | if(index < 0) index = 0; 167 | 168 | wRegion += factor * index; 169 | factor *= W_R2_LENGTH; 170 | } 171 | 172 | // --- Attitude --- 173 | int aRegion = 0; 174 | if(C_R2_LENGTH > 1) 175 | { 176 | factor = 1; 177 | for(int i = C_DIM - 1; i >= 0; --i) 178 | { 179 | index = (int)(C_R2_LENGTH * (coord[i + W_DIM] - minRegion[r1 * STATE_DIM + i + W_DIM]) / (C_R1_SIZE)); 180 | if(index >= C_R2_LENGTH) index = C_R2_LENGTH - 1; 181 | if(index < 0) index = 0; 182 | 183 | aRegion += factor * index; 184 | factor *= C_R2_LENGTH; 185 | } 186 | } 187 | 188 | // --- Velocity --- 189 | int vRegion = 0; 190 | if(V_R2_LENGTH > 1) 191 | { 192 | factor = 1; 193 | for(int i = V_DIM - 1; i >= 0; --i) 194 | { 195 | index = (int)(V_R2_LENGTH * (coord[i + W_DIM + C_DIM] - minRegion[r1 * STATE_DIM + i + W_DIM + C_DIM]) / (V_R1_SIZE)); 196 | if(index >= V_R2_LENGTH) index = V_R2_LENGTH - 1; 197 | if(index < 0) index = 0; 198 | 199 | vRegion += factor * index; 200 | factor *= V_R2_LENGTH; 201 | } 202 | } 203 | 204 | return r1 * NUM_R2_PER_R1 + (wRegion * pow(C_R2_LENGTH, C_DIM) * pow(V_R2_LENGTH, V_DIM) + aRegion * pow(V_R2_LENGTH, V_DIM) + vRegion); 205 | } 206 | 207 | void Graph::updateVertices() 208 | { 209 | if(NUM_R1_REGIONS > 1024) 210 | { 211 | // --- Update R1 Scores --- 212 | partialReduction_kernel<<>>(d_activeSubVertices_ptr_, d_validCounterArray_ptr_, 213 | d_counterArray_ptr_, d_vertexScoreArray_ptr_, d_partialSums_ptr_); 214 | // --- Sum R1 Scores --- 215 | globalReduction_kernel<<<1, h_numPartialSums_>>>(d_partialSums_ptr_, d_totalScore_ptr_, h_numPartialSums_); 216 | 217 | // --- Normalize R1 Scores --- 218 | updateSampleAcceptance_kernel<<>>(d_validCounterArray_ptr_, d_vertexScoreArray_ptr_, 219 | d_totalScore_ptr_); 220 | } 221 | else 222 | { 223 | // --- Update vertex scores and sampleScoreThreshold --- 224 | updateVertices_kernel<<<1, NUM_R1_REGIONS>>>(d_activeSubVertices_ptr_, d_validCounterArray_ptr_, d_counterArray_ptr_, 225 | d_vertexScoreArray_ptr_); 226 | } 227 | } 228 | 229 | /***************************/ 230 | /* PARTIAL REDUCTION KERNEL */ 231 | /***************************/ 232 | // --- calculates score for each region and does a partial blockwise sum of scores. --- 233 | __global__ void 234 | partialReduction_kernel(int* activeSubVertices, int* validCounterArray, int* counterArray, float* vertexScores, float* partialSums) 235 | { 236 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 237 | if(tid >= NUM_R1_REGIONS) return; 238 | 239 | float score = 0.0; 240 | 241 | if(validCounterArray[tid] > 0) 242 | { 243 | int numValidSamples = validCounterArray[tid]; 244 | float coverage = 0; 245 | 246 | // --- Thread loops through all sub vertices to determine vertex coverage. --- 247 | for(int i = tid * NUM_R2_PER_R1; i < (tid + 1) * NUM_R2_PER_R1; ++i) 248 | { 249 | coverage += activeSubVertices[i]; 250 | } 251 | coverage /= NUM_R2_PER_R1; 252 | 253 | // --- From OMPL Syclop ref: https://ompl.kavrakilab.org/classompl_1_1control_1_1Syclop.html--- 254 | float freeVol = (EPSILON + numValidSamples) / (EPSILON + numValidSamples + (counterArray[tid] - numValidSamples)) * W_R1_VOL; 255 | score = pow(freeVol, 4) / ((1 + coverage) * (1 + pow(counterArray[tid], 2))); 256 | vertexScores[tid] = score; 257 | } 258 | 259 | // --- Sum scores from each thread to determine score threshold --- 260 | typedef cub::BlockReduce BlockReduceFloatT; 261 | __shared__ typename BlockReduceFloatT::TempStorage tempStorageFloat; 262 | float blockSum = BlockReduceFloatT(tempStorageFloat).Sum(score); 263 | 264 | if(threadIdx.x == 0) 265 | { 266 | partialSums[threadIdx.x] = blockSum; 267 | } 268 | } 269 | 270 | /***************************/ 271 | /* GLOBAL REDUCTION KERNEL */ 272 | /***************************/ 273 | // --- Sums all partial sums into totalScore --- 274 | __global__ void globalReduction_kernel(float* partialSums, float* totalScore, int numPartialSums) 275 | { 276 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 277 | if(tid >= numPartialSums) return; 278 | 279 | typedef cub::BlockReduce BlockReduceFloatT; 280 | __shared__ typename BlockReduceFloatT::TempStorage tempStorageFloat; 281 | float blockSum = BlockReduceFloatT(tempStorageFloat).Sum(partialSums[tid]); 282 | 283 | if(threadIdx.x == 0) 284 | { 285 | atomicAdd(totalScore, blockSum); 286 | } 287 | } 288 | 289 | /***************************/ 290 | /* UPDATE SAMPLE ACCEPTANCE KERNEL */ 291 | /***************************/ 292 | // --- normalizes score for each active region --- 293 | __global__ void updateSampleAcceptance_kernel(int* validCounterArray, float* vertexScores, float* totalScore) 294 | { 295 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 296 | if(tid >= NUM_R1_REGIONS) return; 297 | if(validCounterArray[tid] == 0) 298 | { 299 | vertexScores[tid] = 1.0f; 300 | } 301 | else 302 | { 303 | vertexScores[tid] = EPSILON + (vertexScores[tid] / *totalScore); 304 | } 305 | } 306 | 307 | /***************************/ 308 | /* VERTICES UPDATE KERNEL */ 309 | /***************************/ 310 | // --- Updates Vertex Scores for device graph vectors. Determines new threshold score for future samples in expansion set. --- 311 | __global__ void updateVertices_kernel(int* activeSubVertices, int* validCounterArray, int* counterArray, float* vertexScores) 312 | { 313 | int tid = threadIdx.x + blockIdx.x * blockDim.x; 314 | if(tid >= NUM_R1_REGIONS - 1) return; 315 | 316 | __shared__ float s_totalScore; 317 | float score = 0.0; 318 | 319 | if(validCounterArray[tid] > 0) 320 | { 321 | int numValidSamples = validCounterArray[tid]; 322 | float coverage = 0; 323 | 324 | // --- Thread loops through all sub vertices to determine vertex coverage. --- 325 | for(int i = tid * NUM_R2_PER_R1; i < (tid + 1) * NUM_R2_PER_R1; ++i) 326 | { 327 | coverage += activeSubVertices[i]; 328 | } 329 | 330 | coverage /= NUM_R2_PER_R1; 331 | 332 | // --- From OMPL Syclop ref: https://ompl.kavrakilab.org/classompl_1_1control_1_1Syclop.html--- 333 | float freeVol = (EPSILON + numValidSamples) / (EPSILON + numValidSamples + (counterArray[tid] - numValidSamples)) * W_R1_VOL; 334 | score = pow(freeVol, 4) / ((1 + coverage) * (1 + pow(counterArray[tid], 2))); 335 | } 336 | 337 | // --- Sum scores from each thread to determine score threshold --- 338 | typedef cub::BlockReduce BlockReduceFloatT; 339 | __shared__ typename BlockReduceFloatT::TempStorage tempStorageFloat; 340 | float blockSum = BlockReduceFloatT(tempStorageFloat).Sum(score); 341 | 342 | if(threadIdx.x == 0) 343 | { 344 | s_totalScore = blockSum; 345 | } 346 | __syncthreads(); 347 | 348 | // --- Update vertex scores --- 349 | if(validCounterArray[tid] == 0) 350 | { 351 | vertexScores[tid] = 1.0f; 352 | } 353 | else 354 | { 355 | // TODO: check if adding epsilon is ok. 356 | vertexScores[tid] = EPSILON + (score / s_totalScore); 357 | } 358 | } -------------------------------------------------------------------------------- /viz/KGMT_3D_GPU.asv: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 9; 7 | radius = .05; 8 | N = 8; 9 | n = 4; 10 | sampleSize = 10; 11 | stateSize = 6; 12 | controlSize = 3; 13 | xGoal = [.70, .95, .90]; 14 | alpha = .7; 15 | STEP_SIZE = .1; 16 | model = 2; 17 | 18 | % Obstacle file path 19 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/trees/obstacles.csv'; 20 | obstacles = gpuArray(readmatrix(obstacleFilePath)); 21 | 22 | treeSizePath = "/home/nicolas/dev/research/KPAX/build/Data/TreeSize/TreeSize0/treeSize.csv"; 23 | treeSizes = gpuArray(readmatrix(treeSizePath)); 24 | treeSizes = [0; treeSizes]; 25 | 26 | colors = gpuArray([0 0 1; % Blue 27 | 0 .9 .2; % Green 28 | 1 0 1; % Pink 29 | .7 .7 0; % Yellow 30 | 0 .7 .7; % Turquoise 31 | 1 .5 0]); % Orange 32 | 33 | fig = figure('Position', [100, 100, 1000, 1000]); 34 | hold on; 35 | axis equal; 36 | title('Iteration 0'); 37 | 38 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples1.csv"; 39 | samples = gpuArray(readmatrix(sampleFilePath)); 40 | 41 | controlPath = '/home/nicolas/dev/research/KPAX/build/Data/ControlPathToGoal/ControlPathToGoal0/controlPathToGoal.csv'; 42 | controls = gpuArray(flipud(readmatrix(controlPath))); 43 | % controls = [samples(1,1), samples(1,2), samples(1,3), samples(1,4), samples(1,5), samples(1,6), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; controls]; 44 | 45 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 46 | 47 | [X, Y, Z] = sphere(20); 48 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 49 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 50 | 51 | for j = 1:size(obstacles, 1) 52 | x_min = obstacles(j, 1); 53 | y_min = obstacles(j, 2); 54 | z_min = obstacles(j, 3); 55 | x_max = obstacles(j, 4); 56 | y_max = obstacles(j, 5); 57 | z_max = obstacles(j, 6); 58 | vertices = gpuArray([ 59 | x_min, y_min, z_min; 60 | x_max, y_min, z_min; 61 | x_max, y_max, z_min; 62 | x_min, y_max, z_min; 63 | x_min, y_min, z_max; 64 | x_max, y_min, z_max; 65 | x_max, y_max, z_max; 66 | x_min, y_max, z_max]); 67 | faces = gpuArray([ 68 | 1, 2, 6, 5; 69 | 2, 3, 7, 6; 70 | 3, 4, 8, 7; 71 | 4, 1, 5, 8; 72 | 1, 2, 3, 4; 73 | 5, 6, 7, 8]); 74 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 75 | end 76 | 77 | camlight('headlight'); 78 | camlight('right'); 79 | lighting phong; 80 | 81 | % view(3); 82 | % drawnow; 83 | % saveas(gcf, 'figs/KGMT_Iteration_0.jpg'); 84 | % print('figs/KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 85 | % 86 | % view(2); 87 | % drawnow; 88 | % saveas(gcf, 'figs/top_KGMT_Iteration_0.jpg'); 89 | % print('figs/top_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 90 | % 91 | % midY = 0.5 * xGoal(2); 92 | % midZ = 0.5 * xGoal(3); 93 | % campos([0, midY, xGoal(3) + 1]); 94 | % camtarget([0, midY, midZ]); 95 | % view([-.4, -.2, 0.5]); 96 | % drawnow; 97 | % saveas(gcf, 'figs/xAxis_KGMT_Iteration_0.jpg'); 98 | % print('figs/xAxis_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 99 | 100 | close(gcf); 101 | iteration = 1; 102 | 103 | for i = 1:numFiles 104 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples" + i + ".csv"; 105 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Parents/Parents0/parents" + i + ".csv"; 106 | 107 | samples = gpuArray(readmatrix(sampleFilePath)); 108 | parentRelations = gpuArray(readmatrix(parentFilePath)); 109 | 110 | fig = figure('Position', [100, 100, 1000, 1000]); 111 | hold on; 112 | axis equal; 113 | % title(sprintf('Iteration %d', i)); 114 | 115 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 116 | 117 | [X, Y, Z] = sphere(20); 118 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 119 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 120 | 121 | for j = 1:size(obstacles, 1) 122 | x_min = obstacles(j, 1); 123 | y_min = obstacles(j, 2); 124 | z_min = obstacles(j, 3); 125 | x_max = obstacles(j, 4); 126 | y_max = obstacles(j, 5); 127 | z_max = obstacles(j, 6); 128 | vertices = gpuArray([ 129 | x_min, y_min, z_min; 130 | x_max, y_min, z_min; 131 | x_max, y_max, z_min; 132 | x_min, y_max, z_min; 133 | x_min, y_min, z_max; 134 | x_max, y_min, z_max; 135 | x_max, y_max, z_max; 136 | x_min, y_max, z_max]); 137 | faces = gpuArray([ 138 | 1, 2, 6, 5; 139 | 2, 3, 7, 6; 140 | 3, 4, 8, 7; 141 | 4, 1, 5, 8; 142 | 1, 2, 3, 4; 143 | 5, 6, 7, 8]); 144 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 145 | end 146 | 147 | camlight('headlight'); 148 | camlight('right'); 149 | lighting phong; 150 | 151 | colorIndex = 1; 152 | for j = 2:size(parentRelations, 1) 153 | if j > treeSizes(iteration) 154 | colorIndex = 3; 155 | else 156 | colorIndex = 1; 157 | end 158 | if parentRelations(j) == -1 159 | iteration = iteration + 1; 160 | break; 161 | end 162 | x0 = samples((parentRelations(j) + 1), 1:stateSize); 163 | sample = samples(j, :); 164 | if model == 1 165 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 166 | elseif model == 2 167 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 168 | elseif model == 3 169 | [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 170 | end 171 | plot3(gather(segmentX), gather(segmentY), gather(segmentZ), '-.', 'Color', 'k', 'LineWidth', 0.01); 172 | plot3(gather(samples(j, 1)), gather(samples(j, 2)), gather(samples(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 173 | end 174 | 175 | % Define the filename for the GIF 176 | gifFilename = sprintf('figs/KGMT_Iteration_%d.gif', i); 177 | 178 | 179 | if i == numFiles 180 | for j = 2:size(controls, 1) 181 | x0 = controls(j-1, 1:stateSize); 182 | sample = controls(j,:); 183 | if model == 1 184 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 185 | elseif model == 2 186 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 187 | elseif model == 3 188 | [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 189 | end 190 | 191 | plot3(gather(segmentX), gather(segmentY), gather(segmentZ), 'Color', 'g', 'LineWidth', 1); 192 | plot3(gather(controls(j, 1)), gather(controls(j, 2)), gather(controls(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 193 | end 194 | end 195 | 196 | % Set up the initial view 197 | view(3); 198 | axis vis3d; % Maintain aspect ratio during rotation 199 | 200 | % Rotate and capture frames for 360-degree view 201 | for angle = 0:2:360 % Adjust step size for smoother or faster rotation 202 | view(angle, 30); 203 | % Capture the plot as a frame 204 | frame = getframe(gcf); 205 | im = frame2im(frame); 206 | [imind, cm] = rgb2ind(im, 256); 207 | 208 | % Write to the GIF file 209 | if angle == 0 210 | % Create the GIF file on the first iteration 211 | imwrite(imind, cm, gifFilename, 'gif', 'Loopcount', inf, 'DelayTime', 0.1); 212 | else 213 | % Append to the GIF file on subsequent iterations 214 | imwrite(imind, cm, gifFilename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1); 215 | end 216 | end 217 | 218 | % view(3); 219 | % drawnow; 220 | % saveas(gcf, sprintf('figs/KGMT_Iteration_%d.jpg', i)); 221 | % print(sprintf('figs/KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 222 | % 223 | % view(2); 224 | % drawnow; 225 | % saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 226 | % print(sprintf('figs/top_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 227 | % 228 | % midY = (min(gather(samples(:,2))) + max(gather(samples(:,2)))) / 2; 229 | % midZ = (min(gather(samples(:,3))) + max(gather(samples(:,3)))) / 2; 230 | % campos([0, midY, max(gather(samples(:,3))) + 1]); 231 | % camtarget([0, midY, midZ]); 232 | % view([-.4, -.2, 0.5]); 233 | % drawnow; 234 | % 235 | % saveas(gcf, sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i)); 236 | % print(sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 237 | 238 | close(gcf); 239 | end 240 | 241 | function [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize) 242 | segmentX = gpuArray(x0(1)); 243 | segmentY = gpuArray(x0(2)); 244 | segmentZ = gpuArray(x0(3)); 245 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 246 | duration = gpuArray(sample(sampleSize)); 247 | numDisc = gpuArray(duration / STEP_SIZE); 248 | x = gpuArray(x0(1)); 249 | y = gpuArray(x0(2)); 250 | z = gpuArray(x0(3)); 251 | vx = gpuArray(x0(4)); 252 | vy = gpuArray(x0(5)); 253 | vz = gpuArray(x0(6)); 254 | ax = u(1); 255 | ay = u(2); 256 | az = u(3); 257 | for k = 1:numDisc 258 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 259 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 260 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 261 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 262 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 263 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 264 | segmentX = [segmentX, x]; 265 | segmentY = [segmentY, y]; 266 | segmentZ = [segmentZ, z]; 267 | end 268 | % segmentX = [segmentX, gpuArray(sample(1))]; 269 | % segmentY = [segmentY, gpuArray(sample(2))]; 270 | % segmentZ = [segmentZ, gpuArray(sample(3))]; 271 | end 272 | 273 | function [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize) 274 | segmentX = gpuArray(x0(1)); 275 | segmentY = gpuArray(x0(2)); 276 | segmentZ = gpuArray(x0(3)); 277 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 278 | duration = gpuArray(sample(sampleSize)); 279 | numDisc = gpuArray(duration / STEP_SIZE); 280 | x = gpuArray(x0(1)); 281 | y = gpuArray(x0(2)); 282 | z = gpuArray(x0(3)); 283 | yaw = gpuArray(x0(4)); 284 | pitch = gpuArray(x0(5)); 285 | v = gpuArray(x0(6)); 286 | yawRate = u(1); 287 | pitchRate = u(2); 288 | a = u(3); 289 | for k = 1:numDisc 290 | x = x + (STEP_SIZE / 6.0) * ... 291 | (v * cos(pitch) * cos(yaw) + ... 292 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate) + ... 293 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 294 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * cos(yaw + STEP_SIZE * yawRate)); 295 | 296 | y = y + (STEP_SIZE / 6.0) * ... 297 | (v * cos(pitch) * sin(yaw) + ... 298 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate) + ... 299 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 300 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * sin(yaw + STEP_SIZE * yawRate)); 301 | 302 | z = z + (STEP_SIZE / 6.0) * ... 303 | (v * sin(pitch) + ... 304 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate) + ... 305 | (v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate)) + ... 306 | (v + STEP_SIZE * a) * sin(pitch + STEP_SIZE * pitchRate)); 307 | 308 | yaw = yaw + STEP_SIZE * yawRate; 309 | pitch = pitch + STEP_SIZE * pitchRate; 310 | v = v + (STEP_SIZE / 6.0) * (a + 2.0 * (a + a) + a); 311 | segmentX = [segmentX, x]; 312 | segmentY = [segmentY, y]; 313 | segmentZ = [segmentZ, z]; 314 | end 315 | segmentX = [segmentX, gpuArray(sample(1))]; 316 | segmentY = [segmentY, gpuArray(sample(2))]; 317 | segmentZ = [segmentZ, gpuArray(sample(3))]; 318 | end 319 | 320 | function [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize) 321 | segmentX = gpuArray(x0(1)); 322 | segmentY = gpuArray(x0(2)); 323 | segmentZ = gpuArray(x0(3)); 324 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 325 | duration = gpuArray(sample(sampleSize)); 326 | numDisc = gpuArray(duration / STEP_SIZE); 327 | Zc = u(1); 328 | Lc = u(2); 329 | Mc = u(3); 330 | Nc = u(4); 331 | h0 = x0(1:12); 332 | x = x0(1); 333 | y = x0(2); 334 | z = x0(3); 335 | 336 | for k = 1:numDisc 337 | h1 = ode(h0, Zc, Lc, Mc, Nc); 338 | h2 = ode(h0 + 0.5 * STEP_SIZE * h1, Zc, Lc, Mc, Nc); 339 | h3 = ode(h0 + 0.5 * STEP_SIZE * h2, Zc, Lc, Mc, Nc); 340 | h4 = ode(h0 + STEP_SIZE * h3, Zc, Lc, Mc, Nc); 341 | 342 | h0 = h0 + (STEP_SIZE / 6) * (h1 + 2 * h2 + 2 * h3 + h4); 343 | 344 | x = h0(1); 345 | y = h0(2); 346 | z = h0(3); 347 | 348 | segmentX = [segmentX, x]; 349 | segmentY = [segmentY, y]; 350 | segmentZ = [segmentZ, z]; 351 | end 352 | 353 | segmentX = [segmentX, gpuArray(sample(1))]; 354 | segmentY = [segmentY, gpuArray(sample(2))]; 355 | segmentZ = [segmentZ, gpuArray(sample(3))]; 356 | end 357 | 358 | function x0dot = ode(x0, Zc, Lc, Mc, Nc) 359 | 360 | NU = 10e-3; 361 | MU = 2e-6; 362 | IX = 1.0; 363 | IY = 1.0; 364 | IZ = 2.0; 365 | GRAVITY = -9.81; 366 | MASS = 1.0; 367 | MASS_INV = 1.0 / MASS; 368 | 369 | 370 | phi = x0(4); 371 | theta = x0(5); 372 | psi = x0(6); 373 | u = x0(7); 374 | v = x0(8); 375 | w = x0(9); 376 | p = x0(10); 377 | q = x0(11); 378 | r = x0(12); 379 | 380 | x0dot = zeros(1, 12); 381 | 382 | x0dot(1) = cos(theta) * cos(psi) * u + (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)) * v + ... 383 | (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)) * w; 384 | 385 | x0dot(2) = cos(theta) * sin(psi) * u + (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) * v + ... 386 | (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)) * w; 387 | 388 | x0dot(3) = -sin(theta) * u + sin(phi) * cos(theta) * v + cos(phi) * cos(theta) * w; 389 | 390 | x0dot(4) = p + (q * sin(phi) + r * cos(phi)) * tan(theta); 391 | 392 | x0dot(5) = q * cos(phi) - r * sin(phi); 393 | 394 | x0dot(6) = (q * sin(phi) + r * cos(phi)) / cos(theta); 395 | 396 | XYZ = -NU * sqrt(u^2 + v^2 + w^2); 397 | X = XYZ * u; 398 | x0dot(7) = (r * v - q * w) - GRAVITY * sin(theta) + MASS_INV * X; 399 | 400 | Y = XYZ * v; 401 | x0dot(8) = (p * w - r * u) + GRAVITY * cos(theta) * sin(phi) + MASS_INV * Y; 402 | 403 | Z = XYZ * w; 404 | x0dot(9) = (q * u - p * v) + GRAVITY * cos(theta) * cos(phi) + MASS_INV * Z + MASS_INV * Zc; 405 | 406 | LMN = -MU * sqrt(p^2 + q^2 + r^2); 407 | L = LMN * p; 408 | x0dot(10) = (IY - IZ) / IX * q * r + (1 / IX) * L + (1 / IX) * Lc; 409 | 410 | M = LMN * q; 411 | x0dot(11) = (IZ - IX) / IY * p * r + (1 / IY) * M + (1 / IY) * Mc; 412 | 413 | N = LMN * r; 414 | x0dot(12) = (IX - IY) / IZ * p * q + (1 / IZ) * N + (1 / IZ) * Nc; 415 | end -------------------------------------------------------------------------------- /viz/KGMT_3D_MultiColor.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 9; 7 | radius = .05; 8 | N = 8; 9 | n = 4; 10 | sampleSize = 10; 11 | stateSize = 6; 12 | controlSize = 2; 13 | xGoal = [.70, .95, .90]; 14 | alpha = .7; 15 | STEP_SIZE = .1; 16 | model = 2; 17 | 18 | % Obstacle file path 19 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/pillars/obstacles.csv'; 20 | obstacles = gpuArray(readmatrix(obstacleFilePath)); 21 | 22 | treeSizePath = "/home/nicolas/dev/research/KPAX/build/Data/TreeSize/TreeSize0/treeSize.csv"; 23 | treeSizes = gpuArray(readmatrix(treeSizePath)); 24 | treeSizes = [0; treeSizes]; 25 | 26 | colors = gpuArray([0 0 1; % Blue 27 | 0 .9 .2; % Green 28 | 1 0 1; % Pink 29 | .7 .7 0; % Yellow 30 | 0 .7 .7; % Turquoise 31 | 1 .5 0]); % Orange 32 | 33 | fig = figure('Position', [100, 100, 1000, 1000]); 34 | hold on; 35 | axis equal; 36 | axis off; 37 | title('Iteration 0'); 38 | 39 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples1.csv"; 40 | samples = gpuArray(readmatrix(sampleFilePath)); 41 | 42 | controlPath = '/home/nicolas/dev/research/KPAX/build/Data/ControlPathToGoal/ControlPathToGoal0/controlPathToGoal.csv'; 43 | controls = gpuArray(flipud(readmatrix(controlPath))); 44 | % controls = [samples(1,1), samples(1,2), samples(1,3), samples(1,4), samples(1,5), samples(1,6), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; controls]; 45 | 46 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 47 | 48 | [X, Y, Z] = sphere(20); 49 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 50 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 51 | 52 | for j = 1:size(obstacles, 1) 53 | x_min = obstacles(j, 1); 54 | y_min = obstacles(j, 2); 55 | z_min = obstacles(j, 3); 56 | x_max = obstacles(j, 4); 57 | y_max = obstacles(j, 5); 58 | z_max = obstacles(j, 6); 59 | vertices = gpuArray([ 60 | x_min, y_min, z_min; 61 | x_max, y_min, z_min; 62 | x_max, y_max, z_min; 63 | x_min, y_max, z_min; 64 | x_min, y_min, z_max; 65 | x_max, y_min, z_max; 66 | x_max, y_max, z_max; 67 | x_min, y_max, z_max]); 68 | faces = gpuArray([ 69 | 1, 2, 6, 5; 70 | 2, 3, 7, 6; 71 | 3, 4, 8, 7; 72 | 4, 1, 5, 8; 73 | 1, 2, 3, 4; 74 | 5, 6, 7, 8]); 75 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 76 | end 77 | 78 | camlight('headlight'); 79 | camlight('right'); 80 | lighting phong; 81 | 82 | % view(3); 83 | % drawnow; 84 | % saveas(gcf, 'figs/KGMT_Iteration_0.jpg'); 85 | % print('figs/KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 86 | % 87 | view(2); 88 | drawnow; 89 | saveas(gcf, 'figs/top_KGMT_Iteration_0.jpg'); 90 | print('figs/top_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 91 | % 92 | % midY = 0.5 * xGoal(2); 93 | % midZ = 0.5 * xGoal(3); 94 | % campos([0, midY, xGoal(3) + 1]); 95 | % camtarget([0, midY, midZ]); 96 | % view([-.4, -.2, 0.5]); 97 | % drawnow; 98 | % saveas(gcf, 'figs/xAxis_KGMT_Iteration_0.jpg'); 99 | % print('figs/xAxis_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 100 | 101 | close(gcf); 102 | iteration = 1; 103 | 104 | for i = 1:numFiles 105 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples" + i + ".csv"; 106 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Parents/Parents0/parents" + i + ".csv"; 107 | 108 | samples = gpuArray(readmatrix(sampleFilePath)); 109 | parentRelations = gpuArray(readmatrix(parentFilePath)); 110 | 111 | fig = figure('Position', [100, 100, 1000, 1000]); 112 | hold on; 113 | axis equal; 114 | axis off; 115 | % title(sprintf('Iteration %d', i)); 116 | 117 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 118 | 119 | [X, Y, Z] = sphere(20); 120 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 121 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 122 | 123 | for j = 1:size(obstacles, 1) 124 | x_min = obstacles(j, 1); 125 | y_min = obstacles(j, 2); 126 | z_min = obstacles(j, 3); 127 | x_max = obstacles(j, 4); 128 | y_max = obstacles(j, 5); 129 | z_max = obstacles(j, 6); 130 | vertices = gpuArray([ 131 | x_min, y_min, z_min; 132 | x_max, y_min, z_min; 133 | x_max, y_max, z_min; 134 | x_min, y_max, z_min; 135 | x_min, y_min, z_max; 136 | x_max, y_min, z_max; 137 | x_max, y_max, z_max; 138 | x_min, y_max, z_max]); 139 | faces = gpuArray([ 140 | 1, 2, 6, 5; 141 | 2, 3, 7, 6; 142 | 3, 4, 8, 7; 143 | 4, 1, 5, 8; 144 | 1, 2, 3, 4; 145 | 5, 6, 7, 8]); 146 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 147 | end 148 | 149 | camlight('headlight'); 150 | camlight('right'); 151 | lighting phong; 152 | 153 | colorIndex = 1; 154 | for j = 2:size(parentRelations, 1) 155 | if j > treeSizes(iteration) 156 | colorIndex = 3; 157 | else 158 | colorIndex = 1; 159 | end 160 | if parentRelations(j) == -1 161 | iteration = iteration + 1; 162 | break; 163 | end 164 | x0 = samples((parentRelations(j) + 1), 1:stateSize); 165 | sample = samples(j, :); 166 | if model == 1 167 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 168 | elseif model == 2 169 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 170 | elseif model == 3 171 | [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 172 | end 173 | plot3(gather(segmentX), gather(segmentY), gather(segmentZ), '-.', 'Color', 'k', 'LineWidth', 0.01); 174 | plot3(gather(samples(j, 1)), gather(samples(j, 2)), gather(samples(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 175 | end 176 | 177 | % Define the filename for the GIF 178 | % gifFilename = sprintf('figs/KGMT_Iteration_%d.gif', i); 179 | 180 | 181 | if i == numFiles 182 | for j = 2:size(controls, 1) 183 | x0 = controls(j-1, 1:stateSize); 184 | sample = controls(j,:); 185 | if model == 1 186 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 187 | elseif model == 2 188 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 189 | elseif model == 3 190 | [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 191 | end 192 | 193 | plot3(gather(segmentX), gather(segmentY), gather(segmentZ), 'Color', 'g', 'LineWidth', 1); 194 | plot3(gather(controls(j, 1)), gather(controls(j, 2)), gather(controls(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 195 | end 196 | end 197 | 198 | % Set up the initial view 199 | % view(3); 200 | % axis vis3d; % Maintain aspect ratio during rotation 201 | % 202 | % % Rotate and capture frames for 360-degree view 203 | % for angle = 0:1:360 % Adjust step size for smoother or faster rotation 204 | % view(angle, 30); 205 | % % Capture the plot as a frame 206 | % frame = getframe(gcf); 207 | % im = frame2im(frame); 208 | % [imind, cm] = rgb2ind(im, 256); 209 | % 210 | % % Write to the GIF file 211 | % if angle == 0 212 | % % Create the GIF file on the first iteration 213 | % imwrite(imind, cm, gifFilename, 'gif', 'Loopcount', inf, 'DelayTime', 0.1); 214 | % else 215 | % % Append to the GIF file on subsequent iterations 216 | % imwrite(imind, cm, gifFilename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1); 217 | % end 218 | % end 219 | 220 | % view(3); 221 | % drawnow; 222 | % saveas(gcf, sprintf('figs/KGMT_Iteration_%d.jpg', i)); 223 | % print(sprintf('figs/KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 224 | % 225 | view(2); 226 | drawnow; 227 | saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 228 | print(sprintf('figs/top_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 229 | % 230 | % midY = (min(gather(samples(:,2))) + max(gather(samples(:,2)))) / 2; 231 | % midZ = (min(gather(samples(:,3))) + max(gather(samples(:,3)))) / 2; 232 | % campos([0, midY, max(gather(samples(:,3))) + 1]); 233 | % camtarget([0, midY, midZ]); 234 | % view([-.4, -.2, 0.5]); 235 | % drawnow; 236 | % 237 | % saveas(gcf, sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i)); 238 | % print(sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 239 | 240 | close(gcf); 241 | end 242 | 243 | function [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize) 244 | segmentX = gpuArray(x0(1)); 245 | segmentY = gpuArray(x0(2)); 246 | segmentZ = gpuArray(x0(3)); 247 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 248 | duration = gpuArray(sample(sampleSize)); 249 | numDisc = gpuArray(duration / STEP_SIZE); 250 | x = gpuArray(x0(1)); 251 | y = gpuArray(x0(2)); 252 | z = gpuArray(x0(3)); 253 | vx = gpuArray(x0(4)); 254 | vy = gpuArray(x0(5)); 255 | vz = gpuArray(x0(6)); 256 | ax = u(1); 257 | ay = u(2); 258 | az = u(3); 259 | for k = 1:numDisc 260 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 261 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 262 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 263 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 264 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 265 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 266 | segmentX = [segmentX, x]; 267 | segmentY = [segmentY, y]; 268 | segmentZ = [segmentZ, z]; 269 | end 270 | % segmentX = [segmentX, gpuArray(sample(1))]; 271 | % segmentY = [segmentY, gpuArray(sample(2))]; 272 | % segmentZ = [segmentZ, gpuArray(sample(3))]; 273 | end 274 | 275 | function [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize) 276 | segmentX = gpuArray(x0(1)); 277 | segmentY = gpuArray(x0(2)); 278 | segmentZ = gpuArray(x0(3)); 279 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 280 | duration = gpuArray(sample(sampleSize)); 281 | numDisc = gpuArray(duration / STEP_SIZE); 282 | x = gpuArray(x0(1)); 283 | y = gpuArray(x0(2)); 284 | z = gpuArray(x0(3)); 285 | yaw = gpuArray(x0(4)); 286 | pitch = gpuArray(x0(5)); 287 | v = gpuArray(x0(6)); 288 | yawRate = u(1); 289 | pitchRate = u(2); 290 | a = u(3); 291 | for k = 1:numDisc 292 | x = x + (STEP_SIZE / 6.0) * ... 293 | (v * cos(pitch) * cos(yaw) + ... 294 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate) + ... 295 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 296 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * cos(yaw + STEP_SIZE * yawRate)); 297 | 298 | y = y + (STEP_SIZE / 6.0) * ... 299 | (v * cos(pitch) * sin(yaw) + ... 300 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate) + ... 301 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 302 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * sin(yaw + STEP_SIZE * yawRate)); 303 | 304 | z = z + (STEP_SIZE / 6.0) * ... 305 | (v * sin(pitch) + ... 306 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate) + ... 307 | (v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate)) + ... 308 | (v + STEP_SIZE * a) * sin(pitch + STEP_SIZE * pitchRate)); 309 | 310 | yaw = yaw + STEP_SIZE * yawRate; 311 | pitch = pitch + STEP_SIZE * pitchRate; 312 | v = v + (STEP_SIZE / 6.0) * (a + 2.0 * (a + a) + a); 313 | segmentX = [segmentX, x]; 314 | segmentY = [segmentY, y]; 315 | segmentZ = [segmentZ, z]; 316 | end 317 | segmentX = [segmentX, gpuArray(sample(1))]; 318 | segmentY = [segmentY, gpuArray(sample(2))]; 319 | segmentZ = [segmentZ, gpuArray(sample(3))]; 320 | end 321 | 322 | function [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize) 323 | segmentX = gpuArray(x0(1)); 324 | segmentY = gpuArray(x0(2)); 325 | segmentZ = gpuArray(x0(3)); 326 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 327 | duration = gpuArray(sample(sampleSize)); 328 | numDisc = gpuArray(duration / STEP_SIZE); 329 | Zc = u(1); 330 | Lc = u(2); 331 | Mc = u(3); 332 | Nc = u(4); 333 | h0 = x0(1:12); 334 | x = x0(1); 335 | y = x0(2); 336 | z = x0(3); 337 | 338 | for k = 1:numDisc 339 | h1 = ode(h0, Zc, Lc, Mc, Nc); 340 | h2 = ode(h0 + 0.5 * STEP_SIZE * h1, Zc, Lc, Mc, Nc); 341 | h3 = ode(h0 + 0.5 * STEP_SIZE * h2, Zc, Lc, Mc, Nc); 342 | h4 = ode(h0 + STEP_SIZE * h3, Zc, Lc, Mc, Nc); 343 | 344 | h0 = h0 + (STEP_SIZE / 6) * (h1 + 2 * h2 + 2 * h3 + h4); 345 | 346 | x = h0(1); 347 | y = h0(2); 348 | z = h0(3); 349 | 350 | segmentX = [segmentX, x]; 351 | segmentY = [segmentY, y]; 352 | segmentZ = [segmentZ, z]; 353 | end 354 | 355 | segmentX = [segmentX, gpuArray(sample(1))]; 356 | segmentY = [segmentY, gpuArray(sample(2))]; 357 | segmentZ = [segmentZ, gpuArray(sample(3))]; 358 | end 359 | 360 | function x0dot = ode(x0, Zc, Lc, Mc, Nc) 361 | 362 | NU = 10e-3; 363 | MU = 2e-6; 364 | IX = 1.0; 365 | IY = 1.0; 366 | IZ = 2.0; 367 | GRAVITY = -9.81; 368 | MASS = 1.0; 369 | MASS_INV = 1.0 / MASS; 370 | 371 | 372 | phi = x0(4); 373 | theta = x0(5); 374 | psi = x0(6); 375 | u = x0(7); 376 | v = x0(8); 377 | w = x0(9); 378 | p = x0(10); 379 | q = x0(11); 380 | r = x0(12); 381 | 382 | x0dot = zeros(1, 12); 383 | 384 | x0dot(1) = cos(theta) * cos(psi) * u + (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)) * v + ... 385 | (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)) * w; 386 | 387 | x0dot(2) = cos(theta) * sin(psi) * u + (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) * v + ... 388 | (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)) * w; 389 | 390 | x0dot(3) = -sin(theta) * u + sin(phi) * cos(theta) * v + cos(phi) * cos(theta) * w; 391 | 392 | x0dot(4) = p + (q * sin(phi) + r * cos(phi)) * tan(theta); 393 | 394 | x0dot(5) = q * cos(phi) - r * sin(phi); 395 | 396 | x0dot(6) = (q * sin(phi) + r * cos(phi)) / cos(theta); 397 | 398 | XYZ = -NU * sqrt(u^2 + v^2 + w^2); 399 | X = XYZ * u; 400 | x0dot(7) = (r * v - q * w) - GRAVITY * sin(theta) + MASS_INV * X; 401 | 402 | Y = XYZ * v; 403 | x0dot(8) = (p * w - r * u) + GRAVITY * cos(theta) * sin(phi) + MASS_INV * Y; 404 | 405 | Z = XYZ * w; 406 | x0dot(9) = (q * u - p * v) + GRAVITY * cos(theta) * cos(phi) + MASS_INV * Z + MASS_INV * Zc; 407 | 408 | LMN = -MU * sqrt(p^2 + q^2 + r^2); 409 | L = LMN * p; 410 | x0dot(10) = (IY - IZ) / IX * q * r + (1 / IX) * L + (1 / IX) * Lc; 411 | 412 | M = LMN * q; 413 | x0dot(11) = (IZ - IX) / IY * p * r + (1 / IY) * M + (1 / IY) * Mc; 414 | 415 | N = LMN * r; 416 | x0dot(12) = (IX - IY) / IZ * p * q + (1 / IZ) * N + (1 / IZ) * Nc; 417 | end -------------------------------------------------------------------------------- /src/statePropagator/statePropagator.cu: -------------------------------------------------------------------------------- 1 | 2 | #include "statePropagator/statePropagator.cuh" 3 | 4 | __device__ bool propagateAndCheck(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount) 5 | { 6 | PropagateAndCheckFunc func = getPropagateAndCheckFunc(); 7 | return func ? func(x0, x1, seed, obstacles, obstaclesCount) : false; 8 | } 9 | 10 | /***************************/ 11 | /* UNICYCLE PROPAGATION FUNCTION */ 12 | /***************************/ 13 | __device__ bool propagateAndCheckUnicycle(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount) 14 | { 15 | float a = A_MIN + curand_uniform(seed) * (A_MAX - A_MIN); 16 | float steering = UNI_MIN_STEERING + curand_uniform(seed) * (UNI_MAX_STEERING - UNI_MIN_STEERING); 17 | float duration = UNI_MIN_DT + curand_uniform(seed) * (UNI_MAX_DT - UNI_MIN_DT); 18 | int propagationDuration = 1 + (int)(curand_uniform(seed) * (MAX_PROPAGATION_DURATION)); 19 | 20 | float x = x0[0]; 21 | float y = x0[1]; 22 | float theta = x0[2]; 23 | float v = x0[3]; 24 | 25 | float cosTheta, sinTheta, tanSteering; 26 | float bbMin[W_DIM], bbMax[W_DIM]; 27 | 28 | bool motionValid = true; 29 | for(int i = 0; i < propagationDuration; i++) 30 | { 31 | float x0State[W_DIM] = {x, y}; 32 | cosTheta = cos(theta); 33 | sinTheta = sin(theta); 34 | tanSteering = tan(steering); 35 | 36 | // --- State Propagation --- 37 | x += v * cosTheta * STEP_SIZE; 38 | y += v * sinTheta * STEP_SIZE; 39 | theta += (v / UNI_LENGTH) * tanSteering * STEP_SIZE; 40 | v += a * STEP_SIZE; 41 | float x1State[W_DIM] = {x, y}; 42 | 43 | // --- Workspace Limit Check --- 44 | if(x < 0 || x > W_SIZE || y < 0 || y > W_SIZE) 45 | { 46 | motionValid = false; 47 | break; 48 | } 49 | 50 | // --- Obstacle Collision Check --- 51 | for(int d = 0; d < W_DIM; d++) 52 | { 53 | if(x0State[d] > x1State[d]) 54 | { 55 | bbMin[d] = x1State[d]; 56 | bbMax[d] = x0State[d]; 57 | } 58 | else 59 | { 60 | bbMin[d] = x0State[d]; 61 | bbMax[d] = x1State[d]; 62 | } 63 | } 64 | 65 | motionValid = motionValid && isMotionValid(x0State, x1State, bbMin, bbMax, obstacles, obstaclesCount); 66 | if(!motionValid) break; 67 | } 68 | 69 | x1[0] = x, x1[1] = y, x1[2] = theta, x1[3] = v, x1[4] = a, x1[5] = steering, x1[6] = duration; 70 | return motionValid; 71 | } 72 | 73 | /***************************/ 74 | /* DOUBLE INTEGRATOR PROPAGATION FUNCTION */ 75 | /***************************/ 76 | __device__ bool propagateAndCheckDoubleIntRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount) 77 | { 78 | float ax = A_MIN + curand_uniform(seed) * (A_MAX - A_MIN); 79 | float ay = A_MIN + curand_uniform(seed) * (A_MAX - A_MIN); 80 | float az = A_MIN + curand_uniform(seed) * (A_MAX - A_MIN); 81 | int propagationDuration = 1 + (int)(curand_uniform(seed) * (MAX_PROPAGATION_DURATION)); 82 | 83 | float x = x0[0]; 84 | float y = x0[1]; 85 | float z = x0[2]; 86 | float vx = x0[3]; 87 | float vy = x0[4]; 88 | float vz = x0[5]; 89 | 90 | bool motionValid = true; 91 | float bbMin[W_DIM], bbMax[W_DIM]; 92 | for(int i = 0; i < propagationDuration; i++) 93 | { 94 | float x0State[W_DIM] = {x, y, z}; 95 | 96 | // --- State Propagation. 4th order Runge Kutta --- 97 | x += (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 98 | y += (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 99 | z += (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 100 | vx += (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 101 | vy += (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 102 | vz += (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 103 | 104 | // --- Dyanmics Validity Check --- 105 | if(vx < V_MIN || vx > V_MAX || vy < V_MIN || vy > V_MAX || vz < V_MIN || vz > V_MAX) 106 | { 107 | motionValid = false; 108 | break; 109 | } 110 | 111 | float x1State[W_DIM] = {x, y, z}; 112 | 113 | // --- Workspace Limit Check --- 114 | if(x < 0 || x > W_SIZE || y < 0 || y > W_SIZE || z < 0 || z > W_SIZE) 115 | { 116 | motionValid = false; 117 | break; 118 | } 119 | 120 | // --- Obstacle Collision Check --- 121 | for(int d = 0; d < W_DIM; d++) 122 | { 123 | if(x0State[d] > x1State[d]) 124 | { 125 | bbMin[d] = x1State[d]; 126 | bbMax[d] = x0State[d]; 127 | } 128 | else 129 | { 130 | bbMin[d] = x0State[d]; 131 | bbMax[d] = x1State[d]; 132 | } 133 | } 134 | 135 | motionValid = motionValid && isMotionValid(x0State, x1State, bbMin, bbMax, obstacles, obstaclesCount); 136 | if(!motionValid) break; 137 | } 138 | 139 | x1[0] = x, x1[1] = y, x1[2] = z, x1[3] = vx, x1[4] = vy, x1[5] = vz, x1[6] = ax, x1[7] = ay, x1[8] = az, 140 | x1[9] = STEP_SIZE * propagationDuration; 141 | return motionValid; 142 | } 143 | 144 | /***************************/ 145 | /* DUBINS AIRPLANE PROPAGATION FUNCTION */ 146 | /***************************/ 147 | __device__ bool propagateAndCheckDubinsAirplaneRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount) 148 | { 149 | float a = A_MIN + curand_uniform(seed) * (A_MAX - A_MIN); 150 | float yawRate = DUBINS_AIRPLANE_MIN_YR + curand_uniform(seed) * (DUBINS_AIRPLANE_MAX_YR - DUBINS_AIRPLANE_MIN_YR); 151 | float pitchRate = DUBINS_AIRPLANE_MIN_PR + curand_uniform(seed) * (DUBINS_AIRPLANE_MAX_PR - DUBINS_AIRPLANE_MIN_PR); 152 | int propagationDuration = 1 + (int)(curand_uniform(seed) * (MAX_PROPAGATION_DURATION)); 153 | 154 | float x = x0[0]; 155 | float y = x0[1]; 156 | float z = x0[2]; 157 | float yaw = x0[3]; 158 | float pitch = x0[4]; 159 | float v = x0[5]; 160 | 161 | bool motionValid = true; 162 | float bbMin[W_DIM], bbMax[W_DIM]; 163 | 164 | for(int i = 0; i < propagationDuration; i++) 165 | { 166 | float x0State[W_DIM] = {x, y, z}; 167 | 168 | // --- State Propagation using 4th Order Runge-Kutta Method --- 169 | x += 170 | (STEP_SIZE / 6.0f) * 171 | (v * cosf(pitch) * cosf(yaw) + 172 | 2.0f * ((v + 0.5f * STEP_SIZE * a) * cosf(pitch + 0.5f * STEP_SIZE * pitchRate) * cosf(yaw + 0.5f * STEP_SIZE * yawRate) + 173 | (v + 0.5f * STEP_SIZE * a) * cosf(pitch + 0.5f * STEP_SIZE * pitchRate) * cosf(yaw + 0.5f * STEP_SIZE * yawRate)) + 174 | (v + STEP_SIZE * a) * cosf(pitch + STEP_SIZE * pitchRate) * cosf(yaw + STEP_SIZE * yawRate)); 175 | y += 176 | (STEP_SIZE / 6.0f) * 177 | (v * cosf(pitch) * sinf(yaw) + 178 | 2.0f * ((v + 0.5f * STEP_SIZE * a) * cosf(pitch + 0.5f * STEP_SIZE * pitchRate) * sinf(yaw + 0.5f * STEP_SIZE * yawRate) + 179 | (v + 0.5f * STEP_SIZE * a) * cosf(pitch + 0.5f * STEP_SIZE * pitchRate) * sinf(yaw + 0.5f * STEP_SIZE * yawRate)) + 180 | (v + STEP_SIZE * a) * cosf(pitch + STEP_SIZE * pitchRate) * sinf(yaw + STEP_SIZE * yawRate)); 181 | z += (STEP_SIZE / 6.0f) * (v * sinf(pitch) + 182 | 2.0f * ((v + 0.5f * STEP_SIZE * a) * sinf(pitch + 0.5f * STEP_SIZE * pitchRate) + 183 | (v + 0.5f * STEP_SIZE * a) * sinf(pitch + 0.5f * STEP_SIZE * pitchRate)) + 184 | (v + STEP_SIZE * a) * sinf(pitch + STEP_SIZE * pitchRate)); 185 | yaw += STEP_SIZE * yawRate; 186 | pitch += STEP_SIZE * pitchRate; 187 | v += (STEP_SIZE / 6.0f) * (a + 2.0f * (a + a) + a); 188 | 189 | // --- Dynamics Validity Check ---' 190 | if(v < V_MIN || v > V_MAX) 191 | { 192 | motionValid = false; 193 | break; 194 | } 195 | else if(pitch < DUBINS_AIRPLANE_MIN_PITCH || pitch > DUBINS_AIRPLANE_MAX_PITCH) 196 | { 197 | motionValid = false; 198 | break; 199 | } 200 | 201 | float x1State[W_DIM] = {x, y, z}; 202 | 203 | // --- Workspace Limit Check --- 204 | if(x < 0 || x > W_SIZE || y < 0 || y > W_SIZE || z < 0 || z > W_SIZE) 205 | { 206 | motionValid = false; 207 | break; 208 | } 209 | 210 | // --- Obstacle Collision Check --- 211 | for(int d = 0; d < W_DIM; d++) 212 | { 213 | if(x0State[d] > x1State[d]) 214 | { 215 | bbMin[d] = x1State[d]; 216 | bbMax[d] = x0State[d]; 217 | } 218 | else 219 | { 220 | bbMin[d] = x0State[d]; 221 | bbMax[d] = x1State[d]; 222 | } 223 | } 224 | 225 | motionValid = motionValid && isMotionValid(x0State, x1State, bbMin, bbMax, obstacles, obstaclesCount); 226 | if(!motionValid) break; 227 | } 228 | 229 | x1[0] = x; 230 | x1[1] = y; 231 | x1[2] = z; 232 | x1[3] = yaw; 233 | x1[4] = pitch; 234 | x1[5] = v; 235 | x1[6] = yawRate; 236 | x1[7] = pitchRate; 237 | x1[8] = a; 238 | x1[9] = STEP_SIZE * propagationDuration; 239 | 240 | return motionValid; 241 | } 242 | 243 | /***************************/ 244 | /* QUAD PROPAGATION FUNCTION */ 245 | /***************************/ 246 | __device__ bool propagateAndCheckQuadRungeKutta(float* x0, float* x1, curandState* seed, float* obstacles, int obstaclesCount) 247 | { 248 | float Zc = QUAD_MIN_Zc + curand_uniform(seed) * (QUAD_MAX_Zc - QUAD_MIN_Zc); 249 | float Lc = QUAD_MIN_Lc + curand_uniform(seed) * (QUAD_MAX_Lc - QUAD_MIN_Lc); 250 | float Mc = QUAD_MIN_Mc + curand_uniform(seed) * (QUAD_MAX_Mc - QUAD_MIN_Mc); 251 | float Nc = QUAD_MIN_Nc + curand_uniform(seed) * (QUAD_MAX_Nc - QUAD_MIN_Nc); 252 | 253 | int propagationDuration = 1 + (int)(curand_uniform(seed) * (MAX_PROPAGATION_DURATION)); 254 | 255 | bool motionValid = true; 256 | float bbMin[W_DIM], bbMax[W_DIM]; 257 | 258 | float h0[STATE_DIM]; 259 | float h1[STATE_DIM]; 260 | float h2[STATE_DIM]; 261 | float h3[STATE_DIM]; 262 | float h4[STATE_DIM]; 263 | 264 | for(int j = 0; j < STATE_DIM; j++) h0[j] = x0[j]; 265 | 266 | for(int i = 0; i < propagationDuration; i++) 267 | { 268 | float x0State[W_DIM] = {h0[0], h0[1], h0[2]}; 269 | 270 | ode(h1, h0, nullptr, Zc, Lc, Mc, Nc, 0); 271 | ode(h2, h0, h1, Zc, Lc, Mc, Nc, 1); 272 | ode(h3, h0, h2, Zc, Lc, Mc, Nc, 2); 273 | ode(h4, h0, h3, Zc, Lc, Mc, Nc, 3); 274 | for(int j = 0; j < STATE_DIM; j++) 275 | { 276 | h0[j] += STEP_SIZE / 6.0f * (h1[j] + 2.0f * h2[j] + 2.0f * h3[j] + h4[j]); 277 | } 278 | 279 | float x1State[W_DIM] = {h0[0], h0[1], h0[2]}; 280 | 281 | // --- Vehicle Dynamics Check --- 282 | if(h0[6] < V_MIN || h0[6] > V_MAX || h0[7] < V_MIN || h0[7] > V_MAX || h0[8] < V_MIN || h0[8] > V_MAX) 283 | { 284 | motionValid = false; 285 | break; 286 | } 287 | 288 | // --- Workspace Limit Check --- 289 | if(h0[0] < W_MIN || h0[0] > W_MAX || h0[1] < W_MIN || h0[1] > W_MAX || h0[2] < W_MIN || h0[2] > W_MAX) 290 | { 291 | motionValid = false; 292 | break; 293 | } 294 | 295 | // --- Obstacle Collision Check --- 296 | for(int d = 0; d < W_DIM; d++) 297 | { 298 | if(x0State[d] > x1State[d]) 299 | { 300 | bbMin[d] = x1State[d]; 301 | bbMax[d] = x0State[d]; 302 | } 303 | else 304 | { 305 | bbMin[d] = x0State[d]; 306 | bbMax[d] = x1State[d]; 307 | } 308 | } 309 | 310 | motionValid = motionValid && isMotionValid(x0State, x1State, bbMin, bbMax, obstacles, obstaclesCount); 311 | if(!motionValid) break; 312 | } 313 | 314 | for(int j = 0; j < STATE_DIM; j++) x1[j] = h0[j]; 315 | 316 | x1[12] = Zc; 317 | x1[13] = Lc; 318 | x1[14] = Mc; 319 | x1[15] = Nc; 320 | x1[16] = STEP_SIZE * propagationDuration; 321 | 322 | return motionValid; 323 | } 324 | 325 | __device__ void ode(float* x0dot, float* x0, float* h, float Zc, float Lc, float Mc, float Nc, int itr) 326 | { 327 | float phi, theta, psi, u, v, w, p, q, r, delta; 328 | 329 | if(itr == 0) 330 | { 331 | delta = 1; 332 | } 333 | else if(itr == 1 || itr == 2) 334 | { 335 | delta = 0.5f * STEP_SIZE; 336 | } 337 | else if(itr == 3) 338 | { 339 | delta = STEP_SIZE; 340 | } 341 | 342 | phi = x0[3] + delta * (h ? h[3] : 0); 343 | theta = x0[4] + delta * (h ? h[4] : 0); 344 | psi = x0[5] + delta * (h ? h[5] : 0); 345 | u = x0[6] + delta * (h ? h[6] : 0); 346 | v = x0[7] + delta * (h ? h[7] : 0); 347 | w = x0[8] + delta * (h ? h[8] : 0); 348 | p = x0[9] + delta * (h ? h[9] : 0); 349 | q = x0[10] + delta * (h ? h[10] : 0); 350 | r = x0[11] + delta * (h ? h[11] : 0); 351 | 352 | x0dot[0] = cos(theta) * cos(psi) * u + (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)) * v + 353 | (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)) * w; 354 | x0dot[1] = cos(theta) * sin(psi) * u + (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) * v + 355 | (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)) * w; 356 | x0dot[2] = -sin(theta) * u + sin(phi) * cos(theta) * v + cos(phi) * cos(theta) * w; 357 | x0dot[3] = p + (q * sin(phi) + r * cos(phi)) * tan(theta); 358 | x0dot[4] = q * cos(phi) - r * sin(phi); 359 | x0dot[5] = (q * sin(phi) + r * cos(phi)) / cos(theta); 360 | 361 | float XYZ = -NU * sqrt(u * u + v * v + w * w); 362 | x0dot[6] = (r * v - q * w) - GRAVITY * sin(theta) + MASS_INV * XYZ * u; 363 | x0dot[7] = (p * w - r * u) + GRAVITY * cos(theta) * sin(phi) + MASS_INV * XYZ * v; 364 | x0dot[8] = (q * u - p * v) + GRAVITY * cos(theta) * cos(phi) + MASS_INV * XYZ * w + MASS_INV * Zc; 365 | 366 | float LMN = -MU * sqrt(p * p + q * q + r * r); 367 | x0dot[9] = (IY - IZ) / IX * q * r + (1 / IX) * LMN * p + (1 / IX) * Lc; 368 | x0dot[10] = (IZ - IX) / IY * p * r + (1 / IY) * LMN * q + (1 / IY) * Mc; 369 | x0dot[11] = (IX - IY) / IZ * p * q + (1 / IZ) * LMN * r + (1 / IZ) * Nc; 370 | } 371 | 372 | /***************************/ 373 | /* GET PROPAGATION FUNCTION */ 374 | /***************************/ 375 | __device__ PropagateAndCheckFunc getPropagateAndCheckFunc() 376 | { 377 | switch(MODEL) 378 | { 379 | case 0: 380 | return propagateAndCheckUnicycle; 381 | case 1: 382 | return propagateAndCheckDoubleIntRungeKutta; 383 | case 2: 384 | return propagateAndCheckDubinsAirplaneRungeKutta; 385 | case 3: 386 | return propagateAndCheckQuadRungeKutta; 387 | default: 388 | return nullptr; 389 | } 390 | } -------------------------------------------------------------------------------- /viz/KGMT_3D_GPU.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | clear all 4 | 5 | % Parameters 6 | numFiles = 1; 7 | radius = .05; 8 | N = 8; 9 | n = 4; 10 | sampleSize = 10; 11 | stateSize = 6; 12 | controlSize = 3; 13 | xGoal = [.80, .95, .90]; 14 | alpha = .7; 15 | STEP_SIZE = .2; 16 | model = 3; 17 | 18 | % Obstacle file path 19 | obstacleFilePath = '/home/nicolas/dev/research/KPAX/include/config/obstacles/house/obstacles.csv'; 20 | obstacles = gpuArray(readmatrix(obstacleFilePath)); 21 | 22 | treeSizePath = "/home/nicolas/dev/research/KPAX/build/Data/TreeSize/TreeSize0/treeSize.csv"; 23 | treeSizes = gpuArray(readmatrix(treeSizePath)); 24 | treeSizes = [0; treeSizes]; 25 | 26 | colors = gpuArray([0 0 1; % Blue 27 | 0 .9 .2; % Green 28 | 1 0 1; % Pink 29 | .7 .7 0; % Yellow 30 | 0 .7 .7; % Turquoise 31 | 1 .5 0]); % Orange 32 | 33 | fig = figure('Position', [100, 100, 1000, 1000]); 34 | hold on; 35 | axis equal; 36 | title('Iteration 0'); 37 | 38 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples1.csv"; 39 | samples = gpuArray(readmatrix(sampleFilePath)); 40 | 41 | controlPath = '/home/nicolas/dev/research/KPAX/build/Data/ControlPathToGoal/ControlPathToGoal0/controlPathToGoal.csv'; 42 | controls = gpuArray(flipud(readmatrix(controlPath))); 43 | controls = [samples(1,1), samples(1,2), samples(1,3), samples(1,4), samples(1,5), samples(1,6), 0, 0, 0, 0; controls]; 44 | 45 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 46 | 47 | [X, Y, Z] = sphere(20); 48 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 49 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 50 | 51 | for j = 1:size(obstacles, 1) 52 | x_min = obstacles(j, 1); 53 | y_min = obstacles(j, 2); 54 | z_min = obstacles(j, 3); 55 | x_max = obstacles(j, 4); 56 | y_max = obstacles(j, 5); 57 | z_max = obstacles(j, 6); 58 | vertices = gpuArray([ 59 | x_min, y_min, z_min; 60 | x_max, y_min, z_min; 61 | x_max, y_max, z_min; 62 | x_min, y_max, z_min; 63 | x_min, y_min, z_max; 64 | x_max, y_min, z_max; 65 | x_max, y_max, z_max; 66 | x_min, y_max, z_max]); 67 | faces = gpuArray([ 68 | 1, 2, 6, 5; 69 | 2, 3, 7, 6; 70 | 3, 4, 8, 7; 71 | 4, 1, 5, 8; 72 | 1, 2, 3, 4; 73 | 5, 6, 7, 8]); 74 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 75 | end 76 | 77 | camlight('headlight'); 78 | camlight('right'); 79 | lighting phong; 80 | 81 | % view(3); 82 | % drawnow; 83 | % saveas(gcf, 'figs/KGMT_Iteration_0.jpg'); 84 | % print('figs/KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 85 | % 86 | % view(2); 87 | % drawnow; 88 | % saveas(gcf, 'figs/top_KGMT_Iteration_0.jpg'); 89 | % print('figs/top_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 90 | % 91 | % midY = 0.5 * xGoal(2); 92 | % midZ = 0.5 * xGoal(3); 93 | % campos([0, midY, xGoal(3) + 1]); 94 | % camtarget([0, midY, midZ]); 95 | % view([-.4, -.2, 0.5]); 96 | % drawnow; 97 | % saveas(gcf, 'figs/xAxis_KGMT_Iteration_0.jpg'); 98 | % print('figs/xAxis_KGMT_Iteration_0.jpg', '-djpeg', '-r300'); 99 | 100 | close(gcf); 101 | iteration = 1; 102 | 103 | for i = 1:numFiles 104 | sampleFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Samples/Samples0/samples" + i + ".csv"; 105 | parentFilePath = "/home/nicolas/dev/research/KPAX/build/Data/Parents/Parents0/parents" + i + ".csv"; 106 | 107 | samples = gpuArray(readmatrix(sampleFilePath)); 108 | parentRelations = gpuArray(readmatrix(parentFilePath)); 109 | 110 | fig = figure('Position', [100, 100, 1000, 1000]); 111 | hold on; 112 | axis equal; 113 | axis off; 114 | % title(sprintf('Iteration %d', i)); 115 | 116 | plot3(gather(samples(1,1)), gather(samples(1,2)), gather(samples(1,3)), 'ko', 'MarkerFaceColor', 'b', 'MarkerSize', 10); 117 | 118 | cubeVertices = [ 119 | 0, 0, 0; 120 | 1, 0, 0; 121 | 1, 1, 0; 122 | 0, 1, 0; 123 | 0, 0, 1; 124 | 1, 0, 1; 125 | 1, 1, 1; 126 | 0, 1, 1 127 | ]; 128 | 129 | cubeEdges = [ 130 | 1, 2; 131 | 2, 3; 132 | 3, 4; 133 | 4, 1; 134 | 5, 6; 135 | 6, 7; 136 | 7, 8; 137 | 8, 5; 138 | 1, 5; 139 | 2, 6; 140 | 3, 7; 141 | 4, 8 142 | ]; 143 | 144 | for k = 1:size(cubeEdges, 1) 145 | plot3([cubeVertices(cubeEdges(k, 1), 1), cubeVertices(cubeEdges(k, 2), 1)], ... 146 | [cubeVertices(cubeEdges(k, 1), 2), cubeVertices(cubeEdges(k, 2), 2)], ... 147 | [cubeVertices(cubeEdges(k, 1), 3), cubeVertices(cubeEdges(k, 2), 3)], ... 148 | 'k-', 'LineWidth', .05); 149 | end 150 | 151 | [X, Y, Z] = sphere(20); 152 | surf(radius * gather(X) + xGoal(1), radius * gather(Y) + xGoal(2), radius * gather(Z) + xGoal(3), ... 153 | 'FaceColor', 'g', 'FaceAlpha', 0.5, 'EdgeColor', 'none'); 154 | 155 | for j = 1:size(obstacles, 1) 156 | x_min = obstacles(j, 1); 157 | y_min = obstacles(j, 2); 158 | z_min = obstacles(j, 3); 159 | x_max = obstacles(j, 4); 160 | y_max = obstacles(j, 5); 161 | z_max = obstacles(j, 6); 162 | vertices = gpuArray([ 163 | x_min, y_min, z_min; 164 | x_max, y_min, z_min; 165 | x_max, y_max, z_min; 166 | x_min, y_max, z_min; 167 | x_min, y_min, z_max; 168 | x_max, y_min, z_max; 169 | x_max, y_max, z_max; 170 | x_min, y_max, z_max]); 171 | faces = gpuArray([ 172 | 1, 2, 6, 5; 173 | 2, 3, 7, 6; 174 | 3, 4, 8, 7; 175 | 4, 1, 5, 8; 176 | 1, 2, 3, 4; 177 | 5, 6, 7, 8]); 178 | patch('Vertices', gather(vertices), 'Faces', gather(faces), 'FaceColor', 'r', 'EdgeColor', 'k', 'FaceAlpha', alpha); 179 | end 180 | 181 | camlight('headlight'); 182 | camlight('right'); 183 | lighting phong; 184 | 185 | colorIndex = 1; 186 | % for j = 2:size(parentRelations, 1) 187 | % if j > treeSizes(iteration) 188 | % colorIndex = 3; 189 | % else 190 | % colorIndex = 1; 191 | % end 192 | % if parentRelations(j) == -1 193 | % iteration = iteration + 1; 194 | % break; 195 | % end 196 | % x0 = samples((parentRelations(j) + 1), 1:stateSize); 197 | % sample = samples(j, :); 198 | % if model == 1 199 | % [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 200 | % elseif model == 2 201 | % [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 202 | % elseif model == 3 203 | % [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 204 | % end 205 | % plot3(gather(segmentX), gather(segmentY), gather(segmentZ), '-.', 'Color', 'k', 'LineWidth', 0.01); 206 | % plot3(gather(samples(j, 1)), gather(samples(j, 2)), gather(samples(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 207 | % end 208 | 209 | % Define the filename for the GIF 210 | gifFilename = sprintf('figs/KGMT_Iteration_%d.gif', i); 211 | 212 | 213 | if i == numFiles 214 | for j = 2:size(controls, 1) 215 | x0 = controls(j-1, 1:stateSize); 216 | sample = controls(j,:); 217 | if model == 1 218 | [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize); 219 | elseif model == 2 220 | [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize); 221 | elseif model == 3 222 | [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize); 223 | end 224 | 225 | plot3(gather(segmentX), gather(segmentY), gather(segmentZ), 'Color', 'g', 'LineWidth', 1); 226 | % plot3(gather(controls(j, 1)), gather(controls(j, 2)), gather(controls(j, 3)), 'o', 'Color', gather(colors(colorIndex, :)), 'MarkerFaceColor', gather(colors(colorIndex, :)), 'MarkerSize', 2); 227 | end 228 | end 229 | 230 | % Set up the initial view 231 | % view(3); 232 | % axis vis3d; % Maintain aspect ratio during rotation 233 | 234 | % Rotate and capture frames for 360-degree view 235 | % for angle = 0:1:360 % Adjust step size for smoother or faster rotation 236 | % view(angle, 30); 237 | % % Capture the plot as a frame 238 | % frame = getframe(gcf); 239 | % im = frame2im(frame); 240 | % [imind, cm] = rgb2ind(im, 256); 241 | % 242 | % % Write to the GIF file 243 | % if angle == 0 244 | % % Create the GIF file on the first iteration 245 | % imwrite(imind, cm, gifFilename, 'gif', 'Loopcount', inf, 'DelayTime', 0.1); 246 | % else 247 | % % Append to the GIF file on subsequent iterations 248 | % imwrite(imind, cm, gifFilename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1); 249 | % end 250 | % end 251 | 252 | view(3); 253 | drawnow; 254 | saveas(gcf, sprintf('figs/KGMT_Iteration_%d.jpg', i)); 255 | print(sprintf('figs/KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 256 | 257 | view(2); 258 | drawnow; 259 | saveas(gcf, sprintf('figs/top_KGMT_Iteration_%d.jpg', i)); 260 | print(sprintf('figs/top_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 261 | 262 | midY = (min(gather(samples(:,2))) + max(gather(samples(:,2)))) / 2; 263 | midZ = (min(gather(samples(:,3))) + max(gather(samples(:,3)))) / 2; 264 | campos([0, midY, max(gather(samples(:,3))) + 1]); 265 | camtarget([0, midY, midZ]); 266 | view([-.4, -.2, 0.5]); 267 | drawnow; 268 | 269 | saveas(gcf, sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i)); 270 | print(sprintf('figs/xAxis_KGMT_Iteration_%d.jpg', i), '-djpeg', '-r300'); 271 | 272 | % close(gcf); 273 | end 274 | 275 | function [segmentX, segmentY, segmentZ] = propDoubleIntegrator(x0, sample, STEP_SIZE, stateSize, sampleSize) 276 | segmentX = gpuArray(x0(1)); 277 | segmentY = gpuArray(x0(2)); 278 | segmentZ = gpuArray(x0(3)); 279 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 280 | duration = gpuArray(sample(sampleSize)); 281 | numDisc = gpuArray(duration / STEP_SIZE); 282 | x = gpuArray(x0(1)); 283 | y = gpuArray(x0(2)); 284 | z = gpuArray(x0(3)); 285 | vx = gpuArray(x0(4)); 286 | vy = gpuArray(x0(5)); 287 | vz = gpuArray(x0(6)); 288 | ax = u(1); 289 | ay = u(2); 290 | az = u(3); 291 | for k = 1:numDisc 292 | x = x + (vx + (vx + 2 * (vx + ax * STEP_SIZE / 2) + (vx + ax * STEP_SIZE))) * STEP_SIZE / 6; 293 | y = y + (vy + (vy + 2 * (vy + ay * STEP_SIZE / 2) + (vy + ay * STEP_SIZE))) * STEP_SIZE / 6; 294 | z = z + (vz + (vz + 2 * (vz + az * STEP_SIZE / 2) + (vz + az * STEP_SIZE))) * STEP_SIZE / 6; 295 | vx = vx + (ax + 2 * ax + 2 * ax + ax) * STEP_SIZE / 6; 296 | vy = vy + (ay + 2 * ay + 2 * ay + ay) * STEP_SIZE / 6; 297 | vz = vz + (az + 2 * az + 2 * az + az) * STEP_SIZE / 6; 298 | segmentX = [segmentX, x]; 299 | segmentY = [segmentY, y]; 300 | segmentZ = [segmentZ, z]; 301 | end 302 | segmentX = [segmentX, gpuArray(sample(1))]; 303 | segmentY = [segmentY, gpuArray(sample(2))]; 304 | segmentZ = [segmentZ, gpuArray(sample(3))]; 305 | end 306 | 307 | function [segmentX, segmentY, segmentZ] = propDubinsAirplane(x0, sample, STEP_SIZE, stateSize, sampleSize) 308 | segmentX = gpuArray(x0(1)); 309 | segmentY = gpuArray(x0(2)); 310 | segmentZ = gpuArray(x0(3)); 311 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 312 | duration = gpuArray(sample(sampleSize)); 313 | numDisc = gpuArray(duration / STEP_SIZE); 314 | x = gpuArray(x0(1)); 315 | y = gpuArray(x0(2)); 316 | z = gpuArray(x0(3)); 317 | yaw = gpuArray(x0(4)); 318 | pitch = gpuArray(x0(5)); 319 | v = gpuArray(x0(6)); 320 | yawRate = u(1); 321 | pitchRate = u(2); 322 | a = u(3); 323 | for k = 1:numDisc 324 | x = x + (STEP_SIZE / 6.0) * ... 325 | (v * cos(pitch) * cos(yaw) + ... 326 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate) + ... 327 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * cos(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 328 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * cos(yaw + STEP_SIZE * yawRate)); 329 | 330 | y = y + (STEP_SIZE / 6.0) * ... 331 | (v * cos(pitch) * sin(yaw) + ... 332 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate) + ... 333 | (v + 0.5 * STEP_SIZE * a) * cos(pitch + 0.5 * STEP_SIZE * pitchRate) * sin(yaw + 0.5 * STEP_SIZE * yawRate)) + ... 334 | (v + STEP_SIZE * a) * cos(pitch + STEP_SIZE * pitchRate) * sin(yaw + STEP_SIZE * yawRate)); 335 | 336 | z = z + (STEP_SIZE / 6.0) * ... 337 | (v * sin(pitch) + ... 338 | 2.0 * ((v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate) + ... 339 | (v + 0.5 * STEP_SIZE * a) * sin(pitch + 0.5 * STEP_SIZE * pitchRate)) + ... 340 | (v + STEP_SIZE * a) * sin(pitch + STEP_SIZE * pitchRate)); 341 | 342 | yaw = yaw + STEP_SIZE * yawRate; 343 | pitch = pitch + STEP_SIZE * pitchRate; 344 | v = v + (STEP_SIZE / 6.0) * (a + 2.0 * (a + a) + a); 345 | segmentX = [segmentX, x]; 346 | segmentY = [segmentY, y]; 347 | segmentZ = [segmentZ, z]; 348 | end 349 | segmentX = [segmentX, gpuArray(sample(1))]; 350 | segmentY = [segmentY, gpuArray(sample(2))]; 351 | segmentZ = [segmentZ, gpuArray(sample(3))]; 352 | end 353 | 354 | function [segmentX, segmentY, segmentZ] = propQuad(x0, sample, STEP_SIZE, stateSize, sampleSize) 355 | segmentX = gpuArray(x0(1)); 356 | segmentY = gpuArray(x0(2)); 357 | segmentZ = gpuArray(x0(3)); 358 | u = gpuArray(sample(stateSize+1:sampleSize-1)); 359 | duration = gpuArray(sample(sampleSize)); 360 | numDisc = gpuArray(duration / STEP_SIZE); 361 | Zc = u(1); 362 | Lc = u(2); 363 | Mc = u(3); 364 | Nc = u(4); 365 | h0 = x0(1:12); 366 | x = x0(1); 367 | y = x0(2); 368 | z = x0(3); 369 | 370 | for k = 1:numDisc 371 | h1 = ode(h0, Zc, Lc, Mc, Nc); 372 | h2 = ode(h0 + 0.5 * STEP_SIZE * h1, Zc, Lc, Mc, Nc); 373 | h3 = ode(h0 + 0.5 * STEP_SIZE * h2, Zc, Lc, Mc, Nc); 374 | h4 = ode(h0 + STEP_SIZE * h3, Zc, Lc, Mc, Nc); 375 | 376 | h0 = h0 + (STEP_SIZE / 6) * (h1 + 2 * h2 + 2 * h3 + h4); 377 | 378 | x = h0(1); 379 | y = h0(2); 380 | z = h0(3); 381 | 382 | segmentX = [segmentX, x]; 383 | segmentY = [segmentY, y]; 384 | segmentZ = [segmentZ, z]; 385 | end 386 | 387 | segmentX = [segmentX, gpuArray(sample(1))]; 388 | segmentY = [segmentY, gpuArray(sample(2))]; 389 | segmentZ = [segmentZ, gpuArray(sample(3))]; 390 | end 391 | 392 | function x0dot = ode(x0, Zc, Lc, Mc, Nc) 393 | 394 | NU = 10e-3; 395 | MU = 2e-6; 396 | IX = 1.0; 397 | IY = 1.0; 398 | IZ = 2.0; 399 | GRAVITY = -9.81; 400 | MASS = 1.0; 401 | MASS_INV = 1.0 / MASS; 402 | 403 | 404 | phi = x0(4); 405 | theta = x0(5); 406 | psi = x0(6); 407 | u = x0(7); 408 | v = x0(8); 409 | w = x0(9); 410 | p = x0(10); 411 | q = x0(11); 412 | r = x0(12); 413 | 414 | x0dot = zeros(1, 12); 415 | 416 | x0dot(1) = cos(theta) * cos(psi) * u + (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)) * v + ... 417 | (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)) * w; 418 | 419 | x0dot(2) = cos(theta) * sin(psi) * u + (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) * v + ... 420 | (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)) * w; 421 | 422 | x0dot(3) = -sin(theta) * u + sin(phi) * cos(theta) * v + cos(phi) * cos(theta) * w; 423 | 424 | x0dot(4) = p + (q * sin(phi) + r * cos(phi)) * tan(theta); 425 | 426 | x0dot(5) = q * cos(phi) - r * sin(phi); 427 | 428 | x0dot(6) = (q * sin(phi) + r * cos(phi)) / cos(theta); 429 | 430 | XYZ = -NU * sqrt(u^2 + v^2 + w^2); 431 | X = XYZ * u; 432 | x0dot(7) = (r * v - q * w) - GRAVITY * sin(theta) + MASS_INV * X; 433 | 434 | Y = XYZ * v; 435 | x0dot(8) = (p * w - r * u) + GRAVITY * cos(theta) * sin(phi) + MASS_INV * Y; 436 | 437 | Z = XYZ * w; 438 | x0dot(9) = (q * u - p * v) + GRAVITY * cos(theta) * cos(phi) + MASS_INV * Z + MASS_INV * Zc; 439 | 440 | LMN = -MU * sqrt(p^2 + q^2 + r^2); 441 | L = LMN * p; 442 | x0dot(10) = (IY - IZ) / IX * q * r + (1 / IX) * L + (1 / IX) * Lc; 443 | 444 | M = LMN * q; 445 | x0dot(11) = (IZ - IX) / IY * p * r + (1 / IY) * M + (1 / IY) * Mc; 446 | 447 | N = LMN * r; 448 | x0dot(12) = (IX - IY) / IZ * p * q + (1 / IZ) * N + (1 / IZ) * Nc; 449 | end --------------------------------------------------------------------------------