├── .github └── workflows │ ├── linux_ci.yml │ └── macos_ci.yml ├── .gitignore ├── CHANGELOG ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── THANKS.md ├── doc ├── CplusplusDevelopment.md ├── ExampleMatlab2D.md ├── ExampleMatlab3D.md ├── ExampleReplanning.md ├── Install.md ├── Parameters.md ├── index.md └── pics │ ├── 2d_3link_arm_dense.gif │ ├── 2d_3link_arm_nondense.gif │ ├── 3link_setting.png │ ├── wam_arm_plan_result.gif │ ├── wam_replan_1.gif │ ├── wam_replan_2.gif │ ├── wam_replan_setting.png │ ├── wam_results.png │ └── wam_settings.png ├── gpmp2.i ├── gpmp2 ├── CMakeLists.txt ├── config.h.in ├── dynamics │ ├── CMakeLists.txt │ ├── VehicleDynamics.h │ ├── VehicleDynamicsFactorPose2.h │ ├── VehicleDynamicsFactorPose2Vector.h │ ├── VehicleDynamicsFactorVector.h │ └── tests │ │ └── testVehicleDynamics.cpp ├── geometry │ ├── CMakeLists.txt │ ├── DynamicLieTraits.h │ ├── DynamicVector.h │ ├── Pose2Vector.h │ ├── ProductDynamicLieGroup.h │ ├── numericalDerivativeDynamic.h │ ├── tests │ │ ├── testDynamicVector.cpp │ │ ├── testPose2Vector.cpp │ │ └── testProductDynamicLieGroup.cpp │ └── utilsDynamic.h ├── gp │ ├── CMakeLists.txt │ ├── GPutils.cpp │ ├── GPutils.h │ ├── GaussianProcessInterpolatorLie.h │ ├── GaussianProcessInterpolatorLinear.h │ ├── GaussianProcessInterpolatorPose2.h │ ├── GaussianProcessInterpolatorPose2Vector.h │ ├── GaussianProcessInterpolatorPose3.h │ ├── GaussianProcessPriorLie.h │ ├── GaussianProcessPriorLinear.h │ ├── GaussianProcessPriorPose2.h │ ├── GaussianProcessPriorPose2Vector.h │ ├── GaussianProcessPriorPose3.h │ └── tests │ │ ├── testGaussianProcessInterpolatorLinear.cpp │ │ ├── testGaussianProcessInterpolatorPose2.cpp │ │ ├── testGaussianProcessInterpolatorPose2Vector.cpp │ │ ├── testGaussianProcessInterpolatorPose3.cpp │ │ ├── testGaussianProcessPriorLinear.cpp │ │ ├── testGaussianProcessPriorPose2.cpp │ │ ├── testGaussianProcessPriorPose2Vector.cpp │ │ └── testGaussianProcessPriorPose3.cpp ├── kinematics │ ├── Arm.cpp │ ├── Arm.h │ ├── ArmModel.h │ ├── CMakeLists.txt │ ├── ForwardKinematics-inl.h │ ├── ForwardKinematics.h │ ├── GaussianPriorWorkspaceOrientation.h │ ├── GaussianPriorWorkspaceOrientationArm.h │ ├── GaussianPriorWorkspacePose.h │ ├── GaussianPriorWorkspacePoseArm.h │ ├── GaussianPriorWorkspacePosition.h │ ├── GaussianPriorWorkspacePositionArm.h │ ├── GoalFactorArm.h │ ├── JointLimitCost.h │ ├── JointLimitFactorPose2Vector.h │ ├── JointLimitFactorVector.h │ ├── PointRobot.cpp │ ├── PointRobot.h │ ├── PointRobotModel.h │ ├── Pose2Mobile2Arms.cpp │ ├── Pose2Mobile2Arms.h │ ├── Pose2Mobile2ArmsModel.h │ ├── Pose2MobileArm.cpp │ ├── Pose2MobileArm.h │ ├── Pose2MobileArmModel.h │ ├── Pose2MobileBase.cpp │ ├── Pose2MobileBase.h │ ├── Pose2MobileBaseModel.h │ ├── Pose2MobileVetLin2Arms.cpp │ ├── Pose2MobileVetLin2Arms.h │ ├── Pose2MobileVetLin2ArmsModel.h │ ├── Pose2MobileVetLinArm.cpp │ ├── Pose2MobileVetLinArm.h │ ├── Pose2MobileVetLinArmModel.h │ ├── RobotModel-inl.h │ ├── RobotModel.h │ ├── VelocityLimitFactorVector.h │ ├── mobileBaseUtils.cpp │ ├── mobileBaseUtils.h │ └── tests │ │ ├── testArm.cpp │ │ ├── testArmModel.cpp │ │ ├── testGaussianPriorWorkspaceOrientation.cpp │ │ ├── testGaussianPriorWorkspacePose.cpp │ │ ├── testGaussianPriorWorkspacePosition.cpp │ │ ├── testGoalFactorArm.cpp │ │ ├── testJointLimitFactorPose2Vector.cpp │ │ ├── testJointLimitFactorVector.cpp │ │ ├── testMobileBaseUtils.cpp │ │ ├── testPointRobotModel.cpp │ │ ├── testPose2Mobile2Arms.cpp │ │ ├── testPose2MobileArm.cpp │ │ ├── testPose2MobileBase.cpp │ │ ├── testPose2MobileVetLin2Arms.cpp │ │ └── testPose2MobileVetLinArm.cpp ├── obstacle │ ├── CMakeLists.txt │ ├── ObstacleCost.h │ ├── ObstaclePlanarSDFFactor-inl.h │ ├── ObstaclePlanarSDFFactor.h │ ├── ObstaclePlanarSDFFactorArm.h │ ├── ObstaclePlanarSDFFactorGP-inl.h │ ├── ObstaclePlanarSDFFactorGP.h │ ├── ObstaclePlanarSDFFactorGPArm.h │ ├── ObstaclePlanarSDFFactorGPPointRobot.h │ ├── ObstaclePlanarSDFFactorGPPose2Mobile2Arms.h │ ├── ObstaclePlanarSDFFactorGPPose2MobileArm.h │ ├── ObstaclePlanarSDFFactorGPPose2MobileBase.h │ ├── ObstaclePlanarSDFFactorPointRobot.h │ ├── ObstaclePlanarSDFFactorPose2Mobile2Arms.h │ ├── ObstaclePlanarSDFFactorPose2MobileArm.h │ ├── ObstaclePlanarSDFFactorPose2MobileBase.h │ ├── ObstacleSDFFactor-inl.h │ ├── ObstacleSDFFactor.h │ ├── ObstacleSDFFactorArm.h │ ├── ObstacleSDFFactorGP-inl.h │ ├── ObstacleSDFFactorGP.h │ ├── ObstacleSDFFactorGPArm.h │ ├── ObstacleSDFFactorGPPose2Mobile2Arms.h │ ├── ObstacleSDFFactorGPPose2MobileArm.h │ ├── ObstacleSDFFactorGPPose2MobileBase.h │ ├── ObstacleSDFFactorGPPose2MobileVetLin2Arms.h │ ├── ObstacleSDFFactorGPPose2MobileVetLinArm.h │ ├── ObstacleSDFFactorPose2Mobile2Arms.h │ ├── ObstacleSDFFactorPose2MobileArm.h │ ├── ObstacleSDFFactorPose2MobileBase.h │ ├── ObstacleSDFFactorPose2MobileVetLin2Arms.h │ ├── ObstacleSDFFactorPose2MobileVetLinArm.h │ ├── PlanarSDF.h │ ├── SDFexception.h │ ├── SelfCollision.h │ ├── SelfCollisionArm.h │ ├── SignedDistanceField.cpp │ ├── SignedDistanceField.h │ └── tests │ │ ├── testObstaclePlanarSDFFactorArm.cpp │ │ ├── testObstaclePlanarSDFFactorGPArm.cpp │ │ ├── testObstacleSDFFactorArm.cpp │ │ ├── testObstacleSDFFactorGPArm.cpp │ │ ├── testPlanarSDF.cpp │ │ ├── testSelfCollision.cpp │ │ └── testSignedDistanceField.cpp ├── planner │ ├── BatchTrajOptimizer-inl.h │ ├── BatchTrajOptimizer.cpp │ ├── BatchTrajOptimizer.h │ ├── CMakeLists.txt │ ├── ISAM2TrajOptimizer-inl.h │ ├── ISAM2TrajOptimizer.h │ ├── TrajOptimizerSetting.cpp │ ├── TrajOptimizerSetting.h │ ├── TrajUtils.cpp │ ├── TrajUtils.h │ └── tests │ │ ├── testISAM2TrajOptimizer.cpp │ │ └── testTrajUtils.cpp └── utils │ ├── CMakeLists.txt │ ├── OpenRAVEutils.cpp │ ├── OpenRAVEutils.h │ ├── Timer.h │ ├── fileUtils.cpp │ ├── fileUtils.h │ ├── matlabUtils.cpp │ ├── matlabUtils.h │ └── tests │ └── testTimer.cpp ├── matlab ├── +gpmp2 │ ├── generate2Ddataset.m │ ├── generate3Ddataset.m │ ├── generateArm.m │ ├── generateMobileArm.m │ ├── plotArm.m │ ├── plotEvidenceMap2D.m │ ├── plotMap3D.m │ ├── plotPlanarArm.m │ ├── plotPlanarMobile2Arms.m │ ├── plotPlanarMobileArm.m │ ├── plotPlanarMobileBase.m │ ├── plotPointRobot2D.m │ ├── plotRobotModel.m │ ├── plotSignedDistanceField2D.m │ ├── plotSignedDistanceField3D.m │ ├── set3DPlotRange.m │ ├── signedDistanceField2D.m │ └── signedDistanceField3D.m ├── CMakeLists.txt ├── gpmp2_examples │ ├── Arm2FactorGraphExample.m │ ├── Arm3FactorGraphExample.m │ ├── Arm3GoalReachExample.m │ ├── Arm3JointLimitExample.m │ ├── Arm3PlannerExample.m │ ├── CMakeLists.txt │ ├── KinovaGen3FactorGraphExample.m │ ├── Mobile2ArmsFactorGraphExample.m │ ├── MobileArm2FactorGraphExample.m │ ├── MobileBaseFactorGraphExample.m │ ├── PointRobot2DFactorGraphExample.m │ ├── SaveSDFExample.m │ ├── WAMFactorGraphExample.m │ ├── WAMPlannerExample.m │ ├── WAMReplannerExample.m │ └── WAMWorkspaceConstraintsExample.m ├── gpmp2_tests │ └── test_parameterization.m └── matlab.h └── python ├── .gitignore ├── CMakeLists.txt ├── examples ├── Arm2FactorGraphExample.py ├── WAMFactorGraphExample.py ├── multi_graph │ ├── graph_pointRobot.py │ └── graph_utils.py ├── pointRobot2FactorExample.py ├── pointRobot2Factor_rh.py ├── pointRobot3FactorExample.py └── pointRobot3FactorExample_rh.py ├── gpmp2 ├── __init__.py ├── datasets │ ├── __init__.py │ ├── generate2Ddataset.py │ └── generate3Ddataset.py ├── preamble │ └── gpmp2.h ├── robots │ ├── __init__.py │ ├── generateArm.py │ └── generateMobileArm.py ├── specializations │ └── gpmp2.h └── utils │ ├── __init__.py │ ├── plot_utils.py │ ├── signedDistanceField2D.py │ └── signedDistanceField3D.py ├── requirements.txt └── templates ├── gpmp2.tpl └── setup.py.in /.github/workflows/linux_ci.yml: -------------------------------------------------------------------------------- 1 | name: Linux CI 2 | 3 | on: [pull_request] 4 | 5 | jobs: 6 | build: 7 | name: ${{ matrix.name }} ${{ matrix.build_type }} 8 | runs-on: ${{ matrix.os }} 9 | 10 | env: 11 | CTEST_OUTPUT_ON_FAILURE: ON 12 | CTEST_PARALLEL_LEVEL: 2 13 | CMAKE_BUILD_TYPE: ${{ matrix.build_type }} 14 | 15 | strategy: 16 | fail-fast: true 17 | matrix: 18 | # Github Actions requires a single row to be added to the build matrix. 19 | # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. 20 | name: [ 21 | ubuntu-20.04-gcc-9, 22 | ubuntu-20.04-clang-10, 23 | ] 24 | 25 | build_type: [Debug, Release] 26 | include: 27 | - name: ubuntu-20.04-gcc-9 28 | os: ubuntu-20.04 29 | compiler: gcc 30 | version: "9" 31 | 32 | - name: ubuntu-20.04-clang-10 33 | os: ubuntu-20.04 34 | compiler: clang 35 | version: "10" 36 | 37 | steps: 38 | - name: Setup Compiler 39 | run: | 40 | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. 41 | if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then 42 | # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool 43 | # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure 44 | # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository 45 | # This key is not in the keystore by default for Ubuntu so we need to add it. 46 | LLVM_KEY=15CF4D18AF4F7421 47 | gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY 48 | gpg -a --export $LLVM_KEY | sudo apt-key add - 49 | sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" 50 | fi 51 | sudo apt-get -y update 52 | 53 | if [ "${{ matrix.compiler }}" = "gcc" ]; then 54 | sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib 55 | echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV 56 | echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV 57 | else 58 | sudo apt-get install -y clang-${{ matrix.version }} g++-multilib 59 | echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV 60 | echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV 61 | fi 62 | - name: Install Dependencies 63 | run: | 64 | sudo apt-get -y install cmake build-essential pkg-config libicu-dev 65 | sudo apt-get -y install libtbb-dev libboost-all-dev 66 | 67 | # Install CppUnitLite. 68 | git clone https://github.com/borglab/CppUnitLite.git 69 | cd CppUnitLite && mkdir build && cd $_ 70 | cmake .. && sudo make -j4 install 71 | cd ../../ 72 | 73 | - name: Install GTSAM 74 | run: | 75 | git clone https://github.com/borglab/gtsam.git 76 | cd gtsam && mkdir build && cd $_ 77 | cmake -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF -DGTSAM_WITH_TBB=OFF -DGTSAM_BUILD_UNSTABLE=OFF .. && sudo make -j4 install 78 | cd ../../ 79 | 80 | - name: Checkout 81 | uses: actions/checkout@v3 82 | 83 | - name: Build Directory 84 | run: mkdir build 85 | 86 | - name: Configure 87 | run: | 88 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. 89 | working-directory: ./build 90 | 91 | - name: Build 92 | run: make -j$(nproc) 93 | working-directory: ./build 94 | 95 | - name: Test 96 | run: make -j$(nproc) check 97 | working-directory: ./build 98 | -------------------------------------------------------------------------------- /.github/workflows/macos_ci.yml: -------------------------------------------------------------------------------- 1 | name: macOS CI 2 | 3 | on: [pull_request] 4 | 5 | jobs: 6 | build: 7 | name: ${{ matrix.name }} ${{ matrix.build_type }} 8 | runs-on: ${{ matrix.os }} 9 | 10 | env: 11 | CTEST_OUTPUT_ON_FAILURE: ON 12 | CTEST_PARALLEL_LEVEL: 2 13 | CMAKE_BUILD_TYPE: ${{ matrix.build_type }} 14 | 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | # Github Actions requires a single row to be added to the build matrix. 19 | # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. 20 | name: [macOS-12-xcode-14.2] 21 | 22 | build_type: [Debug, Release] 23 | include: 24 | - name: macOS-12-xcode-14.2 25 | os: macOS-12 26 | compiler: xcode 27 | version: "14.2" 28 | 29 | steps: 30 | - name: Setup Compiler 31 | run: | 32 | sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app 33 | echo "CC=clang" >> $GITHUB_ENV 34 | echo "CXX=clang++" >> $GITHUB_ENV 35 | 36 | - name: Dependencies 37 | run: | 38 | brew install boost 39 | brew tap borglab/core 40 | 41 | - name: GTSAM 42 | run: brew install --HEAD gtsam@latest 43 | 44 | - name: Checkout 45 | uses: actions/checkout@v3 46 | 47 | - name: Build Directory 48 | run: mkdir ./build 49 | 50 | - name: Configure 51 | run: | 52 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. 53 | working-directory: ./build 54 | 55 | - name: Build 56 | run: make -j$(sysctl -n hw.physicalcpu) 57 | working-directory: ./build 58 | 59 | - name: Test 60 | run: make -j$(sysctl -n hw.physicalcpu) check 61 | working-directory: ./build 62 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | *.pyc 3 | *.asv 4 | **/.DS_Store 5 | -------------------------------------------------------------------------------- /CHANGELOG: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for GPMP2 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.0 (2021-08-13) 6 | ------------------ 7 | * Python wrapper with utils and examples 8 | * Self collision factor 9 | * Bugfix for theta_bias in Arm.h 10 | * Contributors: Kalyan Vasudev Alwala, Mustafa Mukadam 11 | 12 | 0.2.1 (2021-07-21) 13 | ------------------ 14 | * Joint limit factors with examples 15 | * Optimizer no increase feature 16 | * More robot models (two arm, w/ mobile base, w/ linear actuator) 17 | * Workspace i.e. end effector position, orientation, and full pose constraint 18 | * Updated documentation and some bugfixes 19 | * Contributors: Jing Dong, Mustafa Mukadam 20 | 21 | 0.2.0 (2017-06-16) 22 | ------------------ 23 | * Update installation documentation 24 | * Fix compilation on Windows + Visual Studio 2015 25 | * Fix compilation on Linux + GCC 5.4 26 | * Add saving and loading signed distance field as binary .dat file 27 | * Add Gaussian process prior on Lie group, see details in Arxiv preprint 28 | * Add planar mobile manipulator (SE(2) x Vector(N)) class 29 | * Fix Matlab signed distance field generation error when no obstacle presents 30 | * Fix Matlab wrapper for obstacle factors 31 | * Add planar point robot (Vector(2)) class and plot utils in Matlab 32 | * Contributors: Jing Dong, Mustafa Mukadam 33 | 34 | 0.1.0 (2016-06-16) 35 | ------------------ 36 | * Initial release 37 | * Contributors: Jing Dong, Mustafa Mukadam 38 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | enable_testing() 3 | project(gpmp2 4 | LANGUAGES CXX C 5 | VERSION 1.0.0) 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") 7 | 8 | add_compile_options(-faligned-new) 9 | 10 | # Enforce c++17 standards 11 | add_compile_options(-std=c++17) # CMake 3.1 and earlier 12 | set(CMAKE_CXX_STANDARD 17) 13 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 14 | #TODO(Varun) Verify C++17 is enabled on Windows as well 15 | # Look at GtsamBuildTypes L153 16 | 17 | # Mac ONLY. Define Relative Path on Mac OS 18 | if(NOT DEFINED CMAKE_MACOSX_RPATH) 19 | set(CMAKE_MACOSX_RPATH 0) 20 | endif() 21 | 22 | # version indicator 23 | set(GPMP2_VERSION_MAJOR 1) 24 | set(GPMP2_VERSION_MINOR 0) 25 | set(GPMP2_VERSION_PATCH 0) 26 | set(gpmp2_VERSION_STRING "${GPMP2_VERSION_MAJOR}.${GPMP2_VERSION_MINOR}.${GPMP2_VERSION_PATCH}") 27 | set(GPMP2_VERSION_STRING gpmp2_VERSION_STRING) 28 | 29 | # options: whether turn on Matlab/Python toolbox 30 | option(GPMP2_BUILD_STATIC_LIBRARY "whether build static library" OFF) 31 | option(GPMP2_BUILD_MATLAB_TOOLBOX "whether build matlab toolbox, need shared lib" OFF) 32 | option(GPMP2_BUILD_PYTHON_TOOLBOX "whether build python toolbox, need shared lib" OFF) 33 | 34 | # Enable or disable serialization with GPMP2_ENABLE_BOOST_SERIALIZATION 35 | option(GPMP2_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) 36 | if(GPMP2_ENABLE_BOOST_SERIALIZATION) 37 | add_compile_definitions(GTSAM_ENABLE_BOOST_SERIALIZATION) 38 | add_compile_definitions(GPMP2_ENABLE_BOOST_SERIALIZATION) 39 | endif() 40 | 41 | if(GPMP2_BUILD_STATIC_LIBRARY AND GPMP2_BUILD_MATLAB_TOOLBOX) 42 | message(FATAL_ERROR "Matlab toolbox needs static lib to be built") 43 | endif() 44 | 45 | 46 | # Find GTSAM components 47 | find_package(GTSAM REQUIRED) # Uses installed package 48 | include_directories(${GTSAM_INCLUDE_DIR}) 49 | set(GTSAM_LIBRARIES gtsam) # TODO: automatic search libs 50 | 51 | find_package(GTSAMCMakeTools) 52 | include(GtsamMakeConfigFile) 53 | include(GtsamBuildTypes) 54 | include(GtsamTesting) 55 | 56 | # for unittest scripts 57 | set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools") 58 | 59 | # Boost - same requirement as gtsam 60 | # find_package(Boost 1.65 COMPONENTS filesystem system thread serialization REQUIRED) 61 | include_directories(${Boost_INCLUDE_DIR}) 62 | 63 | 64 | # Generate and install config and dllexport files 65 | configure_file("gpmp2/config.h.in" "gpmp2/config.h") 66 | list(APPEND gpmp2_srcs "${PROJECT_BINARY_DIR}/gpmp2/config.h") 67 | include_directories(BEFORE ${PROJECT_BINARY_DIR}) # So we can include generated config header files 68 | install(FILES "${PROJECT_BINARY_DIR}/gpmp2/config.h" DESTINATION include/gpmp2) 69 | 70 | 71 | # include current source folder, at the very beginning 72 | include_directories(BEFORE ${CMAKE_CURRENT_SOURCE_DIR}) 73 | 74 | # Process source subdirs 75 | add_subdirectory(gpmp2) 76 | 77 | # Wrapping to MATLAB 78 | if(GPMP2_BUILD_MATLAB_TOOLBOX) 79 | # Build Matlab package 80 | add_subdirectory(matlab) 81 | endif() 82 | 83 | 84 | # Wrapping to Python 85 | if(GPMP2_BUILD_PYTHON_TOOLBOX) 86 | add_subdirectory(python) 87 | endif() 88 | 89 | # Install config and export files 90 | GtsamMakeConfigFile(gpmp2) 91 | export(TARGETS ${GPMP2_EXPORTED_TARGETS} FILE gpmp2-exports.cmake) 92 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | How to contribute 2 | =================================================== 3 | 4 | Please follow the guidelines below if you are interested in making contributions to this library: 5 | 6 | 1. *Open a new issue* and describe the bug you want to fix or the feature you want to implement (see the TODO list below for ideas) so it can be assigned to you 7 | 8 | 2. [*Fork the repository*](https://help.github.com/articles/fork-a-repo/) and *make a new branch* where updates will be added 9 | 10 | 3. Have [*descriptive commit messages*](http://tbaggery.com/2008/04/19/a-note-about-git-commit-messages.html) especially if the API is affected, and *check your commits* to make sure it compiles and runs 11 | 12 | 4. *Include tests* for new features or significant changes 13 | 14 | 5. *Push commits to your fork* and [*make a pull request*](https://help.github.com/articles/creating-a-pull-request/) 15 | 16 | 17 | TODO List 18 | ------ 19 | More or less in order of priority 20 | 21 | - Update to latest GTSAM version 22 | - Support Python 3.x 23 | - Load environments from urdf/xml style formats to construct signed distance fields 24 | - Load robot urdf/xml files and construct forward kinematics for them automatically 25 | - Simple visualizer for environment, robot, trajectory, etc on the python wrapper side 26 | - Alternate obstacle cost functions like, barrier function 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Georgia Tech Research Corporation 2 | Atlanta, Georgia 30332-0415 3 | All Rights Reserved 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 11 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 14 | -------------------------------------------------------------------------------- /THANKS.md: -------------------------------------------------------------------------------- 1 | GPMP2 was made possible by the efforts of the following people: 2 | 3 | - [Jing Dong](https://github.com/dongjing3309) 4 | - [Mustafa Mukadam](https://github.com/mhmukadam) 5 | - [Xinyan Yan](https://github.com/XinyanGT) 6 | - [Kalyan Vasudev Alvala](https://github.com/kalyanvasudev) 7 | -------------------------------------------------------------------------------- /doc/ExampleMatlab3D.md: -------------------------------------------------------------------------------- 1 | Example: A real-world 3D Matlab example 2 | =================================================== 3 | After the 2D example, we look at a 3D matlab example. The code can be found in [matlab/WAMPlannerExample.m](../matlab/WAMPlannerExample.m). 4 | 5 | Dataset 6 | ----- 7 | The generate3Ddataset utility function will generate a 3D dataset object that includes the ground truth 3D occupancy grid, 8 | cell size of the occupancy grid, and the occupancy grid origin (in gtsam.Point3 object) in world coordinate frame. 9 | 10 | ```matlab 11 | % dataset 12 | dataset = generate3Ddataset('WAMDeskDataset'); 13 | ``` 14 | 15 | To plot the 3D scene and arm with spheres model, use plotMap3D and plotRobotModel. 16 | set3DPlotRange function is used to setup and correct coordinate limits. 17 | 18 | ```matlab 19 | % plot problem setting 20 | figure(1), hold on 21 | title('Problem Settings') 22 | plotMap3D(dataset.corner_idx, origin, cell_size); 23 | plotRobotModel(arm, start_conf) 24 | plotRobotModel(arm, end_conf) 25 | % plot config 26 | set3DPlotRange(dataset) 27 | grid on, view(3) 28 | hold off 29 | ``` 30 | 31 | |**Figure 1:** WAM arm dataset visualized in Matlab, with start and end configuration.| 32 | |:-----------| 33 | |![Matlab scene](pics/wam_settings.png)| 34 | 35 | Signed Distance Field 36 | ----- 37 | The signed distance field in 3D cases is different from 2D cases. 38 | In matlab, the calculated 3D signed distance field is a 3-dimensional matrix, where 39 | the first dimension represents X direction, second dimension represents Y, 40 | and third one represents Z. 41 | 42 | ```matlab 43 | % init sdf 44 | field = signedDistanceField3D(dataset.map, dataset.cell_size); 45 | ``` 46 | 47 | A [SignedDistanceField](../gpmp2/obstacle/SignedDistanceField.h) object 48 | which stores the signed distance field is then initialized. 49 | Inside SignedDistanceField, the 3D signed distance field is stored in a vector of matrices. 50 | ```cpp 51 | class SignedDistanceField { 52 | ... 53 | // sdf data 54 | std::vector data_; 55 | ... 56 | ``` 57 | Each matrix represents a X-Y 'slice' of signed distance field, and vector index represents the Z direction. 58 | So when we initialize SignedDistanceField object from Matlab 3-D matrix, 59 | we get each 2-D layer matrix from Matlab 3-D matrix, and initialize each slice of SignedDistanceField. 60 | 61 | ```cpp 62 | class SignedDistanceField { 63 | ... 64 | /// insert data matrix to each layer of sdf 65 | /// @param z_idx the z index of 3-D sdf 66 | /// @param field_layer matrix of each slice of 3-D sdf, Matrix represent the X (col) & Y (row) 67 | void initFieldData(size_t z_idx, const gtsam::Matrix& field_layer) { 68 | ... 69 | 70 | ``` 71 | Note that for each X-Y 'slice' matrix, column index represents X direction, and row index represents Y direction, 72 | whose sequence is reversed compared to 3-D Matlab matrix, 73 | so when initializing SignedDistanceField slice, a transpose is needed. 74 | 75 | ```matlab 76 | sdf = SignedDistanceField(origin_point3, cell_size, size(field, 1), ... 77 | size(field, 2), size(field, 3)); 78 | for z = 1:size(field, 3) 79 | sdf.initFieldData(z-1, field(:,:,z)'); 80 | end 81 | ``` 82 | 83 | Results 84 | ----- 85 | Other than dataset and signed distance field calculation, remaining of the 3D example is quite similar to the 2D example. 86 | You can refer to [matlab/WAMPlannerExample.m](../matlab/WAMPlannerExample.m) for further details. 87 | 88 | |**Figure 2:** WAM arm example results visualized in Matlab, animation.| 89 | |:-----------| 90 | |![Animation](pics/wam_arm_plan_result.gif)| 91 | 92 | |**Figure 3:** WAM arm example results visualized in Matlab, static trajectory| 93 | |:-----------| 94 | |![Static trajectory](pics/wam_results.png)| 95 | -------------------------------------------------------------------------------- /doc/Install.md: -------------------------------------------------------------------------------- 1 | Installation guide 2 | ===== 3 | 4 | CMake settings 5 | ----- 6 | 7 | GPMP2 can be compiled by steps in README if Boost and GTSAM installed at default locations, and the default GPMP2 install location is ```/usr/local``` on Linux and ```C:\Program Files\gpmp2``` on Windows. If Boost and GTSAM are installed at non-default locations, or if you want to install GPMP2 to a non-default location, please update following CMake entries. 8 | 9 | 10 | | Entry | Value | Comment | 11 | | ------------- |:--------------|:---------| 12 | | ```CMAKE_INSTALL_PREFIX``` | ```${gpmp2_installed_path}``` | | 13 | | ```BOOST_INCLUDEDIR``` | ```${boost_include_installed_path}``` | | 14 | | ```BOOST_LIBRARYDIR``` | ```${boost_library_installed_path}``` | | 15 | | ```Boost_USE_STATIC_LIBS``` | ON/OFF | Set to ON if use statlic Boost libraries | 16 | | ```GTSAM_DIR``` | ```${gtsam_installed_path}/CMake``` | | 17 | | ```GTSAMCMakeTools_DIR``` | ```${gtsam_installed_path}/CMake/GTSAMCMakeTools``` | | 18 | 19 | Troubleshooting 20 | ----- 21 | 22 | ### Ubuntu 23 | 24 | * If you compile Matlab toolbox and everything compiles smoothly, but when you run any of the Matlab script, you get following error messages in Matlab 25 | ``` 26 | Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': 27 | Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' 28 | Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' 29 | ... 30 | ``` 31 | you can run following bash line 32 | ```bash 33 | export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9 34 | ``` 35 | before you run Matlab, or write this line in your `$HOME/.bashrc` so you don't have to type everytime before start Matlab. This mainly happens if you have GCC >= 5 and newer version Matlab like R2017a. 36 | 37 | 38 | -------------------------------------------------------------------------------- /doc/index.md: -------------------------------------------------------------------------------- 1 | Documentation 2 | ========= 3 | 4 | This is documentation for GPMP2 library. 5 | The Beginner section is a collection of tutorials, that will help the users understand and run the basic examples provided. 6 | The Advanced section is a collection of notes, that will help the users, play around with the examples/algorithms, use GPMP2 in their own projects, or even contribute to GPMP2. 7 | 8 | This is only a preliminary documentation, so please do not hesitate to let us know what can to be added. 9 | 10 | Beginner 11 | --------- 12 | 13 | - [Installation CMake settings](Install.md) 14 | - [Example: A simple 2D Matlab example](ExampleMatlab2D.md) 15 | - [Example: A real-world 3D Matlab example](ExampleMatlab3D.md) 16 | - [Example: A 3D re-planning Matlab example](ExampleReplanning.md) 17 | 18 | 19 | Advanced 20 | ------- 21 | - [Parameters of GPMP2](Parameters.md) 22 | - [C++ development guide](CplusplusDevelopment.md) -------------------------------------------------------------------------------- /doc/pics/2d_3link_arm_dense.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/2d_3link_arm_dense.gif -------------------------------------------------------------------------------- /doc/pics/2d_3link_arm_nondense.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/2d_3link_arm_nondense.gif -------------------------------------------------------------------------------- /doc/pics/3link_setting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/3link_setting.png -------------------------------------------------------------------------------- /doc/pics/wam_arm_plan_result.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_arm_plan_result.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_replan_1.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_replan_2.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_setting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_replan_setting.png -------------------------------------------------------------------------------- /doc/pics/wam_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_results.png -------------------------------------------------------------------------------- /doc/pics/wam_settings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/doc/pics/wam_settings.png -------------------------------------------------------------------------------- /gpmp2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # We split the library in to separate subfolders, each containing 2 | # tests, timing, and an optional convenience library. 3 | # The following variable is the master list of subdirs to add 4 | set(gpmp2_subdirs 5 | geometry 6 | gp 7 | kinematics 8 | dynamics 9 | obstacle 10 | planner 11 | utils 12 | ) 13 | set(gpmp2_srcs) 14 | 15 | # files want to be excluded 16 | set(excluded_sources "") 17 | 18 | # Library sources 19 | foreach(subdir ${gpmp2_subdirs}) 20 | file(GLOB subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h") 21 | list(REMOVE_ITEM subdir_srcs "${excluded_sources}") 22 | 23 | file(GLOB subdir_test_files "${subdir}/tests/*") 24 | 25 | list(APPEND gpmp2_srcs ${subdir_srcs}) 26 | message(STATUS "Building Module: ${subdir}") 27 | 28 | # local and tests 29 | add_subdirectory(${subdir}) 30 | endforeach(subdir) 31 | 32 | 33 | # Versions 34 | set(gpmp2_version ${GPMP2_VERSION_MAJOR}.${GPMP2_VERSION_MINOR}.${GPMP2_VERSION_PATCH}) 35 | set(gpmp2_soversion ${GPMP2_VERSION_MAJOR}) 36 | message(STATUS "GPMP2 Version: ${gpmp2_version}") 37 | message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") 38 | 39 | 40 | # build shared lib 41 | if(GPMP2_BUILD_STATIC_LIBRARY) 42 | # static 43 | message(STATUS "Build static library") 44 | add_library(${PROJECT_NAME} STATIC ${gpmp2_srcs}) 45 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${GTSAM_LIBRARIES}) 46 | set_target_properties(gpmp2 PROPERTIES 47 | OUTPUT_NAME gpmp2 48 | CLEAN_DIRECT_OUTPUT 1 49 | VERSION ${gpmp2_version} 50 | SOVERSION ${gpmp2_soversion}) 51 | if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library 52 | set_target_properties(gpmp2 PROPERTIES 53 | PREFIX "lib") 54 | endif() 55 | install(TARGETS ${PROJECT_NAME} EXPORT gpmp2-exports ARCHIVE DESTINATION lib) 56 | 57 | else() 58 | # shared 59 | message(STATUS "Build shared library") 60 | add_library(${PROJECT_NAME} SHARED ${gpmp2_srcs}) 61 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${GTSAM_LIBRARIES}) 62 | set_target_properties(gpmp2 PROPERTIES 63 | OUTPUT_NAME gpmp2 64 | CLEAN_DIRECT_OUTPUT 1 65 | VERSION ${gpmp2_version} 66 | SOVERSION ${gpmp2_soversion}) 67 | if(WIN32) 68 | set_target_properties(gpmp2 PROPERTIES 69 | PREFIX "" 70 | DEFINE_SYMBOL GPMP2_EXPORTS 71 | RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") 72 | endif() 73 | install(TARGETS ${PROJECT_NAME} EXPORT gpmp2-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) 74 | 75 | endif() 76 | 77 | list(APPEND GPMP2_EXPORTED_TARGETS gpmp2) 78 | set(GPMP2_EXPORTED_TARGETS "${GPMP2_EXPORTED_TARGETS}" PARENT_SCOPE) 79 | 80 | -------------------------------------------------------------------------------- /gpmp2/config.h.in: -------------------------------------------------------------------------------- 1 | /** 2 | * @file config.h 3 | * @brief configurations import from CMake 4 | * @author Jing Dong 5 | * @date May 21, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | 11 | // Whether GPMP2 is compiled as static or DLL in windows. 12 | // This will be used to decide whether include __declspec(dllimport) or not in headers 13 | #cmakedefine GPMP2_BUILD_STATIC_LIBRARY 14 | 15 | 16 | // Macros for exporting DLL symbols on Windows 17 | // Usage example: 18 | // In header file: 19 | // class GPMP2_EXPORT MyClass { ... }; 20 | // 21 | // Results in the following declarations: 22 | // When included while compiling the GPMP2 library itself: 23 | // class __declspec(dllexport) MyClass { ... }; 24 | // When included while compiling other code against GPMP2: 25 | // class __declspec(dllimport) MyClass { ... }; 26 | 27 | #ifdef _WIN32 28 | # ifdef GPMP2_BUILD_STATIC_LIBRARY 29 | # define GPMP2_EXPORT 30 | # define GPMP2_EXTERN_EXPORT extern 31 | # else /* GPMP2_BUILD_STATIC_LIBRARY */ 32 | # ifdef GPMP2_EXPORTS 33 | # define GPMP2_EXPORT __declspec(dllexport) 34 | # define GPMP2_EXTERN_EXPORT __declspec(dllexport) extern 35 | # else /* GPMP2_EXPORTS */ 36 | # define GPMP2_EXPORT __declspec(dllimport) 37 | # define GPMP2_EXTERN_EXPORT __declspec(dllimport) 38 | # endif /* GPMP2_EXPORTS */ 39 | # endif /* GPMP2_BUILD_STATIC_LIBRARY */ 40 | #else /* _WIN32 */ 41 | # define GPMP2_EXPORT 42 | # define GPMP2_EXTERN_EXPORT extern 43 | #endif 44 | -------------------------------------------------------------------------------- /gpmp2/dynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB dynamics_headers "*.h") 3 | install(FILES ${dynamics_headers} DESTINATION include/gpmp2/dynamics) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(dynamics "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamics.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamics.h 3 | * @brief vehicle dynamics functions 4 | * @author Jing Dong 5 | * @date Oct 14, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /// simple 2D vehicle dynamics: vehicle heading is consistent with velocity 16 | /// direction return sliding velocity for Lie group, lower is better 17 | inline double simple2DVehicleDynamicsPose2( 18 | const gtsam::Pose2& p, const gtsam::Vector3& v, 19 | gtsam::OptionalJacobian<1, 3> Hp = {}, 20 | gtsam::OptionalJacobian<1, 3> Hv = {}) { 21 | if (Hp) *Hp = (gtsam::Matrix13() << 0, 0, 0).finished(); 22 | if (Hv) *Hv = (gtsam::Matrix13() << 0, 1, 0).finished(); 23 | 24 | return v(1); 25 | } 26 | 27 | /// simple 2D vehicle dynamics: vehicle heading is consistent with velocity 28 | /// direction return sliding velocity for vector space, lower is better 29 | inline double simple2DVehicleDynamicsVector3( 30 | const gtsam::Vector3& p, const gtsam::Vector3& v, 31 | gtsam::OptionalJacobian<1, 3> Hp = {}, 32 | gtsam::OptionalJacobian<1, 3> Hv = {}) { 33 | if (Hp) 34 | *Hp = (gtsam::Matrix13() << 0, 0, -(v(1) * sin(p(2)) + v(0) * cos(p(2)))) 35 | .finished(); 36 | if (Hv) *Hv = (gtsam::Matrix13() << -sin(p(2)), cos(p(2)), 0).finished(); 37 | 38 | // sin(a-b) = sin(b)cos(a) - cos(a)sin(b) 39 | return v(1) * cos(p(2)) - v(0) * sin(p(2)); 40 | } 41 | 42 | } // namespace gpmp2 43 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamicsFactorPose2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamicsFactorPose2.h 3 | * @brief simple 2D vehicle dynamics factor for mobile base in Lie group SE(2) 4 | * @author Mustafa Mukadam 5 | * @date Jan 24, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * unary factor for vehicle dynamics 23 | */ 24 | class VehicleDynamicsFactorPose2 25 | : public gtsam::NoiseModelFactorN { 26 | private: 27 | // typedefs 28 | typedef VehicleDynamicsFactorPose2 This; 29 | typedef gtsam::NoiseModelFactorN Base; 30 | 31 | public: 32 | /// shorthand for a smart pointer to a factor 33 | typedef std::shared_ptr shared_ptr; 34 | 35 | /* Default constructor */ 36 | VehicleDynamicsFactorPose2() {} 37 | 38 | /** 39 | * Constructor 40 | * @param cost_sigma cost function covariance, should to identity model 41 | */ 42 | VehicleDynamicsFactorPose2(gtsam::Key poseKey, gtsam::Key velKey, 43 | double cost_sigma) 44 | : Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, 45 | velKey) {} 46 | 47 | virtual ~VehicleDynamicsFactorPose2() {} 48 | 49 | /// error function 50 | /// numerical/analytic Jacobians from cost function 51 | gtsam::Vector evaluateError( 52 | const gtsam::Pose2& pose, const gtsam::Vector& vel, 53 | gtsam::OptionalMatrixType H1 = nullptr, 54 | gtsam::OptionalMatrixType H2 = nullptr) const override { 55 | using namespace gtsam; 56 | 57 | if (H1 || H2) { 58 | Matrix13 Hp, Hv; 59 | const double err = 60 | simple2DVehicleDynamicsPose2(pose, vel.head<3>(), Hp, Hv); 61 | if (H1) { 62 | *H1 = Matrix::Zero(1, 3); 63 | H1->block<1, 3>(0, 0) = Hp; 64 | } 65 | if (H2) { 66 | *H2 = Matrix::Zero(1, 3); 67 | H2->block<1, 3>(0, 0) = Hv; 68 | } 69 | return (Vector(1) << err).finished(); 70 | 71 | } else { 72 | return (Vector(1) << simple2DVehicleDynamicsPose2(pose, vel.head<3>())) 73 | .finished(); 74 | } 75 | } 76 | 77 | /// @return a deep copy of this factor 78 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 79 | return std::static_pointer_cast( 80 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 81 | } 82 | 83 | /** print contents */ 84 | void print(const std::string& s = "", 85 | const gtsam::KeyFormatter& keyFormatter = 86 | gtsam::DefaultKeyFormatter) const { 87 | std::cout << s << "VehicleDynamicsFactorPose2 :" << std::endl; 88 | Base::print("", keyFormatter); 89 | } 90 | 91 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 92 | /** Serialization function */ 93 | friend class boost::serialization::access; 94 | template 95 | void serialize(ARCHIVE& ar, const unsigned int version) { 96 | ar& boost::serialization::make_nvp( 97 | "NoiseModelFactor2", boost::serialization::base_object(*this)); 98 | } 99 | #endif 100 | }; 101 | 102 | } // namespace gpmp2 103 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamicsFactorPose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamicsFactorPose2Vector.h 3 | * @brief simple 2D vehicle dynamics factor for mobile arm base in Lie group 4 | *SE(2) 5 | * @author Jing Dong, Mustafa Mukadam 6 | * @date Oct 14, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * unary factor for vehicle dynamics 25 | */ 26 | class VehicleDynamicsFactorPose2Vector 27 | : public gtsam::NoiseModelFactorN { 28 | private: 29 | // typedefs 30 | typedef VehicleDynamicsFactorPose2Vector This; 31 | typedef gtsam::NoiseModelFactorN Base; 32 | 33 | public: 34 | /// shorthand for a smart pointer to a factor 35 | typedef std::shared_ptr shared_ptr; 36 | 37 | /* Default constructor */ 38 | VehicleDynamicsFactorPose2Vector() {} 39 | 40 | /** 41 | * Constructor 42 | * @param cost_sigma cost function covariance, should to identity model 43 | */ 44 | VehicleDynamicsFactorPose2Vector(gtsam::Key poseKey, gtsam::Key velKey, 45 | double cost_sigma) 46 | : Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, 47 | velKey) {} 48 | 49 | virtual ~VehicleDynamicsFactorPose2Vector() {} 50 | 51 | /// error function 52 | /// numerical/analytic Jacobians from cost function 53 | gtsam::Vector evaluateError( 54 | const Pose2Vector& conf, const gtsam::Vector& vel, 55 | gtsam::OptionalMatrixType H1 = nullptr, 56 | gtsam::OptionalMatrixType H2 = nullptr) const override { 57 | using namespace gtsam; 58 | 59 | if (H1 || H2) { 60 | Matrix13 Hp, Hv; 61 | const double err = 62 | simple2DVehicleDynamicsPose2(conf.pose(), vel.head<3>(), Hp, Hv); 63 | if (H1) { 64 | *H1 = Matrix::Zero(1, conf.dim()); 65 | H1->block<1, 3>(0, 0) = Hp; 66 | } 67 | if (H2) { 68 | *H2 = Matrix::Zero(1, conf.dim()); 69 | H2->block<1, 3>(0, 0) = Hv; 70 | } 71 | return (Vector(1) << err).finished(); 72 | 73 | } else { 74 | return (Vector(1) << simple2DVehicleDynamicsPose2(conf.pose(), 75 | vel.head<3>())) 76 | .finished(); 77 | } 78 | } 79 | 80 | /// @return a deep copy of this factor 81 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 82 | return std::static_pointer_cast( 83 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 84 | } 85 | 86 | /** print contents */ 87 | void print(const std::string& s = "", 88 | const gtsam::KeyFormatter& keyFormatter = 89 | gtsam::DefaultKeyFormatter) const { 90 | std::cout << s << "VehicleDynamicsFactorPose2Vector :" << std::endl; 91 | Base::print("", keyFormatter); 92 | } 93 | 94 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 95 | /** Serialization function */ 96 | friend class boost::serialization::access; 97 | template 98 | void serialize(ARCHIVE& ar, const unsigned int version) { 99 | ar& boost::serialization::make_nvp( 100 | "NoiseModelFactor2", boost::serialization::base_object(*this)); 101 | } 102 | #endif 103 | }; 104 | 105 | } // namespace gpmp2 106 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamicsFactorVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamicsFactorVector.h 3 | * @brief simple 2D vehicle dynamics factor for mobile arm base in vector 4 | *space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * unary factor for vehicle dynamics 23 | */ 24 | class VehicleDynamicsFactorVector 25 | : public gtsam::NoiseModelFactorN { 26 | private: 27 | // typedefs 28 | typedef VehicleDynamicsFactorVector This; 29 | typedef gtsam::NoiseModelFactorN Base; 30 | 31 | public: 32 | /// shorthand for a smart pointer to a factor 33 | typedef std::shared_ptr shared_ptr; 34 | 35 | /* Default constructor */ 36 | VehicleDynamicsFactorVector() {} 37 | 38 | /** 39 | * Constructor 40 | * @param cost_sigma cost function covariance, should to identity model 41 | */ 42 | VehicleDynamicsFactorVector(gtsam::Key poseKey, gtsam::Key velKey, 43 | double cost_sigma) 44 | : Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, 45 | velKey) {} 46 | 47 | virtual ~VehicleDynamicsFactorVector() {} 48 | 49 | /// error function 50 | /// numerical/analytic Jacobians from cost function 51 | gtsam::Vector evaluateError( 52 | const gtsam::Vector& conf, const gtsam::Vector& vel, 53 | gtsam::OptionalMatrixType H1 = nullptr, 54 | gtsam::OptionalMatrixType H2 = nullptr) const override { 55 | using namespace gtsam; 56 | 57 | if (H1 || H2) { 58 | Matrix13 Hp, Hv; 59 | const double err = 60 | simple2DVehicleDynamicsVector3(conf.head<3>(), vel.head<3>(), Hp, Hv); 61 | if (H1) { 62 | *H1 = Matrix::Zero(1, conf.size()); 63 | H1->block<1, 3>(0, 0) = Hp; 64 | } 65 | if (H2) { 66 | *H2 = Matrix::Zero(1, conf.size()); 67 | H2->block<1, 3>(0, 0) = Hv; 68 | } 69 | return (Vector(1) << err).finished(); 70 | 71 | } else { 72 | return (Vector(1) << simple2DVehicleDynamicsVector3(conf.head<3>(), 73 | vel.head<3>())) 74 | .finished(); 75 | } 76 | } 77 | 78 | /// @return a deep copy of this factor 79 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 80 | return std::static_pointer_cast( 81 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 82 | } 83 | 84 | /** print contents */ 85 | void print(const std::string& s = "", 86 | const gtsam::KeyFormatter& keyFormatter = 87 | gtsam::DefaultKeyFormatter) const { 88 | std::cout << s << "VehicleDynamicsFactorVector :" << std::endl; 89 | Base::print("", keyFormatter); 90 | } 91 | 92 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 93 | /** Serialization function */ 94 | friend class boost::serialization::access; 95 | template 96 | void serialize(ARCHIVE& ar, const unsigned int version) { 97 | ar& boost::serialization::make_nvp( 98 | "NoiseModelFactor2", boost::serialization::base_object(*this)); 99 | } 100 | #endif 101 | }; 102 | 103 | } // namespace gpmp2 104 | -------------------------------------------------------------------------------- /gpmp2/geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB geometry_headers "*.h") 3 | install(FILES ${geometry_headers} DESTINATION include/gpmp2/geometry) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(geometry "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/geometry/DynamicLieTraits.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file DynamicLieTraits.h 3 | * @date Oct 4, 2016 4 | * @author Frank Dellaert 5 | * @author Mike Bosse 6 | * @author Jing Dong 7 | * @brief gtsam Lie group traits for dynamic size types 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | namespace gtsam { 15 | namespace internal { 16 | 17 | /// A helper class that implements the traits interface for GTSAM lie groups. 18 | /// To use this for your gtsam type, define: 19 | /// template<> struct traits : public internal::LieGroupTraits {}; 20 | /// Assumes existence of: identity, dimension, localCoordinates, retract, 21 | /// and additionally Logmap, Expmap, compose, between, and inverse 22 | template 23 | struct DynamicLieGroupTraits { 24 | typedef lie_group_tag structure_category; 25 | 26 | /// @name Group 27 | /// @{ 28 | typedef multiplicative_group_tag group_flavor; 29 | static Class Identity() { return Class::Identity(); } 30 | /// @} 31 | 32 | /// @name Manifold 33 | /// @{ 34 | typedef Class ManifoldType; 35 | enum { dimension = Class::dimension }; 36 | typedef Eigen::Matrix TangentVector; 37 | typedef OptionalJacobian ChartJacobian; 38 | 39 | static int GetDimension(const Class& m) { return m.dim(); } 40 | 41 | static TangentVector Local(const Class& origin, const Class& other, 42 | ChartJacobian Horigin = {}, 43 | ChartJacobian Hother = {}) { 44 | return origin.localCoordinates(other, Horigin, Hother); 45 | } 46 | 47 | static Class Retract(const Class& origin, const TangentVector& v, 48 | ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { 49 | return origin.retract(v, Horigin, Hv); 50 | } 51 | /// @} 52 | 53 | /// @name Lie Group 54 | /// @{ 55 | static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { 56 | return Class::Logmap(m, Hm); 57 | } 58 | 59 | static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { 60 | return Class::Expmap(v, Hv); 61 | } 62 | 63 | static Class Compose(const Class& m1, const Class& m2, // 64 | ChartJacobian H1 = {}, ChartJacobian H2 = {}) { 65 | return m1.compose(m2, H1, H2); 66 | } 67 | 68 | static Class Between(const Class& m1, const Class& m2, // 69 | ChartJacobian H1 = {}, ChartJacobian H2 = {}) { 70 | return m1.between(m2, H1, H2); 71 | } 72 | 73 | static Class Inverse(const Class& m, // 74 | ChartJacobian H = {}) { 75 | return m.inverse(H); 76 | } 77 | /// @} 78 | }; 79 | 80 | /// Both LieGroupTraits and Testable 81 | template 82 | struct DynamicLieGroup : DynamicLieGroupTraits, Testable {}; 83 | 84 | } // namespace internal 85 | } // namespace gtsam 86 | -------------------------------------------------------------------------------- /gpmp2/geometry/DynamicVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file DynamicVector.h 3 | * @brief Wrapper of dynamic size eigen vector with all Lie group traits 4 | *implemented 5 | * @author Jing Dong 6 | * @date Oct 3, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include // from gtsam included path, use gtsam eigen version 16 | #include 17 | 18 | namespace gpmp2 { 19 | 20 | /** 21 | * wrapper of dynamic size eigen vector 22 | * replace raw dynamic size eigen vector, which does not have all traits 23 | * fully implemented in gtsam 24 | */ 25 | class GPMP2_EXPORT DynamicVector { 26 | private: 27 | size_t dim_; 28 | Eigen::VectorXd vector_; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | /// Default constructor 34 | DynamicVector() {} 35 | 36 | // eigen constructor 37 | // non-explicit conversion enabled 38 | DynamicVector(const Eigen::VectorXd& vector) 39 | : dim_(vector.size()), vector_(vector) {} 40 | 41 | ~DynamicVector() {} 42 | 43 | // access element for convenience 44 | double operator()(size_t i) const { return vector_(i); } 45 | 46 | /// @name VectorSpace 47 | /// @{ 48 | 49 | // dimension 50 | enum { dimension = Eigen::Dynamic }; // no static dimension implemented 51 | size_t dim() const { return dim_; } 52 | 53 | // no static identity implemented 54 | static DynamicVector Identity() { 55 | throw std::runtime_error( 56 | "[DynamicVector] ERROR: no static identity implemented"); 57 | return DynamicVector(); 58 | } 59 | 60 | // addition 61 | DynamicVector operator+(const DynamicVector& m2) const { 62 | return DynamicVector(this->vector_ + m2.vector_); 63 | } 64 | 65 | // subtraction 66 | DynamicVector operator-(const DynamicVector& m2) const { 67 | return DynamicVector(this->vector_ - m2.vector_); 68 | } 69 | 70 | // inversion 71 | DynamicVector operator-() const { return DynamicVector(-this->vector_); } 72 | 73 | // conversion to gtsam::Vector 74 | const Eigen::VectorXd& vector() const { return vector_; } 75 | 76 | // addition of a vector on the right 77 | DynamicVector operator+(const Eigen::VectorXd& vec) const { 78 | return DynamicVector(this->vector_ + vec); 79 | } 80 | 81 | /// @} 82 | /// @name Testable 83 | /// @{ 84 | 85 | // print 86 | void print(const std::string& s) const { 87 | std::cout << s << vector_ << std::endl; 88 | } 89 | 90 | // equals 91 | bool equals(const DynamicVector& m2, double tol) const { 92 | return gtsam::equal_with_abs_tol(this->vector_, m2.vector_, tol); 93 | } 94 | 95 | /// @} 96 | }; 97 | 98 | } // namespace gpmp2 99 | 100 | // gtsam traits for DynamicVector 101 | namespace gtsam { 102 | 103 | template <> 104 | struct traits 105 | : public internal::VectorSpace {}; 106 | 107 | } // namespace gtsam 108 | -------------------------------------------------------------------------------- /gpmp2/geometry/Pose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2Vector.h 3 | * @brief Lie group product of gtsam Pose2 and eigen vector 4 | * @author Jing Dong 5 | * @date Oct 3, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Pose2Vector is the Lie group product of gtsam Pose2 and eigen vector 23 | * use to describe the pose of a mobile manipulator with planner base 24 | */ 25 | class GPMP2_EXPORT Pose2Vector 26 | : public ProductDynamicLieGroup { 27 | private: 28 | typedef ProductDynamicLieGroup Base; 29 | 30 | public: 31 | // default constructor 32 | Pose2Vector() : Base() {} 33 | 34 | // constructors 35 | Pose2Vector(const gtsam::Pose2& p, const DynamicVector& c) : Base(p, c) {} 36 | Pose2Vector(const gtsam::Pose2& p, const gtsam::Vector& c) 37 | : Base(p, DynamicVector(c)) {} 38 | 39 | // copy constructors 40 | Pose2Vector(const Pose2Vector& b) : Base(b.first, b.second) {} 41 | Pose2Vector(const Base& b) : Base(b.first, b.second) {} 42 | 43 | // access 44 | const gtsam::Pose2& pose() const { return this->first; } 45 | const gtsam::Vector& configuration() const { return this->second.vector(); } 46 | 47 | // print 48 | void print(const std::string& s = "") const { 49 | std::cout << s; 50 | this->first.print("Pose: \n"); 51 | this->second.print("Configuration: \n"); 52 | } 53 | }; 54 | 55 | } // namespace gpmp2 56 | 57 | // gtsam traits for Pose2Vector 58 | namespace gtsam { 59 | template <> 60 | struct traits 61 | : internal::DynamicLieGroupTraits { 62 | static void Print(const gpmp2::Pose2Vector& m, const std::string& s = "") { 63 | std::cout << s; 64 | m.first.print("Pose: \n"); 65 | m.second.print("Configuration: \n"); 66 | } 67 | 68 | static bool Equals(const gpmp2::Pose2Vector& m1, const gpmp2::Pose2Vector& m2, 69 | double tol = 1e-8) { 70 | return traits::Equals(m1.first, m2.first, tol) && 71 | traits::Equals(m1.second, m2.second, tol); 72 | } 73 | }; 74 | 75 | } // namespace gtsam 76 | -------------------------------------------------------------------------------- /gpmp2/geometry/numericalDerivativeDynamic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDerivativeDynamic.h 3 | * @brief Dynamic size version of numericalDerivative 4 | * @author Frank Dellaert 5 | * @author Jing Dong 6 | * @date Oct 3, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | template 20 | gtsam::Matrix numericalDerivativeDynamic(const std::function h, 21 | const X& x, double delta = 1e-5) { 22 | BOOST_STATIC_ASSERT_MSG( 23 | (std::is_base_of::structure_category>::value), 25 | "Template argument Y must be a manifold type."); 26 | typedef gtsam::traits TraitsY; 27 | typedef typename TraitsY::TangentVector TangentY; 28 | 29 | BOOST_STATIC_ASSERT_MSG( 30 | (std::is_base_of::structure_category>::value), 32 | "Template argument X must be a manifold type."); 33 | static const int N = 34 | DimensionUtils::dimension>::getDimension(x); 35 | typedef gtsam::traits TraitsX; 36 | typedef typename TraitsX::TangentVector TangentX; 37 | 38 | // get value at x, and corresponding chart 39 | const Y hx = h(x); 40 | 41 | // Bit of a hack for now to find number of rows 42 | const TangentY zeroY = TraitsY::Local(hx, hx); 43 | const size_t m = zeroY.size(); 44 | 45 | // Prepare a tangent vector to perturb x with, only works for fixed size 46 | TangentX dx; 47 | dx.setZero(N); 48 | 49 | // Fill in Jacobian H 50 | gtsam::Matrix H = gtsam::Matrix::Zero(m, N); 51 | const double factor = 1.0 / (2.0 * delta); 52 | for (int j = 0; j < N; j++) { 53 | dx(j) = delta; 54 | const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); 55 | dx(j) = -delta; 56 | const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); 57 | dx(j) = 0; 58 | H.col(j) << (dy1 - dy2) * factor; 59 | } 60 | return H; 61 | } 62 | 63 | /** use a raw C++ function pointer */ 64 | template 65 | typename gtsam::Matrix numericalDerivativeDynamic(Y (*h)(const X&), const X& x, 66 | double delta = 1e-5) { 67 | return numericalDerivativeDynamic(std::bind(h, std::placeholders::_1), 68 | x, delta); 69 | } 70 | 71 | } // namespace gpmp2 72 | -------------------------------------------------------------------------------- /gpmp2/geometry/utilsDynamic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file utilsDynamic.h 3 | * @date Oct 4, 2016 4 | * @author Jing Dong 5 | * @brief dynamic size utility 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | /// get dimension from fixed size type 15 | template 16 | struct DimensionUtils { 17 | static inline int getDimension(const T&) { return N; } 18 | }; 19 | 20 | /// template specialization get dimension from dynamic size type 21 | template 22 | struct DimensionUtils { 23 | static inline int getDimension(const T& m) { return m.dim(); } 24 | }; 25 | 26 | } // namespace gpmp2 27 | -------------------------------------------------------------------------------- /gpmp2/gp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB gp_headers "*.h") 3 | install(FILES ${gp_headers} DESTINATION include/gpmp2/gp) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(gp "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/gp/GPutils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GPutils.cpp 3 | * @brief GP utils, calculation of Qc, Q, Lamda matrices etc. 4 | * @author Xinyan Yan, Jing Dong 5 | * @date Qct 26, 2015 6 | **/ 7 | 8 | #include 9 | 10 | using namespace gtsam; 11 | 12 | namespace gpmp2 { 13 | 14 | /* ************************************************************************** */ 15 | Matrix getQc(const SharedNoiseModel Qc_model) { 16 | noiseModel::Gaussian *Gassian_model = 17 | dynamic_cast(Qc_model.get()); 18 | return (Gassian_model->R().transpose() * Gassian_model->R()).inverse(); 19 | } 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/gp/GPutils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GPutils.h 3 | * @brief GP utils, calculation of Qc, Q, Lamda matrices etc. 4 | * @author Xinyan Yan, Jing Dong 5 | * @date Qct 26, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /// get Qc covariance matrix from noise model 20 | GPMP2_EXPORT gtsam::Matrix getQc(const gtsam::SharedNoiseModel Qc_model); 21 | 22 | /// calculate Q 23 | inline gtsam::Matrix calcQ(const gtsam::Matrix& Qc, double tau) { 24 | assert(Qc.rows() == Qc.cols()); 25 | return (gtsam::Matrix(2 * Qc.rows(), 2 * Qc.rows()) 26 | << 1.0 / 3 * pow(tau, 3.0) * Qc, 27 | 1.0 / 2 * pow(tau, 2.0) * Qc, 1.0 / 2 * pow(tau, 2.0) * Qc, tau * Qc) 28 | .finished(); 29 | } 30 | 31 | /// calculate Q_inv 32 | inline gtsam::Matrix calcQ_inv(const gtsam::Matrix& Qc, double tau) { 33 | assert(Qc.rows() == Qc.cols()); 34 | const gtsam::Matrix Qc_inv = Qc.inverse(); 35 | return (gtsam::Matrix(2 * Qc.rows(), 2 * Qc.rows()) 36 | << 12.0 * pow(tau, -3.0) * Qc_inv, 37 | (-6.0) * pow(tau, -2.0) * Qc_inv, (-6.0) * pow(tau, -2.0) * Qc_inv, 38 | 4.0 * pow(tau, -1.0) * Qc_inv) 39 | .finished(); 40 | } 41 | 42 | /// calculate Phi 43 | inline gtsam::Matrix calcPhi(size_t dof, double tau) { 44 | return (gtsam::Matrix(2 * dof, 2 * dof) << gtsam::Matrix::Identity(dof, dof), 45 | tau * gtsam::Matrix::Identity(dof, dof), 46 | gtsam::Matrix::Zero(dof, dof), gtsam::Matrix::Identity(dof, dof)) 47 | .finished(); 48 | } 49 | 50 | /// calculate Lambda 51 | inline gtsam::Matrix calcLambda(const gtsam::Matrix& Qc, double delta_t, 52 | const double tau) { 53 | assert(Qc.rows() == Qc.cols()); 54 | return calcPhi(Qc.rows(), tau) - 55 | calcQ(Qc, tau) * (calcPhi(Qc.rows(), delta_t - tau).transpose()) * 56 | calcQ_inv(Qc, delta_t) * calcPhi(Qc.rows(), delta_t); 57 | } 58 | 59 | /// calculate Psi 60 | inline gtsam::Matrix calcPsi(const gtsam::Matrix& Qc, double delta_t, 61 | double tau) { 62 | assert(Qc.rows() == Qc.cols()); 63 | return calcQ(Qc, tau) * (calcPhi(Qc.rows(), delta_t - tau).transpose()) * 64 | calcQ_inv(Qc, delta_t); 65 | } 66 | 67 | } // namespace gpmp2 68 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose2.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, 4 | * works only in SE(2) 5 | * @author Jing Dong 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessInterpolatorLie 16 | GaussianProcessInterpolatorPose2; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose2Vector.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, 4 | * works only in SE(2) + Vector space 5 | * @author Jing Dong 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessInterpolatorLie 16 | GaussianProcessInterpolatorPose2Vector; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose3.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose3.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, 4 | * works only in SE(3) 5 | * @author Jing Dong 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessInterpolatorLie 16 | GaussianProcessInterpolatorPose3; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessPriorPose2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessPriorPose2.h 3 | * @brief Pose2 GP prior 4 | * @author Jing Dong 5 | **/ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | typedef GaussianProcessPriorLie GaussianProcessPriorPose2; 15 | 16 | } // namespace gpmp2 17 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessPriorPose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessPriorPose2Vector.h 3 | * @brief Pose2Vector GP prior 4 | * @author Jing Dong 5 | **/ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | typedef GaussianProcessPriorLie GaussianProcessPriorPose2Vector; 15 | 16 | } // namespace gpmp2 17 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessPriorPose3.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessPriorPose3.h 3 | * @brief Pose2 GP prior 4 | * @author Jing Dong 5 | **/ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | typedef GaussianProcessPriorLie GaussianProcessPriorPose3; 15 | 16 | } // namespace gpmp2 17 | -------------------------------------------------------------------------------- /gpmp2/kinematics/ArmModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ArmModel.h 3 | * @brief Arm with physical body, which represented by spheres 4 | * @author Jing Dong 5 | * @date Dec 30, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * Arm with physical body, which is represented by spheres 17 | * Used to check collisions 18 | */ 19 | typedef RobotModel ArmModel; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB kinematics_headers "*.h") 3 | install(FILES ${kinematics_headers} DESTINATION include/gpmp2/kinematics) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(kinematics "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/kinematics/ForwardKinematics-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ForwardKinematics-inl.h 3 | * @brief Abstract forward kinematics model 4 | * @author Jing Dong 5 | * @date May 28, 2015 6 | **/ 7 | 8 | #include 9 | 10 | namespace gpmp2 { 11 | 12 | /* ************************************************************************** */ 13 | template 14 | gtsam::Matrix ForwardKinematics::forwardKinematicsPose( 15 | const Pose& jp) const { 16 | std::vector jpx; 17 | forwardKinematics(jp, {}, jpx, nullptr); 18 | 19 | // convert vector in matrix 20 | gtsam::Matrix jpx_mat(6, nr_links_); 21 | for (size_t i = 0; i < nr_links_; i++) 22 | jpx_mat.col(i) = (gtsam::Vector6() << gtsam::Vector3( 23 | jpx[i].rotation().yaw(), jpx[i].rotation().pitch(), 24 | jpx[i].rotation().roll()), 25 | jpx[i].translation()) 26 | .finished(); 27 | return jpx_mat; 28 | } 29 | 30 | /* ************************************************************************** */ 31 | template 32 | gtsam::Matrix ForwardKinematics::forwardKinematicsPosition( 33 | const Pose& jp) const { 34 | std::vector jpx; 35 | forwardKinematics(jp, {}, jpx, nullptr); 36 | 37 | // convert vector in matrix 38 | gtsam::Matrix jpx_mat(6, nr_links_); 39 | for (size_t i = 0; i < nr_links_; i++) jpx_mat.col(i) = jpx[i].translation(); 40 | return jpx_mat; 41 | } 42 | 43 | /* ************************************************************************** */ 44 | template 45 | gtsam::Matrix ForwardKinematics::forwardKinematicsVel( 46 | const Pose& jp, const Velocity& jv) const { 47 | std::vector jpx; 48 | std::vector jvx; 49 | forwardKinematics(jp, jv, jpx, &jvx); 50 | 51 | // convert vector in matrix 52 | gtsam::Matrix jpv_mat(6, nr_links_); 53 | for (size_t i = 0; i < nr_links_; i++) jpv_mat.col(i) = jvx[i]; 54 | return jpv_mat; 55 | } 56 | 57 | } // namespace gpmp2 58 | -------------------------------------------------------------------------------- /gpmp2/kinematics/ForwardKinematics.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ForwardKinematics.h 3 | * @brief Abstract forward kinematics model 4 | * @author Jing Dong 5 | * @date May 28, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /// Enum for specifying Denavit-Hartenberg parameterization 20 | enum Parameterization { DH, MODIFIED_DH }; 21 | 22 | /** 23 | * Abstract forward kinematics model, without actual model and physical 24 | * representation template parameters are system pose and velocity state types 25 | */ 26 | template 27 | class ForwardKinematics { 28 | private: 29 | size_t dof_; // system degree of freedom 30 | size_t nr_links_; // number of links (piece of robot part) 31 | 32 | public: 33 | /// type defs 34 | typedef POSE Pose; 35 | typedef VELOCITY Velocity; 36 | 37 | /// default contructor, for serialization 38 | ForwardKinematics() {} 39 | 40 | /// Contructor take system DOF and number of links 41 | /// and the base pose (default zero pose) 42 | ForwardKinematics(size_t dof, size_t nr_links) 43 | : dof_(dof), nr_links_(nr_links) {} 44 | 45 | /// Default destructor 46 | virtual ~ForwardKinematics() {} 47 | 48 | /** 49 | * Forward kinematics: poses from configuration space to 3D workspace 50 | * Velocity kinematics: optional velocities from configuration space to 3D 51 | * workspace, pure virtual method, need implementation in derived class 52 | * 53 | * @param jp robot pose in config space 54 | * @param jv robot velocity in config space 55 | * @param jpx link poses in 3D work space 56 | * @param jvx link velocities in 3D work space, no angular rate 57 | * @param J_jpx_jp et al. optional Jacobians 58 | **/ 59 | virtual void forwardKinematics( 60 | const Pose& jp, std::optional jv, 61 | std::vector& jpx, 62 | std::vector* jvx = nullptr, 63 | gtsam::OptionalMatrixVecType J_jpx_jp = nullptr, 64 | gtsam::OptionalMatrixVecType J_jvx_jp = nullptr, 65 | gtsam::OptionalMatrixVecType J_jvx_jv = nullptr) const = 0; 66 | 67 | /** 68 | * Matrix wrapper for forwardKinematics, mainly used by matlab 69 | * each column is a single point / velocity of the joint, size 6xN, 3xN, 3xN 70 | * No Jacobians provided by this version 71 | */ 72 | gtsam::Matrix forwardKinematicsPose(const Pose& jp) const; 73 | gtsam::Matrix forwardKinematicsPosition(const Pose& jp) const; 74 | gtsam::Matrix forwardKinematicsVel(const Pose& jp, const Velocity& jv) const; 75 | 76 | /// accesses 77 | size_t dof() const { return dof_; } 78 | size_t nr_links() const { return nr_links_; } 79 | }; 80 | 81 | } // namespace gpmp2 82 | 83 | #include 84 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspaceOrientation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspaceOrientation.h 3 | * @brief Gaussian prior defined on the workspace orientation of any joint of 4 | *a robot given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /** 20 | * Gaussian prior defined on the workspace orientation 21 | */ 22 | template 23 | class GaussianPriorWorkspaceOrientation 24 | : public gtsam::NoiseModelFactorN { 25 | public: 26 | // typedefs 27 | typedef ROBOT Robot; 28 | typedef typename Robot::Pose Pose; 29 | 30 | private: 31 | // typedefs 32 | typedef GaussianPriorWorkspaceOrientation This; 33 | typedef gtsam::NoiseModelFactorN Base; 34 | 35 | const Robot& robot_; // Robot 36 | int joint_; // joint on the robot to be constrained 37 | gtsam::Rot3 des_orientation_; // desired workspace orientation for joint 38 | 39 | public: 40 | /* Default constructor */ 41 | GaussianPriorWorkspaceOrientation() {} 42 | 43 | /// Constructor 44 | GaussianPriorWorkspaceOrientation(gtsam::Key poseKey, const Robot& robot, 45 | int joint, 46 | const gtsam::Rot3& des_orientation, 47 | const gtsam::SharedNoiseModel& cost_model) 48 | : Base(cost_model, poseKey), 49 | robot_(robot), 50 | joint_(joint), 51 | des_orientation_(des_orientation) {} 52 | 53 | ~GaussianPriorWorkspaceOrientation() {} 54 | 55 | /// factor error function 56 | gtsam::Vector evaluateError( 57 | const Pose& pose, gtsam::OptionalMatrixType H1 = nullptr) const override { 58 | using namespace gtsam; 59 | 60 | std::vector joint_pos; 61 | std::vector J_jpx_jp; 62 | robot_.fk_model().forwardKinematics(pose, {}, joint_pos, nullptr, 63 | &J_jpx_jp); 64 | 65 | if (H1) { 66 | Matrix36 H_rp; 67 | Matrix33 H_er; 68 | Rot3 curr_orientation = joint_pos[joint_].rotation(H_rp); 69 | Vector error = des_orientation_.logmap(curr_orientation, {}, H_er); 70 | *H1 = H_er * H_rp * J_jpx_jp[joint_]; 71 | return error; 72 | } else { 73 | return des_orientation_.logmap(joint_pos[joint_].rotation()); 74 | } 75 | } 76 | 77 | /// @return a deep copy of this factor 78 | gtsam::NonlinearFactor::shared_ptr clone() const override { 79 | return std::static_pointer_cast( 80 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 81 | } 82 | 83 | /** print contents */ 84 | void print(const std::string& s = "", 85 | const gtsam::KeyFormatter& keyFormatter = 86 | gtsam::DefaultKeyFormatter) const { 87 | std::cout << s << "GaussianPriorWorkspaceOrientation :" << std::endl; 88 | Base::print("", keyFormatter); 89 | std::cout << "desired orientation : "; 90 | des_orientation_.print(); 91 | } 92 | 93 | private: 94 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 95 | /** Serialization function */ 96 | friend class boost::serialization::access; 97 | template 98 | void serialize(ARCHIVE& ar, const unsigned int version) { 99 | ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 100 | } 101 | #endif 102 | }; 103 | 104 | } // namespace gpmp2 105 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspaceOrientationArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspaceOrientationArm.h 3 | * @brief Gaussian prior defined on the workspace Orientation of any link of 4 | *an arm given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef GaussianPriorWorkspaceOrientation 18 | GaussianPriorWorkspaceOrientationArm; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePose.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePose.h 3 | * @brief Gaussian prior defined on the workspace pose of any joint of a robot 4 | * given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /** 20 | * Gaussian prior defined on the workspace pose 21 | */ 22 | template 23 | class GaussianPriorWorkspacePose 24 | : public gtsam::NoiseModelFactorN { 25 | public: 26 | // typedefs 27 | typedef ROBOT Robot; 28 | typedef typename Robot::Pose Pose; 29 | 30 | private: 31 | // typedefs 32 | typedef GaussianPriorWorkspacePose This; 33 | typedef gtsam::NoiseModelFactorN Base; 34 | 35 | const Robot& robot_; // Robot 36 | int joint_; // joint on the robot to be constrained 37 | gtsam::Pose3 des_pose_; // desired workspace pose for joint 38 | 39 | public: 40 | /* Default constructor */ 41 | GaussianPriorWorkspacePose() {} 42 | 43 | /// Constructor 44 | GaussianPriorWorkspacePose(gtsam::Key poseKey, const Robot& robot, int joint, 45 | const gtsam::Pose3& des_pose, 46 | const gtsam::SharedNoiseModel& cost_model) 47 | : Base(cost_model, poseKey), 48 | robot_(robot), 49 | joint_(joint), 50 | des_pose_(des_pose) {} 51 | 52 | virtual ~GaussianPriorWorkspacePose() {} 53 | 54 | /// factor error function 55 | gtsam::Vector evaluateError( 56 | const Pose& pose, gtsam::OptionalMatrixType H1 = nullptr) const override { 57 | using namespace gtsam; 58 | 59 | std::vector joint_pos; 60 | std::vector J_jpx_jp; 61 | robot_.fk_model().forwardKinematics(pose, {}, joint_pos, {}, &J_jpx_jp); 62 | 63 | if (H1) { 64 | Matrix66 H_ep; 65 | Vector error = des_pose_.logmap(joint_pos[joint_], {}, H_ep); 66 | *H1 = H_ep * J_jpx_jp[joint_]; 67 | return error; 68 | } else { 69 | return des_pose_.logmap(joint_pos[joint_]); 70 | } 71 | } 72 | 73 | /// @return a deep copy of this factor 74 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 75 | return std::static_pointer_cast( 76 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 77 | } 78 | 79 | /** print contents */ 80 | void print(const std::string& s = "", 81 | const gtsam::KeyFormatter& keyFormatter = 82 | gtsam::DefaultKeyFormatter) const { 83 | std::cout << s << "GaussianPriorWorkspacePose :" << std::endl; 84 | Base::print("", keyFormatter); 85 | std::cout << "desired pose : "; 86 | des_pose_.print(); 87 | } 88 | 89 | private: 90 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 91 | /** Serialization function */ 92 | friend class boost::serialization::access; 93 | template 94 | void serialize(ARCHIVE& ar, const unsigned int version) { 95 | ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 96 | } 97 | #endif 98 | }; 99 | 100 | } // namespace gpmp2 101 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePoseArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePoseArm.h 3 | * @brief Gaussian prior defined on the workspace pose of any link of an arm 4 | * given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef GaussianPriorWorkspacePose GaussianPriorWorkspacePoseArm; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePosition.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePosition.h 3 | * @brief Gaussian prior defined on the workspace position of any joint of a 4 | *robot given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /** 20 | * Gaussian prior defined on the workspace position 21 | */ 22 | template 23 | class GaussianPriorWorkspacePosition 24 | : public gtsam::NoiseModelFactorN { 25 | public: 26 | // typedefs 27 | typedef ROBOT Robot; 28 | typedef typename Robot::Pose Pose; 29 | 30 | private: 31 | // typedefs 32 | typedef GaussianPriorWorkspacePosition This; 33 | typedef gtsam::NoiseModelFactorN Base; 34 | 35 | const Robot& robot_; // Robot 36 | int joint_; // joint on the robot to be constrained 37 | gtsam::Point3 des_position_; // desired workspace position for joint 38 | 39 | public: 40 | /* Default constructor */ 41 | GaussianPriorWorkspacePosition() {} 42 | 43 | /// Constructor 44 | GaussianPriorWorkspacePosition(gtsam::Key poseKey, const Robot& robot, 45 | int joint, const gtsam::Point3& des_position, 46 | const gtsam::SharedNoiseModel& cost_model) 47 | : Base(cost_model, poseKey), 48 | robot_(robot), 49 | joint_(joint), 50 | des_position_(des_position) {} 51 | 52 | ~GaussianPriorWorkspacePosition() {} 53 | 54 | /// factor error function 55 | gtsam::Vector evaluateError( 56 | const Pose& pose, gtsam::OptionalMatrixType H1 = nullptr) const override { 57 | using namespace gtsam; 58 | 59 | std::vector joint_pos; 60 | std::vector J_jpx_jp; 61 | robot_.fk_model().forwardKinematics(pose, {}, joint_pos, {}, &J_jpx_jp); 62 | 63 | if (H1) { 64 | Matrix36 Hpp; 65 | Point3 curr_position = joint_pos[joint_].translation(Hpp); 66 | *H1 = Hpp * J_jpx_jp[joint_]; 67 | return curr_position - des_position_; 68 | } else { 69 | return joint_pos[joint_].translation() - des_position_; 70 | } 71 | } 72 | 73 | /// @return a deep copy of this factor 74 | gtsam::NonlinearFactor::shared_ptr clone() const override { 75 | return std::static_pointer_cast( 76 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 77 | } 78 | 79 | /** print contents */ 80 | void print(const std::string& s = "", 81 | const gtsam::KeyFormatter& keyFormatter = 82 | gtsam::DefaultKeyFormatter) const { 83 | std::cout << s << "GaussianPriorWorkspacePosition :" << std::endl; 84 | Base::print("", keyFormatter); 85 | std::cout << "desired position : " << des_position_.transpose() 86 | << std::endl; 87 | ; 88 | } 89 | 90 | private: 91 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 92 | /** Serialization function */ 93 | friend class boost::serialization::access; 94 | template 95 | void serialize(ARCHIVE& ar, const unsigned int version) { 96 | ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 97 | } 98 | #endif 99 | }; 100 | 101 | } // namespace gpmp2 102 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePositionArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePositionArm.h 3 | * @brief Gaussian prior defined on the workspace position of any link of an 4 | *arm given its state in configuration space 5 | * @author Mustafa Mukadam 6 | * @date Jan 8, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef GaussianPriorWorkspacePosition 18 | GaussianPriorWorkspacePositionArm; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GoalFactorArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GoalFactorArm.h 3 | * @brief generate error for guild an Arm to reach a 3D point destination 4 | * @author Jing Dong 5 | * @date Nov 23, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * unary factor connect to the last pose in arm configuration space 23 | */ 24 | class GoalFactorArm : public gtsam::NoiseModelFactorN { 25 | private: 26 | // typedefs 27 | typedef GoalFactorArm This; 28 | typedef gtsam::NoiseModelFactorN Base; 29 | 30 | // arm 31 | Arm arm_; 32 | 33 | // destination point 34 | gtsam::Point3 dest_point_; 35 | 36 | public: 37 | /// shorthand for a smart pointer to a factor 38 | typedef std::shared_ptr shared_ptr; 39 | 40 | /// Default constructor 41 | GoalFactorArm() {} 42 | 43 | /** 44 | * Constructor 45 | * @param cost_model cost function covariance 46 | */ 47 | GoalFactorArm(gtsam::Key poseKey, const gtsam::SharedNoiseModel& cost_model, 48 | const Arm& arm, const gtsam::Point3& dest_point) 49 | : Base(cost_model, poseKey), arm_(arm), dest_point_(dest_point) {} 50 | 51 | ~GoalFactorArm() {} 52 | 53 | /// error function 54 | gtsam::Vector evaluateError( 55 | const gtsam::Vector& conf, 56 | gtsam::OptionalMatrixType H1 = nullptr) const override { 57 | using namespace gtsam; 58 | 59 | // fk 60 | std::vector joint_pos; 61 | std::vector J_jpx_jp; 62 | arm_.forwardKinematics(conf, {}, joint_pos, {}, &J_jpx_jp); 63 | 64 | if (H1) { 65 | Matrix36 Hpp; 66 | Point3 end_point = joint_pos[arm_.dof() - 1].translation(Hpp); 67 | *H1 = Hpp * J_jpx_jp[arm_.dof() - 1]; 68 | return end_point - dest_point_; 69 | 70 | } else { 71 | return joint_pos[arm_.dof() - 1].translation() - dest_point_; 72 | } 73 | } 74 | 75 | /// @return a deep copy of this factor 76 | gtsam::NonlinearFactor::shared_ptr clone() const override { 77 | return std::static_pointer_cast( 78 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 79 | } 80 | 81 | /** print contents */ 82 | void print(const std::string& s = "", 83 | const gtsam::KeyFormatter& keyFormatter = 84 | gtsam::DefaultKeyFormatter) const { 85 | std::cout << s << "GoalFactorArm :" << std::endl; 86 | Base::print("", keyFormatter); 87 | std::cout << "dest : " << dest_point_.transpose() << std::endl; 88 | } 89 | 90 | private: 91 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 92 | /** Serialization function */ 93 | friend class boost::serialization::access; 94 | template 95 | void serialize(ARCHIVE& ar, const unsigned int version) { 96 | ar& boost::serialization::make_nvp( 97 | "NoiseModelFactor4", boost::serialization::base_object(*this)); 98 | } 99 | #endif 100 | }; 101 | 102 | } // namespace gpmp2 103 | -------------------------------------------------------------------------------- /gpmp2/kinematics/JointLimitCost.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file JointLimitCost.h 3 | * @brief apply joint limit cost 4 | * @author Jing Dong 5 | * @date June 12, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | /// hinge loss joint limit cost function 15 | inline double hingeLossJointLimitCost(double p, double down_limit, 16 | double up_limit, double thresh, 17 | double* H_p = nullptr) { 18 | if (p < down_limit + thresh) { 19 | if (H_p) *H_p = -1.0; 20 | return down_limit + thresh - p; 21 | 22 | } else if (p <= up_limit - thresh) { 23 | if (H_p) *H_p = 0.0; 24 | return 0.0; 25 | 26 | } else { 27 | if (H_p) *H_p = 1.0; 28 | return p - up_limit + thresh; 29 | } 30 | } 31 | 32 | } // namespace gpmp2 33 | -------------------------------------------------------------------------------- /gpmp2/kinematics/JointLimitFactorVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file JointLimitFactorVector.h 3 | * @brief apply joint limit to vector configuration space 4 | * @author Jing Dong 5 | * @date June 12, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace gpmp2 { 19 | 20 | /** 21 | * unary factor to apply joint limit cost to vector configuration space 22 | */ 23 | class JointLimitFactorVector : public gtsam::NoiseModelFactorN { 24 | private: 25 | // typedefs 26 | typedef JointLimitFactorVector This; 27 | typedef gtsam::NoiseModelFactorN Base; 28 | 29 | // joint limit value 30 | gtsam::Vector down_limit_, up_limit_; 31 | 32 | // hinge loss threshold 33 | gtsam::Vector limit_thresh_; 34 | 35 | public: 36 | /// shorthand for a smart pointer to a factor 37 | typedef std::shared_ptr shared_ptr; 38 | 39 | /** 40 | * Constructor 41 | * @param cost_model joint limit cost weight 42 | * @param limit_thresh hinge loss threshold 43 | */ 44 | JointLimitFactorVector(gtsam::Key poseKey, 45 | const gtsam::SharedNoiseModel& cost_model, 46 | const gtsam::Vector& down_limit, 47 | const gtsam::Vector& up_limit, 48 | const gtsam::Vector& limit_thresh) 49 | : Base(cost_model, poseKey), 50 | down_limit_(down_limit), 51 | up_limit_(up_limit), 52 | limit_thresh_(limit_thresh) { 53 | // check dimensions 54 | if ((size_t)down_limit.size() != cost_model->dim() || 55 | (size_t)up_limit.size() != cost_model->dim() || 56 | (size_t)limit_thresh.size() != cost_model->dim()) 57 | throw std::runtime_error( 58 | "[JointLimitFactorVector] ERROR: limit vector dim does not fit."); 59 | } 60 | 61 | ~JointLimitFactorVector() {} 62 | 63 | /// error function 64 | gtsam::Vector evaluateError( 65 | const gtsam::Vector& conf, 66 | gtsam::OptionalMatrixType H1 = nullptr) const override { 67 | using namespace gtsam; 68 | if (H1) *H1 = Matrix::Zero(conf.size(), conf.size()); 69 | Vector err(conf.size()); 70 | for (size_t i = 0; i < (size_t)conf.size(); i++) { 71 | if (H1) { 72 | double Hp; 73 | err(i) = hingeLossJointLimitCost(conf(i), down_limit_(i), up_limit_(i), 74 | limit_thresh_(i), &Hp); 75 | (*H1)(i, i) = Hp; 76 | } else { 77 | err(i) = hingeLossJointLimitCost(conf(i), down_limit_(i), up_limit_(i), 78 | limit_thresh_(i)); 79 | } 80 | } 81 | return err; 82 | } 83 | 84 | /// @return a deep copy of this factor 85 | gtsam::NonlinearFactor::shared_ptr clone() const override { 86 | return std::static_pointer_cast( 87 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 88 | } 89 | 90 | /** print contents */ 91 | void print(const std::string& s = "", 92 | const gtsam::KeyFormatter& keyFormatter = 93 | gtsam::DefaultKeyFormatter) const { 94 | std::cout << s << "JointLimitFactorVector :" << std::endl; 95 | Base::print("", keyFormatter); 96 | std::cout << "Limit cost threshold : " << limit_thresh_ << std::endl; 97 | } 98 | 99 | private: 100 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 101 | /** Serialization function */ 102 | friend class boost::serialization::access; 103 | template 104 | void serialize(ARCHIVE& ar, const unsigned int version) { 105 | ar& boost::serialization::make_nvp( 106 | "NoiseModelFactor1", boost::serialization::base_object(*this)); 107 | } 108 | #endif 109 | }; 110 | 111 | } // namespace gpmp2 112 | -------------------------------------------------------------------------------- /gpmp2/kinematics/PointRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PointRobot.cpp 3 | * @brief Abstract Point Robot 4 | * @author Mustafa Mukadam 5 | * @date July 20, 2016 6 | **/ 7 | 8 | #include 9 | 10 | using namespace gtsam; 11 | 12 | namespace gpmp2 { 13 | 14 | /* ************************************************************************** */ 15 | void PointRobot::forwardKinematics( 16 | const Vector& jp, std::optional jv, 17 | std::vector& jpx, std::vector* jvx, 18 | gtsam::OptionalMatrixVecType J_jpx_jp, 19 | gtsam::OptionalMatrixVecType J_jvx_jp, 20 | gtsam::OptionalMatrixVecType J_jvx_jv) const { 21 | // space for output 22 | jpx.resize(nr_links()); 23 | if (jvx) jvx->resize(nr_links()); 24 | if (J_jpx_jp) J_jpx_jp->assign(nr_links(), Matrix::Zero(6, dof())); 25 | if (J_jvx_jp) J_jvx_jp->assign(nr_links(), Matrix::Zero(6, dof())); 26 | if (J_jvx_jv) J_jvx_jv->assign(nr_links(), Matrix::Zero(6, dof())); 27 | 28 | Matrix H1, H2; 29 | 30 | // start calculating Forward and velocity kinematics and Jacobians 31 | for (size_t i = 0; i < nr_links(); i++) { 32 | // pose in workspace 33 | jpx[i] = Pose3::Create(Rot3(), Point3(jp[0], jp[1], 0), H1, H2); 34 | 35 | // velocity in workspace 36 | if (jv && jvx) (*jvx)[i] << (*jv)[0], (*jv)[1], 0, 0, 0, 0; 37 | 38 | // Jacobians 39 | if (J_jpx_jp) { 40 | (*J_jpx_jp)[i].col(0) = H2.col(0); 41 | (*J_jpx_jp)[i].col(1) = H2.col(1); 42 | } 43 | if (J_jvx_jv) { 44 | (*J_jvx_jv)[i].block<2, 2>(0, 0) = Matrix::Identity(2, 2); 45 | } 46 | } 47 | } 48 | 49 | } // namespace gpmp2 50 | -------------------------------------------------------------------------------- /gpmp2/kinematics/PointRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PointRobot.h 3 | * @brief Abstract Point Robot 4 | * @author Mustafa Mukadam 5 | * @date July 20, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Abstract PointRobot class without any physical model representation 23 | * Inherited from ForwardKinematics 24 | */ 25 | class GPMP2_EXPORT PointRobot 26 | : public ForwardKinematics { 27 | private: 28 | // typedefs 29 | typedef ForwardKinematics Base; 30 | 31 | public: 32 | /// default constructor 33 | PointRobot() {} 34 | 35 | /// Constructor takes in DOF 36 | PointRobot(size_t dof, size_t nr_links) : Base(dof, nr_links) {} 37 | 38 | /// Default destructor 39 | virtual ~PointRobot() {} 40 | 41 | /** 42 | * Forward kinematics: robot configuration to poses in workspace 43 | * Velocity kinematics: optional robot velocities to velocities in workspace 44 | * 45 | * @param jp robot pose in config space 46 | * @param jv robot velocity in config space 47 | * @param jpx robot pose in work space 48 | * @param jvx robot velocity in work space 49 | * @param J_jpx_jp et al. optional Jacobians 50 | **/ 51 | void forwardKinematics(const gtsam::Vector& jp, 52 | std::optional jv, 53 | std::vector& jpx, 54 | std::vector* jvx = nullptr, 55 | gtsam::OptionalMatrixVecType J_jpx_jp = nullptr, 56 | gtsam::OptionalMatrixVecType J_jvx_jp = nullptr, 57 | gtsam::OptionalMatrixVecType J_jvx_jv = nullptr) const; 58 | 59 | }; // PointRobot 60 | 61 | } // namespace gpmp2 62 | -------------------------------------------------------------------------------- /gpmp2/kinematics/PointRobotModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PointRobotModel.h 3 | * @brief PointRobot with a sphere at the centroid 4 | * @author Mustafa Mukadam 5 | * @date July 20, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * PointRobot with a sphere at the centroid 17 | */ 18 | typedef RobotModel PointRobotModel; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2Mobile2Arms.h 3 | * @brief Plannar mobile manipulator with two arms 4 | * @author Jing Dong 5 | * @date Aug 20, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Abstract plannar mobile manipulator with 2 arm 23 | * pose class is Pose2Vector, arm1 uses first arm1.dof as pose, then arm2 uses 24 | * last arm2.dof total DOF = 3 + arm1.dof + arm2.dof 25 | */ 26 | class GPMP2_EXPORT Pose2Mobile2Arms 27 | : public ForwardKinematics { 28 | private: 29 | // typedefs 30 | typedef ForwardKinematics Base; 31 | 32 | // base to arm pose 33 | gtsam::Pose3 base_T_arm1_, base_T_arm2_; 34 | // arm class 35 | Arm arm1_, arm2_; 36 | 37 | public: 38 | /// default contructor 39 | Pose2Mobile2Arms() {} 40 | 41 | /// constructor from Arm 42 | Pose2Mobile2Arms(const Arm& arm1, const Arm& arm2, 43 | const gtsam::Pose3& base_T_arm1 = gtsam::Pose3(), 44 | const gtsam::Pose3& base_T_arm2 = gtsam::Pose3()); 45 | 46 | /// Default destructor 47 | virtual ~Pose2Mobile2Arms() {} 48 | 49 | /** 50 | * Forward kinematics: joint configuration to poses in workspace 51 | * Velocity kinematics: optional joint velocities to velocities in workspace 52 | * 53 | * @param p position in config space 54 | * @param v velocity in config space 55 | * @param px link pose in work space 56 | * @param vx link velocity in work space 57 | * @param J_px_p et al. optional Jacobians 58 | **/ 59 | void forwardKinematics(const Pose2Vector& p, 60 | std::optional v, 61 | std::vector& px, 62 | std::vector* vx = nullptr, 63 | gtsam::OptionalMatrixVecType J_px_p = nullptr, 64 | gtsam::OptionalMatrixVecType J_vx_p = nullptr, 65 | gtsam::OptionalMatrixVecType J_vx_v = nullptr) const; 66 | 67 | /// accesses 68 | const Arm& arm1() const { return arm1_; } 69 | const Arm& arm2() const { return arm2_; } 70 | const gtsam::Pose3& base_T_arm1() const { return base_T_arm1_; } 71 | const gtsam::Pose3& base_T_arm2() const { return base_T_arm2_; } 72 | }; 73 | 74 | } // namespace gpmp2 75 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2Mobile2ArmsModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2Mobile2ArmsModel.h 3 | * @brief mobile pose2 + 2 x Arm with physical body, which represented by 4 | *spheres 5 | * @author Jing Dong 6 | * @date Aug 20, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | /** 17 | * Pose2 + Arm with physical body, which is represented by spheres 18 | * Used to check collisions 19 | */ 20 | typedef RobotModel Pose2Mobile2ArmsModel; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileArm.h 3 | * @brief Abstract plannar mobile manipulator, Pose2 + Arm 4 | * @author Jing Dong 5 | * @date Oct 4, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Abstract plannar mobile manipulator, without any physical model 23 | * representation Inherited from ForwardKinematics 24 | */ 25 | class GPMP2_EXPORT Pose2MobileArm 26 | : public ForwardKinematics { 27 | private: 28 | // typedefs 29 | typedef ForwardKinematics Base; 30 | 31 | // base to arm pose 32 | gtsam::Pose3 base_T_arm_; 33 | // arm class 34 | Arm arm_; 35 | 36 | public: 37 | /// default contructor 38 | Pose2MobileArm() {} 39 | 40 | /// constructor from Arm 41 | explicit Pose2MobileArm(const Arm& arm, 42 | const gtsam::Pose3& base_T_arm = gtsam::Pose3()); 43 | 44 | /// Default destructor 45 | virtual ~Pose2MobileArm() {} 46 | 47 | /** 48 | * Forward kinematics: joint configuration to poses in workspace 49 | * Velocity kinematics: optional joint velocities to velocities in workspace 50 | * 51 | * @param p position in config space 52 | * @param v velocity in config space 53 | * @param px link pose in work space 54 | * @param vx link velocity in work space 55 | * @param J_px_p et al. optional Jacobians 56 | **/ 57 | void forwardKinematics(const Pose2Vector& p, 58 | std::optional v, 59 | std::vector& px, 60 | std::vector* vx = nullptr, 61 | gtsam::OptionalMatrixVecType J_px_p = nullptr, 62 | gtsam::OptionalMatrixVecType J_vx_p = nullptr, 63 | gtsam::OptionalMatrixVecType J_vx_v = nullptr) const; 64 | 65 | /// accesses 66 | const gtsam::Pose3& base_T_arm() const { return base_T_arm_; } 67 | const Arm& arm() const { return arm_; } 68 | }; 69 | 70 | } // namespace gpmp2 71 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileArmModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileArmModel.h 3 | * @brief mobile pose2 + Arm with physical body, which represented by spheres 4 | * @author Jing Dong 5 | * @date Oct 13, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * Pose2 + Arm with physical body, which is represented by spheres 17 | * Used to check collisions 18 | */ 19 | typedef RobotModel Pose2MobileArmModel; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileBase.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileBase.cpp 3 | * @brief Abstract SE(2) mobile base 4 | * @author Mustafa Mukadam 5 | * @date Jan 22, 2018 6 | **/ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace gtsam; 14 | using namespace std; 15 | 16 | namespace gpmp2 { 17 | 18 | /* ************************************************************************** */ 19 | void Pose2MobileBase::forwardKinematics( 20 | const gtsam::Pose2& p, std::optional v, 21 | std::vector& px, 22 | std::vector* vx, 23 | gtsam::OptionalMatrixVecType J_px_p, 24 | gtsam::OptionalMatrixVecType J_vx_p, 25 | gtsam::OptionalMatrixVecType J_vx_v) const { 26 | if (v) 27 | throw runtime_error("[Pose2MobileBase] TODO: velocity not implemented"); 28 | 29 | if (!v && (vx || J_vx_p || J_vx_v)) 30 | throw runtime_error( 31 | "[Pose2MobileBase] ERROR: only ask for velocity in workspace given " 32 | "velocity in " 33 | "configuration space"); 34 | 35 | // allocate space 36 | px.resize(nr_links()); 37 | if (vx) vx->resize(nr_links()); 38 | if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof())); 39 | if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(6, dof())); 40 | if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(6, dof())); 41 | 42 | // assign values 43 | Matrix63 Hveh_base; 44 | if (J_px_p || J_vx_p || J_vx_v) { 45 | px[0] = computeBasePose3(p, Hveh_base); 46 | } else { 47 | px[0] = computeBasePose3(p); 48 | } 49 | if (J_px_p) (*J_px_p)[0].block<6, 3>(0, 0) = Hveh_base; 50 | if (vx) { 51 | (*vx)[0] = Vector6((*v)[0], (*v)[1], 0.0, 0.0, 0.0, 0.0); 52 | // (*J_vx_p)[0] is zero 53 | if (J_vx_v) (*J_vx_v)[0].block<2, 2>(0, 0) = Matrix2::Identity(); 54 | } 55 | } 56 | 57 | } // namespace gpmp2 58 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileBase.h 3 | * @brief Abstract SE(2) mobile base 4 | * @author Mustafa Mukadam 5 | * @date Jan 22, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace gpmp2 { 19 | 20 | /** 21 | * Abstract SE(2) mobile base inherited from ForwardKinematics 22 | */ 23 | class GPMP2_EXPORT Pose2MobileBase 24 | : public ForwardKinematics { 25 | private: 26 | // typedefs 27 | typedef ForwardKinematics Base; 28 | 29 | public: 30 | /// default constructor 31 | explicit Pose2MobileBase() : Base(3, 1) {} 32 | 33 | /// Default destructor 34 | virtual ~Pose2MobileBase() {} 35 | 36 | /** 37 | * Forward kinematics: joint configuration to poses in workspace 38 | * Velocity kinematics: optional joint velocities to velocities workspace 39 | * 40 | * @param p position in config space 41 | * @param v velocity in config space 42 | * @param px link pose in work space 43 | * @param vx link velocity in work space 44 | * @param J_px_p et al. optional Jacobians 45 | **/ 46 | void forwardKinematics(const gtsam::Pose2& p, 47 | std::optional v, 48 | std::vector& px, 49 | std::vector* vx = nullptr, 50 | gtsam::OptionalMatrixVecType J_px_p = nullptr, 51 | gtsam::OptionalMatrixVecType J_vx_p = nullptr, 52 | gtsam::OptionalMatrixVecType J_vx_v = nullptr) const; 53 | }; 54 | 55 | } // namespace gpmp2 56 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileBaseModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileBaseModel.h 3 | * @brief SE(2) mobile base with physical body, which is represented by 4 | *spheres 5 | * @author Mustafa Mukadam 6 | * @date Jan 22, 2018 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | /** 17 | * SE(2) mobile base with physical body, which is represented by spheres 18 | * Used to check collisions 19 | */ 20 | typedef RobotModel Pose2MobileBaseModel; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLin2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLin2Arms.h 3 | * @brief Plannar mobile manipulator with two arms on vertical linear actuator 4 | * @author Jing Dong 5 | * @date Aug 22, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Abstract plannar mobile manipulator with 2 arm on vertical linear actuator 23 | * pose class is Pose2Vector, linear actuator use first, arm1 uses next 24 | * arm1.dof, then arm2 uses last arm2.dof vector DOF = 1 + arm1.dof + arm2.dof 25 | * total DOF = 3 + 1 + arm1.dof + arm2.dof 26 | */ 27 | class GPMP2_EXPORT Pose2MobileVetLin2Arms 28 | : public ForwardKinematics { 29 | private: 30 | // typedefs 31 | typedef ForwardKinematics Base; 32 | 33 | // base to arm pose, when linear actuator is on zero 34 | gtsam::Pose3 base_T_torso_, torso_T_arm1_, torso_T_arm2_; 35 | // if reverse_linact_ == true, positive value on lin act means move down 36 | bool reverse_linact_; 37 | // arm class 38 | Arm arm1_, arm2_; 39 | 40 | public: 41 | /// default contructor 42 | Pose2MobileVetLin2Arms() {} 43 | 44 | /// constructor from Arm 45 | /// if reverse_linact == true, positive value on lin act means move down 46 | Pose2MobileVetLin2Arms(const Arm& arm1, const Arm& arm2, 47 | const gtsam::Pose3& base_T_torso = gtsam::Pose3(), 48 | const gtsam::Pose3& torso_T_arm1 = gtsam::Pose3(), 49 | const gtsam::Pose3& torso_T_arm2 = gtsam::Pose3(), 50 | bool reverse_linact = false); 51 | 52 | /// Default destructor 53 | virtual ~Pose2MobileVetLin2Arms() {} 54 | 55 | /** 56 | * Forward kinematics: joint configuration to poses in workspace 57 | * Velocity kinematics: optional joint velocities to velocities in workspace 58 | * 59 | * @param p position in config space 60 | * @param v velocity in config space 61 | * @param px link pose in work space 62 | * @param vx link velocity in work space 63 | * @param J_px_p et al. optional Jacobians 64 | **/ 65 | void forwardKinematics(const Pose2Vector& p, 66 | std::optional v, 67 | std::vector& px, 68 | std::vector* vx = nullptr, 69 | gtsam::OptionalMatrixVecType J_px_p = nullptr, 70 | gtsam::OptionalMatrixVecType J_vx_p = nullptr, 71 | gtsam::OptionalMatrixVecType J_vx_v = nullptr) const; 72 | 73 | /// accesses 74 | const Arm& arm1() const { return arm1_; } 75 | const Arm& arm2() const { return arm2_; } 76 | const gtsam::Pose3& base_T_torso() const { return base_T_torso_; } 77 | const gtsam::Pose3& torso_T_arm1() const { return torso_T_arm1_; } 78 | const gtsam::Pose3& torso_T_arm2() const { return torso_T_arm2_; } 79 | bool reverse_linact() const { return reverse_linact_; } 80 | }; 81 | 82 | } // namespace gpmp2 83 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLin2ArmsModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLin2ArmsModel.h 3 | * @brief mobile pose2 + linear actuator + 2 x Arm with physical body, 4 | *represented by spheres 5 | * @author Mustafa Mukadam 6 | * @date Sep 3, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | /** 17 | * Pose2 + lin actuator + 2 X Arm with physical body, represented by spheres 18 | * used to check collisions 19 | */ 20 | typedef RobotModel Pose2MobileVetLin2ArmsModel; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLinArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLinArm.h 3 | * @brief Abstract plannar mobile manipulator, Arm on a vetical linear actuator 4 | * @author Jing Dong 5 | * @date Aug 18, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Abstract plannar mobile manipulator 23 | * an Arm on Pose2 mobile base with a vetical linear actuator 24 | * Linear actuator on 1st dim of gtsam::Vector, remaining are Arm's 25 | */ 26 | class GPMP2_EXPORT Pose2MobileVetLinArm 27 | : public ForwardKinematics { 28 | private: 29 | // typedefs 30 | typedef ForwardKinematics Base; 31 | 32 | // base to arm pose, when linear actuator is on zero 33 | gtsam::Pose3 base_T_torso_, torso_T_arm_; 34 | // if reverse_linact_ == true, positive value on lin act means move down 35 | bool reverse_linact_; 36 | // arm class 37 | Arm arm_; 38 | 39 | public: 40 | /// default contructor 41 | Pose2MobileVetLinArm() {} 42 | 43 | /// constructor from Arm 44 | /// if reverse_linact == true, positive value on lin act means move down 45 | explicit Pose2MobileVetLinArm( 46 | const Arm& arm, const gtsam::Pose3& base_T_torso = gtsam::Pose3(), 47 | const gtsam::Pose3& torso_T_arm = gtsam::Pose3(), 48 | bool reverse_linact = false); 49 | 50 | /// Default destructor 51 | virtual ~Pose2MobileVetLinArm() {} 52 | 53 | /** 54 | * Forward kinematics: joint configuration to poses in workspace 55 | * Velocity kinematics: optional joint velocities to velocities in workspace 56 | * 57 | * @param p position in config space 58 | * @param v velocity in config space 59 | * @param px link pose in work space 60 | * @param vx link velocity in work space 61 | * @param J_px_p et al. optional Jacobians 62 | **/ 63 | void forwardKinematics(const Pose2Vector& p, 64 | std::optional v, 65 | std::vector& px, 66 | std::vector* vx = nullptr, 67 | gtsam::OptionalMatrixVecType J_px_p = nullptr, 68 | gtsam::OptionalMatrixVecType J_vx_p = nullptr, 69 | gtsam::OptionalMatrixVecType J_vx_v = nullptr) const; 70 | 71 | /// accesses 72 | const gtsam::Pose3& base_T_torso() const { return base_T_torso_; } 73 | const gtsam::Pose3& torso_T_arm() const { return torso_T_arm_; } 74 | const Arm& arm() const { return arm_; } 75 | bool reverse_linact() const { return reverse_linact_; } 76 | }; 77 | 78 | } // namespace gpmp2 79 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLinArmModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLinArmModel.h 3 | * @brief mobile pose2 + linear actuator + Arm with physical body, represented 4 | *by spheres 5 | * @author Mustafa Mukadam 6 | * @date Sep 3, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | /** 17 | * Pose2 + lin actuator + Arm with physical body, represented by spheres 18 | * used to check collisions 19 | */ 20 | typedef RobotModel Pose2MobileVetLinArmModel; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/kinematics/RobotModel-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RobotModel-inl.h 3 | * @brief Abstract robot model, provide sphere-based robot model. Templated by 4 | *FK model 5 | * @author Jing Dong 6 | * @date May 29, 2015 7 | **/ 8 | 9 | namespace gpmp2 { 10 | 11 | /* ************************************************************************** */ 12 | template 13 | void RobotModel::sphereCenters( 14 | const Pose& jp, std::vector& sph_centers, 15 | gtsam::OptionalMatrixVecType J_point_conf) const { 16 | // apply fk 17 | std::vector link_poses; 18 | std::vector J_pose_jp; 19 | if (J_point_conf) 20 | fk_model_.forwardKinematics(jp, {}, link_poses, nullptr, &J_pose_jp); 21 | else 22 | fk_model_.forwardKinematics(jp, {}, link_poses, nullptr); 23 | 24 | // convert to sphere centers 25 | sph_centers.resize(nr_body_spheres()); 26 | if (J_point_conf) J_point_conf->resize(nr_body_spheres()); 27 | 28 | for (size_t sph_idx = 0; sph_idx < nr_body_spheres(); sph_idx++) { 29 | if (J_point_conf) { 30 | gtsam::Matrix36 J_point_pose; 31 | sph_centers[sph_idx] = 32 | link_poses[body_spheres_[sph_idx].link_id].transformFrom( 33 | body_spheres_[sph_idx].center, J_point_pose); 34 | (*J_point_conf)[sph_idx] = 35 | J_point_pose * J_pose_jp[body_spheres_[sph_idx].link_id]; 36 | 37 | } else { 38 | sph_centers[sph_idx] = 39 | link_poses[body_spheres_[sph_idx].link_id].transformFrom( 40 | body_spheres_[sph_idx].center); 41 | } 42 | } 43 | } 44 | 45 | /* ************************************************************************** */ 46 | template 47 | gtsam::Point3 RobotModel::sphereCenter( 48 | size_t sph_idx, const Pose& jp, 49 | gtsam::OptionalMatrixType J_point_conf) const { 50 | // apply fk 51 | std::vector link_poses; 52 | std::vector J_pose_jp; 53 | if (J_point_conf) 54 | fk_model_.forwardKinematics(jp, {}, link_poses, nullptr, &J_pose_jp); 55 | else 56 | fk_model_.forwardKinematics(jp, {}, link_poses, nullptr); 57 | 58 | gtsam::Point3 sph_center; 59 | if (J_point_conf) { 60 | gtsam::Matrix36 J_point_pose; 61 | sph_center = link_poses[body_spheres_[sph_idx].link_id].transformFrom( 62 | body_spheres_[sph_idx].center, J_point_pose); 63 | *J_point_conf = J_point_pose * J_pose_jp[body_spheres_[sph_idx].link_id]; 64 | 65 | } else { 66 | sph_center = link_poses[body_spheres_[sph_idx].link_id].transformFrom( 67 | body_spheres_[sph_idx].center); 68 | } 69 | 70 | return sph_center; 71 | } 72 | 73 | /* ************************************************************************** */ 74 | template 75 | gtsam::Matrix RobotModel::sphereCentersMat(const Pose& jp) const { 76 | std::vector sph_centers; 77 | sphereCenters(jp, sph_centers); 78 | 79 | // convert to matrix 80 | gtsam::Matrix points_mat(3, nr_body_spheres()); 81 | for (size_t i = 0; i < nr_body_spheres(); i++) 82 | points_mat.col(i) = sph_centers[i]; 83 | return points_mat; 84 | } 85 | 86 | } // namespace gpmp2 87 | -------------------------------------------------------------------------------- /gpmp2/kinematics/RobotModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RobotModel.h 3 | * @brief Abstract robot model, provide sphere-based robot model. Templated by 4 | *FK model 5 | * @author Jing Dong 6 | * @date May 29, 2015 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /// body sphere class, each one indicate a body sphere 20 | struct BodySphere { 21 | size_t link_id; // attched link id, 0 - nr_link-1 22 | double radius; // sphere radius 23 | gtsam::Point3 center; // sphere center position to the link base 24 | // constructor 25 | BodySphere(size_t id, double r, const gtsam::Point3& c) 26 | : link_id(id), radius(r), center(c) {} 27 | ~BodySphere() {} 28 | }; 29 | 30 | /// vector of body sphere, typedef here to wrap in matlab 31 | using BodySphereVector = std::vector; 32 | 33 | /** 34 | * Abstract robot model, provide sphere-based robot model. 35 | * template parameter is forward kinematics type 36 | */ 37 | template 38 | class RobotModel { 39 | public: 40 | // typedefs 41 | typedef FK FKModel; 42 | typedef typename FKModel::Pose Pose; 43 | typedef typename FKModel::Velocity Velocity; 44 | 45 | private: 46 | FKModel fk_model_; // fk 47 | BodySphereVector body_spheres_; // body spheres 48 | 49 | public: 50 | /// default contructor for serialization 51 | RobotModel() {} 52 | 53 | /// Contructor take a fk model 54 | RobotModel(const FKModel& fk_model, const BodySphereVector& body_spheres) 55 | : fk_model_(fk_model), body_spheres_(body_spheres) {} 56 | 57 | /// Default destructor 58 | virtual ~RobotModel() {} 59 | 60 | /// given pose in configuration space, solve sphere center vector in work 61 | /// space with optional associated jacobian of pose 62 | void sphereCenters(const Pose& jp, std::vector& sph_centers, 63 | gtsam::OptionalMatrixVecType J_point_conf = nullptr) const; 64 | 65 | /// given pose in configuration space, solve a single sphere center in work 66 | /// space for fast call of a single sphere with optional associated jacobian 67 | /// of pose 68 | gtsam::Point3 sphereCenter( 69 | size_t sph_idx, const Pose& jp, 70 | gtsam::OptionalMatrixType J_point_conf = nullptr) const; 71 | 72 | /// given pose in configuration space, solve sphere center vector in work 73 | /// space matlab version, no jacobians output matrix 3 * nr_sphere 74 | gtsam::Matrix sphereCentersMat(const Pose& jp) const; 75 | 76 | /// accesses from fk_model 77 | const FKModel& fk_model() const { return fk_model_; } 78 | size_t dof() const { return fk_model_.dof(); } 79 | 80 | /// accesses 81 | size_t nr_body_spheres() const { return body_spheres_.size(); } 82 | size_t sphere_link_id(size_t i) const { return body_spheres_[i].link_id; } 83 | double sphere_radius(size_t i) const { return body_spheres_[i].radius; } 84 | const gtsam::Point3& sphere_center_wrt_link(size_t i) const { 85 | return body_spheres_[i].center; 86 | } 87 | }; 88 | 89 | } // namespace gpmp2 90 | 91 | #include 92 | -------------------------------------------------------------------------------- /gpmp2/kinematics/mobileBaseUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mobileBaseUtils.h 3 | * @brief Some utilities for mobile robot base 4 | * @author Jing Dong 5 | * @date Aug 18, 2017 6 | **/ 7 | 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace gtsam; 13 | 14 | namespace gpmp2 { 15 | 16 | /* ************************************************************************** */ 17 | gtsam::Pose3 computeBasePose3(const gtsam::Pose2& base_pose2, 18 | gtsam::OptionalJacobian<6, 3> J = {}) { 19 | if (J) { 20 | J->setZero(); 21 | const gtsam::Matrix3 Hzrot3 = 22 | gtsam::Rot3::ExpmapDerivative(gtsam::Vector3(0, 0, base_pose2.theta())); 23 | J->block<3, 1>(0, 2) = Hzrot3.col(2); 24 | J->block<2, 2>(3, 0) = gtsam::Matrix2::Identity(); 25 | } 26 | return gtsam::Pose3( 27 | gtsam::Rot3::Rodrigues(gtsam::Vector3(0, 0, base_pose2.theta())), 28 | gtsam::Point3(base_pose2.x(), base_pose2.y(), 0.0)); 29 | } 30 | 31 | /* ************************************************************************** */ 32 | gtsam::Pose3 computeBaseTransPose3(const gtsam::Pose2& base_pose2, 33 | const gtsam::Pose3& base_T_trans, 34 | gtsam::OptionalJacobian<6, 3> J = {}) { 35 | if (J) { 36 | gtsam::Matrix63 Hbasep3; 37 | const gtsam::Pose3 base_pose3 = computeBasePose3(base_pose2, Hbasep3); 38 | gtsam::Matrix6 Hcomp; 39 | const gtsam::Pose3 arm_base = base_pose3.compose(base_T_trans, Hcomp); 40 | *J = Hcomp * Hbasep3; 41 | return arm_base; 42 | } else { 43 | return computeBasePose3(base_pose2).compose(base_T_trans); 44 | } 45 | } 46 | 47 | /* ************************************************************************** */ 48 | gtsam::Pose3 liftBasePose3(const gtsam::Pose2& base_pose2, double lift, 49 | const gtsam::Pose3& base_T_trans, 50 | bool reverse_linact, 51 | gtsam::OptionalJacobian<6, 4> J = {}) { 52 | // a const pose to lift base 53 | Pose3 lift_base_pose; 54 | // Matrix63 Jliftbase_z; 55 | if (reverse_linact) { 56 | lift_base_pose = Pose3::Create(Rot3(), Point3(0, 0, -lift)); 57 | } else { 58 | lift_base_pose = Pose3::Create(Rot3(), Point3(0, 0, lift)); 59 | } 60 | 61 | if (J) { 62 | Matrix63 Harmbase; 63 | const Pose3 armbase = 64 | computeBaseTransPose3(base_pose2, base_T_trans, Harmbase); 65 | Matrix6 Hcomp1, Hcomp2; 66 | const Pose3 armbaselift = lift_base_pose.compose(armbase, Hcomp1, Hcomp2); 67 | // J over pose2 68 | J->block<6, 3>(0, 0) = Hcomp2 * Harmbase; 69 | // J over lift z 70 | // D_lift_base_pose = [0; I], so (Hcomp1 * D_lift_base_pose).col(2) == 71 | // Hcomp1.col(5) 72 | // J->block<6,1>(0,3) = (Hcomp1 * Jliftbase_z).col(2); 73 | if (reverse_linact) 74 | J->block<6, 1>(0, 3) = -Hcomp1.col(5); 75 | else 76 | J->block<6, 1>(0, 3) = Hcomp1.col(5); 77 | return armbaselift; 78 | } else { 79 | return lift_base_pose.compose( 80 | computeBaseTransPose3(base_pose2, base_T_trans)); 81 | } 82 | } 83 | 84 | } // namespace gpmp2 85 | -------------------------------------------------------------------------------- /gpmp2/kinematics/mobileBaseUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mobileBaseUtils.h 3 | * @brief Some utilities for mobile robot base 4 | * @author Jing Dong 5 | * @date Aug 18, 2017 6 | **/ 7 | 8 | #include 9 | #include 10 | 11 | namespace gpmp2 { 12 | 13 | /// convert 2D pose to 3D pose on x-y plane, with jacobian 14 | gtsam::Pose3 computeBasePose3(const gtsam::Pose2& base_pose2, 15 | gtsam::OptionalJacobian<6, 3> J = {}); 16 | 17 | /// compute a pose3 given vehicle base pose2 and transform, with jacobian 18 | gtsam::Pose3 computeBaseTransPose3(const gtsam::Pose2& base_pose2, 19 | const gtsam::Pose3& base_T_trans, 20 | gtsam::OptionalJacobian<6, 3> J = {}); 21 | 22 | /// lift arm base in z direction, with jacobian 23 | gtsam::Pose3 liftBasePose3(const gtsam::Pose2& base_pose2, double lift, 24 | const gtsam::Pose3& base_T_trans, 25 | bool reverse_linact, 26 | gtsam::OptionalJacobian<6, 4> J = {}); 27 | 28 | } // namespace gpmp2 29 | -------------------------------------------------------------------------------- /gpmp2/kinematics/tests/testGaussianPriorWorkspaceOrientation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testGaussianPriorWorkspaceOrientation.cpp 3 | * @author Mustafa Mukadam 4 | **/ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | using namespace std; 20 | using namespace gtsam; 21 | using namespace gpmp2; 22 | 23 | /* ************************************************************************** */ 24 | TEST(GaussianPriorWorkspaceOrientationArm, error) { 25 | Vector2 a(1, 1), alpha(M_PI / 2, 0), d(0, 0); 26 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 27 | Vector2 q; 28 | Rot3 des; 29 | Vector actual, expect; 30 | Matrix H_exp, H_act; 31 | noiseModel::Gaussian::shared_ptr cost_model = 32 | noiseModel::Isotropic::Sigma(3, 1.0); 33 | 34 | q = Vector2(M_PI / 4.0, -M_PI / 2); 35 | des = Rot3(Rot3::RzRyRx(0, 0, -M_PI / 2)); 36 | GaussianPriorWorkspaceOrientationArm factor(0, arm, 1, des, cost_model); 37 | actual = factor.evaluateError(q, &H_act); 38 | expect = Vector3(-0.613943126, 1.48218982, 0.613943126); 39 | H_exp = numericalDerivative11( 40 | std::function( 41 | std::bind(&GaussianPriorWorkspaceOrientationArm::evaluateError, 42 | factor, std::placeholders::_1, nullptr)), 43 | q, 1e-6); 44 | EXPECT(assert_equal(expect, actual, 1e-6)); 45 | EXPECT(assert_equal(H_exp, H_act, 1e-6)); 46 | } 47 | 48 | /* ************************************************************************** */ 49 | TEST(GaussianPriorWorkspaceOrientationArm, optimization) { 50 | noiseModel::Gaussian::shared_ptr cost_model = 51 | noiseModel::Isotropic::Sigma(3, 0.1); 52 | 53 | Vector a = (Vector(2) << 1, 1).finished(); 54 | Vector alpha = (Vector(2) << 0, 0).finished(); 55 | Vector d = (Vector(2) << 0, 0).finished(); 56 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 57 | Rot3 des = Rot3(); 58 | 59 | Key qkey = Symbol('x', 0); 60 | Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); 61 | Vector q = (Vector(2) << 0, 0).finished(); 62 | 63 | NonlinearFactorGraph graph; 64 | graph.add( 65 | GaussianPriorWorkspaceOrientationArm(qkey, arm, 1, des, cost_model)); 66 | Values init_values; 67 | init_values.insert(qkey, qinit); 68 | 69 | LevenbergMarquardtParams parameters; 70 | parameters.setVerbosity("ERROR"); 71 | parameters.setAbsoluteErrorTol(1e-12); 72 | LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters); 73 | optimizer.optimize(); 74 | Values results = optimizer.values(); 75 | 76 | EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3); 77 | EXPECT(assert_equal(q, results.at(qkey), 1e-3)); 78 | } 79 | 80 | /* ************************************************************************** */ 81 | /* main function */ 82 | int main() { 83 | TestResult tr; 84 | return TestRegistry::runAllTests(tr); 85 | } 86 | -------------------------------------------------------------------------------- /gpmp2/kinematics/tests/testGaussianPriorWorkspacePose.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testGaussianPriorWorkspacePose.cpp 3 | * @author Mustafa Mukadam 4 | **/ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | using namespace std; 20 | using namespace gtsam; 21 | using namespace gpmp2; 22 | 23 | /* ************************************************************************** */ 24 | TEST(GaussianPriorWorkspacePoseArm, error) { 25 | Vector2 a(1, 1), alpha(M_PI / 2, 0), d(0, 0); 26 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 27 | Vector2 q; 28 | Pose3 des; 29 | Vector actual, expect; 30 | Matrix H_exp, H_act; 31 | noiseModel::Gaussian::shared_ptr cost_model = 32 | noiseModel::Isotropic::Sigma(6, 1.0); 33 | 34 | q = Vector2(M_PI / 4.0, -M_PI / 2); 35 | des = Pose3(); 36 | GaussianPriorWorkspacePoseArm factor(0, arm, 1, des, cost_model); 37 | actual = factor.evaluateError(q, &H_act); 38 | expect = (Vector(6) << 0.613943126, 1.48218982, -0.613943126, 1.1609828, 39 | 0.706727485, -0.547039678) 40 | .finished(); 41 | H_exp = 42 | numericalDerivative11(std::function(std::bind( 43 | &GaussianPriorWorkspacePoseArm::evaluateError, 44 | factor, std::placeholders::_1, nullptr)), 45 | q, 1e-6); 46 | EXPECT(assert_equal(expect, actual, 1e-6)); 47 | EXPECT(assert_equal(H_exp, H_act, 1e-6)); 48 | } 49 | 50 | /* ************************************************************************** */ 51 | TEST(GaussianPriorWorkspacePoseArm, optimization) { 52 | noiseModel::Gaussian::shared_ptr cost_model = 53 | noiseModel::Isotropic::Sigma(6, 0.1); 54 | 55 | Vector a = (Vector(2) << 1, 1).finished(); 56 | Vector alpha = (Vector(2) << 0, 0).finished(); 57 | Vector d = (Vector(2) << 0, 0).finished(); 58 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 59 | Pose3 des = Pose3(Rot3(), Point3(2, 0, 0)); 60 | 61 | Key qkey = Symbol('x', 0); 62 | Vector q = (Vector(2) << 0, 0).finished(); 63 | Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); 64 | 65 | NonlinearFactorGraph graph; 66 | graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model)); 67 | Values init_values; 68 | init_values.insert(qkey, qinit); 69 | 70 | LevenbergMarquardtParams parameters; 71 | parameters.setVerbosity("ERROR"); 72 | parameters.setAbsoluteErrorTol(1e-12); 73 | LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters); 74 | optimizer.optimize(); 75 | Values results = optimizer.values(); 76 | 77 | EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3); 78 | EXPECT(assert_equal(q, results.at(qkey), 1e-3)); 79 | } 80 | 81 | /* ************************************************************************** */ 82 | /* main function */ 83 | int main() { 84 | TestResult tr; 85 | return TestRegistry::runAllTests(tr); 86 | } 87 | -------------------------------------------------------------------------------- /gpmp2/obstacle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB obstacle_headers "*.h") 3 | install(FILES ${obstacle_headers} DESTINATION include/gpmp2/obstacle) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(obstacle "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleCost.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleCost.h 3 | * @brief obstacle cost functions, implement hinge loss function 4 | * @author Jing Dong 5 | * @date May 9, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace gpmp2 { 21 | 22 | /// hinge loss obstacle cost function 23 | inline double hingeLossObstacleCost( 24 | const gtsam::Point3& point, const SignedDistanceField& sdf, double eps, 25 | gtsam::OptionalJacobian<1, 3> H_point = nullptr) { 26 | gtsam::Vector3 field_gradient; 27 | double dist_signed; 28 | try { 29 | dist_signed = sdf.getSignedDistance(point, field_gradient); 30 | } catch (SDFQueryOutOfRange&) { 31 | // std::cout << "[hingeLossObstacleCost] WARNING: querying signed distance 32 | // out of range, " 33 | // "assume zero obstacle cost." << std::endl; 34 | if (H_point) *H_point = gtsam::Matrix13::Zero(); 35 | return 0.0; 36 | } 37 | 38 | if (dist_signed > eps) { 39 | // faraway no error 40 | if (H_point) *H_point = gtsam::Matrix13::Zero(); 41 | return 0.0; 42 | 43 | } else { 44 | // outside but < eps or inside object 45 | if (H_point) *H_point = -field_gradient.transpose(); 46 | return eps - dist_signed; 47 | } 48 | } 49 | 50 | /// hinge loss obstacle cost function, planar version 51 | inline double hingeLossObstacleCost( 52 | const gtsam::Point2& point, const PlanarSDF& sdf, double eps, 53 | gtsam::OptionalJacobian<1, 2> H_point = nullptr) { 54 | gtsam::Vector2 field_gradient; 55 | double dist_signed; 56 | try { 57 | dist_signed = sdf.getSignedDistance(point, field_gradient); 58 | } catch (SDFQueryOutOfRange&) { 59 | // std::cout << "[hingeLossObstacleCost] WARNING: querying signed distance 60 | // out of range, " 61 | // "assume zero obstacle cost." << std::endl; 62 | if (H_point) *H_point = gtsam::Matrix12::Zero(); 63 | return 0.0; 64 | } 65 | 66 | if (dist_signed > eps) { 67 | // faraway no error 68 | if (H_point) *H_point = gtsam::Matrix12::Zero(); 69 | return 0.0; 70 | 71 | } else { 72 | // outside but < eps or inside object 73 | if (H_point) *H_point = -field_gradient.transpose(); 74 | return eps - dist_signed; 75 | } 76 | } 77 | 78 | } // namespace gpmp2 79 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactor-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactor-inl.h 3 | * @brief Obstacle avoidance cost factor, using planar signed distance field 4 | * @author Jing Dong 5 | * @date Nov 15, 2015 6 | **/ 7 | 8 | #include 9 | 10 | using namespace std; 11 | using namespace gtsam; 12 | 13 | namespace gpmp2 { 14 | 15 | /* ************************************************************************** */ 16 | template 17 | gtsam::Vector ObstaclePlanarSDFFactor::evaluateError( 18 | const Pose& conf, gtsam::OptionalMatrixType H1) const { 19 | // if Jacobians used, initialize as zeros 20 | // size: arm_nr_points_ * DOF 21 | if (H1) *H1 = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 22 | 23 | // run forward kinematics of this configuration 24 | vector sph_centers; 25 | vector J_px_jp; 26 | if (H1) 27 | robot_.sphereCenters(conf, sph_centers, &J_px_jp); 28 | else 29 | robot_.sphereCenters(conf, sph_centers); 30 | 31 | // allocate cost vector 32 | Vector err(robot_.nr_body_spheres()); 33 | 34 | // for each point on arm stick, get error 35 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 36 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 37 | 38 | if (H1) { 39 | Matrix12 Jerr_point; 40 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), 41 | sph_centers[sph_idx].y()); 42 | err(sph_idx) = 43 | hingeLossObstacleCost(sph_center_2d, sdf_, total_eps, Jerr_point); 44 | 45 | // chain rules 46 | H1->row(sph_idx) = Jerr_point * J_px_jp[sph_idx].topRows<2>(); 47 | 48 | } else { 49 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), 50 | sph_centers[sph_idx].y()); 51 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps); 52 | } 53 | } 54 | 55 | return err; 56 | } 57 | 58 | } // namespace gpmp2 59 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactor.h 3 | * @brief Obstacle avoidance cost factor, using planar signed distance field 4 | * @author Jing Dong 5 | * @date Nov 15, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * unary factor for obstacle avoidance, planar version 23 | * template robot model version 24 | */ 25 | template 26 | class ObstaclePlanarSDFFactor 27 | : public gtsam::NoiseModelFactorN { 28 | public: 29 | // typedefs 30 | using Robot = ROBOT; 31 | using Pose = typename Robot::Pose; 32 | 33 | private: 34 | // typedefs 35 | typedef ObstaclePlanarSDFFactor This; 36 | typedef gtsam::NoiseModelFactorN Base; 37 | 38 | // obstacle cost settings 39 | double epsilon_; // distance from object that start non-zero cost 40 | 41 | // arm: planar one, all alpha = 0 42 | Robot robot_; 43 | 44 | // signed distance field from matlab 45 | PlanarSDF sdf_; 46 | 47 | public: 48 | /// shorthand for a smart pointer to a factor 49 | typedef std::shared_ptr shared_ptr; 50 | 51 | /** Default constructor */ 52 | ObstaclePlanarSDFFactor() {} 53 | 54 | /** 55 | * Constructor 56 | * @param cost_model cost function covariance, should to identity model 57 | * @param field signed distance field 58 | * @param nn_index nearest neighbour index of signed distance field 59 | */ 60 | ObstaclePlanarSDFFactor(gtsam::Key poseKey, const Robot& robot, 61 | const PlanarSDF& sdf, double cost_sigma, 62 | double epsilon) 63 | : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nr_body_spheres(), 64 | cost_sigma), 65 | poseKey), 66 | epsilon_(epsilon), 67 | robot_(robot), 68 | sdf_(sdf) { 69 | // TODO: check robot is planar 70 | } 71 | 72 | virtual ~ObstaclePlanarSDFFactor() {} 73 | 74 | /// error function 75 | /// numerical jacobians / analytic jacobians from cost function 76 | gtsam::Vector evaluateError( 77 | const Pose& conf, gtsam::OptionalMatrixType H1 = nullptr) const override; 78 | 79 | /// @return a deep copy of this factor 80 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 81 | return std::static_pointer_cast( 82 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 83 | } 84 | 85 | /** print contents */ 86 | void print(const std::string& s = "", 87 | const gtsam::KeyFormatter& keyFormatter = 88 | gtsam::DefaultKeyFormatter) const { 89 | std::cout << s << "ObstaclePlanarSDFFactor :" << std::endl; 90 | Base::print("", keyFormatter); 91 | } 92 | 93 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 94 | /** Serialization function */ 95 | friend class boost::serialization::access; 96 | template 97 | void serialize(ARCHIVE& ar, const unsigned int version) { 98 | ar& boost::serialization::make_nvp( 99 | "NoiseModelFactor1", boost::serialization::base_object(*this)); 100 | } 101 | #endif 102 | }; 103 | 104 | } // namespace gpmp2 105 | 106 | #include 107 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar and signed distance 4 | *field 5 | * @author Jing Dong 6 | * @date May 29, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef ObstaclePlanarSDFFactor ObstaclePlanarSDFFactorArm; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGP-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPlanarArmGP-inl.h 3 | * @brief Obstacle avoidance cost factor, for 2D arm, using GP and signed 4 | *distance field 5 | * @author Jing Dong 6 | * @date Nov 15, 2015 7 | **/ 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace gtsam; 14 | 15 | namespace gpmp2 { 16 | 17 | /* ************************************************************************** */ 18 | template 19 | gtsam::Vector ObstaclePlanarSDFFactorGP::evaluateError( 20 | const Pose& conf1, const Velocity& vel1, const Pose& conf2, 21 | const Velocity& vel2, gtsam::OptionalMatrixType H1, 22 | gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, 23 | gtsam::OptionalMatrixType H4) const { 24 | const bool use_H = (H1 || H2 || H3 || H4); 25 | 26 | // if Jacobians used, initialize Jerr_conf as zeros 27 | // size: arm_nr_points_ * DOF 28 | Matrix Jerr_conf = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 29 | 30 | // get conf by interpolation, except last pose 31 | typename Robot::Pose conf; 32 | Matrix Jconf_c1, Jconf_c2, Jconf_v1, Jconf_v2; 33 | if (use_H) 34 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2, Jconf_c1, Jconf_v1, 35 | Jconf_c2, Jconf_v2); 36 | else 37 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2); 38 | 39 | // run forward kinematics of this configuration 40 | vector sph_centers; 41 | vector J_px_jp; 42 | if (H1) 43 | robot_.sphereCenters(conf, sph_centers, &J_px_jp); 44 | else 45 | robot_.sphereCenters(conf, sph_centers); 46 | 47 | // allocate cost vector 48 | Vector err(robot_.nr_body_spheres()); 49 | 50 | // for each point on arm stick, get error 51 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 52 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 53 | 54 | if (H1) { 55 | Matrix12 Jerr_point; 56 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), 57 | sph_centers[sph_idx].y()); 58 | err(sph_idx) = 59 | hingeLossObstacleCost(sph_center_2d, sdf_, total_eps, Jerr_point); 60 | 61 | // chain rules 62 | Jerr_conf.row(sph_idx) = Jerr_point * J_px_jp[sph_idx].topRows<2>(); 63 | 64 | } else { 65 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), 66 | sph_centers[sph_idx].y()); 67 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps); 68 | } 69 | } 70 | 71 | // update jacobians 72 | if (use_H) 73 | GPBase::updatePoseJacobians(Jerr_conf, Jconf_c1, Jconf_v1, Jconf_c2, 74 | Jconf_v2, H1, H2, H3, H4); 75 | 76 | return err; 77 | } 78 | 79 | } // namespace gpmp2 80 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar, linear GP and 4 | *signed distance field 5 | * @author Jing Dong 6 | * @date May 29, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template use ArmModel as robot type 18 | typedef ObstaclePlanarSDFFactorGP 19 | ObstaclePlanarSDFFactorGPArm; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPointRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPointRobot.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed 4 | *distance field, and GP interpolation 5 | * @author Mustafa Mukadam 6 | * @date July 20, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template uses PointRobotModel as robot type 18 | typedef ObstaclePlanarSDFFactorGP 20 | ObstaclePlanarSDFFactorGPPointRobot; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPose2Mobile2Arms.h 3 | * @brief Obstacle avoidance cost factor, for mobile robot with 2 arm 4 | * @author Jing Dong 5 | * @date Aug 20, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses PointRobotModel as robot type 17 | typedef ObstaclePlanarSDFFactorGP 19 | ObstaclePlanarSDFFactorGPPose2Mobile2Arms; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPose2MobileArm.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed 4 | *distance field, and GP interpolation 5 | * @author Jing Dong 6 | * @date Oct 14, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template uses PointRobotModel as robot type 18 | typedef ObstaclePlanarSDFFactorGP 20 | ObstaclePlanarSDFFactorGPPose2MobileArm; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPose2MobileBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPose2MobileBase.h 3 | * @brief Obstacle avoidance interpolated GP factor for 2D SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Jan 23, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses PointRobotModel as robot type 17 | typedef ObstaclePlanarSDFFactorGP 19 | ObstaclePlanarSDFFactorGPPose2MobileBase; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPointRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPointRobot.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed 4 | *distance field 5 | * @author Mustafa Mukadam 6 | * @date July 20, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses PointRobotModel as robot type 17 | typedef ObstaclePlanarSDFFactor 18 | ObstaclePlanarSDFFactorPointRobot; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPose2Mobile2Arms.h 3 | * @brief Obstacle avoidance cost factor, for mobile robot with 2 arms 4 | * @author Jing Dong 5 | * @date Aug 20, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use ArmModel as robot type 16 | typedef ObstaclePlanarSDFFactor 17 | ObstaclePlanarSDFFactorPose2Mobile2Arms; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPose2MobileArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar and signed distance 4 | *field 5 | * @author Jing Dong 6 | * @date Oct 13, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef ObstaclePlanarSDFFactor 18 | ObstaclePlanarSDFFactorPose2MobileArm; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPose2MobileBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPose2MobileBase.h 3 | * @brief Obstacle avoidance cost factor for 2D SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Jan 23, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use BaseModel as robot type 16 | typedef ObstaclePlanarSDFFactor 17 | ObstaclePlanarSDFFactorPose2MobileBase; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactor-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactor-inl.h 3 | * @brief Obstacle avoidance cost factor, using 3D signed distance field 4 | * @author Jing Dong 5 | * @date May 11, 2016 6 | **/ 7 | 8 | #include 9 | 10 | using namespace std; 11 | using namespace gtsam; 12 | 13 | namespace gpmp2 { 14 | 15 | /* ************************************************************************** */ 16 | template 17 | gtsam::Vector ObstacleSDFFactor::evaluateError( 18 | const typename Robot::Pose& conf, gtsam::OptionalMatrixType H1) const { 19 | // if Jacobians used, initialize as zeros 20 | // size: arm_nr_points_ * DOF 21 | if (H1) *H1 = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 22 | 23 | // run forward kinematics of this configuration 24 | vector sph_centers; 25 | vector J_px_jp; 26 | if (H1) 27 | robot_.sphereCenters(conf, sph_centers, &J_px_jp); 28 | else 29 | robot_.sphereCenters(conf, sph_centers); 30 | 31 | // allocate cost vector 32 | Vector err(robot_.nr_body_spheres()); 33 | 34 | // for each point on arm stick, get error 35 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 36 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 37 | 38 | if (H1) { 39 | Matrix13 Jerr_point; 40 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, 41 | total_eps, Jerr_point); 42 | 43 | // chain rules 44 | H1->row(sph_idx) = Jerr_point * J_px_jp[sph_idx]; 45 | 46 | } else { 47 | err(sph_idx) = 48 | hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps); 49 | } 50 | } 51 | 52 | return err; 53 | } 54 | 55 | } // namespace gpmp2 56 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactor.h 3 | * @brief Obstacle avoidance cost factor, using 3D signed distance field 4 | * @author Jing Dong 5 | * @date May 11, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * unary factor for obstacle avoidance 23 | * template robot model version 24 | */ 25 | template 26 | class ObstacleSDFFactor 27 | : public gtsam::NoiseModelFactorN { 28 | public: 29 | // typedefs 30 | typedef ROBOT Robot; 31 | typedef typename Robot::Pose Pose; 32 | 33 | private: 34 | // typedefs 35 | typedef ObstacleSDFFactor This; 36 | typedef gtsam::NoiseModelFactorN Base; 37 | 38 | // obstacle cost settings 39 | double epsilon_; // distance from object that start non-zero cost 40 | 41 | // arm: planar one, all alpha = 0 42 | const Robot& robot_; 43 | 44 | // signed distance field from matlab 45 | const SignedDistanceField& sdf_; 46 | 47 | public: 48 | /// shorthand for a smart pointer to a factor 49 | typedef std::shared_ptr shared_ptr; 50 | 51 | /* Default constructor */ 52 | ObstacleSDFFactor() {} 53 | 54 | /** 55 | * Constructor 56 | * @param cost_model cost function covariance, should to identity model 57 | * @param field signed distance field 58 | * @param nn_index nearest neighbour index of signed distance field 59 | */ 60 | ObstacleSDFFactor(gtsam::Key poseKey, const Robot& robot, 61 | const SignedDistanceField& sdf, double cost_sigma, 62 | double epsilon) 63 | : Base(gtsam::noiseModel::Isotropic::Sigma(robot.nr_body_spheres(), 64 | cost_sigma), 65 | poseKey), 66 | epsilon_(epsilon), 67 | robot_(robot), 68 | sdf_(sdf) {} 69 | 70 | virtual ~ObstacleSDFFactor() {} 71 | 72 | /// error function 73 | /// numerical jacobians / analytic jacobians from cost function 74 | gtsam::Vector evaluateError( 75 | const typename Robot::Pose& conf, 76 | gtsam::OptionalMatrixType H1 = nullptr) const override; 77 | 78 | /// @return a deep copy of this factor 79 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 80 | return std::static_pointer_cast( 81 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); 82 | } 83 | 84 | /** print contents */ 85 | void print(const std::string& s = "", 86 | const gtsam::KeyFormatter& keyFormatter = 87 | gtsam::DefaultKeyFormatter) const { 88 | std::cout << s << "ObstacleSDFFactor :" << std::endl; 89 | Base::print("", keyFormatter); 90 | } 91 | 92 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 93 | /** Serialization function */ 94 | friend class boost::serialization::access; 95 | template 96 | void serialize(ARCHIVE& ar, const unsigned int version) { 97 | ar& boost::serialization::make_nvp( 98 | "NoiseModelFactor1", boost::serialization::base_object(*this)); 99 | } 100 | #endif 101 | }; 102 | 103 | } // namespace gpmp2 104 | 105 | #include 106 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm and signed distance field 4 | * @author Jing Dong 5 | * @date May 29, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use ArmModel as robot type 16 | typedef ObstacleSDFFactor ObstacleSDFFactorArm; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGP-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGP-inl.h 3 | * @brief Obstacle avoidance cost factor, for general arm, using GP and signed 4 | *distance field 5 | * @author Jing Dong 6 | * @date Dec 4, 2015 7 | **/ 8 | 9 | #include 10 | 11 | using namespace std; 12 | using namespace gtsam; 13 | 14 | namespace gpmp2 { 15 | 16 | /* ************************************************************************** */ 17 | template 18 | gtsam::Vector ObstacleSDFFactorGP::evaluateError( 19 | const typename Robot::Pose& conf1, const typename Robot::Velocity& vel1, 20 | const typename Robot::Pose& conf2, const typename Robot::Velocity& vel2, 21 | gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, 22 | gtsam::OptionalMatrixType H3, gtsam::OptionalMatrixType H4) const { 23 | const bool use_H = (H1 || H2 || H3 || H4); 24 | 25 | // if Jacobians used, initialize Jerr_conf as zeros 26 | // size: arm_nr_points_ * DOF 27 | Matrix Jerr_conf = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 28 | 29 | // get conf by interpolation, except last pose 30 | typename Robot::Pose conf; 31 | Matrix Jconf_c1, Jconf_c2, Jconf_v1, Jconf_v2; 32 | if (use_H) 33 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2, Jconf_c1, Jconf_v1, 34 | Jconf_c2, Jconf_v2); 35 | else 36 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2); 37 | 38 | // run forward kinematics of this configuration 39 | vector sph_centers; 40 | vector J_px_jp; 41 | if (H1) 42 | robot_.sphereCenters(conf, sph_centers, &J_px_jp); 43 | else 44 | robot_.sphereCenters(conf, sph_centers); 45 | 46 | // allocate cost vector 47 | Vector err(robot_.nr_body_spheres()); 48 | 49 | // for each point on arm stick, get error 50 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 51 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 52 | 53 | if (H1) { 54 | Matrix13 Jerr_point; 55 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, 56 | total_eps, Jerr_point); 57 | 58 | // chain rules 59 | Jerr_conf.row(sph_idx) = Jerr_point * J_px_jp[sph_idx]; 60 | 61 | } else { 62 | err(sph_idx) = 63 | hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps); 64 | } 65 | } 66 | 67 | // update jacobians 68 | if (use_H) 69 | GPBase::updatePoseJacobians(Jerr_conf, Jconf_c1, Jconf_v1, Jconf_c2, 70 | Jconf_v2, H1, H2, H3, H4); 71 | 72 | return err; 73 | } 74 | 75 | } // namespace gpmp2 76 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar, linear GP and 4 | *signed distance field 5 | * @author Jing Dong 6 | * @date May 29, 2016 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template use ArmModel as robot type 18 | typedef ObstacleSDFFactorGP 19 | ObstacleSDFFactorGPArm; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2Mobile2Arms.h 3 | * @brief Obstacle avoidance interpolated GP factor for 2 X 3D arms with SE(2) 4 | *base 5 | * @author Mustafa Mukadam 6 | * @date Sep 4, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template uses Pose2Mobile2ArmsModel as robot type 18 | typedef ObstacleSDFFactorGP 20 | ObstacleSDFFactorGPPose2Mobile2Arms; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2MobileArm.h 3 | * @brief Obstacle avoidance interpolated GP factor for 3D arm with SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Nov 2, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses Pose2MobileArmModel as robot type 17 | typedef ObstacleSDFFactorGP 19 | ObstacleSDFFactorGPPose2MobileArm; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2MobileBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2MobileBase.h 3 | * @brief Obstacle avoidance interpolated GP factor for 3D SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Jan 23, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses Pose2MobileBaseModel as robot type 17 | typedef ObstacleSDFFactorGP 19 | ObstacleSDFFactorGPPose2MobileBase; 20 | 21 | } // namespace gpmp2 22 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2MobileVetLin2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2MobileVetLin2Arms.h 3 | * @brief Obstacle avoidance interpolated GP factor for 2 X 3D arms 4 | * with SE(2) base and linear actuator 5 | * @author Mustafa Mukadam 6 | * @date Sep 4, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template uses Pose2MobileVetLin2ArmsModel as robot type 18 | typedef ObstacleSDFFactorGP 20 | ObstacleSDFFactorGPPose2MobileVetLin2Arms; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2MobileVetLinArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2MobileVetLinArm.h 3 | * @brief Obstacle avoidance interpolated GP factor for 3D arm 4 | * with SE(2) base and linear actuator 5 | * @author Mustafa Mukadam 6 | * @date Sep 4, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | // template uses Pose2MobileVetLinArmModel as robot type 18 | typedef ObstacleSDFFactorGP 20 | ObstacleSDFFactorGPPose2MobileVetLinArm; 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorPose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPose2Mobile2Arms.h 3 | * @brief Obstacle avoidance cost factor for 2 X 3D arms with SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Sep 4, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use Pose2Mobile2ArmsModel as robot type 16 | typedef ObstacleSDFFactor 17 | ObstacleSDFFactorPose2Mobile2Arms; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPose2MobileArm.h 3 | * @brief Obstacle avoidance cost factor for 3D arm with SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Nov 2, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use Pose2MobileArmModel as robot type 16 | typedef ObstacleSDFFactor ObstacleSDFFactorPose2MobileArm; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorPose2MobileBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPose2MobileBase.h 3 | * @brief Obstacle avoidance cost factor for 3D SE(2) base 4 | * @author Mustafa Mukadam 5 | * @date Jan 23, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use Pose2MobileBaseModel as robot type 16 | typedef ObstacleSDFFactor 17 | ObstacleSDFFactorPose2MobileBase; 18 | 19 | } // namespace gpmp2 20 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorPose2MobileVetLin2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPose2MobileVetLin2Arms.h 3 | * @brief Obstacle avoidance cost factor for 2 X 3D arms 4 | * with SE(2) base and linear actuator 5 | * @author Mustafa Mukadam 6 | * @date Sep 4, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use Pose2MobileVetLin2ArmsModel as robot type 17 | typedef ObstacleSDFFactor 18 | ObstacleSDFFactorPose2MobileVetLin2Arms; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorPose2MobileVetLinArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPose2MobileVetLinArm.h 3 | * @brief Obstacle avoidance cost factor for 3D arm 4 | * with SE(2) base and linear actuator 5 | * @author Mustafa Mukadam 6 | * @date Sep 4, 2017 7 | **/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use Pose2MobileVetLinArmModel as robot type 17 | typedef ObstacleSDFFactor 18 | ObstacleSDFFactorPose2MobileVetLinArm; 19 | 20 | } // namespace gpmp2 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/SDFexception.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SDFexception.h 3 | * @brief custom exceptions for signed distance field 4 | * @author Jing Dong 5 | * @date May 26, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | /// query out of range exception 17 | class GPMP2_EXPORT SDFQueryOutOfRange : public std::runtime_error { 18 | public: 19 | /// constructor 20 | SDFQueryOutOfRange() : std::runtime_error("Querying SDF out of range") {} 21 | }; 22 | 23 | } // namespace gpmp2 24 | -------------------------------------------------------------------------------- /gpmp2/obstacle/SelfCollisionArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SelfCollisionArm.h 3 | * @brief Self collision cost factor for Arm 4 | * @author Mustafa Mukadam 5 | * @date Sep 22, 2020 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | // template use ArmModel as robot type 16 | typedef SelfCollision SelfCollisionArm; 17 | 18 | } // namespace gpmp2 19 | -------------------------------------------------------------------------------- /gpmp2/obstacle/SignedDistanceField.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @File SignedDistanceField.h 3 | * @brief util functions for signed distance field 4 | * @author Mustafa Mukadam 5 | * @date May 4, 2017 6 | **/ 7 | 8 | #include 9 | 10 | namespace gpmp2 { 11 | 12 | /* ************************************************************************** */ 13 | void SignedDistanceField::saveSDF(const std::string filename) { 14 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 15 | std::ofstream ofs(filename.c_str()); 16 | assert(ofs.good()); 17 | std::string fext = filename.substr(filename.find_last_of(".") + 1); 18 | if (fext == "xml") { 19 | boost::archive::xml_oarchive oa(ofs); 20 | oa << BOOST_SERIALIZATION_NVP(*this); 21 | } else if (fext == "bin") { 22 | boost::archive::binary_oarchive oa(ofs); 23 | oa << *this; 24 | } else { 25 | boost::archive::text_oarchive oa(ofs); 26 | oa << *this; 27 | } 28 | #else 29 | std::cerr << "Cannot save SDF since Boost serialization is disabled" 30 | << std::endl; 31 | #endif 32 | } 33 | 34 | /* ************************************************************************** */ 35 | void SignedDistanceField::loadSDF(const std::string filename) { 36 | #ifdef GPMP2_ENABLE_BOOST_SERIALIZATION 37 | std::ifstream ifs(filename.c_str()); 38 | if (!ifs.good()) 39 | std::cout << "File \'" << filename << "\' does not exist!" << std::endl; 40 | std::string fext = filename.substr(filename.find_last_of(".") + 1); 41 | if (fext == "xml") { 42 | boost::archive::xml_iarchive ia(ifs); 43 | ia >> BOOST_SERIALIZATION_NVP(*this); 44 | } else if (fext == "bin") { 45 | boost::archive::binary_iarchive ia(ifs); 46 | ia >> *this; 47 | } else { 48 | boost::archive::text_iarchive ia(ifs); 49 | ia >> *this; 50 | } 51 | #else 52 | std::cerr << "Cannot load SDF since Boost serialization is disabled" 53 | << std::endl; 54 | #endif 55 | } 56 | 57 | } // namespace gpmp2 58 | -------------------------------------------------------------------------------- /gpmp2/obstacle/tests/testPlanarSDF.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testPlanarSDFutils.cpp 3 | * @author Jing Dong 4 | * @date May 8 2016 5 | **/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | using namespace std; 15 | using namespace gtsam; 16 | using namespace gpmp2; 17 | 18 | double sdf_wrapper(const PlanarSDF& field, const Point2& p) { 19 | return field.getSignedDistance(p); 20 | } 21 | 22 | /* ************************************************************************** */ 23 | TEST(PlanarSDFutils, test1) { 24 | // data 25 | Matrix data; 26 | data = (Matrix(5, 5) << 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, // 27 | 1.4142, 1, 1, 1, 1.4142, // 28 | 1.4142, 1, 1, 1, 1.4142, // 29 | 1.4142, 1, 1, 1, 1.4142, // 30 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321) 31 | .finished(); 32 | Point2 origin(-0.2, -0.2); 33 | double cell_size = 0.1; 34 | 35 | // constructor 36 | PlanarSDF field(origin, cell_size, data); 37 | EXPECT_LONGS_EQUAL(5, field.x_count()); 38 | EXPECT_LONGS_EQUAL(5, field.y_count()); 39 | EXPECT_DOUBLES_EQUAL(0.1, field.cell_size(), 1e-9); 40 | EXPECT(assert_equal(origin, field.origin())); 41 | 42 | // access 43 | PlanarSDF::float_index idx; 44 | idx = field.convertPoint2toCell(Point2(0, 0)); 45 | EXPECT_DOUBLES_EQUAL(2, std::get<0>(idx), 1e-9); 46 | EXPECT_DOUBLES_EQUAL(2, std::get<1>(idx), 1e-9); 47 | EXPECT_DOUBLES_EQUAL(1, field.signed_distance(idx), 1e-9) 48 | 49 | idx = field.convertPoint2toCell( 50 | Point2(0.18, -0.17)); // tri-linear interpolation 51 | EXPECT_DOUBLES_EQUAL(0.3, std::get<0>(idx), 1e-9); 52 | EXPECT_DOUBLES_EQUAL(3.8, std::get<1>(idx), 1e-9); 53 | EXPECT_DOUBLES_EQUAL(1.567372, field.signed_distance(idx), 1e-9) 54 | 55 | idx = std::make_tuple(1.0, 2.0); 56 | EXPECT(assert_equal(Point2(0.0, -0.1), field.convertCelltoPoint2(idx))); 57 | 58 | // gradient 59 | Vector2 grad_act, grad_exp; 60 | Point2 p; 61 | p = Point2(-0.13, -0.14); 62 | field.getSignedDistance(p, grad_act); 63 | grad_exp = 64 | numericalDerivative11(std::function(std::bind( 65 | sdf_wrapper, field, std::placeholders::_1)), 66 | p, 1e-6); 67 | EXPECT(assert_equal(grad_exp, grad_act, 1e-6)); 68 | 69 | p = Point2(0.18, 0.12); 70 | field.getSignedDistance(p, grad_act); 71 | grad_exp = 72 | numericalDerivative11(std::function(std::bind( 73 | sdf_wrapper, field, std::placeholders::_1)), 74 | p, 1e-6); 75 | EXPECT(assert_equal(grad_exp, grad_act, 1e-6)); 76 | } 77 | 78 | /* ************************************************************************** */ 79 | /* main function */ 80 | int main() { 81 | TestResult tr; 82 | return TestRegistry::runAllTests(tr); 83 | } 84 | -------------------------------------------------------------------------------- /gpmp2/obstacle/tests/testSelfCollision.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testSelfCollisionArm.cpp 3 | * @author Mustafa Mukadam 4 | **/ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace gtsam; 17 | using namespace gpmp2; 18 | 19 | /* ************************************************************************** */ 20 | TEST(SelfCollisionArm, error) { 21 | // 3 link arm 22 | Vector3 a(1, 1, 1), alpha(0, 0, 0), d(0, 0, 0); 23 | Arm abs_arm(3, a, alpha, d); 24 | BodySphereVector body_spheres; 25 | body_spheres.push_back(BodySphere(0, 0.1, Point3(-0.5, 0, 0))); 26 | body_spheres.push_back(BodySphere(1, 0.1, Point3(-0.5, 0, 0))); 27 | body_spheres.push_back(BodySphere(1, 0.1, Point3(0, 0, 0))); 28 | body_spheres.push_back(BodySphere(2, 0.1, Point3(0, 0, 0))); 29 | ArmModel arm(abs_arm, body_spheres); 30 | 31 | Matrix data(2, 4); 32 | // sphere A id, sphere B id, epsilon, sigma 33 | data << 0, 1, 2.0, 0.1, 2, 3, 5.0, 0.1; 34 | SelfCollisionArm factor = SelfCollisionArm(0, arm, data); 35 | 36 | Vector actual, expect; 37 | Matrix H_exp, H_act; 38 | 39 | Vector3 q; 40 | q = Vector3(0.0, M_PI / 2.0, 0.0); 41 | actual = factor.evaluateError(q, &H_act); 42 | expect = (Vector(2) << 1.4928932188134527, 4.2).finished(); 43 | H_exp = 44 | numericalDerivative11(std::function(std::bind( 45 | &SelfCollisionArm::evaluateError, factor, 46 | std::placeholders::_1, nullptr)), 47 | q, 1e-6); 48 | EXPECT(assert_equal(expect, actual, 1e-6)); 49 | EXPECT(assert_equal(H_exp, H_act, 1e-6)); 50 | } 51 | 52 | /* ************************************************************************** */ 53 | /* main function */ 54 | int main() { 55 | TestResult tr; 56 | return TestRegistry::runAllTests(tr); 57 | } 58 | -------------------------------------------------------------------------------- /gpmp2/planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB planner_headers "*.h") 3 | install(FILES ${planner_headers} DESTINATION include/gpmp2/planner) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(planner "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/planner/TrajOptimizerSetting.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file TrajOptimizerSetting.cpp 3 | * @brief settings of trajectory optimizer 4 | * @author Jing Dong, Mustafa Mukadam 5 | * @date May 11, 2015 6 | **/ 7 | 8 | #include 9 | 10 | using namespace gtsam; 11 | 12 | namespace gpmp2 { 13 | 14 | /* ************************************************************************** */ 15 | TrajOptimizerSetting::TrajOptimizerSetting() 16 | : dof(0), 17 | total_step(10), 18 | total_time(1.0), 19 | flag_pos_limit(false), 20 | flag_vel_limit(false), 21 | epsilon(0.2), 22 | cost_sigma(0.1), 23 | obs_check_inter(5), 24 | opt_type(Dogleg), 25 | opt_verbosity(None), 26 | final_iter_no_increase(true), 27 | rel_thresh(1e-2), 28 | max_iter(50) {} 29 | 30 | /* ************************************************************************** */ 31 | TrajOptimizerSetting::TrajOptimizerSetting(size_t system_dof) 32 | : dof(system_dof), 33 | total_step(10), 34 | total_time(1.0), 35 | conf_prior_model(noiseModel::Isotropic::Sigma(system_dof, 0.0001)), 36 | vel_prior_model(noiseModel::Isotropic::Sigma(system_dof, 0.0001)), 37 | flag_pos_limit(false), 38 | flag_vel_limit(false), 39 | joint_pos_limits_up(1e6 * Vector::Ones(system_dof)), 40 | joint_pos_limits_down(-1e6 * Vector::Ones(system_dof)), 41 | vel_limits(1e6 * Vector::Ones(system_dof)), 42 | pos_limit_thresh(0.001 * Vector::Ones(system_dof)), 43 | vel_limit_thresh(0.001 * Vector::Ones(system_dof)), 44 | pos_limit_model(noiseModel::Isotropic::Sigma(system_dof, 0.001)), 45 | vel_limit_model(noiseModel::Isotropic::Sigma(system_dof, 0.001)), 46 | epsilon(0.2), 47 | cost_sigma(0.1), 48 | obs_check_inter(5), 49 | Qc_model(noiseModel::Unit::Create(system_dof)), 50 | opt_type(Dogleg), 51 | opt_verbosity(None), 52 | final_iter_no_increase(true), 53 | rel_thresh(1e-2), 54 | max_iter(50) {} 55 | 56 | /* ************************************************************************** */ 57 | void TrajOptimizerSetting::set_pos_limit_model(const gtsam::Vector& v) { 58 | pos_limit_model = noiseModel::Diagonal::Sigmas(v); 59 | } 60 | 61 | /* ************************************************************************** */ 62 | void TrajOptimizerSetting::set_vel_limit_model(const gtsam::Vector& v) { 63 | vel_limit_model = noiseModel::Diagonal::Sigmas(v); 64 | } 65 | 66 | /* ************************************************************************** */ 67 | void TrajOptimizerSetting::set_conf_prior_model(double sigma) { 68 | conf_prior_model = noiseModel::Isotropic::Sigma(dof, sigma); 69 | } 70 | 71 | /* ************************************************************************** */ 72 | void TrajOptimizerSetting::set_vel_prior_model(double sigma) { 73 | vel_prior_model = noiseModel::Isotropic::Sigma(dof, sigma); 74 | } 75 | 76 | /* ************************************************************************** */ 77 | void TrajOptimizerSetting::set_Qc_model(const Matrix& Qc) { 78 | Qc_model = noiseModel::Gaussian::Covariance(Qc); 79 | } 80 | 81 | } // namespace gpmp2 82 | -------------------------------------------------------------------------------- /gpmp2/planner/tests/testISAM2TrajOptimizer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testISAM2TrajOptimizer.cpp 3 | * @author Jing Dong 4 | * @date May 30, 2016 5 | **/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | using namespace std; 15 | using namespace gtsam; 16 | using namespace gpmp2; 17 | 18 | /* ************************************************************************** */ 19 | TEST(ISAM2TrajOptimizer, constructor) { 20 | // 2 link simple example, with none zero base poses 21 | Vector2 a(1, 1), alpha(0, 0), d(0, 0); 22 | Pose3 base_pose(Rot3(), Point3(2.0, 1.0, -1.0)); 23 | Arm abs_arm(2, a, alpha, d, base_pose); 24 | // body spheres 25 | BodySphereVector body_spheres; 26 | body_spheres.push_back(BodySphere(0, 0.5, Point3(-1.0, 0, 0))); 27 | body_spheres.push_back(BodySphere(0, 0.1, Point3(-0.5, 0, 0))); 28 | body_spheres.push_back(BodySphere(0, 0.1, Point3(0, 0, 0))); 29 | body_spheres.push_back(BodySphere(1, 0.1, Point3(-0.5, 0, 0))); 30 | body_spheres.push_back(BodySphere(1, 0.1, Point3(0, 0, 0))); 31 | ArmModel arm(abs_arm, body_spheres); 32 | 33 | // sdf 34 | vector data; 35 | data.push_back((Matrix(5, 5) << 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 36 | 1.4142, 1, 1, 1, 1.4142, 1.4142, 1, 1, 1, 1.4142, 1.4142, 1, 37 | 1, 1, 1.4142, 1.7321, 1.4142, 1.4142, 1.4142, 1.7321) 38 | .finished()); 39 | data.push_back((Matrix(5, 5) << 1.4142, 1, 1, 1, 1.4142, 1, 0, 0, 0, 1, 1, 0, 40 | 0, 0, 1, 1, 0, 0, 0, 1, 1.4142, 1, 1, 1, 4142) 41 | .finished()); 42 | data.push_back((Matrix(5, 5) << 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 43 | 1.4142, 1, 1, 1, 1.4142, 1.4142, 1, 1, 1, 1.4142, 1.4142, 1, 44 | 1, 1, 1.4142, 1.7321, 1.4142, 1.4142, 1.4142, 1.7321) 45 | .finished()); 46 | Point3 origin(-0.2, -0.2, -0.1); 47 | double cell_size = 0.1; 48 | SignedDistanceField field(origin, cell_size, data); 49 | 50 | // settings 51 | TrajOptimizerSetting setting(2); 52 | 53 | ISAM2TrajOptimizer3DArm isam(arm, field, setting); 54 | } 55 | 56 | /* ************************************************************************** */ 57 | /* main function */ 58 | int main() { 59 | TestResult tr; 60 | return TestRegistry::runAllTests(tr); 61 | } 62 | -------------------------------------------------------------------------------- /gpmp2/planner/tests/testTrajUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testTrajUtils.cpp 3 | * @author Jing Dong 4 | * @date May 30, 2016 5 | **/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | using namespace std; 17 | using namespace gtsam; 18 | using namespace gpmp2; 19 | 20 | /* ************************************************************************** */ 21 | TEST(TrajUtils, interpolateLinear) { 22 | Matrix Qc = 0.01 * Matrix::Identity(2, 2); 23 | noiseModel::Gaussian::shared_ptr Qc_model = 24 | noiseModel::Gaussian::Covariance(Qc); 25 | 26 | // values to be interpolated 27 | Values init_values; 28 | // const velocity vb = 10, dt = 0.1 29 | init_values.insert(Symbol('x', 0), (Vector(2) << 0, 0).finished()); 30 | init_values.insert(Symbol('x', 1), (Vector(2) << 1, 0).finished()); 31 | init_values.insert(Symbol('v', 0), (Vector(2) << 10, 0).finished()); 32 | init_values.insert(Symbol('v', 1), (Vector(2) << 10, 0).finished()); 33 | 34 | // interpolate 5 interval in the middle 35 | Values inter_values = interpolateArmTraj(init_values, Qc_model, 0.1, 4); 36 | 37 | EXPECT(assert_equal((Vector(2) << 0, 0).finished(), 38 | inter_values.at(Symbol('x', 0)), 1e-6)); 39 | EXPECT(assert_equal((Vector(2) << 0.2, 0).finished(), 40 | inter_values.at(Symbol('x', 1)), 1e-6)); 41 | EXPECT(assert_equal((Vector(2) << 0.4, 0).finished(), 42 | inter_values.at(Symbol('x', 2)), 1e-6)); 43 | EXPECT(assert_equal((Vector(2) << 0.6, 0).finished(), 44 | inter_values.at(Symbol('x', 3)), 1e-6)); 45 | EXPECT(assert_equal((Vector(2) << 0.8, 0).finished(), 46 | inter_values.at(Symbol('x', 4)), 1e-6)); 47 | EXPECT(assert_equal((Vector(2) << 1, 0).finished(), 48 | inter_values.at(Symbol('x', 5)), 1e-6)); 49 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 50 | inter_values.at(Symbol('v', 0)), 1e-6)); 51 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 52 | inter_values.at(Symbol('v', 1)), 1e-6)); 53 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 54 | inter_values.at(Symbol('v', 2)), 1e-6)); 55 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 56 | inter_values.at(Symbol('v', 3)), 1e-6)); 57 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 58 | inter_values.at(Symbol('v', 4)), 1e-6)); 59 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), 60 | inter_values.at(Symbol('v', 5)), 1e-6)); 61 | } 62 | 63 | /* ************************************************************************** */ 64 | TEST(TrajUtils, initPose2VectorTrajStraightLine_1) { 65 | Values init_values = initPose2VectorTrajStraightLine( 66 | Pose2(1, 3, M_PI - 0.5), (Vector(2) << 2, 4).finished(), 67 | Pose2(3, 7, -M_PI + 0.5), (Vector(2) << 4, 8).finished(), 5); 68 | 69 | EXPECT(assert_equal( 70 | Pose2Vector(Pose2(1, 3, M_PI - 0.5), (Vector(2) << 2, 4).finished()), 71 | init_values.at(Symbol('x', 0)), 1e-6)); 72 | // EXPECT(assert_equal(Pose2Vector(Pose2(1.4,3.8,M_PI-0.3), (Vector(2) 73 | // << 2.4,4.8).finished()), 74 | // init_values.at(Symbol('x', 1)), 1e-6)); 75 | } 76 | 77 | /* ************************************************************************** */ 78 | /* main function */ 79 | int main() { 80 | TestResult tr; 81 | return TestRegistry::runAllTests(tr); 82 | } 83 | -------------------------------------------------------------------------------- /gpmp2/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB utils_headers "*.h") 3 | install(FILES ${utils_headers} DESTINATION include/gpmp2/utils) 4 | 5 | # Build tests 6 | gtsamAddTestsGlob(utils "tests/*.cpp" "" ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /gpmp2/utils/OpenRAVEutils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OpenRAVEutils.cpp 3 | * @brief utility functions wrapped in OpenRAVE 4 | * @author Jing Dong 5 | * @date Oct 31, 2015 6 | **/ 7 | 8 | #include 9 | #include 10 | 11 | using namespace gtsam; 12 | 13 | namespace gpmp2 { 14 | 15 | /* ************************************************************************** */ 16 | void convertValuesOpenRavePointer(size_t dof, const gtsam::Values& results, 17 | double* pointer, size_t total_step, 18 | const gtsam::Vector& joint_lower_limit, 19 | const gtsam::Vector& joint_upper_limit) { 20 | // check limit dof 21 | if (joint_lower_limit.size() != static_cast(dof) || 22 | joint_upper_limit.size() != static_cast(dof)) 23 | throw std::runtime_error( 24 | "[convertValuesOpenRavePointer] ERROR: Joint limit size is different " 25 | "from DOF."); 26 | 27 | for (size_t i = 0; i <= total_step; i++) { 28 | const Key pose_key = Symbol('x', i); 29 | const Key vel_key = Symbol('v', i); 30 | const Vector& conf = results.at(pose_key); 31 | const Vector& vel = results.at(vel_key); 32 | 33 | // copy memory 34 | for (size_t j = 0; j < dof; j++) { 35 | // check for joint limits 36 | if (conf(j) < joint_lower_limit(j)) 37 | pointer[dof * i + j] = joint_lower_limit(j); 38 | else if (conf(j) > joint_upper_limit(j)) 39 | pointer[dof * i + j] = joint_upper_limit(j); 40 | else 41 | pointer[dof * i + j] = conf(j); 42 | // vel 43 | pointer[dof * (i + total_step + 1) + j] = vel(j); 44 | } 45 | } 46 | } 47 | 48 | /* ************************************************************************** */ 49 | void convertOpenRavePointerValues(size_t dof, gtsam::Values& results, 50 | double* pointer, size_t total_step) { 51 | results.clear(); 52 | 53 | for (size_t i = 0; i <= total_step; i++) { 54 | // key 55 | const Key pose_key = Symbol('x', i); 56 | const Key vel_key = Symbol('v', i); 57 | 58 | // get conf and vel values 59 | Vector conf(dof), vel(dof); 60 | for (size_t j = 0; j < dof; j++) { 61 | conf(j) = pointer[dof * i + j]; 62 | vel(j) = pointer[dof * (i + total_step + 1) + j]; 63 | } 64 | 65 | // insert values 66 | results.insert(pose_key, conf); 67 | results.insert(vel_key, vel); 68 | } 69 | } 70 | 71 | } // namespace gpmp2 72 | -------------------------------------------------------------------------------- /gpmp2/utils/OpenRAVEutils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OpenRAVEutils.h 3 | * @brief utility functions wrapped in OpenRAVE 4 | * @author Jing Dong 5 | * @date Oct 31, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gpmp2 { 18 | 19 | /// convert values into double pointer to vis in openrave plugin 20 | /// the pointer is size (2 x step) x DOF, include both conf pose and vel 21 | /// the pointer must be allocated and free by outside utils (memory not managed 22 | /// here) also joint limits are applied here to make sure the range of output 23 | /// conf 24 | GPMP2_EXPORT void convertValuesOpenRavePointer( 25 | size_t dof, const gtsam::Values& results, double* pointer, 26 | size_t total_step, const gtsam::Vector& joint_lower_limit, 27 | const gtsam::Vector& joint_upper_limit); 28 | 29 | /// convert double pointer in openrave plugin to gtsam values 30 | /// the pointer is size (2 x step) x DOF, include both conf pose and vel 31 | GPMP2_EXPORT void convertOpenRavePointerValues(size_t dof, 32 | gtsam::Values& results, 33 | double* pointer, 34 | size_t total_step); 35 | 36 | } // namespace gpmp2 37 | -------------------------------------------------------------------------------- /gpmp2/utils/fileUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fileUtils.cpp 3 | * @brief utility functions wrapped in matlab 4 | * @author Jing Dong 5 | * @date Jan 28, 2017 6 | **/ 7 | 8 | #include 9 | 10 | using namespace gtsam; 11 | using namespace std; 12 | 13 | namespace gpmp2 { 14 | 15 | /* ************************************************************************** */ 16 | bool readSDFvolfile(const std::string& filename_pre, SignedDistanceField& sdf) { 17 | const string headfilename = filename_pre + ".vol.head"; 18 | ifstream head_file(headfilename.c_str(), ios::in); 19 | if (!head_file.is_open()) return false; 20 | 21 | size_t field_rows, field_cols, field_z; 22 | double origin_x, origin_y, origin_z; 23 | double vol_res; 24 | head_file >> field_cols >> field_rows >> field_z; 25 | head_file >> origin_x >> origin_y >> origin_z; 26 | head_file >> vol_res; 27 | 28 | cout << field_cols << ", " << field_cols << ", " << field_cols << endl; 29 | cout << origin_x << ", " << origin_y << ", " << origin_z << endl; 30 | cout << vol_res << endl; 31 | 32 | head_file.close(); 33 | 34 | const string datafilename = filename_pre + ".vol.data"; 35 | ifstream data_file(datafilename.c_str(), ios::in); 36 | if (!data_file.is_open()) return false; 37 | 38 | vector vmat; 39 | for (size_t z = 0; z < field_z; z++) { 40 | vmat.push_back(Matrix(field_rows, field_cols)); 41 | } 42 | 43 | double tmpsdf; 44 | for (size_t x = 0; x < field_cols; x++) { 45 | for (size_t y = 0; y < field_rows; y++) { 46 | for (size_t z = 0; z < field_z; z++) { 47 | data_file >> tmpsdf; 48 | vmat[z](y, x) = tmpsdf; 49 | } 50 | } 51 | } 52 | 53 | data_file.close(); 54 | 55 | sdf = 56 | SignedDistanceField(Point3(origin_x, origin_y, origin_z), vol_res, vmat); 57 | 58 | return true; 59 | } 60 | 61 | } // namespace gpmp2 62 | -------------------------------------------------------------------------------- /gpmp2/utils/fileUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fileUtils.h 3 | * @brief utility functions wrapped in matlab 4 | * @author Jing Dong 5 | * @date Jan 28, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /// read SDF from .vol file 16 | /// the filename_pre.vol.head has the size/origin/resolution data 17 | /// the filename_pre.vol.data contains actual data 18 | /// return whether successfully read 19 | GPMP2_EXPORT bool readSDFvolfile(const std::string& filename_pre, 20 | SignedDistanceField& sdf); 21 | 22 | } // namespace gpmp2 23 | -------------------------------------------------------------------------------- /gpmp2/utils/matlabUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file matlabUtils.cpp 3 | * @brief utility functions wrapped in matlab 4 | * @author Jing Dong 5 | * @date Oct 14, 2016 6 | **/ 7 | 8 | #include 9 | 10 | namespace gpmp2 { 11 | 12 | /* ************************************************************************** */ 13 | void insertPose2VectorInValues(gtsam::Key key, const gpmp2::Pose2Vector& p, 14 | gtsam::Values& values) { 15 | values.insert(key, p); 16 | } 17 | 18 | /* ************************************************************************** */ 19 | gpmp2::Pose2Vector atPose2VectorValues(gtsam::Key key, 20 | const gtsam::Values& values) { 21 | return values.at(key); 22 | } 23 | 24 | } // namespace gpmp2 25 | -------------------------------------------------------------------------------- /gpmp2/utils/matlabUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file matlabUtils.h 3 | * @brief utility functions wrapped in matlab 4 | * @author Jing Dong 5 | * @date Oct 14, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace gpmp2 { 16 | 17 | /// prior factor on Pose2Vector 18 | typedef gtsam::PriorFactor PriorFactorPose2Vector; 19 | 20 | /// Pose2Vector Values utils 21 | GPMP2_EXPORT void insertPose2VectorInValues(gtsam::Key key, 22 | const gpmp2::Pose2Vector& p, 23 | gtsam::Values& values); 24 | GPMP2_EXPORT gpmp2::Pose2Vector atPose2VectorValues( 25 | gtsam::Key key, const gtsam::Values& values); 26 | 27 | } // namespace gpmp2 28 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotArm.m: -------------------------------------------------------------------------------- 1 | function h = plotArm(arm, conf, color, width) 2 | %PLOTARM Plot Arm class in 3D 3 | % 4 | % Usage: PLOTARM(arm, conf, color, width) 5 | % @arm Arm object 6 | % @conf arm configuration vector 7 | % @color color string, use plot convention, e.g. 'r' is red 8 | % @width line width 9 | 10 | position = arm.forwardKinematicsPosition(conf); 11 | 12 | style = strcat(color, '-'); 13 | h(1) = plot3(position(1,:), position(2,:), position(3,:), style, 'LineWidth', width); 14 | 15 | h(2) = plot3(position(1,1:end-1), position(2,1:end-1), position(3,1:end-1), 'k.', ... 16 | 'MarkerSize', 10*width); 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotEvidenceMap2D.m: -------------------------------------------------------------------------------- 1 | function h = plotEvidenceMap2D(prob_grid, origin_x, origin_y, cell_size) 2 | %PLOTEVIDENCEMAP2D Plot 2D evidence grid map, for 2D dataset visualization 3 | % 4 | % Usage: PLOTEVIDENCEMAP2D(prob_grid, origin_x, origin_y, cell_size) 5 | % @prob_grid evidence grid matrix 6 | % @origin_x, origin_y origin (down-left) corner of the map 7 | % @cell_size cell size 8 | 9 | % map display setting 10 | colormap([0.3 0.3 0.3; 0.7 0.7 0.7; 1 1 1]); 11 | 12 | % get X-Y coordinates 13 | grid_rows = size(prob_grid, 1); 14 | grid_cols = size(prob_grid, 2); 15 | grid_corner_x = origin_x + (grid_cols-1)*cell_size; 16 | grid_corner_y = origin_y + (grid_rows-1)*cell_size; 17 | grid_X = origin_x : cell_size : grid_corner_x; 18 | grid_Y = origin_y : cell_size : grid_corner_y; 19 | 20 | h = image(grid_X, grid_Y, (1-prob_grid)*2+1); 21 | 22 | set(gca,'YDir','normal') 23 | 24 | axis equal 25 | axis([origin_x-cell_size/2, grid_corner_x+cell_size/2, ... 26 | origin_y-cell_size/2, grid_corner_y+cell_size/2]) 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotMap3D.m: -------------------------------------------------------------------------------- 1 | function plotMap3D(corner_idx, origin, cell_size) 2 | %PLOTMAP3D Plot 3D evidence grid map, for visualization of 3D dataset 3 | % 4 | % Usage: PLOTMAP3D(corner_idx, origin, cell_size) 5 | % @corner_idx corner index [x1, x2, y1, y2, z1, z2], generated by generate3Ddataset 6 | % @origin origin (down-left) corner of the map 7 | % @cell_size cell size 8 | 9 | % for each obj 10 | 11 | colormap([0.4, 0.4, 0.4]) 12 | 13 | for i = 1:size(corner_idx, 1) 14 | x1 = corner_idx(i, 1)*cell_size + origin(1); 15 | x2 = corner_idx(i, 2)*cell_size + origin(1); 16 | y1 = corner_idx(i, 3)*cell_size + origin(2); 17 | y2 = corner_idx(i, 4)*cell_size + origin(2); 18 | z1 = corner_idx(i, 5)*cell_size + origin(3); 19 | z2 = corner_idx(i, 6)*cell_size + origin(3); 20 | surf([x1, x2; x1, x2], [y1, y1; y2, y2], [z1, z1; z1, z1]); 21 | surf([x1, x2; x1, x2], [y1, y1; y2, y2], [z2, z2; z2, z2]); 22 | surf([x1, x1; x1, x1], [y1, y1; y2, y2], [z1, z2; z1, z2]); 23 | surf([x2, x2; x2, x2], [y1, y1; y2, y2], [z1, z2; z1, z2]); 24 | surf([x1, x2; x1, x2], [y1, y1; y1, y1], [z1, z1; z2, z2]); 25 | surf([x1, x2; x1, x2], [y2, y2; y2, y2], [z1, z1; z2, z2]); 26 | end 27 | 28 | axis equal 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotPlanarArm.m: -------------------------------------------------------------------------------- 1 | function h = plotPlanarArm(arm, conf, color, width) 2 | %PLOTPLANARARM Plot Arm class in 2D 3 | % 4 | % Usage: PLOTPLANARARM(arm, conf, color, width) 5 | % @arm Arm object 6 | % @conf arm configuration vector 7 | % @color color string, use plot convention, e.g. 'r' is red 8 | % @width line width 9 | 10 | position = arm.forwardKinematicsPosition(conf); 11 | position = position(1:2, :); 12 | position = [[0;0], position]; 13 | 14 | style = strcat(color, '-'); 15 | h(1) = plot(position(1,:), position(2,:), style, 'LineWidth', width); 16 | 17 | h(2) = plot(position(1,1:end-1), position(2,1:end-1), 'k.', 'MarkerSize', 20); 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotPlanarMobile2Arms.m: -------------------------------------------------------------------------------- 1 | function h = plotPlanarMobile2Arms(marm, p, vehsize, color, width) 2 | %PLOTPLANARMOBILE2ARMS Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | import gtsam.* 6 | import gpmp2.* 7 | 8 | pose = p.pose(); 9 | % vehicle corners 10 | corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2)); 11 | corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2)); 12 | corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2)); 13 | corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2)); 14 | 15 | % vehicle base black lines 16 | h(1) = plot([corner1(1) corner2(1) corner3(1) corner4(1) corner1(1)], ... 17 | [corner1(2) corner2(2) corner3(2) corner4(2) corner1(2)], 'k-'); 18 | 19 | % arm 20 | position = marm.forwardKinematicsPosition(p); 21 | position = position(1:2, :); 22 | 23 | style = strcat(color, '-'); 24 | 25 | h(2) = plot(position(1,1:marm.arm1.dof+1), position(2,1:marm.arm1.dof+1), ... 26 | style, 'LineWidth', width); 27 | h(2) = plot(position(1,[1,marm.arm1.dof+2:end]), position(2,[1,marm.arm1.dof+2:end]), ... 28 | style, 'LineWidth', width); 29 | 30 | h(3) = plot(position(1,1:marm.arm1.dof), position(2,1:marm.arm1.dof), ... 31 | 'k.', 'MarkerSize', 5); 32 | h(3) = plot(position(1,marm.arm1.dof+2:end-1), position(2,marm.arm1.dof+2:end-1), ... 33 | 'k.', 'MarkerSize', 5); 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotPlanarMobileArm.m: -------------------------------------------------------------------------------- 1 | function h = plotPlannarMobileArm(marm, p, vehsize, color, width) 2 | %PLOTPLANNARMOBILEARM Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | import gtsam.* 6 | import gpmp2.* 7 | 8 | pose = p.pose(); 9 | % vehicle corners 10 | corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2)); 11 | corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2)); 12 | corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2)); 13 | corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2)); 14 | 15 | % vehicle base black lines 16 | h(1) = plot([corner1(1) corner2(1) corner3(1) corner4(1) corner1(1)], ... 17 | [corner1(2) corner2(2) corner3(2) corner4(2) corner1(2)], 'k-'); 18 | 19 | % arm 20 | position = marm.forwardKinematicsPosition(p); 21 | position = position(1:2, :); 22 | 23 | style = strcat(color, '-'); 24 | h(2) = plot(position(1,:), position(2,:), style, 'LineWidth', width); 25 | 26 | h(3) = plot(position(1,1:end-1), position(2,1:end-1), 'k.', 'MarkerSize', 5); 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotPlanarMobileBase.m: -------------------------------------------------------------------------------- 1 | function h = plotPlanarMobileBase(robot, pose, vehsize, color, width) 2 | %PLOTPLANARMOBILEBASE Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | import gtsam.* 6 | import gpmp2.* 7 | 8 | % vehicle corners 9 | corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2)); 10 | corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2)); 11 | corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2)); 12 | corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2)); 13 | 14 | % vehicle base black lines 15 | h(1) = plot([corner1(1) corner2(1) corner3(1) corner4(1) corner1(1)], ... 16 | [corner1(2) corner2(2) corner3(2) corner4(2) corner1(2)], 'k-'); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotPointRobot2D.m: -------------------------------------------------------------------------------- 1 | function h = plotPointRobot2D(robot, conf, color_rgb) 2 | %plotPointRobot2D Plot PointRobotModel in 2D 3 | % 4 | % Usage: plotRobotModel(robot, conf, color_rgb) 5 | % @robot PointRobotModel object 6 | % @conf robot configuration vector 7 | % @color_rgb optional color RGB values, default is gray [0.4 0.4 0.4] 8 | 9 | switch nargin 10 | case 2 11 | color_rgb = [0.4 0.4 0.4]; 12 | end 13 | 14 | % points 15 | body_points = robot.sphereCentersMat(conf); 16 | r = robot.sphere_radius(0); 17 | 18 | theta = linspace(0,2*pi); 19 | x = r * cos(theta) + body_points(1,:); 20 | y = r * sin(theta) + body_points(2,:); 21 | h = plot(x, y, 'Color', color_rgb); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotRobotModel.m: -------------------------------------------------------------------------------- 1 | function h = plotRobotModel(robot, conf, color_rgb) 2 | %plotRobotModel Plot RobotModel class in 3D, visualize the body spheres 3 | % also it can plot any child class of RobotModelm like ArmModel 4 | % 5 | % Usage: plotRobotModel(robot, conf, color_rgb) 6 | % @robot RobotModel(or child) object 7 | % @conf robot configuration vector 8 | % @color_rgb optional color RGB values, default is gray [0.4 0.4 0.4] 9 | 10 | switch nargin 11 | case 2 12 | color_rgb = [0.4 0.4 0.4]; 13 | end 14 | 15 | % points 16 | body_points = robot.sphereCentersMat(conf); 17 | 18 | % show 19 | [X_ball, Y_ball, Z_ball] = sphere(16); 20 | 21 | for i=1:robot.nr_body_spheres() 22 | h(i) = surf(X_ball * robot.sphere_radius(i-1) + body_points(1, i), ... 23 | Y_ball * robot.sphere_radius(i-1) + body_points(2, i), ... 24 | Z_ball * robot.sphere_radius(i-1) + body_points(3, i)); 25 | h(i).FaceColor = color_rgb; 26 | end 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotSignedDistanceField2D.m: -------------------------------------------------------------------------------- 1 | function h = plotSignedDistanceField2D(field, origin_x, origin_y, cell_size, epsilon_dist) 2 | %PLOTSIGNEDDISTANCEFIELD2D plot 2D SignedDistanceField 3 | % 4 | % Usage: PLOTSIGNEDDISTANCEFIELD2D(field, origin_x, origin_y, cell_size, epsilon_dist) 5 | % @field field matrix 6 | % @origin_x, origin_y origin (down-left) corner of the map 7 | % @cell_size cell size 8 | % @epsilon_dist optional plot obstacle safety distance, default = 0 9 | 10 | if nargin < 5 11 | epsilon_dist = 0; 12 | end 13 | 14 | % get X-Y coordinates 15 | grid_rows = size(field, 1); 16 | grid_cols = size(field, 2); 17 | grid_corner_x = origin_x + (grid_cols-1)*cell_size; 18 | grid_corner_y = origin_y + (grid_rows-1)*cell_size; 19 | grid_X = origin_x : cell_size : grid_corner_x; 20 | grid_Y = origin_y : cell_size : grid_corner_y; 21 | 22 | h = imagesc(grid_X, grid_Y, field); 23 | 24 | colormap(hsv(64)) 25 | 26 | % colormap(gray(64)) 27 | % caxis([min(min(field)), epsilon_dist]); 28 | 29 | set(gca,'YDir','normal') 30 | 31 | axis equal 32 | axis([origin_x-cell_size/2, grid_corner_x+cell_size/2, ... 33 | origin_y-cell_size/2, grid_corner_y+cell_size/2]) 34 | 35 | colorbar 36 | 37 | xlabel('X/m') 38 | ylabel('Y/m') 39 | 40 | 41 | end 42 | 43 | -------------------------------------------------------------------------------- /matlab/+gpmp2/plotSignedDistanceField3D.m: -------------------------------------------------------------------------------- 1 | function h = plotSignedDistanceField3D(field, origin, cell_size, epsilon_dist, marker_size) 2 | %PLOTSIGNEDDISTANCEFIELD3D plot 3D SignedDistanceField 3 | % 4 | % Usage: PLOTSIGNEDDISTANCEFIELD3D(field, origin, cell_size, epsilon_dist) 5 | % @field field 3D matrix 6 | % @origin origin of the map 7 | % @cell_size cell size 8 | % @epsilon_dist optional plot obstacle safety distance, default = 0 9 | % @marker_size marker size, default = 0 10 | 11 | if nargin < 4 12 | epsilon_dist = 0; 13 | marker_size = 10; 14 | elseif nargin < 5 15 | marker_size = 10; 16 | end 17 | 18 | % get X-Y coordinates 19 | grid_rows = size(field, 1); 20 | grid_cols = size(field, 2); 21 | grid_z = size(field, 3); 22 | grid_corner_x = origin(1) + (grid_cols-1)*cell_size; 23 | grid_corner_y = origin(2) + (grid_rows-1)*cell_size; 24 | grid_corner_z = origin(3) + (grid_z-1)*cell_size; 25 | grid_X = origin(1) : cell_size : grid_corner_x; 26 | grid_Y = origin(2) : cell_size : grid_corner_y; 27 | grid_Z = origin(3) : cell_size : grid_corner_z; 28 | 29 | idx = find(field 0.75); 17 | % inverse map 18 | inv_map = 1 - map; 19 | 20 | % get signed distance from map and inverse map 21 | map_dist = bwdist(map); 22 | inv_map_dist = bwdist(inv_map); 23 | 24 | field = map_dist - inv_map_dist; 25 | 26 | % metric 27 | field = field * cell_size; 28 | field = double(field); 29 | 30 | % limit inf 31 | if isinf(field(1,1)) 32 | field = ones(size(field)) * 1000; 33 | end 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /matlab/+gpmp2/signedDistanceField3D.m: -------------------------------------------------------------------------------- 1 | function field = signedDistanceField3D(ground_truth_map, cell_size) 2 | %SIGNEDDISTANCEFIELD3D 3D signed distance field 3 | % Given a ground truth 3D map defined in Matrix in 0-1, 4 | % calculate 3D signed distance field, which is defined as a matrix 5 | % map matrix and signed distance field matrix have the same resolution. 6 | % 7 | % Usage: field = SIGNEDDISTANCEFIELD3D(ground_truth_map, cell_siz) 8 | % @map evidence grid from dataset, map use 0 show open area, 1 show objects. 9 | % @cell_size cell sizeto given metric information 10 | % 11 | % Output: 12 | % @field sdf, row is X, col is Y, 3rd index is Z 13 | 14 | 15 | % regularize unknow area to open area 16 | map = (ground_truth_map > 0.75); 17 | % inverse map 18 | inv_map = 1 - map; 19 | 20 | % get signed distance from map and inverse map 21 | map_dist = bwdist(map); 22 | inv_map_dist = bwdist(inv_map); 23 | 24 | field = map_dist - inv_map_dist; 25 | 26 | % metric 27 | field = field * cell_size; 28 | field = double(field); 29 | 30 | % limit inf 31 | if isinf(field(1,1)) 32 | field = ones(size(field)) * 1000; 33 | end 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Import the gtwrap package. 2 | find_package(gtwrap) 3 | 4 | set(WRAP_PYTHON_VERSION 5 | "Default" 6 | CACHE STRING "The Python version to use for wrapping") 7 | 8 | gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) 9 | 10 | # ############################################################ 11 | # #### Necessary variables to set for Matlab wrapper to work 12 | # Set up cache options 13 | option(WRAP_MEX_BUILD_STATIC_MODULE 14 | "Build MATLAB wrapper statically (increases build time)" OFF) 15 | set(WRAP_BUILD_MEX_BINARY_FLAGS 16 | "" 17 | CACHE STRING "Extra flags for running Matlab MEX compilation") 18 | set(WRAP_TOOLBOX_INSTALL_PATH 19 | "${CMAKE_INSTALL_PREFIX}/gpmp2_toolbox" 20 | CACHE 21 | PATH 22 | "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gpmp2_toolbox" 23 | ) 24 | 25 | option( 26 | WRAP_BUILD_TYPE_POSTFIXES 27 | "Enable/Disable appending the build type to the name of compiled libraries" 28 | ON) 29 | 30 | # ############################################################ 31 | 32 | # Copy matlab.h to the correct folder. 33 | configure_file(${PROJECT_SOURCE_DIR}/matlab/matlab.h 34 | ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY) 35 | 36 | # Include the Matlab related code. 37 | include(MatlabWrap) 38 | 39 | # Ensure that local folder is searched before library folders 40 | include_directories(BEFORE "${PROJECT_SOURCE_DIR}") 41 | 42 | # Install library 43 | install( 44 | TARGETS ${PROJECT_NAME} 45 | LIBRARY DESTINATION lib 46 | ARCHIVE DESTINATION lib 47 | RUNTIME DESTINATION bin) 48 | 49 | # ############################################################################## 50 | # Build MATLAB wrapper 51 | # (CMake tracks the dependecy to link with GTSAM through our project's static library) 52 | set(interface_files "${PROJECT_SOURCE_DIR}/gpmp2.i") 53 | matlab_wrap("${interface_files}" "${PROJECT_NAME}" "" "" "" "" "${GPMP2_ENABLE_BOOST_SERIALIZATION}") 54 | 55 | # Add the Matlab scripts that are a part of the toolbox 56 | install_matlab_scripts("${PROJECT_SOURCE_DIR}/matlab/" "*.m;*.fig") 57 | 58 | # We print out our configuration for an easy visual check 59 | message(STATUS "MATLAB Root: ${MATLAB_ROOT}") 60 | message(STATUS "Mex Compiler: ${Matlab_MEX_COMPILER}") 61 | -------------------------------------------------------------------------------- /matlab/gpmp2_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Examples 2 | message(STATUS "Installing Matlab toolbox scripts") 3 | 4 | install_matlab_scripts("${CMAKE_CURRENT_SOURCE_DIR}/" "+gpmp2/*.m") 5 | -------------------------------------------------------------------------------- /matlab/gpmp2_examples/SaveSDFExample.m: -------------------------------------------------------------------------------- 1 | % Example to save sdf as gpmp2 datatype 2 | % @author Mustafa Mukadam 3 | % @date May 4, 2017 4 | 5 | close all 6 | clear 7 | 8 | import gtsam.* 9 | import gpmp2.* 10 | 11 | 12 | %% dataset 13 | dataset = generate3Ddataset('WAMDeskDataset'); 14 | origin = [dataset.origin_x, dataset.origin_y, dataset.origin_z]; 15 | origin_point3 = Point3(origin'); 16 | cell_size = dataset.cell_size; 17 | 18 | % init sdf 19 | disp('calculating signed distance field ...'); 20 | field = signedDistanceField3D(dataset.map, dataset.cell_size); 21 | disp('calculating signed distance field done'); 22 | 23 | sdf = SignedDistanceField(origin_point3, cell_size, size(field, 1), ... 24 | size(field, 2), size(field, 3)); 25 | for z = 1:size(field, 3) 26 | sdf.initFieldData(z-1, field(:,:,z)'); 27 | end 28 | 29 | %% save SDF 30 | disp('saving sdf to .bin file...'); 31 | sdf.saveSDF('WAMDeskDataset.bin'); 32 | -------------------------------------------------------------------------------- /matlab/gpmp2_tests/test_parameterization.m: -------------------------------------------------------------------------------- 1 | import gtsam.* 2 | import gpmp2.* 3 | 4 | % Test parameterization 5 | arm = generateArm('KinovaGen3'); 6 | 7 | % Verify 8 | t = arm.fk_model(); 9 | assert(strcmp(t.parameterization,"MODIFIED_DH"),"Wrong parameterization returned"); 10 | assert(strcmp(t.parameterizationString,"Modified Denavit-Hartenberg"),"Wrong parameterization returned"); -------------------------------------------------------------------------------- /python/.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Byte-compiled / optimized / DLL files 35 | __pycache__/ 36 | *.py[cod] 37 | *$py.class 38 | 39 | # C extensions 40 | *.so 41 | 42 | # Distribution / packaging 43 | .Python 44 | build/ 45 | develop-eggs/ 46 | dist/ 47 | downloads/ 48 | eggs/ 49 | .eggs/ 50 | lib/ 51 | lib64/ 52 | parts/ 53 | sdist/ 54 | var/ 55 | wheels/ 56 | *.egg-info/ 57 | .installed.cfg 58 | *.egg 59 | MANIFEST 60 | 61 | # PyInstaller 62 | # Usually these files are written by a python script from a template 63 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 64 | *.manifest 65 | *.spec 66 | 67 | # Installer logs 68 | pip-log.txt 69 | pip-delete-this-directory.txt 70 | 71 | # Unit test / coverage reports 72 | htmlcov/ 73 | .tox/ 74 | .nox/ 75 | .coverage 76 | .coverage.* 77 | .cache 78 | nosetests.xml 79 | coverage.xml 80 | *.cover 81 | .hypothesis/ 82 | .pytest_cache/ 83 | 84 | # Translations 85 | *.mo 86 | *.pot 87 | 88 | # Django stuff: 89 | *.log 90 | local_settings.py 91 | db.sqlite3 92 | 93 | # Flask stuff: 94 | instance/ 95 | .webassets-cache 96 | 97 | # Scrapy stuff: 98 | .scrapy 99 | 100 | # Sphinx documentation 101 | docs/_build/ 102 | 103 | # PyBuilder 104 | target/ 105 | 106 | # Jupyter Notebook 107 | .ipynb_checkpoints 108 | 109 | # IPython 110 | profile_default/ 111 | ipython_config.py 112 | 113 | # pyenv 114 | .python-version 115 | 116 | # celery beat schedule file 117 | celerybeat-schedule 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env.bak/ 126 | venv.bak/ 127 | 128 | # Spyder project settings 129 | .spyderproject 130 | .spyproject 131 | 132 | # Rope project settings 133 | .ropeproject 134 | 135 | # mkdocs documentation 136 | /site 137 | 138 | # mypy 139 | .mypy_cache/ 140 | .dmypy.json 141 | dmypy.json 142 | 143 | *.pyc 144 | **/.idea/* 145 | **/cmake-build-debug/* 146 | 147 | # models 148 | *.pth 149 | *.th 150 | *.npz 151 | *.bin 152 | 153 | # tmp files for testing 154 | tmp/* 155 | 156 | # docusaurus 157 | node_modules 158 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(gtwrap) 2 | 3 | ### Get the default python version if not specified 4 | set(WRAP_PYTHON_VERSION 5 | "Default" 6 | CACHE STRING "The Python version to use for wrapping") 7 | gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) 8 | message(STATUS "Wrapper Python Version: ${WRAP_PYTHON_VERSION}") 9 | 10 | set(PYTHON_REQUIREMENTS_PATH "requirements.txt") 11 | set(PYTHON_TESTS "${PROJECT_NAME}.tests") 12 | 13 | ### Set the path to the Python directory of GPMP2 and the Python binary directory. 14 | set(GPMP2_PYTHON_PATH ${PROJECT_SOURCE_DIR}/python/) 15 | file(TO_NATIVE_PATH "${PROJECT_BINARY_DIR}/python" GPMP2_PYTHON_BINARY_DIR) 16 | 17 | ### Configure templates and generate necessary files in build. 18 | configure_file(${GPMP2_PYTHON_PATH}/templates/setup.py.in 19 | ${GPMP2_PYTHON_BINARY_DIR}/setup.py) 20 | configure_file(${GPMP2_PYTHON_PATH}/requirements.txt 21 | ${GPMP2_PYTHON_BINARY_DIR}/requirements.txt COPYONLY) 22 | configure_file(${GPMP2_PYTHON_PATH}/templates/${PROJECT_NAME}.tpl 23 | ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl) 24 | 25 | ### Copy the gpmp2 python directory 26 | file(COPY ${GPMP2_PYTHON_PATH}/${PROJECT_NAME} 27 | DESTINATION ${GPMP2_PYTHON_BINARY_DIR}) 28 | 29 | ### Set the name(s) of the interface file used for wrapping 30 | set(interface_headers 31 | ${PROJECT_SOURCE_DIR}/${PROJECT_NAME}.i 32 | ${ADDITIONAL_INTERFACE_FILES} 33 | ) 34 | 35 | pybind_wrap( 36 | ${PROJECT_NAME}_py # target 37 | "${interface_headers}" # interface header file 38 | "${PROJECT_NAME}.cpp" # the generated cpp 39 | "${PROJECT_NAME}" # module_name 40 | "${PROJECT_NAME}" # top namespace in the cpp file 41 | "${ignore}" # ignore classes 42 | ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl 43 | ${PROJECT_NAME} # libraries to link against 44 | "${PROJECT_NAME}" # dependencies to build first 45 | ON # use boost serialization 46 | ) 47 | 48 | set_target_properties( 49 | ${PROJECT_NAME}_py 50 | PROPERTIES OUTPUT_NAME "${PROJECT_NAME}" 51 | LIBRARY_OUTPUT_DIRECTORY "${GPMP2_PYTHON_BINARY_DIR}/${PROJECT_NAME}" 52 | DEBUG_POSTFIX "" # Otherwise you will have a wrong name 53 | RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name 54 | ) 55 | 56 | ### Custom make command to install python library `make python-install` 57 | add_custom_target( 58 | python-install 59 | COMMAND ${PYTHON_EXECUTABLE} -m pip install . 60 | DEPENDS ${PROJECT_NAME}_py 61 | WORKING_DIRECTORY ${GPMP2_PYTHON_BINARY_DIR}) 62 | 63 | if(UNIX) 64 | set(GPMP2_PATH_SEP ":") 65 | else() 66 | set(GPMP2_PATH_SEP ";") 67 | endif() 68 | 69 | # Unit tests 70 | set(python_unit_test_suites) 71 | 72 | macro(PYTHON_UNIT_TEST_SUITE suiteName directory) 73 | add_custom_target( 74 | python-test.${suiteName} 75 | COMMAND 76 | ${CMAKE_COMMAND} -E env # add package to python path so no need to install 77 | "PYTHONPATH=${GPMP2_PYTHON_BINARY_DIR}${GPMP2_PATH_SEP}$ENV{PYTHONPATH}" 78 | ${PYTHON_EXECUTABLE} -m unittest discover -v -s . 79 | DEPENDS ${PROJECT_NAME}_py 80 | WORKING_DIRECTORY ${directory}) 81 | list(APPEND python_unit_test_suites python-test.${suiteName}) 82 | endmacro() 83 | 84 | PYTHON_UNIT_TEST_SUITE(base ${PROJECT_SOURCE_DIR}/python/tests) 85 | 86 | # Custom make command to run all Python tests 87 | add_custom_target( 88 | python-test 89 | DEPENDS ${PROJECT_NAME}_py ${python_unit_test_suites}) 90 | -------------------------------------------------------------------------------- /python/gpmp2/__init__.py: -------------------------------------------------------------------------------- 1 | # Python needs to know about gtsam base classes before it can import module classes 2 | # Else will throw cryptic "referenced unknown base type" error. 3 | import gtsam 4 | 5 | from gpmp2.gpmp2 import * 6 | 7 | __version__ = "0.3.0" 8 | -------------------------------------------------------------------------------- /python/gpmp2/datasets/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/python/gpmp2/datasets/__init__.py -------------------------------------------------------------------------------- /python/gpmp2/preamble/gpmp2.h: -------------------------------------------------------------------------------- 1 | /* Please refer to: 2 | * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html 3 | * These are required to save one copy operation on Python calls. 4 | * 5 | * NOTES 6 | * ================= 7 | * 8 | * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 9 | * automatic STL binding, such that the raw objects can be accessed in Python. 10 | * Without this they will be automatically converted to a Python object, and all 11 | * mutations on Python side will not be reflected on C++. 12 | */ 13 | 14 | // Including can cause some serious hard-to-debug bugs! 15 | #include 16 | 17 | PYBIND11_MAKE_OPAQUE(gpmp2::BodySphereVector); 18 | -------------------------------------------------------------------------------- /python/gpmp2/robots/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/python/gpmp2/robots/__init__.py -------------------------------------------------------------------------------- /python/gpmp2/specializations/gpmp2.h: -------------------------------------------------------------------------------- 1 | // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html 2 | // These are required to save one copy operation on Python calls 3 | -------------------------------------------------------------------------------- /python/gpmp2/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/gpmp2/c8f280014b3d6667dc2d0e6d9297fad2037e6bb1/python/gpmp2/utils/__init__.py -------------------------------------------------------------------------------- /python/gpmp2/utils/signedDistanceField2D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gpmp2 import * 3 | from gtsam import * 4 | from scipy import ndimage 5 | 6 | 7 | def signedDistanceField2D(ground_truth_map, cell_size): 8 | # SIGNEDDISTANCEFIELD2D 2D signed distance field 9 | # Given a ground truth 2D map defined in Matrix in 0-1, 10 | # calculate 2D signed distance field, which is defined as a matrix 11 | # map matrix and signed distance field matrix have the same resolution. 12 | # 13 | # Usage: field = SIGNEDDISTANCEFIELD2D(ground_truth_map, cell_siz) 14 | # @map evidence grid from dataset, map use 0 show open area, 1 show objects. 15 | # @cell_size cell sizeto given metric information 16 | # 17 | # Output: 18 | # @field sdf, row is Y, col is X 19 | 20 | # regularize unknow area to open area 21 | cur_map = ground_truth_map > 0.75 22 | cur_map = cur_map.astype(int) 23 | 24 | if np.amax(cur_map) is 0: 25 | return np.ones(ground_truth_map.shape) * 1000 26 | 27 | # inverse map 28 | inv_map = 1 - cur_map 29 | 30 | # get signed distance from map and inverse map 31 | # since bwdist(foo) = ndimage.distance_transform_edt(1-foo) 32 | map_dist = ndimage.distance_transform_edt(inv_map) 33 | inv_map_dist = ndimage.distance_transform_edt(cur_map) 34 | 35 | field = map_dist - inv_map_dist 36 | 37 | # metric 38 | field = field * cell_size 39 | field = field.astype(float) 40 | 41 | return field 42 | -------------------------------------------------------------------------------- /python/gpmp2/utils/signedDistanceField3D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gpmp2 import * 3 | from gtsam import * 4 | from scipy import ndimage 5 | 6 | 7 | def signedDistanceField3D(ground_truth_map, cell_size): 8 | # SIGNEDDISTANCEFIELD3D 3D signed distance field 9 | # Given a ground truth 3D map defined in Matrix in 0-1, 10 | # calculate 3D signed distance field, which is defined as a matrix 11 | # map matrix and signed distance field matrix have the same resolution. 12 | # 13 | # Usage: field = SIGNEDDISTANCEFIELD3D(ground_truth_map, cell_siz) 14 | # @map evidence grid from dataset, map use 0 show open area, 1 show objects. 15 | # @cell_size cell sizeto given metric information 16 | # 17 | # Output: 18 | # @field sdf, row is X, col is Y, 3rd index is Z 19 | 20 | # regularize unknow area to open area 21 | cur_map = ground_truth_map > 0.75 22 | cur_map = cur_map.astype(int) 23 | 24 | if np.amax(cur_map) is 0: 25 | return np.ones(ground_truth_map.shape) * 1000 26 | 27 | # inverse map 28 | inv_map = 1 - cur_map 29 | 30 | # get signed distance from map and inverse map 31 | # since bwdist(foo) = ndimage.distance_transform_edt(1-foo) 32 | map_dist = ndimage.distance_transform_edt(inv_map) 33 | inv_map_dist = ndimage.distance_transform_edt(cur_map) 34 | 35 | field = map_dist - inv_map_dist 36 | 37 | # metric 38 | field = field * cell_size 39 | field = field.astype(float) 40 | 41 | return field 42 | -------------------------------------------------------------------------------- /python/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | gtsam -------------------------------------------------------------------------------- /python/templates/gpmp2.tpl: -------------------------------------------------------------------------------- 1 | // Minimal template file for wrapping C++ code. 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "gtsam/config.h" 12 | #include "gtsam/base/serialization.h" 13 | #include "gtsam/base/utilities.h" // for RedirectCout. 14 | 15 | {includes} 16 | #include 17 | 18 | {boost_class_export} 19 | 20 | // Preamble for STL classes 21 | #include "python/gpmp2/preamble/{module_name}.h" 22 | 23 | using namespace std; 24 | 25 | namespace py = pybind11; 26 | 27 | {submodules} 28 | 29 | {module_def} {{ 30 | m_.doc() = "pybind11 wrapper of {module_name}"; 31 | 32 | {submodules_init} 33 | 34 | {wrapped_namespace} 35 | // Specializations for STL classes 36 | #include "python/gpmp2/specializations/{module_name}.h" 37 | 38 | }} 39 | -------------------------------------------------------------------------------- /python/templates/setup.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | try: 4 | from setuptools import setup, find_packages 5 | except ImportError: 6 | from distutils.core import setup, find_packages 7 | 8 | packages = find_packages() 9 | 10 | package_data = { 11 | package: [ 12 | f 13 | for f in os.listdir(package.replace(".", os.path.sep)) 14 | if os.path.splitext(f)[1] in (".so", ".pyd") 15 | ] 16 | for package in packages 17 | } 18 | 19 | dependency_list = open("${PYTHON_REQUIREMENTS_PATH}").read().split('\n') 20 | dependencies = [x for x in dependency_list if x[0] != '#'] 21 | 22 | setup( 23 | name='${PROJECT_NAME}', 24 | description='${PROJECT_DESCRIPTION}', 25 | url='${PROJECT_HOMEPAGE_URL}', 26 | version='${PROJECT_VERSION}', 27 | author="${PROJECT_AUTHOR}", 28 | author_email="${PROJECT_AUTHOR_EMAIL}", 29 | license='Simplified BSD license', 30 | keywords="", 31 | long_description=open("${PROJECT_SOURCE_DIR}/README.md").read(), 32 | long_description_content_type="text/markdown", 33 | python_requires=">=3.6", 34 | # https://pypi.org/pypi?%3Aaction=list_classifiers 35 | classifiers=[ 36 | 'Development Status :: 5 - Production/Stable', 37 | 'Intended Audience :: Education', 38 | 'Intended Audience :: Developers', 39 | 'Intended Audience :: Science/Research', 40 | 'Operating System :: MacOS', 41 | 'Operating System :: Microsoft :: Windows', 42 | 'Operating System :: POSIX', 43 | 'License :: OSI Approved :: BSD License', 44 | 'Programming Language :: Python :: 2', 45 | 'Programming Language :: Python :: 3', 46 | ], 47 | packages=packages, 48 | # Load the built shared object files 49 | package_data=package_data, 50 | include_package_data=True, 51 | test_suite="${PYTHON_TESTS}", 52 | # Ensure that the compiled .so file is properly packaged 53 | zip_safe=False, 54 | platforms="any", 55 | install_requires=dependencies, 56 | ) 57 | --------------------------------------------------------------------------------