├── .gitignore ├── CHANGELOG ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── jaco2.yaml ├── pr2_rightarm.yaml ├── vector.yaml └── wam.yaml ├── doc ├── config.md ├── development.md ├── index.md ├── problem.md ├── sdf.md └── usage.md ├── launch ├── gpmp2_interface.launch └── steap_interface.launch ├── package.xml ├── piper ├── base │ ├── misc.h │ ├── problem.cpp │ ├── problem.h │ ├── robot.cpp │ ├── robot.h │ ├── traj.cpp │ └── traj.h ├── gpmp2_interface │ ├── gpmp2_interface.cpp │ └── gpmp2_interface.h └── steap_interface │ ├── steap_interface.cpp │ └── steap_interface.h ├── problem └── example_vector.yaml └── sdf └── empty_sdf.bin /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *~ 3 | *pyc 4 | -------------------------------------------------------------------------------- /CHANGELOG: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for PIPER 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.0 (2017-06-20) 6 | ------------------ 7 | * Initial release 8 | * Contributors: Mustafa Mukadam 9 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(piper) 3 | 4 | # version indicator 5 | set(PIPER_VERSION_MAJOR 0) 6 | set(PIPER_VERSION_MINOR 1) 7 | set(PIPER_VERSION_PATCH 0) 8 | set(PIPER_VERSION_STRING "${PIPER_VERSION_MAJOR}.${PIPER_VERSION_MINOR}.${PIPER_VERSION_PATCH}") 9 | 10 | # Find catkin macros and libraries 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | control_msgs 16 | actionlib 17 | roslib 18 | sensor_msgs 19 | geometry_msgs 20 | trajectory_msgs 21 | ) 22 | 23 | # C++11 flags for GTSAM 4.0 24 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 25 | 26 | # find GTSAM 27 | find_package(GTSAM REQUIRED) 28 | include_directories(${GTSAM_INCLUDE_DIR}) 29 | set(GTSAM_LIBRARIES gtsam) 30 | 31 | # find GPMP2 32 | find_package(gpmp2 REQUIRED) 33 | include_directories(${gpmp2_INCLUDE_DIR}) 34 | set(gpmp2_LIBRARIES gpmp2) 35 | 36 | catkin_package() 37 | 38 | # Specify additional locations of header files 39 | include_directories(${catkin_INCLUDE_DIRS}) 40 | 41 | 42 | ################################################ 43 | ## base ## 44 | ################################################ 45 | include_directories(piper/base/) 46 | 47 | file(GLOB BASE_SRC "piper/base/*.cpp") 48 | add_library(piperbase ${BASE_SRC}) 49 | 50 | target_link_libraries(piperbase 51 | ${catkin_LIBRARIES} 52 | ${GTSAM_LIBRARIES} 53 | ${gpmp2_LIBRARIES} 54 | ) 55 | 56 | install(DIRECTORY piper/base/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 59 | ) 60 | 61 | # options for which interfaces need to be built and installed 62 | option(BUILD_ALL_INTERFACE "whether to build all interfaces" OFF) 63 | option(BUILD_GPMP2_INTERFACE "whether to build interface for batch GPMP2" OFF) 64 | option(BUILD_STEAP_INTERFACE "whether to build interface for STEAP" OFF) 65 | 66 | 67 | ################################################ 68 | ## all ## 69 | ################################################ 70 | if(BUILD_ALL_INTERFACE) 71 | set(BUILD_GPMP2_INTERFACE ON) 72 | set(BUILD_STEAP_INTERFACE ON) 73 | endif() 74 | 75 | 76 | ################################################ 77 | ## gpmp2_interface ## 78 | ################################################ 79 | if(BUILD_GPMP2_INTERFACE) 80 | include_directories(piper/gpmp2_interface/) 81 | 82 | file(GLOB GPMP2_INTERFACE_SRC "piper/gpmp2_interface/*.cpp") 83 | add_executable(gpmp2_interface ${GPMP2_INTERFACE_SRC}) 84 | 85 | target_link_libraries(gpmp2_interface piperbase) 86 | 87 | install(TARGETS gpmp2_interface 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | 91 | install(DIRECTORY piper/gpmp2_interface/ 92 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 93 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 94 | ) 95 | endif() 96 | 97 | 98 | ################################################ 99 | ## steap_interface ## 100 | ################################################ 101 | if(BUILD_STEAP_INTERFACE) 102 | include_directories(piper/steap_interface/) 103 | 104 | file(GLOB STEAP_INTERFACE_SRC "piper/steap_interface/*.cpp") 105 | add_executable(steap_interface ${STEAP_INTERFACE_SRC}) 106 | 107 | target_link_libraries(steap_interface piperbase) 108 | 109 | install(TARGETS steap_interface 110 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 111 | ) 112 | 113 | install(DIRECTORY piper/steap_interface/ 114 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 115 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 116 | ) 117 | endif() 118 | 119 | 120 | ################################################ 121 | ## launch ## 122 | ################################################ 123 | install(DIRECTORY launch/ 124 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 125 | ) 126 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | PIPER 2 | =================================================== 3 | 4 | PIPER (Probabilistic Inference based Platform for Essential problems in Robotics) is a modular package that provides support for algorithms that use probabilistic inference on factor graphs to solve various robotics problems. Each module can be independently installed and implements an easy ROS interface for that algorithm to run on any simulated or real robots. Currently PIPER supports the following algorithms: 5 | 6 | - [GPMP2](http://www.cc.gatech.edu/~bboots3/files/GPMP2.pdf) - Gaussian Process Motion Planner 2 7 | - [STEAP](http://www.cc.gatech.edu/~bboots3/files/STEAP.pdf) - Simultaneous Trajectory Estimation and Planning 8 | - more coming soon ... 9 | 10 | PIPER is being developed by [Mustafa Mukadam](mailto:mmukadam3@gatech.edu) at the Georgia Tech Robot Learning Lab. See [documentation](doc/index.md) for information on usage and development. 11 | 12 | --- 13 | Table of Contents 14 | --- 15 | - [Prerequisites](#prerequisites) 16 | - [Compilation and Installation](#compilation-and-installation) 17 | - [Questions and Bug reporting](#questions-and-bug-reporting) 18 | - [Citing](#citing) 19 | - [FAQs](#faqs) 20 | - [License](#license) 21 | 22 | --- 23 | Prerequisites 24 | ------ 25 | 26 | - Install [ROS](http://wiki.ros.org/Distributions). We have tested up to ROS Kinetic on Ubuntu 16.04. 27 | - Install [GPMP2](https://github.com/gtrll/gpmp2) C++ library. 28 | 29 | 30 | Compilation and Installation 31 | ------ 32 | 33 | - Initialize a catkin workspace (if you are using an existing catkin workspace this step is not needed) 34 | 35 | ```bash 36 | mkdir -p ~/piper_ws/src 37 | cd ~/piper_ws/src 38 | catkin_init_workspace 39 | ``` 40 | 41 | Before running setup the environment variables 42 | 43 | ```bash 44 | source ~/piper_ws/devel/setup.bash 45 | ``` 46 | 47 | - Clone this repository in ```~/piper_ws/src``` 48 | 49 | ```bash 50 | git clone https://github.com/gtrll/piper.git 51 | ``` 52 | 53 | - To compile only the ```piperbase``` library, in the catkin workspace directory do 54 | 55 | ```bash 56 | catkin_make 57 | ``` 58 | 59 | - Otherwise, to install some module, for example, _X_: first make sure to install dependencies for _X_, besides the prerequisites for piperbase, then do 60 | 61 | ```bash 62 | catkin_make -build_flag_X 63 | ``` 64 | 65 | - Similarly, to install multiple modules, for example, _X_ and _Y_: install all their dependencies and then use their appropriate flags together 66 | 67 | ```bash 68 | catkin_make -build_flag_X -build_flag_Y 69 | ``` 70 | 71 | See table below for currently supported modules, their dependencies and build flags 72 | 73 | | Module | Other Dependencies | Build Flag | 74 | |:------:|:------------------:|:----------:| 75 | |GPMP2|None|```-DBUILD_GPMP2_INTERFACE:OPTION=ON```| 76 | |STEAP|None|```-DBUILD_STEAP_INTERFACE:OPTION=ON```| 77 | |ALL|All from above|```-DBUILD_ALL_INTERFACE:OPTION=ON```| 78 | 79 | 80 | Questions and Bug reporting 81 | ----- 82 | 83 | Please use Github issue tracker to report bugs. For other questions please contact [Mustafa Mukadam](mailto:mmukadam3@gatech.edu). 84 | 85 | 86 | Citing 87 | ----- 88 | 89 | If you use PIPER in an academic context, please cite any module/algorithm specific publications you use, and cite the following: 90 | 91 | ``` 92 | @article{mukadam2017piper, 93 | title={{PIPER}}, 94 | author={Mukadam, Mustafa}, 95 | journal={[Online] Available at \url{https://github.com/gtrll/piper}}, 96 | year={2017} 97 | } 98 | ``` 99 | 100 | 101 | FAQs 102 | ----- 103 | 104 | - Q: Cannot find trajectory_control server error 105 | 106 | A: To be able to run the included examples you need to have an [action server](http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29) set up in your real or simulated robot's API. See ```trajectory_control_topic``` in [robot config](https://github.com/gtrll/piper/blob/master/doc/config.md). 107 | 108 | - Q: Node crashes without running anything 109 | 110 | A: First verify if the error is related to different versions of Eigen being linked. 111 | Run ```gdb ~/piper_ws/devel/lib/piper/gpmp2_interface``` and ```r```. If you get the following error 112 | ```Program received signal SIGSEGV, Segmentation fault. 0x00007ffff672a2e7 in gtsam::noiseModel::Constrained::Constrained(Eigen::Matrix const& Eigen::Matrix const&) () from /usr/local/lib/libgtsam.so.4```, 113 | it means that there are at least two versions of eigen conflicting. Make sure to install GTSAM with system Eigen. Then install GPMP2 and try this again. 114 | 115 | 116 | License 117 | ----- 118 | 119 | PIPER is released under the BSD license. See LICENSE file in this directory. 120 | -------------------------------------------------------------------------------- /config/jaco2.yaml: -------------------------------------------------------------------------------- 1 | piper: 2 | robot: 3 | DOF: 6 4 | arm_base: {orientation: [0, 0, 0, 1], position: [0, 0, 0]} 5 | DH: 6 | alpha: [1.5708, 3.1416, 1.5708, 1.0472, 1.0472, 3.1416] 7 | a: [0, 0.41, 0, 0, 0, 0] 8 | d: [0.2755, 0, -0.0098, -0.2501, -0.0856, -0.2228] 9 | theta: [0, -1.5708, 1.5708, 0, -3.1416, 1.5708] 10 | theta_neg: [true, false, false, false, false, false] 11 | spheres: 12 | js: [0,0,0,0,1,1,1,1,1,1,1,2,2,2,2,3,3,4,4,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5] 13 | xs: [0,0,0,0,0,-0.06,-0.12,-0.18,-0.24,-0.3,-0.36,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] 14 | ys: [0,-0.08,-0.155,-0.23,0,0,0,0,0,0,0,-0.01,-0.01,0,0,0,0,0,-0.008,0.05,0.05,0.06,0.06,0.035, 15 | -0.05,-0.05,-0.06,-0.06,-0.035,0.015,0.025,0,-0.025,-0.015] 16 | zs: [0,0,0,0,0,0.03,0.03,0.03,0.03,0.03,0.03,-0.05,-0.1,-0.15,-0.2,0,-0.045,0,-0.075,-0.01,0.01, 17 | -0.039,-0.067,-0.042,-0.01,0.01,-0.039,-0.067,-0.042,-0.055,-0.08,-0.08,-0.08,-0.055] 18 | rs: [0.053,0.053,0.053,0.053,0.053,0.04,0.04,0.04,0.04,0.04,0.04,0.035,0.03,0.035,0.035,0.04,0.04, 19 | 0.04,0.05,0.013,0.013,0.018,0.018,0.018,0.013,0.013,0.018,0.018,0.018,0.02,0.02,0.02,0.02,0.02] 20 | arm_joint_names: [right_shoulder_pan_joint, right_shoulder_lift_joint, right_elbow_joint, 21 | right_wrist_1_joint, right_wrist_2_joint, right_wrist_3_joint] 22 | sensor_arm_sigma: 0.0001 23 | trajectory_control_topic: /jaco2/arm_controller/trajectory 24 | est_traj_pub_topic: /piper/est_traj 25 | plan_traj_pub_topic: /piper/plan_traj 26 | arm_state_topic: /joint_states 27 | -------------------------------------------------------------------------------- /config/pr2_rightarm.yaml: -------------------------------------------------------------------------------- 1 | piper: 2 | robot: 3 | DOF: 7 4 | arm_base: {orientation: [0, 0, 0, 1], position: [-0.05, -0.188, 0.802175]} 5 | DH: 6 | alpha: [-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0] 7 | a: [0.1, 0, 0, 0, 0, 0, 0] 8 | d: [0, 0, 0.4, 0, 0.321, 0, 0] 9 | theta: [0, 1.5708, 0, 0, 0, 0, 0] 10 | spheres: 11 | js: [0,2,2,2,2,4,4,4,4,4,4,4,6,6,6,6,6,6,6,6,6,6,6] 12 | xs: [-0.01,0.015,0.035,0.035,0,-0.005,0.01,0.01,0.015,0.015,0.005,0.005,0,0,0,0,0,0,0,0,0,0,0] 13 | ys: [0,0.22,0.14,0.0725,0,0.191,0.121,0.121,0.056,0.056,0.001,0.001,-0.0175,0.0175,0,0.036,0.027, 14 | 0.009,0.0095,-0.036,-0.027,-0.009,-0.0095] 15 | zs: [0,0,0,0,0,0,-0.025,0.025,-0.0275,0.0275,-0.0225,0.0225,0.0725,0.0725,0.0925,0.11,0.155,0.18, 16 | 0.205,0.11,0.155,0.18,0.205] 17 | rs: [0.18,0.11,0.08,0.08,0.105,0.075,0.055,0.055,0.05,0.05,0.05,0.05,0.04,0.04,0.04,0.04,0.035, 18 | 0.03,0.02,0.04,0.035,0.03,0.02] 19 | arm_joint_names: [r_shoulder_pan_joint, r_shoulder_lift_joint, r_upper_arm_roll_joint, 20 | r_elbow_flex_joint, r_forearm_roll_joint, r_wrist_flex_joint, r_wrist_roll_joint] 21 | trajectory_control_topic: /pr2/right_arm_controller/trajectory 22 | est_traj_pub_topic: /piper/est_traj 23 | plan_traj_pub_topic: /piper/plan_traj 24 | arm_state_topic: /joint_states 25 | -------------------------------------------------------------------------------- /config/vector.yaml: -------------------------------------------------------------------------------- 1 | piper: 2 | robot: 3 | mobile_base: true 4 | DOF: 9 5 | arm_base: {orientation: [0.5, 0.5, 0.5, 0.5], position: [0.2889, 0, 1.0864]} 6 | DH: 7 | alpha: [1.5708, 3.1416, 1.5708, 1.0472, 1.0472, 3.1416] 8 | a: [0, 0.41, 0, 0, 0, 0] 9 | d: [0.2755, 0, -0.0098, -0.2501, -0.0856, -0.2228] 10 | theta: [0, -1.5708, 1.5708, 0, -3.1416, 1.5708] 11 | theta_neg: [true, false, false, false, false, false] 12 | spheres: 13 | js: [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 14 | 0,0,0,0,0,0,0,0,0,0,1,1,1,1,2,2,2,2,2,2,2,3,3,3,3,4,4,5,5,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6] 15 | xs: [-0.01,-0.26,-0.26,-0.26,0.24,0.24,0.24,0.04,-0.2,-0.2,0.16,0.16,0.16,0.16,0.33,-0.01, 16 | -0.12,-0.22,-0.32,0.1,0.2,0.3,-0.01,-0.12,-0.22,-0.32,0.1,0.2,0.3,-0.32,-0.32,-0.32,0.32, 17 | 0.32,0.32,0.12,0.14,0.14,0.19,0.14,0.14,0.175,0.175,0.175,0.175,0.175,0.27,0.37,0.37,0.37, 18 | 0.37,0.37,0,0,0,0,0,-0.06,-0.12,-0.18,-0.24,-0.3,-0.36,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 19 | 0,0,0,0,0,0] 20 | ys: [-0.01,-0.01,0.15,-0.17,-0.01,0.15,-0.17,-0.01,-0.06,0.04,-0.07,0.05,-0.18,0.16,-0.01, 21 | -0.24,-0.24,-0.24,-0.24,-0.24,-0.24,-0.24,0.22,0.22,0.22,0.22,0.22,0.22,0.22,-0.01,0.1,-0.13, 22 | -0.01,0.1,-0.13,-0.01,-0.11,0.09,-0.01,-0.11,0.09,-0.01,-0.01,-0.01,-0.01,-0.01,-0.01, 23 | -0.01,-0.01,-0.01,-0.1,0.08,0,-0.08,-0.155,-0.23,0,0,0,0,0,0,0,-0.01,-0.01,0,0,0,0,0,-0.008, 24 | 0.05,0.05,0.06,0.06,0.035,-0.05,-0.05,-0.06,-0.06,-0.035,0.015,0.025,0,-0.025,-0.015] 25 | zs: [0.22,0.08,0.08,0.08,0.08,0.08,0.08,0.6,0.45,0.45,0.41,0.41,0.41,0.41,0.29,0.31,0.31,0.31, 26 | 0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.31,0.87, 27 | 0.78,0.78,1.07,0.97,0.97,1.2,1.3,1.4,1.5,1.62,1.5,1.5,1.6,1.66,1.66,1.66,0,0,0,0,0,0.03, 28 | 0.03,0.03,0.03,0.03,0.03,-0.05,-0.1,-0.15,-0.2,0,-0.045,0,-0.075,-0.01,0.01,-0.039,-0.067, 29 | -0.042,-0.01,0.01,-0.039,-0.067,-0.042,-0.055,-0.08,-0.08,-0.08,-0.055] 30 | rs: [0.21,0.08,0.08,0.08,0.08,0.08,0.08,0.18,0.1,0.1,0.06,0.06,0.06,0.06,0.05,0.05,0.05,0.05, 31 | 0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.1, 32 | 0.08,0.08,0.08,0.08,0.08,0.05,0.05,0.05,0.05,0.07,0.05,0.05,0.05,0.045,0.045,0.045,0.053, 33 | 0.053,0.053,0.053,0.053,0.04,0.04,0.04,0.04,0.04,0.04,0.035,0.03,0.035,0.035,0.04,0.04,0.04, 34 | 0.05,0.013,0.013,0.018,0.018,0.018,0.013,0.013,0.018,0.018,0.018,0.02,0.02,0.02,0.02,0.02] 35 | arm_joint_names: [right_shoulder_pan_joint, right_shoulder_lift_joint, right_elbow_joint, 36 | right_wrist_1_joint, right_wrist_2_joint, right_wrist_3_joint] 37 | sensor_arm_sigma: 0.0001 38 | sensor_base_sigma: 0.0001 39 | trajectory_control_topic: /vector/full_body_controller/trajectory 40 | est_traj_pub_topic: /piper/est_traj 41 | plan_traj_pub_topic: /piper/plan_traj 42 | arm_state_topic: /joint_states 43 | base_state_topic: /base_state 44 | -------------------------------------------------------------------------------- /config/wam.yaml: -------------------------------------------------------------------------------- 1 | piper: 2 | robot: 3 | DOF: 7 4 | arm_base: {orientation: [0, 0, 0, 1], position: [0, 0, 0]} 5 | DH: 6 | alpha: [-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0] 7 | a: [0, 0, 0.045, -0.045, 0, 0, 0] 8 | d: [0, 0, 0.55, 0, 0.3, 0, 0.06] 9 | theta: [0, 0, 0, 0, 0, 0, 0] 10 | spheres: 11 | js: [0,1,1,1,1,2,3,3,3,5,6,6,6,6,6,6] 12 | xs: [0,0,0,0,0,0,0,0,0,0,0.1,0.1,-0.1,0.15,0.15,-0.15] 13 | ys: [0,0,0,0,0,0,0,0,0,0,-0.025,0.025,0,-0.025,0.025,0] 14 | zs: [0,0.2,0.3,0.4,0.5,0,0.1,0.2,0.3,0.1,0.08,0.08,0.08,0.13,0.13,0.13] 15 | rs: [0.15,0.06,0.06,0.06,0.06,0.06,0.06,0.06,0.06,0.06,0.04,0.04,0.04,0.04,0.04,0.04] 16 | arm_joint_names: [Shoulder_Yaw, Shoulder_Pitch, Shoulder_Roll, 17 | Elbow, Wrist_Yaw, Wrist_Pitch, Wrist_Roll] 18 | trajectory_control_topic: /wam/arm_controller/trajectory 19 | est_traj_pub_topic: /piper/est_traj 20 | plan_traj_pub_topic: /piper/plan_traj 21 | arm_state_topic: /joint_states 22 | -------------------------------------------------------------------------------- /doc/config.md: -------------------------------------------------------------------------------- 1 | Robot Configuration File 2 | ====================== 3 | Robot configuration and settings are provided to any algorithm via a ```.yaml``` file in the [```/config```](../config) directory. Part of this file is read by the [```Robot```](../piper/base/robot.cpp) class which creates a GPMP2 [```RobotModel```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/kinematics/RobotModel.h), and the remaining part is read by the [```Traj```](../piper/base/traj.cpp) class which assigns settings for controlling the robot through ROS. 4 | 5 | For any robot, following is a list of parameters that can be provided: 6 | 7 | - ```mobile_base```: boolean flag to be set to _True_ if the robot has a mobile base and is being used by any algorithm 8 | - ```DOF```: total degree-of-freedom (DOF) of the robot. For example, a mobile manipulator with a 6 DOF arm has a total of 9 DOF (3 for base + 6 for arm) 9 | - ```arm_base```: pose of the arm's base relative to the base of the robot if using the full robot with the base, otherwise the absolute pose of the the arm's base relative to the world coordinates 10 | - ```DH```: DH parameters of the robot (arm), where ```theta``` is the additive bias applied to any joint and ```theta_neg``` is specified if any joint angles needs to flip their sign 11 | - ```spheres```: for collision checking the robot body is approximated with a collection of spheres, where any sphere of radius ```r``` is located at ```(x, y, z)``` relative to any joint ```j```, and this information is provided with individual lists ```js```, ```xs```, ```ys```, ```zs```, and ```rs``` in order of the spheres 12 | - ```arm_joint_names```: an array of strings with the names of all the joints on the arm in order 13 | - ```sensor_arm_sigma```: variance of sensor model for arm state measurement 14 | - ```sensor_base_sigma```: variance of sensor model for base state measurement 15 | - ```trajectory_control_topic```: topic name for a action client on the robot API that accepts FollowJointTrajectoryAction ROS message type and is used to execute any trajectory on the robot 16 | - ```est_traj_pub_topic```: (optional) publishes estimated trajectory to this topic 17 | - ```plan_traj_pub_topic```: (optional) publishes planned trajectory to this topic 18 | - ```arm_state_topic```: current arm state can be read from this topic and is published by the robot API 19 | - ```base_state_topic```: current base state can be read from this topic and is published by the robot API 20 | 21 | To create a robot config file for your own robot you simply need its DH parameters, an API to execute passed trajectories and publish state information and a list of spheres that well approximate the robot's body. The spheres, for example, in [GPMP2](https://github.com/gtrll/gpmp2) were put in the robot ```.xml``` files and were set by loading the robot model in OpenRAVE or Matlab and then manually working out the sphere locations. Some examples of robot config files are already included in the [```/config```](../config) directory. 22 | 23 | --- 24 | [Back to Usage](usage.md) 25 | 26 | [Back to Documentation home](index.md) 27 | -------------------------------------------------------------------------------- /doc/development.md: -------------------------------------------------------------------------------- 1 | Development Guide 2 | ================== 3 | The [```piperbase```](../piper/base) library implements the basic functionality needed to load robots, problems, etc and interface the trajectory controller for the robot via ROS, and can be used by any algorithm within PIPER. Each remaining directory in [```/piper```](../piper) is associated to interfacing a given algorithm to any robot via ROS. This modular construction allows for easy and selective installation based on what algorithms are desired to be used. 4 | 5 | To add your own algorithm say ```myAlgo``` to the PIPER package you need to create a module for it by following these steps: 6 | 7 | - Add any dependencies say ```myDepend``` and the installation set up to the [CMakeLists.txt](../CMakeLists.txt) file 8 | - Create an option to install your module ```myAlgo_interface``` 9 | 10 | ```cmake 11 | # options for which interfaces need to be built and installed 12 | ... 13 | option(BUILD_myAlgo_INTERFACE "whether to build interface for myAlgo" OFF) 14 | ``` 15 | 16 | - Add option when building all interfaces 17 | 18 | ```cmake 19 | ################################################ 20 | ## all ## 21 | ################################################ 22 | if(BUILD_ALL_INTERFACE) 23 | ... 24 | set(BUILD_myAlgo_INTERFACE ON) 25 | endif() 26 | ``` 27 | 28 | - Add module installation settings 29 | 30 | ```cmake 31 | ################################################ 32 | ## myAlgo_interface ## 33 | ################################################ 34 | if(BUILD_myAlgo_INTERFACE) 35 | find_package(myDepend REQUIRED) 36 | include_directories(${myDepend_INCLUDE_DIR}) 37 | set(myDepend_LIBRARIES myDepend) 38 | 39 | include_directories(piper/myAlgo_interface/) 40 | 41 | file(GLOB myAlgo_INTERFACE_SRC "piper/myAlgo_interface/*.cpp") 42 | add_executable(myAlgo_interface ${myAlgo_INTERFACE_SRC}) 43 | 44 | target_link_libraries(myAlgo_interface ${myDepend_LIBRARIES} piperbase) 45 | 46 | install(TARGETS myAlgo_interface 47 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | 50 | install(DIRECTORY piper/myAlgo_interface/ 51 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 52 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 53 | ) 54 | endif() 55 | ``` 56 | 57 | - Then, add all the source files to this directory: ```/piper/myAlgo_interface``` 58 | 59 | - Finally, to compile do the following in the catkin workspace where PIPER was installed 60 | 61 | ```bash 62 | catkin_make -DBUILD_myAlgo_INTERFACE:OPTION=ON 63 | ``` 64 | 65 | --- 66 | [Back to Documentation home](index.md) 67 | -------------------------------------------------------------------------------- /doc/index.md: -------------------------------------------------------------------------------- 1 | Documentation 2 | ============= 3 | 4 | This is documentation for the PIPER package. The **usage** section will help the user understand, set up and run the included algorithms with their own problems on simulated or real robots via ROS. See the **development guide** if you want to add your own algorithm to this package. For installation guide please see the [README](../README.md) file. 5 | 6 | - [Usage](usage.md) 7 | - [Config files for robot settings](config.md) 8 | - [Problem files for algorithm and problem settings](problem.md) 9 | - [Signed Distance Field files](sdf.md) 10 | - [Development guide](development.md) 11 | -------------------------------------------------------------------------------- /doc/problem.md: -------------------------------------------------------------------------------- 1 | Problem File 2 | ====================== 3 | Problem settings are provided to any algorithm via a ```.yaml``` file in the [```/problem```](../problem) directory. This file is read by the [```Problem```](../piper/base/problem.cpp) class and creates a [BatchTrajOptimizer](https://github.com/gtrll/gpmp2/blob/master/gpmp2/planner/BatchTrajOptimizer.h) or [ISAM2TrajOptimizer](https://github.com/gtrll/gpmp2/blob/master/gpmp2/planner/ISAM2TrajOptimizer.h) and sets up the optimization settings for the algorithm being used. 4 | 5 | For any problem, following is a list of parameters that can be provided. For some of the parameters please see the [GPMP2 documentation](https://github.com/gtrll/gpmp2/blob/master/doc/Parameters.md) to gain more insight: 6 | 7 | - ```start_conf```: (optional) start configuration of the arm, read from ```arm_state_topic``` if not passed 8 | - ```start_pose```: (optional) start pose of the base, read from ```base_state_topic``` if not passed 9 | - ```goal_conf```: goal configuration of the arm 10 | - ```goal_pose```: goal pose of the base 11 | - ```sdf_file```: SDF file in the [```/sdf```](../sdf) directory, used for collision avoidance, see [how to create SDF files](sdf.md) 12 | - ```total_time```: total time length of the trajectory in seconds 13 | - ```total_step```: total number of time steps (support states) on the trajectory (includes start and goal) 14 | - ```obs_check_inter```: number of states to be interpolated between any two support states, for collision cost calculation during optimization 15 | - ```control_inter```: number of states to be interpolated between any two support states, to generate executable trajectory for a robot 16 | - ```cost_sigma```: covariance for obstacle factors 17 | - ```epsilon```: safety distance from obstacles in meters 18 | - ```fix_pose_sigma```: small covariance to fix the position part of the state (mainly used for start and goal) 19 | - ```fix_vel_sigma```: small covariance to fix the velocity part of the state (mainly used for start and goal) 20 | - ```Qc```: covariance for GP factors 21 | - ```opt_type```: optimization method to be used, ```LM``` works fine in most cases 22 | 23 | --- 24 | [Back to Usage](usage.md) 25 | 26 | [Back to Documentation home](index.md) 27 | -------------------------------------------------------------------------------- /doc/sdf.md: -------------------------------------------------------------------------------- 1 | Signed Distance Field File 2 | ====================== 3 | A Signed Distance Field (SDF) file is used for collision avoidance and can be loaded in to any algorithm from the [```/sdf```](../sdf) directory, by passing its filename in the problem file. The [```Problem```](../piper/base/problem.cpp) class create a GPMP2 [```SignedDistanceField```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/obstacle/SignedDistanceField.h) object by reading it with the [```loadSDF()```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/obstacle/SignedDistanceField.h) or [```readSDFvolfile()```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/utils/fileUtils.cpp) function. 4 | 5 | For any of your environments you can create your own SDF file with any of the following options: 6 | 7 | - **Matlab**: A Matlab wrapper is already included with GPMP2. Create your environment (occupancy grid) in Matlab and then generate a SDF by following this [example](https://github.com/gtrll/gpmp2/blob/master/matlab/SaveSDFExample.m). 8 | - **OpenRAVE**: The GPMP2 OpenRAVE plugin - [orgpmp2](https://github.com/gtrll/orgpmp2) can be used to create a SDF. First create your environment with a ```.xml``` file and then follow this [example](https://github.com/gtrll/orgpmp2/blob/master/examples/save_sdf_pr2.py). If you are using only the arm on a mobile manipulator make sure to keep the body of the robot minus the arm in the environment when generating the SDF. 9 | - **Real sensor data**: You can write you own library to map an environment with real sensor data (LIDAR, camera, etc) and create an occupancy grid. Then convert that to the [```SignedDistanceField```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/obstacle/SignedDistanceField.h) data type and save it using the [```saveSDF()```](https://github.com/gtrll/gpmp2/blob/master/gpmp2/obstacle/SignedDistanceField.h) function. A similar process is described in the [STEAP](http://www.cc.gatech.edu/~bboots3/files/STEAP.pdf) publication (see section IV D). 10 | 11 | --- 12 | [Back to Problem file](problem.md) 13 | 14 | [Back to Usage](usage.md) 15 | 16 | [Back to Documentation home](index.md) 17 | -------------------------------------------------------------------------------- /doc/usage.md: -------------------------------------------------------------------------------- 1 | Usage 2 | ============== 3 | Any algorithm can be run on a simulated or real robot with a ```.launch``` file (see some examples in the [```/launch```](../launch) directory) via ROS using the following command 4 | 5 | ```bash 6 | roslaunch piper myAlgo_interface.launch robot:=myRobot problem:=myProblem 7 | ``` 8 | 9 | - ```myAlgo``` is the algorithm you want to run on the robot. Interfaces to all the currently supported algorithms can be found in the [```/piper```](../piper) directory. To add your own algorithm, see the [development guide](development.md). 10 | - ```myRobot``` is a ```.yaml``` file that specifies the robot parameters and settings. They are located in the [```/config```](../config) directory. See [how to use or write config files](config.md) to understand settings of robots currently included or easily write one for your own robot. 11 | - ```myProblem``` is a ```.yaml``` file that specifies the problem to be solved and its settings. They are located in the [```/problem```](../problem) directory. See [how to write problem files](problem.md) to make use of any algorithms that are included. 12 | 13 | --- 14 | [Back to Documentation home](index.md) 15 | -------------------------------------------------------------------------------- /launch/gpmp2_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/steap_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | piper 4 | 0.1.0 5 | The PIPER package 6 | 7 | mmukadam 8 | mmukadam 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | std_msgs 17 | control_msgs 18 | actionlib 19 | roslib 20 | sensor_msgs 21 | geometry_msgs 22 | trajectory_msgs 23 | libgtsam 24 | libgpmp2 25 | 26 | roscpp 27 | rospy 28 | std_msgs 29 | control_msgs 30 | actionlib 31 | roslib 32 | sensor_msgs 33 | geometry_msgs 34 | trajectory_msgs 35 | libgtsam 36 | libgpmp2 37 | 38 | -------------------------------------------------------------------------------- /piper/base/misc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file misc.h 3 | * @brief miscellaneous functions 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #ifndef MISC_H_ 9 | #define MISC_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | namespace piper { 21 | 22 | /** 23 | * Convert std double vector type to gtsam Vector 24 | * 25 | * @param v std double vector 26 | **/ 27 | static const gtsam::Vector getVector(const std::vector& v) 28 | { 29 | gtsam::Vector send(v.size()); 30 | for (size_t i=0; i 9 | 10 | 11 | namespace piper { 12 | 13 | /* ************************************************************************** */ 14 | Problem::Problem(ros::NodeHandle nh) 15 | { 16 | // first load robot 17 | robot = Robot(nh); 18 | 19 | // given the robot load the planning problem 20 | ROS_INFO("Loading planning problem."); 21 | 22 | // start and goal 23 | if (nh.hasParam("start_conf")) 24 | { 25 | nh.getParam("start_conf", sc_); 26 | start_conf = getVector(sc_); 27 | if (robot.isThetaNeg()) 28 | robot.negateTheta(start_conf); 29 | } 30 | nh.getParam("goal_conf", gc_); 31 | goal_conf = getVector(gc_); 32 | if (robot.isThetaNeg()) 33 | robot.negateTheta(goal_conf); 34 | if (robot.isMobileBase()) 35 | { 36 | if (nh.hasParam("start_pose")) 37 | { 38 | nh.getParam("start_pose", sp_); 39 | start_pose = gtsam::Pose2(sp_[0], sp_[1], sp_[2]); 40 | } 41 | nh.getParam("goal_pose", gp_); 42 | goal_pose = gtsam::Pose2(gp_[0], gp_[1], gp_[2]); 43 | pgoal = gpmp2::Pose2Vector(goal_pose, goal_conf); 44 | } 45 | 46 | // signed distance field 47 | nh.getParam("sdf_file", sdf_file_); 48 | sdf_file_ = ros::package::getPath("piper") + "/" + sdf_file_; 49 | std::string fext = sdf_file_.substr(sdf_file_.find_last_of(".") + 1); 50 | if (fext == "vol") 51 | gpmp2::readSDFvolfile(sdf_file_, sdf); 52 | else 53 | sdf.loadSDF(sdf_file_); 54 | 55 | // optimization settings 56 | nh.getParam("total_time", total_time); 57 | nh.getParam("total_step", total_step); 58 | nh.getParam("obs_check_inter", obs_check_inter); 59 | nh.getParam("control_inter", control_inter); 60 | nh.getParam("cost_sigma", cost_sigma); 61 | nh.getParam("epsilon", epsilon); 62 | nh.getParam("opt_type", opt_type_); 63 | nh.getParam("Qc", Qc_); 64 | nh.getParam("fix_pose_sigma", fix_pose_sigma_); 65 | nh.getParam("fix_vel_sigma", fix_vel_sigma_); 66 | int DOF = robot.getDOF(); 67 | opt_setting = gpmp2::TrajOptimizerSetting(DOF); 68 | opt_setting.total_time = total_time; 69 | opt_setting.total_step = total_step-1; 70 | delta_t = total_time/(total_step-1); 71 | opt_setting.obs_check_inter = obs_check_inter; 72 | opt_setting.cost_sigma = cost_sigma; 73 | opt_setting.epsilon = epsilon; 74 | opt_setting.Qc_model = gtsam::noiseModel::Gaussian::Covariance(Qc_*gtsam::Matrix::Identity(DOF, DOF)); 75 | opt_setting.conf_prior_model = gtsam::noiseModel::Isotropic::Sigma(DOF, fix_pose_sigma_); 76 | opt_setting.vel_prior_model = gtsam::noiseModel::Isotropic::Sigma(DOF, fix_vel_sigma_); 77 | if (opt_type_ == "LM") 78 | opt_setting.opt_type = gpmp2::TrajOptimizerSetting::LM; 79 | else if (opt_type_ == "Dogleg") 80 | opt_setting.opt_type = gpmp2::TrajOptimizerSetting::Dogleg; 81 | else if (opt_type_ == "GaussNewton") 82 | opt_setting.opt_type = gpmp2::TrajOptimizerSetting::GaussNewton; 83 | else 84 | { 85 | ROS_ERROR("Optimization type \'%s\' not known!", opt_type_.c_str()); 86 | sigintHandler(0); 87 | } 88 | } 89 | 90 | } // piper namespace 91 | -------------------------------------------------------------------------------- /piper/base/problem.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file problem.h 3 | * @brief problem: load, create, etc 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #ifndef PROBLEM_H_ 9 | #define PROBLEM_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | 27 | namespace piper { 28 | 29 | class Problem 30 | { 31 | public: 32 | Robot robot; 33 | gtsam::Vector start_conf, goal_conf; 34 | gtsam::Pose2 start_pose, goal_pose; 35 | gpmp2::Pose2Vector pstart, pgoal; 36 | gpmp2::SignedDistanceField sdf; 37 | double total_time, cost_sigma, epsilon, delta_t; 38 | int total_step, obs_check_inter, control_inter; 39 | gpmp2::TrajOptimizerSetting opt_setting; 40 | 41 | private: 42 | std::vector sc_, gc_, sp_, gp_; 43 | std::string sdf_file_; 44 | double Qc_, fix_pose_sigma_, fix_vel_sigma_; 45 | std::string opt_type_; 46 | 47 | public: 48 | /// Default constructor 49 | Problem() {} 50 | 51 | /** 52 | * Loads problem from yaml file 53 | * 54 | * @param nh node handle for namespace 55 | **/ 56 | Problem(ros::NodeHandle nh); 57 | 58 | /// Default destructor 59 | virtual ~Problem() {} 60 | }; 61 | 62 | } // piper namespace 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /piper/base/robot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot.h 3 | * @brief robot: load, create, etc 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #include 9 | 10 | 11 | namespace piper { 12 | 13 | /* ************************************************************************** */ 14 | Robot::Robot(ros::NodeHandle nh) 15 | { 16 | // get parameters for desired robot 17 | ROS_INFO("Loading robot parameters."); 18 | nh.getParam("robot/DOF", DOF_); 19 | if (nh.hasParam("robot/mobile_base")) 20 | nh.getParam("robot/mobile_base", mobile_base_); 21 | else 22 | mobile_base_ = false; 23 | if (nh.hasParam("robot/arm_base/orientation")) 24 | nh.getParam("robot/arm_base/orientation", orientation_); // quaternion: [x, y, z, w] 25 | else 26 | orientation_ = (std::vector){0, 0, 0, 1}; 27 | if (nh.hasParam("robot/arm_base/position")) 28 | nh.getParam("robot/arm_base/position", position_); // [x, y, z] 29 | else 30 | position_ = (std::vector){0, 0, 0}; 31 | nh.getParam("robot/DH/a", a_); 32 | nh.getParam("robot/DH/alpha", alpha_); 33 | nh.getParam("robot/DH/d", d_); 34 | nh.getParam("robot/DH/theta", theta_); 35 | if (nh.hasParam("robot/DH/theta_neg")) 36 | nh.getParam("robot/DH/theta_neg", theta_neg_); 37 | nh.getParam("robot/spheres/js", js_); 38 | nh.getParam("robot/spheres/xs", xs_); 39 | nh.getParam("robot/spheres/ys", ys_); 40 | nh.getParam("robot/spheres/zs", zs_); 41 | nh.getParam("robot/spheres/rs", rs_); 42 | if (nh.hasParam("robot/sensor_arm_sigma")) 43 | nh.getParam("robot/sensor_arm_sigma", sensor_arm_sigma); 44 | else 45 | sensor_arm_sigma = 0.0001; 46 | if (nh.hasParam("robot/sensor_base_sigma")) 47 | nh.getParam("robot/sensor_base_sigma", sensor_base_sigma); 48 | else 49 | sensor_base_sigma = 0.0001; 50 | 51 | // arm's base pose (relative to robot base if mobile_base_ is true) 52 | arm_base_ = gtsam::Pose3(gtsam::Rot3::Quaternion(orientation_[3], orientation_[0], orientation_[1], 53 | orientation_[2]), gtsam::Point3(getVector(position_))); 54 | 55 | // spheres to approximate robot body: js - link id, rs - radius, [xs, ys, zs] - center 56 | for (size_t i=0; i 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | 22 | namespace piper { 23 | 24 | class Robot 25 | { 26 | public: 27 | gpmp2::ArmModel arm; 28 | gpmp2::Pose2MobileArmModel marm; 29 | double sensor_arm_sigma, sensor_base_sigma; 30 | 31 | private: 32 | bool mobile_base_; 33 | int DOF_, DOF_arm_; 34 | std::vector a_, alpha_, d_, theta_; 35 | std::vector theta_neg_; 36 | std::vector orientation_, position_; 37 | gtsam::Pose3 arm_base_; 38 | std::vector js_, xs_, ys_, zs_, rs_; 39 | gpmp2::BodySphereVector spheres_data_; 40 | 41 | public: 42 | /// Default constructor 43 | Robot() {} 44 | 45 | /** 46 | * Constructor loads robot parameters from yaml file and constructs a robot 47 | * 48 | * @param nh node handle for namespace 49 | **/ 50 | Robot(ros::NodeHandle nh); 51 | 52 | /// Default destructor 53 | virtual ~Robot() {} 54 | 55 | /// check to see if robot has a mobile base 56 | inline bool isMobileBase() const { return mobile_base_; } 57 | 58 | /// get DOF for full body robot 59 | inline int getDOF() const { return DOF_; } 60 | 61 | /// get DOF for just the arm 62 | inline int getDOFarm() const { return DOF_arm_; } 63 | 64 | /// theta offset in DH params has a sign flip 65 | inline bool isThetaNeg() const { return !theta_neg_.empty(); } 66 | 67 | /** 68 | * Flip sign of theta in DH parameters 69 | * 70 | * @param conf change conf to account for theta bias 71 | **/ 72 | void negateTheta(gtsam::Vector& conf); 73 | }; 74 | 75 | } // piper namespace 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /piper/base/traj.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file traj.cpp 3 | * @brief trajectory: action client, initialize, etc 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #include 9 | 10 | 11 | namespace piper { 12 | 13 | /* ************************************************************************** */ 14 | Traj::Traj(ros::NodeHandle nh) 15 | { 16 | // ros trajectory joint names 17 | nh.getParam("robot/arm_joint_names", arm_joint_names); 18 | traj_.trajectory.joint_names = arm_joint_names; 19 | 20 | // to visualize estimated trajectory 21 | if (nh.hasParam("robot/est_traj_pub_topic")) 22 | { 23 | nh.getParam("robot/est_traj_pub_topic", est_traj_pub_topic_); 24 | est_traj_pub = nh.advertise(est_traj_pub_topic_, 1); 25 | } 26 | 27 | // to visualize planned trajectory 28 | if (nh.hasParam("robot/plan_traj_pub_topic")) 29 | { 30 | nh.getParam("robot/plan_traj_pub_topic", plan_traj_pub_topic_); 31 | plan_traj_pub = nh.advertise(plan_traj_pub_topic_, 1); 32 | } 33 | 34 | // trajectory action client 35 | if (nh.hasParam("robot/trajectory_control_topic")) 36 | { 37 | nh.getParam("robot/trajectory_control_topic", trajectory_control_topic_); 38 | traj_client_ = new Traj::TrajClient(trajectory_control_topic_, true); 39 | if (!traj_client_->waitForServer(ros::Duration(5.0))) 40 | { 41 | ROS_INFO("Waiting for trajectory_control server..."); 42 | if (!traj_client_->waitForServer(ros::Duration(5.0))) 43 | { 44 | ROS_ERROR("Cannot find trajectory_control server \'%s\'", trajectory_control_topic_.c_str()); 45 | sigintHandler(0); 46 | } 47 | } 48 | } 49 | else 50 | ROS_WARN("No trajectory control topic. Trajectory will not be executed."); 51 | } 52 | 53 | /* ************************************************************************** */ 54 | void Traj::initializeTrajectory(gtsam::Values& init_values, Problem& problem) 55 | { 56 | ROS_INFO("Initializing trajectory."); 57 | gtsam::Vector conf, avg_vel; 58 | if (!problem.robot.isMobileBase()) 59 | { 60 | avg_vel = (problem.goal_conf - problem.start_conf)/problem.total_time; 61 | for (size_t i=0; i(i)/static_cast(problem.total_step-1); 64 | conf = (1.0 - ratio)*problem.start_conf + ratio*problem.goal_conf; 65 | init_values.insert(gtsam::Symbol('x',i), conf); 66 | init_values.insert(gtsam::Symbol('v',i), avg_vel); 67 | } 68 | } 69 | else 70 | { 71 | gtsam::Pose2 pose; 72 | avg_vel = (gtsam::Vector(problem.robot.getDOF()) << problem.goal_pose.x()-problem.start_pose.x(), 73 | problem.goal_pose.y()-problem.start_pose.y(), problem.goal_pose.theta()-problem.start_pose.theta(), 74 | problem.goal_conf - problem.start_conf).finished()/problem.total_time; 75 | for (size_t i=0; i(i)/static_cast(problem.total_step-1); 78 | pose = gtsam::Pose2((1.0 - ratio)*problem.start_pose.x() + ratio*problem.goal_pose.x(), 79 | (1.0 - ratio)*problem.start_pose.y() + ratio*problem.goal_pose.y(), 80 | (1.0 - ratio)*problem.start_pose.theta() + ratio*problem.goal_pose.theta()); 81 | conf = (1.0 - ratio)*problem.start_conf + ratio*problem.goal_conf; 82 | init_values.insert(gtsam::Symbol('x',i), gpmp2::Pose2Vector(pose, conf)); 83 | init_values.insert(gtsam::Symbol('v',i), avg_vel); 84 | } 85 | } 86 | } 87 | 88 | /* ************************************************************************** */ 89 | void Traj::executeTrajectory(gtsam::Values& exec_values, Problem& problem, size_t exec_step) 90 | { 91 | gtsam::Pose2 pose; 92 | gtsam::Vector conf, vel; 93 | int DOF = problem.robot.getDOF(); 94 | int DOF_arm = problem.robot.getDOFarm(); 95 | 96 | // create ros trajectory 97 | traj_.trajectory.points.resize(exec_step); 98 | for (size_t i=0; i(gtsam::Symbol('x',i)); 105 | vel = exec_values.at(gtsam::Symbol('v',i)); 106 | if (problem.robot.isThetaNeg()) 107 | problem.robot.negateTheta(conf); 108 | for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); 119 | conf = exec_values.at(gtsam::Symbol('x',i)).configuration(); 120 | vel = exec_values.at(gtsam::Symbol('v',i)); 121 | if (problem.robot.isThetaNeg()) 122 | problem.robot.negateTheta(conf); 123 | traj_.trajectory.points[i].positions[0] = pose.x(); 124 | traj_.trajectory.points[i].positions[1] = pose.y(); 125 | traj_.trajectory.points[i].positions[2] = pose.theta(); 126 | for (size_t j=0; jsendGoal(traj_); 137 | traj_client_->waitForResult(); 138 | } 139 | 140 | /* ************************************************************************** */ 141 | void Traj::publishEstimatedTrajectory(gtsam::Values& values, Problem& problem, size_t step) 142 | { 143 | gtsam::Vector conf; 144 | gtsam::Pose2 pose; 145 | trajectory_msgs::JointTrajectory est_traj; 146 | est_traj.points.resize(step+1); 147 | for (size_t i=0; i(gtsam::Symbol('x',i)); 153 | if (problem.robot.isThetaNeg()) 154 | problem.robot.negateTheta(conf); 155 | for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); 162 | conf = values.at(gtsam::Symbol('x',i)).configuration(); 163 | if (problem.robot.isThetaNeg()) 164 | problem.robot.negateTheta(conf); 165 | est_traj.points[i].positions[0] = pose.x(); 166 | est_traj.points[i].positions[1] = pose.y(); 167 | est_traj.points[i].positions[2] = pose.theta(); 168 | for (size_t j=0; j(gtsam::Symbol('x',i)); 188 | if (problem.robot.isThetaNeg()) 189 | problem.robot.negateTheta(conf); 190 | for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); 197 | conf = values.at(gtsam::Symbol('x',i)).configuration(); 198 | if (problem.robot.isThetaNeg()) 199 | problem.robot.negateTheta(conf); 200 | plan_traj.points[i-step].positions[0] = pose.x(); 201 | plan_traj.points[i-step].positions[1] = pose.y(); 202 | plan_traj.points[i-step].positions[2] = pose.theta(); 203 | for (size_t j=0; j 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | 29 | namespace piper { 30 | 31 | class Traj 32 | { 33 | public: 34 | typedef actionlib::SimpleActionClient TrajClient; 35 | ros::Publisher est_traj_pub, plan_traj_pub; 36 | std::vector arm_joint_names; 37 | 38 | private: 39 | std::string trajectory_control_topic_, est_traj_pub_topic_, plan_traj_pub_topic_; 40 | TrajClient* traj_client_; 41 | control_msgs::FollowJointTrajectoryGoal traj_; 42 | 43 | public: 44 | /// Default constructor 45 | Traj() {} 46 | 47 | /** 48 | * setup traj client and load params 49 | * 50 | * @param nh node handle for namespace 51 | **/ 52 | Traj(ros::NodeHandle nh); 53 | 54 | /// Default destructor 55 | virtual ~Traj() {} 56 | 57 | /** 58 | * initialize trajectory for optimization 59 | * 60 | * @param init_values initialized traj save in to this variable 61 | * @param problem all problem params and settings 62 | **/ 63 | void initializeTrajectory(gtsam::Values& init_values, Problem& problem); 64 | 65 | /** 66 | * initialize trajectory for optimization 67 | * 68 | * @param exec_values optimized, interpolated, and collision checked traj to execute 69 | * @param problem all problem params and settings 70 | * @param exec_step number of points on the trajectory 71 | **/ 72 | void executeTrajectory(gtsam::Values& exec_values, Problem& problem, size_t exec_step); 73 | 74 | /** 75 | * publish estimated trajectory 76 | * 77 | * @param values estimated part of traj 78 | * @param problem all problem params and settings 79 | * @param step estimated traj is from 0 to step 80 | **/ 81 | void publishEstimatedTrajectory(gtsam::Values& values, Problem& problem, size_t step); 82 | 83 | /** 84 | * publish planned trajectory 85 | * 86 | * @param values planned part of traj 87 | * @param problem all problem params and settings 88 | * @param step planned traj is from step to total_step 89 | **/ 90 | void publishPlannedTrajectory(gtsam::Values& values, Problem& problem, size_t step); 91 | }; 92 | 93 | } // piper namespace 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /piper/gpmp2_interface/gpmp2_interface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gpmp2_interface.cpp 3 | * @brief ROS interface between GPMP2 and a real/sim robot 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #include 9 | 10 | 11 | namespace piper { 12 | 13 | /* ************************************************************************** */ 14 | GPMP2Interface::GPMP2Interface(ros::NodeHandle nh) 15 | { 16 | // first load problem and setup trajectory client 17 | problem_ = Problem(nh); 18 | traj_ = Traj(nh); 19 | 20 | // robot state subscriber (used to initialize start state if not passed as param) 21 | if (nh.hasParam("robot/arm_state_topic")) 22 | { 23 | nh.getParam("robot/arm_state_topic", arm_state_topic_); 24 | arm_state_sub_ = nh.subscribe(arm_state_topic_, 1, &GPMP2Interface::armStateCallback, this); 25 | arm_pos_ = gtsam::Vector::Zero(problem_.robot.getDOFarm()); 26 | arm_pos_time_ = ros::Time::now(); 27 | } 28 | if (problem_.robot.isMobileBase() && nh.hasParam("robot/base_state_topic")) 29 | { 30 | nh.getParam("robot/base_state_topic", base_state_topic_); 31 | base_state_sub_ = nh.subscribe(base_state_topic_, 1, &GPMP2Interface::baseStateCallback, this); 32 | base_pos_ = gtsam::Pose2(); 33 | base_pos_time_ = ros::Time::now(); 34 | } 35 | ros::Duration(1.0).sleep(); 36 | 37 | // get start from measurement if not passed as param 38 | if (!nh.hasParam("start_conf")) 39 | { 40 | problem_.start_conf = arm_pos_; 41 | if (problem_.robot.isThetaNeg()) 42 | problem_.robot.negateTheta(problem_.start_conf); 43 | } 44 | if (problem_.robot.isMobileBase()) 45 | { 46 | if (!nh.hasParam("start_pose")) 47 | problem_.start_pose = base_pos_; 48 | problem_.pstart = gpmp2::Pose2Vector(problem_.start_pose, problem_.start_conf); 49 | } 50 | 51 | // initialize trajectory 52 | traj_.initializeTrajectory(init_values_, problem_); 53 | 54 | // solve with batch gpmp2 55 | ROS_INFO("Optimizing..."); 56 | int DOF = problem_.robot.getDOF(); 57 | if (!problem_.robot.isMobileBase()) 58 | batch_values_ = gpmp2::BatchTrajOptimize3DArm(problem_.robot.arm, problem_.sdf, problem_.start_conf, 59 | gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); 60 | else 61 | batch_values_ = gpmp2::BatchTrajOptimizePose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.pstart, 62 | gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); 63 | ROS_INFO("Batch GPMP2 optimization complete."); 64 | 65 | // publish trajectory for visualization or other use 66 | if (traj_.plan_traj_pub) 67 | traj_.publishPlannedTrajectory(batch_values_, problem_, 0); 68 | } 69 | 70 | /* ************************************************************************** */ 71 | void GPMP2Interface::execute() 72 | { 73 | size_t exec_step; 74 | double coll_cost; 75 | 76 | // interpolate batch solution to a desired resolution for control and check for collision 77 | ROS_INFO("Checking for collision."); 78 | if (!problem_.robot.isMobileBase()) 79 | { 80 | exec_values_ = gpmp2::interpolateArmTraj(batch_values_, problem_.opt_setting.Qc_model, problem_.delta_t, 81 | problem_.control_inter, 0, problem_.total_step-1); 82 | coll_cost = gpmp2::CollisionCost3DArm(problem_.robot.arm, problem_.sdf, exec_values_, problem_.opt_setting); 83 | } 84 | else 85 | { 86 | exec_values_ = gpmp2::interpolatePose2MobileArmTraj(batch_values_, problem_.opt_setting.Qc_model, 87 | problem_.delta_t, problem_.control_inter, 0, problem_.total_step-1); 88 | coll_cost = gpmp2::CollisionCostPose2MobileArm(problem_.robot.marm, problem_.sdf, exec_values_, problem_.opt_setting); 89 | } 90 | if (coll_cost != 0) 91 | { 92 | ROS_FATAL("Plan is not collision free! Collision cost = %.3f", coll_cost); 93 | sigintHandler(0); 94 | } 95 | 96 | // execute trajectory 97 | ROS_INFO("Executing GPMP2 planned trajectory open-loop..."); 98 | exec_step = problem_.total_step+problem_.control_inter*(problem_.total_step-1); 99 | traj_.executeTrajectory(exec_values_, problem_, exec_step); 100 | } 101 | 102 | /* ************************************************************************** */ 103 | void GPMP2Interface::armStateCallback(const sensor_msgs::JointState::ConstPtr& msg) 104 | { 105 | size_t index; 106 | for (size_t i=0; iname.begin(), find(msg->name.begin(), msg->name.end(), 109 | traj_.arm_joint_names[i])); 110 | arm_pos_[i] = msg->position[index]; 111 | } 112 | arm_pos_time_ = ros::Time::now(); 113 | } 114 | 115 | /* ************************************************************************** */ 116 | void GPMP2Interface::baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg) 117 | { 118 | base_pos_ = gtsam::Pose2(msg->position.x, msg->position.y, gtsam::Rot3::Quaternion(msg->orientation.w, 119 | msg->orientation.x, msg->orientation.y, msg->orientation.z).yaw()); 120 | base_pos_time_ = ros::Time::now(); 121 | } 122 | 123 | } // piper namespace 124 | 125 | 126 | /* ************************************************************************** */ 127 | /* main callback */ 128 | void mainCallback(const std_msgs::Bool::ConstPtr& msg) 129 | { 130 | ros::NodeHandle nh("piper"); 131 | piper::GPMP2Interface gpmp2(nh); 132 | gpmp2.execute(); 133 | ROS_INFO("Done."); 134 | ros::shutdown(); 135 | } 136 | 137 | /* ************************************************************************** */ 138 | /* main function */ 139 | int main(int argc, char** argv) 140 | { 141 | ros::init(argc, argv, "gpmp2_interface", ros::init_options::NoSigintHandler); 142 | signal(SIGINT, piper::sigintHandler); 143 | ros::MultiThreadedSpinner spinner(0); 144 | 145 | ros::NodeHandle n; 146 | ros::Publisher main_pub = n.advertise("/piper/run_main", 1); 147 | ros::Subscriber main_sub = n.subscribe("/piper/run_main", 1, mainCallback); 148 | main_pub.publish(std_msgs::Bool()); 149 | 150 | spinner.spin(); 151 | } 152 | -------------------------------------------------------------------------------- /piper/gpmp2_interface/gpmp2_interface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gpmp2_interface.h 3 | * @brief ROS interface between GPMP2 and a real/sim robot 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #ifndef GPMP2_INTERFACE_H_ 9 | #define GPMP2_INTERFACE_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | namespace piper { 29 | 30 | class GPMP2Interface 31 | { 32 | private: 33 | Problem problem_; 34 | Traj traj_; 35 | gtsam::Values init_values_, batch_values_, exec_values_; 36 | 37 | std::string arm_state_topic_, base_state_topic_; 38 | ros::Subscriber arm_state_sub_, base_state_sub_; 39 | gtsam::Vector arm_pos_; 40 | gtsam::Pose2 base_pos_; 41 | ros::Time arm_pos_time_, base_pos_time_; 42 | 43 | public: 44 | /// Default constructor 45 | GPMP2Interface() {} 46 | 47 | /** 48 | * batch gpmp2 49 | * 50 | * @param nh node handle for namespace 51 | **/ 52 | GPMP2Interface(ros::NodeHandle nh); 53 | 54 | /// Default destructor 55 | virtual ~GPMP2Interface() {} 56 | 57 | /** 58 | * Open-loop execution of GPMP2 59 | **/ 60 | void execute(); 61 | 62 | /** 63 | * Call back to get current state of the arm 64 | * 65 | * @param msg message from arm state subscriber 66 | **/ 67 | void armStateCallback(const sensor_msgs::JointState::ConstPtr& msg); 68 | 69 | /** 70 | * Call back to get current state of the base 71 | * 72 | * @param msg message from base state subscriber 73 | **/ 74 | void baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg); 75 | }; 76 | 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /piper/steap_interface/steap_interface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file steap_interface.h 3 | * @brief ROS interface between STEAP and a real/sim robot 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #include 9 | 10 | 11 | namespace piper { 12 | 13 | /* ************************************************************************** */ 14 | STEAPInterface::STEAPInterface(ros::NodeHandle nh) 15 | { 16 | // first load problem and setup trajectory client 17 | problem_ = Problem(nh); 18 | traj_ = Traj(nh); 19 | 20 | // robot state subscriber 21 | if (nh.hasParam("robot/arm_state_topic")) 22 | { 23 | nh.getParam("robot/arm_state_topic", arm_state_topic_); 24 | arm_state_sub_ = nh.subscribe(arm_state_topic_, 1, &STEAPInterface::armStateCallback, this); 25 | arm_pos_ = gtsam::Vector::Zero(problem_.robot.getDOFarm()); 26 | arm_pos_time_ = ros::Time::now(); 27 | } 28 | if (problem_.robot.isMobileBase() && nh.hasParam("robot/base_state_topic")) 29 | { 30 | nh.getParam("robot/base_state_topic", base_state_topic_); 31 | base_state_sub_ = nh.subscribe(base_state_topic_, 1, &STEAPInterface::baseStateCallback, this); 32 | base_pos_ = gtsam::Pose2(); 33 | base_pos_time_ = ros::Time::now(); 34 | } 35 | ros::Duration(1.0).sleep(); 36 | 37 | // get start from measurement if not passed as param 38 | if (!nh.hasParam("start_conf")) 39 | { 40 | problem_.start_conf = arm_pos_; 41 | if (problem_.robot.isThetaNeg()) 42 | problem_.robot.negateTheta(problem_.start_conf); 43 | } 44 | if (problem_.robot.isMobileBase()) 45 | { 46 | if (!nh.hasParam("start_pose")) 47 | problem_.start_pose = base_pos_; 48 | problem_.pstart = gpmp2::Pose2Vector(problem_.start_pose, problem_.start_conf); 49 | } 50 | 51 | // initialize trajectory 52 | traj_.initializeTrajectory(init_values_, problem_); 53 | 54 | // solve for initial plan with batch gpmp2 55 | ROS_INFO("Optimizing..."); 56 | int DOF = problem_.robot.getDOF(); 57 | if (!problem_.robot.isMobileBase()) 58 | batch_values_ = gpmp2::BatchTrajOptimize3DArm(problem_.robot.arm, problem_.sdf, problem_.start_conf, 59 | gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); 60 | else 61 | batch_values_ = gpmp2::BatchTrajOptimizePose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.pstart, 62 | gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); 63 | ROS_INFO("Batch optimization complete."); 64 | 65 | // publish trajectory for visualization or other use 66 | if (traj_.plan_traj_pub) 67 | traj_.publishPlannedTrajectory(batch_values_, problem_, 0); 68 | 69 | // set up incremental inference 70 | ROS_INFO("Initializing incremental inference..."); 71 | if (!problem_.robot.isMobileBase()) 72 | { 73 | arm_inc_inf_ = gpmp2::ISAM2TrajOptimizer3DArm(problem_.robot.arm, problem_.sdf, problem_.opt_setting); 74 | arm_inc_inf_.initFactorGraph(problem_.start_conf, gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF)); 75 | arm_inc_inf_.initValues(batch_values_); 76 | arm_inc_inf_.update(); 77 | inc_inf_values_ = arm_inc_inf_.values(); 78 | } 79 | else 80 | { 81 | marm_inc_inf_ = gpmp2::ISAM2TrajOptimizerPose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.opt_setting); 82 | marm_inc_inf_.initFactorGraph(problem_.pstart, gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF)); 83 | marm_inc_inf_.initValues(batch_values_); 84 | marm_inc_inf_.update(); 85 | inc_inf_values_ = marm_inc_inf_.values(); 86 | } 87 | ROS_INFO("Online incremental inference ready."); 88 | } 89 | 90 | /* ************************************************************************** */ 91 | void STEAPInterface::execute() 92 | { 93 | size_t exec_step; 94 | double coll_cost; 95 | gtsam::Matrix sensor_model; 96 | int DOF_arm = problem_.robot.getDOFarm(); 97 | int DOF = problem_.robot.getDOF(); 98 | gtsam::Vector conf; 99 | gtsam::Pose2 pose; 100 | 101 | // sensor model for measurements 102 | if (!problem_.robot.isMobileBase()) 103 | sensor_model = problem_.robot.sensor_arm_sigma*gtsam::Matrix::Identity(DOF_arm, DOF_arm); 104 | else 105 | sensor_model = (gtsam::Matrix(DOF, DOF) << 106 | problem_.robot.sensor_base_sigma*gtsam::Matrix::Identity(3, 3), gtsam::Matrix::Zero(3, DOF_arm), 107 | gtsam::Matrix::Zero(DOF_arm, 3), problem_.robot.sensor_arm_sigma*gtsam::Matrix::Identity(DOF_arm, DOF_arm)).finished(); 108 | 109 | // solve and execute STEAP problem 110 | ROS_INFO("Executing STEAP online..."); 111 | for (size_t step=0; step("/piper/run_main", 1); 219 | ros::Subscriber main_sub = n.subscribe("/piper/run_main", 1, mainCallback); 220 | main_pub.publish(std_msgs::Bool()); 221 | 222 | spinner.spin(); 223 | } 224 | -------------------------------------------------------------------------------- /piper/steap_interface/steap_interface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file steap_interface.h 3 | * @brief ROS interface between STEAP and a real/sim robot 4 | * @author Mustafa Mukadam 5 | * @date Dec 13, 2016 6 | **/ 7 | 8 | #ifndef STEAP_INTERFACE_H_ 9 | #define STEAP_INTERFACE_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace piper { 30 | 31 | class STEAPInterface 32 | { 33 | private: 34 | Problem problem_; 35 | Traj traj_; 36 | gtsam::Values init_values_, batch_values_, inc_inf_values_, exec_values_; 37 | gpmp2::ISAM2TrajOptimizer3DArm arm_inc_inf_; 38 | gpmp2::ISAM2TrajOptimizerPose2MobileArm marm_inc_inf_; 39 | 40 | std::string arm_state_topic_, base_state_topic_; 41 | ros::Subscriber arm_state_sub_, base_state_sub_; 42 | gtsam::Vector arm_pos_; 43 | gtsam::Pose2 base_pos_; 44 | ros::Time arm_pos_time_, base_pos_time_; 45 | 46 | public: 47 | /// Default constructor 48 | STEAPInterface() {} 49 | 50 | /** 51 | * STEAP: simultaneous trajectory estimation and planning 52 | * 53 | * @param nh node handle for namespace 54 | **/ 55 | STEAPInterface(ros::NodeHandle nh); 56 | 57 | /// Default destructor 58 | virtual ~STEAPInterface() {} 59 | 60 | /** 61 | * closed loop online execution 62 | **/ 63 | void execute(); 64 | 65 | /** 66 | * Call back to get current state of the arm 67 | * 68 | * @param msg message from arm state subscriber 69 | **/ 70 | void armStateCallback(const sensor_msgs::JointState::ConstPtr& msg); 71 | 72 | /** 73 | * Call back to get current state of the base 74 | * 75 | * @param msg message from base state subscriber 76 | **/ 77 | void baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg); 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /problem/example_vector.yaml: -------------------------------------------------------------------------------- 1 | piper: 2 | start_conf: [-1.92, 1.5, 0.53, -2.5, -2.91, 0.72] 3 | start_pose: [0, 0, 0] 4 | goal_conf: [4.7124, 3.1416, 3.1416, 0.0, 0.0, 0.0] 5 | goal_pose: [2, -1, -1.5708] 6 | sdf_file: sdf/empty_sdf.bin 7 | total_time: 10.0 8 | total_step: 10 9 | obs_check_inter: 5 10 | control_inter: 5 11 | cost_sigma: 0.005 12 | epsilon: 0.1 13 | fix_pose_sigma: 0.0001 14 | fix_vel_sigma: 0.0001 15 | Qc: 1.0 16 | opt_type: LM 17 | -------------------------------------------------------------------------------- /sdf/empty_sdf.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/piper/c2c1545cf65a5cca9559d83f29cee1b932d75407/sdf/empty_sdf.bin --------------------------------------------------------------------------------