├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── __init__.py ├── generateGRiD.py ├── imgs ├── GRiD.png └── benchmark_multi_fd_grad.png ├── printGRiD.cu ├── printGRiD.py ├── printReferenceValues.py ├── requirements.txt ├── testGRiDRefactorings.py └── util ├── __init__.py └── util.py /.gitignore: -------------------------------------------------------------------------------- 1 | # The following is adapted and extended from: https://github.com/github/gitignore 2 | 3 | # Generated GRiD header 4 | grid.cuh 5 | 6 | # Custom debug files 7 | t.txt 8 | t2.txt 9 | t3.txt 10 | 11 | # Prerequisites 12 | *.d 13 | 14 | # Compiled Object files 15 | *.slo 16 | *.lo 17 | *.o 18 | *.obj 19 | 20 | # Precompiled Headers 21 | *.gch 22 | *.pch 23 | 24 | # Compiled Dynamic libraries 25 | *.so 26 | *.dylib 27 | *.dll 28 | 29 | # Fortran module files 30 | *.mod 31 | *.smod 32 | 33 | # Compiled Static libraries 34 | *.lai 35 | *.la 36 | *.a 37 | *.lib 38 | 39 | # Executables 40 | *.exe 41 | *.out 42 | *.app 43 | 44 | # CUDA 45 | *.i 46 | *.ii 47 | *.gpu 48 | *.ptx 49 | *.cubin 50 | *.fatbin 51 | 52 | # Byte-compiled / optimized / DLL files 53 | __pycache__/ 54 | *.py[cod] 55 | *$py.class 56 | 57 | # Distribution / packaging 58 | .Python 59 | .eggs/ 60 | *.egg-info/ 61 | .installed.cfg 62 | *.egg 63 | 64 | # Installer logs 65 | pip-log.txt 66 | pip-delete-this-directory.txt 67 | 68 | # Jupyter Notebook 69 | .ipynb_checkpoints 70 | 71 | # IPython 72 | profile_default/ 73 | ipython_config.py 74 | 75 | # Python Environments 76 | .env 77 | .venv 78 | env/ 79 | venv/ 80 | ENV/ 81 | env.bak/ 82 | venv.bak/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "RBDReference"] 2 | path = RBDReference 3 | url = git@github.com:robot-acceleration/RBDReference.git 4 | [submodule "GRiDCodeGenerator"] 5 | path = GRiDCodeGenerator 6 | url = git@github.com:robot-acceleration/GRiDCodeGenerator.git 7 | [submodule "URDFParser"] 8 | path = URDFParser 9 | url = git@github.com:robot-acceleration/URDFParser.git 10 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Hardware Acceleration for Robotics 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GRiD 2 | 3 | A GPU-accelerated library for computing rigid body dynamics with analytical gradients. 4 | 5 | GRiD wraps our [URDFParser](https://github.com/robot-acceleration/URDFParser), [GRiDCodeGenerator](https://github.com/robot-acceleration/GRiDCodeGenerator), and [RBDReference](https://github.com/robot-acceleration/RBDReference) packages. Using its scripts, users can easily generate and test optimized rigid body dynamics CUDA C++ code for their URDF files. 6 | 7 | For additional information and links to our paper on this work, check out our [project website](https://brianplancher.com/publication/GRiD). 8 | 9 | **This package contains submodules make sure to run ```git submodule update --init --recursive```** after cloning! 10 | 11 | ![The GRiD library package ecosystem, showing how a user's URDF file can be transformed into optimized CUDA C++ code which can then be validated against reference outputs and benchmarked for performance.](imgs/GRiD.png) 12 | 13 | ## Usage: 14 | + To generate the ```grid.cuh``` header file please run: ```generateGRiD.py PATH_TO_URDF (-D)``` where ```-D``` indicates full debug mode which will include print statements after ever step of ever algorithm 15 | + To test the python refactored algorithms against our reference implmentations please run ```testGRiDRefactorings.py PATH_TO_URDF (-D)``` where ```-D``` prints extra debug values as compared to just the comparisons 16 | + To print and compare GRiD to reference values please do the following steps: 17 | 1) Print the reference values by running ```printReferenceValues.py PATH_TO_URDF (-D)``` where ```-D``` prints the full debug reference values from the refactorings 18 | 2) Run ```printGrid.py PATH_TO_URDF (-D)``` to compile, run, and print the same values from CUDA C++ 19 | 20 | ## Current Support 21 | GRiD currently fully supports any robot model consisting of revolute, prismatic, and fixed joints that does not have closed kinematic loops. 22 | 23 | GRiD currently implements the following rigid body dynamics algorithms: 24 | + Inverse Dynamics via the Recursive Newton Euler Algorithm (RNEA) from [Featherstone](https://link.springer.com/book/10.1007/978-1-4899-7560-7) 25 | + The Direct Inverse of Mass Matrix from [Carpentier](https://www.researchgate.net/publication/343098270_Analytical_Inverse_of_the_Joint_Space_Inertia_Matrix) 26 | + Forward Dynamics by combining the above algorithms as qdd = -M^{-1}(u-RNEA(q,qd,0)) 27 | + Analytical Gradients of Inverse Dynamics from [Carpentier](https://hal.archives-ouvertes.fr/hal-01790971) 28 | + Analytical Gradient of Forward Dynamics from [Carpentier](https://hal.archives-ouvertes.fr/hal-01790971) 29 | 30 | Additional algorithms and features are in development. If you have a particular algorithm or feature in mind please let us know by posting a GitHub issue. We'd also love your collaboration in implementing the Python reference implementation of any algorithm you'd like implemented! 31 | 32 | ## C++ API 33 | To enable GRiD to be used by both expert and novice GPU programmers we provide the following API interface for each rigid body dynamics algorithm: 34 | + ```ALGORITHM_inner```: a device function that computes the core computation. These functions assume that inputs are already loaded into GPU shared memory, require a pointer to additional scratch shared memory, and store the result back in shared memory. 35 | + ```ALGORITHM_device```: a device function that handles the shared memory allocation for the ```\_inner``` function. These functions assume that inputs are already loaded into, and return results to, GPU shared memory. 36 | + ```ALGORITHM_kernel```: a kernel that handles the shared memory allocation for the ```\_inner``` function. These functions assume that inputs are loaded into, and return results to, the global GPU memory. 37 | + ```ALGORITHM```: a host function that wraps the ```_kernel``` and handles the transfer of inputs to the GPU and the results back to the CPU. 38 | 39 | ## Citing GRiD 40 | To cite GRiD in your research, please use the following bibtex for our paper ["GRiD: GPU-Accelerated Rigid Body Dynamics with Analytical Gradients"](https://brianplancher.com/publication/grid/): 41 | ``` 42 | @inproceedings{plancher2022grid, 43 | title={GRiD: GPU-Accelerated Rigid Body Dynamics with Analytical Gradients}, 44 | author={Brian Plancher and Sabrina M. Neuman and Radhika Ghosal and Scott Kuindersma and Vijay Janapa Reddi}, 45 | booktitle={IEEE International Conference on Robotics and Automation (ICRA)}, 46 | year={2022}, 47 | month={May} 48 | } 49 | ``` 50 | 51 | ## Performance 52 | When performing multiple computations of rigid body dynamics algorithms, GRiD provides as much as a 7.6x speedup over a state-of-the-art, multi-threaded CPU implementation, and maintains as much as a 2.6x speedup when accounting for I/O overhead. 53 | 54 | ![Latency (including GPU I/O overhead) for N = 16, 32, 64, 128, and 256 computations of the gradient of forward dynamics for both the Pinocchio CPU baseline and the GRiD GPU library for various robot models (IIWA, HyQ, and Atlas). Overlayed is the speedup (or slowdown) of GRiD as compared to Pinocchio both in terms of pure computation and including I/O overhead.](imgs/benchmark_multi_fd_grad.png) 55 | 56 | To learn more about GRiD's performance results and to run your own benchmark analysis of GRiD's performance please check out our [GRiDBenchmarks](https://github.com/robot-acceleration/GRiDBenchmarks) repository and our [paper](https://brianplancher.com/publication/GRiD/). 57 | 58 | ## Instalation Instructions: 59 | ### Install Python Dependencies 60 | In order to support the wrapped packages there are 4 required external packages ```beautifulsoup4, lxml, numpy, sympy``` which can be automatically installed by running: 61 | ```shell 62 | pip3 install -r requirements.txt 63 | ``` 64 | ### Install CUDA Dependencies 65 | ``` 66 | sudo apt-get update 67 | sudo apt-get -y install xorg xorg-dev linux-headers-$(uname -r) apt-transport-https 68 | ``` 69 | ### Download and Install CUDA 70 | Note: for Ubuntu 20.04 see [https://developer.nvidia.com/cuda-downloads](https://developer.nvidia.com/cuda-downloads) for other distros 71 | ``` 72 | wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin 73 | sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600 74 | sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/7fa2af80.pub 75 | sudo add-apt-repository "deb https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/ /" 76 | sudo apt-get update 77 | sudo apt-get -y install cuda 78 | ``` 79 | ### Add the following to ```~/.bashrc``` 80 | ``` 81 | export PATH="/usr/local/cuda/bin:$PATH" 82 | export LD_LIBRARY_PATH="/usr/local/cuda/lib64:$LD_LIBRARY_PATH" 83 | export PATH="opt/nvidia/nsight-compute/:$PATH" 84 | ``` 85 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["GRiDCodeGenerator", "RBDReference", "URDFParser", "util", "generateGRiD", "printGRiD", "printReferenceValues", "testGRiDRefactorings"] -------------------------------------------------------------------------------- /generateGRiD.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | from URDFParser import URDFParser 3 | from GRiDCodeGenerator import GRiDCodeGenerator 4 | from util import parseInputs, printUsage, validateRobot 5 | 6 | def main(): 7 | URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME = parseInputs() 8 | 9 | parser = URDFParser() 10 | robot = parser.parse(URDF_PATH) 11 | 12 | validateRobot(robot) 13 | 14 | codegen = GRiDCodeGenerator(robot,DEBUG_MODE,True, FILE_NAMESPACE = FILE_NAMESPACE_NAME) 15 | codegen.gen_all_code() 16 | print("New code generated and saved to grid.cuh!") 17 | 18 | if __name__ == "__main__": 19 | main() -------------------------------------------------------------------------------- /imgs/GRiD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robot-acceleration/GRiD/56293d09f682864afd43de0169b176695eb1028c/imgs/GRiD.png -------------------------------------------------------------------------------- /imgs/benchmark_multi_fd_grad.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robot-acceleration/GRiD/56293d09f682864afd43de0169b176695eb1028c/imgs/benchmark_multi_fd_grad.png -------------------------------------------------------------------------------- /printGRiD.cu: -------------------------------------------------------------------------------- 1 | /*** 2 | nvcc -std=c++11 -o printGRiD.exe printGRiD.cu -gencode arch=compute_86,code=sm_86 -O3 -ftz=true -prec-div=false -prec-sqrt=false 3 | ***/ 4 | 5 | #include 6 | #include 7 | #include "grid.cuh" 8 | #define RANDOM_MEAN 0 9 | #define RANDOM_STDEV 1 10 | std::default_random_engine randEng(1337); // fixed seed 11 | std::normal_distribution randDist(RANDOM_MEAN, RANDOM_STDEV); //mean followed by stdiv 12 | template 13 | T getRand(){return static_cast(randDist(randEng));} 14 | 15 | template 16 | __host__ 17 | void test(){ 18 | T gravity = static_cast(9.81); 19 | dim3 dimms(grid::SUGGESTED_THREADS,1,1); 20 | cudaStream_t *streams = grid::init_grid(); 21 | grid::robotModel *d_robotModel = grid::init_robotModel(); 22 | grid::gridData *hd_data = grid::init_gridData(); 23 | 24 | // load q,qd,u 25 | for(int j = 0; j < grid::NUM_JOINTS; j++){ 26 | hd_data->h_q_qd_u[j] = getRand(); 27 | hd_data->h_q_qd_u[j+grid::NUM_JOINTS] = getRand(); 28 | hd_data->h_q_qd_u[j+2*grid::NUM_JOINTS] = getRand(); 29 | } 30 | gpuErrchk(cudaMemcpy(hd_data->d_q_qd_u,hd_data->h_q_qd_u,3*grid::NUM_JOINTS*sizeof(T),cudaMemcpyHostToDevice)); 31 | gpuErrchk(cudaDeviceSynchronize()); 32 | 33 | printf("q,qd,u\n"); 34 | printMat(hd_data->h_q_qd_u,1); 35 | printMat(&hd_data->h_q_qd_u[grid::NUM_JOINTS],1); 36 | printMat(&hd_data->h_q_qd_u[2*grid::NUM_JOINTS],1); 37 | 38 | printf("c\n"); 39 | grid::inverse_dynamics(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams); 40 | printMat(hd_data->h_c,1); 41 | 42 | printf("Minv\n"); 43 | grid::direct_minv(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams); 44 | printMat(hd_data->h_Minv,grid::NUM_JOINTS); 45 | 46 | printf("qdd\n"); 47 | grid::forward_dynamics(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams); 48 | printMat(hd_data->h_qdd,1); 49 | 50 | grid::inverse_dynamics_gradient(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams); 51 | printf("dc_dq\n"); 52 | printMat(hd_data->h_dc_du,grid::NUM_JOINTS); 53 | printf("dc_dqd\n"); 54 | printMat(&hd_data->h_dc_du[grid::NUM_JOINTS*grid::NUM_JOINTS],grid::NUM_JOINTS); 55 | 56 | grid::forward_dynamics_gradient(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams); 57 | printf("df_dq\n"); 58 | printMat(hd_data->h_df_du,grid::NUM_JOINTS); 59 | printf("df_dqd\n"); 60 | printMat(&hd_data->h_df_du[grid::NUM_JOINTS*grid::NUM_JOINTS],grid::NUM_JOINTS); 61 | 62 | grid::close_grid(streams,d_robotModel,hd_data); 63 | } 64 | 65 | int main(void){ 66 | test(); return 0; 67 | } -------------------------------------------------------------------------------- /printGRiD.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | from URDFParser import URDFParser 3 | from GRiDCodeGenerator import GRiDCodeGenerator 4 | from util import parseInputs, printUsage, validateRobot, initializeValues 5 | import subprocess 6 | import sys 7 | 8 | def main(): 9 | inputs = parseInputs(NO_ARG_OPTION = True) 10 | if not inputs is None: 11 | URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME = inputs 12 | parser = URDFParser() 13 | robot = parser.parse(URDF_PATH) 14 | 15 | validateRobot(robot, NO_ARG_OPTION = True) 16 | 17 | codegen = GRiDCodeGenerator(robot,DEBUG_MODE,True, FILE_NAMESPACE = FILE_NAMESPACE_NAME) 18 | print("-----------------") 19 | print("Generating GRiD.cuh") 20 | print("-----------------") 21 | codegen.gen_all_code() 22 | print("New code generated and saved to grid.cuh!") 23 | 24 | print("-----------------") 25 | print("Compiling printGRiD") 26 | print("-----------------") 27 | result = subprocess.run( \ 28 | ["nvcc", "-std=c++11", "-o", "printGRiD.exe", "printGRiD.cu", \ 29 | "-gencode", "arch=compute_86,code=sm_86", \ 30 | "-O3", "-ftz=true", "-prec-div=false", "-prec-sqrt=false"], \ 31 | capture_output=True, text=True \ 32 | ) 33 | if result.stderr: 34 | print("Compilation errors follow:") 35 | print(result.stderr) 36 | exit() 37 | 38 | print("-----------------") 39 | print("Running printGRiD") 40 | print("-----------------") 41 | result = subprocess.run(["./printGRiD.exe"], capture_output=True, text=True) 42 | if result.stderr: 43 | print("Runtime errors follow:") 44 | print(result.stderr) 45 | exit() 46 | 47 | print(result.stdout) 48 | 49 | if __name__ == "__main__": 50 | main() -------------------------------------------------------------------------------- /printReferenceValues.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | from URDFParser import URDFParser 3 | from RBDReference import RBDReference 4 | from GRiDCodeGenerator import GRiDCodeGenerator 5 | from util import parseInputs, printUsage, validateRobot, initializeValues, printErr 6 | import numpy as np 7 | 8 | def main(): 9 | URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME = parseInputs() 10 | 11 | parser = URDFParser() 12 | robot = parser.parse(URDF_PATH) 13 | 14 | validateRobot(robot) 15 | 16 | reference = RBDReference(robot) 17 | q, qd, u, n = initializeValues(robot, MATCH_CPP_RANDOM = True) 18 | 19 | print("q") 20 | print(q) 21 | print("qd") 22 | print(qd) 23 | print("u") 24 | print(u) 25 | 26 | (c, v, a, f) = reference.rnea(q,qd) 27 | print("c") 28 | print(c) 29 | 30 | print("Minv") 31 | Minv = reference.minv(q) 32 | print(Minv) 33 | 34 | print("qdd") 35 | qdd = np.matmul(Minv,(u-c)) 36 | print(qdd) 37 | 38 | dc_du = reference.rnea_grad(q, qd, qdd) 39 | print("dc/dq with qdd") 40 | print(dc_du[:,:n]) 41 | print("dc/dqd with qdd") 42 | print(dc_du[:,n:]) 43 | 44 | df_du = np.matmul(-Minv,dc_du) 45 | print("df/dq") 46 | print(df_du[:,:n]) 47 | print("df/dqd") 48 | print(df_du[:,n:]) 49 | 50 | if DEBUG_MODE: 51 | print("-------------------") 52 | print("printing intermediate outputs from refactorings") 53 | print("-------------------") 54 | codegen = GRiDCodeGenerator(robot, DEBUG_MODE, FILE_NAMESPACE = FILE_NAMESPACE_NAME) 55 | (c, v, a, f) = codegen.test_rnea(q,qd) 56 | print("v") 57 | print(v) 58 | print("a") 59 | print(a) 60 | print("f") 61 | print(f) 62 | print("c") 63 | print(c) 64 | 65 | Minv = codegen.test_minv(q) 66 | print("Minv") 67 | print(Minv) 68 | 69 | print("u-c") 70 | umc = u-c 71 | print(umc) 72 | print("qdd") 73 | qdd = np.matmul(Minv,umc) 74 | print(qdd) 75 | 76 | dc_du = codegen.test_rnea_grad(q, qd, qdd) 77 | print("dc/dq with qdd") 78 | print(dc_du[:,:n]) 79 | print("dc/dqd with qdd") 80 | print(dc_du[:,n:]) 81 | 82 | df_du = np.matmul(-Minv,dc_du) 83 | print("df/dq") 84 | print(df_du[:,:n]) 85 | print("df/dqd") 86 | print(df_du[:,n:]) 87 | 88 | if __name__ == "__main__": 89 | main() -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy >= 1.17.4 2 | sympy >= 1.7.1 3 | beautifulsoup4 >= 4.9.3 4 | lxml >= 4.2.1 -------------------------------------------------------------------------------- /testGRiDRefactorings.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | from URDFParser import URDFParser 3 | from RBDReference import RBDReference 4 | from GRiDCodeGenerator import GRiDCodeGenerator 5 | from util import parseInputs, printUsage, validateRobot, initializeValues, printErr 6 | import copy 7 | 8 | def main(): 9 | URDF_PATH, DEBUG_MODE, _ = parseInputs() 10 | 11 | parser = URDFParser() 12 | robot = parser.parse(URDF_PATH) 13 | 14 | validateRobot(robot) 15 | 16 | codegen = GRiDCodeGenerator(robot) 17 | reference = RBDReference(robot) 18 | q, qd, u, n = initializeValues(robot) 19 | 20 | print("rnea fpass err") 21 | (v, a, f) = codegen.test_rnea_fpass(q,qd) 22 | (v2, a2, f2) = reference.rnea_fpass(q,qd) 23 | print("v") 24 | printErr(v,v2,DEBUG_MODE) 25 | print("a") 26 | printErr(a,a2,DEBUG_MODE) 27 | print("f") 28 | printErr(f,f2,DEBUG_MODE) 29 | 30 | print("rnea bpass err") 31 | (c, fbp) = codegen.test_rnea_bpass(q,qd,f) 32 | (c2, fbp2) = reference.rnea_bpass(q,qd,f2) 33 | print("fbp") 34 | printErr(fbp,fbp2,DEBUG_MODE) 35 | print("c") 36 | printErr(c,c2,DEBUG_MODE) 37 | 38 | print("full rbd error") 39 | (c, v, a, f) = codegen.test_rnea(q,qd) 40 | (c2, v2, a2, f2) = reference.rnea(q,qd) 41 | print("v") 42 | printErr(v,v2,DEBUG_MODE) 43 | print("a") 44 | printErr(a,a2,DEBUG_MODE) 45 | print("f") 46 | printErr(f,f2,DEBUG_MODE) 47 | print("c") 48 | printErr(c,c2,DEBUG_MODE) 49 | 50 | print("minv bpass err") 51 | Minv, F, U, Dinv = codegen.test_minv_bpass(q) 52 | Minv2, F2, U2, Dinv2 = reference.minv_bpass(q) 53 | print("U") 54 | printErr(U,U2,DEBUG_MODE) 55 | print("Dinv") 56 | printErr(Dinv,Dinv2,DEBUG_MODE) 57 | print("F") 58 | printErr(F,F2,DEBUG_MODE) 59 | print("Minv") 60 | printErr(Minv,Minv2,DEBUG_MODE) 61 | 62 | print("minv fpass err") 63 | Minv = codegen.test_minv_fpass(q, Minv, F, U, Dinv) 64 | Minv2 = reference.minv_fpass(q, Minv2, F2, U2, Dinv2) 65 | print("Minv") 66 | printErr(Minv,Minv2,DEBUG_MODE) 67 | 68 | print("full minv err") 69 | Minv = codegen.test_minv(q) 70 | Minv2 = reference.minv(q) 71 | print("Minv") 72 | printErr(Minv,Minv2,DEBUG_MODE) 73 | 74 | print("dRnea err") 75 | (dc_dq, dc_dqd, dv_dq, dv_dqd, da_dq, da_dqd, df_fp_dq, df_fp_dqd, df_dq, df_dqd) = codegen.test_rnea_grad_inner(q, qd, v, a, f) 76 | (dv_dq2, da_dq2, df_fp_dq2) = reference.rnea_grad_fpass_dq(q, qd, v2, a2) 77 | (dv_dqd2, da_dqd2, df_fp_dqd2) = reference.rnea_grad_fpass_dqd(q, qd, v2) 78 | df_dq2 = copy.deepcopy(df_fp_dq2) 79 | df_dqd2 = copy.deepcopy(df_fp_dqd2) 80 | dc_dq2 = reference.rnea_grad_bpass_dq(q, f2, df_dq2) 81 | dc_dqd2 = reference.rnea_grad_bpass_dqd(q, df_dqd2) 82 | print("dv/dq") 83 | printErr(dv_dq,dv_dq2,DEBUG_MODE) 84 | print("dv/dqd") 85 | printErr(dv_dqd,dv_dqd2,DEBUG_MODE) 86 | print("da/dq") 87 | printErr(da_dq,da_dq2,DEBUG_MODE) 88 | print("da/dqd") 89 | printErr(da_dqd,da_dqd2,DEBUG_MODE) 90 | print("df/dqfp") 91 | printErr(df_fp_dq,df_fp_dq2,DEBUG_MODE) 92 | print("df/dqdfp") 93 | printErr(df_fp_dqd,df_fp_dqd2,DEBUG_MODE) 94 | print("df/dq") 95 | printErr(df_dq,df_dq2,DEBUG_MODE) 96 | print("df/dqd") 97 | printErr(df_dqd,df_dqd2,DEBUG_MODE) 98 | print("dc/dq") 99 | printErr(dc_dq,dc_dq2,DEBUG_MODE) 100 | print("dc/dqd") 101 | printErr(dc_dqd,dc_dqd2,DEBUG_MODE) 102 | 103 | if __name__ == "__main__": 104 | main() -------------------------------------------------------------------------------- /util/__init__.py: -------------------------------------------------------------------------------- 1 | from .util import * -------------------------------------------------------------------------------- /util/util.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pathlib 3 | import random 4 | import numpy as np 5 | np.set_printoptions(precision=4, suppress=True, linewidth = 100) 6 | 7 | def printUsage(NO_ARG_OPTION = False): 8 | print("Usage is: script.py PATH_TO_URDF (FILE_NAMESPACE_NAME) (-D)") 9 | print(" where -D indicates full debug mode") 10 | if NO_ARG_OPTION: 11 | print("Alternative usage assuming grid.cuh is already generated: script.py") 12 | 13 | def fileExists(FILE_PATH): 14 | return pathlib.Path(FILE_PATH).is_file() 15 | 16 | def validateFile(FILE_PATH, NO_ARG_OPTION = False): 17 | if not fileExists(FILE_PATH): 18 | print("[!Error] grid.cuh does not exist") 19 | printUsage(NO_ARG_OPTION) 20 | exit() 21 | 22 | def parseInputs(NO_ARG_OPTION = False): 23 | args = sys.argv[1:] 24 | if len(args) == 0: 25 | if NO_ARG_OPTION: 26 | validateFile("grid.cuh", NO_ARG_OPTION) 27 | print("Using generated grid.cuh") 28 | return None 29 | print("[!Error] No URDF filepath specified") 30 | printUsage(NO_ARG_OPTION) 31 | exit() 32 | 33 | URDF_PATH = args[0] 34 | validateFile(URDF_PATH, NO_ARG_OPTION) 35 | 36 | DEBUG_MODE_1 = True if len(args) > 1 and ((args[1] == "-D") or (args[1] == "-d")) else False 37 | DEBUG_MODE_2 = True if len(args) > 2 and ((args[2] == "-D") or (args[2] == "-d")) else False 38 | DEBUG_MODE = DEBUG_MODE_1 or DEBUG_MODE_2 39 | 40 | if len(args) > 2: 41 | FILE_NAMESPACE_NAME = args[1] if DEBUG_MODE_2 else args[2] 42 | elif len(args) > 1 and not DEBUG_MODE_1: 43 | FILE_NAMESPACE_NAME = args[1] 44 | else: 45 | FILE_NAMESPACE_NAME = "grid" 46 | 47 | print("Running with: DEBUG_MODE = " + str(DEBUG_MODE)) 48 | print(" URDF = " + URDF_PATH) 49 | print(" NAME = " + FILE_NAMESPACE_NAME) 50 | 51 | return (URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME) 52 | 53 | def validateRobot(robot, NO_ARG_OPTION = False): 54 | if robot == None: 55 | print("[!Error] URDF parsing failed. Please make sure you input a valid URDF file.") 56 | printUsage(NO_ARG_OPTION) 57 | exit() 58 | 59 | def printErr(a, b, FULL_DEBUG = False, TOLERANCE = 1e-10): 60 | err = a - b 61 | err = abs(err) > TOLERANCE 62 | if err.any(): 63 | print(err) 64 | if (FULL_DEBUG): 65 | print("Inputs were:") 66 | print(a) 67 | print(b) 68 | else: 69 | print(" passed") 70 | 71 | def initializeValues(robot, MATCH_CPP_RANDOM = False): 72 | # allocate memory 73 | n = robot.get_num_pos() 74 | q = np.zeros((n)) 75 | qd = np.zeros((n)) 76 | u = np.zeros((n)) 77 | 78 | if (MATCH_CPP_RANDOM): 79 | # load CPP rand point 80 | if n > 0: 81 | q[0] = -0.336899 82 | qd[0] = 0.43302 83 | u[0] = 0.741788 84 | if n > 1: 85 | q[1] = 1.29662 86 | qd[1] = -0.421561 87 | u[1] = 1.92844 88 | if n > 2: 89 | q[2] = -0.677475 90 | qd[2] = -0.645439 91 | u[2] = -0.903882 92 | if n > 3: 93 | q[3] = -1.42182 94 | qd[3] = -1.86055 95 | u[3] = 0.0333959 96 | if n > 4: 97 | q[4] = -0.706676 98 | qd[4] = -0.0130938 99 | u[4] = 1.17986 100 | if n > 5: 101 | q[5] = -0.134981 102 | qd[5] = -0.458284 103 | u[5] = -1.94599 104 | if n > 6: 105 | q[6] = -1.14953 106 | qd[6] = 0.741174 107 | u[6] = 0.32869 108 | if n > 7: 109 | q[7] = -0.296646 110 | qd[7] = 1.76642 111 | u[7] = -0.139457 112 | if n > 8: 113 | q[8] = 2.13845 114 | qd[8] = 0.898011 115 | u[8] = 2.00667 116 | if n > 9: 117 | q[9] = 2.00956 118 | qd[9] = -1.85675 119 | u[9] = -0.519292 120 | if n > 10: 121 | q[10] = 1.55163 122 | qd[10] = 1.62223 123 | u[10] = -0.711198 124 | if n > 11: 125 | q[11] = 2.2893 126 | qd[11] = 0.709379 127 | u[11] = 0.376638 128 | if n > 12: 129 | q[12] = 0.0418005 130 | qd[12] = -0.382885 131 | u[12] = -0.209225 132 | if n > 13: 133 | q[13] = -0.125271 134 | qd[13] = -0.239602 135 | u[13] = -0.816928 136 | if n > 14: 137 | q[14] = -1.35512 138 | qd[14] = 1.88499 139 | u[14] = -0.943019 140 | if n > 15: 141 | q[15] = -0.606463 142 | qd[15] = -2.20784 143 | u[15] = -2.16433 144 | if n > 16: 145 | q[16] = -2.13552 146 | qd[16] = -0.921183 147 | u[16] = 1.37954 148 | if n > 17: 149 | q[17] = 0.229695 150 | qd[17] = -0.110463 151 | u[17] = 0.456738 152 | if n > 18: 153 | q[18] = 0.229592 154 | qd[18] = -1.64542 155 | u[18] = -0.702506 156 | if n > 19: 157 | q[19] = -0.197398 158 | qd[19] = -1.7481 159 | u[19] = 0.159814 160 | if n > 20: 161 | q[20] = -0.221438 162 | qd[20] = -0.562579 163 | u[20] = 0.944469 164 | if n > 21: 165 | q[21] = 1.02441 166 | qd[21] = 1.02289 167 | u[21] = 0.100297 168 | if n > 22: 169 | q[22] = -0.9309 170 | qd[22] = 0.21233 171 | u[22] = -0.1311 172 | if n > 23: 173 | q[23] = 1.12961 174 | qd[23] = 1.30624 175 | u[23] = 0.750389 176 | if n > 24: 177 | q[24] = 0.864741 178 | qd[24] = 1.31059 179 | u[24] = -0.666778 180 | if n > 25: 181 | q[25] = 0.705222 182 | qd[25] = -0.0383565 183 | u[25] = 0.486885 184 | if n > 26: 185 | q[26] = 0.0810176 186 | qd[26] = 0.317353 187 | u[26] = 0.513445 188 | if n > 27: 189 | q[27] = 0.541962 190 | qd[27] = 0.479234 191 | u[27] = 0.0573834 192 | if n > 28: 193 | q[28] = 1.01213 194 | qd[28] = 0.55686 195 | u[28] = 0.425883 196 | if n > 29: 197 | q[29] = 2.213 198 | qd[29] = 0.541122 199 | u[29] = 0.293804 200 | if n > 30: 201 | print("[!ERROR] CPP Random Match only implemented up to n = 30. Please use a lower dof URDF.") 202 | exit() 203 | else: 204 | for i in range(n): 205 | q[i] = random.random() 206 | qd[i] = random.random() 207 | u[i] = random.random() 208 | 209 | return q, qd, u, n --------------------------------------------------------------------------------