├── .ci ├── README.md ├── check-builds.sh ├── check-format.sh ├── check-tidy.sh ├── clean-docker.sh ├── documentation.sh ├── run-tests.sh └── tidy-functions.sh ├── .clang-format ├── .clang-tidy ├── .docker ├── README.md ├── kinetic │ ├── Dockerfile │ └── build-kinetic.sh ├── melodic-source │ ├── Dockerfile │ └── build-melodic-source.sh ├── melodic │ ├── Dockerfile │ └── build-melodic.sh ├── noetic │ ├── Dockerfile │ └── build-noetic.sh └── tesseract │ ├── Dockerfile │ └── build-tesseract.sh ├── .docs ├── CMakeLists.txt ├── Doxyfile.in ├── README.md ├── html │ ├── footer.html │ ├── header.html │ └── stylesheet.css ├── markdown │ ├── benchmarking.md │ ├── design.md │ ├── home.md │ ├── installation.md │ ├── rviz.md │ ├── scripts.md │ └── style.md └── tag │ ├── cppreference.tag │ └── moveit_core.tag ├── .gitattributes ├── .github ├── ISSUE_TEMPLATE │ ├── bug-report.md │ └── feature-request.md └── workflows │ ├── build-kinetic.yml │ ├── build-melodic.yml │ ├── build-noetic.yml │ ├── documentation.yml │ └── linting.yml ├── .gitignore ├── .style.yapf ├── CMakeModules ├── CompileOptions.cmake ├── FindEigen3.cmake ├── FindGFlags.cmake ├── FindTinyXML2.cmake └── HelperFunctions.cmake ├── LICENSE ├── README.md ├── format.sh ├── iwyu.sh ├── robowflex ├── CMakeLists.txt └── package.xml ├── robowflex_dart ├── CMakeLists.txt ├── CMakeModules ├── README.md ├── include │ ├── robowflex_dart.h │ └── robowflex_dart │ │ ├── acm.h │ │ ├── gui.h │ │ ├── io.h │ │ ├── joints.h │ │ ├── planner.h │ │ ├── planning.h │ │ ├── robot.h │ │ ├── space.h │ │ ├── structure.h │ │ ├── tsr.h │ │ └── world.h ├── package.xml ├── scripts │ ├── fetch_bimanual.cpp │ ├── fetch_ik.cpp │ ├── fetch_lift.cpp │ ├── fetch_pick.cpp │ ├── fetch_plan.cpp │ ├── fetch_robowflex_plan.cpp │ ├── fetch_robowflex_planner.cpp │ ├── fetch_se2.cpp │ ├── gui_test.cpp │ └── tsr_helper.cpp └── src │ ├── acm.cpp │ ├── gui.cpp │ ├── io.cpp │ ├── joints │ ├── joint.cpp │ ├── rnjoint.cpp │ ├── so2joint.cpp │ └── so3joint.cpp │ ├── planner.cpp │ ├── planning.cpp │ ├── robot.cpp │ ├── space.cpp │ ├── structure.cpp │ ├── tsr.cpp │ └── world.cpp ├── robowflex_library ├── CMakeLists.txt ├── CMakeModules ├── include │ └── robowflex_library │ │ ├── adapter.h │ │ ├── benchmarking.h │ │ ├── builder.h │ │ ├── class_forward.h │ │ ├── constants.h │ │ ├── detail │ │ ├── cob4.h │ │ ├── fetch.h │ │ ├── r2.h │ │ └── ur5.h │ │ ├── geometry.h │ │ ├── id.h │ │ ├── io.h │ │ ├── io │ │ ├── bag.h │ │ ├── broadcaster.h │ │ ├── colormap.h │ │ ├── gnuplot.h │ │ ├── handler.h │ │ ├── hdf5.h │ │ ├── plugin.h │ │ ├── visualization.h │ │ └── yaml.h │ │ ├── log.h │ │ ├── macros.h │ │ ├── openrave.h │ │ ├── planning.h │ │ ├── pool.h │ │ ├── random.h │ │ ├── robot.h │ │ ├── robowflex.h │ │ ├── scene.h │ │ ├── tf.h │ │ ├── trajectory.h │ │ ├── util.h │ │ └── yaml.h ├── package.xml ├── scripts │ ├── baxter_multi_target.cpp │ ├── cob4_multi_target.cpp │ ├── cob4_test.cpp │ ├── cob4_visualization.cpp │ ├── fetch_benchmark.cpp │ ├── fetch_cartesian.cpp │ ├── fetch_chomp.cpp │ ├── fetch_scenes_benchmark.cpp │ ├── fetch_scenes_visualize.cpp │ ├── fetch_test.cpp │ ├── fetch_visualization.cpp │ ├── plugin_io.cpp │ ├── resources_visualization.cpp │ ├── shadowhand_ik.cpp │ ├── ur5_benchmark.cpp │ ├── ur5_cartesian.cpp │ ├── ur5_cylinder.cpp │ ├── ur5_ik.cpp │ ├── ur5_io.cpp │ ├── ur5_pool.cpp │ ├── ur5_test.cpp │ ├── ur5_visualization.cpp │ ├── wam7_benchmark.cpp │ └── wam7_test.cpp ├── src │ ├── benchmarking.cpp │ ├── builder.cpp │ ├── detail │ │ ├── cob4.cpp │ │ ├── fetch.cpp │ │ └── ur5.cpp │ ├── geometry.cpp │ ├── id.cpp │ ├── io.cpp │ ├── io │ │ ├── broadcaster.cpp │ │ ├── colormap.cpp │ │ ├── gnuplot.cpp │ │ ├── hdf5.cpp │ │ └── visualization.cpp │ ├── log.cpp │ ├── openrave.cpp │ ├── planning.cpp │ ├── pool.cpp │ ├── random.cpp │ ├── robot.cpp │ ├── scene.cpp │ ├── tf.cpp │ ├── trajectory.cpp │ ├── util.cpp │ └── yaml.cpp ├── test │ ├── robot_scene.cpp │ └── yaml.cpp └── yaml │ ├── fetch_box │ ├── request0001.yaml │ ├── request0002.yaml │ ├── request0003.yaml │ ├── request0004.yaml │ ├── request0005.yaml │ ├── request0006.yaml │ ├── request0007.yaml │ ├── request0008.yaml │ ├── request0009.yaml │ ├── request0010.yaml │ ├── scene0001.yaml │ ├── scene0002.yaml │ ├── scene0003.yaml │ ├── scene0004.yaml │ ├── scene0005.yaml │ ├── scene0006.yaml │ ├── scene0007.yaml │ ├── scene0008.yaml │ ├── scene0009.yaml │ └── scene0010.yaml │ ├── fetch_scenes │ ├── request0001.yaml │ ├── request0002.yaml │ ├── request0003.yaml │ ├── request0004.yaml │ ├── request0005.yaml │ ├── request0006.yaml │ ├── request0007.yaml │ ├── request0008.yaml │ ├── request0009.yaml │ ├── request0010.yaml │ ├── scene_vicon0001.yaml │ ├── scene_vicon0002.yaml │ ├── scene_vicon0003.yaml │ ├── scene_vicon0004.yaml │ ├── scene_vicon0005.yaml │ ├── scene_vicon0006.yaml │ ├── scene_vicon0007.yaml │ ├── scene_vicon0008.yaml │ ├── scene_vicon0009.yaml │ └── scene_vicon0010.yaml │ ├── test.yml │ ├── test_cob4.yml │ ├── test_fetch.yml │ └── test_fetch_wall.yml ├── robowflex_movegroup ├── CMakeLists.txt ├── CMakeModules ├── include │ └── robowflex_movegroup │ │ └── services.h ├── package.xml ├── scripts │ └── tapedeck.cpp └── src │ └── services.cpp ├── robowflex_ompl ├── CMakeLists.txt ├── CMakeModules ├── include │ └── robowflex_ompl │ │ ├── ompl_interface.h │ │ └── ompl_trajectory.h ├── package.xml ├── scripts │ ├── fetch_ompl_benchmark.cpp │ ├── fetch_ompl_interface.cpp │ ├── fetch_ompl_scenes_benchmark.cpp │ ├── fetch_profile.cpp │ └── ur5_ompl_interface.cpp └── src │ ├── ompl_interface.cpp │ └── ompl_trajectory.cpp ├── robowflex_tesseract ├── CMakeLists.txt ├── CMakeModules ├── README.md ├── include │ └── robowflex_tesseract │ │ ├── conversions.h │ │ └── trajopt_planner.h ├── package.xml ├── scenes │ └── table │ │ ├── request.yaml │ │ └── scene.yaml ├── scripts │ ├── fetch_tabletop_goalpose.cpp │ ├── fetch_tabletop_goalstate.cpp │ ├── fetch_tabletop_inits.cpp │ ├── fetch_tabletop_planning_time.cpp │ ├── fetch_trajopt.cpp │ └── ur5_custom_planning.cpp └── src │ ├── conversions.cpp │ └── trajopt_planner.cpp ├── robowflex_visualization ├── CMakeLists.txt ├── README.md ├── old │ ├── README.md │ ├── blender.py │ ├── blender_animate_robot.py │ ├── blender_convex_hulls.py │ ├── blender_load_scene.py │ ├── blender_render_scene.py │ ├── blender_utils.py │ └── utils.py ├── package.xml ├── robowflex.blend ├── robowflex_visualization │ ├── __init__.py │ ├── primitives.py │ ├── robot.py │ ├── scene.py │ └── utils.py ├── scripts │ ├── blender.py │ └── robowflex.py ├── setup.py └── yaml │ ├── fetch_path.yml │ ├── fetch_pick.yml │ └── fetch_scene.yml └── tidy.sh /.ci/README.md: -------------------------------------------------------------------------------- 1 | # Robowflex CI Scripts 2 | 3 | Robowflex CI is done through [Github Actions](https://github.com/KavrakiLab/robowflex/actions). 4 | CI does the following: 5 | - Checks formatting of code base with `clang-format` (see `check-format.sh` and `.github/workflows/linting.yml`). 6 | - Builds and deploys documentation to Github Pages (see `documentation.sh` and `.github/workflows/documentation.yml`) 7 | - Builds code to check compilation (see `.github/workflows/build.yml`). 8 | -------------------------------------------------------------------------------- /.ci/check-builds.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | cd "${0%/*}" 5 | pushd ../.docker 6 | ./kinetic/build-kinetic.sh 7 | ./melodic/build-melodic.sh 8 | ./melodic-source/build-melodic-source.sh 9 | ./noetic/build-noetic.sh 10 | ./tesseract/build-tesseract.sh 11 | popd 12 | -------------------------------------------------------------------------------- /.ci/check-format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | find .. -regex '.*\.\(cpp\|h\)' -not -path "../.docs/*" -exec clang-format -style=file -output-replacements-xml {} \; | grep " /dev/null &" 8 | docker exec ${container}_test /bin/bash -c "source ros_entrypoint.sh; cd /ws/; catkin run_tests robowflex_library --no-deps --limit-status-rate 0.001 --no-notify; catkin_test_results" 9 | local result=$? 10 | docker stop ${container}_test 11 | docker rm ${container}_test 12 | echo "$result" 13 | } 14 | -------------------------------------------------------------------------------- /.ci/tidy-functions.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Note: this script requires the local ROS environment to be sourced. 4 | function generate-compile-commands() 5 | { 6 | mkdir .build-tmp 7 | pushd .build-tmp 8 | cmake ../$1/. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON 9 | commands=`realpath compile_commands.json` 10 | popd 11 | } 12 | 13 | function fix-tidy() 14 | { 15 | run-clang-tidy -fix -header-filter=".*" -quiet -p .build-tmp 16 | rm -rf .build-tmp 17 | } 18 | 19 | function check-tidy() 20 | { 21 | run-clang-tidy -header-filter=".*" -quiet -p .build-tmp 22 | rm -rf .build-tmp 23 | } 24 | 25 | # Use include-what-you-use to find missing / unnecessary includes 26 | function get-iwyu() 27 | { 28 | iwyu_tool.py -p .build-tmp | tee -a iwyu.out 29 | rm -rf .build-tmp 30 | } 31 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -4 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AllowShortBlocksOnASingleLine: false 13 | AlwaysBreakTemplateDeclarations: true 14 | AlwaysBreakBeforeMultilineStrings: false 15 | BreakBeforeBinaryOperators: false 16 | BreakBeforeTernaryOperators: false 17 | BreakConstructorInitializersBeforeComma: true 18 | BinPackParameters: true 19 | ColumnLimit: 110 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | PointerAlignment: Right 22 | DerivePointerBinding: false 23 | ExperimentalAutoDetectBinPacking: false 24 | IndentCaseLabels: true 25 | MaxEmptyLinesToKeep: 1 26 | NamespaceIndentation: All 27 | ObjCSpaceBeforeProtocolList: true 28 | PenaltyBreakBeforeFirstCallParameter: 19 29 | PenaltyBreakComment: 60 30 | PenaltyBreakString: 1 31 | PenaltyBreakFirstLessLess: 1000 32 | PenaltyExcessCharacter: 1000 33 | PenaltyReturnTypeOnItsOwnLine: 90 34 | PointerBindsToType: false 35 | SpacesBeforeTrailingComments: 2 36 | Cpp11BracedListStyle: true 37 | Language: Cpp 38 | Standard: Cpp11 39 | IndentWidth: 4 40 | TabWidth: 4 41 | UseTab: Never 42 | BreakBeforeBraces: Allman 43 | IndentFunctionDeclarationAfterType: false 44 | SpacesInParentheses: false 45 | SpacesInAngles: false 46 | SpaceInEmptyParentheses: false 47 | SpacesInCStyleCastParentheses: false 48 | SpaceAfterControlStatementKeyword: true 49 | SpaceBeforeAssignmentOperators: true 50 | ContinuationIndentWidth: 4 51 | SortIncludes: false 52 | ... 53 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: '-*, 3 | performance-*, 4 | boost-use-to-string, 5 | llvm-include-order, 6 | llvm-namespace-comment, 7 | misc-unused-parameters, 8 | modernize-avoid-bind, 9 | modernize-make-shared, 10 | modernize-loop-convert, 11 | modernize-redundant-void-arg, 12 | modernize-use-bool-literals, 13 | modernize-use-default, 14 | modernize-use-emplace, 15 | modernize-use-nullptr, 16 | modernize-use-override, 17 | modernize-use-using, 18 | modernize-use-auto, 19 | readability-avoid-const-params-in-decls, 20 | readability-const-return-type, 21 | readability-container-size-empty, 22 | readability-else-after-return, 23 | readability-identifier-naming, 24 | readability-make-member-function-const 25 | readability-named-parameter, 26 | readability-qualified-auto, 27 | readability-redundant-control-flow, 28 | readability-redundant-smartptr-get, 29 | readability-redundant-string-cstr, 30 | readability-redundant-string-init, 31 | readability-simplify-boolean-expr, 32 | readability-use-anyofallof, 33 | ' 34 | HeaderFilterRegex: '' 35 | AnalyzeTemporaryDtors: false 36 | CheckOptions: 37 | - key: llvm-namespace-comment.ShortNamespaceLines 38 | value: '10' 39 | - key: llvm-namespace-comment.SpacesBeforeComments 40 | value: '2' 41 | - key: readability-braces-around-statements.ShortStatementLines 42 | value: '2' 43 | # type names 44 | - key: readability-identifier-naming.ClassCase 45 | value: CamelCase 46 | - key: readability-identifier-naming.EnumCase 47 | value: CamelCase 48 | - key: readability-identifier-naming.UnionCase 49 | value: CamelCase 50 | # method names 51 | - key: readability-identifier-naming.MethodCase 52 | value: camelBack 53 | # variable names 54 | - key: readability-identifier-naming.VariableCase 55 | value: lower_case 56 | - key: readability-identifier-naming.ClassMemberSuffix 57 | value: '_' 58 | # const static or global variables are UPPER_CASE 59 | - key: readability-identifier-naming.EnumConstantCase 60 | value: UPPER_CASE 61 | - key: readability-identifier-naming.StaticConstantCase 62 | value: UPPER_CASE 63 | - key: readability-identifier-naming.ClassConstantCase 64 | value: UPPER_CASE 65 | - key: readability-identifier-naming.GlobalVariableCase 66 | value: UPPER_CASE 67 | ... 68 | -------------------------------------------------------------------------------- /.docker/README.md: -------------------------------------------------------------------------------- 1 | # Robowflex Docker Containers 2 | 3 | Here are docker containers for compiling Robowflex, for use locally or within CI. 4 | Each container comes with a `build-*.sh` script that runs the `docker build` command. 5 | The current list of containers are: 6 | 7 | ## robowflex/kinetic 8 | Builds the Robowflex code on ROS Kinetic, Ubuntu 16.04. 9 | Builds: 10 | - `robowflex_library` 11 | - `robowflex_ompl` 12 | - `robowflex_movegroup` 13 | 14 | ## robowflex/melodic 15 | Builds the Robowflex code on ROS Melodic, Ubuntu 18.04. 16 | Also builds the DART module, which is built with OMPL from source (1.5.0). 17 | Builds: 18 | - `robowflex_library` 19 | - `robowflex_ompl` 20 | - `robowflex_movegroup` 21 | - `robowflex_dart` 22 | 23 | ## robowflex/noetic 24 | Builds the Robowflex code on ROS Noetic, Ubuntu 20.04. 25 | Also builds the DART module, which is built with OMPL from source (1.5.0). 26 | Builds: 27 | - `robowflex_library` 28 | - `robowflex_ompl` 29 | - `robowflex_movegroup` 30 | - `robowflex_dart` 31 | 32 | ## robowflex/melodic-source 33 | Builds the Robowflex code on ROS Melodic, Ubuntu 18.04. 34 | Also builds OMPL (1.5.0) and MoveIt (`master` branch) from source. 35 | Builds: 36 | - `robowflex_library` 37 | - `robowflex_ompl` 38 | - `robowflex_movegroup` 39 | 40 | ## robowflex/tesseract 41 | Builds the Robowflex code on ROS Melodic, Ubuntu 18.04. 42 | In addition, builds the Tesseract module. 43 | Builds: 44 | - `robowflex_library` 45 | - `robowflex_tesseract` 46 | -------------------------------------------------------------------------------- /.docker/kinetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-ros-base 2 | MAINTAINER Zachary Kingston zak@rice.edu 3 | 4 | # Download Dependencies 5 | RUN apt-get update && \ 6 | apt-get install -y \ 7 | build-essential pkg-config python-catkin-tools cmake git \ 8 | libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ 9 | ros-${ROS_DISTRO}-moveit 10 | 11 | # Setup Catkin Workspace 12 | RUN mkdir -p ws/src 13 | WORKDIR /ws/src 14 | RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources 15 | WORKDIR /ws 16 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 17 | catkin build --limit-status-rate 0.001 --no-notify 18 | 19 | # Build Robowflex 20 | WORKDIR /ws/src 21 | ADD ./CMakeModules CMakeModules 22 | 23 | # Main Library 24 | ADD ./robowflex_library robowflex_library 25 | RUN catkin build robowflex_library --start-with robowflex_library --limit-status-rate 0.001 --no-notify 26 | 27 | # OMPL Module 28 | ADD ./robowflex_ompl robowflex_ompl 29 | RUN catkin build robowflex_ompl --start-with robowflex_ompl --limit-status-rate 0.001 --no-notify 30 | 31 | # Movegroup Module 32 | ADD ./robowflex_movegroup robowflex_movegroup 33 | RUN catkin build robowflex_movegroup --start-with robowflex_movegroup --limit-status-rate 0.001 --no-notify 34 | WORKDIR / -------------------------------------------------------------------------------- /.docker/kinetic/build-kinetic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | docker build -t robowflex/kinetic -f Dockerfile `git rev-parse --show-toplevel` 5 | -------------------------------------------------------------------------------- /.docker/melodic-source/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-ros-base 2 | MAINTAINER Zachary Kingston zak@rice.edu 3 | 4 | # Set Arguments 5 | ENV CAKTIN_BUILD_ARGS="--limit-status-rate 0.001 --no-notify" 6 | ENV MOVEIT_BRANCH="master" 7 | 8 | # Download Dependencies 9 | RUN apt-get update && \ 10 | apt-get install -y \ 11 | build-essential wget pkg-config python-catkin-tools python-wstool cmake git \ 12 | libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev 13 | 14 | # Setup Catkin Workspace 15 | RUN mkdir -p ws/src 16 | WORKDIR /ws/src 17 | RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources 18 | WORKDIR /ws 19 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 20 | catkin build ${CATKIN_BUILD_ARGS} 21 | 22 | # Get & Install OMPL 23 | WORKDIR /ws/src 24 | RUN wget -O - https://github.com/ompl/ompl/archive/1.5.0.tar.gz | tar zxf - 25 | RUN catkin build ompl --start-with ompl $CAKTIN_BUILD_ARGS 26 | 27 | # Get & Install MoveIt from Source 28 | WORKDIR /ws 29 | RUN wstool init src && \ 30 | wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/${MOVEIT_BRANCH}/moveit.rosinstall && \ 31 | wstool update -t src && \ 32 | rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} 33 | RUN catkin build $CAKTIN_BUILD_ARGS 34 | 35 | # Build Robowflex 36 | WORKDIR /ws/src 37 | ADD ./CMakeModules CMakeModules 38 | 39 | # Main Library 40 | ADD ./robowflex_library robowflex_library 41 | RUN catkin build robowflex_library --start-with robowflex_library $CAKTIN_BUILD_ARGS 42 | 43 | # OMPL Module 44 | ADD ./robowflex_ompl robowflex_ompl 45 | RUN catkin build robowflex_ompl --start-with robowflex_ompl $CAKTIN_BUILD_ARGS 46 | 47 | # Movegroup Module 48 | ADD ./robowflex_movegroup robowflex_movegroup 49 | RUN catkin build robowflex_movegroup --start-with robowflex_movegroup $CAKTIN_BUILD_ARGS 50 | WORKDIR / -------------------------------------------------------------------------------- /.docker/melodic-source/build-melodic-source.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | docker build -t robowflex/melodic-source -f Dockerfile `git rev-parse --show-toplevel` 5 | -------------------------------------------------------------------------------- /.docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-ros-base 2 | MAINTAINER Zachary Kingston zak@rice.edu 3 | 4 | # Download Dependencies 5 | RUN apt-get update && \ 6 | apt-get install -y software-properties-common && \ 7 | apt-add-repository ppa:dartsim/ppa && \ 8 | apt-get update && \ 9 | apt-get install -y \ 10 | build-essential wget pkg-config python-catkin-tools cmake git \ 11 | libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ 12 | ros-${ROS_DISTRO}-moveit \ 13 | libdart6-all-dev 14 | 15 | # Setup Catkin Workspace 16 | RUN mkdir -p ws/src 17 | WORKDIR /ws/src 18 | RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources 19 | WORKDIR /ws 20 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 21 | catkin build --limit-status-rate 0.001 --no-notify 22 | 23 | # Build Robowflex 24 | WORKDIR /ws/src 25 | ADD ./CMakeModules CMakeModules 26 | 27 | # Main Library 28 | ADD ./robowflex_library robowflex_library 29 | RUN catkin build robowflex_library --start-with robowflex_library --limit-status-rate 0.001 --no-notify 30 | 31 | # OMPL Module 32 | ADD ./robowflex_ompl robowflex_ompl 33 | RUN catkin build robowflex_ompl --start-with robowflex_ompl --limit-status-rate 0.001 --no-notify 34 | 35 | # Movegroup Module 36 | ADD ./robowflex_movegroup robowflex_movegroup 37 | RUN catkin build robowflex_movegroup --start-with robowflex_movegroup --limit-status-rate 0.001 --no-notify 38 | 39 | # DART Module 40 | ADD ./robowflex_dart robowflex_dart 41 | RUN catkin build robowflex_dart --start-with robowflex_dart --limit-status-rate 0.001 --no-notify 42 | WORKDIR / -------------------------------------------------------------------------------- /.docker/melodic/build-melodic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | docker build -t robowflex/melodic -f Dockerfile `git rev-parse --show-toplevel` 5 | -------------------------------------------------------------------------------- /.docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic-ros-base 2 | MAINTAINER Zachary Kingston zak@rice.edu 3 | 4 | # Download Dependencies 5 | RUN apt-get update && \ 6 | apt-get install -y software-properties-common && \ 7 | apt-add-repository ppa:dartsim/ppa && \ 8 | apt-get update && \ 9 | apt-get install -y \ 10 | build-essential wget pkg-config python3-catkin-tools python3-osrf-pycommon cmake git \ 11 | libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ 12 | ros-${ROS_DISTRO}-moveit \ 13 | libdart6-all-dev 14 | 15 | # Setup Catkin Workspace 16 | RUN mkdir -p ws/src 17 | WORKDIR /ws/src 18 | RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources 19 | WORKDIR /ws 20 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 21 | catkin build --limit-status-rate 0.001 --no-notify 22 | 23 | # Build Robowflex 24 | WORKDIR /ws/src 25 | ADD ./CMakeModules CMakeModules 26 | 27 | # Main Library 28 | ADD ./robowflex_library robowflex_library 29 | RUN catkin build robowflex_library --start-with robowflex_library --limit-status-rate 0.001 --no-notify 30 | 31 | # OMPL Module 32 | ADD ./robowflex_ompl robowflex_ompl 33 | RUN catkin build robowflex_ompl --start-with robowflex_ompl --limit-status-rate 0.001 --no-notify 34 | 35 | # Movegroup Module 36 | ADD ./robowflex_movegroup robowflex_movegroup 37 | RUN catkin build robowflex_movegroup --start-with robowflex_movegroup --limit-status-rate 0.001 --no-notify 38 | 39 | # DART Module 40 | ADD ./robowflex_dart robowflex_dart 41 | RUN catkin build robowflex_dart --start-with robowflex_dart --limit-status-rate 0.001 --no-notify 42 | WORKDIR / -------------------------------------------------------------------------------- /.docker/noetic/build-noetic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | docker build -t robowflex/noetic -f Dockerfile `git rev-parse --show-toplevel` 5 | -------------------------------------------------------------------------------- /.docker/tesseract/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-ros-base 2 | MAINTAINER Zachary Kingston zak@rice.edu 3 | 4 | ENV TESSERACT_BRANCH="kinetic-devel" 5 | 6 | # Download Dependencies 7 | RUN apt-get update && \ 8 | apt-get install -y \ 9 | build-essential git wget pkg-config python-catkin-tools python-rosdep cmake \ 10 | libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ 11 | ros-${ROS_DISTRO}-moveit 12 | 13 | # Setup rosdep 14 | RUN rosdep update 15 | 16 | # Setup Catkin Workspace 17 | RUN mkdir -p ws/src 18 | WORKDIR /ws/src 19 | RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources 20 | WORKDIR /ws 21 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 22 | catkin build --limit-status-rate 0.001 --no-notify 23 | 24 | # Setup Tesseract 25 | WORKDIR /ws/src/ 26 | RUN git clone --recursive --single-branch --branch ${TESSERACT_BRANCH} --depth 1 https://github.com/ros-industrial-consortium/tesseract.git 27 | RUN git clone --recursive --single-branch --branch ${TESSERACT_BRANCH} --depth 1 https://github.com/ros-industrial-consortium/trajopt_ros.git 28 | WORKDIR /ws 29 | RUN rosdep install --from-paths src --ignore-src -r -y 30 | RUN catkin build --limit-status-rate 0.001 --no-notify 31 | 32 | # Build Robowflex 33 | WORKDIR /ws/src 34 | ADD ./CMakeModules CMakeModules 35 | 36 | # Main Library 37 | ADD ./robowflex_library robowflex_library 38 | RUN catkin build robowflex_library --start-with robowflex_library --limit-status-rate 0.001 --no-notify 39 | 40 | # Tesseract Module 41 | ADD ./robowflex_tesseract robowflex_tesseract 42 | RUN catkin build robowflex_tesseract --start-with robowflex_tesseract --limit-status-rate 0.001 --no-notify 43 | WORKDIR / -------------------------------------------------------------------------------- /.docker/tesseract/build-tesseract.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | docker build -t robowflex/tesseract -f Dockerfile `git rev-parse --show-toplevel` 5 | -------------------------------------------------------------------------------- /.docs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robowflex_docs) 3 | 4 | macro(add_doc project) 5 | get_filename_component(ABS_FILENAME_SRC "../${project}/src" 6 | REALPATH BASE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 7 | 8 | get_filename_component(ABS_FILENAME_PACKAGE "../${project}/${project}" 9 | REALPATH BASE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 10 | 11 | get_filename_component(ABS_FILENAME_INCLUDE "../${project}/include" 12 | REALPATH BASE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 13 | 14 | get_filename_component(ABS_FILENAME_SCRIPTS "../${project}/scripts" 15 | REALPATH BASE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 16 | 17 | get_filename_component(ABS_FILENAME_README "../${project}/README.md" 18 | REALPATH BASE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 19 | 20 | set(DOC_SOURCES "${DOC_SOURCES} ${ABS_FILENAME_SRC} ${ABS_FILENAME_PACKAGE} ${ABS_FILENAME_INCLUDE} ${ABS_FILENAME_SCRIPTS} ${ABS_FILENAME_README}") 21 | endmacro(add_doc) 22 | 23 | add_doc(robowflex_library) 24 | add_doc(robowflex_movegroup) 25 | add_doc(robowflex_ompl) 26 | add_doc(robowflex_tesseract) 27 | add_doc(robowflex_visualization) 28 | add_doc(robowflex_dart) 29 | add_doc(.docker) 30 | 31 | find_package(Doxygen REQUIRED) 32 | 33 | set(DOXYFILE "${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in") 34 | 35 | set(DOXYGEN_SOURCE_DIRS ${DOC_SOURCES}) 36 | 37 | set(DOXYGEN_DOC_DIR ${CMAKE_CURRENT_SOURCE_DIR}) 38 | 39 | get_filename_component(DOXYGEN_README "./home.md" 40 | REALPATH BASE_DIR "${DOXYGEN_DOC_DIR}/markdown/") 41 | 42 | set(DOXYGEN_TAG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/tag) 43 | set(DOXYGEN_HTML_DIR ${CMAKE_CURRENT_SOURCE_DIR}/html) 44 | set(DOXYGEN_ROS_VERSION "melodic") 45 | 46 | file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc) 47 | 48 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY) 49 | # add a target to generate API documentation with Doxygen 50 | add_custom_target(doc ALL 51 | ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile 52 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc 53 | ) 54 | -------------------------------------------------------------------------------- /.docs/html/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.docs/html/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | $projectname: $title 10 | $title 11 | 12 | 13 | 14 | 15 | $treeview 16 | $search 17 | $mathjax 18 | 19 | 20 | $extrastylesheet 21 | 22 | 23 |
24 | 25 | 26 |
27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 40 | 41 | 42 | 43 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 |
35 |
$projectname 36 |  $projectnumber 37 |
38 |
$projectbrief
39 | 		44 |
$projectbrief
45 | 		$searchbox
56 |
57 | 58 | 59 | -------------------------------------------------------------------------------- /.docs/markdown/home.md: -------------------------------------------------------------------------------- 1 | # Robowflex {#home} 2 | 3 | Making MoveIt Easy! 4 | An overview of what this project is can be found in the [design description](\ref design). 5 | Examples of basic motion planning, benchmarking of planning requests, input/output, and more can be found in [scripts](\ref scripts). 6 | 7 | ## robowflex_library 8 | A library that simplifies using [_MoveIt!_](https://github.com/ros-planning/moveit) in applications. 9 | This is the core library used by all other components in Robowflex. 10 | Take a look at the [design description](\ref design) for more information on the core library. 11 | 12 | ## Optional Components 13 | 14 | There are also a few optional modules that are compiled if the requisite libraries are available. 15 | These will only compile if the necessary components are found. 16 | 17 | ### robowflex_ompl 18 | An optionally compiled library component that adds more direct access to [OMPL](http://ompl.kavrakilab.org/) through a new `robowflex::Planner`. 19 | Requires [`moveit_planners_ompl`](https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_planners/ompl), from [_MoveIt!_](https://github.com/ros-planning/moveit). 20 | 21 | ### robowflex_tesseract 22 | An optionally compiled library component that adds support for [tesseract](https://github.com/ros-industrial-consortium/tesseract)-based planners. 23 | Currently, only [OMPL](http://ompl.kavrakilab.org/)-based planning is available. 24 | Requires both [tesseract](https://github.com/ros-industrial-consortium/tesseract) and [trajopt](https://github.com/ros-industrial-consortium/trajopt_ros). 25 | 26 | ### robowflex_movegroup 27 | A library component with helper classes and functions to interact with a `move_group` process being used for motion planning. 28 | Scenes can be pushed and pulled and trajectories can be executed with this component through `move_group`. 29 | 30 | ### robowflex_dart 31 | A optionally compiled library that adds support for modeling and planning through [DART (Dynamic Animation and Robotics Toolkit)](https://dartsim.github.io/). 32 | There are features for loading robots just through DART or by converting __MoveIt__ robots into the DART representation. 33 | Motion planning is supported through [OMPL](http://ompl.kavrakilab.org/). 34 | This module offers easy multi-robot motion planning through composing complex worlds with multiple robots. 35 | Additionally, this module has [manifold-constrained motion planning](http://ompl.kavrakilab.org/constrainedPlanning.html) with a Task Space Region constraint specification. 36 | 37 | ### robowflex_visualization 38 | Python scripts for visualizing robots and motion plans in [Blender](https://www.blender.org/). 39 | See [the readme](md__home_runner_work_robowflex_robowflex_robowflex_visualization_README.html) for more information on how to use. 40 | -------------------------------------------------------------------------------- /.docs/markdown/installation.md: -------------------------------------------------------------------------------- 1 | # Installation Instructions 2 | 3 | Robowflex is supported on many ROS platforms, from Kinetic on Ubuntu 16.04 to Noetic on 20.04. 4 | 5 | Here are some bare-bones installation instructions to get up and running on a new Ubuntu 18.04 machine without ROS already installed. 6 | This will only enable the core modules to be built, as some modules require special packages to be installed. 7 | 8 | First, [install ROS](http://wiki.ros.org/melodic/Installation/Ubuntu) following the directions copied below: 9 | 10 | ```sh 11 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 12 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 13 | sudo apt update 14 | sudo apt install ros-melodic-desktop-full 15 | sudo rosdep init 16 | rosdep update 17 | source /opt/ros/melodic/setup.bash 18 | sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential 19 | ``` 20 | 21 | We recommend `catkin-tools` to build your ROS workspace: 22 | ```sh 23 | sudo apt install python-catkin-tools 24 | ``` 25 | 26 | Install _MoveIt_: 27 | ```sh 28 | sudo apt install ros-melodic-moveit 29 | ``` 30 | 31 | Finally, create a workspace with Robowflex inside: 32 | ```sh 33 | cd ~ 34 | mkdir -p rb_ws/src 35 | cd rb_ws 36 | source /opt/ros/melodic/setup.bash # if you haven't already 37 | catkin config --init 38 | cd src 39 | git clone https://github.com/KavrakiLab/robowflex.git 40 | catkin build 41 | ``` 42 | 43 | To try out a demo script, you first need a robot description. 44 | The easiest to try is the _Fetch_ robot, either by debian or source: 45 | ```sh 46 | # Debian 47 | sudo apt install ros-melodic-fetch-ros 48 | 49 | # Or, Source 50 | cd ~/rb_ws/src 51 | git clone https://github.com/fetchrobotics/fetch_ros 52 | catkin build 53 | ``` 54 | 55 | After the workspace is built, source and run a demo: 56 | ```sh 57 | cd ~/rb_ws 58 | source ./devel/setup.bash 59 | rosrun robowflex_library fetch_test 60 | ``` 61 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.cpp text diff=cpp 2 | *.h text diff=cpp 3 | *.py text diff=python 4 | *.css text diff=css 5 | *.html text diff=html 6 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug-report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug Report 3 | about: Create a bug report to help us improve 4 | title: "[BUG]" 5 | labels: bug 6 | assignees: zkingston 7 | 8 | --- 9 | 10 | 11 | 12 | ## Expected Behavior 13 | 14 | 15 | ## Current Behavior 16 | 17 | 18 | ## Possible Solution 19 | 20 | 21 | ## Steps to Reproduce 22 | 23 | 24 | 1. 25 | 2. 26 | 3. 27 | 4. 28 | 29 | ## Context (Environment) 30 | 31 | 32 | 33 | 34 | 35 | ## Detailed Description 36 | 37 | 38 | ## Possible Implementation 39 | 40 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature-request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature Request 3 | about: Suggest an idea for this project 4 | title: "[FEATURE]" 5 | labels: enhancement 6 | assignees: zkingston 7 | 8 | --- 9 | 10 | 11 | 12 | ## Related Issues 13 | 14 | 15 | ## Brief Description 16 | 17 | 18 | ## Detailed Description 19 | 20 | 21 | ## Possible Implementation 22 | 23 | -------------------------------------------------------------------------------- /.github/workflows/build-kinetic.yml: -------------------------------------------------------------------------------- 1 | name: Build Kinetic 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build-kinetic: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout Repository 15 | uses: actions/checkout@v2 16 | 17 | - name: Compile Docker Container 18 | run: ./.docker/kinetic/build-kinetic.sh 19 | 20 | - name: Run Tests 21 | run: source ./.ci/run-tests.sh; check-docker-tests kinetic 22 | 23 | - name: Clean Docker 24 | run: sudo ./.ci/clean-docker.sh 25 | -------------------------------------------------------------------------------- /.github/workflows/build-melodic.yml: -------------------------------------------------------------------------------- 1 | name: Build Melodic 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build-melodic: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout Repository 15 | uses: actions/checkout@v2 16 | 17 | - name: Compile Docker Container 18 | run: ./.docker/melodic/build-melodic.sh 19 | 20 | - name: Run Tests 21 | run: source ./.ci/run-tests.sh; check-docker-tests melodic 22 | 23 | - name: Clean Docker 24 | run: sudo ./.ci/clean-docker.sh 25 | -------------------------------------------------------------------------------- /.github/workflows/build-noetic.yml: -------------------------------------------------------------------------------- 1 | name: Build Noetic 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build-noetic: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout Repository 15 | uses: actions/checkout@v2 16 | 17 | - name: Compile Docker Container 18 | run: ./.docker/noetic/build-noetic.sh 19 | 20 | - name: Run Tests 21 | run: source ./.ci/run-tests.sh; check-docker-tests noetic 22 | 23 | - name: Clean Docker 24 | run: sudo ./.ci/clean-docker.sh 25 | -------------------------------------------------------------------------------- /.github/workflows/documentation.yml: -------------------------------------------------------------------------------- 1 | name: Documentation 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | 7 | jobs: 8 | documentation: 9 | runs-on: ubuntu-latest 10 | 11 | steps: 12 | - name: Checkout Repository 13 | uses: actions/checkout@v2 14 | 15 | - name: Download Dependencies 16 | run: sudo apt-get install doxygen graphviz cmake 17 | 18 | - name: Build Documentation 19 | run: ./.ci/documentation.sh 20 | 21 | - name: GitHub Pages 22 | uses: crazy-max/ghaction-github-pages@v2 23 | with: 24 | target_branch: gh-pages 25 | build_dir: .docs/build/doc/html 26 | env: 27 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 28 | -------------------------------------------------------------------------------- /.github/workflows/linting.yml: -------------------------------------------------------------------------------- 1 | name: Linting 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | linting: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout Repository 15 | uses: actions/checkout@v2 16 | 17 | - name: Download Dependencies 18 | run: sudo apt-get install clang-format 19 | 20 | - name: Check Formatting 21 | run: ./.ci/check-format.sh 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build 2 | */build 3 | 4 | # Prerequisites 5 | *.d 6 | 7 | # Compiled Object files 8 | *.slo 9 | *.lo 10 | *.o 11 | *.obj 12 | 13 | # Precompiled Headers 14 | *.gch 15 | *.pch 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | *.dll 21 | 22 | # Fortran module files 23 | *.mod 24 | *.smod 25 | 26 | # Compiled Static libraries 27 | *.lai 28 | *.la 29 | *.a 30 | *.lib 31 | 32 | # Executables 33 | *.exe 34 | *.out 35 | *.app 36 | core 37 | 38 | # Compile Commands 39 | *compile_commands.json 40 | 41 | # Python 42 | __pycache__ 43 | *.pyc 44 | 45 | #Vim 46 | *.swp 47 | 48 | *.build-tmp* 49 | *.blend1 50 | -------------------------------------------------------------------------------- /.style.yapf: -------------------------------------------------------------------------------- 1 | [style] 2 | based_on_style = pep8 3 | column_limit = 80 4 | spaces_around_default_or_named_assign = true 5 | spaces_before_comment = 4 6 | split_before_logical_operator = true -------------------------------------------------------------------------------- /CMakeModules/CompileOptions.cmake: -------------------------------------------------------------------------------- 1 | if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 2 | set(CMAKE_BUILD_TYPE Release) 3 | endif() 4 | 5 | if (CMAKE_VERSION VERSION_LESS "3.1") 6 | add_definitions(-std=c++14) 7 | else() 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | # this next line shouldn't be necessary, but doesn't always get added by cmake (e.g., for clang++-5) 12 | add_definitions(-std=c++14) 13 | endif() 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX) 16 | add_definitions(-W -Wall -Wextra -Wcast-qual -Wwrite-strings -Wunreachable-code -Wpointer-arith -Winit-self -Wredundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-noexcept-type -Wno-deprecated-declarations) 17 | # prepend optimizion flag (in case the default setting doesn't include one) 18 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}") 19 | endif(CMAKE_COMPILER_IS_GNUCXX) 20 | 21 | if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 22 | add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Qunused-arguments -Wno-deprecated-register -Wno-mismatched-tags -Wno-deprecated-declarations) 23 | # prepend optimizion flag (in case the default setting doesn't include one) 24 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}") 25 | endif() 26 | 27 | if((CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) AND NOT MINGW) 28 | add_definitions(-fPIC) 29 | endif((CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) AND NOT MINGW) 30 | 31 | # no prefix needed for python modules 32 | set(CMAKE_SHARED_MODULE_PREFIX "") 33 | -------------------------------------------------------------------------------- /CMakeModules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | -------------------------------------------------------------------------------- /CMakeModules/FindGFlags.cmake: -------------------------------------------------------------------------------- 1 | find_path(GFLAGS_INCLUDE_DIR NAMES gflags/gflags.h) 2 | find_library(GFLAGS_LIBRARY NAMES gflags) 3 | 4 | set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY}) 5 | set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR}) 6 | 7 | include(FindPackageHandleStandardArgs) 8 | find_package_handle_standard_args(GFLAGS DEFAULT_MSG 9 | GFLAGS_LIBRARY GFLAGS_INCLUDE_DIR) 10 | 11 | mark_as_advanced(GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY) 12 | -------------------------------------------------------------------------------- /CMakeModules/FindTinyXML2.cmake: -------------------------------------------------------------------------------- 1 | find_path(TINYXML2_INCLUDE_DIR NAMES tinyxml2.h) 2 | find_library(TINYXML2_LIBRARIES NAMES tinyxml2) 3 | 4 | set(TINYXML2_LIBRARIES ${TINYXML2_LIBRARY}) 5 | set(TINYXML2_INCLUDE_DIRS ${TINYXML2_INCLUDE_DIR}) 6 | 7 | include(FindPackageHandleStandardArgs) 8 | find_package_handle_standard_args(TinyXML2 DEFAULT_MSG 9 | TINYXML2_LIBRARIES TINYXML2_INCLUDE_DIR) 10 | 11 | mark_as_advanced(TINYXML2_INCLUDE_DIR TINYXML2_LIBRARIES) 12 | -------------------------------------------------------------------------------- /CMakeModules/HelperFunctions.cmake: -------------------------------------------------------------------------------- 1 | # Adds a test (in a source file under the `tests` directory) to the list of 2 | # executables to compile. Adds the name to the list `TESTS`. 3 | macro(add_test_script test_name) 4 | if(CATKIN_ENABLE_TESTING) 5 | list(APPEND TESTS test_${test_name}) 6 | catkin_add_gtest(test_${test_name} test/${test_name}.cpp) 7 | target_link_libraries(test_${test_name} ${LIBRARY_NAME} ${catkin_LIBRARIES}) 8 | endif() 9 | endmacro(add_test_script) 10 | 11 | # Install tests added via `add_test` to the install directory. 12 | macro(install_tests) 13 | install(TARGETS 14 | ${TESTS} 15 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 16 | endmacro(install_tests) 17 | 18 | # Adds a script (in a source file under the `scripts` directory) to the list of 19 | # executables to compile. Adds the name to the list `SCRIPTS`. 20 | macro(add_script script_name) 21 | list(APPEND SCRIPTS ${script_name}) 22 | add_executable(${script_name} scripts/${script_name}.cpp) 23 | target_link_libraries(${script_name} ${LIBRARY_NAME} ${catkin_LIBRARIES}) 24 | endmacro(add_script) 25 | 26 | # Install scripts added via `add_script` to the install directory. 27 | macro(install_scripts) 28 | install(TARGETS 29 | ${SCRIPTS} 30 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 31 | endmacro(install_scripts) 32 | 33 | # Installs library to install directory. Assumes LIBRARY_NAME is set. 34 | macro(install_library) 35 | install(TARGETS ${LIBRARY_NAME} 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 39 | 40 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 41 | endmacro(install_library) 42 | 43 | # Installs a directory (e.g., YAML, etc.) to the install directory. 44 | macro(install_directory directory) 45 | install(DIRECTORY ${directory} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 46 | endmacro(install_directory) 47 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Robowflex is released as Open Source under the terms of a 3-clause 2 | BSD license. 3 | 4 | Copyright (c) 2018, Rice University 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions 9 | are met: 10 | 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above 14 | copyright notice, this list of conditions and the following 15 | disclaimer in the documentation and/or other materials provided 16 | with the distribution. 17 | * Neither the name of the Rice University nor the names of its 18 | contributors may be used to endorse or promote products derived 19 | from this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | LIBSDED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | BUT NOT LIBSDED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | find . -regex '.*\.\(cpp\|h\)' -not -path ".docs/*" -exec clang-format -style=file -i {} \; 5 | -------------------------------------------------------------------------------- /iwyu.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | source .ci/tidy-functions.sh 5 | 6 | generate-compile-commands robowflex_library 7 | get-iwyu 8 | generate-compile-commands robowflex_ompl 9 | get-iwyu 10 | generate-compile-commands robowflex_movegroup 11 | get-iwyu 12 | generate-compile-commands robowflex_dart 13 | get-iwyu 14 | -------------------------------------------------------------------------------- /robowflex/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex 3 | VERSION 1.3 4 | ) 5 | 6 | find_package(catkin REQUIRED) 7 | catkin_metapackage() 8 | -------------------------------------------------------------------------------- /robowflex/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex 4 | 1.3.0 5 | Robowflex: Making MoveIt Easy! 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | 16 | robowflex_library 17 | robowflex_ompl 18 | robowflex_tesseract 19 | robowflex_movegroup 20 | robowflex_visualization 21 | robowflex_dart 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /robowflex_dart/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex_dart 3 | VERSION 1.3 4 | LANGUAGES CXX 5 | ) 6 | 7 | set(LIBRARY_NAME ${PROJECT_NAME}) 8 | 9 | set(CMAKE_MODULE_PATH 10 | "${CMAKE_MODULE_PATH}" 11 | "${CMAKE_ROOT_DIR}/cmake/Modules" 12 | "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 13 | 14 | include(CompileOptions) 15 | include(HelperFunctions) 16 | 17 | find_package(DART 6 QUIET 18 | COMPONENTS utils utils-urdf gui-osg 19 | OPTIONAL_COMPONENTS 20 | CONFIG) 21 | 22 | if (DART_LIBRARIES) 23 | 24 | find_package(Boost REQUIRED COMPONENTS filesystem system program_options) 25 | include_directories(SYSTEM ${Boost_INCLUDE_DIRS}) 26 | list(APPEND LIBRARIES ${Boost_LIBRARIES}) 27 | 28 | find_package(Eigen3 REQUIRED) 29 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) 30 | list(APPEND LIBRARIES ${EIGEN3_LIBRARIES}) 31 | 32 | find_package(ompl REQUIRED) 33 | if (ompl_LIBRARIES) 34 | include_directories(SYSTEM ${ompl_INCLUDE_DIRS}) 35 | list(APPEND LIBRARIES ${ompl_LIBRARIES}) 36 | else() 37 | include_directories(SYSTEM ${OMPL_INCLUDE_DIRS}) 38 | list(APPEND LIBRARIES ${OMPL_LIBRARIES}) 39 | endif (ompl_LIBRARIES) 40 | 41 | find_package(TinyXML2 REQUIRED) 42 | include_directories(SYSTEM ${TINYXML2_INCLUDE_DIRS}) 43 | list(APPEND LIBRARIES ${TINYXML2_LIBRARIES}) 44 | 45 | find_package(Threads REQUIRED) 46 | list(APPEND LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) 47 | 48 | include_directories(SYSTEM ${DART_INCLUDE_DIRS}) 49 | list(APPEND LIBRARIES ${DART_LIBRARIES}) 50 | 51 | find_package(catkin REQUIRED COMPONENTS 52 | robowflex_library 53 | ) 54 | 55 | catkin_package( 56 | LIBRARIES ${LIBRARY_NAME} 57 | CATKIN_DEPENDS 58 | robowflex_library 59 | DEPENDS 60 | INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/include 61 | ) 62 | 63 | include_directories(SYSTEM ${CMAKE_CURRENT_LIST_DIR}/include ${catkin_INCLUDE_DIRS}) 64 | link_directories(${catkin_LIBRARY_DIRS}) 65 | 66 | add_library(${LIBRARY_NAME} 67 | src/robot.cpp 68 | src/io.cpp 69 | src/space.cpp 70 | src/world.cpp 71 | src/acm.cpp 72 | src/structure.cpp 73 | src/tsr.cpp 74 | src/planning.cpp 75 | src/gui.cpp 76 | src/planner.cpp 77 | src/joints/joint.cpp 78 | src/joints/rnjoint.cpp 79 | src/joints/so2joint.cpp 80 | src/joints/so3joint.cpp 81 | ) 82 | 83 | set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) 84 | target_link_libraries(${LIBRARY_NAME} PUBLIC ${LIBRARIES}) 85 | 86 | add_script(fetch_plan) 87 | add_script(fetch_pick) 88 | add_script(fetch_ik) 89 | add_script(fetch_bimanual) 90 | add_script(fetch_lift) 91 | add_script(fetch_se2) 92 | add_script(fetch_robowflex_plan) 93 | add_script(fetch_robowflex_planner) 94 | add_script(gui_test) 95 | add_script(tsr_helper) 96 | 97 | install_scripts() 98 | install_library() 99 | else() 100 | message("`DART` not found, not compiling robowflex_dart library or executables.") 101 | endif() 102 | -------------------------------------------------------------------------------- /robowflex_dart/CMakeModules: -------------------------------------------------------------------------------- 1 | ../CMakeModules/ -------------------------------------------------------------------------------- /robowflex_dart/README.md: -------------------------------------------------------------------------------- 1 | # Robowflex Dart 2 | 3 | Modeling and planning through [DART (Dynamic Animation and Robotics Toolkit)](https://dartsim.github.io/). 4 | Robots (`robowflex::darts::Robot`) and scene geometry (`robowflex::darts::Structure`) are added to a world (`robowflex::darts::World`), which is then used as the base for planning. 5 | An [OMPL](http://ompl.kavrakilab.org/) state space is provided for worlds (`robowflex::darts::StateSpace`) which can control any joint in the world, for multiple robots. 6 | Constraints and inverse kinematics are given through a Task Space Region specification (`robowflex::darts::TSR`). 7 | For convenience, a helper class (`robowflex::darts::PlanBuilder`) makes it easy to specify motion planning queries. 8 | 9 | # Scripts 10 | 11 | There are a few example scripts to demonstrate the module, in the `scripts` directory. 12 | - `fetch_plan.cpp`: Plan for 1 to 4 Fetch robots. Demonstrates how to do multi-robot planning by cloning robots (`robowflex::darts::Robot::clone()`) and adding more groups to the planning group. 13 | - `fetch_robowflex_plan.cpp`: Constrained planning for a Fetch robot, where that Fetch robot is loaded via `robowflex_library` as a _MoveIt_ robot. 14 | 15 | # Installation Instructions 16 | 17 | DART can either be installed via a PPA or be added as a catkin package in your local workspace. 18 | See [DART's own installation instructions for more details](https://dartsim.github.io/install_dart_on_ubuntu.html#install-dart). 19 | 20 | ## PPA Installation 21 | 22 | To install DART via PPA, simply do the following: 23 | 24 | ```sh 25 | sudo apt-add-repository ppa:dartsim/ppa 26 | sudo apt-get update # not necessary since Bionic 27 | ``` 28 | 29 | After the PPA is installed, then get DART: 30 | ```sh 31 | sudo apt-get install libdart6-all-dev 32 | ``` 33 | 34 | ## Source Installation 35 | 36 | Install DART's dependencies: 37 | ```sh 38 | sudo apt-get install \ 39 | libeigen3-dev \ 40 | libassimp-dev \ 41 | libccd-dev \ 42 | libfcl-dev \ 43 | libboost-regex-dev \ 44 | libboost-system-dev \ 45 | libopenscenegraph-dev \ 46 | libnlopt-dev \ 47 | coinor-libipopt-dev \ 48 | libbullet-dev \ 49 | libode-dev \ 50 | liboctomap-dev \ 51 | libflann-dev \ 52 | libtinyxml2-dev \ 53 | liburdfdom-dev \ 54 | libxi-dev \ 55 | libxmu-dev \ 56 | freeglut3-dev \ 57 | libopenscenegraph-dev 58 | ``` 59 | 60 | Then, add DART to your local catkin workspace: 61 | ```sh 62 | git clone -b v6.10.0 git://github.com/dartsim/dart.git 63 | ``` 64 | 65 | ## OMPL Installation 66 | 67 | The DART module relies on recent versions of OMPL (> 1.4.0). 68 | If you are using an old OMPL (e.g., on Kinetic), either download the source into your local catkin workspace: 69 | ```sh 70 | wget -O - https://github.com/ompl/ompl/archive/1.5.0.tar.gz | tar zxf - 71 | ``` 72 | Or clone from the repository: 73 | ```sh 74 | git clone git:://github.com/ompl/ompl.git 75 | ``` 76 | -------------------------------------------------------------------------------- /robowflex_dart/include/robowflex_dart.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_DART_ 4 | #define ROBOWFLEX_DART_ 5 | 6 | namespace robowflex 7 | { 8 | /** \brief DART-based robot modeling and planning 9 | */ 10 | namespace darts 11 | { 12 | } 13 | } // namespace robowflex 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /robowflex_dart/include/robowflex_dart/io.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_DART_IO_ 4 | #define ROBOWFLEX_DART_IO_ 5 | 6 | #include 7 | 8 | namespace robowflex 9 | { 10 | namespace darts 11 | { 12 | /** \cond IGNORE */ 13 | ROBOWFLEX_CLASS_FORWARD(Robot) 14 | /** \endcond */ 15 | 16 | namespace IO 17 | { 18 | /** \brief Add a ROS package to Dart's searchable index. Looks up package with rospack. 19 | * \param[in] package Package name 20 | */ 21 | void addPackage(const std::string &package); 22 | 23 | /** \brief Add a ROS package to Dart's searchable index. 24 | * \param[in] package Package name 25 | * \param[in] location Directory of package 26 | */ 27 | void addPackage(const std::string &package, const std::string &location); 28 | 29 | /** \brief Loads a URDF at \a urdf into a robot. 30 | * \param[out] robot Robot to load URDF in. 31 | * \param[in] urdf URDF location. 32 | * \return True on success, false on failure. 33 | */ 34 | bool loadURDF(Robot &robot, const std::string &urdf); 35 | 36 | /** \brief Get the filename for a package URI using Dart's lookup. 37 | * \param[in] uri URI to lookup. 38 | * \return Path to file. 39 | */ 40 | std::string getPackageFile(const std::string &uri); 41 | } // namespace IO 42 | } // namespace darts 43 | 44 | } // namespace robowflex 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /robowflex_dart/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_dart 4 | 1.3.0 5 | Robowflex: DART Planning Tools 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | 16 | robowflex_library 17 | dartsim 18 | 19 | -------------------------------------------------------------------------------- /robowflex_dart/scripts/fetch_ik.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | using namespace robowflex; 20 | 21 | int main(int /*argc*/, char ** /*argv*/) 22 | { 23 | auto world = std::make_shared(); 24 | 25 | auto fetch1 = darts::loadMoveItRobot("fetch1", // 26 | "package://fetch_description/robots/fetch.urdf", // 27 | "package://fetch_moveit_config/config/fetch.srdf"); 28 | auto fetch2 = fetch1->cloneRobot("fetch2"); 29 | fetch2->setDof(4, 1); 30 | 31 | world->addRobot(fetch1); 32 | world->addRobot(fetch2); 33 | 34 | darts::Window window(world); 35 | 36 | darts::PlanBuilder builder(world); 37 | builder.addGroup("fetch1", "arm_with_torso"); 38 | builder.addGroup("fetch2", "arm_with_torso"); 39 | 40 | builder.setStartConfiguration({ 41 | 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // 42 | 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // 43 | }); 44 | 45 | builder.initialize(); 46 | 47 | darts::TSR::Specification goal1_spec; 48 | goal1_spec.setFrame("fetch1", "wrist_roll_link", "base_link"); 49 | goal1_spec.setPose(0.4, 0.3, 0.92, // 50 | // 0.5, -0.5, 0.5, 0.5); 51 | 0.707, 0, 0, 0.707); 52 | 53 | darts::TSR::Specification goal2_spec; 54 | goal2_spec.setFrame("fetch2", "wrist_roll_link", "base_link"); 55 | goal2_spec.setPose(0.4, -0.3, 0.92, // 56 | 0.707, 0, 0, -0.707); 57 | 58 | auto goal1_tsr = std::make_shared(world, goal1_spec); 59 | auto goal2_tsr = std::make_shared(world, goal2_spec); 60 | 61 | auto goal = builder.getGoalTSR({goal1_tsr, goal2_tsr}); 62 | builder.setGoal(goal); 63 | 64 | auto rrt = std::make_shared(builder.info, true); 65 | rrt->setRange(1.); 66 | builder.ss->setPlanner(rrt); 67 | 68 | builder.setup(); 69 | builder.ss->print(); 70 | 71 | window.run([&]() { 72 | std::this_thread::sleep_for(std::chrono::milliseconds(2000)); 73 | while (true) 74 | { 75 | goal->startSampling(); 76 | ompl::base::PlannerStatus solved = builder.ss->solve(30.0); 77 | goal->stopSampling(); 78 | 79 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 80 | if (solved) 81 | { 82 | RBX_INFO("Found solution!"); 83 | window.animatePath(builder, builder.getSolutionPath()); 84 | } 85 | else 86 | RBX_WARN("No solution found"); 87 | 88 | builder.ss->clear(); 89 | } 90 | }); 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /robowflex_dart/scripts/fetch_robowflex_planner.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | using namespace robowflex; 15 | 16 | static const std::string GROUP = "arm_with_torso"; 17 | 18 | int main(int argc, char **argv) 19 | { 20 | // Startup ROS 21 | ROS ros(argc, argv); 22 | 23 | // Create the default Fetch robot. 24 | auto fetch = std::make_shared(); 25 | fetch->initialize(); 26 | 27 | // Create an RViz visualization helper 28 | IO::RVIZHelper rviz(fetch); 29 | RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)..."); 30 | std::cin.get(); 31 | 32 | // Create an empty scene. 33 | auto scene = std::make_shared(fetch); 34 | 35 | // Create the default planner for the Fetch. 36 | auto planner = std::make_shared(fetch); 37 | 38 | // Create a motion planning request with a pose goal. 39 | MotionRequestBuilder request(planner, GROUP); 40 | fetch->setGroupState(GROUP, {0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0}); // Stow 41 | request.setStartConfiguration(fetch->getScratchState()); 42 | 43 | fetch->setGroupState(GROUP, {0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); // Unfurl 44 | request.setGoalConfiguration(fetch->getScratchState()); 45 | 46 | request.setConfig("rrt::RRTConnect"); 47 | 48 | // Do motion planning! 49 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 50 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 51 | return 1; 52 | 53 | // Publish the trajectory to a topic to display in RViz 54 | rviz.updateTrajectory(res); 55 | 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /robowflex_dart/scripts/tsr_helper.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace robowflex; 16 | 17 | int main(int argc, char **argv) 18 | { 19 | int n = 1; 20 | if (argc > 1) 21 | n = std::atoi(argv[1]); 22 | 23 | if (n < 3) 24 | { 25 | auto world = std::make_shared(); 26 | 27 | auto fetch1 = darts::loadMoveItRobot("fetch1", // 28 | "package://fetch_description/robots/fetch.urdf", // 29 | "package://fetch_moveit_config/config/fetch.srdf"); 30 | auto start = fetch1->getSkeleton()->getState(); 31 | world->addRobot(fetch1); 32 | 33 | darts::Window window(world); 34 | 35 | if (n == 2) 36 | { 37 | darts::TSR::Specification spec1; 38 | spec1.setFrame("fetch1", "wrist_roll_link", "base_link"); 39 | spec1.pose = fetch1->getFrame("wrist_roll_link")->getWorldTransform(); 40 | spec1.setXPosTolerance(0.05); 41 | spec1.setYPosTolerance(0.05); 42 | spec1.setZPosTolerance(0.05); 43 | // spec1.setNoRotTolerance(); 44 | auto ew1 = std::make_shared("EE", spec1); 45 | window.addWidget(ew1); 46 | 47 | darts::TSR::Specification spec2; 48 | spec2.setFrame("fetch1", "elbow_flex_link", "base_link"); 49 | spec2.pose = fetch1->getFrame("elbow_flex_link")->getWorldTransform(); 50 | // spec2.setNoRotTolerance(); 51 | spec2.setXPosTolerance(0.05); 52 | spec2.setYPosTolerance(0.05); 53 | spec2.setZPosTolerance(0.05); 54 | auto ew2 = std::make_shared("Elbow", spec2); 55 | window.addWidget(ew2); 56 | 57 | std::vector tsrs{ew1->getTSR(), ew2->getTSR()}; 58 | auto sw = std::make_shared(world, tsrs); 59 | window.addWidget(sw); 60 | } 61 | else 62 | { 63 | darts::TSR::Specification spec; 64 | spec.setFrame("fetch1", "wrist_roll_link", "base_link"); 65 | spec.pose = fetch1->getFrame("wrist_roll_link")->getWorldTransform(); 66 | auto ew = std::make_shared("EE", spec); 67 | window.addWidget(ew); 68 | 69 | std::vector tsrs{ew->getTSR()}; 70 | auto sw = std::make_shared(world, tsrs); 71 | window.addWidget(sw); 72 | } 73 | 74 | window.run(); 75 | } 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /robowflex_dart/src/acm.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace robowflex::darts; 5 | 6 | /// 7 | /// ACM 8 | /// 9 | 10 | ACM::ACM(const Structure *structure) 11 | : structure_(structure), filter_(std::make_shared()) 12 | { 13 | } 14 | 15 | void ACM::disableCollision(const std::string &a, const std::string &b) 16 | { 17 | auto key = makeKey(a, b); 18 | if (acm_.find(key) == acm_.end()) 19 | { 20 | acm_.emplace(key); 21 | filter_->addBodyNodePairToBlackList(getBodyNode(key.first), getBodyNode(key.second)); 22 | } 23 | } 24 | 25 | void ACM::enableCollision(const std::string &a, const std::string &b) 26 | { 27 | auto key = makeKey(a, b); 28 | auto it = acm_.find(key); 29 | if (it != acm_.end()) 30 | { 31 | filter_->removeBodyNodePairFromBlackList(getBodyNode(key.first), getBodyNode(key.second)); 32 | acm_.erase(it); 33 | } 34 | } 35 | 36 | std::shared_ptr ACM::getFilter() 37 | { 38 | return filter_; 39 | } 40 | 41 | const std::shared_ptr &ACM::getFilterConst() const 42 | { 43 | return filter_; 44 | } 45 | 46 | const Structure *ACM::getStructure() const 47 | { 48 | return structure_; 49 | } 50 | 51 | std::set> &ACM::getDisabledPairs() 52 | { 53 | return acm_; 54 | } 55 | 56 | const std::set> &ACM::getDisabledPairsConst() const 57 | { 58 | return acm_; 59 | } 60 | 61 | std::pair ACM::makeKey(const std::string &a, const std::string &b) const 62 | { 63 | if (a < b) 64 | return std::make_pair(a, b); 65 | 66 | return std::make_pair(b, a); 67 | } 68 | 69 | dart::dynamics::BodyNode *ACM::getBodyNode(const std::string &key) 70 | { 71 | return structure_->getSkeletonConst()->getBodyNode(key); 72 | } 73 | -------------------------------------------------------------------------------- /robowflex_dart/src/io.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | using namespace robowflex::darts; 11 | 12 | static dart::utils::DartLoader URDF; 13 | static dart::utils::PackageResourceRetriever PACKAGE; 14 | 15 | void IO::addPackage(const std::string &package) 16 | { 17 | const auto path = robowflex::IO::resolvePackage("package://" + package); 18 | if (not path.empty()) 19 | addPackage(package, path); 20 | } 21 | 22 | void IO::addPackage(const std::string &package, const std::string &location) 23 | { 24 | URDF.addPackageDirectory(package, location); 25 | PACKAGE.addPackageDirectory(package, location); 26 | } 27 | 28 | bool IO::loadURDF(Robot &robot, const std::string &urdf) 29 | { 30 | // Pre-load URDF and extract all relevant ROS packages 31 | const auto &file = IO::getPackageFile(urdf); 32 | const auto &text = robowflex::IO::loadXMLToString(file); 33 | 34 | const auto packages = robowflex::IO::findPackageURIs(text); 35 | for (const auto &package : packages) 36 | IO::addPackage(package); 37 | 38 | auto skeleton = URDF.parseSkeletonString(text, ""); 39 | if (not skeleton) 40 | return false; 41 | 42 | skeleton->setSelfCollisionCheck(true); 43 | 44 | for (auto *joint : skeleton->getJoints()) 45 | joint->setPositionLimitEnforced(true); 46 | 47 | robot.setSkeleton(skeleton); 48 | return true; 49 | } 50 | 51 | std::string IO::getPackageFile(const std::string &uri) 52 | { 53 | std::string file = robowflex::IO::resolvePackage(uri); 54 | if (file.empty()) 55 | file = PACKAGE.getFilePath(uri); 56 | 57 | return file; 58 | } 59 | -------------------------------------------------------------------------------- /robowflex_dart/src/joints/so2joint.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace constants = robowflex::constants; 9 | using namespace robowflex::darts; 10 | 11 | /// 12 | /// SO2Joint 13 | /// 14 | 15 | SO2Joint::SO2Joint(StateSpace *space, // 16 | dart::dynamics::Joint *joint, unsigned int index) 17 | : Joint(space, joint, 1, index, 1) 18 | { 19 | space_->addDimension(-constants::pi, constants::pi); 20 | } 21 | 22 | // L1 23 | double SO2Joint::distance(const Eigen::Ref &a, 24 | const Eigen::Ref &b) const 25 | { 26 | double d = std::fabs(a[0] - b[0]); 27 | return (d > constants::pi) ? constants::two_pi - d : d; 28 | } 29 | 30 | double SO2Joint::getMaximumExtent() const 31 | { 32 | return constants::pi; 33 | } 34 | 35 | void SO2Joint::interpolate(const Eigen::Ref &a, // 36 | const Eigen::Ref &b, // 37 | double t, // 38 | Eigen::Ref c) const 39 | { 40 | double diff = b[0] - a[0]; 41 | if (std::fabs(diff) <= constants::pi) 42 | c[0] = a[0] + diff * t; 43 | else 44 | { 45 | if (diff > 0.0) 46 | diff = constants::two_pi - diff; 47 | else 48 | diff = -constants::two_pi - diff; 49 | 50 | double &v = c[0]; 51 | v = a[0] - diff * t; 52 | 53 | if (v > constants::pi) 54 | v -= constants::two_pi; 55 | else if (v < -constants::pi) 56 | v += constants::two_pi; 57 | } 58 | } 59 | 60 | void SO2Joint::enforceBounds(Eigen::Ref a) const 61 | { 62 | double &v = a[0]; 63 | v = std::fmod(v, constants::two_pi); 64 | if (v < -constants::pi) 65 | v += constants::two_pi; 66 | else if (v >= constants::pi) 67 | v -= constants::two_pi; 68 | } 69 | 70 | bool SO2Joint::satisfiesBounds(const Eigen::Ref &a) const 71 | { 72 | const double &v = a[0]; 73 | return (v >= -constants::pi) and (v <= constants::pi); 74 | } 75 | 76 | void SO2Joint::sample(Eigen::Ref a) const 77 | { 78 | a[0] = rng_.uniformReal(-constants::pi, constants::pi); 79 | } 80 | 81 | void SO2Joint::sampleNear(Eigen::Ref a, // 82 | const Eigen::Ref &near, // 83 | double r) const 84 | { 85 | a[0] = rng_.uniformReal(near[0] - r, near[0] + r); 86 | enforceBounds(a); 87 | } 88 | -------------------------------------------------------------------------------- /robowflex_library/CMakeModules: -------------------------------------------------------------------------------- 1 | ../CMakeModules/ -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/adapter.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_ADAPTER_ 4 | #define ROBOWFLEX_ADAPTER_ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace robowflex 14 | { 15 | /** \brief A pose (point in SE(3)) used in various functions. Defined from what \e MoveIt! uses. 16 | * 17 | * Either an Eigen::Affine3d or Eigen::Isometry3d. These are both transforms, and thus code using a 18 | * RobotPose will generally work for both. 19 | */ 20 | using RobotPose = std::decay< // 21 | decltype( // 22 | std::declval().getTransform("") // 23 | ) // 24 | >::type; 25 | 26 | /** \brief A vector of poses. 27 | */ 28 | using RobotPoseVector = std::vector>; 29 | 30 | /** \brief Convert the Robowflex pose type to another transform type. 31 | */ 32 | template 33 | M toMatrix(const RobotPose &pose) 34 | { 35 | M newpose; 36 | newpose.matrix() = pose.matrix(); 37 | return newpose; 38 | } 39 | } // namespace robowflex 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/class_forward.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_CLASS_FORWARD_ 4 | #define ROBOWFLEX_CLASS_FORWARD_ 5 | 6 | #include // for std::shared_ptr 7 | 8 | /** \file */ 9 | 10 | /** 11 | * \def ROBOWFLEX_CLASS_FORWARD 12 | * \brief Macro that forward declares a class and defines two shared ptrs types: 13 | * - ${Class}Ptr = shared_ptr<${Class}> 14 | * - ${Class}ConstPtr = shared_ptr 15 | */ 16 | #define ROBOWFLEX_CLASS_FORWARD(C) \ 17 | class C; \ 18 | typedef std::shared_ptr C##Ptr; \ 19 | typedef std::shared_ptr C##ConstPtr; 20 | 21 | /** 22 | * \def ROBOWFLEX_EIGEN 23 | * \brief Macro for classes with fixed width Eigen classes. 24 | */ 25 | #define ROBOWFLEX_EIGEN EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/constants.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_CONSTANTS_ 4 | #define ROBOWFLEX_CONSTANTS_ 5 | 6 | #include 7 | #include 8 | 9 | namespace robowflex 10 | { 11 | namespace constants 12 | { 13 | // common 14 | static const double half = boost::math::constants::half(); 15 | static const double third = boost::math::constants::third(); 16 | static const double eps = std::numeric_limits::epsilon(); 17 | static const double inf = std::numeric_limits::infinity(); 18 | static const double nan = std::numeric_limits::quiet_NaN(); 19 | 20 | // pi 21 | static const double pi = boost::math::constants::pi(); 22 | static const double half_pi = boost::math::constants::half_pi(); 23 | static const double quarter_pi = half_pi * half; 24 | static const double two_pi = boost::math::constants::two_pi(); 25 | 26 | // tolerances 27 | static const double ik_tolerance = 1e-5; 28 | static const unsigned int ik_attempts = 50; 29 | static const Eigen::Vector3d ik_vec_tolerance = {ik_tolerance, ik_tolerance, ik_tolerance}; 30 | static const double cart_rot_step_size = 0.01; 31 | static const double cart_pos_step_size = 0.01; 32 | static const double cart_rot_jump_tol = 0.25; 33 | static const double cart_pos_jump_tol = 0.25; 34 | 35 | // planning 36 | static const double default_workspace_bound = 1.0; 37 | static const double default_allowed_planning_time = 5.0; 38 | 39 | } // namespace constants 40 | } // namespace robowflex 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/io/bag.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_IO_BAG_ 4 | #define ROBOWFLEX_IO_BAG_ 5 | 6 | #include 7 | 8 | // clang-format off 9 | ROBOWFLEX_PUSH_DISABLE_GCC_WARNING(-Wcast-qual) 10 | // clang-format on 11 | #include 12 | ROBOWFLEX_POP_GCC 13 | 14 | #include 15 | 16 | namespace robowflex 17 | { 18 | namespace IO 19 | { 20 | /** \brief `rosbag` management class to ease message saving and loading. 21 | */ 22 | class Bag 23 | { 24 | public: 25 | /** \brief File modes 26 | */ 27 | enum Mode 28 | { 29 | READ, ///< Read-only 30 | WRITE ///< Write-only 31 | }; 32 | 33 | /** \brief Constructor. 34 | * \param[in] file File to open or create. 35 | * \param[in] mode Mode to open file in. 36 | */ 37 | Bag(const std::string &file, Mode mode = WRITE); 38 | 39 | /** \brief Destructor. 40 | * Closes opened bag. 41 | */ 42 | ~Bag(); 43 | 44 | /** \brief Adds a message to the bag under \a topic. 45 | * \param[in] topic Topic to save message under. 46 | * \param[in] msg Message to write. 47 | * \tparam T Type of message. 48 | */ 49 | template 50 | bool addMessage(const std::string &topic, const T &msg) 51 | { 52 | if (mode_ == WRITE) 53 | { 54 | bag_.write(topic, ros::Time::now(), msg); 55 | return true; 56 | } 57 | 58 | return false; 59 | } 60 | 61 | /** \brief Gets messages from an opened bag. Returns all messages of type \a T from a list of 62 | * topics \a topics. 63 | * \param[in] topics List of topics to load messages from. 64 | * \tparam T type of messages to load from topics. 65 | */ 66 | template 67 | std::vector getMessages(const std::vector &topics) 68 | { 69 | std::vector msgs; 70 | 71 | if (mode_ != READ) 72 | return msgs; 73 | 74 | rosbag::View view(bag_, rosbag::TopicQuery(topics)); 75 | for (auto &msg : view) 76 | { 77 | typename T::ConstPtr ptr = msg.instantiate(); 78 | if (ptr != nullptr) 79 | msgs.emplace_back(*ptr); 80 | } 81 | 82 | return msgs; 83 | } 84 | 85 | private: 86 | const Mode mode_; ///< Mode to open file in. 87 | const std::string file_; ///< File opened. 88 | rosbag::Bag bag_; ///< `rosbag` opened. 89 | }; 90 | } // namespace IO 91 | } // namespace robowflex 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/io/colormap.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston, Constantinos Chamzas */ 2 | 3 | #ifndef ROBOWFLEX_IO_COLORMAP_ 4 | #define ROBOWFLEX_IO_COLORMAP_ 5 | 6 | #include 7 | 8 | namespace robowflex 9 | { 10 | namespace color 11 | { 12 | /** \brief Maps a scalar s in [0, 1] to the Viridis colormap. 13 | * \param[in] s Scalar to map. 14 | * \param[out] color Output color. 15 | */ 16 | void viridis(double s, Eigen::Ref color); 17 | 18 | /** \brief Maps a scalar s in [0, 1] to the Cool-Warm colormap. 19 | * \param[in] s Scalar to map. 20 | * \param[out] color Output color. 21 | */ 22 | void coolwarm(double s, Eigen::Ref color); 23 | 24 | /** \brief Maps a scalar s in [0, 1] to the Extended Kindlmann colormap. 25 | * \param[in] s Scalar to map. 26 | * \param[out] color Output color. 27 | */ 28 | void extKindlmann(double s, Eigen::Ref color); 29 | 30 | /** \brief Maps a scalar s in [0, 1] to the Plasma colormap. 31 | * \param[in] s Scalar to map. 32 | * \param[out] color Output color. 33 | */ 34 | void plasma(double s, Eigen::Ref color); 35 | 36 | /** \brief Maps a scalar s in [0, 1] to the Turbo colormap. 37 | * \param[in] s Scalar to map. 38 | * \param[out] color Output color. 39 | */ 40 | void turbo(double s, Eigen::Ref color); 41 | 42 | /** \brief Maps a scalar s in [0, 1] to greyscale. 43 | * \param[in] s Scalar to map. 44 | * \param[out] color Output color. 45 | */ 46 | void grayscale(double s, Eigen::Ref color); 47 | 48 | /** \brief Maps an RGB color to a greyscale color based on luminosity. 49 | * \param[in,out] color Color to convert. 50 | */ 51 | void toGrayscale(Eigen::Ref color); 52 | 53 | // Commonly used named colors. 54 | const static Eigen::Vector4d BLACK{0., 0, 0, 1}; 55 | const static Eigen::Vector4d WHITE{1, 1, 1, 1}; 56 | const static Eigen::Vector4d GRAY{0.5, 0.5, 0.5, 1}; 57 | const static Eigen::Vector4d RED{1, 0, 0, 1}; 58 | const static Eigen::Vector4d PINK{1, 0.37, 0.81, 1}; 59 | const static Eigen::Vector4d PURPLE{0.62, 0.32, 1, 1}; 60 | const static Eigen::Vector4d GREEN{0, 1, 0, 1}; 61 | const static Eigen::Vector4d BLUE{0, 0, 1, 1}; 62 | const static Eigen::Vector4d YELLOW{1, 0.88, 0.12, 1}; 63 | const static Eigen::Vector4d ORANGE{1, 0.6, 0.06, 1}; 64 | const static Eigen::Vector4d BROWN{0.6, 0.5, 0.38, 1}; 65 | 66 | } // namespace color 67 | } // namespace robowflex 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/openrave.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOWFLEX_OPENRAVE_ 2 | #define ROBOWFLEX_OPENRAVE_ 3 | 4 | #include 5 | 6 | namespace robowflex 7 | { 8 | namespace openrave 9 | { 10 | /** \brief Loads a planning_scene from an OpenRAVE Environment XML 11 | * \param[out] planning_scene The output MoveIt message that will be filled with the planning scene 12 | * contents. 13 | * \param[in] file The path to the OpenRAVE environment XML. 14 | * \param[in] model_dir The path to the models directory, which should contain files referenced by 15 | * the passed in file. In OpenRAVE, "the root directory for all models files is the folder openrave is 16 | * launched at." 17 | * \return True on success, false on failure. 18 | */ 19 | bool fromXMLFile(moveit_msgs::PlanningScene &planning_scene, const std::string &file, 20 | const std::string &model_dir); 21 | } // namespace openrave 22 | } // namespace robowflex 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/robowflex.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_ 4 | #define ROBOWFLEX_ 5 | 6 | /** \file */ 7 | 8 | /** \brief Main namespace. Contains all library classes and functions. 9 | */ 10 | namespace robowflex 11 | { 12 | } 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /robowflex_library/include/robowflex_library/util.h: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #ifndef ROBOWFLEX_UTIL_ 4 | #define ROBOWFLEX_UTIL_ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace robowflex 11 | { 12 | /** \brief Exception that contains a message and an error code. 13 | */ 14 | class Exception : public std::exception 15 | { 16 | public: 17 | /** \brief Constructor. 18 | * \param[in] value Error code. 19 | * \param[in] message Error message. 20 | */ 21 | Exception(int value, const std::string &message) : value_(value), message_(message) 22 | { 23 | } 24 | 25 | /** \brief Get error code. 26 | */ 27 | int getValue() const 28 | { 29 | return value_; 30 | } 31 | 32 | /** \brief Get error message. 33 | */ 34 | const std::string &getMessage() const 35 | { 36 | return message_; 37 | } 38 | 39 | virtual const char *what() const throw() 40 | { 41 | return message_.c_str(); 42 | } 43 | 44 | protected: 45 | const int value_; ///< Error code. 46 | const std::string message_; ///< Error message. 47 | }; 48 | 49 | /** \brief RAII-pattern for starting up ROS. 50 | */ 51 | class ROS 52 | { 53 | public: 54 | /** \brief Constructor. Start-up ROS. 55 | * If Boost version is greater than 1.64, `rosmaster` is started if it is not already running. A 56 | * signal handler or SIGINT and SIGSEGV is installed to gracefully exit. 57 | * \param[in] argc Argument count forwarded to ros::init 58 | * \param[in] argv Arguments forwarded to ros::init 59 | * \param[in] name Name of ROS node. 60 | * \param[in] threads Threads to use for ROS spinning. If 0 no spinner is created. 61 | */ 62 | ROS(int argc, char **argv, const std::string &name = "robowflex", unsigned int threads = 1); 63 | 64 | /** \brief Destructor. Shutdown ROS. 65 | */ 66 | ~ROS(); 67 | 68 | /** \brief Get command-line arguments without ROS parameters. 69 | * \return Vector of command line arguments as strings. 70 | */ 71 | std::vector getArgs() const; 72 | 73 | /** \brief Waits for the process to be killed via some means (normally Ctrl-C) 74 | */ 75 | void wait() const; 76 | 77 | private: 78 | int argc_; ///< Argument count. 79 | char **argv_; ///< Arguments. 80 | }; 81 | 82 | /** \brief Trigger a SEGSEGV. 83 | */ 84 | void explode(); 85 | } // namespace robowflex 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /robowflex_library/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_library 4 | 1.3.0 5 | Core Robowflex Library 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | pkg-config 16 | 17 | boost 18 | yaml-cpp 19 | tinyxml2 20 | eigen 21 | hdf5 22 | 23 | std_msgs 24 | sensor_msgs 25 | geometry_msgs 26 | visualization_msgs 27 | shape_msgs 28 | octomap_msgs 29 | trajectory_msgs 30 | object_recognition_msgs 31 | moveit_msgs 32 | 33 | roscpp 34 | rosbag 35 | urdf 36 | srdfdom 37 | pluginlib 38 | geometric_shapes 39 | moveit_core 40 | moveit_ros_planning 41 | tf2_ros 42 | xmlrpcpp 43 | 44 | moveit_kinematics 45 | 46 | -------------------------------------------------------------------------------- /robowflex_library/scripts/fetch_chomp.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace robowflex; 11 | 12 | /* \file fetch_chomp.cpp 13 | * A basic script that demonstrates using MoveIt's built-in CHOMP planner. The 14 | * resulting trajectory is output to a YAML file. This file can be visualized 15 | * using Blender. See the corresponding robowflex_visualization readme. 16 | */ 17 | 18 | static const std::string GROUP = "arm_with_torso"; 19 | 20 | int main(int argc, char **argv) 21 | { 22 | // Startup ROS 23 | ROS ros(argc, argv); 24 | 25 | // Create the default Fetch robot. 26 | auto fetch = std::make_shared(); 27 | fetch->initialize(); 28 | 29 | // Create an empty scene. 30 | auto scene = std::make_shared(fetch); 31 | 32 | // Create a CHOMP planner for Fetch. 33 | auto planner = std::make_shared(fetch); 34 | planner->initialize(); 35 | 36 | // Create a motion planning request with a pose goal. 37 | MotionRequestBuilder request(planner, GROUP); 38 | fetch->setGroupState(GROUP, {0.265, 0.201, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); 39 | request.setStartConfiguration(fetch->getScratchState()); 40 | 41 | fetch->setGroupState(GROUP, {0.265, 1.301, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); 42 | request.setGoalConfiguration(fetch->getScratchState()); 43 | 44 | request.setConfig("RRTConnect"); 45 | 46 | // Do motion planning! 47 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 48 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 49 | return 1; 50 | 51 | // Create a trajectory object for better manipulation. 52 | auto trajectory = std::make_shared(res.trajectory_); 53 | 54 | // Output path to a file for visualization. 55 | trajectory->toYAMLFile("fetch_chomp_path.yml"); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /robowflex_library/scripts/fetch_test.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace robowflex; 10 | 11 | /* \file fetch_test.cpp 12 | * A simple script that demonstrates motion planning with the Fetch robot. The 13 | * resulting trajectory is output to a YAML file. This file can be visualized 14 | * using Blender. See the corresponding robowflex_visualization readme. 15 | */ 16 | 17 | static const std::string GROUP = "arm_with_torso"; 18 | 19 | int main(int argc, char **argv) 20 | { 21 | // Startup ROS 22 | ROS ros(argc, argv); 23 | 24 | // Create the default Fetch robot. 25 | auto fetch = std::make_shared(); 26 | fetch->initialize(); 27 | 28 | // Create an empty scene. 29 | auto scene = std::make_shared(fetch); 30 | 31 | // Create the default planner for the Fetch. 32 | auto planner = std::make_shared(fetch, "default"); 33 | planner->initialize(); 34 | 35 | // Sets the Fetch's base pose. 36 | fetch->setBasePose(1, 1, 0.5); 37 | 38 | // Sets the Fetch's head pose to look at a point. 39 | fetch->pointHead({2, 1, 1.5}); 40 | 41 | // Create a motion planning request with a pose goal. 42 | MotionRequestBuilder request(planner, GROUP); 43 | fetch->setGroupState(GROUP, {0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0}); // Stow 44 | request.setStartConfiguration(fetch->getScratchState()); 45 | 46 | fetch->setGroupState(GROUP, {0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); // Unfurl 47 | request.setGoalConfiguration(fetch->getScratchState()); 48 | 49 | request.setConfig("RRTConnect"); 50 | 51 | // Do motion planning! 52 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 53 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 54 | return 1; 55 | 56 | // Create a trajectory object for better manipulation. 57 | auto trajectory = std::make_shared(res.trajectory_); 58 | 59 | // Output path to a file for visualization. 60 | trajectory->toYAMLFile("fetch_path.yml"); 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /robowflex_library/scripts/plugin_io.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | using namespace robowflex; 9 | 10 | /* \file plugin_io.cpp 11 | * Demonstrates how to use the plugin loader helper class to load some MoveIt 12 | * plugins. 13 | */ 14 | 15 | int main(int argc, char **argv) 16 | { 17 | ROS ros(argc, argv); 18 | 19 | auto plugin1 = IO::PluginManager::load( // 20 | "moveit_core", "default_planner_request_adapters/AddTimeParameterization"); 21 | 22 | auto plugin2 = IO::PluginManager::load( // 23 | "moveit_core", "default_planner_request_adapters/FixStartStateBounds"); 24 | 25 | auto plugin3 = IO::PluginManager::load( // 26 | "moveit_core", "default_planner_request_adapters/FixStartStateCollision"); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_benchmark.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace robowflex; 11 | 12 | /* \file ur5_benchmark.cpp 13 | * A basic script that demonstrates benchmarking with the UR5 robot. 14 | * Benchmarking output is saved in the OMPL format. See 15 | * https://ompl.kavrakilab.org/benchmark.html for more information on the 16 | * benchmark data format and how to use. http://plannerarena.org/ can be used to 17 | * visualize results. 18 | */ 19 | 20 | int main(int argc, char **argv) 21 | { 22 | // Startup ROS 23 | ROS ros(argc, argv); 24 | 25 | // Create the default UR5 robot. 26 | auto ur5 = std::make_shared(); 27 | ur5->initialize(); 28 | 29 | // Create an empty scene. 30 | auto scene = std::make_shared(ur5); 31 | 32 | // Create the default planner for the UR5. 33 | auto planner = std::make_shared(ur5); 34 | planner->initialize(); 35 | 36 | // Create a motion planning request with a joint position goal. 37 | MotionRequestBuilderPtr joint_request(new MotionRequestBuilder(planner, "manipulator")); 38 | joint_request->setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 39 | joint_request->setGoalConfiguration({-0.39, -0.69, -2.12, 2.82, -0.39, 0}); 40 | 41 | // Create a motion planning request with a pose goal. 42 | MotionRequestBuilderPtr pose_request(new MotionRequestBuilder(planner, "manipulator")); 43 | pose_request->setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 44 | 45 | RobotPose pose = RobotPose::Identity(); 46 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 47 | Eigen::Quaterniond orn{0, 0, 1, 0}; 48 | 49 | pose_request->setGoalRegion("ee_link", "world", // links 50 | pose, Geometry::makeSphere(0.1), // position 51 | orn, {0.01, 0.01, 0.01} // orientation 52 | ); 53 | 54 | Profiler::Options options; 55 | Experiment experiment("ur5_demo", // Name of experiment 56 | options, // Options for internal profiler 57 | 5.0, // Timeout allowed for ALL queries 58 | 50); // Number of trials 59 | 60 | experiment.addQuery("joint", scene, planner, joint_request); 61 | experiment.addQuery("pose", scene, planner, pose_request); 62 | 63 | auto dataset = experiment.benchmark(4); 64 | 65 | OMPLPlanDataSetOutputter output("robowflex_ur5_demo"); 66 | output.dump(*dataset); 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_cylinder.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace robowflex; 12 | 13 | /* \file ur5_cylinder.cpp 14 | * A simple script that demonstrates how to use RViz with Robowflex with the UR5 15 | * robot. Here, the goal is created using the `addCylinderSideGrasp` function, 16 | * which creates a more complicated grasp to grasp any side of a cylinder. See 17 | * https://kavrakilab.github.io/robowflex/rviz.html for how to use RViz 18 | * visualization. Here, the scene, the pose goal, and motion plan displayed in 19 | * RViz. 20 | */ 21 | 22 | int main(int argc, char **argv) 23 | { 24 | // Startup ROS 25 | ROS ros(argc, argv); 26 | 27 | // Create the default UR5 robot. 28 | auto ur5 = std::make_shared(); 29 | ur5->initialize(); 30 | 31 | // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by default. 32 | IO::RVIZHelper rviz(ur5); 33 | 34 | RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)..."); 35 | std::cin.get(); 36 | 37 | // Create the cylinder we want to grasp 38 | RobotPose pose = RobotPose::Identity(); 39 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 40 | 41 | auto cylinder = Geometry::makeCylinder(0.025, 0.1); 42 | 43 | // Create an empty scene. 44 | auto scene = std::make_shared(ur5); 45 | scene->updateCollisionObject("cylinder", cylinder, pose); 46 | 47 | // Visualize the scene. 48 | rviz.updateScene(scene); 49 | 50 | // Create the default planner for the UR5. 51 | auto planner = std::make_shared(ur5); 52 | planner->initialize(); 53 | 54 | // Create a motion planning request with a pose goal. 55 | MotionRequestBuilder request(planner, "manipulator"); 56 | request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 57 | 58 | request.addCylinderSideGrasp("ee_link", "world", // 59 | pose, cylinder, // 60 | 0.15, 0.04, 16); // 61 | 62 | rviz.addGoalMarker("goal", request); // Visualize the grasping regions 63 | rviz.updateMarkers(); 64 | 65 | RBX_INFO("Scene and Goal displayed! Press enter to plan..."); 66 | std::cin.get(); 67 | 68 | // Do motion planning! 69 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 70 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 71 | return 1; 72 | 73 | // Publish the trajectory to a topic to display in RViz 74 | rviz.updateTrajectory(res); 75 | 76 | RBX_INFO("Press enter to remove goal and scene."); 77 | std::cin.get(); 78 | 79 | rviz.removeMarker("goal"); 80 | rviz.updateMarkers(); 81 | 82 | rviz.removeScene(); 83 | 84 | RBX_INFO("Press enter to exit."); 85 | std::cin.get(); 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_ik.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace robowflex; 9 | 10 | /* \file ur5_ik.cpp 11 | * Simple demonstration of how to use IK. 12 | */ 13 | 14 | int main(int argc, char **argv) 15 | { 16 | // Startup ROS 17 | ROS ros(argc, argv); 18 | 19 | // Create the default UR5 robot. 20 | auto ur5 = std::make_shared(); 21 | ur5->initialize(); 22 | ur5->setGroupState("manipulator", {0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 23 | 24 | // Use IK to shift robot arm over by desired amount. 25 | RobotPose goal_pose = ur5->getLinkTF("ee_link"); 26 | goal_pose.translate(Eigen::Vector3d{0.0, -0.3, 0.0}); 27 | 28 | if (not ur5->setFromIK(Robot::IKQuery("manipulator", goal_pose))) 29 | { 30 | RBX_ERROR("IK Failed"); 31 | return 1; 32 | } 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_io.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace robowflex; 9 | 10 | /* \file ur5_io.cpp 11 | * Simple test of IO::Bag for loading and saving messages. 12 | */ 13 | 14 | int main(int argc, char **argv) 15 | { 16 | // Startup ROS 17 | ROS ros(argc, argv); 18 | 19 | // Create the default UR5 robot. 20 | auto ur5 = std::make_shared(); 21 | ur5->initialize(); 22 | 23 | // Load a scene from a file. 24 | auto scene = std::make_shared(ur5); 25 | scene->fromYAMLFile("package://robowflex_library/yaml/test.yml"); 26 | 27 | // Output scene to a rosbag file. 28 | { 29 | IO::Bag bag_out("scene.bag", IO::Bag::WRITE); 30 | bag_out.addMessage("scene", scene->getMessage()); 31 | } 32 | 33 | // Load the same scene from the rosbag file. 34 | { 35 | IO::Bag bag_in("scene.bag", IO::Bag::READ); 36 | 37 | // Load all `moveit_msgs::PlanningScene` from the topic `scene`. 38 | auto msgs = bag_in.getMessages({"scene"}); 39 | } 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_pool.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace robowflex; 11 | 12 | /* \file ur5_pool.cpp 13 | * Demonstration of PoolPlanner with the UR5. Here, many planning requests are 14 | * executed asynchronously. 15 | */ 16 | 17 | int main(int argc, char **argv) 18 | { 19 | // Startup ROS 20 | ROS ros(argc, argv); 21 | 22 | // Create the default UR5 robot. 23 | auto ur5 = std::make_shared(); 24 | ur5->initialize(); 25 | 26 | // Create an empty scene. 27 | auto scene = std::make_shared(ur5); 28 | 29 | // Create a pool of default planners for the UR5. 30 | auto planner = std::make_shared(ur5); 31 | planner->initialize(); 32 | 33 | // Create a motion planning request with a pose goal. 34 | MotionRequestBuilder request(planner, "manipulator"); 35 | request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 36 | 37 | RobotPose pose = RobotPose::Identity(); 38 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 39 | Eigen::Quaterniond orn{0, 0, 1, 0}; 40 | 41 | request.setGoalRegion("ee_link", "world", // links 42 | pose, Geometry::makeSphere(0.1), // position 43 | orn, {0.01, 0.01, 0.01} // orientation 44 | ); 45 | 46 | // Submit a blocking call for planning. This plan is executed in a different thread 47 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 48 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 49 | return 1; 50 | 51 | // Submit a set of asynchronous planning calls. 52 | auto job1 = planner->submit(scene, request.getRequest()); 53 | auto job2 = planner->submit(scene, request.getRequest()); 54 | auto job3 = planner->submit(scene, request.getRequest()); 55 | auto job4 = planner->submit(scene, request.getRequest()); 56 | auto job5 = planner->submit(scene, request.getRequest()); 57 | auto job6 = planner->submit(scene, request.getRequest()); 58 | auto job7 = planner->submit(scene, request.getRequest()); 59 | auto job8 = planner->submit(scene, request.getRequest()); 60 | 61 | // Cancel a job (if already running, nothing happens, but if canceled before execution the job is skipped) 62 | job5->cancel(); 63 | 64 | // Wait for a job to complete 65 | job1->wait(); 66 | 67 | // Get results of plans. These block until results are available. 68 | planning_interface::MotionPlanResponse res1 = job1->get(); 69 | planning_interface::MotionPlanResponse res2 = job2->get(); 70 | planning_interface::MotionPlanResponse res3 = job3->get(); 71 | planning_interface::MotionPlanResponse res4 = job4->get(); 72 | 73 | // We canceled job5, so no result is guaranteed. 74 | // planning_interface::MotionPlanResponse res5 = job5->get(); 75 | 76 | planning_interface::MotionPlanResponse res6 = job6->get(); 77 | planning_interface::MotionPlanResponse res7 = job7->get(); 78 | planning_interface::MotionPlanResponse res8 = job8->get(); 79 | 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_test.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace robowflex; 11 | 12 | /* \file fetch_test.cpp 13 | * A simple script that demonstrates motion planning with the UR5 robot. Here, 14 | * two planners are created: the default OMPL planner, and the default OMPL 15 | * planner but with simplified solutions disabled. 16 | */ 17 | 18 | int main(int argc, char **argv) 19 | { 20 | // Startup ROS 21 | ROS ros(argc, argv); 22 | 23 | // Create the default UR5 robot. 24 | auto ur5 = std::make_shared(); 25 | ur5->initialize(); 26 | 27 | // Create an empty scene. 28 | auto scene = std::make_shared(ur5); 29 | 30 | // Create the default planner for the UR5. 31 | auto default_planner = std::make_shared(ur5, "default"); 32 | default_planner->initialize(); 33 | 34 | // Create the a planner for the UR5, and disable simplification. 35 | auto simple_planner = std::make_shared(ur5, "simple"); 36 | 37 | OMPL::Settings settings; 38 | settings.simplify_solutions = false; 39 | 40 | simple_planner->initialize(settings); 41 | 42 | // Run a motion plan for each planner. 43 | for (const auto &planner : {default_planner, simple_planner}) 44 | { 45 | // Create a motion planning request with a pose goal. 46 | MotionRequestBuilder request(planner, "manipulator"); 47 | request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 48 | 49 | RobotPose pose = RobotPose::Identity(); 50 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 51 | Eigen::Quaterniond orn{0, 0, 1, 0}; 52 | 53 | request.setGoalRegion("ee_link", "world", // links 54 | pose, Geometry::makeSphere(0.1), // position 55 | orn, {0.01, 0.01, 0.01} // orientation 56 | ); 57 | 58 | // Do motion planning! 59 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 60 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 61 | return 1; 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /robowflex_library/scripts/ur5_visualization.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace robowflex; 12 | 13 | /* \file ur5_visualization.cpp 14 | * A simple script that demonstrates how to use RViz with Robowflex with the 15 | * UR5 robot. See https://kavrakilab.github.io/robowflex/rviz.html for how to 16 | * use RViz visualization. Here, the scene, the pose goal, and motion plan 17 | * displayed in RViz. 18 | */ 19 | 20 | int main(int argc, char **argv) 21 | { 22 | // Startup ROS 23 | ROS ros(argc, argv); 24 | 25 | // Create the default UR5 robot. 26 | auto ur5 = std::make_shared(); 27 | ur5->initialize(); 28 | 29 | // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by default. 30 | IO::RVIZHelper rviz(ur5); 31 | 32 | RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)..."); 33 | std::cin.get(); 34 | 35 | // Load a scene from a YAML file. 36 | auto scene = std::make_shared(ur5); 37 | scene->fromYAMLFile("package://robowflex_library/yaml/test.yml"); 38 | 39 | // Visualize the scene. 40 | rviz.updateScene(scene); 41 | 42 | // Create the default planner for the UR5. 43 | auto planner = std::make_shared(ur5); 44 | planner->initialize(); 45 | 46 | // Create a motion planning request with a pose goal. 47 | MotionRequestBuilder request(planner, "manipulator"); 48 | request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 49 | 50 | RobotPose pose = RobotPose::Identity(); 51 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 52 | Eigen::Quaterniond orn{0, 0, 1, 0}; 53 | 54 | auto region = Geometry::makeSphere(0.1); 55 | 56 | request.setGoalRegion("ee_link", "world", // links 57 | pose, region, // position 58 | orn, {0.1, 0.1, 0.1} // orientation 59 | ); 60 | 61 | rviz.addGoalMarker("goal", request); 62 | rviz.updateMarkers(); 63 | 64 | RBX_INFO("Scene and Goal displayed! Press enter to plan..."); 65 | std::cin.get(); 66 | 67 | // Do motion planning! 68 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 69 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 70 | return 1; 71 | 72 | // Publish the trajectory to a topic to display in RViz 73 | rviz.updateTrajectory(res); 74 | 75 | RBX_INFO("Press enter to remove goal and scene."); 76 | std::cin.get(); 77 | 78 | rviz.removeMarker("goal"); 79 | rviz.updateMarkers(); 80 | 81 | rviz.removeScene(); 82 | 83 | RBX_INFO("Press enter to exit."); 84 | std::cin.get(); 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /robowflex_library/scripts/wam7_benchmark.cpp: -------------------------------------------------------------------------------- 1 | /** \author Bryce Willey */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace robowflex; 12 | 13 | /* \file wam7_benchmark.cpp 14 | * A basic script that demonstrates benchmarking with the WAM7 arm. Here, a 15 | * scene is loaded from an OpenRAVE scene file. Benchmarking output is saved in 16 | * a JSON file and a ROS bag of trajectories. 17 | */ 18 | 19 | int main(int argc, char **argv) 20 | { 21 | // Startup ROS 22 | ROS ros(argc, argv); 23 | 24 | // Create a WAM7 robot, specifying all necessary files. 25 | auto wam7 = std::make_shared("wam7"); 26 | wam7->initialize("package://barrett_model/robots/wam7_bhand.urdf.xacro", // urdf 27 | "package://barrett_wam_moveit_config/config/wam7_hand.srdf", // srdf 28 | "package://barrett_wam_moveit_config/config/joint_limits.yaml", // joint limits 29 | "package://barrett_wam_moveit_config/config/kinematics.yaml" // kinematics 30 | ); 31 | 32 | // Load kinematics for the WAM7 arm. 33 | wam7->loadKinematics("arm"); 34 | 35 | // Create an empty scene. 36 | auto scene = std::make_shared(wam7); 37 | scene->fromOpenRAVEXMLFile("package://optplanners_openrave/scripts/data/envs/wam7_realistic.env.xml"); 38 | 39 | // Create the default OMPL planner, with the WAM7 planning configuration. 40 | auto planner = std::make_shared(wam7); 41 | planner->initialize("package://barrett_wam_moveit_config/config/ompl_planning.yaml" // planner config 42 | ); 43 | 44 | // Create a motion planning request with a joint position goal. 45 | MotionRequestBuilderPtr request(new MotionRequestBuilder(planner, "arm")); 46 | request->setStartConfiguration({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); 47 | request->setGoalConfiguration({0.0, 1.89, 0.0, -0.3, 1.3, 0.0, 0.2}); 48 | 49 | Profiler::Options options; 50 | options.progress_update_rate = 0.1; 51 | options.metrics = Profiler::LENGTH; 52 | 53 | Experiment experiment("wam7_demo", options, 5.0, 10); 54 | experiment.addQuery("ompl", scene, planner, request); 55 | 56 | auto dataset = experiment.benchmark(4); 57 | 58 | JSONPlanDataSetOutputter json_output("test_log.json"); 59 | json_output.dump(*dataset); 60 | 61 | TrajectoryPlanDataSetOutputter traj_output("test_log.bag"); 62 | traj_output.dump(*dataset); 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /robowflex_library/scripts/wam7_test.cpp: -------------------------------------------------------------------------------- 1 | /** \author Bryce Willey */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace robowflex; 10 | 11 | int main(int argc, char **argv) 12 | { 13 | // Startup ROS 14 | ROS ros(argc, argv); 15 | 16 | // Create a WAM7 robot, specifying all necessary files. 17 | auto wam7 = std::make_shared("wam7"); 18 | wam7->initialize("package://barrett_model/robots/wam_7dof_wam_bhand.urdf.xacro", // urdf 19 | "package://barrett_wam_moveit_config/config/wam7_hand.srdf", // srdf 20 | "package://barrett_wam_moveit_config/config/joint_limits.yaml", // joint limits 21 | "package://barrett_wam_moveit_config/config/kinematics.yaml" // kinematics 22 | ); 23 | 24 | // Load kinematics for the WAM7 arm. 25 | wam7->loadKinematics("arm"); 26 | 27 | // Create an empty scene. 28 | auto scene = std::make_shared(wam7); 29 | 30 | // Create the default OMPL planner with path simplification disabled, with the WAM7 planning 31 | // configuration. 32 | auto planner = std::make_shared(wam7); 33 | 34 | OMPL::Settings settings; 35 | settings.simplify_solutions = false; 36 | planner->initialize("package://barrett_wam_moveit_config/config/ompl_planning.yaml", // planner config 37 | settings); 38 | 39 | // Create a motion planning request with a joint position goal. 40 | MotionRequestBuilder request(planner, "arm"); 41 | request.setStartConfiguration({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); 42 | request.setGoalConfiguration({0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0}); 43 | request.setConfig("BKPIECE"); 44 | 45 | // Do motion planning! 46 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 47 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 48 | return 1; 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /robowflex_library/src/detail/ur5.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace robowflex; 8 | 9 | const std::string // 10 | UR5Robot::DEFAULT_URDF{"package://ur_description/urdf/ur5_robotiq_robot_limited.urdf.xacro"}; 11 | const std::string // 12 | UR5Robot::DEFAULT_SRDF{"package://ur5_robotiq85_moveit_config/config/ur5_robotiq85.srdf"}; 13 | const std::string // 14 | UR5Robot::DEFAULT_LIMITS{"package://ur5_robotiq85_moveit_config/config/joint_limits.yaml"}; 15 | const std::string // 16 | UR5Robot::DEFAULT_KINEMATICS{"package://ur5_robotiq85_moveit_config/config/kinematics.yaml"}; 17 | const std::string // 18 | OMPL::UR5OMPLPipelinePlanner::DEFAULT_CONFIG{ 19 | "package://ur5_robotiq85_moveit_config/config/ompl_planning.yaml" // 20 | }; 21 | 22 | const std::string // 23 | UR5Robot::RESOURCE_URDF{"package://robowflex_resources/ur/robots/ur5_robotiq_robot_limited.urdf.xacro"}; 24 | const std::string // 25 | UR5Robot::RESOURCE_SRDF{"package://robowflex_resources/ur/config/ur5/ur5_robotiq85.srdf.xacro"}; 26 | const std::string // 27 | UR5Robot::RESOURCE_LIMITS{"package://robowflex_resources/ur/config/ur5/joint_limits.yaml"}; 28 | const std::string // 29 | UR5Robot::RESOURCE_KINEMATICS{"package://robowflex_resources/ur/config/ur5/kinematics.yaml"}; 30 | const std::string // 31 | OMPL::UR5OMPLPipelinePlanner::RESOURCE_CONFIG{ 32 | "package://robowflex_resources/ur/config/ur5/ompl_planning.yaml" // 33 | }; 34 | 35 | UR5Robot::UR5Robot() : Robot("ur5") 36 | { 37 | } 38 | 39 | bool UR5Robot::initialize() 40 | { 41 | bool success = false; 42 | 43 | // First attempt the `robowflex_resources` package, then attempt the "actual" resource files. 44 | if (IO::resolvePackage(RESOURCE_URDF).empty() or IO::resolvePackage(RESOURCE_SRDF).empty()) 45 | { 46 | RBX_INFO("Initializing UR5 with `ur_description`"); 47 | success = Robot::initialize(DEFAULT_URDF, DEFAULT_SRDF, DEFAULT_LIMITS, DEFAULT_KINEMATICS); 48 | } 49 | else 50 | { 51 | RBX_INFO("Initializing UR5 with `robowflex_resources`"); 52 | success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS); 53 | } 54 | 55 | loadKinematics("manipulator"); 56 | 57 | return success; 58 | } 59 | 60 | OMPL::UR5OMPLPipelinePlanner::UR5OMPLPipelinePlanner(const RobotPtr &robot, const std::string &name) 61 | : OMPLPipelinePlanner(robot, name) 62 | { 63 | } 64 | 65 | bool OMPL::UR5OMPLPipelinePlanner::initialize(const Settings &settings, 66 | const std::vector &adapters) 67 | { 68 | if (IO::resolvePackage(RESOURCE_CONFIG).empty()) 69 | return OMPLPipelinePlanner::initialize(DEFAULT_CONFIG, settings, DEFAULT_PLUGIN, adapters); 70 | return OMPLPipelinePlanner::initialize(RESOURCE_CONFIG, settings, DEFAULT_PLUGIN, adapters); 71 | } 72 | -------------------------------------------------------------------------------- /robowflex_library/src/id.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | 6 | using namespace robowflex; 7 | 8 | ID::ID() : id_(IO::generateUUID()), version_(0) 9 | { 10 | } 11 | 12 | const std::string &ID::getID() const 13 | { 14 | return id_; 15 | } 16 | 17 | std::size_t ID::getVersion() const 18 | { 19 | return version_.load(); 20 | } 21 | 22 | ID::Key ID::getKey() const 23 | { 24 | return {getID(), getVersion()}; 25 | } 26 | 27 | ID::Key ID::getNullKey() 28 | { 29 | return {"NULL", 0}; 30 | } 31 | 32 | bool ID::operator==(const ID &b) const 33 | { 34 | return getID() == b.getID() and getVersion() == b.getVersion(); 35 | } 36 | 37 | bool ID::operator==(const ID::Key &b) const 38 | { 39 | return getID() == b.first and getVersion() == b.second; 40 | } 41 | 42 | void ID::incrementVersion() 43 | { 44 | version_++; 45 | } 46 | 47 | bool robowflex::compareIDs(const ID &a, const ID &b) 48 | { 49 | return a == b; 50 | } 51 | 52 | bool robowflex::compareIDs(const IDPtr &a, const IDPtr &b) 53 | { 54 | return compareIDs(*a, *b); 55 | } 56 | 57 | bool robowflex::compareIDs(const IDConstPtr &a, const IDConstPtr &b) 58 | { 59 | return compareIDs(*a, *b); 60 | } 61 | 62 | bool robowflex::compareIDs(const ID &a, const ID::Key &b) 63 | { 64 | return a == b; 65 | } 66 | 67 | bool robowflex::compareIDs(const IDPtr &a, const ID::Key &b) 68 | { 69 | return compareIDs(*a, b); 70 | } 71 | 72 | bool robowflex::compareIDs(const IDConstPtr &a, const ID::Key &b) 73 | { 74 | return compareIDs(*a, b); 75 | } 76 | 77 | bool robowflex::compareIDs(const ID::Key &a, const ID::Key &b) 78 | { 79 | return a == b; 80 | } 81 | -------------------------------------------------------------------------------- /robowflex_library/src/log.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | using namespace robowflex; 6 | 7 | namespace 8 | { 9 | void setLoggerLevel(ros::console::Level level) 10 | { 11 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level)) 12 | ros::console::notifyLoggerLevelsChanged(); 13 | } 14 | } // namespace 15 | 16 | std::string log::formatRecurse(boost::format &f) 17 | { 18 | return boost::str(f); 19 | } 20 | 21 | void log::showUpToFatal() 22 | { 23 | setLoggerLevel(ros::console::levels::Fatal); 24 | } 25 | 26 | void log::showUpToError() 27 | { 28 | setLoggerLevel(ros::console::levels::Error); 29 | } 30 | 31 | void log::showUpToWarning() 32 | { 33 | setLoggerLevel(ros::console::levels::Warn); 34 | } 35 | 36 | void log::showUpToInfo() 37 | { 38 | setLoggerLevel(ros::console::levels::Info); 39 | } 40 | 41 | void log::showUpToDebug() 42 | { 43 | setLoggerLevel(ros::console::levels::Debug); 44 | } 45 | -------------------------------------------------------------------------------- /robowflex_library/src/pool.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | using namespace robowflex; 6 | 7 | /// 8 | /// Joblet 9 | /// 10 | 11 | void Pool::Joblet::cancel() 12 | { 13 | canceled = true; 14 | } 15 | 16 | bool Pool::Joblet::isCancled() const 17 | { 18 | return canceled; 19 | } 20 | 21 | /// 22 | /// Pool 23 | /// 24 | 25 | Pool::Pool(unsigned int n) : active_(true) 26 | { 27 | for (unsigned int i = 0; i < n; ++i) 28 | threads_.emplace_back(std::bind(&Pool::run, this)); 29 | } 30 | 31 | Pool::~Pool() 32 | { 33 | active_ = false; 34 | cv_.notify_all(); 35 | 36 | for (auto &thread : threads_) 37 | thread.join(); 38 | } 39 | 40 | unsigned int Pool::getThreadCount() const 41 | { 42 | return threads_.size(); 43 | } 44 | 45 | void Pool::run() 46 | { 47 | while (active_) 48 | { 49 | std::unique_lock lock(mutex_); 50 | cv_.wait(lock, [&] { return (active_ && !jobs_.empty()) || !active_; }); 51 | 52 | if (!active_) 53 | break; 54 | 55 | auto job = jobs_.front(); 56 | jobs_.pop(); 57 | 58 | lock.unlock(); 59 | 60 | // Ignore canceled jobs. 61 | if (!job->isCancled()) 62 | job->execute(); 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /robowflex_library/src/util.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #if IS_BOOST_164 12 | #include 13 | #endif 14 | 15 | using namespace robowflex; 16 | 17 | namespace 18 | { 19 | static std::shared_ptr SPINNER; 20 | 21 | #if IS_BOOST_164 22 | static boost::process::child ROSCORE; 23 | static bool ROSCORE_INIT{false}; 24 | #endif 25 | 26 | void shutdown(int sig) 27 | { 28 | if (sig) 29 | RBX_INFO("Shutting down with signal %s.", strsignal(sig)); 30 | else 31 | RBX_INFO("Shutting down."); 32 | 33 | if (SPINNER) 34 | SPINNER->stop(); 35 | 36 | // Some stuff for later 37 | ros::shutdown(); 38 | 39 | #if IS_BOOST_164 40 | if (ROSCORE_INIT) 41 | ROSCORE.terminate(); 42 | #endif 43 | 44 | exit(0); 45 | } 46 | 47 | void startup() 48 | { 49 | if (!ros::master::check()) 50 | { 51 | RBX_ERROR("rosmaster is not running!"); 52 | #if IS_BOOST_164 53 | RBX_WARN("Booting rosmaster..."); 54 | ROSCORE = boost::process::child("rosmaster", // 55 | boost::process::std_in.close(), // 56 | boost::process::std_out > boost::process::null, // 57 | boost::process::std_err > boost::process::null // 58 | ); 59 | 60 | ROSCORE_INIT = true; 61 | #endif 62 | } 63 | 64 | ros::start(); 65 | } 66 | } // namespace 67 | 68 | ROS::ROS(int argc, char **argv, const std::string &name, unsigned int threads) : argc_(argc), argv_(argv) 69 | { 70 | ros::init(argc, argv, name, ros::init_options::NoSigintHandler); 71 | startup(); 72 | 73 | signal(SIGINT, shutdown); 74 | signal(SIGSEGV, shutdown); 75 | 76 | if (threads) 77 | { 78 | SPINNER.reset(new ros::AsyncSpinner(threads)); 79 | SPINNER->start(); 80 | } 81 | } 82 | 83 | ROS::~ROS() 84 | { 85 | shutdown(0); 86 | } 87 | 88 | std::vector ROS::getArgs() const 89 | { 90 | std::vector args; 91 | ros::removeROSArgs(argc_, argv_, args); 92 | 93 | return args; 94 | } 95 | 96 | void ROS::wait() const 97 | { 98 | ros::waitForShutdown(); 99 | } 100 | 101 | void robowflex::explode() 102 | { 103 | raise(SIGSEGV); 104 | } 105 | -------------------------------------------------------------------------------- /robowflex_library/test/robot_scene.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston, Carlos Quintero Pena */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace robowflex; 13 | 14 | namespace 15 | { 16 | UR5RobotPtr getUR5Robot() 17 | { 18 | static UR5RobotPtr ur5; 19 | 20 | // Create the default UR5 robot. 21 | if (not ur5) 22 | { 23 | ur5 = std::make_shared(); 24 | ur5->initialize(); 25 | } 26 | 27 | return ur5; 28 | } 29 | } // namespace 30 | 31 | TEST(Scene, detatchObject) 32 | { 33 | auto ur5 = getUR5Robot(); 34 | 35 | // Create an empty scene and add a cylinder to it 36 | auto scene = std::make_shared(ur5); 37 | 38 | const auto cylinder_position = Eigen::Vector3d{-0.270, 0.42, 1.1572}; // Initial cylinder position. 39 | const auto shift = Eigen::Vector3d{0.0, -0.3, 0.0}; // Desired offset movement of cylinder. 40 | const auto desired_pose = cylinder_position + shift; // Desired final position of cylinder. 41 | 42 | // Add cylinder to scene. 43 | scene->updateCollisionObject( 44 | "cylinder", // 45 | Geometry::makeCylinder(0.025, 0.1), // 46 | TF::createPoseQ(cylinder_position, Eigen::Quaterniond{0.707, 0.0, 0.707, 0.0})); 47 | 48 | // Set start configuration to the robot scratch state and attach the cylinder to it 49 | ur5->setGroupState("manipulator", {0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 50 | scene->attachObject(*ur5->getScratchState(), "cylinder"); 51 | 52 | // Use IK to shift robot arm over by desired amount. 53 | RobotPose goal_pose = ur5->getLinkTF("ee_link"); 54 | goal_pose.translate(shift); 55 | 56 | bool success = ur5->setFromIK(Robot::IKQuery("manipulator", goal_pose)); 57 | ASSERT_TRUE(success); 58 | 59 | // Get new position for the cylinder and compare to its theoretical value. 60 | scene->detachObject(*ur5->getScratchState(), "cylinder"); 61 | auto new_pose = scene->getObjectPose("cylinder"); 62 | auto diff = desired_pose - new_pose.translation(); 63 | 64 | ASSERT_NEAR(diff.norm(), 0., 0.005); 65 | } 66 | 67 | int main(int argc, char **argv) 68 | { 69 | // Startup ROS 70 | ROS ros(argc, argv); 71 | 72 | testing::InitGoogleTest(&argc, argv); 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /robowflex_library/test/yaml.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace robowflex; 10 | 11 | TEST(YAML, isNode) 12 | { 13 | YAML::Node node = YAML::Load("key1: null\nkey2: \"test\""); 14 | ASSERT_EQ(false, IO::isNode(node["key1"])); 15 | ASSERT_EQ(true, IO::isNode(node["key2"])); 16 | ASSERT_EQ(false, IO::isNode(node["key3"])); 17 | } 18 | 19 | int main(int argc, char **argv) 20 | { 21 | testing::InitGoogleTest(&argc, argv); 22 | return RUN_ALL_TESTS(); 23 | } 24 | -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0001.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | header: 3 | frame_id: "" 4 | max_corner: [1, 1, 1] 5 | min_corner: [-1, -1, -1] 6 | start_state: 7 | joint_state: 8 | header: 9 | frame_id: base_link 10 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 11 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 12 | goal_constraints: 13 | - joint_constraints: 14 | - joint_name: torso_lift_joint 15 | position: 0.3436688262338475 16 | - joint_name: shoulder_pan_joint 17 | position: 0.4540267853952992 18 | - joint_name: shoulder_lift_joint 19 | position: -0.09392303771049544 20 | - joint_name: upperarm_roll_joint 21 | position: 0.3482258480003766 22 | - joint_name: elbow_flex_joint 23 | position: 0.5398396849327455 24 | - joint_name: forearm_roll_joint 25 | position: -0.3795501109934688 26 | - joint_name: wrist_flex_joint 27 | position: 1.164951206846258 28 | - joint_name: wrist_roll_joint 29 | position: 0.4983129148878644 30 | planner_id: BKPIECEGood 31 | group_name: arm_with_torso 32 | num_planning_attempts: 2 33 | allowed_planning_time: 60 34 | max_velocity_scaling_factor: 0 35 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0002.yaml: -------------------------------------------------------------------------------- 1 | goal_constraints: 2 | - joint_constraints: 3 | - joint_name: torso_lift_joint 4 | position: 0.2784981721539778 5 | - joint_name: shoulder_pan_joint 6 | position: -0.4826268607503797 7 | - joint_name: shoulder_lift_joint 8 | position: -0.1513071571485109 9 | - joint_name: upperarm_roll_joint 10 | position: 0.4019689664398911 11 | - joint_name: elbow_flex_joint 12 | position: 0.9142947323790461 13 | - joint_name: forearm_roll_joint 14 | position: -0.5155850410143795 15 | - joint_name: wrist_flex_joint 16 | position: 0.8823544027902994 17 | - joint_name: wrist_roll_joint 18 | position: 0.1752026460928385 19 | workspace_parameters: 20 | max_corner: [1, 1, 1] 21 | min_corner: [-1, -1, -1] 22 | header: 23 | frame_id: "" 24 | start_state: 25 | joint_state: 26 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 27 | header: 28 | frame_id: base_link 29 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 30 | planner_id: BKPIECEGood 31 | group_name: arm_with_torso 32 | num_planning_attempts: 2 33 | allowed_planning_time: 60 34 | max_velocity_scaling_factor: 0 35 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0003.yaml: -------------------------------------------------------------------------------- 1 | start_state: 2 | joint_state: 3 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 4 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 5 | header: 6 | frame_id: base_link 7 | workspace_parameters: 8 | header: 9 | frame_id: "" 10 | min_corner: [-1, -1, -1] 11 | max_corner: [1, 1, 1] 12 | allowed_planning_time: 60 13 | max_velocity_scaling_factor: 0 14 | max_acceleration_scaling_factor: 0 15 | goal_constraints: 16 | - joint_constraints: 17 | - joint_name: torso_lift_joint 18 | position: 0.2211947419667415 19 | - joint_name: shoulder_pan_joint 20 | position: -1.553916788418684 21 | - joint_name: shoulder_lift_joint 22 | position: 0.04659921947761923 23 | - joint_name: upperarm_roll_joint 24 | position: 0.2716234593736249 25 | - joint_name: elbow_flex_joint 26 | position: 0.3872802924185552 27 | - joint_name: forearm_roll_joint 28 | position: 2.847027388037661 29 | - position: -1.156012253278968 30 | joint_name: wrist_flex_joint 31 | - position: -3.140006533021991 32 | joint_name: wrist_roll_joint 33 | planner_id: BKPIECEGood 34 | group_name: arm_with_torso 35 | num_planning_attempts: 2 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0004.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | max_corner: [1, 1, 1] 3 | header: 4 | frame_id: "" 5 | min_corner: [-1, -1, -1] 6 | planner_id: BKPIECEGood 7 | max_acceleration_scaling_factor: 0 8 | goal_constraints: 9 | - joint_constraints: 10 | - position: 0.3591466082818735 11 | joint_name: torso_lift_joint 12 | - joint_name: shoulder_pan_joint 13 | position: -1.440682247356212 14 | - joint_name: shoulder_lift_joint 15 | position: 0.09387465795913356 16 | - joint_name: upperarm_roll_joint 17 | position: 0.2540101659839867 18 | - joint_name: elbow_flex_joint 19 | position: 0.4480927706581037 20 | - joint_name: forearm_roll_joint 21 | position: 2.853827994369075 22 | - position: -1.038073912195298 23 | joint_name: wrist_flex_joint 24 | - position: -2.939530823414248 25 | joint_name: wrist_roll_joint 26 | group_name: arm_with_torso 27 | num_planning_attempts: 2 28 | allowed_planning_time: 60 29 | max_velocity_scaling_factor: 0 30 | start_state: 31 | joint_state: 32 | header: 33 | frame_id: base_link 34 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 35 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0005.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | max_corner: [1, 1, 1] 3 | min_corner: [-1, -1, -1] 4 | header: 5 | frame_id: "" 6 | start_state: 7 | joint_state: 8 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 9 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 10 | header: 11 | frame_id: base_link 12 | planner_id: BKPIECEGood 13 | group_name: arm_with_torso 14 | num_planning_attempts: 2 15 | max_velocity_scaling_factor: 0 16 | allowed_planning_time: 60 17 | max_acceleration_scaling_factor: 0 18 | goal_constraints: 19 | - joint_constraints: 20 | - position: 0.3295331270648365 21 | joint_name: torso_lift_joint 22 | - joint_name: shoulder_pan_joint 23 | position: 0.2504648892273546 24 | - joint_name: shoulder_lift_joint 25 | position: -0.2223325553735797 26 | - position: -3.133105852170915 27 | joint_name: upperarm_roll_joint 28 | - joint_name: elbow_flex_joint 29 | position: -0.9000652206514465 30 | - joint_name: forearm_roll_joint 31 | position: 3.141592653589793 32 | - position: 0.8984365372379135 33 | joint_name: wrist_flex_joint 34 | - position: -0.2137057915519533 35 | joint_name: wrist_roll_joint -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0006.yaml: -------------------------------------------------------------------------------- 1 | planner_id: BKPIECEGood 2 | workspace_parameters: 3 | min_corner: [-1, -1, -1] 4 | header: 5 | frame_id: "" 6 | max_corner: [1, 1, 1] 7 | start_state: 8 | joint_state: 9 | header: 10 | frame_id: base_link 11 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 12 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 13 | max_acceleration_scaling_factor: 0 14 | goal_constraints: 15 | - joint_constraints: 16 | - joint_name: torso_lift_joint 17 | position: 0.3252835919844548 18 | - joint_name: shoulder_pan_joint 19 | position: 0.1375041533555526 20 | - joint_name: shoulder_lift_joint 21 | position: -0.234062062584644 22 | - position: -3.135829578022396 23 | joint_name: upperarm_roll_joint 24 | - position: -0.8072078417162789 25 | joint_name: elbow_flex_joint 26 | - joint_name: forearm_roll_joint 27 | position: 3.141592653589793 28 | - joint_name: wrist_flex_joint 29 | position: 0.9983232950323466 30 | - joint_name: wrist_roll_joint 31 | position: -0.06449523836752767 32 | max_velocity_scaling_factor: 0 33 | allowed_planning_time: 60 34 | group_name: arm_with_torso 35 | num_planning_attempts: 2 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0007.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | header: 3 | frame_id: "" 4 | min_corner: [-1, -1, -1] 5 | max_corner: [1, 1, 1] 6 | goal_constraints: 7 | - joint_constraints: 8 | - joint_name: torso_lift_joint 9 | position: 0.1985841366309761 10 | - position: -0.09638376225013194 11 | joint_name: shoulder_pan_joint 12 | - joint_name: shoulder_lift_joint 13 | position: -0.3275245121091383 14 | - joint_name: upperarm_roll_joint 15 | position: 0.287141832010339 16 | - joint_name: elbow_flex_joint 17 | position: 1.038389276852635 18 | - position: -0.3540726047253691 19 | joint_name: forearm_roll_joint 20 | - position: 0.9063812114871916 21 | joint_name: wrist_flex_joint 22 | - position: 0.06322198746195686 23 | joint_name: wrist_roll_joint 24 | planner_id: BKPIECEGood 25 | group_name: arm_with_torso 26 | start_state: 27 | joint_state: 28 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 29 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 30 | header: 31 | frame_id: base_link 32 | num_planning_attempts: 2 33 | allowed_planning_time: 60 34 | max_velocity_scaling_factor: 0 35 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0008.yaml: -------------------------------------------------------------------------------- 1 | num_planning_attempts: 2 2 | max_velocity_scaling_factor: 0 3 | max_acceleration_scaling_factor: 0 4 | workspace_parameters: 5 | header: 6 | frame_id: "" 7 | min_corner: [-1, -1, -1] 8 | max_corner: [1, 1, 1] 9 | group_name: arm_with_torso 10 | goal_constraints: 11 | - joint_constraints: 12 | - position: 0.2105904035129725 13 | joint_name: torso_lift_joint 14 | - joint_name: shoulder_pan_joint 15 | position: 0.8621792852975757 16 | - position: -0.4260897271190365 17 | joint_name: shoulder_lift_joint 18 | - position: -0.1592372784420268 19 | joint_name: upperarm_roll_joint 20 | - position: 1.09066566456648 21 | joint_name: elbow_flex_joint 22 | - position: -2.968809133574433 23 | joint_name: forearm_roll_joint 24 | - joint_name: wrist_flex_joint 25 | position: -0.9187587544933949 26 | - joint_name: wrist_roll_joint 27 | position: 3.141592653589793 28 | allowed_planning_time: 60 29 | planner_id: BKPIECEGood 30 | start_state: 31 | joint_state: 32 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 33 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 34 | header: 35 | frame_id: base_link -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0009.yaml: -------------------------------------------------------------------------------- 1 | num_planning_attempts: 2 2 | planner_id: BKPIECEGood 3 | max_velocity_scaling_factor: 0 4 | allowed_planning_time: 60 5 | max_acceleration_scaling_factor: 0 6 | start_state: 7 | joint_state: 8 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 9 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 10 | header: 11 | frame_id: base_link 12 | group_name: arm_with_torso 13 | goal_constraints: 14 | - joint_constraints: 15 | - joint_name: torso_lift_joint 16 | position: 0.3601683922821377 17 | - joint_name: shoulder_pan_joint 18 | position: 0.8464706524869471 19 | - joint_name: shoulder_lift_joint 20 | position: 0.07443583222545308 21 | - position: -2.301207566398276 22 | joint_name: upperarm_roll_joint 23 | - joint_name: elbow_flex_joint 24 | position: -0.6650218148569015 25 | - joint_name: forearm_roll_joint 26 | position: 2.137683185703032 27 | - position: 1.073253913298448 28 | joint_name: wrist_flex_joint 29 | - joint_name: wrist_roll_joint 30 | position: 0.4618639210930424 31 | workspace_parameters: 32 | max_corner: [1, 1, 1] 33 | min_corner: [-1, -1, -1] 34 | header: 35 | frame_id: "" -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_box/request0010.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | min_corner: [-1, -1, -1] 3 | max_corner: [1, 1, 1] 4 | header: 5 | frame_id: "" 6 | start_state: 7 | joint_state: 8 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 9 | header: 10 | frame_id: base_link 11 | position: [0, 0, 0.1, 0.05, 0, 0, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0, 0.05, 0.05] 12 | max_velocity_scaling_factor: 0 13 | max_acceleration_scaling_factor: 0 14 | allowed_planning_time: 60 15 | planner_id: BKPIECEGood 16 | group_name: arm_with_torso 17 | num_planning_attempts: 2 18 | goal_constraints: 19 | - joint_constraints: 20 | - position: 0.2123239757587973 21 | joint_name: torso_lift_joint 22 | - joint_name: shoulder_pan_joint 23 | position: 0.1905334513799544 24 | - joint_name: shoulder_lift_joint 25 | position: -0.1497657466266507 26 | - joint_name: upperarm_roll_joint 27 | position: -0.6597301359866571 28 | - joint_name: elbow_flex_joint 29 | position: 0.6924487968971108 30 | - joint_name: forearm_roll_joint 31 | position: 0.7201200566166803 32 | - position: 1.173762023639547 33 | joint_name: wrist_flex_joint 34 | - position: -0.4715023617696898 35 | joint_name: wrist_roll_joint -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0001.yaml: -------------------------------------------------------------------------------- 1 | start_state: 2 | joint_state: 3 | position: [-0.001301890656828952, 0.001095591422250131, 0, 0.006638057602043759, 7.905552577014419e-07, 0.002398346642077165, 1.319999501869205, 1.39998238413443, -0.1999861759683759, 1.719957206645441, 9.227849400161858e-07, 1.660019041691295, 2.456531061234557e-06, 0.04992857891283312, 0.04995543695423495] 4 | header: 5 | frame_id: base_link 6 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 7 | goal_constraints: 8 | - joint_constraints: 9 | - joint_name: torso_lift_joint 10 | position: 0.3813146885304878 11 | - joint_name: shoulder_pan_joint 12 | position: -1.00151795317852 13 | - joint_name: shoulder_lift_joint 14 | position: 0.7259824827996758 15 | - joint_name: upperarm_roll_joint 16 | position: -2.247113267302562 17 | - joint_name: elbow_flex_joint 18 | position: 1.385221349117354 19 | - joint_name: forearm_roll_joint 20 | position: -2.738178902248606 21 | - joint_name: wrist_flex_joint 22 | position: 0.6328322327387069 23 | - joint_name: wrist_roll_joint 24 | position: -0.9584235758850088 25 | planner_id: RRTConnectkConfigDefault 26 | group_name: arm_with_torso 27 | num_planning_attempts: 3 28 | allowed_planning_time: 10 29 | max_velocity_scaling_factor: 0 30 | max_acceleration_scaling_factor: 0 31 | workspace_parameters: 32 | header: 33 | frame_id: "" 34 | max_corner: [1, 1, 1] 35 | min_corner: [-1, -1, -1] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0002.yaml: -------------------------------------------------------------------------------- 1 | workspace_parameters: 2 | header: 3 | frame_id: "" 4 | max_corner: [1, 1, 1] 5 | min_corner: [-1, -1, -1] 6 | goal_constraints: 7 | - joint_constraints: 8 | - joint_name: torso_lift_joint 9 | position: 0.001383420381405535 10 | - joint_name: shoulder_pan_joint 11 | position: 0.4648722840775982 12 | - joint_name: shoulder_lift_joint 13 | position: -0.5074878033675915 14 | - joint_name: upperarm_roll_joint 15 | position: 1.778365204990892 16 | - joint_name: elbow_flex_joint 17 | position: 1.399065046316606 18 | - joint_name: forearm_roll_joint 19 | position: -2.465591618081946 20 | - joint_name: wrist_flex_joint 21 | position: 0.9340101381804361 22 | - joint_name: wrist_roll_joint 23 | position: 1.259637205870057 24 | planner_id: RRTConnectkConfigDefault 25 | group_name: arm_with_torso 26 | num_planning_attempts: 3 27 | allowed_planning_time: 10 28 | max_velocity_scaling_factor: 0 29 | max_acceleration_scaling_factor: 0 30 | start_state: 31 | joint_state: 32 | position: [-0.001216658772094981, 0.005465717010629589, 0.0001944184190247067, 0.006739301443282752, -1.57, 0.06100790031371073, 1.320009175556049, 1.399961007146933, -0.1999865151776357, 1.719984239537961, 5.04440564874642e-07, 1.65998376638543, -6.490962181082693e-06, 0.04996258843986121, 0.04998905699031816] 33 | header: 34 | frame_id: base_link 35 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0003.yaml: -------------------------------------------------------------------------------- 1 | goal_constraints: 2 | - joint_constraints: 3 | - joint_name: torso_lift_joint 4 | position: 0 5 | - joint_name: shoulder_pan_joint 6 | position: -0.3219457710162654 7 | - joint_name: shoulder_lift_joint 8 | position: -1.031237409034907 9 | - joint_name: upperarm_roll_joint 10 | position: -0.5238098155892518 11 | - joint_name: elbow_flex_joint 12 | position: 1.248264881847507 13 | - joint_name: forearm_roll_joint 14 | position: 2.514573833120962 15 | - joint_name: wrist_flex_joint 16 | position: 0.2453362368243334 17 | - joint_name: wrist_roll_joint 18 | position: -2.251217315718256 19 | start_state: 20 | joint_state: 21 | position: [-0.001562896033541428, 0.009571000190605083, 0.000149862967199013, 0.006733143270455835, 0.1296726620695008, 0.05959118989887102, 1.320012968934249, 1.39988060011404, -0.1999708197304848, 1.72038053399174, -1.901323256703336e-05, 1.6596845021088, 1.903639478317842e-05, 0.04986566348681871, 0.04989687234441947] 22 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 23 | header: 24 | frame_id: base_link 25 | workspace_parameters: 26 | header: 27 | frame_id: "" 28 | max_corner: [1, 1, 1] 29 | min_corner: [-1, -1, -1] 30 | planner_id: RRTConnectkConfigDefault 31 | group_name: arm_with_torso 32 | num_planning_attempts: 3 33 | allowed_planning_time: 10 34 | max_velocity_scaling_factor: 0 35 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0004.yaml: -------------------------------------------------------------------------------- 1 | planner_id: RRTConnectkConfigDefault 2 | group_name: arm_with_torso 3 | num_planning_attempts: 3 4 | allowed_planning_time: 10 5 | max_velocity_scaling_factor: 0 6 | max_acceleration_scaling_factor: 0 7 | start_state: 8 | joint_state: 9 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 10 | header: 11 | frame_id: base_link 12 | position: [-0.0007849328624542906, 0.01518767700433266, 0.0002210966487781457, 0.006770516995822764, -1.456004262010633, 0.06927821557024672, 1.319992589578521, 1.40005654689207, -0.2000057364221055, 1.719647223770044, 1.545727283236431e-05, 1.660256418083981, -1.775061979358838e-05, 0.04986774346086155, 0.04989131080041784] 13 | goal_constraints: 14 | - joint_constraints: 15 | - joint_name: torso_lift_joint 16 | position: 0.2970653046338838 17 | - joint_name: shoulder_pan_joint 18 | position: 0.8268250225140396 19 | - joint_name: shoulder_lift_joint 20 | position: 0.2149171972880675 21 | - joint_name: upperarm_roll_joint 22 | position: -1.442793157354775 23 | - joint_name: elbow_flex_joint 24 | position: -1.406203255911677 25 | - joint_name: forearm_roll_joint 26 | position: -0.175685296938704 27 | - joint_name: wrist_flex_joint 28 | position: 0.9390853579292325 29 | - joint_name: wrist_roll_joint 30 | position: 1.478886118706163 31 | workspace_parameters: 32 | header: 33 | frame_id: "" 34 | min_corner: [-1, -1, -1] 35 | max_corner: [1, 1, 1] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0005.yaml: -------------------------------------------------------------------------------- 1 | goal_constraints: 2 | - joint_constraints: 3 | - joint_name: torso_lift_joint 4 | position: 8.267346270450965e-05 5 | - joint_name: shoulder_pan_joint 6 | position: -0.7390899331497579 7 | - joint_name: shoulder_lift_joint 8 | position: -1.146840886899364 9 | - joint_name: upperarm_roll_joint 10 | position: 2.173463541140414 11 | - joint_name: elbow_flex_joint 12 | position: -1.548607351735822 13 | - joint_name: forearm_roll_joint 14 | position: -0.9368171321369169 15 | - joint_name: wrist_flex_joint 16 | position: 0.6847151250403862 17 | - joint_name: wrist_roll_joint 18 | position: -1.907179170364188 19 | start_state: 20 | joint_state: 21 | header: 22 | frame_id: base_link 23 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 24 | position: [-0.00117985636496698, 0.01819934087407304, 0.000216298199833016, 0.006745437057228717, 0.346159108759629, 0.06220715250170183, 1.320024019859318, 1.399989855275235, -0.1999744819550378, 1.719963485404481, 6.291687152604197e-06, 1.659964506788162, -9.409996287068623e-06, 0.04983253857063437, 0.04985824435648394] 25 | planner_id: RRTConnectkConfigDefault 26 | group_name: arm_with_torso 27 | num_planning_attempts: 3 28 | allowed_planning_time: 10 29 | max_velocity_scaling_factor: 0 30 | max_acceleration_scaling_factor: 0 31 | workspace_parameters: 32 | min_corner: [-1, -1, -1] 33 | header: 34 | frame_id: "" 35 | max_corner: [1, 1, 1] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0006.yaml: -------------------------------------------------------------------------------- 1 | start_state: 2 | joint_state: 3 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 4 | position: [-0.0007513192310932837, 0.02299412389440647, 0.0002172780170763, 0.006728862970468036, -1.57, 0.06739120431890111, 1.319991266344941, 1.399973211217874, -0.1999901678891369, 1.719987616872583, 4.924826370711344e-06, 1.659980714834496, 7.422737644091626e-06, 0.04989747207375791, 0.04992557815134854] 5 | header: 6 | frame_id: base_link 7 | planner_id: RRTConnectkConfigDefault 8 | group_name: arm_with_torso 9 | num_planning_attempts: 3 10 | allowed_planning_time: 10 11 | max_velocity_scaling_factor: 0 12 | max_acceleration_scaling_factor: 0 13 | goal_constraints: 14 | - joint_constraints: 15 | - joint_name: torso_lift_joint 16 | position: 5.817591087453589e-08 17 | - joint_name: shoulder_pan_joint 18 | position: 0.09492901534222749 19 | - joint_name: shoulder_lift_joint 20 | position: -1.206952814771632 21 | - joint_name: upperarm_roll_joint 22 | position: -1.255509487338636 23 | - joint_name: elbow_flex_joint 24 | position: 1.726402749304374 25 | - joint_name: forearm_roll_joint 26 | position: -0.4336693715661215 27 | - joint_name: wrist_flex_joint 28 | position: -0.365113537340298 29 | - joint_name: wrist_roll_joint 30 | position: 0.7728088779874197 31 | workspace_parameters: 32 | max_corner: [1, 1, 1] 33 | header: 34 | frame_id: "" 35 | min_corner: [-1, -1, -1] -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0007.yaml: -------------------------------------------------------------------------------- 1 | max_velocity_scaling_factor: 0 2 | max_acceleration_scaling_factor: 0 3 | workspace_parameters: 4 | min_corner: [-1, -1, -1] 5 | header: 6 | frame_id: "" 7 | max_corner: [1, 1, 1] 8 | start_state: 9 | joint_state: 10 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 11 | header: 12 | frame_id: base_link 13 | position: [-0.0006874728445751543, 0.02769563119700269, 0.0002213302370215809, 0.006736989767427419, -1.569999997497361, 0.06965324267155282, 1.319996221911405, 1.400018004952129, -0.1999844857139932, 1.719834047133574, 7.232769951848184e-06, 1.660103908853722, 5.915770127984388e-06, 0.04986454496168111, 0.04989077390219541] 14 | goal_constraints: 15 | - joint_constraints: 16 | - joint_name: torso_lift_joint 17 | position: 0.3861444722313734 18 | - joint_name: shoulder_pan_joint 19 | position: -1.60083726386252 20 | - joint_name: shoulder_lift_joint 21 | position: 0.3707313698276314 22 | - joint_name: upperarm_roll_joint 23 | position: 1.457443182393985 24 | - joint_name: elbow_flex_joint 25 | position: 1.759846359693174 26 | - joint_name: forearm_roll_joint 27 | position: 2.738633984846905 28 | - joint_name: wrist_flex_joint 29 | position: 1.255672021645022 30 | - joint_name: wrist_roll_joint 31 | position: 1.62439083857203 32 | planner_id: RRTConnectkConfigDefault 33 | group_name: arm_with_torso 34 | num_planning_attempts: 3 35 | allowed_planning_time: 10 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0008.yaml: -------------------------------------------------------------------------------- 1 | goal_constraints: 2 | - joint_constraints: 3 | - joint_name: torso_lift_joint 4 | position: 0.3852348202322444 5 | - joint_name: shoulder_pan_joint 6 | position: -0.376341543404233 7 | - joint_name: shoulder_lift_joint 8 | position: 0.677521661849988 9 | - joint_name: upperarm_roll_joint 10 | position: -1.866405142106291 11 | - joint_name: elbow_flex_joint 12 | position: 1.617132464907942 13 | - joint_name: forearm_roll_joint 14 | position: 0.4226829856092462 15 | - joint_name: wrist_flex_joint 16 | position: -0.7696303403480902 17 | - joint_name: wrist_roll_joint 18 | position: 1.955231978011004 19 | start_state: 20 | joint_state: 21 | header: 22 | frame_id: base_link 23 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 24 | position: [-0.0003661547976419044, 0.03268707960804473, 0.0002160706730618214, 0.006774792825353941, -1.57, 0.06620072154522205, 1.319968022726414, 1.399984845102675, -0.1999907512239805, 1.719859276875177, 1.219980768851769e-06, 1.660135770250502, 2.540539805107045e-05, 0.0498983482937544, 0.04992479113090313] 25 | planner_id: RRTConnectkConfigDefault 26 | group_name: arm_with_torso 27 | num_planning_attempts: 3 28 | allowed_planning_time: 10 29 | max_velocity_scaling_factor: 0 30 | max_acceleration_scaling_factor: 0 31 | workspace_parameters: 32 | min_corner: [-1, -1, -1] 33 | max_corner: [1, 1, 1] 34 | header: 35 | frame_id: "" -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0009.yaml: -------------------------------------------------------------------------------- 1 | goal_constraints: 2 | - joint_constraints: 3 | - joint_name: torso_lift_joint 4 | position: 0.371413740381837 5 | - joint_name: shoulder_pan_joint 6 | position: -0.15675561532155 7 | - joint_name: shoulder_lift_joint 8 | position: 0.3175679938388921 9 | - joint_name: upperarm_roll_joint 10 | position: 1.551679863093635 11 | - joint_name: elbow_flex_joint 12 | position: 1.569599518438739 13 | - joint_name: forearm_roll_joint 14 | position: -0.3425074614785051 15 | - joint_name: wrist_flex_joint 16 | position: -0.8250930226813006 17 | - joint_name: wrist_roll_joint 18 | position: -1.525897672767684 19 | workspace_parameters: 20 | max_corner: [1, 1, 1] 21 | min_corner: [-1, -1, -1] 22 | header: 23 | frame_id: "" 24 | start_state: 25 | joint_state: 26 | position: [-0.0001641950204849252, 0.03825073607645546, 0.0002133661981933388, 0.006740617344219674, -1.57, 0.06739790717392236, 1.319999942987436, 1.39998468319579, -0.1999871990744726, 1.71995177321768, -3.718147961961904e-06, 1.660022539375483, -1.756843925448948e-06, 0.04986444237703464, 0.04989092561838213] 27 | header: 28 | frame_id: base_link 29 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 30 | planner_id: RRTConnectkConfigDefault 31 | group_name: arm_with_torso 32 | num_planning_attempts: 3 33 | allowed_planning_time: 10 34 | max_velocity_scaling_factor: 0 35 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_library/yaml/fetch_scenes/request0010.yaml: -------------------------------------------------------------------------------- 1 | planner_id: RRTConnectkConfigDefault 2 | group_name: arm_with_torso 3 | num_planning_attempts: 3 4 | allowed_planning_time: 10 5 | max_velocity_scaling_factor: 0 6 | max_acceleration_scaling_factor: 0 7 | workspace_parameters: 8 | max_corner: [1, 1, 1] 9 | header: 10 | frame_id: "" 11 | min_corner: [-1, -1, -1] 12 | start_state: 13 | joint_state: 14 | position: [-0.0004643838175075743, 0.0414858518041159, 0.000148374044773668, 0.006686522052895669, -0.3788736860130184, 0.06274970852349959, 1.320012206102006, 1.399956965149221, -0.1999843075418992, 1.720216439411594, -5.840510894472573e-06, 1.65974954837945, -7.347064314267016e-06, 0.04982970990759876, 0.04985722340071728] 15 | header: 16 | frame_id: base_link 17 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 18 | goal_constraints: 19 | - joint_constraints: 20 | - joint_name: torso_lift_joint 21 | position: 0.38615 22 | - joint_name: shoulder_pan_joint 23 | position: 1.603128848110437 24 | - joint_name: shoulder_lift_joint 25 | position: 0.8607754131976091 26 | - joint_name: upperarm_roll_joint 27 | position: -2.4538221326145 28 | - joint_name: elbow_flex_joint 29 | position: 1.467866050628439 30 | - joint_name: forearm_roll_joint 31 | position: -3.027824937791933 32 | - joint_name: wrist_flex_joint 33 | position: 0.5047529648056509 34 | - joint_name: wrist_roll_joint 35 | position: -0.512235424059769 -------------------------------------------------------------------------------- /robowflex_library/yaml/test_cob4.yml: -------------------------------------------------------------------------------- 1 | name: (noname) 2 | fixed_frame_transforms: 3 | - child_frame_id: base_footprint 4 | transform: 5 | translation: [0, 0, 0] 6 | rotation: [0, 0, 0, 1] 7 | world: 8 | collision_objects: 9 | - id: table_right 10 | meshes: 11 | - resource: "package://robowflex_resources/objects/cafe_table.dae" 12 | dimensions: [0.025, 0.025, 0.025] 13 | mesh_poses: 14 | - position: [0.9, 0.0, -0.08] 15 | orientation: [0, 0, 0, 1] 16 | - id: Cube1 17 | primitives: 18 | - type: box 19 | dimensions: [0.07, 0.07, 0.08] 20 | primitive_poses: 21 | - position: [0.6, -0.25, 0.72] 22 | orientation: [0, 0, 0, 1] 23 | - id: Cube2 24 | primitives: 25 | - type: box 26 | dimensions: [0.07, 0.07, 0.08] 27 | primitive_poses: 28 | - position: [0.6, 0.0, 0.72] 29 | orientation: [0, 0, 0, 1] 30 | - id: Cube3 31 | primitives: 32 | - type: box 33 | dimensions: [0.07, 0.07, 0.08] 34 | primitive_poses: 35 | - position: [0.6, 0.25, 0.72] 36 | orientation: [0, 0, 0, 1] 37 | -------------------------------------------------------------------------------- /robowflex_library/yaml/test_fetch.yml: -------------------------------------------------------------------------------- 1 | name: (noname) 2 | robot_model_name: fetch 3 | robot_state: 4 | joint_state: 5 | position: [0, 0, 0.05, 0, 0, 0, 1.32, 1.4, -0.2, 1.72, 0.0, 1.66, 0.0, 0.04, 0.04] 6 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 7 | fixed_frame_transforms: 8 | - child_frame_id: world 9 | transform: 10 | translation: [0, 0, 0] 11 | rotation: [0, 0, 0, 1] 12 | world: 13 | collision_objects: 14 | - id: table_right 15 | meshes: 16 | - resource: "package://robowflex_resources/objects/cafe_table.dae" 17 | dimensions: [0.025, 0.025, 0.025] 18 | mesh_poses: 19 | - position: [0.3, 0.8, -0.08] 20 | orientation: [0, 0, 0, 1] 21 | - id: Cube1 22 | primitives: 23 | - type: box 24 | dimensions: [0.07, 0.07, 0.07] 25 | primitive_poses: 26 | - position: [0.0, 0.6, 0.72] 27 | orientation: [0, 0, 0, 1] 28 | - id: Cube2 29 | primitives: 30 | - type: box 31 | dimensions: [0.07, 0.07, 0.07] 32 | primitive_poses: 33 | - position: [0.2, 0.6, 0.72] 34 | orientation: [0, 0, 0, 1] 35 | - id: Cube3 36 | primitives: 37 | - type: box 38 | dimensions: [0.07, 0.07, 0.07] 39 | primitive_poses: 40 | - position: [0.4, 0.6, 0.72] 41 | orientation: [0, 0, 0, 1] 42 | -------------------------------------------------------------------------------- /robowflex_library/yaml/test_fetch_wall.yml: -------------------------------------------------------------------------------- 1 | name: (noname) 2 | robot_model_name: fetch 3 | robot_state: 4 | joint_state: 5 | position: [0, 0, 0.05, 0, 0, 0, 1.32, 1.4, -0.2, 1.72, 0.0, 1.66, 0.0, 0.04, 0.04] 6 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 7 | fixed_frame_transforms: 8 | - child_frame_id: world 9 | transform: 10 | translation: [0, 0, 0] 11 | rotation: [0, 0, 0, 1] 12 | world: 13 | collision_objects: 14 | - id: table_right 15 | meshes: 16 | - resource: "package://robowflex_resources/objects/cafe_table.dae" 17 | dimensions: [0.025, 0.025, 0.025] 18 | mesh_poses: 19 | - position: [0.3, 0.8, -0.08] 20 | orientation: [0, 0, 0, 1] 21 | - id: Cube1 22 | primitives: 23 | - type: box 24 | dimensions: [0.07, 0.07, 0.07] 25 | primitive_poses: 26 | - position: [0.0, 0.6, 0.72] 27 | orientation: [0, 0, 0, 1] 28 | - id: Cube2 29 | primitives: 30 | - type: box 31 | dimensions: [0.07, 0.07, 0.07] 32 | primitive_poses: 33 | - position: [0.2, 0.6, 0.72] 34 | orientation: [0, 0, 0, 1] 35 | - id: Cube3 36 | primitives: 37 | - type: box 38 | dimensions: [0.07, 0.07, 0.07] 39 | primitive_poses: 40 | - position: [0.4, 0.6, 0.72] 41 | orientation: [0, 0, 0, 1] 42 | 43 | - id: Wall1 44 | primitives: 45 | - type: box 46 | dimensions: [1, 0.07, 0.07] 47 | primitive_poses: 48 | - position: [0.3, 0.75, 0.72] 49 | orientation: [0, 0, 0, 1] 50 | - id: Wall2 51 | primitives: 52 | - type: box 53 | dimensions: [1, 0.07, 0.07] 54 | primitive_poses: 55 | - position: [0.3, 0.45, 0.72] 56 | orientation: [0, 0, 0, 1] 57 | -------------------------------------------------------------------------------- /robowflex_movegroup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex_movegroup 3 | VERSION 1.3 4 | LANGUAGES CXX 5 | ) 6 | 7 | set(LIBRARY_NAME ${PROJECT_NAME}) 8 | 9 | set(CMAKE_MODULE_PATH 10 | "${CMAKE_MODULE_PATH}" 11 | "${CMAKE_ROOT_DIR}/cmake/Modules" 12 | "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 13 | 14 | include(CompileOptions) 15 | include(HelperFunctions) 16 | 17 | find_package(Boost REQUIRED filesystem) 18 | find_library(YAML yaml-cpp REQUIRED) 19 | 20 | find_package(catkin QUIET COMPONENTS 21 | robowflex_library 22 | actionlib 23 | moveit_msgs 24 | ) 25 | 26 | catkin_package( 27 | LIBRARIES ${LIBRARY_NAME} 28 | CATKIN_DEPENDS 29 | robowflex_library 30 | actionlib 31 | moveit_msgs 32 | DEPENDS 33 | INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/include 34 | ) 35 | 36 | include_directories(SYSTEM ${CMAKE_CURRENT_LIST_DIR}/include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} yaml-cpp) 37 | link_directories(${catkin_LIBRARY_DIRS}) 38 | 39 | add_library(${LIBRARY_NAME} 40 | src/services.cpp 41 | ) 42 | 43 | set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) 44 | target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} yaml-cpp) 45 | 46 | add_script(tapedeck) 47 | 48 | install_scripts() 49 | install_library() 50 | -------------------------------------------------------------------------------- /robowflex_movegroup/CMakeModules: -------------------------------------------------------------------------------- 1 | ../CMakeModules/ -------------------------------------------------------------------------------- /robowflex_movegroup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_movegroup 4 | 1.3.0 5 | Robowflex: MoveIt MoveGroup Interaction 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | 16 | robowflex_library 17 | actionlib 18 | moveit_msgs 19 | yaml-cpp 20 | 21 | -------------------------------------------------------------------------------- /robowflex_movegroup/scripts/tapedeck.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using namespace robowflex; 11 | 12 | /* \file tapedeck.cpp 13 | * An example script that shows how to use MoveGroupHelper. Here, a callback is 14 | * installed so that every motion plan issued to MoveGroup is saved to disk as a 15 | * YAML file. This is useful for collecting and replaying motion planning 16 | * requests that are done over the course of an experiment. 17 | */ 18 | 19 | // Output a captured action to a YAML file. 20 | void callback(movegroup::MoveGroupHelper::Action &action) 21 | { 22 | ros::Time time = ros::Time::now(); 23 | 24 | // Save the requests to a folder in the home directory. 25 | const std::string filename = "~/robowflex_tapedeck/" + to_iso_string(time.toBoost()) + ".yml"; 26 | action.toYAMLFile(filename); 27 | 28 | RBX_INFO("Wrote YAML for Request ID `%s` to file `%s`", action.id, filename); 29 | } 30 | 31 | int main(int argc, char **argv) 32 | { 33 | // Startup ROS 34 | ROS ros(argc, argv); 35 | 36 | // Create helper 37 | movegroup::MoveGroupHelper helper; 38 | 39 | // Setup callback function 40 | helper.setResultCallback(callback); 41 | 42 | // Wait until killed 43 | ros.wait(); 44 | } 45 | -------------------------------------------------------------------------------- /robowflex_ompl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex_ompl 3 | VERSION 1.3 4 | LANGUAGES CXX 5 | ) 6 | 7 | set(LIBRARY_NAME ${PROJECT_NAME}) 8 | 9 | set(CMAKE_MODULE_PATH 10 | "${CMAKE_MODULE_PATH}" 11 | "${CMAKE_ROOT_DIR}/cmake/Modules" 12 | "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 13 | 14 | include(CompileOptions) 15 | include(HelperFunctions) 16 | 17 | find_package(Boost REQUIRED COMPONENTS serialization) 18 | 19 | find_package(catkin QUIET COMPONENTS 20 | robowflex_library 21 | moveit_planners_ompl 22 | ) 23 | 24 | catkin_package( 25 | LIBRARIES ${LIBRARY_NAME} 26 | CATKIN_DEPENDS 27 | robowflex_library 28 | moveit_planners_ompl 29 | DEPENDS 30 | INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/include 31 | ) 32 | 33 | if (moveit_planners_ompl_DIR) 34 | include_directories(SYSTEM ${CMAKE_CURRENT_LIST_DIR}/include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 35 | link_directories(${catkin_LIBRARY_DIRS}) 36 | 37 | add_library(${LIBRARY_NAME} 38 | src/ompl_interface.cpp 39 | src/ompl_trajectory.cpp 40 | ) 41 | 42 | set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) 43 | target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 44 | 45 | add_script(ur5_ompl_interface) 46 | add_script(fetch_ompl_interface) 47 | add_script(fetch_ompl_benchmark) 48 | add_script(fetch_ompl_scenes_benchmark) 49 | add_script(fetch_profile) 50 | 51 | install_scripts() 52 | install_library() 53 | else() 54 | message("`moveit_planners_ompl` not found, not compiling robowflex_ompl library or executables.") 55 | endif() 56 | -------------------------------------------------------------------------------- /robowflex_ompl/CMakeModules: -------------------------------------------------------------------------------- 1 | ../CMakeModules/ -------------------------------------------------------------------------------- /robowflex_ompl/include/robowflex_ompl/ompl_trajectory.h: -------------------------------------------------------------------------------- 1 | /* Author: Constantinos Chamzas */ 2 | 3 | #ifndef ROBOWFLEX_OMPL_TRAJECTORY 4 | #define ROBOWFLEX_OMPL_TRAJECTORY 5 | 6 | #include 7 | 8 | // Moveit 9 | #include 10 | 11 | // OMPL 12 | #include 13 | #include 14 | 15 | namespace robowflex 16 | { 17 | /** \cond IGNORE */ 18 | ROBOWFLEX_CLASS_FORWARD(Robot); 19 | ROBOWFLEX_CLASS_FORWARD(Scene); 20 | ROBOWFLEX_CLASS_FORWARD(Trajectory); 21 | /** \endcond */ 22 | 23 | namespace OMPL 24 | { 25 | /** \cond IGNORE */ 26 | ROBOWFLEX_CLASS_FORWARD(OMPLTrajectory); 27 | /** \endcond */ 28 | 29 | /** \class robowflex::OMPL::OMPLTrajectoryPtr 30 | \brief A shared pointer wrapper for robowflex::OMPL::OMPLTrajectory. */ 31 | 32 | /** \class robowflex::OMPL::OMPLTrajectoryConstPtr 33 | \brief A const shared pointer wrapper for robowflex::OMPL::OMPLTrajectory. */ 34 | 35 | /** \brief OMPLTrajectory provides OMPL path utilities to the Trajectory class. 36 | */ 37 | class OMPLTrajectory : public Trajectory 38 | { 39 | public: 40 | /** \brief Constructor. 41 | * \param[in] robot Robot to construct trajectory for. 42 | * \param[in] group Planning group of the trajectory. 43 | */ 44 | OMPLTrajectory(const RobotConstPtr &robot, const std::string &group); 45 | 46 | /** \brief Constructor. 47 | * \param[in] trajectory Trajectory to initialize with. 48 | */ 49 | OMPLTrajectory(robot_trajectory::RobotTrajectory &trajectory); 50 | 51 | /** \brief Converts to an OMPL Path given an OMPL \a simple_setup. 52 | * \param[in] ss a simple setup structure that has the correct stateSpaceInformation. 53 | * \return The Geometric Path equivalent of the trajectory. 54 | */ 55 | ompl::geometric::PathGeometric toOMPLPath(const ompl::geometric::SimpleSetupPtr &ss); 56 | 57 | /** \brief Sets the trajectory from an OMPL Path and a reference state. 58 | * \param[in] reference_state A full state that contains the values for all the joints. 59 | * \param[in] path The geometric OMPL path to convert. 60 | */ 61 | void fromOMPLPath(const robot_state::RobotState &reference_state, 62 | const ompl::geometric::PathGeometric &path); 63 | }; 64 | } // namespace OMPL 65 | } // namespace robowflex 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /robowflex_ompl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_ompl 4 | 1.3.0 5 | Robowflex: Direct OMPL Planning Interface 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | 16 | robowflex_library 17 | moveit_planners_ompl 18 | 19 | -------------------------------------------------------------------------------- /robowflex_ompl/scripts/fetch_ompl_interface.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace robowflex; 13 | 14 | static const std::string GROUP = "arm_with_torso"; 15 | 16 | int main(int argc, char **argv) 17 | { 18 | // Startup ROS 19 | ROS ros(argc, argv); 20 | 21 | // Create the default Fetch robot. 22 | auto fetch = std::make_shared(); 23 | fetch->initialize(); 24 | 25 | // Create an empty scene. 26 | auto scene = std::make_shared(fetch); 27 | 28 | // Create the default planner for the Fetch. 29 | auto planner = std::make_shared(fetch, "default"); 30 | planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml"); 31 | 32 | // Sets the Fetch's base pose. 33 | fetch->setBasePose(1, 1, 0.5); 34 | 35 | // Sets the Fetch's head pose to look at a point. 36 | fetch->pointHead({2, 1, 1.5}); 37 | 38 | // Create a motion planning request with a pose goal. 39 | MotionRequestBuilder request(planner, GROUP); 40 | fetch->setGroupState(GROUP, {0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0}); // Stow 41 | request.setStartConfiguration(fetch->getScratchState()); 42 | 43 | fetch->setGroupState(GROUP, {0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); // Unfurl 44 | request.setGoalConfiguration(fetch->getScratchState()); 45 | 46 | request.setConfig("PRM"); 47 | 48 | // Do motion planning! 49 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 50 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 51 | return 1; 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Constantinos Chamzas, Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | using namespace robowflex; 16 | 17 | static const std::string GROUP = "arm_with_torso"; 18 | 19 | int main(int argc, char **argv) 20 | { 21 | // Startup ROS 22 | ROS ros(argc, argv); 23 | 24 | // Create the default Fetch robot. 25 | auto fetch = std::make_shared(); 26 | 27 | // Do not add the virtual joint since the real robot does not have one. 28 | fetch->initialize(false); 29 | 30 | // Setup a benchmarking request for the joint and pose motion plan requests. 31 | Profiler::Options options; 32 | options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH; 33 | Experiment experiment("fetch_scenes", options, 30.0, 2); 34 | 35 | const std::size_t start = 1; 36 | const std::size_t end = 10; 37 | for (std::size_t i = start; i <= end; i++) 38 | { 39 | const auto &scene_file = 40 | log::format("package://robowflex_library/yaml/fetch_scenes/scene_vicon%1$04d.yaml", i); 41 | const auto &request_file = 42 | log::format("package://robowflex_library/yaml/fetch_scenes/request%1$04d.yaml", i); 43 | 44 | // Create an empty Scene. 45 | auto scene = std::make_shared(fetch); 46 | if (not scene->fromYAMLFile(scene_file)) 47 | { 48 | RBX_ERROR("Failed to read file: %s for scene", scene_file); 49 | continue; 50 | } 51 | 52 | // Create the default planner for the Fetch. 53 | auto planner = std::make_shared(fetch); 54 | planner->initialize(); 55 | 56 | // Create an empty motion planning request. 57 | auto request = std::make_shared(planner, GROUP); 58 | if (not request->fromYAMLFile(request_file)) 59 | { 60 | RBX_ERROR("Failed to read file: %s for request", request_file); 61 | continue; 62 | } 63 | 64 | request->setConfig("PRMstar"); 65 | 66 | // Add request 67 | experiment.addQuery(log::format("scene%1$04d", i), scene, planner, request); 68 | } 69 | 70 | auto dataset = experiment.benchmark(1); 71 | 72 | OMPLPlanDataSetOutputter output("robowflex_fetch_scenes_ompl"); 73 | output.dump(*dataset); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /robowflex_ompl/scripts/ur5_ompl_interface.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Zachary Kingston */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace robowflex; 14 | 15 | int main(int argc, char **argv) 16 | { 17 | // Startup ROS 18 | ROS ros(argc, argv); 19 | 20 | // Create the default UR5 robot. 21 | auto ur5 = std::make_shared(); 22 | ur5->initialize(); 23 | 24 | // Create an empty scene. 25 | auto scene = std::make_shared(ur5); 26 | 27 | // Create an OMPL interface planner for the ur5. 28 | auto planner = std::make_shared(ur5); 29 | planner->initialize("package://ur5_robotiq85_moveit_config/config/ompl_planning.yaml"); 30 | 31 | // Create a motion planning request with a pose goal. 32 | MotionRequestBuilder request(planner, "manipulator"); 33 | request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0}); 34 | 35 | RobotPose pose = RobotPose::Identity(); 36 | pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313}); 37 | Eigen::Quaterniond orn{0, 0, 1, 0}; 38 | 39 | auto sphere = std::make_shared(Geometry::ShapeType::SPHERE, Eigen::Vector3d{0.01, 0, 0}); 40 | request.setGoalRegion("ee_link", "world", // links 41 | pose, sphere, // position 42 | orn, {0.01, 0.01, 0.01} // orientation 43 | ); 44 | 45 | // Do motion planning! 46 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 47 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 48 | return 1; 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /robowflex_ompl/src/ompl_trajectory.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Constantinos Chamzas */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | using namespace robowflex; 12 | 13 | OMPL::OMPLTrajectory::OMPLTrajectory(const RobotConstPtr &robot, const std::string &group) 14 | : Trajectory(robot, group) 15 | { 16 | } 17 | 18 | OMPL::OMPLTrajectory::OMPLTrajectory(robot_trajectory::RobotTrajectory &trajectory) : Trajectory(trajectory) 19 | { 20 | } 21 | 22 | ompl::geometric::PathGeometric OMPL::OMPLTrajectory::toOMPLPath(const ompl::geometric::SimpleSetupPtr &ss) 23 | { 24 | auto path = ompl::geometric::PathGeometric(ss->getSpaceInformation()); 25 | auto *tstate = ss->getSpaceInformation()->allocState(); 26 | auto traj_msg = getMessage(); 27 | 28 | // transform to Path geometric 29 | for (const auto &state_vec : traj_msg.joint_trajectory.points) 30 | for (unsigned int j = 0; j < state_vec.positions.size(); j++) 31 | tstate->as()->values[j] = state_vec.positions[j]; 32 | 33 | path.append(tstate); 34 | 35 | return path; 36 | } 37 | 38 | void OMPL::OMPLTrajectory::fromOMPLPath(const robot_state::RobotState &reference_state, 39 | const ompl::geometric::PathGeometric &path) 40 | { 41 | const auto &mbss = std::dynamic_pointer_cast( 42 | path.getSpaceInformation()->getStateSpace()); 43 | 44 | if (not mbss) 45 | throw Exception(1, "Failed to extract StateSpace from provided OMPL path!"); 46 | 47 | moveit::core::RobotState ks = reference_state; 48 | for (std::size_t i = 0; i < path.getStateCount(); ++i) 49 | { 50 | mbss->copyToRobotState(ks, path.getState(i)); 51 | trajectory_->addSuffixWayPoint(ks, 0.0); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /robowflex_tesseract/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex_tesseract 3 | VERSION 1.3 4 | LANGUAGES CXX 5 | ) 6 | 7 | set(LIBRARY_NAME ${PROJECT_NAME}) 8 | 9 | set(CMAKE_MODULE_PATH 10 | "${CMAKE_MODULE_PATH}" 11 | "${CMAKE_ROOT_DIR}/cmake/Modules" 12 | "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 13 | 14 | include(CompileOptions) 15 | include(HelperFunctions) 16 | 17 | list(APPEND CATKIN_DEPS 18 | robowflex_library 19 | tesseract_planning 20 | tesseract_ros 21 | ) 22 | 23 | find_package(catkin QUIET COMPONENTS ${CATKIN_DEPS}) 24 | 25 | catkin_package( 26 | LIBRARIES ${LIBRARY_NAME} 27 | CATKIN_DEPENDS ${CATKIN_DEPS} 28 | DEPENDS 29 | INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/include 30 | ) 31 | 32 | if (tesseract_planning_DIR) 33 | include_directories(SYSTEM 34 | ${CMAKE_CURRENT_LIST_DIR}/include ${catkin_INCLUDE_DIRS}) 35 | 36 | link_directories(${catkin_LIBRARY_DIRS}) 37 | 38 | add_library(${LIBRARY_NAME} 39 | src/conversions.cpp 40 | src/trajopt_planner.cpp 41 | ) 42 | 43 | set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) 44 | target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES}) 45 | 46 | add_script(fetch_tabletop_goalpose) 47 | add_script(fetch_tabletop_goalstate) 48 | add_script(fetch_tabletop_inits) 49 | add_script(fetch_tabletop_planning_time) 50 | add_script(fetch_trajopt) 51 | add_script(ur5_custom_planning) 52 | 53 | install_scripts() 54 | install_tests() 55 | install_library() 56 | else() 57 | message("`tesseract_planning` not found, not compiling robowflex_tesseract library or executables.") 58 | endif() 59 | -------------------------------------------------------------------------------- /robowflex_tesseract/CMakeModules: -------------------------------------------------------------------------------- 1 | ../CMakeModules/ -------------------------------------------------------------------------------- /robowflex_tesseract/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_tesseract 4 | 1.3.0 5 | Robowflex: ROS Industrial Tesseract Interface 6 | 7 | Zachary Kingston 8 | Bryce Willey 9 | Carlos Quintero Pena 10 | 11 | BSD 12 | https://github.com/KavrakiLab/robowflex/issues 13 | https://github.com/KavrakiLab/robowflex 14 | 15 | catkin 16 | 17 | robowflex_library 18 | tesseract_planning 19 | tesseract_ros 20 | 21 | -------------------------------------------------------------------------------- /robowflex_tesseract/scenes/table/request.yaml: -------------------------------------------------------------------------------- 1 | start_state: 2 | attached_collision_objects: 3 | - object: 4 | primitive_poses: 5 | - position: [0.03360957426989708, -0.0005185608964976551, -0.02462558968502093] 6 | orientation: [0.001092153463991719, -0.001006562221771093, -0.001405502234554409, 0.9999979092962012] 7 | header: 8 | frame_id: gripper_link 9 | id: Can1 10 | primitives: 11 | - dimensions: [0.1, 0.03] 12 | type: cylinder 13 | touch_links: 14 | - gripper_link 15 | - l_gripper_finger_link 16 | - r_gripper_finger_link 17 | link_name: gripper_link 18 | weight: 0 19 | joint_state: 20 | header: 21 | frame_id: base_link 22 | position: [-0.0003997190310860432, 0.0002133940717845562, 0.38615, 0.1997121599204449, 9.028836123192718e-07, 0.002399987167772899, -0.1349994403792381, 1.453915381564148, 1.50204308714454, 1.551087883649017, 1.684639616960671, 1.498991074971971, 1.599030772138053, 0.04992403458690093, 0.04995082766085816] 23 | name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint] 24 | goal_constraints: 25 | - joint_constraints: 26 | - joint_name: shoulder_pan_joint 27 | position: -1.389999440379238 28 | - joint_name: shoulder_lift_joint 29 | position: 0.1977395737257543 30 | - joint_name: upperarm_roll_joint 31 | position: 0.5930076552859318 32 | - joint_name: elbow_flex_joint 33 | position: 1.80876210908553 34 | - joint_name: forearm_roll_joint 35 | position: 1.908199851260852 36 | - joint_name: wrist_flex_joint 37 | position: 1.088849801995825 38 | - joint_name: wrist_roll_joint 39 | position: 2.566546324089854 40 | workspace_parameters: 41 | max_corner: [1, 1, 1] 42 | min_corner: [-1, -1, -1] 43 | header: 44 | frame_id: "" 45 | planner_id: RRTConnectkConfigDefault 46 | group_name: arm 47 | allowed_planning_time: 5 48 | max_velocity_scaling_factor: 0 49 | max_acceleration_scaling_factor: 0 -------------------------------------------------------------------------------- /robowflex_tesseract/scenes/table/scene.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | collision_objects: 3 | - header: 4 | frame_id: base_link 5 | id: Can1 6 | primitives: 7 | - type: cylinder 8 | dimensions: [0.1, 0.03] 9 | primitive_poses: 10 | - position: [0.55, 0.2997994722263027, 0.7893448398657438] 11 | orientation: [0.001353898394225947, -0.001413108676048193, 1.86736507787716e-05, 0.9999980848653174] 12 | - header: 13 | frame_id: base_link 14 | id: Cube 15 | primitives: 16 | - type: box 17 | dimensions: [0.25, 0.25, 0.25] 18 | primitive_poses: 19 | - position: [0.76, 8.433902659017084e-06, 0.839018812904158] 20 | orientation: [-3.19427086946521e-05, -3.248814443275678e-06, 1.21284567499891e-06, 0.9999999994838188] 21 | - header: 22 | frame_id: base_link 23 | id: table_bottom 24 | primitives: 25 | - type: box 26 | dimensions: [0.9, 1.7, 0.04] 27 | primitive_poses: 28 | - position: [1.049999494385826, 1.51702591216964e-06, 0.2452286235913045] 29 | orientation: [3.991390862408021e-07, -3.058623738022737e-07, 1.212497197604013e-06, 0.9999999999991385] 30 | - header: 31 | frame_id: base_link 32 | id: table_leg_left_back 33 | primitives: 34 | - type: box 35 | dimensions: [0.05, 0.05, 0.7] 36 | primitive_poses: 37 | - position: [1.499997371966598, 0.8500025284426187, 0.3452295774036421] 38 | orientation: [3.991390862408021e-07, -3.058623738022737e-07, 1.212497197604013e-06, 0.9999999999991385] 39 | - header: 40 | frame_id: base_link 41 | id: table_leg_left_front 42 | primitives: 43 | - type: box 44 | dimensions: [0.05, 0.05, 0.7] 45 | primitive_poses: 46 | - position: [0.5999973719694124, 0.8500003459478829, 0.3452290268504982] 47 | orientation: [3.991390862408021e-07, -3.058623738022737e-07, 1.212497197604013e-06, 0.9999999999991385] 48 | - header: 49 | frame_id: base_link 50 | id: table_leg_right_back 51 | primitives: 52 | - type: box 53 | dimensions: [0.05, 0.05, 0.7] 54 | primitive_poses: 55 | - position: [1.500001494457485, -0.8499974715518412, 0.3452282203320098] 56 | orientation: [3.991390862408021e-07, -3.058623738022737e-07, 1.212497197604013e-06, 0.9999999999991385] 57 | - header: 58 | frame_id: base_link 59 | id: table_leg_right_front 60 | primitives: 61 | - type: box 62 | dimensions: [0.05, 0.05, 0.7] 63 | primitive_poses: 64 | - position: [0.6000014944602995, -0.8499996540465774, 0.3452276697788659] 65 | orientation: [3.991390862408021e-07, -3.058623738022737e-07, 1.212497197604013e-06, 0.9999999999991385] 66 | - header: 67 | frame_id: base_link 68 | id: table_top 69 | primitives: 70 | - type: box 71 | dimensions: [1.2, 2, 0.04] 72 | primitive_poses: 73 | - position: [1.049999219386779, 1.157950802175581e-06, 0.695228623201083] 74 | orientation: [3.990318462230049e-07, -3.056647647420437e-07, 1.212497314272982e-06, 0.9999999999991386] 75 | -------------------------------------------------------------------------------- /robowflex_tesseract/scripts/fetch_tabletop_goalpose.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Carlos Quintero Pena*/ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace robowflex; 13 | 14 | /* \file fetch_tabletop_goalpose.cpp 15 | * A simple script that shows how to use TrajOpt to plan in a manipulation task. The scene and request are 16 | * loaded from yaml files. Instead of providing a goal state, a goal pose for the end effector is encoded as a 17 | * cartesian constraint. An RVIZ Helper object is used to visualize the start/end states and the computed 18 | * trajectory. 19 | */ 20 | 21 | static const std::string GROUP = "arm"; 22 | 23 | int main(int argc, char **argv) 24 | { 25 | // Startup ROS 26 | ROS ros(argc, argv); 27 | 28 | // Create the default Fetch robot. 29 | auto fetch = std::make_shared(); 30 | fetch->initialize(false); 31 | const auto &ee = fetch->getModel()->getEndEffectors()[0]->getLinkModelNames()[0]; 32 | 33 | // Load tabletop scene. 34 | auto scene = std::make_shared(fetch); 35 | scene->fromYAMLFile("package://robowflex_tesseract/scenes/table/scene.yaml"); 36 | 37 | // Attach object to end effector. 38 | scene->attachObject(*fetch->getScratchState(), "Can1"); 39 | 40 | // Create a TrajOpt planner for Fetch. 41 | auto planner = std::make_shared(fetch, GROUP); 42 | planner->initialize("torso_lift_link", "gripper_link"); 43 | 44 | // Set planner parameters. 45 | planner->options.num_waypoints = 10; // Select number of waypoints in trajectory. 46 | planner->options.joint_vel_coeffs = 20.0; // Set weights for velocity costs. 47 | 48 | // Load request. 49 | const auto &request = std::make_shared(fetch); 50 | request->fromYAMLFile("package://robowflex_tesseract/scenes/table/request.yaml"); 51 | const auto &start_state = request->getStartConfiguration(); 52 | const auto &goal_state = request->getGoalConfiguration(); 53 | const auto &goal_ee_pose = goal_state->getFrameTransform(ee); 54 | 55 | // RVIZ helper. 56 | const auto &rviz = std::make_shared(fetch); 57 | rviz->updateScene(scene); 58 | rviz->visualizeState(start_state); 59 | 60 | RBX_INFO("Visualizing start state"); 61 | RBX_INFO("Press Enter to continue"); 62 | std::cin.ignore(); 63 | 64 | // Do motion planning using a goal pose for the end effector. 65 | auto result = planner->plan(scene, start_state, goal_ee_pose, ee); 66 | if (result.first) 67 | rviz->updateTrajectory(planner->getTrajectory()); 68 | 69 | rviz->visualizeState(planner->getTrajectory()->getLastWayPointPtr()); 70 | 71 | RBX_INFO("Visualizing end state"); 72 | RBX_INFO("Press Enter to exit"); 73 | std::cin.ignore(); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /robowflex_tesseract/scripts/fetch_tabletop_goalstate.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Carlos Quintero Pena*/ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace robowflex; 13 | 14 | /* \file fetch_tabletop_goalstate.cpp 15 | * A simple script that shows how to use TrajOpt to plan in a manipulation task. The scene and request are 16 | * loaded from yaml files. An RVIZ Helper object is used to visualize the start/goal states and the computed 17 | * trajectory. 18 | */ 19 | 20 | static const std::string GROUP = "arm"; 21 | 22 | int main(int argc, char **argv) 23 | { 24 | // Startup ROS 25 | ROS ros(argc, argv); 26 | 27 | // Create the default Fetch robot. 28 | auto fetch = std::make_shared(); 29 | fetch->initialize(false); 30 | 31 | // Load tabletop scene. 32 | auto scene = std::make_shared(fetch); 33 | scene->fromYAMLFile("package://robowflex_tesseract/scenes/table/scene.yaml"); 34 | 35 | // Attach object to end effector. 36 | scene->attachObject(*fetch->getScratchState(), "Can1"); 37 | 38 | // Create a TrajOpt planner for Fetch. 39 | auto planner = std::make_shared(fetch, GROUP); 40 | planner->initialize("torso_lift_link", "gripper_link"); 41 | 42 | // Set planner parameters. 43 | planner->options.num_waypoints = 8; // Select number of waypoints in trajectory 44 | planner->setInitType(trajopt::InitInfo::Type::JOINT_INTERPOLATED); // Initialize using a straight-line 45 | // between start and goal in C-space. 46 | 47 | // Load request. 48 | const auto &request = std::make_shared(fetch); 49 | request->fromYAMLFile("package://robowflex_tesseract/scenes/table/request.yaml"); 50 | 51 | // RVIZ helper. 52 | const auto &rviz = std::make_shared(fetch); 53 | rviz->updateScene(scene); 54 | rviz->visualizeState(request->getStartConfiguration()); 55 | 56 | RBX_INFO("Visualizing start state"); 57 | RBX_INFO("Press Enter to continue"); 58 | std::cin.ignore(); 59 | 60 | // Do motion planning. 61 | const auto &res = planner->plan(scene, request->getRequest()); 62 | if (res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS) 63 | rviz->updateTrajectory(res); 64 | 65 | rviz->visualizeState(request->getGoalConfiguration()); 66 | 67 | RBX_INFO("Visualizing goal state"); 68 | RBX_INFO("Press Enter to exit"); 69 | std::cin.ignore(); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /robowflex_tesseract/scripts/fetch_trajopt.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Carlos Quintero Pena */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace robowflex; 13 | 14 | /* \file fetch_trajopt.cpp 15 | * A basic script that demonstrates using Tesseract's TrajOpt planner. The 16 | * resulting trajectory is output to a YAML file. This file can be visualized 17 | * using Blender. See the corresponding robowflex_visualization readme. 18 | */ 19 | 20 | static const std::string GROUP = "arm"; 21 | 22 | int main(int argc, char **argv) 23 | { 24 | // Startup ROS 25 | ROS ros(argc, argv); 26 | 27 | // Create the default Fetch robot. 28 | auto fetch = std::make_shared(); 29 | fetch->initialize(false); 30 | 31 | // Create an empty scene. 32 | auto scene = std::make_shared(fetch); 33 | 34 | // Create a TrajOpt planner for Fetch. 35 | auto planner = std::make_shared(fetch, GROUP); 36 | planner->initialize("torso_lift_link", "gripper_link"); 37 | planner->options.num_waypoints = 3; 38 | 39 | // Create a motion planning request with a pose goal. 40 | MotionRequestBuilder request(planner, GROUP); 41 | fetch->setGroupState(GROUP, {0.201, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); 42 | request.setStartConfiguration(fetch->getScratchState()); 43 | 44 | fetch->setGroupState(GROUP, {1.301, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); 45 | request.setGoalConfiguration(fetch->getScratchState()); 46 | 47 | // Do motion planning! 48 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 49 | if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 50 | return 1; 51 | 52 | // Create a trajectory object for better manipulation. 53 | auto trajectory = std::make_shared(res.trajectory_); 54 | 55 | // Output path to a file for visualization. 56 | trajectory->toYAMLFile("fetch_trajopt_path.yml"); 57 | 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /robowflex_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1...3.10) 2 | project(robowflex_visualization 3 | VERSION 1.3 4 | ) 5 | 6 | find_package(catkin QUIET COMPONENTS 7 | robowflex_library 8 | collada_urdf 9 | ) 10 | 11 | catkin_python_setup() 12 | 13 | catkin_package( 14 | INCLUDE_DIRS 15 | LIBRARIES robowflex_visualization 16 | CATKIN_DEPENDS 17 | robowflex_library 18 | collada_urdf 19 | DEPENDS 20 | ) 21 | 22 | if (collada_urdf_DIR) 23 | install(PROGRAMS 24 | scripts/robowflex.py 25 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 26 | ) 27 | else() 28 | message("`collada_urdf` not found, not compiling robowflex_visualization library or executables.") 29 | endif() 30 | -------------------------------------------------------------------------------- /robowflex_visualization/README.md: -------------------------------------------------------------------------------- 1 | # Robowflex Visualization 2 | Visualize motion plans and robots in tandem with `robowflex_library` in [Blender](https://www.blender.org/). 3 | 4 | ## Requirements 5 | 1. Blender must be installed with COLLADA loading support (`*.dae`). 6 | 2. `python3-yaml` for loading YAML files. 7 | 3. The ROS package `collada_urdf` for translating URDFs into COLLADA files. 8 | 9 | `package://` URIs are supported by calling the `rospack` command-line tool. 10 | Blender should be run in an environment with your ROS workspace sourced. 11 | 12 | ## Design 13 | This package provides the `robowflex_visualization` Python library intended for use within Blender. 14 | All code for this library is within the `robowflex_visualization` folder. 15 | 16 | ## Usage 17 | 18 | 1. Dump a trajectory to a YAML file via the `robowflex::Trajectory::toYAMLFile()` function. 19 | An example of this can be found in `robowflex_library/scripts/fetch_test.cpp`: 20 | ```cpp 21 | ... 22 | // Generate a plan 23 | planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); 24 | 25 | // Output path to a file for visualization. 26 | robowflex::Trajectory trajectory(res.trajectory_); 27 | trajectory.toYAMLFile("fetch_path.yml"); 28 | ... 29 | ``` 30 | 31 | Planning scenes can be edited manually by writing the YAML file (an example is in `robowflex_library/yaml/test.yml`), or by calling `robowflex::Scene::toYAMLFile()` on a loaded planning scene. 32 | 33 | 2. Edit the provided file, `scripts/robowflex.py` to use your desired files. 34 | Note the loaded files for the trajectory and scene in the following script: 35 | ~~~py 36 | import robowflex_visualization as rv 37 | 38 | # Load a robot under the name `Fetch` 39 | fetch = rv.robot.Robot("Fetch", "package://fetch_description/robots/fetch.urdf") 40 | 41 | # Animate a trajectory 42 | fetch.animate_path("package://robowflex_visualization/yaml/fetch_path.yml") 43 | 44 | # Add a planning scene. 45 | scene = rv.scene.Scene("Scene", "package://robowflex_library/yaml/test_fetch.yml") 46 | ~~~ 47 | 48 | 49 | 3. Open blender from the current terminal and open the provided `robowflex.blend` Blender scene. 50 | To the left should be a baked in Python script (if there is no script, it is also available at `scripts/blender.py`). 51 | This script sets up the system path for the built-in Blender Python to look at your Python3 system path as well as load the `robowflex_visualization` Python module. 52 | Run this script to execute whatever code you have in `scripts/robowflex.py`. 53 | You can change what script is called. 54 | 55 | 4. Lather, rinse, repeat! 56 | You can edit code outside the Blender editor, everything should be reload appropriately. 57 | 58 | ## Examples 59 | 60 | ### `robowflex_library/scripts/fetch_test.cpp` 61 | You can render still images of robots. 62 | 63 | 64 | ### `robowflex_library/scripts/ur5_test.cpp` 65 | As well as full animations of motion plans. 66 | 67 | -------------------------------------------------------------------------------- /robowflex_visualization/old/README.md: -------------------------------------------------------------------------------- 1 | # Robowflex Visualization 2 | Visualize motion plans and robots in tandem with `robowflex_library` in [Blender](https://www.blender.org/). 3 | 4 | ## Requirements 5 | 1. Blender must be installed with COLLADA loading support (`*.dae`). 6 | 2. `python3-yaml` for loading YAML files. 7 | 8 | Optionally, `python3-rospkg` can be installed to support resolving of `package://` URIs. 9 | If not found, this is supported by calling the `rospack` command-line tool. 10 | 11 | 12 | ## Usage 13 | 14 | 1. Generate the robot geometry and a motion plan you would like to visualize using `robowflex_library`. 15 | Robot geometry can be generated by calling `robowflex::Robot::dumpGeometry()` on a loaded robot. 16 | A path can be generated by calling `robowflex::Robot::dumpPathTransforms()` on a loaded robot with a computed plan. 17 | An example of this can be found in `robowflex_library/scripts/ur5_test.cpp` and `robowflex_library/scripts/fetch_test.cpp`. 18 | Planning scenes can be edited manually by writing the YAML file (an example is in `robowflex_library/yaml/test.yml`), or by calling `robowflex::Scene::toYAMLFile()` on a loaded planning scene. 19 | 20 | 2. Edit the provided file, `scripts/blender.py` to use your desired files. 21 | ```py 22 | # blender.py 23 | ... 24 | 25 | if __name__ == '__main__': 26 | ... 27 | # Load a robot and a path to animate 28 | blender_robot.animate_robot( 29 | 'my_geometry.yml', # Robot geometry 30 | ['my_path.yml'] # Robot paths 31 | ) 32 | 33 | # Load a planning scene 34 | blender_scene.add_planning_scene('my_scene.yml') 35 | 36 | # Optionally, configure camera, lighting, and other parameters through a YAML file. 37 | blender_render.add_blender_scene('my_settings.yml') 38 | ``` 39 | 40 | 3. Open blender from the current terminal (`blender`). 41 | It's best to run blender from the same directory where you have the `blender.py` script. 42 | 4. Open the [python console](https://docs.blender.org/manual/en/dev/editors/python_console.html) from the blender window. 43 | 5. In the python console, type the following: 44 | ```py 45 | filename = 'blender.py' 46 | exec(compile(open(filename).read(), filename, 'exec')) 47 | ``` 48 | 49 | 6. Repeat while you develop the script outside of blender 50 | (Blender's python editing isn't really worth it.) 51 | 52 | ## Examples 53 | 54 | ### `robowflex_library/scripts/fetch_test.cpp` 55 | You can render still images of robots. 56 | 57 | 58 | ### `robowflex_library/scripts/ur5_test.cpp` 59 | As well as full animations of motion plans. 60 | 61 | -------------------------------------------------------------------------------- /robowflex_visualization/old/blender.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import imp 4 | import subprocess 5 | 6 | def initialize_path(): 7 | '''Initialize Blender Python's system path with the system Python's paths. 8 | ''' 9 | try: 10 | output = subprocess.check_output( 11 | ["python3", "-c", "import sys; print('\\n'.join(sys.path))"]) 12 | paths = output.decode().strip().split('\n') 13 | 14 | for path in paths: 15 | if not path in sys.path: 16 | sys.path.append(path) 17 | print("Adding path {} to system path.".format(path)) 18 | 19 | except subprocess.CalledProcessError: 20 | print("Unable to call system Python3") 21 | return "" 22 | 23 | 24 | def find_package(package): 25 | '''Find a ROS package path. 26 | ''' 27 | try: 28 | output = subprocess.check_output(["rospack", "find", package]) 29 | return output.decode().strip() 30 | 31 | except subprocess.CalledProcessError: 32 | print("Unable to find package: `{}`".format(package)) 33 | return "" 34 | 35 | 36 | def load_robowflex_module(module): 37 | '''Load a robowflex visualization module. 38 | ''' 39 | package = "robowflex_visualization" 40 | directory = find_package(package) 41 | 42 | fp, pathname, description = imp.find_module( 43 | module, [os.path.join(directory, "src")]) 44 | imp.load_module("robowflex_" + module, fp, pathname, description) 45 | 46 | 47 | if __name__ == '__main__': 48 | initialize_path() 49 | load_robowflex_module("utils") 50 | -------------------------------------------------------------------------------- /robowflex_visualization/old/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | '''Simple functions you shouldn't have to worry about. Copied as much as possible from `robowflex_library`, and should 3 | stay updated with that. 4 | 5 | ''' 6 | 7 | import logging 8 | import os.path 9 | import subprocess 10 | 11 | have_rospkg = False 12 | try: 13 | import rospkg 14 | have_rospkg = True 15 | except ImportError: 16 | pass 17 | 18 | import yaml 19 | try: 20 | from yaml import CLoader as Loader 21 | except ImportError: 22 | from yaml import Loader 23 | 24 | 25 | def resolve_package(path): 26 | '''Resolves `package://` URLs to their canonical form. The path does not need 27 | to exist, but the package does. Can be used to write new files in packages. 28 | Returns "" on failure. 29 | 30 | ''' 31 | if not path: 32 | return '' 33 | 34 | package_name = '' 35 | package_path1 = '' 36 | PREFIX = 'package://' 37 | if PREFIX in path: 38 | path = path[len(PREFIX):] # Remove 'package://' 39 | if '/' not in path: 40 | package_name = path 41 | path = '' 42 | else: 43 | package_name = path[:path.find('/')] 44 | path = path[path.find('/'):] 45 | 46 | if have_rospkg: 47 | rospack = rospkg.RosPack() 48 | package_path1 = rospack.get_path(package_name) 49 | else: 50 | package_path1 = subprocess.check_output( 51 | ["rospack", "find", package_name]).decode().strip() 52 | 53 | elif '~' in path: 54 | path = os.path.expanduser(path) 55 | 56 | new_path = os.path.realpath(package_path1 + path) 57 | return new_path 58 | 59 | 60 | def resolve_path(path): 61 | '''Resolves `package://` URLs and relative file paths to their canonical form. 62 | Returns "" on failure. 63 | 64 | ''' 65 | full_path = resolvePackage(path) 66 | if not os.path.exists(full_path): 67 | logging.warn('File {} does not exist'.format(full_path)) 68 | return '' 69 | return full_path 70 | 71 | 72 | def read_YAML_data(file_name): 73 | '''Returns the yaml data structure of the data stored. 74 | 75 | ''' 76 | full_name = resolvePath(file_name) 77 | if not full_name: 78 | logging.warn('Cannot open {}'.format(file_name)) 79 | return None 80 | with open(full_name) as input_file: 81 | return yaml.load(input_file.read(), Loader = Loader) 82 | -------------------------------------------------------------------------------- /robowflex_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robowflex_visualization 4 | 1.3.0 5 | Robowflex: Blender Visualization 6 | 7 | Zachary Kingston 8 | Zachary Kingston 9 | 10 | BSD 11 | https://github.com/KavrakiLab/robowflex/issues 12 | https://github.com/KavrakiLab/robowflex 13 | 14 | catkin 15 | 16 | robowflex_library 17 | rospy 18 | collada_urdf 19 | 20 | -------------------------------------------------------------------------------- /robowflex_visualization/robowflex.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KavrakiLab/robowflex/77cf34861b05f1acf7dfe007c966ad82d1365480/robowflex_visualization/robowflex.blend -------------------------------------------------------------------------------- /robowflex_visualization/robowflex_visualization/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["robot", "scene", "primitives", "utils"] 2 | 3 | import robowflex_visualization.robot as robot 4 | import robowflex_visualization.scene as scene 5 | import robowflex_visualization.primitives as primitives 6 | import robowflex_visualization.utils as utils 7 | -------------------------------------------------------------------------------- /robowflex_visualization/scripts/blender.py: -------------------------------------------------------------------------------- 1 | import bpy 2 | import sys 3 | import os 4 | import importlib.util 5 | import subprocess 6 | """Run this script in order to execute: 7 | `robowflex_visualization/src/robowflex.py` 8 | 9 | This script contains some utility functions for loading system Python paths 10 | and finding the ROS package. You can change which script is ran by changing 11 | the arguments to `load_ROS_module` at the bottom of the file. 12 | 13 | Moreover, the `robowflex_visualization/src` directory is added to the path, 14 | so that the various helper modules can be found. 15 | 16 | You should put all of your visualization code in whatever file is loaded at 17 | the bottom. 18 | """ 19 | 20 | 21 | def add_path(path): 22 | """Add a path to the system search path. 23 | """ 24 | if not path in sys.path: 25 | sys.path.append(path) 26 | print("Adding path {} to system path.".format(path)) 27 | 28 | 29 | def initialize_path(): 30 | """Initialize Blender Python's system path with the system Python's paths. 31 | """ 32 | try: 33 | output = subprocess.check_output( 34 | ["python3", "-c", "import sys; print('\\n'.join(sys.path))"]) 35 | paths = output.decode().strip().split('\n') 36 | 37 | for path in paths: 38 | add_path(path) 39 | 40 | except subprocess.CalledProcessError: 41 | print("Unable to call system Python3") 42 | return "" 43 | 44 | 45 | def find_package(package): 46 | """Find a ROS package path. 47 | """ 48 | try: 49 | output = subprocess.check_output(["rospack", "find", package]) 50 | return output.decode().strip() 51 | 52 | except subprocess.CalledProcessError: 53 | print("Unable to find package: `{}`".format(package)) 54 | return "" 55 | 56 | 57 | def initialize_robowflex_path(): 58 | """Adds the robowflex_visualization/src folder to search path. 59 | """ 60 | directory = find_package("robowflex_visualization") 61 | add_path(directory) 62 | 63 | 64 | def load_ROS_module(module_name, 65 | module_file, 66 | package = "robowflex_visualization"): 67 | """Load a robowflex visualization module. 68 | """ 69 | directory = find_package(package) 70 | 71 | spec = importlib.util.spec_from_file_location( # 72 | module_name, os.path.join(directory, module_file)) 73 | 74 | module = importlib.util.module_from_spec(spec) 75 | spec.loader.exec_module(module) 76 | 77 | 78 | if __name__ == '__main__': 79 | bpy.ops.outliner.orphans_purge() 80 | initialize_path() 81 | initialize_robowflex_path() 82 | load_ROS_module("robowflex", "scripts/robowflex.py") 83 | -------------------------------------------------------------------------------- /robowflex_visualization/scripts/robowflex.py: -------------------------------------------------------------------------------- 1 | import importlib 2 | import robowflex_visualization as rv 3 | import math 4 | 5 | # reload modules, as Blender likes to keep things around. 6 | importlib.reload(rv) 7 | importlib.reload(rv.robot) 8 | importlib.reload(rv.scene) 9 | importlib.reload(rv.primitives) 10 | importlib.reload(rv.utils) 11 | 12 | # Load a Fetch robot. 13 | fetch = rv.robot.Robot("Fetch", "package://robowflex_resources/fetch/robots/fetch.urdf") 14 | 15 | # Open the Gripper 16 | fetch.set_joint("l_gripper_finger_joint", 0.05) 17 | fetch.set_joint("r_gripper_finger_joint", 0.05) 18 | 19 | # Keyframe the head at neutral 20 | fetch.set_joint("head_pan_joint", 0) 21 | fetch.set_joint("head_tilt_joint", 0) 22 | fetch.add_keyframe("head_pan_joint", 0) 23 | fetch.add_keyframe("head_tilt_joint", 0) 24 | 25 | # Keyframe the head looking over at the block 26 | fetch.set_joint("head_pan_joint", math.radians(50)) 27 | fetch.set_joint("head_tilt_joint", math.radians(30)) 28 | fetch.add_keyframe("head_pan_joint", 60) 29 | fetch.add_keyframe("head_tilt_joint", 60) 30 | 31 | # Load a planning scene. 32 | scene = rv.scene.Scene("Scene", 33 | "package://robowflex_library/yaml/test_fetch.yml") 34 | 35 | # Animate a simple motion plan. 36 | frame = fetch.animate_path( 37 | "package://robowflex_visualization/yaml/fetch_pick.yml", 38 | 15, # FPS 39 | 30, # Start Frame 40 | ) 41 | 42 | # Keyframe the gripper at opening 43 | fetch.add_keyframe("l_gripper_finger_joint", frame) 44 | fetch.add_keyframe("r_gripper_finger_joint", frame) 45 | 46 | # Keyframe head still looking over 47 | fetch.add_keyframe("head_pan_joint", frame) 48 | fetch.add_keyframe("head_tilt_joint", frame) 49 | 50 | # Attach the cube to the gripper 51 | cube = scene.get_object("Cube3") 52 | fetch.attach_object("wrist_roll_link", cube, frame) 53 | 54 | # Close the Gripper 55 | fetch.set_joint("l_gripper_finger_joint", 0.04) 56 | fetch.set_joint("r_gripper_finger_joint", 0.04) 57 | fetch.add_keyframe("l_gripper_finger_joint", frame + 30) 58 | fetch.add_keyframe("r_gripper_finger_joint", frame + 30) 59 | 60 | # Animate the plan in reverse 61 | frame = fetch.animate_path( 62 | "package://robowflex_visualization/yaml/fetch_pick.yml", 63 | 15, # FPS 64 | frame + 30, # Start Frame 65 | True, # Reverse the Motion 66 | ) 67 | 68 | # Keyframe head returning to neutral 69 | fetch.set_joint("head_pan_joint", 0) 70 | fetch.set_joint("head_tilt_joint", 0) 71 | fetch.add_keyframe("head_pan_joint", frame) 72 | fetch.add_keyframe("head_tilt_joint", frame) 73 | -------------------------------------------------------------------------------- /robowflex_visualization/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name = 'robowflex_visualization', 5 | version = '1.3.0', 6 | description = 'Robowflex: Blender Visualization Tools', 7 | author = 'Zachary Kingston', 8 | author_email = 'zak@rice.edu', 9 | packages = ['robowflex_visualization'], 10 | ) 11 | -------------------------------------------------------------------------------- /tidy.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "${0%/*}" 4 | source .ci/tidy-functions.sh 5 | 6 | generate-compile-commands robowflex_library 7 | fix-tidy 8 | generate-compile-commands robowflex_ompl 9 | fix-tidy 10 | generate-compile-commands robowflex_movegroup 11 | fix-tidy 12 | generate-compile-commands robowflex_dart 13 | fix-tidy 14 | --------------------------------------------------------------------------------