diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2951182 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +*~ +*pyc diff --git a/CHANGELOG b/CHANGELOG new file mode 100644 index 0000000..b107a8e --- /dev/null +++ b/CHANGELOG @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for PIPER +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2017-06-20) +------------------ +* Initial release +* Contributors: Mustafa Mukadam diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5fbea5c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,125 @@ +cmake_minimum_required(VERSION 2.8.3) +project(piper) + +# version indicator +set(PIPER_VERSION_MAJOR 0) +set(PIPER_VERSION_MINOR 1) +set(PIPER_VERSION_PATCH 0) +set(PIPER_VERSION_STRING "${PIPER_VERSION_MAJOR}.${PIPER_VERSION_MINOR}.${PIPER_VERSION_PATCH}") + +# Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + control_msgs + actionlib + roslib + sensor_msgs + geometry_msgs + trajectory_msgs +) + +# C++11 flags for GTSAM 4.0 +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +# find GTSAM +find_package(GTSAM REQUIRED) +include_directories(${GTSAM_INCLUDE_DIR}) +set(GTSAM_LIBRARIES gtsam) + +# find GPMP2 +find_package(gpmp2 REQUIRED) +include_directories(${gpmp2_INCLUDE_DIR}) +set(gpmp2_LIBRARIES gpmp2) + +catkin_package() + +# Specify additional locations of header files +include_directories(${catkin_INCLUDE_DIRS}) + + +################################################ +## base ## +################################################ +include_directories(piper/base/) + +file(GLOB BASE_SRC "piper/base/*.cpp") +add_library(piperbase ${BASE_SRC}) + +target_link_libraries(piperbase + ${catkin_LIBRARIES} + ${GTSAM_LIBRARIES} + ${gpmp2_LIBRARIES} +) + +install(DIRECTORY piper/base/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" +) + +# options for which interfaces need to be built and installed +option(BUILD_ALL_INTERFACE "whether to build all interfaces" OFF) +option(BUILD_GPMP2_INTERFACE "whether to build interface for batch GPMP2" OFF) +option(BUILD_STEAP_INTERFACE "whether to build interface for STEAP" OFF) + + +################################################ +## all ## +################################################ +if(BUILD_ALL_INTERFACE) + set(BUILD_GPMP2_INTERFACE ON) + set(BUILD_STEAP_INTERFACE ON) +endif() + + +################################################ +## gpmp2_interface ## +################################################ +if(BUILD_GPMP2_INTERFACE) + include_directories(piper/gpmp2_interface/) + + file(GLOB GPMP2_INTERFACE_SRC "piper/gpmp2_interface/*.cpp") + add_executable(gpmp2_interface ${GPMP2_INTERFACE_SRC}) + + target_link_libraries(gpmp2_interface piperbase) + + install(TARGETS gpmp2_interface + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY piper/gpmp2_interface/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" + ) +endif() + + +################################################ +## steap_interface ## +################################################ +if(BUILD_STEAP_INTERFACE) + include_directories(piper/steap_interface/) + + file(GLOB STEAP_INTERFACE_SRC "piper/steap_interface/*.cpp") + add_executable(steap_interface ${STEAP_INTERFACE_SRC}) + + target_link_libraries(steap_interface piperbase) + + install(TARGETS steap_interface + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY piper/steap_interface/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" + ) +endif() + + +################################################ +## launch ## +################################################ +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..4a5892a --- /dev/null +++ b/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2017, Georgia Tech Research Corporation +Atlanta, Georgia 30332-0415 +All Rights Reserved + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +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. + +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. + +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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..5eb620a --- /dev/null +++ b/README.md @@ -0,0 +1,103 @@ +PIPER +=================================================== + +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: + +- [GPMP2](http://www.cc.gatech.edu/~bboots3/files/GPMP2.pdf) - Gaussian Process Motion Planner 2 +- [STEAP](http://www.cc.gatech.edu/~bboots3/files/STEAP.pdf) - Simultaneous Trajectory Estimation and Planning +- more coming soon ... + +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. + +--- +Table of Contents +--- +- [Prerequisites](#prerequisites) +- [Compilation and Installation](#compilation-and-installation) +- [Questions and Bug reporting](#questions-and-bug-reporting) +- [Citing](#citing) +- [License](#license) + +--- +Prerequisites +------ + +- Install [ROS](http://wiki.ros.org/indigo/Installation/Ubuntu). We have tested ROS indigo on Ubuntu 14.04. +- Install [GPMP2](https://github.com/gtrll/gpmp2) C++ library. + + +Compilation and Installation +------ + +- Initialize a catkin workspace (if you are using an existing catkin workspace this step is not needed) + + ```bash + mkdir -p ~/piper_ws/src + cd ~/piper_ws/src + catkin_init_workspace + ``` + + Before running setup the environment variables + + ```bash + source ~/piper_ws/devel/setup.bash + ``` + +- Clone this repository in ```~/piper_ws/src``` + + ```bash + git clone https://github.com/gtrll/piper.git + ``` + +- To compile only the ```piperbase``` library, in the catkin workspace directory do + + ```bash + catkin_make + ``` + +- Otherwise, to install some module, for example, _X_: first make sure to install dependencies for _X_, besides the prerequisites for piperbase, then do + + ```bash + catkin_make -build_flag_X + ``` + +- Similarly, to install multiple modules, for example, _X_ and _Y_: install all their dependencies and then use their appropriate flags together + + ```bash + catkin_make -build_flag_X -build_flag_Y + ``` + + See table below for currently supported modules, their dependencies and build flags + + | Module | Other Dependencies | Build Flag | + |:------:|:------------------:|:----------:| + |GPMP2|None|```-DBUILD_GPMP2_INTERFACE:OPTION=ON```| + |STEAP|None|```-DBUILD_STEAP_INTERFACE:OPTION=ON```| + |ALL|All from above|```-DBUILD_ALL_INTERFACE:OPTION=ON```| + + +Questions and Bug reporting +----- + +Please use Github issue tracker to report bugs. For other questions please contact [Mustafa Mukadam](mailto:mmukadam3@gatech.edu). + + +Citing +----- + +If you use PIPER in an academic context, please cite any module/algorithm specific publications you use, and cite the following: + +``` +@article{mukadam2017piper, + title={{PIPER}}, + author={Mukadam, Mustafa}, + journal={[Online] Available at \url{https://github.com/gtrll/piper}}, + year={2017} +} +``` + + +License +----- + +PIPER is released under the BSD license. See LICENSE file in this directory. diff --git a/config/jaco2.yaml b/config/jaco2.yaml new file mode 100644 index 0000000..27c84b9 --- /dev/null +++ b/config/jaco2.yaml @@ -0,0 +1,26 @@ +piper: + robot: + DOF: 6 + arm_base: {orientation: [0, 0, 0, 1], position: [0, 0, 0]} + DH: + alpha: [1.5708, 3.1416, 1.5708, 1.0472, 1.0472, 3.1416] + a: [0, 0.41, 0, 0, 0, 0] + d: [0.2755, 0, -0.0098, -0.2501, -0.0856, -0.2228] + theta: [0, -1.5708, 1.5708, 0, -3.1416, 1.5708] + theta_neg: [true, false, false, false, false, false] + spheres: + 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] + 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] + 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, + -0.05,-0.05,-0.06,-0.06,-0.035,0.015,0.025,0,-0.025,-0.015] + 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, + -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] + 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, + 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] + arm_joint_names: [right_shoulder_pan_joint, right_shoulder_lift_joint, right_elbow_joint, + right_wrist_1_joint, right_wrist_2_joint, right_wrist_3_joint] + sensor_arm_sigma: 0.0001 + trajectory_control_topic: /jaco2/arm_controller/trajectory + est_traj_pub_topic: /piper/est_traj + plan_traj_pub_topic: /piper/plan_traj + arm_state_topic: /joint_states diff --git a/config/pr2_rightarm.yaml b/config/pr2_rightarm.yaml new file mode 100644 index 0000000..c3347b0 --- /dev/null +++ b/config/pr2_rightarm.yaml @@ -0,0 +1,24 @@ +piper: + robot: + DOF: 7 + arm_base: {orientation: [0, 0, 0, 1], position: [-0.05, -0.188, 0.802175]} + DH: + alpha: [-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0] + a: [0.1, 0, 0, 0, 0, 0, 0] + d: [0, 0, 0.4, 0, 0.321, 0, 0] + theta: [0, 1.5708, 0, 0, 0, 0, 0] + spheres: + js: [0,2,2,2,2,4,4,4,4,4,4,4,6,6,6,6,6,6,6,6,6,6,6] + 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] + 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, + 0.009,0.0095,-0.036,-0.027,-0.009,-0.0095] + 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, + 0.205,0.11,0.155,0.18,0.205] + 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, + 0.03,0.02,0.04,0.035,0.03,0.02] + arm_joint_names: [r_shoulder_pan_joint, r_shoulder_lift_joint, r_upper_arm_roll_joint, + r_elbow_flex_joint, r_forearm_roll_joint, r_wrist_flex_joint, r_wrist_roll_joint] + trajectory_control_topic: /pr2/right_arm_controller/trajectory + est_traj_pub_topic: /piper/est_traj + plan_traj_pub_topic: /piper/plan_traj + arm_state_topic: /joint_states diff --git a/config/vector.yaml b/config/vector.yaml new file mode 100644 index 0000000..46478a5 --- /dev/null +++ b/config/vector.yaml @@ -0,0 +1,44 @@ +piper: + robot: + mobile_base: true + DOF: 9 + arm_base: {orientation: [0.5, 0.5, 0.5, 0.5], position: [0.2889, 0, 1.0864]} + DH: + alpha: [1.5708, 3.1416, 1.5708, 1.0472, 1.0472, 3.1416] + a: [0, 0.41, 0, 0, 0, 0] + d: [0.2755, 0, -0.0098, -0.2501, -0.0856, -0.2228] + theta: [0, -1.5708, 1.5708, 0, -3.1416, 1.5708] + theta_neg: [true, false, false, false, false, false] + spheres: + 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, + 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] + 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, + -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, + 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, + 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, + 0,0,0,0,0,0] + 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, + -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, + -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, + -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, + 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] + 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, + 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, + 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, + 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, + -0.042,-0.01,0.01,-0.039,-0.067,-0.042,-0.055,-0.08,-0.08,-0.08,-0.055] + 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, + 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, + 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, + 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, + 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] + arm_joint_names: [right_shoulder_pan_joint, right_shoulder_lift_joint, right_elbow_joint, + right_wrist_1_joint, right_wrist_2_joint, right_wrist_3_joint] + sensor_arm_sigma: 0.0001 + sensor_base_sigma: 0.0001 + trajectory_control_topic: /vector/full_body_controller/trajectory + est_traj_pub_topic: /piper/est_traj + plan_traj_pub_topic: /piper/plan_traj + arm_state_topic: /joint_states + base_state_topic: /base_state + \ No newline at end of file diff --git a/config/wam.yaml b/config/wam.yaml new file mode 100644 index 0000000..45c54f1 --- /dev/null +++ b/config/wam.yaml @@ -0,0 +1,21 @@ +piper: + robot: + DOF: 7 + arm_base: {orientation: [0, 0, 0, 1], position: [0, 0, 0]} + DH: + alpha: [-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0] + a: [0, 0, 0.045, -0.045, 0, 0, 0] + d: [0, 0, 0.55, 0, 0.3, 0, 0.06] + theta: [0, 0, 0, 0, 0, 0, 0] + spheres: + js: [0,1,1,1,1,2,3,3,3,5,6,6,6,6,6,6] + xs: [0,0,0,0,0,0,0,0,0,0,0.1,0.1,-0.1,0.15,0.15,-0.15] + ys: [0,0,0,0,0,0,0,0,0,0,-0.025,0.025,0,-0.025,0.025,0] + 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] + 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] + arm_joint_names: [Shoulder_Yaw, Shoulder_Pitch, Shoulder_Roll, + Elbow, Wrist_Yaw, Wrist_Pitch, Wrist_Roll] + trajectory_control_topic: /wam/arm_controller/trajectory + est_traj_pub_topic: /piper/est_traj + plan_traj_pub_topic: /piper/plan_traj + arm_state_topic: /joint_states diff --git a/doc/config.md b/doc/config.md new file mode 100644 index 0000000..12d05bc --- /dev/null +++ b/doc/config.md @@ -0,0 +1,26 @@ +Robot Configuration File +====================== +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. + +For any robot, following is a list of parameters that can be provided: + +- ```mobile_base```: boolean flag to be set to _True_ if the robot has a mobile base and is being used by any algorithm +- ```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) +- ```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 +- ```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 +- ```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 +- ```arm_joint_names```: an array of strings with the names of all the joints on the arm in order +- ```sensor_arm_sigma```: variance of sensor model for arm state measurement +- ```sensor_base_sigma```: variance of sensor model for base state measurement +- ```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 +- ```est_traj_pub_topic```: (optional) publishes estimated trajectory to this topic +- ```plan_traj_pub_topic```: (optional) publishes planned trajectory to this topic +- ```arm_state_topic```: current arm state can be read from this topic and is published by the robot API +- ```base_state_topic```: current base state can be read from this topic and is published by the robot API + +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. + +--- +[Back to Usage](usage.md) + +[Back to Documentation home](index.md) diff --git a/doc/development.md b/doc/development.md new file mode 100644 index 0000000..ac1ced8 --- /dev/null +++ b/doc/development.md @@ -0,0 +1,66 @@ +Development Guide +================== +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. + +To add your own algorithm say ```myAlgo``` to the PIPER package you need to create a module for it by following these steps: + +- Add any dependencies say ```myDepend``` and the installation set up to the [CMakeLists.txt](../CMakeLists.txt) file + - Create an option to install your module ```myAlgo_interface``` + + ```cmake + # options for which interfaces need to be built and installed + ... + option(BUILD_myAlgo_INTERFACE "whether to build interface for myAlgo" OFF) + ``` + + - Add option when building all interfaces + + ```cmake + ################################################ + ## all ## + ################################################ + if(BUILD_ALL_INTERFACE) + ... + set(BUILD_myAlgo_INTERFACE ON) + endif() + ``` + + - Add module installation settings + + ```cmake + ################################################ + ## myAlgo_interface ## + ################################################ + if(BUILD_myAlgo_INTERFACE) + find_package(myDepend REQUIRED) + include_directories(${myDepend_INCLUDE_DIR}) + set(myDepend_LIBRARIES myDepend) + + include_directories(piper/myAlgo_interface/) + + file(GLOB myAlgo_INTERFACE_SRC "piper/myAlgo_interface/*.cpp") + add_executable(myAlgo_interface ${myAlgo_INTERFACE_SRC}) + + target_link_libraries(myAlgo_interface ${myDepend_LIBRARIES} piperbase) + + install(TARGETS myAlgo_interface + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY piper/myAlgo_interface/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" + ) + endif() + ``` + +- Then, add all the source files to this directory: ```/piper/myAlgo_interface``` + +- Finally, to compile do the following in the catkin workspace where PIPER was installed + + ```bash + catkin_make -DBUILD_myAlgo_INTERFACE:OPTION=ON + ``` + +--- +[Back to Documentation home](index.md) diff --git a/doc/index.md b/doc/index.md new file mode 100644 index 0000000..7a96dd4 --- /dev/null +++ b/doc/index.md @@ -0,0 +1,10 @@ +Documentation +============= + +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. + +- [Usage](usage.md) + - [Config files for robot settings](config.md) + - [Problem files for algorithm and problem settings](problem.md) + - [Signed Distance Field files](sdf.md) +- [Development guide](development.md) diff --git a/doc/problem.md b/doc/problem.md new file mode 100644 index 0000000..b1d5df0 --- /dev/null +++ b/doc/problem.md @@ -0,0 +1,26 @@ +Problem File +====================== +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. + +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: + +- ```start_conf```: (optional) start configuration of the arm, read from ```arm_state_topic``` if not passed +- ```start_pose```: (optional) start pose of the base, read from ```base_state_topic``` if not passed +- ```goal_conf```: goal configuration of the arm +- ```goal_pose```: goal pose of the base +- ```sdf_file```: SDF file in the [```/sdf```](../sdf) directory, used for collision avoidance, see [how to create SDF files](sdf.md) +- ```total_time```: total time length of the trajectory in seconds +- ```total_step```: total number of time steps (support states) on the trajectory (includes start and goal) +- ```obs_check_inter```: number of states to be interpolated between any two support states, for collision cost calculation during optimization +- ```control_inter```: number of states to be interpolated between any two support states, to generate executable trajectory for a robot +- ```cost_sigma```: covariance for obstacle factors +- ```epsilon```: safety distance from obstacles in meters +- ```fix_pose_sigma```: small covariance to fix the position part of the state (mainly used for start and goal) +- ```fix_vel_sigma```: small covariance to fix the velocity part of the state (mainly used for start and goal) +- ```Qc```: covariance for GP factors +- ```opt_type```: optimization method to be used, ```LM``` works fine in most cases + +--- +[Back to Usage](usage.md) + +[Back to Documentation home](index.md) diff --git a/doc/sdf.md b/doc/sdf.md new file mode 100644 index 0000000..e7806c1 --- /dev/null +++ b/doc/sdf.md @@ -0,0 +1,16 @@ +Signed Distance Field File +====================== +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. + +For any of your environments you can create your own SDF file with any of the following options: + +- **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). +- **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. +- **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). + +--- +[Back to Problem file](problem.md) + +[Back to Usage](usage.md) + +[Back to Documentation home](index.md) diff --git a/doc/usage.md b/doc/usage.md new file mode 100644 index 0000000..388a2c5 --- /dev/null +++ b/doc/usage.md @@ -0,0 +1,14 @@ +Usage +============== +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 + + ```bash + roslaunch piper myAlgo_interface.launch robot:=myRobot problem:=myProblem + ``` + +- ```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). +- ```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. +- ```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. + +--- +[Back to Documentation home](index.md) diff --git a/launch/gpmp2_interface.launch b/launch/gpmp2_interface.launch new file mode 100644 index 0000000..bd59e75 --- /dev/null +++ b/launch/gpmp2_interface.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/steap_interface.launch b/launch/steap_interface.launch new file mode 100644 index 0000000..0137ef7 --- /dev/null +++ b/launch/steap_interface.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6c17a8e --- /dev/null +++ b/package.xml @@ -0,0 +1,37 @@ + + + piper + 0.1.0 + The PIPER package + + mmukadam + mmukadam + + BSD + + catkin + + roscpp + rospy + std_msgs + control_msgs + actionlib + roslib + sensor_msgs + geometry_msgs + trajectory_msgs + libgtsam + libgpmp2 + + roscpp + rospy + std_msgs + control_msgs + actionlib + roslib + sensor_msgs + geometry_msgs + trajectory_msgs + libgtsam + libgpmp2 + diff --git a/piper/base/misc.h b/piper/base/misc.h new file mode 100644 index 0000000..2fd31fd --- /dev/null +++ b/piper/base/misc.h @@ -0,0 +1,47 @@ +/** + * @file misc.h + * @brief miscellaneous functions + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#ifndef MISC_H_ +#define MISC_H_ + +#include +#include + +#include +#include + +#include + + +namespace piper { + +/** + * Convert std double vector type to gtsam Vector + * + * @param v std double vector + **/ +static gtsam::Vector getVector(const std::vector& v) +{ + gtsam::Vector send(v.size()); + for (size_t i=0; i + + +namespace piper { + +/* ************************************************************************** */ +Problem::Problem(ros::NodeHandle nh) +{ + // first load robot + robot = Robot(nh); + + // given the robot load the planning problem + ROS_INFO("Loading planning problem."); + + // start and goal + if (nh.hasParam("start_conf")) + { + nh.getParam("start_conf", sc_); + start_conf = getVector(sc_); + if (robot.isThetaNeg()) + robot.negateTheta(start_conf); + } + nh.getParam("goal_conf", gc_); + goal_conf = getVector(gc_); + if (robot.isThetaNeg()) + robot.negateTheta(goal_conf); + if (robot.isMobileBase()) + { + if (nh.hasParam("start_pose")) + { + nh.getParam("start_pose", sp_); + start_pose = gtsam::Pose2(sp_[0], sp_[1], sp_[2]); + } + nh.getParam("goal_pose", gp_); + goal_pose = gtsam::Pose2(gp_[0], gp_[1], gp_[2]); + pgoal = gpmp2::Pose2Vector(goal_pose, goal_conf); + } + + // signed distance field + nh.getParam("sdf_file", sdf_file_); + sdf_file_ = ros::package::getPath("piper") + "/" + sdf_file_; + std::string fext = sdf_file_.substr(sdf_file_.find_last_of(".") + 1); + if (fext == "vol") + gpmp2::readSDFvolfile(sdf_file_, sdf); + else + sdf.loadSDF(sdf_file_); + + // optimization settings + nh.getParam("total_time", total_time); + nh.getParam("total_step", total_step); + nh.getParam("obs_check_inter", obs_check_inter); + nh.getParam("control_inter", control_inter); + nh.getParam("cost_sigma", cost_sigma); + nh.getParam("epsilon", epsilon); + nh.getParam("opt_type", opt_type_); + nh.getParam("Qc", Qc_); + nh.getParam("fix_pose_sigma", fix_pose_sigma_); + nh.getParam("fix_vel_sigma", fix_vel_sigma_); + int DOF = robot.getDOF(); + opt_setting = gpmp2::TrajOptimizerSetting(DOF); + opt_setting.total_time = total_time; + opt_setting.total_step = total_step-1; + delta_t = total_time/(total_step-1); + opt_setting.obs_check_inter = obs_check_inter; + opt_setting.cost_sigma = cost_sigma; + opt_setting.epsilon = epsilon; + opt_setting.Qc_model = gtsam::noiseModel::Gaussian::Covariance(Qc_*gtsam::Matrix::Identity(DOF, DOF)); + opt_setting.conf_prior_model = gtsam::noiseModel::Isotropic::Sigma(DOF, fix_pose_sigma_); + opt_setting.vel_prior_model = gtsam::noiseModel::Isotropic::Sigma(DOF, fix_vel_sigma_); + if (opt_type_ == "LM") + opt_setting.opt_type = gpmp2::TrajOptimizerSetting::LM; + else if (opt_type_ == "Dogleg") + opt_setting.opt_type = gpmp2::TrajOptimizerSetting::Dogleg; + else if (opt_type_ == "GaussNewton") + opt_setting.opt_type = gpmp2::TrajOptimizerSetting::GaussNewton; + else + { + ROS_ERROR("Optimization type \'%s\' not known!", opt_type_.c_str()); + sigintHandler(0); + } +} + +} // piper namespace diff --git a/piper/base/problem.h b/piper/base/problem.h new file mode 100644 index 0000000..1ac058f --- /dev/null +++ b/piper/base/problem.h @@ -0,0 +1,64 @@ +/** + * @file problem.h + * @brief problem: load, create, etc + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#ifndef PROBLEM_H_ +#define PROBLEM_H_ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +namespace piper { + +class Problem +{ + public: + Robot robot; + gtsam::Vector start_conf, goal_conf; + gtsam::Pose2 start_pose, goal_pose; + gpmp2::Pose2Vector pstart, pgoal; + gpmp2::SignedDistanceField sdf; + double total_time, cost_sigma, epsilon, delta_t; + int total_step, obs_check_inter, control_inter; + gpmp2::TrajOptimizerSetting opt_setting; + + private: + std::vector sc_, gc_, sp_, gp_; + std::string sdf_file_; + double Qc_, fix_pose_sigma_, fix_vel_sigma_; + std::string opt_type_; + + public: + /// Default constructor + Problem() {} + + /** + * Loads problem from yaml file + * + * @param nh node handle for namespace + **/ + Problem(ros::NodeHandle nh); + + /// Default destructor + virtual ~Problem() {} +}; + +} // piper namespace + +#endif diff --git a/piper/base/robot.cpp b/piper/base/robot.cpp new file mode 100644 index 0000000..3a44ba4 --- /dev/null +++ b/piper/base/robot.cpp @@ -0,0 +1,82 @@ +/** + * @file robot.h + * @brief robot: load, create, etc + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#include + + +namespace piper { + +/* ************************************************************************** */ +Robot::Robot(ros::NodeHandle nh) +{ + // get parameters for desired robot + ROS_INFO("Loading robot parameters."); + nh.getParam("robot/DOF", DOF_); + if (nh.hasParam("robot/mobile_base")) + nh.getParam("robot/mobile_base", mobile_base_); + else + mobile_base_ = false; + if (nh.hasParam("robot/arm_base/orientation")) + nh.getParam("robot/arm_base/orientation", orientation_); // quaternion: [x, y, z, w] + else + orientation_ = (std::vector){0, 0, 0, 1}; + if (nh.hasParam("robot/arm_base/position")) + nh.getParam("robot/arm_base/position", position_); // [x, y, z] + else + position_ = (std::vector){0, 0, 0}; + nh.getParam("robot/DH/a", a_); + nh.getParam("robot/DH/alpha", alpha_); + nh.getParam("robot/DH/d", d_); + nh.getParam("robot/DH/theta", theta_); + if (nh.hasParam("robot/DH/theta_neg")) + nh.getParam("robot/DH/theta_neg", theta_neg_); + nh.getParam("robot/spheres/js", js_); + nh.getParam("robot/spheres/xs", xs_); + nh.getParam("robot/spheres/ys", ys_); + nh.getParam("robot/spheres/zs", zs_); + nh.getParam("robot/spheres/rs", rs_); + if (nh.hasParam("robot/sensor_arm_sigma")) + nh.getParam("robot/sensor_arm_sigma", sensor_arm_sigma); + else + sensor_arm_sigma = 0.0001; + if (nh.hasParam("robot/sensor_base_sigma")) + nh.getParam("robot/sensor_base_sigma", sensor_base_sigma); + else + sensor_base_sigma = 0.0001; + + // arm's base pose (relative to robot base if mobile_base_ is true) + arm_base_ = gtsam::Pose3(gtsam::Rot3::Quaternion(orientation_[3], orientation_[0], orientation_[1], + orientation_[2]), gtsam::Point3(getVector(position_))); + + // spheres to approximate robot body: js - link id, rs - radius, [xs, ys, zs] - center + for (size_t i=0; i + +#include + +#include +#include +#include + +#include + + +namespace piper { + +class Robot +{ + public: + gpmp2::ArmModel arm; + gpmp2::Pose2MobileArmModel marm; + double sensor_arm_sigma, sensor_base_sigma; + + private: + bool mobile_base_; + int DOF_, DOF_arm_; + std::vector a_, alpha_, d_, theta_; + std::vector theta_neg_; + std::vector orientation_, position_; + gtsam::Pose3 arm_base_; + std::vector js_, xs_, ys_, zs_, rs_; + gpmp2::BodySphereVector spheres_data_; + + public: + /// Default constructor + Robot() {} + + /** + * Constructor loads robot parameters from yaml file and constructs a robot + * + * @param nh node handle for namespace + **/ + Robot(ros::NodeHandle nh); + + /// Default destructor + virtual ~Robot() {} + + /// check to see if robot has a mobile base + inline bool isMobileBase() const { return mobile_base_; } + + /// get DOF for full body robot + inline int getDOF() const { return DOF_; } + + /// get DOF for just the arm + inline int getDOFarm() const { return DOF_arm_; } + + /// theta offset in DH params has a sign flip + inline bool isThetaNeg() const { return !theta_neg_.empty(); } + + /** + * Flip sign of theta in DH parameters + * + * @param conf change conf to account for theta bias + **/ + void negateTheta(gtsam::Vector& conf); +}; + +} // piper namespace + +#endif diff --git a/piper/base/traj.cpp b/piper/base/traj.cpp new file mode 100644 index 0000000..0ecdd2e --- /dev/null +++ b/piper/base/traj.cpp @@ -0,0 +1,210 @@ +/** + * @file traj.cpp + * @brief trajectory: action client, initialize, etc + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#include + + +namespace piper { + +/* ************************************************************************** */ +Traj::Traj(ros::NodeHandle nh) +{ + // ros trajectory joint names + nh.getParam("robot/arm_joint_names", arm_joint_names); + traj_.trajectory.joint_names = arm_joint_names; + + // to visualize estimated trajectory + if (nh.hasParam("robot/est_traj_pub_topic")) + { + nh.getParam("robot/est_traj_pub_topic", est_traj_pub_topic_); + est_traj_pub = nh.advertise(est_traj_pub_topic_, 1); + } + + // to visualize planned trajectory + if (nh.hasParam("robot/plan_traj_pub_topic")) + { + nh.getParam("robot/plan_traj_pub_topic", plan_traj_pub_topic_); + plan_traj_pub = nh.advertise(plan_traj_pub_topic_, 1); + } + + // trajectory action client + if (nh.hasParam("robot/trajectory_control_topic")) + { + nh.getParam("robot/trajectory_control_topic", trajectory_control_topic_); + traj_client_ = new Traj::TrajClient(trajectory_control_topic_, true); + if (!traj_client_->waitForServer(ros::Duration(5.0))) + { + ROS_INFO("Waiting for trajectory_control server..."); + if (!traj_client_->waitForServer(ros::Duration(5.0))) + { + ROS_ERROR("Cannot find trajectory_control server \'%s\'", trajectory_control_topic_.c_str()); + sigintHandler(0); + } + } + } + else + ROS_WARN("No trajectory control topic. Trajectory will not be executed."); +} + +/* ************************************************************************** */ +void Traj::initializeTrajectory(gtsam::Values& init_values, Problem& problem) +{ + ROS_INFO("Initializing trajectory."); + gtsam::Vector conf, avg_vel; + if (!problem.robot.isMobileBase()) + { + avg_vel = (problem.goal_conf - problem.start_conf)/problem.total_time; + for (size_t i=0; i(i)/static_cast(problem.total_step-1); + conf = (1.0 - ratio)*problem.start_conf + ratio*problem.goal_conf; + init_values.insert(gtsam::Symbol('x',i), conf); + init_values.insert(gtsam::Symbol('v',i), avg_vel); + } + } + else + { + gtsam::Pose2 pose; + avg_vel = (gtsam::Vector(problem.robot.getDOF()) << problem.goal_pose.x()-problem.start_pose.x(), + problem.goal_pose.y()-problem.start_pose.y(), problem.goal_pose.theta()-problem.start_pose.theta(), + problem.goal_conf - problem.start_conf).finished()/problem.total_time; + for (size_t i=0; i(i)/static_cast(problem.total_step-1); + pose = gtsam::Pose2((1.0 - ratio)*problem.start_pose.x() + ratio*problem.goal_pose.x(), + (1.0 - ratio)*problem.start_pose.y() + ratio*problem.goal_pose.y(), + (1.0 - ratio)*problem.start_pose.theta() + ratio*problem.goal_pose.theta()); + conf = (1.0 - ratio)*problem.start_conf + ratio*problem.goal_conf; + init_values.insert(gtsam::Symbol('x',i), gpmp2::Pose2Vector(pose, conf)); + init_values.insert(gtsam::Symbol('v',i), avg_vel); + } + } +} + +/* ************************************************************************** */ +void Traj::executeTrajectory(gtsam::Values& exec_values, Problem& problem, size_t exec_step) +{ + gtsam::Pose2 pose; + gtsam::Vector conf, vel; + int DOF = problem.robot.getDOF(); + int DOF_arm = problem.robot.getDOFarm(); + + // create ros trajectory + traj_.trajectory.points.resize(exec_step); + for (size_t i=0; i(gtsam::Symbol('x',i)); + vel = exec_values.at(gtsam::Symbol('v',i)); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); + conf = exec_values.at(gtsam::Symbol('x',i)).configuration(); + vel = exec_values.at(gtsam::Symbol('v',i)); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + traj_.trajectory.points[i].positions[0] = pose.x(); + traj_.trajectory.points[i].positions[1] = pose.y(); + traj_.trajectory.points[i].positions[2] = pose.theta(); + for (size_t j=0; jsendGoal(traj_); + traj_client_->waitForResult(); +} + +/* ************************************************************************** */ +void Traj::publishEstimatedTrajectory(gtsam::Values& values, Problem& problem, size_t step) +{ + gtsam::Vector conf; + gtsam::Pose2 pose; + trajectory_msgs::JointTrajectory est_traj; + est_traj.points.resize(step+1); + for (size_t i=0; i(gtsam::Symbol('x',i)); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); + conf = values.at(gtsam::Symbol('x',i)).configuration(); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + est_traj.points[i].positions[0] = pose.x(); + est_traj.points[i].positions[1] = pose.y(); + est_traj.points[i].positions[2] = pose.theta(); + for (size_t j=0; j(gtsam::Symbol('x',i)); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + for (size_t j=0; j(gtsam::Symbol('x',i)).pose(); + conf = values.at(gtsam::Symbol('x',i)).configuration(); + if (problem.robot.isThetaNeg()) + problem.robot.negateTheta(conf); + plan_traj.points[i-step].positions[0] = pose.x(); + plan_traj.points[i-step].positions[1] = pose.y(); + plan_traj.points[i-step].positions[2] = pose.theta(); + for (size_t j=0; j +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +namespace piper { + +class Traj +{ + public: + typedef actionlib::SimpleActionClient TrajClient; + ros::Publisher est_traj_pub, plan_traj_pub; + std::vector arm_joint_names; + + private: + std::string trajectory_control_topic_, est_traj_pub_topic_, plan_traj_pub_topic_; + TrajClient* traj_client_; + control_msgs::FollowJointTrajectoryGoal traj_; + + public: + /// Default constructor + Traj() {} + + /** + * setup traj client and load params + * + * @param nh node handle for namespace + **/ + Traj(ros::NodeHandle nh); + + /// Default destructor + virtual ~Traj() {} + + /** + * initialize trajectory for optimization + * + * @param init_values initialized traj save in to this variable + * @param problem all problem params and settings + **/ + void initializeTrajectory(gtsam::Values& init_values, Problem& problem); + + /** + * initialize trajectory for optimization + * + * @param exec_values optimized, interpolated, and collision checked traj to execute + * @param problem all problem params and settings + * @param exec_step number of points on the trajectory + **/ + void executeTrajectory(gtsam::Values& exec_values, Problem& problem, size_t exec_step); + + /** + * publish estimated trajectory + * + * @param values estimated part of traj + * @param problem all problem params and settings + * @param step estimated traj is from 0 to step + **/ + void publishEstimatedTrajectory(gtsam::Values& values, Problem& problem, size_t step); + + /** + * publish planned trajectory + * + * @param values planned part of traj + * @param problem all problem params and settings + * @param step planned traj is from step to total_step + **/ + void publishPlannedTrajectory(gtsam::Values& values, Problem& problem, size_t step); +}; + +} // piper namespace + +#endif diff --git a/piper/gpmp2_interface/gpmp2_interface.cpp b/piper/gpmp2_interface/gpmp2_interface.cpp new file mode 100644 index 0000000..bec92b1 --- /dev/null +++ b/piper/gpmp2_interface/gpmp2_interface.cpp @@ -0,0 +1,151 @@ +/** + * @file gpmp2_interface.cpp + * @brief ROS interface between GPMP2 and a real/sim robot + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#include + + +namespace piper { + +/* ************************************************************************** */ +GPMP2Interface::GPMP2Interface(ros::NodeHandle nh) +{ + // first load problem and setup trajectory client + problem_ = Problem(nh); + traj_ = Traj(nh); + + // robot state subscriber (used to initialize start state if not passed as param) + if (nh.hasParam("robot/arm_state_topic")) + { + nh.getParam("robot/arm_state_topic", arm_state_topic_); + arm_state_sub_ = nh.subscribe(arm_state_topic_, 1, &GPMP2Interface::armStateCallback, this); + arm_pos_ = gtsam::Vector::Zero(problem_.robot.getDOFarm()); + arm_pos_time_ = ros::Time::now(); + } + if (problem_.robot.isMobileBase() && nh.hasParam("robot/base_state_topic")) + { + nh.getParam("robot/base_state_topic", base_state_topic_); + base_state_sub_ = nh.subscribe(base_state_topic_, 1, &GPMP2Interface::baseStateCallback, this); + base_pos_ = gtsam::Pose2(); + base_pos_time_ = ros::Time::now(); + } + ros::Duration(1.0).sleep(); + + // get start from measurement if not passed as param + if (!nh.hasParam("start_conf")) + { + problem_.start_conf = arm_pos_; + if (problem_.robot.isThetaNeg()) + problem_.robot.negateTheta(problem_.start_conf); + } + if (problem_.robot.isMobileBase()) + { + if (!nh.hasParam("start_pose")) + problem_.start_pose = base_pos_; + problem_.pstart = gpmp2::Pose2Vector(problem_.start_pose, problem_.start_conf); + } + + // initialize trajectory + traj_.initializeTrajectory(init_values_, problem_); + + // solve with batch gpmp2 + ROS_INFO("Optimizing..."); + int DOF = problem_.robot.getDOF(); + if (!problem_.robot.isMobileBase()) + batch_values_ = gpmp2::BatchTrajOptimize3DArm(problem_.robot.arm, problem_.sdf, problem_.start_conf, + gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); + else + batch_values_ = gpmp2::BatchTrajOptimizePose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.pstart, + gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); + ROS_INFO("Batch GPMP2 optimization complete."); + + // publish trajectory for visualization or other use + if (traj_.plan_traj_pub) + traj_.publishPlannedTrajectory(batch_values_, problem_, 0); +} + +/* ************************************************************************** */ +void GPMP2Interface::execute() +{ + size_t exec_step; + double coll_cost; + + // interpolate batch solution to a desired resolution for control and check for collision + ROS_INFO("Checking for collision."); + if (!problem_.robot.isMobileBase()) + { + exec_values_ = gpmp2::interpolateArmTraj(batch_values_, problem_.opt_setting.Qc_model, problem_.delta_t, + problem_.control_inter, 0, problem_.total_step-1); + coll_cost = gpmp2::CollisionCost3DArm(problem_.robot.arm, problem_.sdf, exec_values_, problem_.opt_setting); + } + else + { + exec_values_ = gpmp2::interpolatePose2MobileArmTraj(batch_values_, problem_.opt_setting.Qc_model, + problem_.delta_t, problem_.control_inter, 0, problem_.total_step-1); + coll_cost = gpmp2::CollisionCostPose2MobileArm(problem_.robot.marm, problem_.sdf, exec_values_, problem_.opt_setting); + } + if (coll_cost != 0) + { + ROS_FATAL("Plan is not collision free! Collision cost = %.3f", coll_cost); + sigintHandler(0); + } + + // execute trajectory + ROS_INFO("Executing GPMP2 planned trajectory open-loop..."); + exec_step = problem_.total_step+problem_.control_inter*(problem_.total_step-1); + traj_.executeTrajectory(exec_values_, problem_, exec_step); +} + +/* ************************************************************************** */ +void GPMP2Interface::armStateCallback(const sensor_msgs::JointState::ConstPtr& msg) +{ + size_t index; + for (size_t i=0; iname.begin(), find(msg->name.begin(), msg->name.end(), + traj_.arm_joint_names[i])); + arm_pos_[i] = msg->position[index]; + } + arm_pos_time_ = ros::Time::now(); +} + +/* ************************************************************************** */ +void GPMP2Interface::baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg) +{ + base_pos_ = gtsam::Pose2(msg->position.x, msg->position.y, gtsam::Rot3::Quaternion(msg->orientation.w, + msg->orientation.x, msg->orientation.y, msg->orientation.z).yaw()); + base_pos_time_ = ros::Time::now(); +} + +} // piper namespace + + +/* ************************************************************************** */ +/* main callback */ +void mainCallback(const std_msgs::Bool::ConstPtr& msg) +{ + ros::NodeHandle nh("piper"); + piper::GPMP2Interface gpmp2(nh); + gpmp2.execute(); + ROS_INFO("Done."); + ros::shutdown(); +} + +/* ************************************************************************** */ +/* main function */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gpmp2_interface"); + signal(SIGINT, piper::sigintHandler); + ros::MultiThreadedSpinner spinner(0); + + ros::NodeHandle n; + ros::Publisher main_pub = n.advertise("/piper/run_main", 1); + ros::Subscriber main_sub = n.subscribe("/piper/run_main", 1, mainCallback); + main_pub.publish(std_msgs::Bool()); + + spinner.spin(); +} diff --git a/piper/gpmp2_interface/gpmp2_interface.h b/piper/gpmp2_interface/gpmp2_interface.h new file mode 100644 index 0000000..60b829c --- /dev/null +++ b/piper/gpmp2_interface/gpmp2_interface.h @@ -0,0 +1,79 @@ +/** + * @file gpmp2_interface.h + * @brief ROS interface between GPMP2 and a real/sim robot + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#ifndef GPMP2_INTERFACE_H_ +#define GPMP2_INTERFACE_H_ + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + + +namespace piper { + +class GPMP2Interface +{ + private: + Problem problem_; + Traj traj_; + gtsam::Values init_values_, batch_values_, exec_values_; + + std::string arm_state_topic_, base_state_topic_; + ros::Subscriber arm_state_sub_, base_state_sub_; + gtsam::Vector arm_pos_; + gtsam::Pose2 base_pos_; + ros::Time arm_pos_time_, base_pos_time_; + + public: + /// Default constructor + GPMP2Interface() {} + + /** + * batch gpmp2 + * + * @param nh node handle for namespace + **/ + GPMP2Interface(ros::NodeHandle nh); + + /// Default destructor + virtual ~GPMP2Interface() {} + + /** + * Open-loop execution of GPMP2 + **/ + void execute(); + + /** + * Call back to get current state of the arm + * + * @param msg message from arm state subscriber + **/ + void armStateCallback(const sensor_msgs::JointState::ConstPtr& msg); + + /** + * Call back to get current state of the base + * + * @param msg message from base state subscriber + **/ + void baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg); +}; + +} + +#endif diff --git a/piper/steap_interface/steap_interface.cpp b/piper/steap_interface/steap_interface.cpp new file mode 100644 index 0000000..cea3f5d --- /dev/null +++ b/piper/steap_interface/steap_interface.cpp @@ -0,0 +1,223 @@ +/** + * @file steap_interface.h + * @brief ROS interface between STEAP and a real/sim robot + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#include + + +namespace piper { + +/* ************************************************************************** */ +STEAPInterface::STEAPInterface(ros::NodeHandle nh) +{ + // first load problem and setup trajectory client + problem_ = Problem(nh); + traj_ = Traj(nh); + + // robot state subscriber + if (nh.hasParam("robot/arm_state_topic")) + { + nh.getParam("robot/arm_state_topic", arm_state_topic_); + arm_state_sub_ = nh.subscribe(arm_state_topic_, 1, &STEAPInterface::armStateCallback, this); + arm_pos_ = gtsam::Vector::Zero(problem_.robot.getDOFarm()); + arm_pos_time_ = ros::Time::now(); + } + if (problem_.robot.isMobileBase() && nh.hasParam("robot/base_state_topic")) + { + nh.getParam("robot/base_state_topic", base_state_topic_); + base_state_sub_ = nh.subscribe(base_state_topic_, 1, &STEAPInterface::baseStateCallback, this); + base_pos_ = gtsam::Pose2(); + base_pos_time_ = ros::Time::now(); + } + ros::Duration(1.0).sleep(); + + // get start from measurement if not passed as param + if (!nh.hasParam("start_conf")) + { + problem_.start_conf = arm_pos_; + if (problem_.robot.isThetaNeg()) + problem_.robot.negateTheta(problem_.start_conf); + } + if (problem_.robot.isMobileBase()) + { + if (!nh.hasParam("start_pose")) + problem_.start_pose = base_pos_; + problem_.pstart = gpmp2::Pose2Vector(problem_.start_pose, problem_.start_conf); + } + + // initialize trajectory + traj_.initializeTrajectory(init_values_, problem_); + + // solve for initial plan with batch gpmp2 + ROS_INFO("Optimizing..."); + int DOF = problem_.robot.getDOF(); + if (!problem_.robot.isMobileBase()) + batch_values_ = gpmp2::BatchTrajOptimize3DArm(problem_.robot.arm, problem_.sdf, problem_.start_conf, + gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); + else + batch_values_ = gpmp2::BatchTrajOptimizePose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.pstart, + gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF), init_values_, problem_.opt_setting); + ROS_INFO("Batch optimization complete."); + + // publish trajectory for visualization or other use + if (traj_.plan_traj_pub) + traj_.publishPlannedTrajectory(batch_values_, problem_, 0); + + // set up incremental inference + ROS_INFO("Initializing incremental inference..."); + if (!problem_.robot.isMobileBase()) + { + arm_inc_inf_ = gpmp2::ISAM2TrajOptimizer3DArm(problem_.robot.arm, problem_.sdf, problem_.opt_setting); + arm_inc_inf_.initFactorGraph(problem_.start_conf, gtsam::Vector::Zero(DOF), problem_.goal_conf, gtsam::Vector::Zero(DOF)); + arm_inc_inf_.initValues(batch_values_); + arm_inc_inf_.update(); + inc_inf_values_ = arm_inc_inf_.values(); + } + else + { + marm_inc_inf_ = gpmp2::ISAM2TrajOptimizerPose2MobileArm(problem_.robot.marm, problem_.sdf, problem_.opt_setting); + marm_inc_inf_.initFactorGraph(problem_.pstart, gtsam::Vector::Zero(DOF), problem_.pgoal, gtsam::Vector::Zero(DOF)); + marm_inc_inf_.initValues(batch_values_); + marm_inc_inf_.update(); + inc_inf_values_ = marm_inc_inf_.values(); + } + ROS_INFO("Online incremental inference ready."); +} + +/* ************************************************************************** */ +void STEAPInterface::execute() +{ + size_t exec_step; + double coll_cost; + gtsam::Matrix sensor_model; + int DOF_arm = problem_.robot.getDOFarm(); + int DOF = problem_.robot.getDOF(); + gtsam::Vector conf; + gtsam::Pose2 pose; + + // sensor model for measurements + if (!problem_.robot.isMobileBase()) + sensor_model = problem_.robot.sensor_arm_sigma*gtsam::Matrix::Identity(DOF_arm, DOF_arm); + else + sensor_model = (gtsam::Matrix(DOF, DOF) << + problem_.robot.sensor_base_sigma*gtsam::Matrix::Identity(3, 3), gtsam::Matrix::Zero(3, DOF_arm), + gtsam::Matrix::Zero(DOF_arm, 3), problem_.robot.sensor_arm_sigma*gtsam::Matrix::Identity(DOF_arm, DOF_arm)).finished(); + + // solve and execute STEAP problem + ROS_INFO("Executing STEAP online..."); + for (size_t step=0; step("/piper/run_main", 1); + ros::Subscriber main_sub = n.subscribe("/piper/run_main", 1, mainCallback); + main_pub.publish(std_msgs::Bool()); + + spinner.spin(); +} diff --git a/piper/steap_interface/steap_interface.h b/piper/steap_interface/steap_interface.h new file mode 100644 index 0000000..c61658a --- /dev/null +++ b/piper/steap_interface/steap_interface.h @@ -0,0 +1,82 @@ +/** + * @file steap_interface.h + * @brief ROS interface between STEAP and a real/sim robot + * @author Mustafa Mukadam + * @date Dec 13, 2016 + **/ + +#ifndef STEAP_INTERFACE_H_ +#define STEAP_INTERFACE_H_ + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + + +namespace piper { + +class STEAPInterface +{ + private: + Problem problem_; + Traj traj_; + gtsam::Values init_values_, batch_values_, inc_inf_values_, exec_values_; + gpmp2::ISAM2TrajOptimizer3DArm arm_inc_inf_; + gpmp2::ISAM2TrajOptimizerPose2MobileArm marm_inc_inf_; + + std::string arm_state_topic_, base_state_topic_; + ros::Subscriber arm_state_sub_, base_state_sub_; + gtsam::Vector arm_pos_; + gtsam::Pose2 base_pos_; + ros::Time arm_pos_time_, base_pos_time_; + + public: + /// Default constructor + STEAPInterface() {} + + /** + * STEAP: simultaneous trajectory estimation and planning + * + * @param nh node handle for namespace + **/ + STEAPInterface(ros::NodeHandle nh); + + /// Default destructor + virtual ~STEAPInterface() {} + + /** + * closed loop online execution + **/ + void execute(); + + /** + * Call back to get current state of the arm + * + * @param msg message from arm state subscriber + **/ + void armStateCallback(const sensor_msgs::JointState::ConstPtr& msg); + + /** + * Call back to get current state of the base + * + * @param msg message from base state subscriber + **/ + void baseStateCallback(const geometry_msgs::Pose::ConstPtr& msg); +}; + +} + +#endif diff --git a/problem/example_vector.yaml b/problem/example_vector.yaml new file mode 100644 index 0000000..e04ddb4 --- /dev/null +++ b/problem/example_vector.yaml @@ -0,0 +1,16 @@ +piper: + start_conf: [-1.92, 1.5, 0.53, -2.5, -2.91, 0.72] + start_pose: [0, 0, 0] + goal_conf: [4.7124, 3.1416, 3.1416, 0.0, 0.0, 0.0] + goal_pose: [2, -1, -1.5708] + sdf_file: sdf/empty_sdf.bin + total_time: 10.0 + total_step: 10 + obs_check_inter: 5 + control_inter: 5 + cost_sigma: 0.005 + epsilon: 0.1 + fix_pose_sigma: 0.0001 + fix_vel_sigma: 0.0001 + Qc: 1.0 + opt_type: LM diff --git a/sdf/empty_sdf.bin b/sdf/empty_sdf.bin new file mode 100644 index 0000000..55606df Binary files /dev/null and b/sdf/empty_sdf.bin differ