├── .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.h ├── 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 ├── gpmp2_python ├── .gitignore ├── examples │ ├── Arm2FactorGraphExample.py │ ├── WAMFactorGraphExample.py │ ├── multi_graph │ │ ├── graph_pointRobot.py │ │ └── graph_utils.py │ ├── pointRobot2FactorExample.py │ ├── pointRobot2Factor_rh.py │ ├── pointRobot3FactorExample.py │ └── pointRobot3FactorExample_rh.py ├── gpmp2_python │ ├── __init__.py │ ├── datasets │ │ ├── __init__.py │ │ ├── generate2Ddataset.py │ │ └── generate3Ddataset.py │ ├── robots │ │ ├── __init__.py │ │ ├── generateArm.py │ │ └── generateMobileArm.py │ ├── utils │ │ ├── __init__.py │ │ ├── plot_utils.py │ │ ├── signedDistanceField2D.py │ │ └── signedDistanceField3D.py │ └── version.py ├── requirements.txt └── setup.py ├── 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 ├── Arm2FactorGraphExample.m ├── Arm3FactorGraphExample.m ├── Arm3GoalReachExample.m ├── Arm3JointLimitExample.m ├── Arm3PlannerExample.m ├── CMakeLists.txt ├── Mobile2ArmsFactorGraphExample.m ├── MobileArm2FactorGraphExample.m ├── MobileBaseFactorGraphExample.m ├── PointRobot2DFactorGraphExample.m ├── SaveSDFExample.m ├── WAMFactorGraphExample.m ├── WAMPlannerExample.m ├── WAMReplannerExample.m └── WAMWorkspaceConstraintsExample.m └── projects └── jist ├── .gitignore ├── README.md ├── configs ├── algorithm │ └── rrt.yaml ├── dataset │ ├── dynamic2D.yaml │ └── patrol_guard.yaml ├── default_config.yaml └── problem │ ├── example.yaml │ └── example_patrol.yaml ├── jist ├── __init__.py ├── datasets │ ├── __init__.py │ └── generate2Ddataset.py ├── jist.py └── utils │ ├── __init__.py │ ├── plot_utils.py │ └── signedDistanceField2D.py ├── patrol_abl.sh ├── pointRobot_example.py ├── pointRobot_example_patrol_guard.py ├── pointRobot_test.py ├── pointRobot_test_patrol_guard.py └── random_abl.sh /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | *.pyc 3 | -------------------------------------------------------------------------------- /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 CXX C) 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") 6 | 7 | # Mac ONLY. Define Relative Path on Mac OS 8 | if(NOT DEFINED CMAKE_MACOSX_RPATH) 9 | set(CMAKE_MACOSX_RPATH 0) 10 | endif() 11 | 12 | # version indicator 13 | set(GPMP2_VERSION_MAJOR 0) 14 | set(GPMP2_VERSION_MINOR 3) 15 | set(GPMP2_VERSION_PATCH 0) 16 | set(gpmp2_VERSION_STRING "${GPMP2_VERSION_MAJOR}.${GPMP2_VERSION_MINOR}.${GPMP2_VERSION_PATCH}") 17 | 18 | 19 | # option: whether turn on Matlab toolbox 20 | option(GPMP2_BUILD_STATIC_LIBRARY "whether build static library" OFF) 21 | option(GPMP2_BUILD_MATLAB_TOOLBOX "whether build matlab toolbox, need shared lib" OFF) 22 | option(GPMP2_BUILD_PYTHON_TOOLBOX "whether build python toolbox, need shared lib" OFF) 23 | 24 | if(GPMP2_BUILD_STATIC_LIBRARY AND GPMP2_BUILD_MATLAB_TOOLBOX) 25 | message(FATAL_ERROR "matlab toolbox needs shared lib") 26 | endif() 27 | 28 | 29 | # Find GTSAM components 30 | find_package(GTSAM REQUIRED) # Uses installed package 31 | include_directories(${GTSAM_INCLUDE_DIR}) 32 | set(GTSAM_LIBRARIES gtsam) # TODO: automatic search libs 33 | 34 | find_package(GTSAMCMakeTools) 35 | include(GtsamMakeConfigFile) 36 | include(GtsamBuildTypes) 37 | include(GtsamTesting) 38 | 39 | 40 | # for unittest scripts 41 | set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools") 42 | 43 | # Boost - same requirement as gtsam 44 | find_package(Boost 1.50 REQUIRED) 45 | find_package(Boost COMPONENTS filesystem REQUIRED) 46 | find_package(Boost COMPONENTS system REQUIRED) 47 | find_package(Boost COMPONENTS thread REQUIRED) 48 | find_package(Boost COMPONENTS serialization REQUIRED) 49 | 50 | include_directories(${Boost_INCLUDE_DIR}) 51 | 52 | 53 | # Generate and install config and dllexport files 54 | configure_file("gpmp2/config.h.in" "gpmp2/config.h") 55 | list(APPEND gpmp2_srcs "${PROJECT_BINARY_DIR}/gpmp2/config.h") 56 | include_directories(BEFORE ${PROJECT_BINARY_DIR}) # So we can include generated config header files 57 | install(FILES "${PROJECT_BINARY_DIR}/gpmp2/config.h" DESTINATION include/gpmp2) 58 | 59 | 60 | # include current source folder, at the very beginning 61 | include_directories(BEFORE ${CMAKE_CURRENT_SOURCE_DIR}) 62 | 63 | # Process source subdirs 64 | add_subdirectory(gpmp2) 65 | 66 | # Wrapping to MATLAB 67 | if(GPMP2_BUILD_MATLAB_TOOLBOX) 68 | # wrap 69 | include(GtsamMatlabWrap) 70 | wrap_and_install_library(gpmp2.h ${PROJECT_NAME} "${CMAKE_CURRENT_SOURCE_DIR}" "") 71 | 72 | # install matlab functions and scripts 73 | add_subdirectory(matlab) 74 | endif() 75 | 76 | 77 | # Wrapping to Python 78 | if(GPMP2_BUILD_PYTHON_TOOLBOX) 79 | include_directories(${GTSAM_DIR}/cython) 80 | include_directories(/usr/local/cython) 81 | include(GtsamCythonWrap) 82 | include_directories(${GTSAM_EIGENCY_INSTALL_PATH}) 83 | 84 | wrap_and_install_library_cython("gpmp2.h" 85 | "from gtsam.gtsam cimport *" # extra import of gtsam/gtsam.pxd Cython header 86 | "${CMAKE_INSTALL_PREFIX}/cython" # install path 87 | gpmp2 # library to link with 88 | "gtsam" # dependencies which need to be built before wrapping 89 | ) 90 | add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) 91 | endif() 92 | 93 | # Install config and export files 94 | GtsamMakeConfigFile(gpmp2) 95 | export(TARGETS ${GPMP2_EXPORTED_TARGETS} FILE gpmp2-exports.cmake) 96 | -------------------------------------------------------------------------------- /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/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/2d_3link_arm_dense.gif -------------------------------------------------------------------------------- /doc/pics/2d_3link_arm_nondense.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/2d_3link_arm_nondense.gif -------------------------------------------------------------------------------- /doc/pics/3link_setting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/3link_setting.png -------------------------------------------------------------------------------- /doc/pics/wam_arm_plan_result.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/wam_arm_plan_result.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/wam_replan_1.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/wam_replan_2.gif -------------------------------------------------------------------------------- /doc/pics/wam_replan_setting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/wam_replan_setting.png -------------------------------------------------------------------------------- /doc/pics/wam_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/doc/pics/wam_results.png -------------------------------------------------------------------------------- /doc/pics/wam_settings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/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 | 14 | namespace gpmp2 { 15 | 16 | 17 | /// simple 2D vehicle dynamics: vehicle heading is consistent with velocity direction 18 | /// return sliding velocity for Lie group, lower is better 19 | inline double simple2DVehicleDynamicsPose2(const gtsam::Pose2& p, const gtsam::Vector3& v, 20 | gtsam::OptionalJacobian<1, 3> Hp = boost::none, 21 | gtsam::OptionalJacobian<1, 3> Hv = boost::none) { 22 | 23 | if (Hp) *Hp = (gtsam::Matrix13() << 0, 0, 0).finished(); 24 | if (Hv) *Hv = (gtsam::Matrix13() << 0, 1, 0).finished(); 25 | 26 | return v(1); 27 | } 28 | 29 | /// simple 2D vehicle dynamics: vehicle heading is consistent with velocity direction 30 | /// return sliding velocity for vector space, lower is better 31 | inline double simple2DVehicleDynamicsVector3(const gtsam::Vector3& p, const gtsam::Vector3& v, 32 | gtsam::OptionalJacobian<1, 3> Hp = boost::none, 33 | gtsam::OptionalJacobian<1, 3> Hv = boost::none) { 34 | 35 | if (Hp) *Hp = (gtsam::Matrix13() << 0, 0, -(v(1)*sin(p(2))+v(0)*cos(p(2)))).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 | } 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 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | 22 | namespace gpmp2 { 23 | 24 | /** 25 | * unary factor for vehicle dynamics 26 | */ 27 | class VehicleDynamicsFactorPose2: public gtsam::NoiseModelFactor2 { 28 | 29 | private: 30 | // typedefs 31 | typedef VehicleDynamicsFactorPose2 This; 32 | typedef gtsam::NoiseModelFactor2 Base; 33 | 34 | public: 35 | 36 | /// shorthand for a smart pointer to a factor 37 | typedef boost::shared_ptr shared_ptr; 38 | 39 | /* Default constructor do nothing */ 40 | VehicleDynamicsFactorPose2() {} 41 | 42 | /** 43 | * Constructor 44 | * @param cost_sigma cost function covariance, should to identity model 45 | */ 46 | VehicleDynamicsFactorPose2(gtsam::Key poseKey, gtsam::Key velKey, double cost_sigma) : 47 | Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, velKey) {} 48 | 49 | virtual ~VehicleDynamicsFactorPose2() {} 50 | 51 | 52 | /// error function 53 | /// numerical/analytic Jacobians from cost function 54 | gtsam::Vector evaluateError(const gtsam::Pose2& pose, const gtsam::Vector& vel, 55 | boost::optional H1 = boost::none, 56 | boost::optional H2 = boost::none) const { 57 | 58 | using namespace gtsam; 59 | 60 | if (H1 || H2) { 61 | Matrix13 Hp, Hv; 62 | const double err = simple2DVehicleDynamicsPose2(pose, vel.head<3>(), Hp, Hv); 63 | if (H1) { 64 | *H1 = Matrix::Zero(1, 3); 65 | H1->block<1,3>(0,0) = Hp; 66 | } 67 | if (H2) { 68 | *H2 = Matrix::Zero(1, 3); 69 | H2->block<1,3>(0,0) = Hv; 70 | } 71 | return (Vector(1) << err).finished(); 72 | 73 | } else { 74 | return (Vector(1) << simple2DVehicleDynamicsPose2(pose, vel.head<3>())).finished(); 75 | } 76 | } 77 | 78 | 79 | /// @return a deep copy of this factor 80 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 81 | return boost::static_pointer_cast( 82 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 83 | 84 | /** print contents */ 85 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 86 | std::cout << s << "VehicleDynamicsFactorPose2 :" << std::endl; 87 | Base::print("", keyFormatter); 88 | } 89 | 90 | 91 | /** Serialization function */ 92 | friend class boost::serialization::access; 93 | template 94 | void serialize(ARCHIVE & ar, const unsigned int version) { 95 | ar & boost::serialization::make_nvp("NoiseModelFactor2", 96 | boost::serialization::base_object(*this)); 97 | } 98 | }; 99 | 100 | } 101 | 102 | 103 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamicsFactorPose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamicsFactorPose2Vector.h 3 | * @brief simple 2D vehicle dynamics factor for mobile arm base in Lie group SE(2) 4 | * @author Jing Dong, Mustafa Mukadam 5 | * @date Oct 14, 2016 6 | **/ 7 | 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | 23 | namespace gpmp2 { 24 | 25 | /** 26 | * unary factor for vehicle dynamics 27 | */ 28 | class VehicleDynamicsFactorPose2Vector: public gtsam::NoiseModelFactor2 { 29 | 30 | private: 31 | // typedefs 32 | typedef VehicleDynamicsFactorPose2Vector This; 33 | typedef gtsam::NoiseModelFactor2 Base; 34 | 35 | public: 36 | 37 | /// shorthand for a smart pointer to a factor 38 | typedef boost::shared_ptr shared_ptr; 39 | 40 | /* Default constructor do nothing */ 41 | VehicleDynamicsFactorPose2Vector() {} 42 | 43 | /** 44 | * Constructor 45 | * @param cost_sigma cost function covariance, should to identity model 46 | */ 47 | VehicleDynamicsFactorPose2Vector(gtsam::Key poseKey, gtsam::Key velKey, double cost_sigma) : 48 | Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, velKey) {} 49 | 50 | virtual ~VehicleDynamicsFactorPose2Vector() {} 51 | 52 | 53 | /// error function 54 | /// numerical/analytic Jacobians from cost function 55 | gtsam::Vector evaluateError(const Pose2Vector& conf, const gtsam::Vector& vel, 56 | boost::optional H1 = boost::none, 57 | boost::optional H2 = boost::none) const { 58 | 59 | using namespace gtsam; 60 | 61 | if (H1 || H2) { 62 | Matrix13 Hp, Hv; 63 | const double err = simple2DVehicleDynamicsPose2(conf.pose(), 64 | vel.head<3>(), Hp, Hv); 65 | if (H1) { 66 | *H1 = Matrix::Zero(1, conf.dim()); 67 | H1->block<1,3>(0,0) = Hp; 68 | } 69 | if (H2) { 70 | *H2 = Matrix::Zero(1, conf.dim()); 71 | H2->block<1,3>(0,0) = Hv; 72 | } 73 | return (Vector(1) << err).finished(); 74 | 75 | } else { 76 | return (Vector(1) << simple2DVehicleDynamicsPose2(conf.pose(), 77 | vel.head<3>())).finished(); 78 | } 79 | } 80 | 81 | 82 | /// @return a deep copy of this factor 83 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 84 | return boost::static_pointer_cast( 85 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 86 | 87 | /** print contents */ 88 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 89 | std::cout << s << "VehicleDynamicsFactorPose2Vector :" << std::endl; 90 | Base::print("", keyFormatter); 91 | } 92 | 93 | 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("NoiseModelFactor2", 99 | boost::serialization::base_object(*this)); 100 | } 101 | }; 102 | 103 | } 104 | 105 | 106 | -------------------------------------------------------------------------------- /gpmp2/dynamics/VehicleDynamicsFactorVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleDynamicsFactorVector.h 3 | * @brief simple 2D vehicle dynamics factor for mobile arm base in vector space 4 | * @author Mustafa Mukadam 5 | * @date Jan 8, 2018 6 | **/ 7 | 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * unary factor for vehicle dynamics 25 | */ 26 | class VehicleDynamicsFactorVector: public gtsam::NoiseModelFactor2 { 27 | 28 | private: 29 | // typedefs 30 | typedef VehicleDynamicsFactorVector This; 31 | typedef gtsam::NoiseModelFactor2 Base; 32 | 33 | public: 34 | 35 | /// shorthand for a smart pointer to a factor 36 | typedef boost::shared_ptr shared_ptr; 37 | 38 | /* Default constructor do nothing */ 39 | VehicleDynamicsFactorVector() {} 40 | 41 | /** 42 | * Constructor 43 | * @param cost_sigma cost function covariance, should to identity model 44 | */ 45 | VehicleDynamicsFactorVector(gtsam::Key poseKey, gtsam::Key velKey, double cost_sigma) : 46 | Base(gtsam::noiseModel::Isotropic::Sigma(1, cost_sigma), poseKey, velKey) {} 47 | 48 | virtual ~VehicleDynamicsFactorVector() {} 49 | 50 | 51 | /// error function 52 | /// numerical/analytic Jacobians from cost function 53 | gtsam::Vector evaluateError(const gtsam::Vector& conf, const gtsam::Vector& vel, 54 | boost::optional H1 = boost::none, 55 | boost::optional H2 = boost::none) const { 56 | 57 | using namespace gtsam; 58 | 59 | if (H1 || H2) { 60 | Matrix13 Hp, Hv; 61 | const double err = simple2DVehicleDynamicsVector3(conf.head<3>(), 62 | vel.head<3>(), Hp, Hv); 63 | if (H1) { 64 | *H1 = Matrix::Zero(1, conf.size()); 65 | H1->block<1,3>(0,0) = Hp; 66 | } 67 | if (H2) { 68 | *H2 = Matrix::Zero(1, conf.size()); 69 | H2->block<1,3>(0,0) = Hv; 70 | } 71 | return (Vector(1) << err).finished(); 72 | 73 | } else { 74 | return (Vector(1) << simple2DVehicleDynamicsVector3(conf.head<3>(), 75 | vel.head<3>())).finished(); 76 | } 77 | } 78 | 79 | 80 | /// @return a deep copy of this factor 81 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 82 | return boost::static_pointer_cast( 83 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 84 | 85 | /** print contents */ 86 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 87 | std::cout << s << "VehicleDynamicsFactorVector :" << std::endl; 88 | Base::print("", keyFormatter); 89 | } 90 | 91 | 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("NoiseModelFactor2", 97 | boost::serialization::base_object(*this)); 98 | } 99 | }; 100 | 101 | } 102 | 103 | 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 | 25 | typedef lie_group_tag structure_category; 26 | 27 | /// @name Group 28 | /// @{ 29 | typedef multiplicative_group_tag group_flavor; 30 | static Class Identity() { return Class::identity();} 31 | /// @} 32 | 33 | /// @name Manifold 34 | /// @{ 35 | typedef Class ManifoldType; 36 | enum { dimension = Class::dimension }; 37 | typedef Eigen::Matrix TangentVector; 38 | typedef OptionalJacobian ChartJacobian; 39 | 40 | static int GetDimension(const Class& m) {return m.dim();} 41 | 42 | static TangentVector Local(const Class& origin, const Class& other, 43 | ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { 44 | return origin.localCoordinates(other, Horigin, Hother); 45 | } 46 | 47 | static Class Retract(const Class& origin, const TangentVector& v, 48 | ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { 49 | return origin.retract(v, Horigin, Hv); 50 | } 51 | /// @} 52 | 53 | /// @name Lie Group 54 | /// @{ 55 | static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { 56 | return Class::Logmap(m, Hm); 57 | } 58 | 59 | static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { 60 | return Class::Expmap(v, Hv); 61 | } 62 | 63 | static Class Compose(const Class& m1, const Class& m2, // 64 | ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { 65 | return m1.compose(m2, H1, H2); 66 | } 67 | 68 | static Class Between(const Class& m1, const Class& m2, // 69 | ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { 70 | return m1.between(m2, H1, H2); 71 | } 72 | 73 | static Class Inverse(const Class& m, // 74 | ChartJacobian H = boost::none) { 75 | return m.inverse(H); 76 | } 77 | /// @} 78 | }; 79 | 80 | /// Both LieGroupTraits and Testable 81 | template struct DynamicLieGroup: DynamicLieGroupTraits, Testable {}; 82 | 83 | } // namespace internal 84 | } // namespace gtsam 85 | 86 | 87 | -------------------------------------------------------------------------------- /gpmp2/geometry/DynamicVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file DynamicVector.h 3 | * @brief Wrapper of dynamic size eigen vector with all Lie group traits implemented 4 | * @author Jing Dong 5 | * @date Oct 3, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include // from gtsam included path, use gtsam eigen version 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * wrapper of dynamic size eigen vector 23 | * replace raw dynamic size eigen vector, which does not have all traits 24 | * fully implemented in gtsam 25 | */ 26 | class GPMP2_EXPORT DynamicVector { 27 | 28 | private: 29 | size_t dim_; 30 | Eigen::VectorXd vector_; 31 | 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | // default constructor do nothing 36 | DynamicVector() : dim_(0) {} 37 | 38 | // eigen constructor 39 | // non-explicit conversion enabled 40 | DynamicVector(const Eigen::VectorXd& vector) : dim_(vector.size()), vector_(vector) {} 41 | 42 | virtual ~DynamicVector() {} 43 | 44 | // access element for convenience 45 | double operator()(size_t i) const { return vector_(i); } 46 | 47 | 48 | /// @name VectorSpace 49 | /// @{ 50 | 51 | // dimension 52 | enum { dimension = Eigen::Dynamic }; // no static dimension implemented 53 | size_t dim() const { return dim_; } 54 | 55 | // no static identity implemented 56 | static DynamicVector identity() { 57 | throw std::runtime_error("[DynamicVector] ERROR: no static identity implemented"); 58 | return DynamicVector(); 59 | } 60 | 61 | // addition 62 | DynamicVector operator+(const DynamicVector& m2) const { 63 | return DynamicVector(this->vector_ + m2.vector_); 64 | } 65 | 66 | // subtraction 67 | DynamicVector operator-(const DynamicVector& m2) const { 68 | return DynamicVector(this->vector_ - m2.vector_); 69 | } 70 | 71 | // inversion 72 | DynamicVector operator-() const { 73 | return DynamicVector(-this->vector_); 74 | } 75 | 76 | // conversion to gtsam::Vector 77 | const Eigen::VectorXd& vector() const { return vector_; } 78 | 79 | // addition of a vector on the right 80 | DynamicVector operator+(const Eigen::VectorXd& vec) const { 81 | return DynamicVector(this->vector_ + vec); 82 | } 83 | 84 | /// @} 85 | /// @name Testable 86 | /// @{ 87 | 88 | // print 89 | void print(const std::string& s) const { 90 | std::cout << s << vector_ << std::endl; 91 | } 92 | 93 | // equals 94 | bool equals(const DynamicVector& m2, double tol) const { 95 | return gtsam::equal_with_abs_tol(this->vector_, m2.vector_, tol); 96 | } 97 | 98 | /// @} 99 | }; 100 | 101 | } // \ namespace gpmp2 102 | 103 | 104 | // gtsam traits for DynamicVector 105 | namespace gtsam { 106 | 107 | template<> 108 | struct traits : public internal::VectorSpace { 109 | }; 110 | 111 | } 112 | -------------------------------------------------------------------------------- /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 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace gpmp2 { 21 | 22 | /** 23 | * Pose2Vector is the Lie group product of gtsam Pose2 and eigen vector 24 | * use to describe the pose of a mobile manipulator with planner base 25 | */ 26 | class GPMP2_EXPORT Pose2Vector : public ProductDynamicLieGroup { 27 | 28 | private: 29 | typedef ProductDynamicLieGroup Base; 30 | 31 | public: 32 | // default constructor do nothing 33 | Pose2Vector() : Base() {} 34 | 35 | // constructors 36 | Pose2Vector(const gtsam::Pose2& p, const DynamicVector& c) : Base(p, c) {} 37 | Pose2Vector(const gtsam::Pose2& p, const gtsam::Vector& c) : 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 | 56 | } // \ namespace gpmp2 57 | 58 | 59 | // gtsam traits for Pose2Vector 60 | namespace gtsam { 61 | template<> struct traits : internal::DynamicLieGroupTraits { 62 | 63 | static void Print(const gpmp2::Pose2Vector& m, const std::string& s = "") { 64 | std::cout << s; 65 | m.first.print("Pose: \n"); 66 | m.second.print("Configuration: \n"); 67 | } 68 | 69 | static bool Equals(const gpmp2::Pose2Vector& m1, const gpmp2::Pose2Vector& m2, 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 | } 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 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | namespace gpmp2 { 23 | 24 | 25 | template 26 | gtsam::Matrix numericalDerivativeDynamic(boost::function h, const X& x, 27 | double delta = 1e-5) { 28 | 29 | BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), 31 | "Template argument Y must be a manifold type."); 32 | typedef gtsam::traits TraitsY; 33 | typedef typename TraitsY::TangentVector TangentY; 34 | 35 | BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), 37 | "Template argument X must be a manifold type."); 38 | static const int N = DimensionUtils::dimension>::getDimension(x) ; 39 | typedef gtsam::traits TraitsX; 40 | typedef typename TraitsX::TangentVector TangentX; 41 | 42 | // get value at x, and corresponding chart 43 | const Y hx = h(x); 44 | 45 | // Bit of a hack for now to find number of rows 46 | const TangentY zeroY = TraitsY::Local(hx, hx); 47 | const size_t m = zeroY.size(); 48 | 49 | // Prepare a tangent vector to perturb x with, only works for fixed size 50 | TangentX dx; 51 | dx.setZero(N); 52 | 53 | // Fill in Jacobian H 54 | gtsam::Matrix H = gtsam::Matrix::Zero(m, N); 55 | const double factor = 1.0 / (2.0 * delta); 56 | for (int j = 0; j < N; j++) { 57 | dx(j) = delta; 58 | const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); 59 | dx(j) = -delta; 60 | const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); 61 | dx(j) = 0; 62 | H.col(j) << (dy1 - dy2) * factor; 63 | } 64 | return H; 65 | } 66 | 67 | /** use a raw C++ function pointer */ 68 | template 69 | typename gtsam::Matrix numericalDerivativeDynamic(Y (*h)(const X&), const X& x, 70 | double delta = 1e-5) { 71 | return numericalDerivativeDynamic(boost::bind(h, _1), x, delta); 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /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 | 13 | namespace gpmp2 { 14 | 15 | /// get dimension from fixed size type 16 | template 17 | struct DimensionUtils { 18 | static inline int getDimension(const T&) { 19 | return N; 20 | } 21 | }; 22 | 23 | /// template specialization get dimension from dynamic size type 24 | template 25 | struct DimensionUtils { 26 | static inline int getDimension(const T& m) { 27 | return m.dim(); 28 | } 29 | }; 30 | 31 | } // namespace gpmp2 32 | 33 | 34 | -------------------------------------------------------------------------------- /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 | 13 | namespace gpmp2 { 14 | 15 | /* ************************************************************************** */ 16 | Matrix getQc(const SharedNoiseModel Qc_model) { 17 | noiseModel::Gaussian *Gassian_model = 18 | dynamic_cast(Qc_model.get()); 19 | return (Gassian_model->R().transpose() * Gassian_model->R()).inverse(); 20 | } 21 | 22 | } // namespace gpmp2 23 | 24 | 25 | -------------------------------------------------------------------------------- /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 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | 19 | namespace gpmp2 { 20 | 21 | /// get Qc covariance matrix from noise model 22 | GPMP2_EXPORT gtsam::Matrix getQc(const gtsam::SharedNoiseModel Qc_model); 23 | 24 | /// calculate Q 25 | inline gtsam::Matrix calcQ(const gtsam::Matrix& Qc, double tau) { 26 | assert(Qc.rows() == Qc.cols()); 27 | return (gtsam::Matrix(2*Qc.rows(), 2*Qc.rows()) << 28 | 1.0 / 3 * pow(tau, 3.0) * Qc, 1.0 / 2 * pow(tau, 2.0) * Qc, 29 | 1.0 / 2 * pow(tau, 2.0) * Qc, tau * Qc).finished(); 30 | } 31 | 32 | /// calculate Q_inv 33 | inline gtsam::Matrix calcQ_inv(const gtsam::Matrix& Qc, double tau) { 34 | assert(Qc.rows() == Qc.cols()); 35 | const gtsam::Matrix Qc_inv = Qc.inverse(); 36 | return (gtsam::Matrix(2*Qc.rows(), 2*Qc.rows()) << 37 | 12.0 * pow(tau, -3.0) * Qc_inv, (-6.0) * pow(tau, -2.0) * Qc_inv, 38 | (-6.0) * pow(tau, -2.0) * Qc_inv, 4.0 * pow(tau, -1.0) * Qc_inv).finished(); 39 | } 40 | 41 | /// calculate Phi 42 | inline gtsam::Matrix calcPhi(size_t dof, double tau) { 43 | return (gtsam::Matrix(2*dof, 2*dof) << 44 | gtsam::Matrix::Identity(dof, dof), tau * gtsam::Matrix::Identity(dof, dof), 45 | gtsam::Matrix::Zero(dof, dof), gtsam::Matrix::Identity(dof, dof)).finished(); 46 | } 47 | 48 | /// calculate Lambda 49 | inline gtsam::Matrix calcLambda(const gtsam::Matrix& Qc, double delta_t, const double tau) { 50 | assert(Qc.rows() == Qc.cols()); 51 | return calcPhi(Qc.rows(), tau) - calcQ(Qc, tau) * (calcPhi(Qc.rows(), delta_t - tau).transpose()) 52 | * calcQ_inv(Qc, delta_t) * calcPhi(Qc.rows(), delta_t); 53 | } 54 | 55 | /// calculate Psi 56 | inline gtsam::Matrix calcPsi(const gtsam::Matrix& Qc, double delta_t, double tau) { 57 | assert(Qc.rows() == Qc.cols()); 58 | return calcQ(Qc, tau) * (calcPhi(Qc.rows(), delta_t - tau).transpose()) * calcQ_inv(Qc, delta_t); 59 | } 60 | 61 | } // namespace gpmp2 62 | 63 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose2.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, works only in SE(2) 4 | * @author Jing Dong 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessInterpolatorLie GaussianProcessInterpolatorPose2; 16 | 17 | } // \ namespace gpmp2 18 | 19 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose2Vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose2Vector.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, works only in SE(2) + Vector space 4 | * @author Jing Dong 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | namespace gpmp2 { 13 | 14 | typedef GaussianProcessInterpolatorLie GaussianProcessInterpolatorPose2Vector; 15 | 16 | } // \ namespace gpmp2 17 | 18 | -------------------------------------------------------------------------------- /gpmp2/gp/GaussianProcessInterpolatorPose3.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessInterpolatorPose3.h 3 | * @brief Base and utils for Gaussian Process Interpolated measurement factor, works only in SE(3) 4 | * @author Jing Dong 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessInterpolatorLie GaussianProcessInterpolatorPose3; 16 | 17 | } // \ namespace gpmp2 18 | 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 | 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessPriorLie GaussianProcessPriorPose2; 16 | 17 | } // \ namespace gpmp2 18 | 19 | -------------------------------------------------------------------------------- /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 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessPriorLie GaussianProcessPriorPose2Vector; 16 | 17 | } // \ namespace gpmp2 18 | 19 | -------------------------------------------------------------------------------- /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 | 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | typedef GaussianProcessPriorLie GaussianProcessPriorPose3; 16 | 17 | } // \ namespace gpmp2 18 | 19 | -------------------------------------------------------------------------------- /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 | } 22 | 23 | -------------------------------------------------------------------------------- /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 | 11 | namespace gpmp2 { 12 | 13 | /* ************************************************************************** */ 14 | template 15 | gtsam::Matrix ForwardKinematics::forwardKinematicsPose(const Pose& jp) const { 16 | 17 | std::vector jpx; 18 | forwardKinematics(jp, boost::none, jpx, boost::none); 19 | 20 | // convert vector in matrix 21 | gtsam::Matrix jpx_mat(6, nr_links_); 22 | for (size_t i = 0; i < nr_links_; i++) 23 | jpx_mat.col(i) = (gtsam::Vector6() << gtsam::Vector3(jpx[i].rotation().yaw(), 24 | jpx[i].rotation().pitch(), jpx[i].rotation().roll()), 25 | jpx[i].translation().vector()).finished(); 26 | return jpx_mat; 27 | } 28 | 29 | /* ************************************************************************** */ 30 | template 31 | gtsam::Matrix ForwardKinematics::forwardKinematicsPosition(const Pose& jp) const { 32 | 33 | std::vector jpx; 34 | forwardKinematics(jp, boost::none, jpx, boost::none); 35 | 36 | // convert vector in matrix 37 | gtsam::Matrix jpx_mat(3, nr_links_); 38 | for (size_t i = 0; i < nr_links_; i++) 39 | jpx_mat.col(i) = jpx[i].translation().vector(); 40 | return jpx_mat; 41 | } 42 | 43 | /* ************************************************************************** */ 44 | template 45 | gtsam::Matrix ForwardKinematics::forwardKinematicsVel(const Pose& jp, 46 | const Velocity& jv) const { 47 | 48 | std::vector jpx; 49 | std::vector jvx; 50 | forwardKinematics(jp, jv, jpx, jvx); 51 | 52 | // convert vector in matrix 53 | gtsam::Matrix jpv_mat(3, nr_links_); 54 | for (size_t i = 0; i < nr_links_; i++) 55 | jpv_mat.col(i) = jvx[i]; 56 | return jpv_mat; 57 | } 58 | 59 | } 60 | -------------------------------------------------------------------------------- /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 | 14 | #include 15 | 16 | 17 | namespace gpmp2 { 18 | 19 | /** 20 | * Abstract forward kinematics model, without actual model and physical representation 21 | * template parameters are system pose and velocity state types 22 | */ 23 | template 24 | class ForwardKinematics { 25 | 26 | private: 27 | size_t dof_; // system degree of freedom 28 | size_t nr_links_; // number of links (piece of robot part) 29 | 30 | 31 | public: 32 | /// type defs 33 | typedef POSE Pose; 34 | typedef VELOCITY Velocity; 35 | 36 | /// default contructor do nothing, for serialization 37 | ForwardKinematics() : dof_(0), nr_links_(0) {} 38 | 39 | /// Contructor take system DOF and number of links 40 | /// and the base pose (default zero pose) 41 | ForwardKinematics(size_t dof, size_t nr_links) : dof_(dof), nr_links_(nr_links) {} 42 | 43 | /// Default destructor 44 | virtual ~ForwardKinematics() {} 45 | 46 | 47 | /** 48 | * Forward kinematics: poses from configuration space to 3D workspace 49 | * Velocity kinematics: optional velocities from configuration space to 3D workspace, no angular rate 50 | * pure virtual method, need implementation in derived class 51 | * 52 | * @param jp robot pose in config space 53 | * @param jv robot velocity in config space 54 | * @param jpx link poses in 3D work space 55 | * @param jvx link velocities in 3D work space, no angular rate 56 | * @param J_jpx_jp et al. optional Jacobians 57 | **/ 58 | virtual void forwardKinematics(const Pose& jp, boost::optional jv, 59 | std::vector& jpx, boost::optional&> jvx, 60 | boost::optional&> J_jpx_jp = boost::none, 61 | boost::optional&> J_jvx_jp = boost::none, 62 | boost::optional&> J_jvx_jv = boost::none) const = 0; 63 | 64 | 65 | /** 66 | * Matrix wrapper for forwardKinematics, mainly used by matlab 67 | * each column is a single point / velocity of the joint, size 6xN, 3xN, 3xN 68 | * No jacobians provided by this version 69 | */ 70 | gtsam::Matrix forwardKinematicsPose(const Pose& jp) const; 71 | gtsam::Matrix forwardKinematicsPosition(const Pose& jp) const; 72 | gtsam::Matrix forwardKinematicsVel(const Pose& jp, const Velocity& jv) const; 73 | 74 | 75 | /// accesses 76 | size_t dof() const { return dof_; } 77 | size_t nr_links() const { return nr_links_; } 78 | 79 | }; 80 | 81 | } 82 | 83 | #include 84 | 85 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspaceOrientation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspaceOrientation.h 3 | * @brief Gaussian prior defined on the workspace orientation 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 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Gaussian prior defined on the workspace orientation 23 | */ 24 | template 25 | class GaussianPriorWorkspaceOrientation: public gtsam::NoiseModelFactor1 { 26 | 27 | public: 28 | // typedefs 29 | typedef ROBOT Robot; 30 | typedef typename Robot::Pose Pose; 31 | 32 | private: 33 | // typedefs 34 | typedef GaussianPriorWorkspaceOrientation This; 35 | typedef gtsam::NoiseModelFactor1 Base; 36 | 37 | const Robot& robot_; // Robot 38 | int joint_; // joint on the robot to be constrained 39 | gtsam::Rot3 des_orientation_; // desired workspace orientation for joint 40 | 41 | public: 42 | /* Default constructor do nothing */ 43 | GaussianPriorWorkspaceOrientation() {} 44 | 45 | /// Constructor 46 | GaussianPriorWorkspaceOrientation(gtsam::Key poseKey, const Robot& robot, int joint, 47 | const gtsam::Rot3& des_orientation, const gtsam::SharedNoiseModel& cost_model) : 48 | Base(cost_model, poseKey), robot_(robot), joint_(joint), des_orientation_(des_orientation){} 49 | 50 | virtual ~GaussianPriorWorkspaceOrientation() {} 51 | 52 | /// factor error function 53 | gtsam::Vector evaluateError( 54 | const Pose& pose, boost::optional H1 = boost::none) const { 55 | 56 | using namespace gtsam; 57 | 58 | std::vector joint_pos; 59 | std::vector J_jpx_jp; 60 | robot_.fk_model().forwardKinematics(pose, boost::none, joint_pos, boost::none, J_jpx_jp); 61 | 62 | if (H1) { 63 | Matrix36 H_rp; 64 | Matrix33 H_er; 65 | Rot3 curr_orientation = joint_pos[joint_].rotation(H_rp); 66 | Vector error = des_orientation_.logmap(curr_orientation, boost::none, H_er); 67 | *H1 = H_er * H_rp * J_jpx_jp[joint_]; 68 | return error; 69 | } 70 | else { 71 | return des_orientation_.logmap(joint_pos[joint_].rotation()); 72 | } 73 | } 74 | 75 | 76 | /// @return a deep copy of this factor 77 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 78 | return boost::static_pointer_cast( 79 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 80 | 81 | /** print contents */ 82 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 83 | std::cout << s << "GaussianPriorWorkspaceOrientation :" << std::endl; 84 | Base::print("", keyFormatter); 85 | std::cout << "desired orientation : "; des_orientation_.print(); 86 | } 87 | 88 | private: 89 | 90 | /** Serialization function */ 91 | friend class boost::serialization::access; 92 | template 93 | void serialize(ARCHIVE & ar, const unsigned int version) { 94 | ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 95 | } 96 | 97 | }; 98 | 99 | } 100 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspaceOrientationArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspaceOrientationArm.h 3 | * @brief Gaussian prior defined on the workspace Orientation 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 GaussianPriorWorkspaceOrientation GaussianPriorWorkspaceOrientationArm; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /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 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Gaussian prior defined on the workspace pose 23 | */ 24 | template 25 | class GaussianPriorWorkspacePose: public gtsam::NoiseModelFactor1 { 26 | 27 | public: 28 | // typedefs 29 | typedef ROBOT Robot; 30 | typedef typename Robot::Pose Pose; 31 | 32 | private: 33 | // typedefs 34 | typedef GaussianPriorWorkspacePose This; 35 | typedef gtsam::NoiseModelFactor1 Base; 36 | 37 | const Robot& robot_; // Robot 38 | int joint_; // joint on the robot to be constrained 39 | gtsam::Pose3 des_pose_; // desired workspace pose for joint 40 | 41 | public: 42 | /* Default constructor do nothing */ 43 | GaussianPriorWorkspacePose() {} 44 | 45 | /// Constructor 46 | GaussianPriorWorkspacePose(gtsam::Key poseKey, const Robot& robot, int joint, 47 | const gtsam::Pose3& des_pose, const gtsam::SharedNoiseModel& cost_model) : 48 | Base(cost_model, poseKey), robot_(robot), joint_(joint), des_pose_(des_pose){} 49 | 50 | virtual ~GaussianPriorWorkspacePose() {} 51 | 52 | /// factor error function 53 | gtsam::Vector evaluateError( 54 | const Pose& pose, boost::optional H1 = boost::none) const { 55 | 56 | using namespace gtsam; 57 | 58 | std::vector joint_pos; 59 | std::vector J_jpx_jp; 60 | robot_.fk_model().forwardKinematics(pose, boost::none, joint_pos, boost::none, J_jpx_jp); 61 | 62 | if (H1) { 63 | Matrix66 H_ep; 64 | Vector error = des_pose_.logmap(joint_pos[joint_], boost::none, H_ep); 65 | *H1 = H_ep * J_jpx_jp[joint_]; 66 | return error; 67 | } 68 | else { 69 | return des_pose_.logmap(joint_pos[joint_]); 70 | } 71 | } 72 | 73 | 74 | /// @return a deep copy of this factor 75 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 76 | return boost::static_pointer_cast( 77 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 78 | 79 | /** print contents */ 80 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 81 | std::cout << s << "GaussianPriorWorkspacePose :" << std::endl; 82 | Base::print("", keyFormatter); 83 | std::cout << "desired pose : "; des_pose_.print(); 84 | } 85 | 86 | private: 87 | 88 | /** Serialization function */ 89 | friend class boost::serialization::access; 90 | template 91 | void serialize(ARCHIVE & ar, const unsigned int version) { 92 | ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 93 | } 94 | 95 | }; 96 | 97 | } 98 | -------------------------------------------------------------------------------- /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 | } 20 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePosition.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePosition.h 3 | * @brief Gaussian prior defined on the workspace position 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 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace gpmp2 { 20 | 21 | /** 22 | * Gaussian prior defined on the workspace position 23 | */ 24 | template 25 | class GaussianPriorWorkspacePosition: public gtsam::NoiseModelFactor1 { 26 | 27 | public: 28 | // typedefs 29 | typedef ROBOT Robot; 30 | typedef typename Robot::Pose Pose; 31 | 32 | private: 33 | // typedefs 34 | typedef GaussianPriorWorkspacePosition This; 35 | typedef gtsam::NoiseModelFactor1 Base; 36 | 37 | const Robot& robot_; // Robot 38 | int joint_; // joint on the robot to be constrained 39 | gtsam::Point3 des_position_; // desired workspace position for joint 40 | 41 | public: 42 | /* Default constructor do nothing */ 43 | GaussianPriorWorkspacePosition() {} 44 | 45 | /// Constructor 46 | GaussianPriorWorkspacePosition(gtsam::Key poseKey, const Robot& robot, int joint, 47 | const gtsam::Point3& des_position, const gtsam::SharedNoiseModel& cost_model) : 48 | Base(cost_model, poseKey), robot_(robot), joint_(joint), des_position_(des_position){} 49 | 50 | virtual ~GaussianPriorWorkspacePosition() {} 51 | 52 | /// factor error function 53 | gtsam::Vector evaluateError( 54 | const Pose& pose, boost::optional H1 = boost::none) const { 55 | 56 | using namespace gtsam; 57 | 58 | std::vector joint_pos; 59 | std::vector J_jpx_jp; 60 | robot_.fk_model().forwardKinematics(pose, boost::none, joint_pos, boost::none, J_jpx_jp); 61 | 62 | if (H1) { 63 | Matrix36 Hpp; 64 | Point3 curr_position = joint_pos[joint_].translation(Hpp); 65 | *H1 = Hpp * J_jpx_jp[joint_]; 66 | return curr_position.vector() - des_position_.vector(); 67 | } 68 | else { 69 | return joint_pos[joint_].translation().vector() - des_position_.vector(); 70 | } 71 | } 72 | 73 | 74 | /// @return a deep copy of this factor 75 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 76 | return boost::static_pointer_cast( 77 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 78 | 79 | /** print contents */ 80 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 81 | std::cout << s << "GaussianPriorWorkspacePosition :" << std::endl; 82 | Base::print("", keyFormatter); 83 | std::cout << "desired position : "; des_position_.print(); 84 | } 85 | 86 | private: 87 | 88 | /** Serialization function */ 89 | friend class boost::serialization::access; 90 | template 91 | void serialize(ARCHIVE & ar, const unsigned int version) { 92 | ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 93 | } 94 | 95 | }; 96 | 97 | } 98 | -------------------------------------------------------------------------------- /gpmp2/kinematics/GaussianPriorWorkspacePositionArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianPriorWorkspacePositionArm.h 3 | * @brief Gaussian prior defined on the workspace position 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 GaussianPriorWorkspacePosition GaussianPriorWorkspacePositionArm; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /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 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * unary factor connect to the last pose in arm configuration space 25 | */ 26 | class GoalFactorArm: public gtsam::NoiseModelFactor1 { 27 | 28 | private: 29 | // typedefs 30 | typedef GoalFactorArm This; 31 | typedef gtsam::NoiseModelFactor1 Base; 32 | 33 | // arm 34 | Arm arm_; 35 | 36 | // destination point 37 | gtsam::Point3 dest_point_; 38 | 39 | public: 40 | 41 | /// shorthand for a smart pointer to a factor 42 | typedef boost::shared_ptr shared_ptr; 43 | 44 | GoalFactorArm() {} /* Default constructor do nothing */ 45 | 46 | /** 47 | * Constructor 48 | * @param cost_model cost function covariance 49 | */ 50 | GoalFactorArm(gtsam::Key poseKey, const gtsam::SharedNoiseModel& cost_model, 51 | const Arm& arm, const gtsam::Point3& dest_point) : 52 | Base(cost_model, poseKey), arm_(arm), dest_point_(dest_point) {} 53 | 54 | virtual ~GoalFactorArm() {} 55 | 56 | 57 | /// error function 58 | gtsam::Vector evaluateError( 59 | const gtsam::Vector& conf, boost::optional H1 = boost::none) const { 60 | 61 | using namespace gtsam; 62 | 63 | // fk 64 | std::vector joint_pos; 65 | std::vector J_jpx_jp; 66 | arm_.forwardKinematics(conf, boost::none, joint_pos, boost::none, J_jpx_jp); 67 | 68 | if (H1) { 69 | Matrix36 Hpp; 70 | Point3 end_point = joint_pos[arm_.dof() - 1].translation(Hpp); 71 | *H1 = Hpp * J_jpx_jp[arm_.dof() - 1]; 72 | return end_point.vector() - dest_point_.vector(); 73 | 74 | } else { 75 | return joint_pos[arm_.dof() - 1].translation().vector() - dest_point_.vector(); 76 | } 77 | } 78 | 79 | 80 | /// @return a deep copy of this factor 81 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 82 | return boost::static_pointer_cast( 83 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 84 | 85 | /** print contents */ 86 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 87 | std::cout << s << "GoalFactorArm :" << std::endl; 88 | Base::print("", keyFormatter); 89 | std::cout << "dest : "; dest_point_.print(); 90 | } 91 | 92 | private: 93 | 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("NoiseModelFactor4", 99 | boost::serialization::base_object(*this)); 100 | } 101 | }; 102 | 103 | } 104 | -------------------------------------------------------------------------------- /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 | 13 | namespace gpmp2 { 14 | 15 | /// hinge loss joint limit cost function 16 | inline double hingeLossJointLimitCost(double p, double down_limit, double up_limit, 17 | double thresh, boost::optional H_p = boost::none) { 18 | 19 | if (p < down_limit + thresh) { 20 | if (H_p) *H_p = -1.0; 21 | return down_limit + thresh - p; 22 | 23 | } else if (p <= up_limit - thresh) { 24 | if (H_p) *H_p = 0.0; 25 | return 0.0; 26 | 27 | } else { 28 | if (H_p) *H_p = 1.0; 29 | return p - up_limit + thresh; 30 | } 31 | } 32 | 33 | } // namespace gpmp2 34 | -------------------------------------------------------------------------------- /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 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | 20 | namespace gpmp2 { 21 | 22 | /** 23 | * unary factor to apply joint limit cost to vector configuration space 24 | */ 25 | class JointLimitFactorVector: public gtsam::NoiseModelFactor1 { 26 | 27 | private: 28 | // typedefs 29 | typedef JointLimitFactorVector This; 30 | typedef gtsam::NoiseModelFactor1 Base; 31 | 32 | // joint limit value 33 | gtsam::Vector down_limit_, up_limit_; 34 | 35 | // hinge loss threshold 36 | gtsam::Vector limit_thresh_; 37 | 38 | public: 39 | 40 | /// shorthand for a smart pointer to a factor 41 | typedef boost::shared_ptr shared_ptr; 42 | 43 | /** 44 | * Constructor 45 | * @param cost_model joint limit cost weight 46 | * @param limit_thresh hinge loss threshold 47 | */ 48 | JointLimitFactorVector(gtsam::Key poseKey, const gtsam::SharedNoiseModel& cost_model, 49 | const gtsam::Vector& down_limit, const gtsam::Vector& up_limit, const gtsam::Vector& limit_thresh) : 50 | Base(cost_model, poseKey), down_limit_(down_limit), up_limit_(up_limit), 51 | limit_thresh_(limit_thresh) { 52 | // check dimensions 53 | if ((size_t)down_limit.size() != cost_model->dim() 54 | || (size_t)up_limit.size() != cost_model->dim() 55 | || (size_t)limit_thresh.size() != cost_model->dim()) 56 | throw std::runtime_error("[JointLimitFactorVector] ERROR: limit vector dim does not fit."); 57 | } 58 | 59 | virtual ~JointLimitFactorVector() {} 60 | 61 | /// error function 62 | gtsam::Vector evaluateError(const gtsam::Vector& conf, 63 | boost::optional H1 = boost::none) const { 64 | 65 | using namespace gtsam; 66 | if (H1) 67 | *H1 = Matrix::Zero(conf.size(), conf.size()); 68 | Vector err(conf.size()); 69 | for (size_t i = 0; i < (size_t)conf.size(); i++) { 70 | if (H1) { 71 | double Hp; 72 | err(i) = hingeLossJointLimitCost(conf(i), down_limit_(i), up_limit_(i), limit_thresh_(i), Hp); 73 | (*H1)(i, i) = Hp; 74 | } else { 75 | err(i) = hingeLossJointLimitCost(conf(i), down_limit_(i), up_limit_(i), limit_thresh_(i)); 76 | } 77 | } 78 | return err; 79 | } 80 | 81 | 82 | /// @return a deep copy of this factor 83 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 84 | return boost::static_pointer_cast( 85 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 86 | 87 | /** print contents */ 88 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 89 | std::cout << s << "JointLimitFactorVector :" << std::endl; 90 | Base::print("", keyFormatter); 91 | std::cout << "Limit cost threshold : " << limit_thresh_ << std::endl; 92 | } 93 | 94 | private: 95 | 96 | /** Serialization function */ 97 | friend class boost::serialization::access; 98 | template 99 | void serialize(ARCHIVE & ar, const unsigned int version) { 100 | ar & boost::serialization::make_nvp("NoiseModelFactor1", 101 | boost::serialization::base_object(*this)); 102 | } 103 | }; 104 | 105 | } // namespace gpmp2 106 | -------------------------------------------------------------------------------- /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, boost::optional jv, 17 | std::vector& jpx, boost::optional&> jvx, 18 | boost::optional&> J_jpx_jp, 19 | boost::optional&> J_jvx_jp, 20 | boost::optional&> J_jvx_jv) const { 21 | 22 | // space for output 23 | jpx.resize(nr_links()); 24 | if (jvx) jvx->resize(nr_links()); 25 | if (J_jpx_jp) J_jpx_jp->assign(nr_links(), Matrix::Zero(6, dof())); 26 | if (J_jvx_jp) J_jvx_jp->assign(nr_links(), Matrix::Zero(3, dof())); 27 | if (J_jvx_jv) J_jvx_jv->assign(nr_links(), Matrix::Zero(3, dof())); 28 | 29 | Matrix H1, H2; 30 | 31 | // start calculating Forward and velocity kinematics and Jacobians 32 | for (size_t i = 0; i < nr_links(); i++) { 33 | // pose in workspace 34 | jpx[i] = Pose3::Create(Rot3(), Point3(jp[0],jp[1],0), H1, H2); 35 | 36 | // velocity in workspace 37 | if (jv && jvx) 38 | (*jvx)[i] << (*jv)[0], (*jv)[1], 0; 39 | 40 | // Jacobians 41 | if (J_jpx_jp) { 42 | (*J_jpx_jp)[i].col(0) = H2.col(0); 43 | (*J_jpx_jp)[i].col(1) = H2.col(1); 44 | } 45 | if (J_jvx_jv) { 46 | (*J_jvx_jv)[i].block<2,2>(0, 0) = Matrix::Identity(2,2); 47 | } 48 | } 49 | } 50 | 51 | } // namespace gpmp2 52 | -------------------------------------------------------------------------------- /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 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * Abstract PointRobot class without any physical model representation 25 | * Inherited from ForwardKinematics 26 | */ 27 | class GPMP2_EXPORT PointRobot : public ForwardKinematics { 28 | 29 | private: 30 | // typedefs 31 | typedef ForwardKinematics Base; 32 | 33 | public: 34 | /// default constructor do nothing 35 | PointRobot() {} 36 | 37 | /// Constructor takes in DOF 38 | PointRobot(size_t dof, size_t nr_links) : Base(dof, nr_links) {} 39 | 40 | /// Default destructor 41 | virtual ~PointRobot() {} 42 | 43 | 44 | /** 45 | * Forward kinematics: robot configuration to poses in workspace 46 | * Velocity kinematics: optional robot velocities to linear velocities in workspace, no angular rate 47 | * 48 | * @param jp robot pose in config space 49 | * @param jv robot velocity in config space 50 | * @param jpx robot pose in work space 51 | * @param jvx robot velocity in work space 52 | * @param J_jpx_jp et al. optional Jacobians 53 | **/ 54 | void forwardKinematics(const gtsam::Vector& jp, boost::optional jv, 55 | std::vector& jpx, boost::optional&> jvx, 56 | boost::optional&> J_jpx_jp = boost::none, 57 | boost::optional&> J_jvx_jp = boost::none, 58 | boost::optional&> J_jvx_jv = boost::none) const; 59 | 60 | 61 | }; // PointRobot 62 | 63 | } // namespace gpmp2 64 | -------------------------------------------------------------------------------- /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 | } 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 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * Abstract plannar mobile manipulator with 2 arm 25 | * pose class is Pose2Vector, arm1 uses first arm1.dof as pose, then arm2 uses last arm2.dof 26 | * total DOF = 3 + arm1.dof + arm2.dof 27 | */ 28 | class GPMP2_EXPORT Pose2Mobile2Arms : public ForwardKinematics { 29 | 30 | private: 31 | // typedefs 32 | typedef ForwardKinematics Base; 33 | 34 | // base to arm pose 35 | gtsam::Pose3 base_T_arm1_, base_T_arm2_; 36 | // arm class 37 | Arm arm1_, arm2_; 38 | 39 | public: 40 | /// default contructor do nothing 41 | Pose2Mobile2Arms() {} 42 | 43 | /// constructor from Arm 44 | Pose2Mobile2Arms(const Arm& arm1, const Arm& arm2, 45 | const gtsam::Pose3& base_T_arm1 = gtsam::Pose3(), 46 | const gtsam::Pose3& base_T_arm2 = gtsam::Pose3()); 47 | 48 | /// Default destructor 49 | virtual ~Pose2Mobile2Arms() {} 50 | 51 | 52 | /** 53 | * Forward kinematics: joint configuration to poses in workspace 54 | * Velocity kinematics: optional joint velocities to linear velocities in workspace, no anuglar rate 55 | * 56 | * @param p position in config space 57 | * @param v velocity in config space 58 | * @param px link pose in work space 59 | * @param vx link velocity in work space 60 | * @param J_px_p et al. optional Jacobians 61 | **/ 62 | void forwardKinematics(const Pose2Vector& p, boost::optional v, 63 | std::vector& px, boost::optional&> vx, 64 | boost::optional&> J_px_p = boost::none, 65 | boost::optional&> J_vx_p = boost::none, 66 | boost::optional&> J_vx_v = boost::none) const; 67 | 68 | 69 | /// accesses 70 | const Arm& arm1() const { return arm1_; } 71 | const Arm& arm2() const { return arm2_; } 72 | const gtsam::Pose3& base_T_arm1() const { return base_T_arm1_; } 73 | const gtsam::Pose3& base_T_arm2() const { return base_T_arm2_; } 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2Mobile2ArmsModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2Mobile2ArmsModel.h 3 | * @brief mobile pose2 + 2 x Arm with physical body, which represented by spheres 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 | /** 16 | * Pose2 + Arm with physical body, which is represented by spheres 17 | * Used to check collisions 18 | */ 19 | typedef RobotModel Pose2Mobile2ArmsModel; 20 | 21 | } 22 | 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 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * Abstract plannar mobile manipulator, without any physical model representation 25 | * Inherited from ForwardKinematics 26 | */ 27 | class GPMP2_EXPORT Pose2MobileArm : public ForwardKinematics { 28 | 29 | private: 30 | // typedefs 31 | typedef ForwardKinematics Base; 32 | 33 | // base to arm pose 34 | gtsam::Pose3 base_T_arm_; 35 | // arm class 36 | Arm arm_; 37 | 38 | public: 39 | /// default contructor do nothing 40 | Pose2MobileArm() {} 41 | 42 | /// constructor from Arm 43 | explicit Pose2MobileArm(const Arm& arm, const gtsam::Pose3& base_T_arm = gtsam::Pose3()); 44 | 45 | /// Default destructor 46 | virtual ~Pose2MobileArm() {} 47 | 48 | 49 | /** 50 | * Forward kinematics: joint configuration to poses in workspace 51 | * Velocity kinematics: optional joint velocities to linear velocities in workspace, no anuglar rate 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, boost::optional v, 60 | std::vector& px, boost::optional&> vx, 61 | boost::optional&> J_px_p = boost::none, 62 | boost::optional&> J_vx_p = boost::none, 63 | boost::optional&> J_vx_v = boost::none) const; 64 | 65 | 66 | /// accesses 67 | const gtsam::Pose3& base_T_arm() const { return base_T_arm_; } 68 | const Arm& arm() const { return arm_; } 69 | }; 70 | 71 | } 72 | -------------------------------------------------------------------------------- /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 | } 22 | 23 | -------------------------------------------------------------------------------- /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 | 17 | namespace gpmp2 { 18 | 19 | /* ************************************************************************** */ 20 | void Pose2MobileBase::forwardKinematics( 21 | const gtsam::Pose2& p, boost::optional v, 22 | std::vector& px, boost::optional&> vx, 23 | boost::optional&> J_px_p, 24 | boost::optional&> J_vx_p, 25 | boost::optional&> J_vx_v) const { 26 | 27 | if (v) 28 | throw runtime_error("[Pose2MobileBase] TODO: velocity not implemented"); 29 | 30 | if (!v && (vx || J_vx_p || J_vx_v)) 31 | throw runtime_error("[Pose2MobileBase] ERROR: only ask for velocity in workspace given velocity in " 32 | "configuration space"); 33 | 34 | // allocate space 35 | px.resize(nr_links()); 36 | if (vx) vx->resize(nr_links()); 37 | if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof())); 38 | if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof())); 39 | if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof())); 40 | 41 | // assign values 42 | Matrix63 Hveh_base; 43 | if (J_px_p || J_vx_p || J_vx_v) { 44 | px[0] = computeBasePose3(p, Hveh_base); 45 | } else { 46 | px[0] = computeBasePose3(p); 47 | } 48 | if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base; 49 | if (vx) { 50 | (*vx)[0] = Vector3((*v)[0], (*v)[1], 0.0); 51 | // (*J_vx_p)[0] is zero 52 | if (J_vx_v) 53 | (*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 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | namespace gpmp2 { 21 | 22 | /** 23 | * Abstract SE(2) mobile base inherited from ForwardKinematics 24 | */ 25 | class GPMP2_EXPORT Pose2MobileBase : public ForwardKinematics { 26 | 27 | private: 28 | // typedefs 29 | typedef ForwardKinematics Base; 30 | 31 | public: 32 | /// default constructor 33 | explicit Pose2MobileBase() : Base(3,1) {} 34 | 35 | /// Default destructor 36 | virtual ~Pose2MobileBase() {} 37 | 38 | /** 39 | * Forward kinematics: joint configuration to poses in workspace 40 | * Velocity kinematics: optional joint velocities to linear velocities in workspace, no anuglar rate 41 | * 42 | * @param p position in config space 43 | * @param v velocity in config space 44 | * @param px link pose in work space 45 | * @param vx link velocity in work space 46 | * @param J_px_p et al. optional Jacobians 47 | **/ 48 | void forwardKinematics(const gtsam::Pose2& p, boost::optional v, 49 | std::vector& px, boost::optional&> vx, 50 | boost::optional&> J_px_p = boost::none, 51 | boost::optional&> J_vx_p = boost::none, 52 | boost::optional&> J_vx_v = boost::none) const; 53 | }; 54 | 55 | } 56 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileBaseModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileBaseModel.h 3 | * @brief SE(2) mobile base with physical body, which is represented by spheres 4 | * @author Mustafa Mukadam 5 | * @date Jan 22, 2018 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * SE(2) mobile base with physical body, which is represented by spheres 17 | * Used to check collisions 18 | */ 19 | typedef RobotModel Pose2MobileBaseModel; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * Abstract plannar mobile manipulator with 2 arm on vertical linear actuator 25 | * pose class is Pose2Vector, linear actuator use first, arm1 uses next arm1.dof, then arm2 uses last arm2.dof 26 | * vector DOF = 1 + arm1.dof + arm2.dof 27 | * total DOF = 3 + 1 + arm1.dof + arm2.dof 28 | */ 29 | class GPMP2_EXPORT Pose2MobileVetLin2Arms : public ForwardKinematics { 30 | 31 | private: 32 | // typedefs 33 | typedef ForwardKinematics Base; 34 | 35 | // base to arm pose, when linear actuator is on zero 36 | gtsam::Pose3 base_T_torso_, torso_T_arm1_, torso_T_arm2_; 37 | // if reverse_linact_ == true, positive value on lin act means move down 38 | bool reverse_linact_; 39 | // arm class 40 | Arm arm1_, arm2_; 41 | 42 | public: 43 | /// default contructor do nothing 44 | Pose2MobileVetLin2Arms() {} 45 | 46 | /// constructor from Arm 47 | /// if reverse_linact == true, positive value on lin act means move down 48 | Pose2MobileVetLin2Arms(const Arm& arm1, const Arm& arm2, 49 | const gtsam::Pose3& base_T_torso = gtsam::Pose3(), 50 | const gtsam::Pose3& torso_T_arm1 = gtsam::Pose3(), 51 | const gtsam::Pose3& torso_T_arm2 = gtsam::Pose3(), 52 | bool reverse_linact = false); 53 | 54 | /// Default destructor 55 | virtual ~Pose2MobileVetLin2Arms() {} 56 | 57 | 58 | /** 59 | * Forward kinematics: joint configuration to poses in workspace 60 | * Velocity kinematics: optional joint velocities to linear velocities in workspace, no anuglar rate 61 | * 62 | * @param p position in config space 63 | * @param v velocity in config space 64 | * @param px link pose in work space 65 | * @param vx link velocity in work space 66 | * @param J_px_p et al. optional Jacobians 67 | **/ 68 | void forwardKinematics(const Pose2Vector& p, boost::optional v, 69 | std::vector& px, boost::optional&> vx, 70 | boost::optional&> J_px_p = boost::none, 71 | boost::optional&> J_vx_p = boost::none, 72 | boost::optional&> J_vx_v = boost::none) const; 73 | 74 | 75 | /// accesses 76 | const Arm& arm1() const { return arm1_; } 77 | const Arm& arm2() const { return arm2_; } 78 | const gtsam::Pose3& base_T_torso() const { return base_T_torso_; } 79 | const gtsam::Pose3& torso_T_arm1() const { return torso_T_arm1_; } 80 | const gtsam::Pose3& torso_T_arm2() const { return torso_T_arm2_; } 81 | bool reverse_linact() const { return reverse_linact_; } 82 | }; 83 | 84 | } 85 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLin2ArmsModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLin2ArmsModel.h 3 | * @brief mobile pose2 + linear actuator + 2 x Arm with physical body, represented by spheres 4 | * @author Mustafa Mukadam 5 | * @date Sep 3, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * Pose2 + lin actuator + 2 X Arm with physical body, represented by spheres 17 | * used to check collisions 18 | */ 19 | typedef RobotModel Pose2MobileVetLin2ArmsModel; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | 21 | namespace gpmp2 { 22 | 23 | /** 24 | * Abstract plannar mobile manipulator 25 | * an Arm on Pose2 mobile base with a vetical linear actuator 26 | * Linear actuator on 1st dim of gtsam::Vector, remaining are Arm's 27 | */ 28 | class GPMP2_EXPORT Pose2MobileVetLinArm : public ForwardKinematics { 29 | 30 | private: 31 | // typedefs 32 | typedef ForwardKinematics Base; 33 | 34 | // base to arm pose, when linear actuator is on zero 35 | gtsam::Pose3 base_T_torso_, torso_T_arm_; 36 | // if reverse_linact_ == true, positive value on lin act means move down 37 | bool reverse_linact_; 38 | // arm class 39 | Arm arm_; 40 | 41 | public: 42 | /// default contructor do nothing 43 | Pose2MobileVetLinArm() {} 44 | 45 | /// constructor from Arm 46 | /// if reverse_linact == true, positive value on lin act means move down 47 | explicit Pose2MobileVetLinArm(const Arm& arm, 48 | const gtsam::Pose3& base_T_torso = gtsam::Pose3(), 49 | const gtsam::Pose3& torso_T_arm = gtsam::Pose3(), 50 | bool reverse_linact = false); 51 | 52 | /// Default destructor 53 | virtual ~Pose2MobileVetLinArm() {} 54 | 55 | 56 | /** 57 | * Forward kinematics: joint configuration to poses in workspace 58 | * Velocity kinematics: optional joint velocities to linear velocities in workspace, no anuglar rate 59 | * 60 | * @param p position in config space 61 | * @param v velocity in config space 62 | * @param px link pose in work space 63 | * @param vx link velocity in work space 64 | * @param J_px_p et al. optional Jacobians 65 | **/ 66 | void forwardKinematics(const Pose2Vector& p, boost::optional v, 67 | std::vector& px, boost::optional&> vx, 68 | boost::optional&> J_px_p = boost::none, 69 | boost::optional&> J_vx_p = boost::none, 70 | boost::optional&> J_vx_v = boost::none) const; 71 | 72 | 73 | /// accesses 74 | const gtsam::Pose3& base_T_torso() const { return base_T_torso_; } 75 | const gtsam::Pose3& torso_T_arm() const { return torso_T_arm_; } 76 | const Arm& arm() const { return arm_; } 77 | bool reverse_linact() const { return reverse_linact_; } 78 | }; 79 | 80 | } 81 | -------------------------------------------------------------------------------- /gpmp2/kinematics/Pose2MobileVetLinArmModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2MobileVetLinArmModel.h 3 | * @brief mobile pose2 + linear actuator + Arm with physical body, represented by spheres 4 | * @author Mustafa Mukadam 5 | * @date Sep 3, 2017 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace gpmp2 { 14 | 15 | /** 16 | * Pose2 + lin actuator + Arm with physical body, represented by spheres 17 | * used to check collisions 18 | */ 19 | typedef RobotModel Pose2MobileVetLinArmModel; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /gpmp2/kinematics/RobotModel-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RobotModel-inl.h 3 | * @brief Abstract robot model, provide sphere-based robot model. Templated by FK model 4 | * @author Jing Dong 5 | * @date May 29, 2015 6 | **/ 7 | 8 | namespace gpmp2 { 9 | 10 | /* ************************************************************************** */ 11 | template 12 | void RobotModel::sphereCenters(const Pose& jp, 13 | std::vector& sph_centers, 14 | boost::optional&> J_point_conf) const { 15 | 16 | // apply fk 17 | std::vector link_poses; 18 | std::vector J_pose_jp; 19 | if (J_point_conf) 20 | fk_model_.forwardKinematics(jp, boost::none, link_poses, boost::none, J_pose_jp); 21 | else 22 | fk_model_.forwardKinematics(jp, boost::none, link_poses, boost::none); 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] = link_poses[body_spheres_[sph_idx].link_id].transform_from( 32 | body_spheres_[sph_idx].center, J_point_pose); 33 | (*J_point_conf)[sph_idx] = J_point_pose * J_pose_jp[body_spheres_[sph_idx].link_id]; 34 | 35 | } else { 36 | sph_centers[sph_idx] = link_poses[body_spheres_[sph_idx].link_id].transform_from( 37 | body_spheres_[sph_idx].center); 38 | } 39 | } 40 | } 41 | 42 | /* ************************************************************************** */ 43 | template 44 | gtsam::Point3 RobotModel::sphereCenter(size_t sph_idx, const Pose& jp, 45 | boost::optional J_point_conf) const { 46 | 47 | // apply fk 48 | std::vector link_poses; 49 | std::vector J_pose_jp; 50 | if (J_point_conf) 51 | fk_model_.forwardKinematics(jp, boost::none, link_poses, boost::none, J_pose_jp); 52 | else 53 | fk_model_.forwardKinematics(jp, boost::none, link_poses, boost::none); 54 | 55 | gtsam::Point3 sph_center; 56 | if (J_point_conf) { 57 | gtsam::Matrix36 J_point_pose; 58 | sph_center = link_poses[body_spheres_[sph_idx].link_id].transform_from( 59 | body_spheres_[sph_idx].center, J_point_pose); 60 | *J_point_conf = J_point_pose * J_pose_jp[body_spheres_[sph_idx].link_id]; 61 | 62 | } else { 63 | sph_center = link_poses[body_spheres_[sph_idx].link_id].transform_from( 64 | body_spheres_[sph_idx].center); 65 | } 66 | 67 | return sph_center; 68 | } 69 | 70 | /* ************************************************************************** */ 71 | template 72 | gtsam::Matrix RobotModel::sphereCentersMat(const Pose& jp) const { 73 | 74 | std::vector sph_centers; 75 | sphereCenters(jp, sph_centers); 76 | 77 | // convert to matrix 78 | gtsam::Matrix points_mat(3, nr_body_spheres()); 79 | for (size_t i = 0; i < nr_body_spheres(); i++) 80 | points_mat.col(i) = sph_centers[i].vector(); 81 | return points_mat; 82 | } 83 | 84 | } 85 | 86 | 87 | -------------------------------------------------------------------------------- /gpmp2/kinematics/RobotModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RobotModel.h 3 | * @brief Abstract robot model, provide sphere-based robot model. Templated by FK model 4 | * @author Jing Dong 5 | * @date May 29, 2015 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 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) : link_id(id), radius(r), center(c) {} 26 | ~BodySphere() {} 27 | }; 28 | 29 | 30 | /// vector of body sphere, typedef here to wrap in matlab 31 | typedef std::vector BodySphereVector; 32 | 33 | 34 | /** 35 | * Abstract robot model, provide sphere-based robot model. 36 | * template parameter is forward kinematics type 37 | */ 38 | template 39 | class RobotModel { 40 | 41 | public: 42 | // typedefs 43 | typedef FK FKModel; 44 | typedef typename FKModel::Pose Pose; 45 | typedef typename FKModel::Velocity Velocity; 46 | 47 | private: 48 | FKModel fk_model_; // fk 49 | BodySphereVector body_spheres_; // body spheres 50 | 51 | public: 52 | /// default contructor do nothing, for serialization 53 | RobotModel() : fk_model_(), body_spheres_() {} 54 | 55 | /// Contructor take a fk model 56 | RobotModel(const FKModel& fk_model, const BodySphereVector& body_spheres) : 57 | fk_model_(fk_model), body_spheres_(body_spheres) {} 58 | 59 | /// Default destructor 60 | virtual ~RobotModel() {} 61 | 62 | 63 | /// given pose in configuration space, solve sphere center vector in work space 64 | /// with optional associated jacobian of pose 65 | void sphereCenters(const Pose& jp, std::vector& sph_centers, 66 | boost::optional&> J_point_conf = boost::none) const ; 67 | 68 | /// given pose in configuration space, solve a single sphere center in work space 69 | /// for fast call of a single sphere 70 | /// with optional associated jacobian of pose 71 | gtsam::Point3 sphereCenter(size_t sph_idx, const Pose& jp, 72 | boost::optional J_point_conf = boost::none) const ; 73 | 74 | 75 | /// given pose in configuration space, solve sphere center vector in work space 76 | /// matlab version, no jacobians 77 | /// output matrix 3 * nr_sphere 78 | gtsam::Matrix sphereCentersMat(const Pose& jp) const ; 79 | 80 | 81 | /// accesses from fk_model 82 | const FKModel& fk_model() const { return fk_model_; } 83 | size_t dof() const { return fk_model_.dof(); } 84 | 85 | /// accesses 86 | size_t nr_body_spheres() const { return body_spheres_.size(); } 87 | size_t sphere_link_id(size_t i) const { return body_spheres_[i].link_id; } 88 | double sphere_radius(size_t i) const { return body_spheres_[i].radius; } 89 | const gtsam::Point3& sphere_center_wrt_link(size_t i) const { return body_spheres_[i].center; } 90 | }; 91 | 92 | } 93 | 94 | #include 95 | 96 | -------------------------------------------------------------------------------- /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 | 15 | namespace gpmp2 { 16 | 17 | /* ************************************************************************** */ 18 | gtsam::Pose3 computeBasePose3(const gtsam::Pose2& base_pose2, 19 | gtsam::OptionalJacobian<6,3> J = boost::none) { 20 | 21 | if (J) { 22 | J->setZero(); 23 | const gtsam::Matrix3 Hzrot3 = gtsam::Rot3::ExpmapDerivative( 24 | gtsam::Vector3(0, 0, base_pose2.theta())); 25 | J->block<3, 1>(0, 2) = Hzrot3.col(2); 26 | J->block<2, 2>(3, 0) = gtsam::Matrix2::Identity(); 27 | } 28 | return gtsam::Pose3(gtsam::Rot3::Rodrigues( 29 | gtsam::Vector3(0, 0, base_pose2.theta())), 30 | gtsam::Point3(base_pose2.x(), base_pose2.y(), 0.0)); 31 | } 32 | 33 | /* ************************************************************************** */ 34 | gtsam::Pose3 computeBaseTransPose3(const gtsam::Pose2& base_pose2, 35 | const gtsam::Pose3& base_T_trans, 36 | gtsam::OptionalJacobian<6,3> J = boost::none) { 37 | 38 | if (J) { 39 | gtsam::Matrix63 Hbasep3; 40 | const gtsam::Pose3 base_pose3 = computeBasePose3(base_pose2, Hbasep3); 41 | gtsam::Matrix6 Hcomp; 42 | const gtsam::Pose3 arm_base = base_pose3.compose(base_T_trans, Hcomp); 43 | *J = Hcomp * Hbasep3; 44 | return arm_base; 45 | } else { 46 | return computeBasePose3(base_pose2).compose(base_T_trans); 47 | } 48 | } 49 | 50 | /* ************************************************************************** */ 51 | gtsam::Pose3 liftBasePose3(const gtsam::Pose2& base_pose2, 52 | double lift, const gtsam::Pose3& base_T_trans, bool reverse_linact, 53 | gtsam::OptionalJacobian<6,4> J = boost::none) { 54 | 55 | // a const pose to lift base 56 | Pose3 lift_base_pose; 57 | //Matrix63 Jliftbase_z; 58 | if (reverse_linact) { 59 | lift_base_pose = Pose3::Create(Rot3(), Point3(0, 0, -lift)); 60 | } else { 61 | lift_base_pose = Pose3::Create(Rot3(), Point3(0, 0, lift)); 62 | } 63 | 64 | if (J) { 65 | Matrix63 Harmbase; 66 | const Pose3 armbase = computeBaseTransPose3(base_pose2, base_T_trans, Harmbase); 67 | Matrix6 Hcomp1, Hcomp2; 68 | const Pose3 armbaselift = lift_base_pose.compose(armbase, Hcomp1, Hcomp2); 69 | // J over pose2 70 | J->block<6,3>(0,0) = Hcomp2 * Harmbase; 71 | // J over lift z 72 | // D_lift_base_pose = [0; I], so (Hcomp1 * D_lift_base_pose).col(2) == Hcomp1.col(5) 73 | //J->block<6,1>(0,3) = (Hcomp1 * Jliftbase_z).col(2); 74 | if (reverse_linact) 75 | J->block<6,1>(0,3) = -Hcomp1.col(5); 76 | else 77 | J->block<6,1>(0,3) = Hcomp1.col(5); 78 | return armbaselift; 79 | } else { 80 | return lift_base_pose.compose(computeBaseTransPose3(base_pose2, base_T_trans)); 81 | } 82 | } 83 | 84 | } 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 | 12 | namespace gpmp2 { 13 | 14 | /// convert 2D pose to 3D pose on x-y plane, with jacobian 15 | gtsam::Pose3 computeBasePose3(const gtsam::Pose2& base_pose2, 16 | gtsam::OptionalJacobian<6,3> J = boost::none); 17 | 18 | /// compute a pose3 given vehicle base pose2 and transform, with jacobian 19 | gtsam::Pose3 computeBaseTransPose3(const gtsam::Pose2& base_pose2, 20 | const gtsam::Pose3& base_T_trans, 21 | gtsam::OptionalJacobian<6,3> J = boost::none); 22 | 23 | /// lift arm base in z direction, with jacobian 24 | gtsam::Pose3 liftBasePose3(const gtsam::Pose2& base_pose2, 25 | double lift, const gtsam::Pose3& base_T_trans, bool reverse_linact, 26 | gtsam::OptionalJacobian<6,4> J = boost::none); 27 | 28 | } 29 | -------------------------------------------------------------------------------- /gpmp2/kinematics/tests/testGaussianPriorWorkspaceOrientation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testGaussianPriorWorkspaceOrientation.cpp 3 | * @author Mustafa Mukadam 4 | **/ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | using namespace std; 22 | using namespace gtsam; 23 | using namespace gpmp2; 24 | 25 | 26 | /* ************************************************************************** */ 27 | TEST(GaussianPriorWorkspaceOrientationArm, error) { 28 | 29 | Vector2 a(1, 1), alpha(M_PI/2, 0), d(0, 0); 30 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 31 | Vector2 q; 32 | Rot3 des; 33 | Vector actual, expect; 34 | Matrix H_exp, H_act; 35 | noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(3, 1.0); 36 | 37 | q = Vector2(M_PI/4.0, -M_PI/2); 38 | des = Rot3(Rot3::RzRyRx(0, 0, -M_PI/2)); 39 | GaussianPriorWorkspaceOrientationArm factor(0, arm, 1, des, cost_model); 40 | actual = factor.evaluateError(q, H_act); 41 | expect = Vector3(-0.613943126, 1.48218982, 0.613943126); 42 | H_exp = numericalDerivative11(boost::function( 43 | boost::bind(&GaussianPriorWorkspaceOrientationArm::evaluateError, factor, _1, boost::none)), 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 | /* ************************************************************************** */ 50 | TEST(GaussianPriorWorkspaceOrientationArm, optimization) { 51 | 52 | noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(3, 0.1); 53 | 54 | Vector a = (Vector(2) << 1, 1).finished(); 55 | Vector alpha = (Vector(2) << 0, 0).finished(); 56 | Vector d = (Vector(2) << 0, 0).finished(); 57 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 58 | Rot3 des = Rot3(); 59 | 60 | Key qkey = Symbol('x', 0); 61 | Vector qinit = (Vector(2) << M_PI/2, M_PI/2).finished(); 62 | Vector q = (Vector(2) << 0, 0).finished(); 63 | 64 | NonlinearFactorGraph graph; 65 | graph.add(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 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | using namespace std; 22 | using namespace gtsam; 23 | using namespace gpmp2; 24 | 25 | 26 | /* ************************************************************************** */ 27 | TEST(GaussianPriorWorkspacePoseArm, error) { 28 | 29 | Vector2 a(1, 1), alpha(M_PI/2, 0), d(0, 0); 30 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 31 | Vector2 q; 32 | Pose3 des; 33 | Vector actual, expect; 34 | Matrix H_exp, H_act; 35 | noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 1.0); 36 | 37 | q = Vector2(M_PI/4.0, -M_PI/2); 38 | des = Pose3(); 39 | GaussianPriorWorkspacePoseArm factor(0, arm, 1, des, cost_model); 40 | actual = factor.evaluateError(q, H_act); 41 | expect = (Vector(6) << 0.613943126, 1.48218982, -0.613943126, 1.1609828, 0.706727485, -0.547039678).finished(); 42 | H_exp = numericalDerivative11(boost::function( 43 | boost::bind(&GaussianPriorWorkspacePoseArm::evaluateError, factor, _1, boost::none)), 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 | /* ************************************************************************** */ 50 | TEST(GaussianPriorWorkspacePoseArm, optimization) { 51 | 52 | noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 0.1); 53 | 54 | Vector a = (Vector(2) << 1, 1).finished(); 55 | Vector alpha = (Vector(2) << 0, 0).finished(); 56 | Vector d = (Vector(2) << 0, 0).finished(); 57 | ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); 58 | Pose3 des = Pose3(Rot3(), Point3(2, 0, 0)); 59 | 60 | Key qkey = Symbol('x', 0); 61 | Vector q = (Vector(2) << 0, 0).finished(); 62 | Vector qinit = (Vector(2) << M_PI/2, M_PI/2).finished(); 63 | 64 | NonlinearFactorGraph graph; 65 | graph.add(GaussianPriorWorkspacePoseArm(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/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 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | 22 | namespace gpmp2 { 23 | 24 | 25 | /// hinge loss obstacle cost function 26 | inline double hingeLossObstacleCost(const gtsam::Point3& point, const SignedDistanceField& sdf, 27 | double eps, gtsam::OptionalJacobian<1, 3> H_point = boost::none) { 28 | 29 | gtsam::Vector3 field_gradient; 30 | double dist_signed; 31 | try { 32 | dist_signed = sdf.getSignedDistance(point, field_gradient); 33 | } catch (SDFQueryOutOfRange&) { 34 | //std::cout << "[hingeLossObstacleCost] WARNING: querying signed distance out of range, " 35 | // "assume zero obstacle cost." << std::endl; 36 | if (H_point) *H_point = gtsam::Matrix13::Zero(); 37 | return 0.0; 38 | } 39 | 40 | if (dist_signed > eps) { 41 | // faraway no error 42 | if (H_point) *H_point = gtsam::Matrix13::Zero(); 43 | return 0.0; 44 | 45 | } else { 46 | // outside but < eps or inside object 47 | if (H_point) *H_point = -field_gradient.transpose(); 48 | return eps - dist_signed; 49 | } 50 | } 51 | 52 | 53 | /// hinge loss obstacle cost function, planar version 54 | inline double hingeLossObstacleCost(const gtsam::Point2& point, const PlanarSDF& sdf, 55 | double eps, gtsam::OptionalJacobian<1, 2> H_point = boost::none) { 56 | 57 | gtsam::Vector2 field_gradient; 58 | double dist_signed; 59 | try { 60 | dist_signed = sdf.getSignedDistance(point, field_gradient); 61 | } catch (SDFQueryOutOfRange&) { 62 | //std::cout << "[hingeLossObstacleCost] WARNING: querying signed distance out of range, " 63 | // "assume zero obstacle cost." << std::endl; 64 | if (H_point) *H_point = gtsam::Matrix12::Zero(); 65 | return 0.0; 66 | } 67 | 68 | if (dist_signed > eps) { 69 | // faraway no error 70 | if (H_point) *H_point = gtsam::Matrix12::Zero(); 71 | return 0.0; 72 | 73 | } else { 74 | // outside but < eps or inside object 75 | if (H_point) *H_point = -field_gradient.transpose(); 76 | return eps - dist_signed; 77 | } 78 | } 79 | 80 | } 81 | 82 | -------------------------------------------------------------------------------- /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 | 14 | namespace gpmp2 { 15 | 16 | /* ************************************************************************** */ 17 | template 18 | gtsam::Vector ObstaclePlanarSDFFactor::evaluateError( 19 | const Pose& conf, boost::optional H1) const { 20 | 21 | // if Jacobians used, initialize as zeros 22 | // size: arm_nr_points_ * DOF 23 | if (H1) *H1 = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 24 | 25 | // run forward kinematics of this configuration 26 | vector sph_centers; 27 | vector J_px_jp; 28 | if (H1) 29 | robot_.sphereCenters(conf, sph_centers, J_px_jp); 30 | else 31 | robot_.sphereCenters(conf, sph_centers); 32 | 33 | 34 | // allocate cost vector 35 | Vector err(robot_.nr_body_spheres()); 36 | 37 | // for each point on arm stick, get error 38 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 39 | 40 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 41 | 42 | if (H1) { 43 | Matrix12 Jerr_point; 44 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), sph_centers[sph_idx].y()); 45 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps, Jerr_point); 46 | 47 | // chain rules 48 | H1->row(sph_idx) = Jerr_point * J_px_jp[sph_idx].topRows<2>(); 49 | 50 | } else { 51 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), sph_centers[sph_idx].y()); 52 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps); 53 | } 54 | } 55 | 56 | return err; 57 | } 58 | 59 | } 60 | -------------------------------------------------------------------------------- /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 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | 22 | namespace gpmp2 { 23 | 24 | /** 25 | * unary factor for obstacle avoidance, planar version 26 | * template robot model version 27 | */ 28 | template 29 | class ObstaclePlanarSDFFactor: public gtsam::NoiseModelFactor1 { 30 | 31 | public: 32 | // typedefs 33 | typedef ROBOT Robot; 34 | typedef typename Robot::Pose Pose; 35 | 36 | private: 37 | // typedefs 38 | typedef ObstaclePlanarSDFFactor This; 39 | typedef gtsam::NoiseModelFactor1 Base; 40 | 41 | // obstacle cost settings 42 | double epsilon_; // distance from object that start non-zero cost 43 | 44 | // arm: planar one, all alpha = 0 45 | const Robot& robot_; 46 | 47 | // signed distance field from matlab 48 | const PlanarSDF& sdf_; 49 | 50 | 51 | public: 52 | 53 | /// shorthand for a smart pointer to a factor 54 | typedef boost::shared_ptr shared_ptr; 55 | 56 | /* Default constructor do nothing */ 57 | ObstaclePlanarSDFFactor() : robot_(Robot()), sdf_(PlanarSDF()) {} 58 | 59 | /** 60 | * Constructor 61 | * @param cost_model cost function covariance, should to identity model 62 | * @param field signed distance field 63 | * @param nn_index nearest neighbour index of signed distance field 64 | */ 65 | ObstaclePlanarSDFFactor(gtsam::Key poseKey, const Robot& robot, 66 | const PlanarSDF& sdf, double cost_sigma, double epsilon) : 67 | Base(gtsam::noiseModel::Isotropic::Sigma(robot.nr_body_spheres(), cost_sigma), poseKey), 68 | epsilon_(epsilon), robot_(robot), sdf_(sdf) { 69 | 70 | // TODO: check robot is plannar 71 | } 72 | 73 | virtual ~ObstaclePlanarSDFFactor() {} 74 | 75 | 76 | /// error function 77 | /// numerical jacobians / analytic jacobians from cost function 78 | gtsam::Vector evaluateError(const Pose& conf, 79 | boost::optional H1 = boost::none) const ; 80 | 81 | 82 | /// @return a deep copy of this factor 83 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 84 | return boost::static_pointer_cast( 85 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 86 | 87 | /** print contents */ 88 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 89 | std::cout << s << "ObstaclePlanarSDFFactor :" << std::endl; 90 | Base::print("", keyFormatter); 91 | } 92 | 93 | 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("NoiseModelFactor1", 99 | boost::serialization::base_object(*this)); 100 | } 101 | }; 102 | 103 | } 104 | 105 | #include 106 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar 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 ObstaclePlanarSDFFactor ObstaclePlanarSDFFactorArm; 17 | 18 | } 19 | 20 | 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGP-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorPlanarArmGP-inl.h 3 | * @brief Obstacle avoidance cost factor, for 2D arm, using GP and signed distance field 4 | * @author Jing Dong 5 | * @date Nov 15, 2015 6 | **/ 7 | 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace gtsam; 13 | 14 | 15 | namespace gpmp2 { 16 | 17 | /* ************************************************************************** */ 18 | template 19 | gtsam::Vector ObstaclePlanarSDFFactorGP::evaluateError( 20 | const Pose& conf1, const Velocity& vel1, 21 | const Pose& conf2, const Velocity& vel2, 22 | boost::optional H1, boost::optional H2, 23 | boost::optional H3, boost::optional H4) const { 24 | 25 | const bool use_H = (H1 || H2 || H3 || H4); 26 | 27 | // if Jacobians used, initialize Jerr_conf as zeros 28 | // size: arm_nr_points_ * DOF 29 | Matrix Jerr_conf = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 30 | 31 | 32 | // get conf by interpolation, except last pose 33 | typename Robot::Pose conf; 34 | Matrix Jconf_c1, Jconf_c2, Jconf_v1, Jconf_v2; 35 | if (use_H) 36 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2, Jconf_c1, Jconf_v1, Jconf_c2, Jconf_v2); 37 | else 38 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2); 39 | 40 | 41 | // run forward kinematics of this configuration 42 | vector sph_centers; 43 | vector J_px_jp; 44 | if (H1) 45 | robot_.sphereCenters(conf, sph_centers, J_px_jp); 46 | else 47 | robot_.sphereCenters(conf, sph_centers); 48 | 49 | 50 | // allocate cost vector 51 | Vector err(robot_.nr_body_spheres()); 52 | 53 | // for each point on arm stick, get error 54 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 55 | 56 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 57 | 58 | if (H1) { 59 | Matrix12 Jerr_point; 60 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), sph_centers[sph_idx].y()); 61 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps, Jerr_point); 62 | 63 | // chain rules 64 | Jerr_conf.row(sph_idx) = Jerr_point * J_px_jp[sph_idx].topRows<2>(); 65 | 66 | } else { 67 | const Point2 sph_center_2d(sph_centers[sph_idx].x(), sph_centers[sph_idx].y()); 68 | err(sph_idx) = hingeLossObstacleCost(sph_center_2d, sdf_, total_eps); 69 | } 70 | } 71 | 72 | // update jacobians 73 | if (use_H) 74 | GPBase::updatePoseJacobians(Jerr_conf, Jconf_c1, Jconf_v1, Jconf_c2, Jconf_v2, 75 | H1, H2, H3, H4); 76 | 77 | return err; 78 | } 79 | 80 | } 81 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar, linear GP and signed distance field 4 | * @author Jing Dong 5 | * @date May 29, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef ObstaclePlanarSDFFactorGP 18 | ObstaclePlanarSDFFactorGPArm; 19 | 20 | } 21 | 22 | 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPointRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPointRobot.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed distance field, 4 | * 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 19 | ObstaclePlanarSDFFactorGPPointRobot; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 18 | ObstaclePlanarSDFFactorGPPose2Mobile2Arms; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorGPPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorGPPose2MobileArm.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed distance field, 4 | * 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 19 | ObstaclePlanarSDFFactorGPPose2MobileArm; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 18 | ObstaclePlanarSDFFactorGPPose2MobileBase; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPointRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPointRobot.h 3 | * @brief Obstacle avoidance cost factor, for point robot, using signed distance field 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 | // template uses PointRobotModel as robot type 16 | typedef ObstaclePlanarSDFFactor ObstaclePlanarSDFFactorPointRobot; 17 | 18 | } 19 | -------------------------------------------------------------------------------- /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 ObstaclePlanarSDFFactorPose2Mobile2Arms; 17 | 18 | } 19 | 20 | 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstaclePlanarSDFFactorPose2MobileArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstaclePlanarSDFFactorPose2MobileArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar and signed distance field 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 | // template use ArmModel as robot type 16 | typedef ObstaclePlanarSDFFactor ObstaclePlanarSDFFactorPose2MobileArm; 17 | 18 | } 19 | 20 | 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 ObstaclePlanarSDFFactorPose2MobileBase; 17 | 18 | } 19 | 20 | 21 | -------------------------------------------------------------------------------- /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 | 14 | namespace gpmp2 { 15 | 16 | /* ************************************************************************** */ 17 | template 18 | gtsam::Vector ObstacleSDFFactor::evaluateError( 19 | const typename Robot::Pose& conf, boost::optional H1) const { 20 | 21 | // if Jacobians used, initialize as zeros 22 | // size: arm_nr_points_ * DOF 23 | if (H1) *H1 = Matrix::Zero(robot_.nr_body_spheres(), robot_.dof()); 24 | 25 | // run forward kinematics of this configuration 26 | vector sph_centers; 27 | vector J_px_jp; 28 | if (H1) 29 | robot_.sphereCenters(conf, sph_centers, J_px_jp); 30 | else 31 | robot_.sphereCenters(conf, sph_centers); 32 | 33 | 34 | // allocate cost vector 35 | Vector err(robot_.nr_body_spheres()); 36 | 37 | // for each point on arm stick, get error 38 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 39 | 40 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 41 | 42 | if (H1) { 43 | Matrix13 Jerr_point; 44 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps, Jerr_point); 45 | 46 | // chain rules 47 | H1->row(sph_idx) = Jerr_point * J_px_jp[sph_idx]; 48 | 49 | } else { 50 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps); 51 | } 52 | } 53 | 54 | return err; 55 | } 56 | 57 | } 58 | -------------------------------------------------------------------------------- /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 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | 22 | namespace gpmp2 { 23 | 24 | /** 25 | * unary factor for obstacle avoidance 26 | * template robot model version 27 | */ 28 | template 29 | class ObstacleSDFFactor: public gtsam::NoiseModelFactor1 { 30 | 31 | public: 32 | // typedefs 33 | typedef ROBOT Robot; 34 | typedef typename Robot::Pose Pose; 35 | 36 | private: 37 | // typedefs 38 | typedef ObstacleSDFFactor This; 39 | typedef gtsam::NoiseModelFactor1 Base; 40 | 41 | // obstacle cost settings 42 | double epsilon_; // distance from object that start non-zero cost 43 | 44 | // arm: planar one, all alpha = 0 45 | const Robot& robot_; 46 | 47 | // signed distance field from matlab 48 | const SignedDistanceField& sdf_; 49 | 50 | 51 | public: 52 | 53 | /// shorthand for a smart pointer to a factor 54 | typedef boost::shared_ptr shared_ptr; 55 | 56 | /* Default constructor do nothing */ 57 | ObstacleSDFFactor() : robot_(Robot()), sdf_(SignedDistanceField()) {} 58 | 59 | /** 60 | * Constructor 61 | * @param cost_model cost function covariance, should to identity model 62 | * @param field signed distance field 63 | * @param nn_index nearest neighbour index of signed distance field 64 | */ 65 | ObstacleSDFFactor(gtsam::Key poseKey, const Robot& robot, 66 | const SignedDistanceField& sdf, double cost_sigma, double epsilon) : 67 | Base(gtsam::noiseModel::Isotropic::Sigma(robot.nr_body_spheres(), cost_sigma), poseKey), 68 | epsilon_(epsilon), robot_(robot), sdf_(sdf) {} 69 | 70 | virtual ~ObstacleSDFFactor() {} 71 | 72 | 73 | /// error function 74 | /// numerical jacobians / analytic jacobians from cost function 75 | gtsam::Vector evaluateError(const typename Robot::Pose& conf, 76 | boost::optional H1 = boost::none) const ; 77 | 78 | 79 | /// @return a deep copy of this factor 80 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 81 | return boost::static_pointer_cast( 82 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 83 | 84 | /** print contents */ 85 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 86 | std::cout << s << "ObstacleSDFFactor :" << std::endl; 87 | Base::print("", keyFormatter); 88 | } 89 | 90 | 91 | /** Serialization function */ 92 | friend class boost::serialization::access; 93 | template 94 | void serialize(ARCHIVE & ar, const unsigned int version) { 95 | ar & boost::serialization::make_nvp("NoiseModelFactor1", 96 | boost::serialization::base_object(*this)); 97 | } 98 | }; 99 | 100 | } 101 | 102 | #include 103 | 104 | 105 | -------------------------------------------------------------------------------- /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 | } 19 | 20 | 21 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGP-inl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGP-inl.h 3 | * @brief Obstacle avoidance cost factor, for general arm, using GP and signed distance field 4 | * @author Jing Dong 5 | * @date Dec 4, 2015 6 | **/ 7 | 8 | #include 9 | 10 | using namespace std; 11 | using namespace gtsam; 12 | 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 | boost::optional H1, boost::optional H2, 22 | boost::optional H3, boost::optional H4) const { 23 | 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 | 31 | // get conf by interpolation, except last pose 32 | typename Robot::Pose conf; 33 | Matrix Jconf_c1, Jconf_c2, Jconf_v1, Jconf_v2; 34 | if (use_H) 35 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2, Jconf_c1, Jconf_v1, Jconf_c2, Jconf_v2); 36 | else 37 | conf = GPbase_.interpolatePose(conf1, vel1, conf2, vel2); 38 | 39 | 40 | // run forward kinematics of this configuration 41 | vector sph_centers; 42 | vector J_px_jp; 43 | if (H1) 44 | robot_.sphereCenters(conf, sph_centers, J_px_jp); 45 | else 46 | robot_.sphereCenters(conf, sph_centers); 47 | 48 | 49 | // allocate cost vector 50 | Vector err(robot_.nr_body_spheres()); 51 | 52 | // for each point on arm stick, get error 53 | for (size_t sph_idx = 0; sph_idx < robot_.nr_body_spheres(); sph_idx++) { 54 | 55 | const double total_eps = robot_.sphere_radius(sph_idx) + epsilon_; 56 | 57 | if (H1) { 58 | Matrix13 Jerr_point; 59 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps, Jerr_point); 60 | 61 | // chain rules 62 | Jerr_conf.row(sph_idx) = Jerr_point * J_px_jp[sph_idx]; 63 | 64 | } else { 65 | err(sph_idx) = hingeLossObstacleCost(sph_centers[sph_idx], sdf_, total_eps); 66 | } 67 | } 68 | 69 | // update jacobians 70 | if (use_H) 71 | GPBase::updatePoseJacobians(Jerr_conf, Jconf_c1, Jconf_v1, Jconf_c2, Jconf_v2, 72 | H1, H2, H3, H4); 73 | 74 | return err; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPArm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPArm.h 3 | * @brief Obstacle avoidance cost factor, using Arm planar, linear GP and signed distance field 4 | * @author Jing Dong 5 | * @date May 29, 2016 6 | **/ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef ObstacleSDFFactorGP 18 | ObstacleSDFFactorGPArm; 19 | 20 | } 21 | 22 | 23 | -------------------------------------------------------------------------------- /gpmp2/obstacle/ObstacleSDFFactorGPPose2Mobile2Arms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ObstacleSDFFactorGPPose2Mobile2Arms.h 3 | * @brief Obstacle avoidance interpolated GP 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 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template uses Pose2Mobile2ArmsModel as robot type 17 | typedef ObstacleSDFFactorGP 18 | ObstacleSDFFactorGPPose2Mobile2Arms; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /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 18 | ObstacleSDFFactorGPPose2MobileArm; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /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 18 | ObstacleSDFFactorGPPose2MobileBase; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /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 19 | ObstacleSDFFactorGPPose2MobileVetLin2Arms; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 19 | ObstacleSDFFactorGPPose2MobileVetLinArm; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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 ObstacleSDFFactorPose2Mobile2Arms; 17 | 18 | } 19 | -------------------------------------------------------------------------------- /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 | } 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 ObstacleSDFFactorPose2MobileBase; 17 | 18 | } 19 | -------------------------------------------------------------------------------- /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 ObstacleSDFFactorPose2MobileVetLin2Arms; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /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 ObstacleSDFFactorPose2MobileVetLinArm; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /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 | #include 12 | 13 | 14 | namespace gpmp2 { 15 | 16 | /// query out of range exception 17 | class GPMP2_EXPORT SDFQueryOutOfRange : public std::runtime_error { 18 | 19 | public: 20 | /// constructor 21 | SDFQueryOutOfRange() : std::runtime_error("Querying SDF out of range") {} 22 | }; 23 | 24 | } 25 | 26 | 27 | -------------------------------------------------------------------------------- /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 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace gpmp2 { 15 | 16 | // template use ArmModel as robot type 17 | typedef SelfCollision SelfCollisionArm; 18 | 19 | } 20 | -------------------------------------------------------------------------------- /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 | 11 | namespace gpmp2 { 12 | 13 | /* ************************************************************************** */ 14 | void SignedDistanceField::saveSDF(const std::string filename) { 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 | } 22 | else if (fext == "bin") { 23 | boost::archive::binary_oarchive oa(ofs); 24 | oa << *this; 25 | } 26 | else { 27 | boost::archive::text_oarchive oa(ofs); 28 | oa << *this; 29 | } 30 | } 31 | 32 | /* ************************************************************************** */ 33 | void SignedDistanceField::loadSDF(const std::string filename) { 34 | std::ifstream ifs(filename.c_str()); 35 | if (!ifs.good()) 36 | std::cout<<"File \'"<> BOOST_SERIALIZATION_NVP(*this); 41 | } 42 | else if (fext == "bin") { 43 | boost::archive::binary_iarchive ia(ifs); 44 | ia >> *this; 45 | } 46 | else { 47 | boost::archive::text_iarchive ia(ifs); 48 | ia >> *this; 49 | } 50 | } 51 | 52 | } 53 | -------------------------------------------------------------------------------- /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 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | using namespace std; 17 | using namespace gtsam; 18 | using namespace gpmp2; 19 | 20 | 21 | double sdf_wrapper(const PlanarSDF& field, const Point2& p) { 22 | return field.getSignedDistance(p); 23 | } 24 | 25 | /* ************************************************************************** */ 26 | TEST(PlanarSDFutils, test1) { 27 | // data 28 | Matrix data; 29 | data = (Matrix(5, 5) << 30 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 31 | 1.4142, 1, 1, 1, 1.4142, 32 | 1.4142, 1, 1, 1, 1.4142, 33 | 1.4142, 1, 1, 1, 1.4142, 34 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321).finished(); 35 | Point2 origin(-0.2, -0.2); 36 | double cell_size = 0.1; 37 | 38 | // constructor 39 | PlanarSDF field(origin, cell_size, data); 40 | EXPECT_LONGS_EQUAL(5, field.x_count()); 41 | EXPECT_LONGS_EQUAL(5, field.y_count()); 42 | EXPECT_DOUBLES_EQUAL(0.1, field.cell_size(), 1e-9); 43 | EXPECT(assert_equal(origin, field.origin())); 44 | 45 | // access 46 | PlanarSDF::float_index idx; 47 | idx = field.convertPoint2toCell(Point2(0, 0)); 48 | EXPECT_DOUBLES_EQUAL(2, idx.get<0>(), 1e-9); 49 | EXPECT_DOUBLES_EQUAL(2, idx.get<1>(), 1e-9); 50 | EXPECT_DOUBLES_EQUAL(1, field.signed_distance(idx), 1e-9) 51 | 52 | idx = field.convertPoint2toCell(Point2(0.18, -0.17)); // tri-linear interpolation 53 | EXPECT_DOUBLES_EQUAL(0.3, idx.get<0>(), 1e-9); 54 | EXPECT_DOUBLES_EQUAL(3.8, idx.get<1>(), 1e-9); 55 | EXPECT_DOUBLES_EQUAL(1.567372, field.signed_distance(idx), 1e-9) 56 | 57 | idx = boost::make_tuple(1.0, 2.0); 58 | EXPECT(assert_equal(Point2(0.0, -0.1), field.convertCelltoPoint2(idx))); 59 | 60 | // gradient 61 | Vector2 grad_act, grad_exp; 62 | Point2 p; 63 | p = Point2(-0.13, -0.14); 64 | field.getSignedDistance(p, grad_act); 65 | grad_exp = numericalDerivative11(boost::function( 66 | boost::bind(sdf_wrapper, field, _1)), 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 = numericalDerivative11(boost::function( 72 | boost::bind(sdf_wrapper, field, _1)), p, 1e-6); 73 | EXPECT(assert_equal(grad_exp, grad_act, 1e-6)); 74 | 75 | } 76 | 77 | /* ************************************************************************** */ 78 | /* main function */ 79 | int main() { 80 | TestResult tr; 81 | return TestRegistry::runAllTests(tr); 82 | } 83 | 84 | 85 | -------------------------------------------------------------------------------- /gpmp2/obstacle/tests/testSelfCollision.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testSelfCollisionArm.cpp 3 | * @author Mustafa Mukadam 4 | **/ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | using namespace gtsam; 19 | using namespace gpmp2; 20 | 21 | 22 | /* ************************************************************************** */ 23 | TEST(SelfCollisionArm, error) { 24 | 25 | // 3 link arm 26 | Vector3 a(1, 1, 1), alpha(0, 0, 0), d(0, 0, 0); 27 | Arm abs_arm(3, a, alpha, d); 28 | BodySphereVector body_spheres; 29 | body_spheres.push_back(BodySphere(0, 0.1, Point3(-0.5, 0, 0))); 30 | body_spheres.push_back(BodySphere(1, 0.1, Point3(-0.5, 0, 0))); 31 | body_spheres.push_back(BodySphere(1, 0.1, Point3(0, 0, 0))); 32 | body_spheres.push_back(BodySphere(2, 0.1, Point3(0, 0, 0))); 33 | ArmModel arm(abs_arm, body_spheres); 34 | 35 | Matrix data(2, 4); 36 | // sphere A id, sphere B id, epsilon, sigma 37 | data << 38 | 0, 1, 2.0, 0.1, 39 | 2, 3, 5.0, 0.1; 40 | SelfCollisionArm factor = SelfCollisionArm(0, arm, data); 41 | 42 | Vector actual, expect; 43 | Matrix H_exp, H_act; 44 | 45 | Vector3 q; 46 | q = Vector3(0.0, M_PI/2.0, 0.0); 47 | actual = factor.evaluateError(q, H_act); 48 | expect = (Vector(2) << 1.4928932188134527, 4.2).finished(); 49 | H_exp = numericalDerivative11(boost::function( 50 | boost::bind(&SelfCollisionArm::evaluateError, factor, _1, boost::none)), q, 1e-6); 51 | EXPECT(assert_equal(expect, actual, 1e-6)); 52 | EXPECT(assert_equal(H_exp, H_act, 1e-6)); 53 | } 54 | 55 | /* ************************************************************************** */ 56 | /* main function */ 57 | int main() { 58 | TestResult tr; 59 | return TestRegistry::runAllTests(tr); 60 | } 61 | -------------------------------------------------------------------------------- /gpmp2/obstacle/tests/testSignedDistanceField.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testSDFutils.cpp 3 | * @author Jing Dong 4 | **/ 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | using namespace std; 16 | using namespace gtsam; 17 | using namespace gpmp2; 18 | 19 | 20 | double sdf_wrapper(const SignedDistanceField& field, const Point3& p) { 21 | return field.getSignedDistance(p); 22 | } 23 | 24 | /* ************************************************************************** */ 25 | TEST(SDFutils, test) { 26 | // data 27 | vector data; 28 | data.push_back((Matrix(5, 5) << 29 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 30 | 1.4142, 1, 1, 1, 1.4142, 31 | 1.4142, 1, 1, 1, 1.4142, 32 | 1.4142, 1, 1, 1, 1.4142, 33 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321).finished()); 34 | data.push_back((Matrix(5, 5) << 35 | 1.4142, 1, 1, 1, 1.4142, 36 | 1, 0, 0, 0, 1, 37 | 1, 0, 0, 0, 1, 38 | 1, 0, 0, 0, 1, 39 | 1.4142, 1, 1, 1,4142).finished()); 40 | data.push_back((Matrix(5, 5) << 41 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 42 | 1.4142, 1, 1, 1, 1.4142, 43 | 1.4142, 1, 1, 1, 1.4142, 44 | 1.4142, 1, 1, 1, 1.4142, 45 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321).finished()); 46 | Point3 origin(-0.2, -0.2, -0.1); 47 | double cell_size = 0.1; 48 | 49 | // constructor 50 | SignedDistanceField field(origin, cell_size, data); 51 | EXPECT_LONGS_EQUAL(5, field.x_count()); 52 | EXPECT_LONGS_EQUAL(5, field.y_count()); 53 | EXPECT_LONGS_EQUAL(3, field.z_count()); 54 | EXPECT_DOUBLES_EQUAL(0.1, field.cell_size(), 1e-9); 55 | EXPECT(assert_equal(origin, field.origin())); 56 | 57 | // access 58 | SignedDistanceField::float_index idx; 59 | idx = field.convertPoint3toCell(Point3(0, 0, 0)); 60 | EXPECT_DOUBLES_EQUAL(2, idx.get<0>(), 1e-9); 61 | EXPECT_DOUBLES_EQUAL(2, idx.get<1>(), 1e-9); 62 | EXPECT_DOUBLES_EQUAL(1, idx.get<2>(), 1e-9); 63 | EXPECT_DOUBLES_EQUAL(0, field.signed_distance(idx), 1e-9) 64 | idx = field.convertPoint3toCell(Point3(0.18, -0.18, 0.07)); // tri-linear interpolation 65 | EXPECT_DOUBLES_EQUAL(0.2, idx.get<0>(), 1e-9); 66 | EXPECT_DOUBLES_EQUAL(3.8, idx.get<1>(), 1e-9); 67 | EXPECT_DOUBLES_EQUAL(1.7, idx.get<2>(), 1e-9); 68 | EXPECT_DOUBLES_EQUAL(1.488288, field.signed_distance(idx), 1e-9) 69 | idx = boost::make_tuple(1.0, 2.0, 3.0); 70 | EXPECT(assert_equal(Point3(0.0, -0.1, 0.2), field.convertCelltoPoint3(idx))); 71 | 72 | // gradient 73 | Vector3 grad_act, grad_exp; 74 | Point3 p; 75 | p = Point3(-0.13, -0.14, 0.06); 76 | field.getSignedDistance(p, grad_act); 77 | grad_exp = numericalDerivative11(boost::function( 78 | boost::bind(sdf_wrapper, field, _1)), p, 1e-6); 79 | EXPECT(assert_equal(grad_exp, grad_act, 1e-6)); 80 | 81 | p = Point3(0.18, 0.12, 0.01); 82 | field.getSignedDistance(p, grad_act); 83 | grad_exp = numericalDerivative11(boost::function( 84 | boost::bind(sdf_wrapper, field, _1)), p, 1e-6); 85 | EXPECT(assert_equal(grad_exp, grad_act, 1e-6)); 86 | } 87 | 88 | /* ************************************************************************** */ 89 | /* main function */ 90 | int main() { 91 | TestResult tr; 92 | return TestRegistry::runAllTests(tr); 93 | } 94 | 95 | 96 | -------------------------------------------------------------------------------- /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 | /* ************************************************************************** */ 32 | TrajOptimizerSetting::TrajOptimizerSetting(size_t system_dof) : 33 | dof(system_dof), 34 | total_step(10), 35 | total_time(1.0), 36 | conf_prior_model(noiseModel::Isotropic::Sigma(system_dof, 0.0001)), 37 | vel_prior_model(noiseModel::Isotropic::Sigma(system_dof, 0.0001)), 38 | flag_pos_limit(false), 39 | flag_vel_limit(false), 40 | joint_pos_limits_up(1e6 * Vector::Ones(system_dof)), 41 | joint_pos_limits_down(-1e6 * Vector::Ones(system_dof)), 42 | vel_limits(1e6 * Vector::Ones(system_dof)), 43 | pos_limit_thresh(0.001 * Vector::Ones(system_dof)), 44 | vel_limit_thresh(0.001 * Vector::Ones(system_dof)), 45 | pos_limit_model(noiseModel::Isotropic::Sigma(system_dof, 0.001)), 46 | vel_limit_model(noiseModel::Isotropic::Sigma(system_dof, 0.001)), 47 | epsilon(0.2), 48 | cost_sigma(0.1), 49 | obs_check_inter(5), 50 | Qc_model(noiseModel::Unit::Create(system_dof)), 51 | opt_type(Dogleg), 52 | opt_verbosity(None), 53 | final_iter_no_increase(true), 54 | rel_thresh(1e-2), 55 | max_iter(50) { 56 | } 57 | 58 | /* ************************************************************************** */ 59 | void TrajOptimizerSetting::set_pos_limit_model(const gtsam::Vector& v) { 60 | pos_limit_model = noiseModel::Diagonal::Sigmas(v); 61 | } 62 | 63 | /* ************************************************************************** */ 64 | void TrajOptimizerSetting::set_vel_limit_model(const gtsam::Vector& v) { 65 | vel_limit_model = noiseModel::Diagonal::Sigmas(v); 66 | } 67 | 68 | /* ************************************************************************** */ 69 | void TrajOptimizerSetting::set_conf_prior_model(double sigma) { 70 | conf_prior_model = noiseModel::Isotropic::Sigma(dof, sigma); 71 | } 72 | 73 | /* ************************************************************************** */ 74 | void TrajOptimizerSetting::set_vel_prior_model(double sigma) { 75 | vel_prior_model = noiseModel::Isotropic::Sigma(dof, sigma); 76 | } 77 | 78 | /* ************************************************************************** */ 79 | void TrajOptimizerSetting::set_Qc_model(const Matrix& Qc) { 80 | Qc_model = noiseModel::Gaussian::Covariance(Qc); 81 | } 82 | 83 | } 84 | -------------------------------------------------------------------------------- /gpmp2/planner/tests/testISAM2TrajOptimizer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testISAM2TrajOptimizer.cpp 3 | * @author Jing Dong 4 | * @date May 30, 2016 5 | **/ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | 17 | using namespace std; 18 | using namespace gtsam; 19 | using namespace gpmp2; 20 | 21 | 22 | 23 | /* ************************************************************************** */ 24 | TEST(ISAM2TrajOptimizer, constructor) { 25 | 26 | // 2 link simple example, with none zero base poses 27 | Vector2 a(1, 1), alpha(0, 0), d(0, 0); 28 | Pose3 base_pose(Rot3(), Point3(2.0, 1.0, -1.0)); 29 | Arm abs_arm(2, a, alpha, d, base_pose); 30 | // body spheres 31 | BodySphereVector body_spheres; 32 | body_spheres.push_back(BodySphere(0, 0.5, Point3(-1.0, 0, 0))); 33 | body_spheres.push_back(BodySphere(0, 0.1, Point3(-0.5, 0, 0))); 34 | body_spheres.push_back(BodySphere(0, 0.1, Point3(0, 0, 0))); 35 | body_spheres.push_back(BodySphere(1, 0.1, Point3(-0.5, 0, 0))); 36 | body_spheres.push_back(BodySphere(1, 0.1, Point3(0, 0, 0))); 37 | ArmModel arm(abs_arm, body_spheres); 38 | 39 | // sdf 40 | vector data; 41 | data.push_back((Matrix(5, 5) << 42 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 43 | 1.4142, 1, 1, 1, 1.4142, 44 | 1.4142, 1, 1, 1, 1.4142, 45 | 1.4142, 1, 1, 1, 1.4142, 46 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321).finished()); 47 | data.push_back((Matrix(5, 5) << 48 | 1.4142, 1, 1, 1, 1.4142, 49 | 1, 0, 0, 0, 1, 50 | 1, 0, 0, 0, 1, 51 | 1, 0, 0, 0, 1, 52 | 1.4142, 1, 1, 1,4142).finished()); 53 | data.push_back((Matrix(5, 5) << 54 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321, 55 | 1.4142, 1, 1, 1, 1.4142, 56 | 1.4142, 1, 1, 1, 1.4142, 57 | 1.4142, 1, 1, 1, 1.4142, 58 | 1.7321, 1.4142, 1.4142, 1.4142, 1.7321).finished()); 59 | Point3 origin(-0.2, -0.2, -0.1); 60 | double cell_size = 0.1; 61 | SignedDistanceField field(origin, cell_size, data); 62 | 63 | // settings 64 | TrajOptimizerSetting setting(2); 65 | 66 | ISAM2TrajOptimizer3DArm isam(arm, field, setting); 67 | } 68 | 69 | /* ************************************************************************** */ 70 | /* main function */ 71 | int main() { 72 | TestResult tr; 73 | return TestRegistry::runAllTests(tr); 74 | } 75 | -------------------------------------------------------------------------------- /gpmp2/planner/tests/testTrajUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file testTrajUtils.cpp 3 | * @author Jing Dong 4 | * @date May 30, 2016 5 | **/ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | 19 | using namespace std; 20 | using namespace gtsam; 21 | using namespace gpmp2; 22 | 23 | 24 | 25 | /* ************************************************************************** */ 26 | TEST(TrajUtils, interpolateLinear) { 27 | 28 | Matrix Qc = 0.01 * Matrix::Identity(2,2); 29 | noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc); 30 | 31 | // values to be interpolated 32 | Values init_values; 33 | // const velocity vb = 10, dt = 0.1 34 | init_values.insert(Symbol('x', 0), (Vector(2) << 0, 0).finished()); 35 | init_values.insert(Symbol('x', 1), (Vector(2) << 1, 0).finished()); 36 | init_values.insert(Symbol('v', 0), (Vector(2) << 10, 0).finished()); 37 | init_values.insert(Symbol('v', 1), (Vector(2) << 10, 0).finished()); 38 | 39 | // interpolate 5 interval in the middle 40 | Values inter_values = interpolateArmTraj(init_values, Qc_model, 0.1, 4); 41 | 42 | EXPECT(assert_equal((Vector(2) << 0, 0).finished(), inter_values.at(Symbol('x', 0)), 1e-6)); 43 | EXPECT(assert_equal((Vector(2) << 0.2, 0).finished(), inter_values.at(Symbol('x', 1)), 1e-6)); 44 | EXPECT(assert_equal((Vector(2) << 0.4, 0).finished(), inter_values.at(Symbol('x', 2)), 1e-6)); 45 | EXPECT(assert_equal((Vector(2) << 0.6, 0).finished(), inter_values.at(Symbol('x', 3)), 1e-6)); 46 | EXPECT(assert_equal((Vector(2) << 0.8, 0).finished(), inter_values.at(Symbol('x', 4)), 1e-6)); 47 | EXPECT(assert_equal((Vector(2) << 1, 0).finished(), inter_values.at(Symbol('x', 5)), 1e-6)); 48 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 0)), 1e-6)); 49 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 1)), 1e-6)); 50 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 2)), 1e-6)); 51 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 3)), 1e-6)); 52 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 4)), 1e-6)); 53 | EXPECT(assert_equal((Vector(2) << 10, 0).finished(), inter_values.at(Symbol('v', 5)), 1e-6)); 54 | } 55 | 56 | /* ************************************************************************** */ 57 | TEST(TrajUtils, initPose2VectorTrajStraightLine_1) { 58 | Values init_values = initPose2VectorTrajStraightLine( 59 | Pose2(1,3,M_PI-0.5), (Vector(2) << 2,4).finished(), 60 | Pose2(3,7,-M_PI+0.5), (Vector(2) << 4,8).finished(), 5); 61 | 62 | EXPECT(assert_equal(Pose2Vector(Pose2(1,3,M_PI-0.5), (Vector(2) << 2,4).finished()), 63 | init_values.at(Symbol('x', 0)), 1e-6)); 64 | //EXPECT(assert_equal(Pose2Vector(Pose2(1.4,3.8,M_PI-0.3), (Vector(2) << 2.4,4.8).finished()), 65 | // init_values.at(Symbol('x', 1)), 1e-6)); 66 | } 67 | 68 | /* ************************************************************************** */ 69 | /* main function */ 70 | int main() { 71 | TestResult tr; 72 | return TestRegistry::runAllTests(tr); 73 | } 74 | -------------------------------------------------------------------------------- /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 | 10 | #include 11 | 12 | using namespace gtsam; 13 | 14 | 15 | namespace gpmp2 { 16 | 17 | /* ************************************************************************** */ 18 | void convertValuesOpenRavePointer(size_t dof, const gtsam::Values& results, double *pointer, size_t total_step, 19 | const gtsam::Vector& joint_lower_limit, const gtsam::Vector& joint_upper_limit) { 20 | 21 | // check limit dof 22 | if (joint_lower_limit.size() != static_cast(dof) || 23 | joint_upper_limit.size() != static_cast(dof)) 24 | throw std::runtime_error("[convertValuesOpenRavePointer] ERROR: Joint limit size is different from DOF."); 25 | 26 | for (size_t i = 0; i <= total_step; i++) { 27 | const Key pose_key = Symbol('x', i); 28 | const Key vel_key = Symbol('v', i); 29 | const Vector& conf = results.at(pose_key); 30 | const Vector& vel = results.at(vel_key); 31 | 32 | // copy memory 33 | for (size_t j = 0; j < dof; j++) { 34 | // check for joint limits 35 | if (conf(j) < joint_lower_limit(j)) pointer[dof*i + j] = joint_lower_limit(j); 36 | else if (conf(j) > joint_upper_limit(j)) pointer[dof*i + j] = joint_upper_limit(j); 37 | else pointer[dof*i + j] = conf(j); 38 | // vel 39 | pointer[dof*(i+total_step+1) + j] = vel(j); 40 | } 41 | } 42 | } 43 | 44 | /* ************************************************************************** */ 45 | void convertOpenRavePointerValues(size_t dof, gtsam::Values& results, double *pointer, 46 | size_t total_step) { 47 | 48 | results.clear(); 49 | 50 | for (size_t i = 0; i <= total_step; i++) { 51 | 52 | // key 53 | const Key pose_key = Symbol('x', i); 54 | const Key vel_key = Symbol('v', i); 55 | 56 | // get conf and vel values 57 | Vector conf(dof), vel(dof); 58 | for (size_t j = 0; j < dof; j++) { 59 | conf(j) = pointer[dof*i + j]; 60 | vel(j) = pointer[dof*(i+total_step+1) + j]; 61 | } 62 | 63 | // insert values 64 | results.insert(pose_key, conf); 65 | results.insert(vel_key, vel); 66 | } 67 | } 68 | 69 | } 70 | 71 | -------------------------------------------------------------------------------- /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 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | namespace gpmp2 { 20 | 21 | /// convert values into double pointer to vis in openrave plugin 22 | /// the pointer is size (2 x step) x DOF, include both conf pose and vel 23 | /// the pointer must be allocated and free by outside utils (memory not managed here) 24 | /// also joint limits are applied here to make sure the range of output conf 25 | GPMP2_EXPORT void convertValuesOpenRavePointer(size_t dof, const gtsam::Values& results, 26 | double *pointer, size_t total_step, const gtsam::Vector& joint_lower_limit, 27 | const gtsam::Vector& joint_upper_limit); 28 | 29 | 30 | /// convert double pointer in openrave plugin to gtsam values 31 | /// the pointer is size (2 x step) x DOF, include both conf pose and vel 32 | GPMP2_EXPORT void convertOpenRavePointerValues(size_t dof, gtsam::Values& results, 33 | double *pointer, size_t total_step); 34 | 35 | } 36 | 37 | 38 | -------------------------------------------------------------------------------- /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 | 14 | namespace gpmp2 { 15 | 16 | /* ************************************************************************** */ 17 | bool readSDFvolfile(const std::string& filename_pre, SignedDistanceField& sdf) { 18 | 19 | const string headfilename = filename_pre + ".vol.head"; 20 | ifstream head_file(headfilename.c_str(), ios::in); 21 | if (!head_file.is_open()) 22 | return false; 23 | 24 | size_t field_rows, field_cols, field_z; 25 | double origin_x, origin_y, origin_z; 26 | double vol_res; 27 | head_file >> field_cols >> field_rows >> field_z; 28 | head_file >> origin_x >> origin_y >> origin_z; 29 | head_file >> vol_res; 30 | 31 | cout << field_cols << ", " << field_cols << ", " << field_cols << endl; 32 | cout << origin_x << ", " << origin_y << ", " << origin_z << endl; 33 | cout << vol_res << endl; 34 | 35 | head_file.close(); 36 | 37 | const string datafilename = filename_pre + ".vol.data"; 38 | ifstream data_file(datafilename.c_str(), ios::in); 39 | if (!data_file.is_open()) 40 | return false; 41 | 42 | vector vmat; 43 | for (size_t z = 0; z < field_z; z++) { 44 | vmat.push_back(Matrix(field_rows, field_cols)); 45 | } 46 | 47 | double tmpsdf; 48 | for (size_t x = 0; x < field_cols; x++) { 49 | for (size_t y = 0; y < field_rows; y++) { 50 | for (size_t z = 0; z < field_z; z++) { 51 | data_file >> tmpsdf; 52 | vmat[z](y,x) = tmpsdf; 53 | } 54 | } 55 | } 56 | 57 | data_file.close(); 58 | 59 | sdf = SignedDistanceField(Point3(origin_x, origin_y, origin_z), vol_res, vmat); 60 | 61 | return true; 62 | } 63 | 64 | 65 | } 66 | 67 | 68 | -------------------------------------------------------------------------------- /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 | 14 | namespace gpmp2 { 15 | 16 | /// read SDF from .vol file 17 | /// the filename_pre.vol.head has the size/origin/resolution data 18 | /// the filename_pre.vol.data contains actual data 19 | /// return whether successfully read 20 | GPMP2_EXPORT bool readSDFvolfile(const std::string& filename_pre, SignedDistanceField& sdf); 21 | 22 | 23 | } 24 | 25 | 26 | -------------------------------------------------------------------------------- /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 | 11 | namespace gpmp2 { 12 | 13 | /* ************************************************************************** */ 14 | void insertPose2VectorInValues(gtsam::Key key, const gpmp2::Pose2Vector& p, 15 | gtsam::Values& values) { 16 | values.insert(key, p); 17 | } 18 | 19 | /* ************************************************************************** */ 20 | gpmp2::Pose2Vector atPose2VectorValues(gtsam::Key key, const gtsam::Values& values) { 21 | return values.at(key); 22 | } 23 | 24 | } 25 | 26 | 27 | -------------------------------------------------------------------------------- /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 | 13 | #include 14 | #include 15 | 16 | 17 | namespace gpmp2 { 18 | 19 | /// prior factor on Pose2Vector 20 | typedef gtsam::PriorFactor PriorFactorPose2Vector; 21 | 22 | 23 | /// Pose2Vector Values utils 24 | GPMP2_EXPORT void insertPose2VectorInValues(gtsam::Key key, const gpmp2::Pose2Vector& p, gtsam::Values& values); 25 | GPMP2_EXPORT gpmp2::Pose2Vector atPose2VectorValues(gtsam::Key key, const gtsam::Values& values); 26 | 27 | } 28 | 29 | 30 | -------------------------------------------------------------------------------- /gpmp2_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 | -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/__init__.py: -------------------------------------------------------------------------------- 1 | from gpmp2_python.version import __version__ 2 | -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/datasets/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/gpmp2_python/gpmp2_python/datasets/__init__.py -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/robots/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/gpmp2_python/gpmp2_python/robots/__init__.py -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/gpmp2_python/gpmp2_python/utils/__init__.py -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/utils/signedDistanceField2D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gtsam import * 3 | from gpmp2 import * 4 | import numpy as np 5 | from gtsam import * 6 | from gpmp2 import * 7 | import math 8 | from scipy import ndimage 9 | 10 | 11 | def signedDistanceField2D(ground_truth_map, cell_size): 12 | # SIGNEDDISTANCEFIELD2D 2D signed distance field 13 | # Given a ground truth 2D map defined in Matrix in 0-1, 14 | # calculate 2D signed distance field, which is defined as a matrix 15 | # map matrix and signed distance field matrix have the same resolution. 16 | # 17 | # Usage: field = SIGNEDDISTANCEFIELD2D(ground_truth_map, cell_siz) 18 | # @map evidence grid from dataset, map use 0 show open area, 1 show objects. 19 | # @cell_size cell sizeto given metric information 20 | # 21 | # Output: 22 | # @field sdf, row is Y, col is X 23 | 24 | # regularize unknow area to open area 25 | cur_map = ground_truth_map > 0.75 26 | cur_map = cur_map.astype(int) 27 | 28 | if np.amax(cur_map) is 0: 29 | return np.ones(ground_truth_map.shape) * 1000 30 | 31 | # inverse map 32 | inv_map = 1 - cur_map 33 | 34 | # get signed distance from map and inverse map 35 | # since bwdist(foo) = ndimage.distance_transform_edt(1-foo) 36 | map_dist = ndimage.distance_transform_edt(inv_map) 37 | inv_map_dist = ndimage.distance_transform_edt(cur_map) 38 | 39 | field = map_dist - inv_map_dist 40 | 41 | # metric 42 | field = field * cell_size 43 | field = field.astype(float) 44 | 45 | return field 46 | -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/utils/signedDistanceField3D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gtsam import * 3 | from gpmp2 import * 4 | import math 5 | from scipy import ndimage 6 | 7 | 8 | def signedDistanceField3D(ground_truth_map, cell_size): 9 | # SIGNEDDISTANCEFIELD3D 3D signed distance field 10 | # Given a ground truth 3D map defined in Matrix in 0-1, 11 | # calculate 3D signed distance field, which is defined as a matrix 12 | # map matrix and signed distance field matrix have the same resolution. 13 | # 14 | # Usage: field = SIGNEDDISTANCEFIELD3D(ground_truth_map, cell_siz) 15 | # @map evidence grid from dataset, map use 0 show open area, 1 show objects. 16 | # @cell_size cell sizeto given metric information 17 | # 18 | # Output: 19 | # @field sdf, row is X, col is Y, 3rd index is Z 20 | 21 | # regularize unknow area to open area 22 | cur_map = ground_truth_map > 0.75 23 | cur_map = cur_map.astype(int) 24 | 25 | if np.amax(cur_map) is 0: 26 | return np.ones(ground_truth_map.shape) * 1000 27 | 28 | # inverse map 29 | inv_map = 1 - cur_map 30 | 31 | # get signed distance from map and inverse map 32 | # since bwdist(foo) = ndimage.distance_transform_edt(1-foo) 33 | map_dist = ndimage.distance_transform_edt(inv_map) 34 | inv_map_dist = ndimage.distance_transform_edt(cur_map) 35 | 36 | field = map_dist - inv_map_dist 37 | 38 | # metric 39 | field = field * cell_size 40 | field = field.astype(float) 41 | 42 | return field 43 | -------------------------------------------------------------------------------- /gpmp2_python/gpmp2_python/version.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.3.0" 2 | -------------------------------------------------------------------------------- /gpmp2_python/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | -------------------------------------------------------------------------------- /gpmp2_python/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from setuptools import setup, find_packages 4 | 5 | dir_path = os.path.dirname(os.path.realpath(__file__)) 6 | 7 | with open(os.path.join(dir_path,"gpmp2_python", "version.py")) as fp: 8 | exec(fp.read()) 9 | 10 | def read_requirements_file(filename): 11 | req_file_path = "%s/%s" % (dir_path, filename) 12 | with open(req_file_path) as f: 13 | return [line.strip() for line in f] 14 | 15 | requirements_file = "requirements.txt" 16 | 17 | setup( 18 | name="gpmp2_python", 19 | version=__version__, 20 | packages=find_packages(exclude=("scripts", "tests")), 21 | install_requires=read_requirements_file(requirements_file) 22 | ) 23 | -------------------------------------------------------------------------------- /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.transform_from(Point2(vehsize(1)/2, vehsize(2)/2)); 11 | corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2)); 12 | corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2)); 13 | corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2)); 14 | 15 | % vehicle base black lines 16 | h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ... 17 | [corner1.y() corner2.y() corner3.y() corner4.y() corner1.y()], '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.transform_from(Point2(vehsize(1)/2, vehsize(2)/2)); 11 | corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2)); 12 | corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2)); 13 | corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2)); 14 | 15 | % vehicle base black lines 16 | h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ... 17 | [corner1.y() corner2.y() corner3.y() corner4.y() corner1.y()], '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.transform_from(Point2(vehsize(1)/2, vehsize(2)/2)); 10 | corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2)); 11 | corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2)); 12 | corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2)); 13 | 14 | % vehicle base black lines 15 | h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ... 16 | [corner1.y() corner2.y() corner3.y() corner4.y() corner1.y()], '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 | colormap(color_rgb); 20 | [X_ball, Y_ball, Z_ball] = sphere(16); 21 | 22 | for i=1:robot.nr_body_spheres() 23 | h(i) = surf(X_ball * robot.sphere_radius(i-1) + body_points(1, i), ... 24 | Y_ball * robot.sphere_radius(i-1) + body_points(2, i), ... 25 | Z_ball * robot.sphere_radius(i-1) + body_points(3, i)); 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 | # Examples 2 | message(STATUS "Installing Matlab toolbox scripts") 3 | 4 | install_matlab_scripts("${CMAKE_CURRENT_SOURCE_DIR}/" "+gpmp2/*.m") 5 | -------------------------------------------------------------------------------- /matlab/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 | -------------------------------------------------------------------------------- /projects/jist/.gitignore: -------------------------------------------------------------------------------- 1 | results/ 2 | multirun/ 3 | ablation_studies/ 4 | -------------------------------------------------------------------------------- /projects/jist/README.md: -------------------------------------------------------------------------------- 1 | # JIST 2 | 3 | This is the implementation for [Joint Sampling and Trajectory Optimization Over Graphs for Online Motion Planning](https://arxiv.org/abs/2011.07171), Kalyan Vasudev Alwala and Mustafa Mukadam, IROS 2021. 4 | 5 | JIST (JoInt Sampling and Trajectory optimization), is a unified approach that leverages the complementary strengths of sampling and optimization, and interleaves them both to tackle highly dynamic environments with long horizons that necessitate a fast online solution. See [project page](https://sites.google.com/view/jistplanner) for more details. 6 | 7 | 8 | ## Installation 9 | 10 | - Install GTSAM and GPMP2 following the [steps here](../../README.md). 11 | - Install Hydra 0.11: `pip install hydra-core==0.11` 12 | - Install OpenCV 4.2.0.32: `pip install opencv-python==4.2.0.32` 13 | 14 | 15 | ## Usage 16 | 17 | Run the following commands to visualize JIST in action. 18 | 19 | - For Random Forest environment 20 | ```python 21 | python pointRobot_example.py problem.node_budget=100 problem.seed_val=1 dataset.obstacle_num=40 problem.sdf_side=65.0 dataset.vel_limit=1.5 22 | ``` 23 | - For Patrol Guard environment 24 | ```python 25 | python pointRobot_example_patrol_guard.py problem=example_patrol dataset.vel_limit=1.6 dataset.obstacle_num=2 problem.sdf_side=20 26 | ``` 27 | 28 | 29 | ## Evaluation 30 | 31 | To replicate Table 1 results from the paper, run the following commands. 32 | 33 | - For Random Forest environment 34 | ```python 35 | python pointRobot_test.py dataset.vel_limit=1.5 dataset.obstacle_num=80 problem.node_budget=60 problem.sdf_side=60.0 36 | ``` 37 | - For Patrol Guard environment 38 | ```python 39 | python pointRobot_test_patrol_guard.py dataset.vel_limit=1.4 dataset.obstacle_num=2 problem.node_budget=100 problem.sdf_side=60.0 problem=example_patrol dataset=patrol_guard 40 | ``` 41 | 42 | 43 | ## Citation 44 | 45 | If you use JIST in an academic context, please cite following publication. 46 | 47 | ```latex 48 | @inproceedings{alwala2021joint, 49 | title={Joint sampling and trajectory optimization over graphs for online motion planning}, 50 | author={Alwala, Kalyan Vasudev and Mukadam, Mustafa}, 51 | booktitle={International Conference on Intelligent Robots and Systems (IROS)}, 52 | year={2021} 53 | } 54 | ``` 55 | 56 | 57 | License 58 | ----- 59 | 60 | JIST is released under the BSD license, reproduced in [LICENSE](../../LICENSE). 61 | -------------------------------------------------------------------------------- /projects/jist/configs/algorithm/rrt.yaml: -------------------------------------------------------------------------------- 1 | problem: 2 | 3 | #RRT specific things 4 | node_num: 100 5 | width: 20.0 6 | height: 20.0 7 | move_along_val: 0.5 8 | move_along_val_theta: 0.2 9 | -------------------------------------------------------------------------------- /projects/jist/configs/dataset/dynamic2D.yaml: -------------------------------------------------------------------------------- 1 | dataset: 2 | #Receding Horizon specific things 3 | name: "Dynamic2Ddataset" 4 | obstacle_num: 80 5 | vel_limit : 1 # m/s 6 | acc_limit: .1 7 | -------------------------------------------------------------------------------- /projects/jist/configs/dataset/patrol_guard.yaml: -------------------------------------------------------------------------------- 1 | dataset: 2 | #Receding Horizon specific things 3 | name: "Patrol2Ddataset" 4 | obstacle_num: 2 5 | vel_limit : 0.8 # m/s 6 | acc_limit: .1 7 | -------------------------------------------------------------------------------- /projects/jist/configs/default_config.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - problem: example 3 | #- algorithm/rh # this extends the "problem" with algo specific params 4 | #- algorithm/rrt # this extends the "problem" with algo specific params 5 | - dataset: dynamic2D 6 | -------------------------------------------------------------------------------- /projects/jist/configs/problem/example.yaml: -------------------------------------------------------------------------------- 1 | output_dir: results_random_final 2 | 3 | 4 | hydra: 5 | run: 6 | dir: results/${output_dir} 7 | 8 | 9 | 10 | problem: 11 | 12 | use_theta : True 13 | seed_val: 0 14 | time_out_time: 300 15 | 16 | #Robot 17 | radius: 1.5 18 | vehicle_dynamics: True 19 | vehicle_model: 0.001 20 | 21 | #Noise model 22 | use_noise: True 23 | sensor_noise: 0.03 24 | action_noise: 0.03 25 | 26 | # Core GPMP config 27 | use_GP_inter: True 28 | delta_t : 0.25 29 | inter_step: 4 30 | avg_vel: 0.1 31 | 32 | 33 | #GPMP-Factor related 34 | cost_sigma: 0.2 35 | epsilon_dist: 4.0 36 | #sigma_goal: 2.0 37 | sigma_start: 0.0001 38 | use_trustregion_opt: False 39 | 40 | # Common 41 | use_vel_limit: True 42 | sigma_vel_limit: 0.001 43 | node_budget: 60 44 | sdf_side: 100.0 45 | 46 | #Receding Horizon specific things 47 | init_fraction_length: 0.2 48 | goal_region_threshold: 0.5 49 | connection_threshold: 0.5 50 | same_state_init: True 51 | sigma_goal_rh: 10.0 52 | 53 | #RRT specific things 54 | width: 100.0 55 | height: 100.0 56 | move_along_val: 0.25 57 | move_along_val_theta: 0.2 58 | use_prev_graph: True 59 | sigma_goal_costco: 4.0 60 | 61 | costco_goal_scaling: 0.0 62 | -------------------------------------------------------------------------------- /projects/jist/configs/problem/example_patrol.yaml: -------------------------------------------------------------------------------- 1 | output_dir: results_patrol_final 2 | 3 | 4 | hydra: 5 | run: 6 | dir: results/${output_dir} 7 | 8 | 9 | 10 | problem: 11 | 12 | use_theta : True 13 | seed_val: 0 14 | time_out_time: 200 15 | #Robot 16 | radius: 0.5 17 | vehicle_dynamics: True 18 | vehicle_model: 0.001 19 | 20 | #Noise model 21 | use_noise: True 22 | sensor_noise: 0.03 23 | action_noise: 0.03 24 | 25 | # Core GPMP config 26 | use_GP_inter: True 27 | delta_t : 0.25 28 | inter_step: 4 29 | avg_vel: 0.3 30 | 31 | #GPMP-Factor related 32 | cost_sigma: 0.1 33 | epsilon_dist: 1.0 34 | #sigma_goal: 2.0 35 | sigma_start: 0.0001 36 | use_trustregion_opt: True 37 | 38 | # Common 39 | use_vel_limit: True 40 | sigma_vel_limit: 0.01 41 | node_budget: 100 42 | sdf_side: 60.0 43 | 44 | #Receding Horizon specific things 45 | 46 | init_fraction_length: 0.2 47 | goal_region_threshold: 1.0 48 | connection_threshold: 0.5 49 | same_state_init: True 50 | sigma_goal_rh: 5.0 51 | #RRT specific things 52 | 53 | width: 20.0 54 | height: 20.0 55 | move_along_val: 0.5 56 | move_along_val_theta: 0.2 57 | use_prev_graph: True 58 | sigma_goal_costco: 5.0 59 | 60 | costco_goal_scaling: 0.0 61 | -------------------------------------------------------------------------------- /projects/jist/jist/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/projects/jist/jist/__init__.py -------------------------------------------------------------------------------- /projects/jist/jist/datasets/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/projects/jist/jist/datasets/__init__.py -------------------------------------------------------------------------------- /projects/jist/jist/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/gpmp2/4289d9a5a756006dae9ba8f4072ff4b95abf2e39/projects/jist/jist/utils/__init__.py -------------------------------------------------------------------------------- /projects/jist/jist/utils/signedDistanceField2D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gtsam import * 3 | from gpmp2 import * 4 | import numpy as np 5 | from gtsam import * 6 | from gpmp2 import * 7 | import math 8 | from scipy import ndimage 9 | 10 | 11 | def signedDistanceField2D(ground_truth_map, cell_size): 12 | # SIGNEDDISTANCEFIELD2D 2D signed distance field 13 | # Given a ground truth 2D map defined in Matrix in 0-1, 14 | # calculate 2D signed distance field, which is defined as a matrix 15 | # map matrix and signed distance field matrix have the same resolution. 16 | # 17 | # Usage: field = SIGNEDDISTANCEFIELD2D(ground_truth_map, cell_siz) 18 | # @map evidence grid from dataset, map use 0 show open area, 1 show objects. 19 | # @cell_size cell sizeto given metric information 20 | # 21 | # Output: 22 | # @field sdf, row is Y, col is X 23 | 24 | # regularize unknow area to open area 25 | cur_map = ground_truth_map > 0.75 26 | cur_map = cur_map.astype(int) 27 | 28 | if np.amax(cur_map) is 0: 29 | return np.ones(ground_truth_map.shape) * 1000 30 | 31 | # inverse map 32 | inv_map = 1 - cur_map 33 | 34 | # get signed distance from map and inverse map 35 | # since bwdist(foo) = ndimage.distance_transform_edt(1-foo) 36 | map_dist = ndimage.distance_transform_edt(inv_map) 37 | inv_map_dist = ndimage.distance_transform_edt(cur_map) 38 | 39 | field = map_dist - inv_map_dist 40 | 41 | # metric 42 | field = field * cell_size 43 | field = field.astype(float) 44 | 45 | return field 46 | -------------------------------------------------------------------------------- /projects/jist/patrol_abl.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | 4 | # velocity ablation 5 | python pointRobot_test_patrol_guard.py --multirun hydra.sweep.dir=./ablation_studies/patrol_vel_abl \ 6 | dataset.vel_limit=1.2,1.4,1.6,1.8 dataset.obstacle_num=2 \ 7 | problem.node_budget=100 problem.sdf_side=60.0 problem=example_patrol dataset=patrol_guard 8 | 9 | # Ostacle Number 10 | python pointRobot_test_patrol_guard.py --multirun hydra.sweep.dir=./ablation_studies/patrol_obs_abl \ 11 | dataset.vel_limit=1.4 dataset.obstacle_num=1,2 \ 12 | problem.node_budget=100 problem.sdf_side=60.0 problem=example_patrol dataset=patrol_guard 13 | 14 | # Node budget 15 | python pointRobot_test_patrol_guard.py --multirun hydra.sweep.dir=./ablation_studies/patrol_node_abl \ 16 | dataset.vel_limit=1.4 dataset.obstacle_num=2 \ 17 | problem.node_budget=40,60,80,100,120 problem.sdf_side=60.0 problem=example_patrol dataset=patrol_guard 18 | 19 | # Noise 20 | python pointRobot_test_patrol_guard.py --multirun hydra.sweep.dir=./ablation_studies/patrol_noise_abl \ 21 | dataset.vel_limit=1.4 dataset.obstacle_num=2 \ 22 | problem.node_budget=100 problem.sdf_side=60.0 problem.action_noise=0.0,0.05,0.1,0.15,0.20 \ 23 | problem=example_patrol dataset=patrol_guard 24 | 25 | 26 | #common 27 | python pointRobot_test_patrol_guard.py \ 28 | dataset.vel_limit=1.4 dataset.obstacle_num=2 \ 29 | problem.node_budget=100 problem.sdf_side=60.0 \ 30 | problem=example_patrol dataset=patrol_guard 31 | -------------------------------------------------------------------------------- /projects/jist/pointRobot_example.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | 4 | 5 | from jist.datasets.generate2Ddataset import * 6 | from jist.jist import rrt_chain, make_problem_from_config 7 | 8 | import hydra 9 | import sys 10 | 11 | #python pointRobot_example.py problem.node_budget=100 problem.seed_val=1 dataset.obstacle_num=40 problem.sdf_side=65.0 dataset.vel_limit=1.5 12 | 13 | @hydra.main(config_path="configs/default_config.yaml") 14 | def main(hydra_cfg): 15 | 16 | print(hydra_cfg.pretty()) 17 | 18 | plot_mode = "debug" 19 | start_conf = np.asarray([10.0, 20.0, 0.0]) # np.asarray([10, 20, 0]) 20 | goal_conf = np.asarray([40.0, 60.0, np.pi / 2]) # np.asarray([20, 40, 0]) 21 | 22 | seed_val = hydra_cfg.problem.seed_val 23 | 24 | 25 | print("########## Running JIST #########################") 26 | # dataset_func = getattr(sys.modules[__name__], hydra_cfg.dataset.name) 27 | dataset = Dynamic2Ddataset() # dataset_func() 28 | dataset.init_obstacles( 29 | seed_val=seed_val + 1, obstacle_num=hydra_cfg.dataset.obstacle_num 30 | ) 31 | problem = make_problem_from_config(hydra_cfg.problem, start_conf, goal_conf) 32 | print(rrt_chain(start_conf, goal_conf, dataset, problem, plot_mode=plot_mode)) 33 | 34 | 35 | 36 | if __name__ == "__main__": 37 | main() 38 | -------------------------------------------------------------------------------- /projects/jist/pointRobot_example_patrol_guard.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | 4 | 5 | from jist.datasets.generate2Ddataset import * 6 | from jist.jist import rrt_chain, make_problem_from_config 7 | 8 | import hydra 9 | import sys 10 | 11 | 12 | # python pointRobot_example_patrol_guard.py problem=example_patrol dataset.vel_limit=1.6 dataset.obstacle_num=2 problem.sdf_side=20 13 | 14 | @hydra.main(config_path="configs/default_config.yaml") 15 | def main(hydra_cfg): 16 | 17 | print(hydra_cfg.pretty()) 18 | 19 | plot_mode = "debug" 20 | start_conf = np.asarray([10.0, 1.0, np.pi/2.0]) # np.asarray([10, 20, 0]) 21 | goal_conf = np.asarray([10.0, 37.0, 0.0]) # np.asarray([20, 40, 0]) 22 | 23 | seed_val = hydra_cfg.problem.seed_val 24 | 25 | print("########## Running jist #########################") 26 | # dataset_func = getattr(sys.modules[__name__], hydra_cfg.dataset.name) 27 | dataset = Patrol2Ddataset() # dataset_func() 28 | dataset.init_obstacles( 29 | seed_val=seed_val + 1, obstacle_num=hydra_cfg.dataset.obstacle_num, vel_limit=hydra_cfg.dataset.vel_limit 30 | ) 31 | problem = make_problem_from_config(hydra_cfg.problem, start_conf, goal_conf) 32 | print(rrt_chain(start_conf, goal_conf, dataset, problem, plot_mode=plot_mode)) 33 | 34 | 35 | 36 | if __name__ == "__main__": 37 | main() 38 | -------------------------------------------------------------------------------- /projects/jist/random_abl.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | 4 | # Ostacle Number 5 | python pointRobot_test.py --multirun hydra.sweep.dir=./ablation_studies/random_obs_abl \ 6 | dataset.vel_limit=1.5 dataset.obstacle_num=20,40,60,80,100 \ 7 | problem.node_budget=60 problem.sdf_side=60.0 8 | 9 | # Node budget 10 | python pointRobot_test.py --multirun hydra.sweep.dir=./ablation_studies/random_node_abl \ 11 | dataset.vel_limit=1.5 dataset.obstacle_num=80 \ 12 | problem.node_budget=40,60,80,100,120 problem.sdf_side=60.0 13 | 14 | # Noise 15 | python pointRobot_test.py --multirun hydra.sweep.dir=./ablation_studies/random_noise_abl \ 16 | dataset.vel_limit=1.5 dataset.obstacle_num=80 \ 17 | problem.node_budget=60 problem.sdf_side=60.0 problem.action_noise=0.0,0.05,0.1,0.15,0.20 18 | 19 | 20 | # velocity ablation 21 | python pointRobot_test.py --multirun hydra.sweep.dir=./ablation_studies/random_vel_abl \ 22 | dataset.vel_limit=0.5,1.0,1.5,2.0,2.5 dataset.obstacle_num=80 \ 23 | problem.node_budget=60 problem.sdf_side=60.0 24 | 25 | # common 26 | python pointRobot_test.py \ 27 | dataset.vel_limit=1.5 dataset.obstacle_num=80 \ 28 | problem.node_budget=60 problem.sdf_side=60.0 29 | --------------------------------------------------------------------------------