├── .gitignore ├── .travis.rosinstall ├── .travis.yml ├── LICENSE ├── README.md ├── moveit_simple ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── moveit_simple_dynamic_reconfigure_.params ├── include │ └── moveit_simple │ │ ├── exceptions.h │ │ ├── joint_lock_options.h │ │ ├── joint_locker.h │ │ ├── moveit_simple.h │ │ ├── online_robot.h │ │ ├── point_types.h │ │ ├── prettyprint.hpp │ │ ├── prettyprint_LICENSE │ │ └── robot.h ├── package.xml ├── src │ ├── joint_lock_options.cpp │ ├── joint_locker.cpp │ ├── online_robot.cpp │ ├── point_types.cpp │ └── robot.cpp └── test │ ├── config │ └── test_display.rviz │ ├── joint_lock_options.cpp │ ├── joint_locker.cpp │ ├── kuka_kr210.cpp │ ├── kuka_kr210_utest.cpp │ ├── launch │ ├── kuka_kr210.launch │ ├── kuka_kr210_utest.launch │ ├── motoman_mh5.launch │ ├── motoman_mh5_utest.launch │ └── test_display.launch │ ├── motoman_mh5.cpp │ ├── motoman_mh5_utest.cpp │ ├── moveit_simple_utest.cpp │ └── resources │ ├── kuka_kr210 │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── kr210l150.urdf │ ├── kuka_custom_tool.urdf │ ├── kuka_kr210.srdf │ └── meshes │ │ ├── collision │ │ ├── base_link.stl │ │ ├── link_1.stl │ │ ├── link_2.stl │ │ ├── link_3.stl │ │ ├── link_4.stl │ │ ├── link_5.stl │ │ └── link_6.stl │ │ └── visual │ │ ├── base_link.STL │ │ ├── base_link.dae │ │ ├── link_1.dae │ │ ├── link_1.stl │ │ ├── link_2.dae │ │ ├── link_2.stl │ │ ├── link_3.dae │ │ ├── link_3.stl │ │ ├── link_4.dae │ │ ├── link_4.stl │ │ ├── link_5.dae │ │ ├── link_5.stl │ │ ├── link_6.dae │ │ └── link_6.stl │ └── motoman_mh5 │ ├── config │ ├── fake_controllers.yaml │ ├── joint_limits.yaml │ ├── joint_names.yaml │ ├── kinematics.yaml │ ├── motoman_mh5_robot.srdf │ └── ompl_planning.yaml │ ├── meshes │ └── mh5 │ │ ├── collision │ │ ├── MH5_BASE_AXIS.stl │ │ ├── MH5_B_AXIS.stl │ │ ├── MH5_L_AXIS.stl │ │ ├── MH5_R_AXIS.stl │ │ ├── MH5_S_AXIS.stl │ │ ├── MH5_T_AXIS.stl │ │ └── MH5_U_AXIS.stl │ │ └── visual │ │ ├── MH5_BASE_AXIS.stl │ │ ├── MH5_B_AXIS.stl │ │ ├── MH5_L_AXIS.stl │ │ ├── MH5_R_AXIS.stl │ │ ├── MH5_S_AXIS.stl │ │ ├── MH5_T_AXIS.stl │ │ └── MH5_U_AXIS.stl │ └── urdf │ ├── mh5_macro.xacro │ ├── motoman_mh5_robot.xacro │ └── motoman_mh5_robot_macro.xacro └── moveit_simple_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg └── CombinedJointPoint.msg ├── package.xml └── srv ├── LookupTrajectory.srv └── LookupWaypoint.srv /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE -------------------------------------------------------------------------------- /.travis.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: industrial_experimental, uri: 'https://github.com/ros-industrial/industrial_experimental.git', version: indigo-devel} 2 | - git: {local-name: motoman, uri: 'https://github.com/ros-industrial/motoman.git', version: kinetic-devel} 3 | - git: {local-name: motoman-experimental, uri: 'https://github.com/ros-industrial/motoman_experimental', version: kinetic-devel} -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | notifications: 7 | email: 8 | on_failure: always 9 | recipients: 10 | - shaun.edwards@gmail.com 11 | env: 12 | matrix: 13 | - ROS_DISTRO="kinetic" UPSTREAM_WORKSPACE=file ROSINSTALL_FILENAME=.travis.rosinstall 14 | install: 15 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 16 | script: 17 | - source .ci_config/travis.sh -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Moveit Simple 2 | 3 | ## Installation 4 | 1. Clone repository into workspace [It doesn't have to be called moveit_simple_ws] 5 | ``` 6 | mkdir moveit_simple_ws && cd moveit_simple_ws 7 | mkdir src && cd src 8 | catkin_init_workspace 9 | git clone -b kinetic-devel https://github.com/plusone-robotics/moveit_simple.git 10 | ``` 11 | 12 | 2. Source Dependencies 13 | ``` 14 | wstool init . moveit_simple/.travis.rosinstall 15 | cd .. 16 | ``` 17 | 18 | 3. Package Dependencies 19 | ``` 20 | rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y 21 | ``` 22 | 23 | 4. Build 24 | ``` 25 | catkin build --cmake-args -DCMAKE_BUILD_TYPE=Debug 26 | or 27 | catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release 28 | 29 | source devel/setup.bash 30 | ``` 31 | 32 | ## Running Tests with Rviz Visualization 33 | ``` 34 | roslaunch moveit_simple test_display.launch 35 | rostest moveit_simple _utest.launch -r 36 | 37 | Example: 38 | rostest moveit_simple motoman_mh5_utest.launch -r 39 | ``` 40 | 41 | Refresh the RobotModel by un-checking and re-checking the checkbox. -------------------------------------------------------------------------------- /moveit_simple/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_simple 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2020-01-28) 6 | ------------------ 7 | * Fix version discrepancy `#100 `_ 8 | 9 | 0.2.0 (2019-07-03) 10 | ------------------ 11 | * [feature] Check to make sure that the goal has been reached (`#93 `_) 12 | * Contributors: Kris Kozak 13 | 14 | 0.1.0 (2019-04-30) 15 | ------------------ 16 | * Numerous bugfixes. 17 | * [feature] Add trajectory training service interface `#73 `_ 18 | * [feature] CombinedTrajectoryPoint type and made it a friend of JoinPoint and CartPoint 19 | * [feature] Joint Locking `#69 `_ 20 | * [feature] a method to refresh the robot `#66 `_ 21 | * [feature] rejected state to stop execution `#61 `_ 22 | * [feature] Check if action is still active before setting timeout to true `#60 `_ 23 | * [feature] non blocking execution call for online robot `#59 `_ 24 | * [improve] Moving the dynamic reconfigure server for move it simple from the sta… `#71 `_ 25 | * [removal] deleteAllMarkers, which was causing collision contact points to not show and use the met method of the smart pointer instead of whatever it was doing 26 | * Contributors: Aaron Wood, Austin Deric, Christina, G.A. vd. Hoorn, Geoffrey Chiou, Isaac I.Y. Saito, Joshua Curtis, Maulesh S Trivedi, Michael Görner, Mike Lautman, Shaun Edwards, Kris Kozak, nsnitish 27 | -------------------------------------------------------------------------------- /moveit_simple/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(moveit_simple) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | control_msgs 9 | dynamic_reconfigure 10 | eigen_conversions 11 | geometry_msgs 12 | moveit_core 13 | moveit_ros_planning 14 | moveit_simple_msgs 15 | moveit_visual_tools 16 | roscpp 17 | rosparam_handler 18 | sensor_msgs 19 | tf2 20 | tf2_geometry_msgs 21 | tf2_ros 22 | trajectory_msgs 23 | ) 24 | 25 | find_package(Boost REQUIRED) 26 | find_package(Eigen3 REQUIRED) 27 | 28 | generate_ros_parameter_files(cfg/moveit_simple_dynamic_reconfigure_.params) 29 | 30 | catkin_package( 31 | INCLUDE_DIRS 32 | include 33 | LIBRARIES 34 | ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | control_msgs 37 | dynamic_reconfigure 38 | eigen_conversions 39 | geometry_msgs 40 | moveit_simple_msgs 41 | roscpp 42 | rosparam_handler 43 | sensor_msgs 44 | tf2 45 | tf2_geometry_msgs 46 | tf2_ros 47 | trajectory_msgs 48 | DEPENDS 49 | Boost 50 | EIGEN3 51 | ) 52 | 53 | include_directories( 54 | include 55 | ${catkin_INCLUDE_DIRS} 56 | ${Boost_INCLUDE_DIRS} 57 | ${EIGEN3_INCLUDE_DIRS} 58 | ) 59 | 60 | add_library(${PROJECT_NAME} 61 | src/joint_lock_options.cpp 62 | src/joint_locker.cpp 63 | src/online_robot.cpp 64 | src/point_types.cpp 65 | src/robot.cpp 66 | ) 67 | 68 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 69 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 70 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) 71 | 72 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 73 | 74 | if(CATKIN_ENABLE_TESTING) 75 | 76 | find_package(rostest REQUIRED) 77 | 78 | set(KUKA_KR210_SRC_FILES test/kuka_kr210_utest.cpp test/kuka_kr210.cpp) 79 | add_rostest_gtest(kuka_kr210_utest test/launch/kuka_kr210_utest.launch ${KUKA_KR210_SRC_FILES}) 80 | target_link_libraries(kuka_kr210_utest ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 81 | 82 | set(MOTOMAN_MH5_SRC_FILES test/motoman_mh5_utest.cpp test/motoman_mh5.cpp) 83 | add_rostest_gtest(motoman_mh5_utest test/launch/motoman_mh5_utest.launch ${MOTOMAN_MH5_SRC_FILES}) 84 | target_link_libraries(motoman_mh5_utest ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 85 | 86 | set(MOVEIT_SIMPLE_TEST_SRC_FILES test/moveit_simple_utest.cpp test/joint_locker.cpp test/joint_lock_options.cpp) 87 | catkin_add_gtest(moveit_simple_utest ${MOVEIT_SIMPLE_TEST_SRC_FILES}) 88 | target_link_libraries(moveit_simple_utest ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 89 | 90 | endif() 91 | 92 | install(TARGETS ${PROJECT_NAME} 93 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 96 | 97 | install(DIRECTORY include/${PROJECT_NAME}/ 98 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 99 | -------------------------------------------------------------------------------- /moveit_simple/cfg/moveit_simple_dynamic_reconfigure_.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "moveit_simple" 3 | 4 | from rosparam_handler.parameter_generator_catkin import * 5 | gen = ParameterGenerator() 6 | 7 | gen.add("speed_modifier", paramtype="double", description="Speed Modifier for Execution", min=0.01, max=1.0, default=0.25, configurable=True) 8 | gen.add("joint_equality_tolerance", paramtype="double", description="Tolerance for equality between joint and cartesian representations of a point", min=0.0, max=100.0, default=0.005, configurable=True) 9 | 10 | exit(gen.generate(PACKAGE, PACKAGE, "moveit_simple_dynamic_reconfigure_")) -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/exceptions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef EXCEPTIONS_H 21 | #define EXCEPTIONS_H 22 | 23 | #include 24 | #include 25 | 26 | namespace moveit_simple 27 | { 28 | /** 29 | * @brief ExecutionFailureException: An exception class to notify 30 | * execution failure 31 | * 32 | * This inherits from std::runtime_error. 33 | * This is an exception class to be thrown when sendGoalAndWait method 34 | * has failed execution. 35 | */ 36 | class ExecutionFailureException : public std::runtime_error 37 | { 38 | public: 39 | ExecutionFailureException(const std::string errorDescription) 40 | : std::runtime_error(errorDescription) { } 41 | }; 42 | 43 | /** 44 | * @brief IKFailException: An exception class to notify IK failure 45 | * 46 | * This inherits from std::runtime_error. 47 | * This is an exception class to be thrown when IK call fails to return 48 | * joint solution. 49 | */ 50 | class IKFailException : public std::runtime_error 51 | { 52 | public: 53 | IKFailException(const std::string errorDescription) 54 | : std::runtime_error(errorDescription) { } 55 | }; 56 | 57 | /** 58 | * @brief IKSolverTransformException: 59 | * 60 | * This inherits from std::runtime_error. 61 | * This is an exception class to be thrown when a custom iksolver is 62 | * used but the required transforms cannot be calculated 63 | */ 64 | class IKSolverTransformException : public std::runtime_error 65 | { 66 | public: 67 | IKSolverTransformException(const std::string error_description) 68 | : std::runtime_error(error_description) { } 69 | }; 70 | 71 | /** 72 | * @brief CollisionDetected: An exception class to notify collision 73 | * 74 | * This inherits from std::runtime_error. 75 | * This is an exception class to be thrown when collision is detected 76 | * at any interpolated point in the tarjectory. 77 | */ 78 | class CollisionDetected : public std::runtime_error 79 | { 80 | public: 81 | CollisionDetected(const std::string errorDescription) 82 | : std::runtime_error(errorDescription) { } 83 | }; 84 | 85 | /** 86 | * @brief JointSeedException: 87 | * 88 | * This inherits from std::runtime_error. 89 | * This is an exception class to be thrown when a pre-defined joint 90 | * seed is given for collision and reach checking but it does not 91 | * actually exist. 92 | */ 93 | class JointSeedException : public std::runtime_error 94 | { 95 | public: 96 | JointSeedException(const std::string error_description) 97 | : std::runtime_error(error_description) { } 98 | }; 99 | } // namespace moveit_simple 100 | #endif // EXCEPTIONS_H -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/joint_lock_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef JOINT_LOCK_OPTIONS_H 20 | #define JOINT_LOCK_OPTIONS_H 21 | 22 | namespace moveit_simple 23 | { 24 | 25 | typedef unsigned char uchar_t; 26 | 27 | // Only supports robots with 6 joints. 28 | enum class JointLockOptions : uchar_t 29 | { 30 | LOCK_NONE = 0x00, 31 | LOCK_JOINT_1 = 1 << 0, 32 | LOCK_JOINT_2 = 1 << 1, 33 | LOCK_JOINT_3 = 1 << 2, 34 | LOCK_JOINT_4 = 1 << 3, 35 | LOCK_JOINT_5 = 1 << 4, 36 | LOCK_JOINT_6 = 1 << 5, 37 | }; 38 | 39 | JointLockOptions operator&(JointLockOptions lhs, JointLockOptions rhs); 40 | 41 | JointLockOptions operator|(JointLockOptions lhs, JointLockOptions rhs); 42 | 43 | } // namespace moveit_simple 44 | 45 | #endif // JOINT_LOCK_OPTIONS_H 46 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/joint_locker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef JOINT_LOCKER_H 20 | #define JOINT_LOCKER_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace moveit_simple 27 | { 28 | 29 | struct JointLocker 30 | { 31 | static void lockJoints(const std::vector &prev_joints, std::vector ¤t_joints, 32 | JointLockOptions options = JointLockOptions::LOCK_NONE); 33 | 34 | static std::string resolveToString(const JointLockOptions options = JointLockOptions::LOCK_NONE); 35 | }; 36 | 37 | 38 | } // namespace moveit_simple 39 | 40 | #endif // JOINT_LOCKER_H 41 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/moveit_simple.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef MOVEIT_SIMPLE_H 21 | #define MOVEIT_SIMPLE_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #endif // MOVEIT_SIMPLE_H 28 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/online_robot.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef ONLINE_ROBOT_H 21 | #define ONLINE_ROBOT_H 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace moveit_simple 32 | { 33 | /** 34 | * @brief OnlineRobot is a wrapper around standard MoveIt objects. 35 | * It inherits from Robot. Added assumption: 36 | * motion is executed via a "JointTrajectoryAction" interface 37 | */ 38 | class OnlineRobot : public Robot 39 | { 40 | public: 41 | /** 42 | * @brief Constructor 43 | */ 44 | OnlineRobot(const ros::NodeHandle &nh, const std::string &robot_description, 45 | const std::string &group_name); 46 | 47 | /** 48 | * @brief Constructor for the case where the IK implementation does not match the 49 | * SRDF. For example: IKFast solutions are generally solved for the base_link to 50 | * tool0 of a robot, and the robot_description is defined for some world_frame to 51 | * some tcp frame. 52 | */ 53 | OnlineRobot(const ros::NodeHandle &nh, const std::string &robot_description, 54 | const std::string &group_name, const std::string &ik_base_frame, 55 | const std::string &ik_tip_frame); 56 | 57 | /** 58 | * @brief execute a given trajectory 59 | * @param traj_name - name of trajectory to be executed (must be filled with 60 | * prior calls to "addTrajPoint". 61 | * @param collision_check - bool to turn check for collision on\off 62 | * @throws (Execution failure) 63 | * @throws (Conversion to joint trajectory failed) 64 | * @throws (Trajectory "traj_name" not found) 65 | * @throws (One of interpolated point is 66 | * in Collision with itself or environment) 67 | */ 68 | void execute(const std::string traj_name, bool collision_check = false); 69 | 70 | /** 71 | * @brief execute a given planned joint trajectory 72 | * @param goal - Joint Trajectory goal which is a known 'Plan' 73 | * @param collision_check - bool to turn check for collision on\off 74 | * @throws (Execution failure) 75 | * @throws (One of interpolated point is 76 | * in Collision with itself or environment) 77 | */ 78 | void execute(std::vector &goal, bool collision_check = false); 79 | 80 | /** 81 | * @brief Starts execution for a given trajectory, non blocking 82 | * @param traj_name - name of trajectory to be executed (must be filled with 83 | * prior calls to "addTrajPoint". 84 | * @param collision_check - bool to turn check for collision on\off 85 | * @throws (Execution failure) 86 | * @throws (Conversion to joint trajectory failed) 87 | * @throws (Trajectory "traj_name" not found) 88 | * @throws (One of interpolated point is 89 | * in Collision with itself or environment) 90 | */ 91 | void startExecution(const std::string &traj_name, bool collision_check = false); 92 | 93 | /** 94 | * @brief Checks if the non-blocking execute is still executing 95 | * @return bool - true if it is, false if not 96 | */ 97 | bool isExecuting(); 98 | 99 | /** 100 | * @brief Stops the non-blocking execution 101 | */ 102 | void stopExecution(); 103 | 104 | /** 105 | * @brief getJointState - Returns a vector of the 106 | * current joint positions of the robot from current_robot_state_. 107 | * 108 | * @return std::vector current_joint_position 109 | */ 110 | virtual std::vector getJointState(void) const; 111 | 112 | protected: 113 | void executing(const ros::Time &timeout); 114 | 115 | void updateCurrentState(const sensor_msgs::JointStateConstPtr &msg); 116 | 117 | protected: 118 | mutable moveit::core::RobotStatePtr current_robot_state_; 119 | moveit_visual_tools::MoveItVisualToolsPtr online_visual_tools_; 120 | actionlib::SimpleActionClient action_; 121 | ros::Subscriber j_state_sub_; 122 | std::atomic is_timed_out_; 123 | }; 124 | } // namespace moveit_simple 125 | #endif // ONLINE_ROBOT_H 126 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/point_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef POINT_TYPES_H 21 | #define POINT_TYPES_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | namespace moveit_simple 31 | { 32 | class Robot; 33 | class TrajectoryPoint; 34 | class JointTrajectoryPoint; 35 | class CartTrajectoryPoint; 36 | class CombinedTrajectoryPoint; 37 | 38 | namespace interpolation_type 39 | { 40 | enum InterpolationType 41 | { 42 | UNKNOWN = 0, 43 | JOINT = 1, 44 | CARTESIAN = 2 45 | }; 46 | } // namespace interpolation_type 47 | 48 | typedef interpolation_type::InterpolationType InterpolationType; 49 | 50 | struct TrajectoryPointInfo 51 | { 52 | std::unique_ptr point; 53 | InterpolationType type; 54 | unsigned int num_steps; 55 | }; 56 | 57 | typedef std::vector TrajectoryInfo; 58 | 59 | class TrajectoryPoint 60 | { 61 | public: 62 | friend class Robot; 63 | 64 | virtual ~TrajectoryPoint() { } 65 | 66 | const double time() const 67 | { 68 | return t_; 69 | } 70 | 71 | const std::string &name() const 72 | { 73 | return name_; 74 | } 75 | 76 | void setJointLockOptions(const JointLockOptions &options); 77 | 78 | JointLockOptions getJointLockOptions(); 79 | 80 | protected: 81 | enum PointType 82 | { 83 | UNKNOWN = 0, 84 | JOINT = 1, 85 | CARTESIAN = 2 86 | }; 87 | 88 | TrajectoryPoint(std::string name, double t, PointType type, JointLockOptions options = JointLockOptions::LOCK_NONE) 89 | : name_(name), t_(t), type_(type), joint_lock_options_(options) { } 90 | 91 | const PointType &type() const 92 | { 93 | return type_; 94 | } 95 | 96 | virtual std::unique_ptr toJointTrajPoint(const Robot &robot, 97 | double timeout, const std::vector &seed, JointLockOptions options = JointLockOptions::LOCK_NONE) const = 0; 98 | 99 | virtual std::unique_ptr toCartTrajPoint(const Robot &robot) const = 0; 100 | 101 | double t_; 102 | std::string name_; 103 | PointType type_; 104 | JointLockOptions joint_lock_options_ = JointLockOptions::LOCK_NONE; 105 | }; 106 | 107 | 108 | 109 | 110 | class JointTrajectoryPoint : public TrajectoryPoint 111 | { 112 | public: 113 | friend class CombinedTrajectoryPoint; 114 | JointTrajectoryPoint() : TrajectoryPoint("", 0.0, PointType::JOINT) { } 115 | 116 | virtual ~JointTrajectoryPoint() { } 117 | 118 | JointTrajectoryPoint(std::vector &joint_point, double t, std::string name = std::string(), 119 | JointLockOptions options = JointLockOptions::LOCK_NONE) 120 | : TrajectoryPoint(name, t, PointType::JOINT, options), joint_point_(joint_point) { } 121 | 122 | const std::vector &jointPoint() const 123 | { 124 | return joint_point_; 125 | } 126 | 127 | protected: 128 | virtual std::unique_ptr toJointTrajPoint(const Robot &robot, 129 | double timeout, const std::vector &seed, JointLockOptions options = JointLockOptions::LOCK_NONE) const; 130 | 131 | virtual std::unique_ptr toCartTrajPoint(const Robot &robot) const; 132 | 133 | private: 134 | std::vector joint_point_; 135 | }; 136 | 137 | class CartTrajectoryPoint : public TrajectoryPoint 138 | { 139 | public: 140 | friend class CombinedTrajectoryPoint; 141 | CartTrajectoryPoint() : TrajectoryPoint("", 0.0, PointType::CARTESIAN) { } 142 | 143 | CartTrajectoryPoint(const Eigen::Affine3d pose, const double t, std::string name = std::string(), 144 | JointLockOptions options = JointLockOptions::LOCK_NONE) 145 | : TrajectoryPoint(name, t, PointType::CARTESIAN, options), pose_(pose) { } 146 | 147 | virtual ~CartTrajectoryPoint() { } 148 | 149 | const Eigen::Affine3d &pose() const 150 | { 151 | return pose_; 152 | } 153 | 154 | protected: 155 | virtual std::unique_ptr toJointTrajPoint(const Robot &robot, 156 | double timeout, const std::vector &seed, JointLockOptions options = JointLockOptions::LOCK_NONE) const; 157 | 158 | virtual std::unique_ptr toCartTrajPoint(const Robot &robot) const; 159 | 160 | private: 161 | Eigen::Affine3d pose_; 162 | }; 163 | 164 | class CombinedTrajectoryPoint : public TrajectoryPoint 165 | { 166 | public: 167 | enum PointPreference 168 | { 169 | CARTESIAN = 0, 170 | JOINT = 1 171 | }; 172 | 173 | CombinedTrajectoryPoint() : TrajectoryPoint("", 0.0, PointType::JOINT) 174 | { 175 | } 176 | 177 | CombinedTrajectoryPoint(std::vector &joint_point, const Eigen::Affine3d pose, double t, double tol, 178 | std::string name = std::string(), PointPreference type = PointPreference::JOINT, 179 | JointLockOptions options = JointLockOptions::LOCK_NONE, double timeout = 5.0) 180 | : TrajectoryPoint(name, t, (type == PointPreference::JOINT) ? PointType::JOINT : PointType::CARTESIAN, options) 181 | , options_(options) 182 | , joint_point_(joint_point) 183 | , pose_(pose) 184 | , tol_(tol) 185 | { 186 | } 187 | 188 | virtual ~CombinedTrajectoryPoint() { } 189 | 190 | const JointLockOptions &jointLockOptions() const 191 | { 192 | return options_; 193 | } 194 | 195 | const Eigen::Affine3d &pose() const 196 | { 197 | return pose_; 198 | } 199 | 200 | const std::vector &jointPoint() const 201 | { 202 | return joint_point_; 203 | } 204 | 205 | const double &timeout() const 206 | { 207 | return timeout_; 208 | } 209 | 210 | std::string pointVecToString(const std::vector &vec) const; 211 | 212 | protected: 213 | 214 | virtual std::unique_ptr 215 | toJointTrajPoint(const Robot &robot, double timeout, const std::vector &seed, 216 | JointLockOptions options = JointLockOptions::LOCK_NONE) const; 217 | 218 | virtual std::unique_ptr toCartTrajPoint(const Robot &robot) const; 219 | 220 | bool compareJointAndCart(const Robot &robot, double timeout) const; 221 | 222 | private: 223 | 224 | JointLockOptions options_; 225 | std::vector joint_point_; 226 | Eigen::Affine3d pose_; 227 | double tol_; 228 | double timeout_; 229 | }; 230 | } // namespace moveit_simple 231 | #endif // POINT_TYPES_H 232 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/prettyprint.hpp: -------------------------------------------------------------------------------- 1 | // Copyright Louis Delacroix 2010 - 2014. 2 | // Distributed under the Boost Software License, Version 1.0. 3 | // (See accompanying file LICENSE_1_0.txt or copy at 4 | // http://www.boost.org/LICENSE_1_0.txt) 5 | // 6 | // A pretty printing library for C++ 7 | // 8 | // Usage: 9 | // Include this header, and operator<< will "just work". 10 | 11 | #ifndef H_PRETTY_PRINT 12 | #define H_PRETTY_PRINT 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace pretty_print 26 | { 27 | namespace detail 28 | { 29 | // SFINAE type trait to detect whether T::const_iterator exists. 30 | 31 | struct sfinae_base 32 | { 33 | using yes = char; 34 | using no = yes[2]; 35 | }; 36 | 37 | template 38 | struct has_const_iterator : private sfinae_base 39 | { 40 | private: 41 | template static yes & test(typename C::const_iterator*); 42 | template static no & test(...); 43 | public: 44 | static const bool value = sizeof(test(nullptr)) == sizeof(yes); 45 | using type = T; 46 | }; 47 | 48 | template 49 | struct has_begin_end : private sfinae_base 50 | { 51 | private: 52 | template 53 | static yes & f(typename std::enable_if< 54 | std::is_same(&C::begin)), 55 | typename C::const_iterator(C::*)() const>::value>::type *); 56 | 57 | template static no & f(...); 58 | 59 | template 60 | static yes & g(typename std::enable_if< 61 | std::is_same(&C::end)), 62 | typename C::const_iterator(C::*)() const>::value, void>::type*); 63 | 64 | template static no & g(...); 65 | 66 | public: 67 | static bool const beg_value = sizeof(f(nullptr)) == sizeof(yes); 68 | static bool const end_value = sizeof(g(nullptr)) == sizeof(yes); 69 | }; 70 | 71 | } // namespace detail 72 | 73 | 74 | // Holds the delimiter values for a specific character type 75 | 76 | template 77 | struct delimiters_values 78 | { 79 | using char_type = TChar; 80 | const char_type * prefix; 81 | const char_type * delimiter; 82 | const char_type * postfix; 83 | }; 84 | 85 | 86 | // Defines the delimiter values for a specific container and character type 87 | 88 | template 89 | struct delimiters 90 | { 91 | using type = delimiters_values; 92 | static const type values; 93 | }; 94 | 95 | 96 | // Functor to print containers. You can use this directly if you want 97 | // to specificy a non-default delimiters type. The printing logic can 98 | // be customized by specializing the nested template. 99 | 100 | template , 103 | typename TDelimiters = delimiters> 104 | struct print_container_helper 105 | { 106 | using delimiters_type = TDelimiters; 107 | using ostream_type = std::basic_ostream; 108 | 109 | template 110 | struct printer 111 | { 112 | static void print_body(const U & c, ostream_type & stream) 113 | { 114 | using std::begin; 115 | using std::end; 116 | 117 | auto it = begin(c); 118 | const auto the_end = end(c); 119 | 120 | if (it != the_end) 121 | { 122 | for ( ; ; ) 123 | { 124 | stream << *it; 125 | 126 | if (++it == the_end) break; 127 | 128 | if (delimiters_type::values.delimiter != NULL) 129 | stream << delimiters_type::values.delimiter; 130 | } 131 | } 132 | } 133 | }; 134 | 135 | print_container_helper(const T & container) 136 | : container_(container) 137 | { } 138 | 139 | inline void operator()(ostream_type & stream) const 140 | { 141 | if (delimiters_type::values.prefix != NULL) 142 | stream << delimiters_type::values.prefix; 143 | 144 | printer::print_body(container_, stream); 145 | 146 | if (delimiters_type::values.postfix != NULL) 147 | stream << delimiters_type::values.postfix; 148 | } 149 | 150 | private: 151 | const T & container_; 152 | }; 153 | 154 | // Specialization for pairs 155 | 156 | template 157 | template 158 | struct print_container_helper::printer> 159 | { 160 | using ostream_type = typename print_container_helper::ostream_type; 161 | 162 | static void print_body(const std::pair & c, ostream_type & stream) 163 | { 164 | stream << c.first; 165 | if (print_container_helper::delimiters_type::values.delimiter != NULL) 166 | stream << print_container_helper::delimiters_type::values.delimiter; 167 | stream << c.second; 168 | } 169 | }; 170 | 171 | // Specialization for tuples 172 | 173 | template 174 | template 175 | struct print_container_helper::printer> 176 | { 177 | using ostream_type = typename print_container_helper::ostream_type; 178 | using element_type = std::tuple; 179 | 180 | template struct Int { }; 181 | 182 | static void print_body(const element_type & c, ostream_type & stream) 183 | { 184 | tuple_print(c, stream, Int<0>()); 185 | } 186 | 187 | static void tuple_print(const element_type &, ostream_type &, Int) 188 | { 189 | } 190 | 191 | static void tuple_print(const element_type & c, ostream_type & stream, 192 | typename std::conditional, std::nullptr_t>::type) 193 | { 194 | stream << std::get<0>(c); 195 | tuple_print(c, stream, Int<1>()); 196 | } 197 | 198 | template 199 | static void tuple_print(const element_type & c, ostream_type & stream, Int) 200 | { 201 | if (print_container_helper::delimiters_type::values.delimiter != NULL) 202 | stream << print_container_helper::delimiters_type::values.delimiter; 203 | 204 | stream << std::get(c); 205 | 206 | tuple_print(c, stream, Int()); 207 | } 208 | }; 209 | 210 | // Prints a print_container_helper to the specified stream. 211 | 212 | template 213 | inline std::basic_ostream & operator<<( 214 | std::basic_ostream & stream, 215 | const print_container_helper & helper) 216 | { 217 | helper(stream); 218 | return stream; 219 | } 220 | 221 | 222 | // Basic is_container template; specialize to derive from std::true_type for all desired container types 223 | 224 | template 225 | struct is_container : public std::integral_constant::value && 227 | detail::has_begin_end::beg_value && 228 | detail::has_begin_end::end_value> { }; 229 | 230 | template 231 | struct is_container : std::true_type { }; 232 | 233 | template 234 | struct is_container : std::false_type { }; 235 | 236 | template 237 | struct is_container> : std::true_type { }; 238 | 239 | template 240 | struct is_container> : std::true_type { }; 241 | 242 | template 243 | struct is_container> : std::true_type { }; 244 | 245 | 246 | // Default delimiters 247 | 248 | template struct delimiters { static const delimiters_values values; }; 249 | template const delimiters_values delimiters::values = { "[", ", ", "]" }; 250 | template struct delimiters { static const delimiters_values values; }; 251 | template const delimiters_values delimiters::values = { L"[", L", ", L"]" }; 252 | 253 | 254 | // Delimiters for (multi)set and unordered_(multi)set 255 | 256 | template 257 | struct delimiters< ::std::set, char> { static const delimiters_values values; }; 258 | 259 | template 260 | const delimiters_values delimiters< ::std::set, char>::values = { "{", ", ", "}" }; 261 | 262 | template 263 | struct delimiters< ::std::set, wchar_t> { static const delimiters_values values; }; 264 | 265 | template 266 | const delimiters_values delimiters< ::std::set, wchar_t>::values = { L"{", L", ", L"}" }; 267 | 268 | template 269 | struct delimiters< ::std::multiset, char> { static const delimiters_values values; }; 270 | 271 | template 272 | const delimiters_values delimiters< ::std::multiset, char>::values = { "{", ", ", "}" }; 273 | 274 | template 275 | struct delimiters< ::std::multiset, wchar_t> { static const delimiters_values values; }; 276 | 277 | template 278 | const delimiters_values delimiters< ::std::multiset, wchar_t>::values = { L"{", L", ", L"}" }; 279 | 280 | template 281 | struct delimiters< ::std::unordered_set, char> { static const delimiters_values values; }; 282 | 283 | template 284 | const delimiters_values delimiters< ::std::unordered_set, char>::values = { "{", ", ", "}" }; 285 | 286 | template 287 | struct delimiters< ::std::unordered_set, wchar_t> { static const delimiters_values values; }; 288 | 289 | template 290 | const delimiters_values delimiters< ::std::unordered_set, wchar_t>::values = { L"{", L", ", L"}" }; 291 | 292 | template 293 | struct delimiters< ::std::unordered_multiset, char> { static const delimiters_values values; }; 294 | 295 | template 296 | const delimiters_values delimiters< ::std::unordered_multiset, char>::values = { "{", ", ", "}" }; 297 | 298 | template 299 | struct delimiters< ::std::unordered_multiset, wchar_t> { static const delimiters_values values; }; 300 | 301 | template 302 | const delimiters_values delimiters< ::std::unordered_multiset, wchar_t>::values = { L"{", L", ", L"}" }; 303 | 304 | 305 | // Delimiters for pair and tuple 306 | 307 | template struct delimiters, char> { static const delimiters_values values; }; 308 | template const delimiters_values delimiters, char>::values = { "(", ", ", ")" }; 309 | template struct delimiters< ::std::pair, wchar_t> { static const delimiters_values values; }; 310 | template const delimiters_values delimiters< ::std::pair, wchar_t>::values = { L"(", L", ", L")" }; 311 | 312 | template struct delimiters, char> { static const delimiters_values values; }; 313 | template const delimiters_values delimiters, char>::values = { "(", ", ", ")" }; 314 | template struct delimiters< ::std::tuple, wchar_t> { static const delimiters_values values; }; 315 | template const delimiters_values delimiters< ::std::tuple, wchar_t>::values = { L"(", L", ", L")" }; 316 | 317 | 318 | // Type-erasing helper class for easy use of custom delimiters. 319 | // Requires TCharTraits = std::char_traits and TChar = char or wchar_t, and MyDelims needs to be defined for TChar. 320 | // Usage: "cout << pretty_print::custom_delims(x)". 321 | 322 | struct custom_delims_base 323 | { 324 | virtual ~custom_delims_base() { } 325 | virtual std::ostream & stream(::std::ostream &) = 0; 326 | virtual std::wostream & stream(::std::wostream &) = 0; 327 | }; 328 | 329 | template 330 | struct custom_delims_wrapper : custom_delims_base 331 | { 332 | custom_delims_wrapper(const T & t_) : t(t_) { } 333 | 334 | std::ostream & stream(std::ostream & s) 335 | { 336 | return s << print_container_helper, Delims>(t); 337 | } 338 | 339 | std::wostream & stream(std::wostream & s) 340 | { 341 | return s << print_container_helper, Delims>(t); 342 | } 343 | 344 | private: 345 | const T & t; 346 | }; 347 | 348 | template 349 | struct custom_delims 350 | { 351 | template 352 | custom_delims(const Container & c) : base(new custom_delims_wrapper(c)) { } 353 | 354 | std::unique_ptr base; 355 | }; 356 | 357 | template 358 | inline std::basic_ostream & operator<<(std::basic_ostream & s, const custom_delims & p) 359 | { 360 | return p.base->stream(s); 361 | } 362 | 363 | 364 | // A wrapper for a C-style array given as pointer-plus-size. 365 | // Usage: std::cout << pretty_print_array(arr, n) << std::endl; 366 | 367 | template 368 | struct array_wrapper_n 369 | { 370 | typedef const T * const_iterator; 371 | typedef T value_type; 372 | 373 | array_wrapper_n(const T * const a, size_t n) : _array(a), _n(n) { } 374 | inline const_iterator begin() const { return _array; } 375 | inline const_iterator end() const { return _array + _n; } 376 | 377 | private: 378 | const T * const _array; 379 | size_t _n; 380 | }; 381 | 382 | 383 | // A wrapper for hash-table based containers that offer local iterators to each bucket. 384 | // Usage: std::cout << bucket_print(m, 4) << std::endl; (Prints bucket 5 of container m.) 385 | 386 | template 387 | struct bucket_print_wrapper 388 | { 389 | typedef typename T::const_local_iterator const_iterator; 390 | typedef typename T::size_type size_type; 391 | 392 | const_iterator begin() const 393 | { 394 | return m_map.cbegin(n); 395 | } 396 | 397 | const_iterator end() const 398 | { 399 | return m_map.cend(n); 400 | } 401 | 402 | bucket_print_wrapper(const T & m, size_type bucket) : m_map(m), n(bucket) { } 403 | 404 | private: 405 | const T & m_map; 406 | const size_type n; 407 | }; 408 | 409 | } // namespace pretty_print 410 | 411 | 412 | // Global accessor functions for the convenience wrappers 413 | 414 | template 415 | inline pretty_print::array_wrapper_n pretty_print_array(const T * const a, size_t n) 416 | { 417 | return pretty_print::array_wrapper_n(a, n); 418 | } 419 | 420 | template pretty_print::bucket_print_wrapper 421 | bucket_print(const T & m, typename T::size_type n) 422 | { 423 | return pretty_print::bucket_print_wrapper(m, n); 424 | } 425 | 426 | 427 | // Main magic entry point: An overload snuck into namespace std. 428 | // Can we do better? 429 | 430 | namespace std 431 | { 432 | // Prints a container to the stream using default delimiters 433 | 434 | template 435 | inline typename enable_if< ::pretty_print::is_container::value, 436 | basic_ostream &>::type 437 | operator<<(basic_ostream & stream, const T & container) 438 | { 439 | return stream << ::pretty_print::print_container_helper(container); 440 | } 441 | } 442 | 443 | 444 | 445 | #endif // H_PRETTY_PRINT 446 | -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/prettyprint_LICENSE: -------------------------------------------------------------------------------- 1 | Boost Software License - Version 1.0 - August 17th, 2003 2 | 3 | Permission is hereby granted, free of charge, to any person or organization 4 | obtaining a copy of the software and accompanying documentation covered by 5 | this license (the "Software") to use, reproduce, display, distribute, 6 | execute, and transmit the Software, and to prepare derivative works of the 7 | Software, and to permit third-parties to whom the Software is furnished to 8 | do so, all subject to the following: 9 | 10 | The copyright notices in the Software and this entire statement, including 11 | the above license grant, this restriction and the following disclaimer, 12 | must be included in all copies of the Software, in whole or in part, and 13 | all derivative works of the Software, unless such copies or derivative 14 | works are solely in the form of machine-executable object code generated by 15 | a source language processor. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 20 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 21 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 22 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 23 | DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /moveit_simple/include/moveit_simple/robot.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef ROBOT_H 21 | #define ROBOT_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace moveit_simple 39 | { 40 | /** 41 | * @brief Robot is a wrapper around standard MoveIt objects. It makes multiple 42 | assumptions about the type and complexity of the robot. Assumptions are: 43 | - single arm manipulator (one joint group) 44 | - all cartesian poses are of the tool of the robot (could be fixed in the future) 45 | - point names are stored as joint positions in the SRDF or frame names in the URDF 46 | - frame transformations outside the URDF are provided by TF 47 | */ 48 | class Robot 49 | { 50 | public: 51 | /** 52 | * @brief Constructor 53 | */ 54 | Robot(const ros::NodeHandle &nh, const std::string &robot_description, 55 | const std::string &group_name); 56 | 57 | /** 58 | * @brief Constructor for the case where the IK implementation does not match the 59 | * SRDF. For example: IKFast solutions are generally solved for the base_link to 60 | * tool0 of a robot, and the robot_description is defined for some world_frame to 61 | * some tcp frame. The transforms to to the IK frames are assumed to be static. 62 | */ 63 | Robot(const ros::NodeHandle &nh, const std::string &robot_description, 64 | const std::string &group_name, const std::string &ik_base_frame, 65 | const std::string &ik_tip_frame); 66 | 67 | /** 68 | * @brief Destructor 69 | */ 70 | ~Robot(); 71 | 72 | void refreshRobot(); 73 | 74 | /** 75 | * @brief isInCollision returns true if pose results in robot config that is 76 | * in collision with the environment as defined by the URDF. 77 | * @param pose - cartesian pose of the tool to check 78 | * @param frame - tool pose relative to frame 79 | * @param joint_seed - named seed to use defined in srdf 80 | * @param timeout - (optional) timeout for IK 81 | * @return 82 | */ 83 | bool isInCollision(const Eigen::Affine3d &pose, const std::string &frame, 84 | const std::string &joint_seed, double timeout = 10.0) const; 85 | 86 | /** 87 | * @brief isInCollision returns true if pose results in robot config that is 88 | * in collision with the environment as defined by the URDF. 89 | * @param pose - cartesian pose of the tool to check 90 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 91 | * @param joint_seed - named seed to use defined in srdf 92 | * @param timeout - (optional) timeout for IK 93 | * @return 94 | */ 95 | bool isInCollision(const Eigen::Affine3d &pose, 96 | const geometry_msgs::TransformStamped &frame_to_robot_base, 97 | const std::string &joint_seed, double timeout = 10.0) const; 98 | 99 | /** 100 | * @brief isInCollision returns true if pose results in robot config that is 101 | * in collision with the environment as defined by the URDF. 102 | * @param pose - cartesian pose of the tool to check 103 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 104 | * @param custom_tool_to_moveit_tool - transform from custom tool frame to moveit_tool frame 105 | * @param joint_seed - named seed to use defined in srdf 106 | * @param timeout - (optional) timeout for IK 107 | * @return 108 | */ 109 | bool isInCollision(const Eigen::Affine3d &pose, 110 | const geometry_msgs::TransformStamped &frame_to_robot_base, 111 | const geometry_msgs::TransformStamped &custom_tool_to_moveit_tool, 112 | const std::string &joint_seed, double timeout = 10.0) const; 113 | 114 | /** 115 | * @brief isInCollision returns true if joint_point results in robot config that is 116 | * in collision with the environment as defined by the URDF. 117 | * @param joint_point(optional) - joint position of the robot to check 118 | * If no joint point is provided current position is checked for collision 119 | * @return 120 | */ 121 | bool isInCollision(const std::vector &joint_point = std::vector()) const; 122 | 123 | /** 124 | * @brief isInCollision returns true if pose results in robot config that is 125 | * in collision with the environment as defined by the URDF. 126 | * @param pose - cartesian pose of the tool to check 127 | * @param frame - tool pose relative to frame 128 | * @param timeout - (optional) timeout for IK 129 | * @param joint_seed (optional) - seed to use 130 | * @return 131 | */ 132 | bool isInCollision(const Eigen::Affine3d &pose, const std::string &frame, 133 | double timeout = 10.0, std::vector joint_seed = std::vector()) const; 134 | 135 | /** 136 | * @brief isInCollision returns true if pose results in robot config that is 137 | * in collision with the environment as defined by the URDF. 138 | * @param pose - cartesian pose of the tool to check 139 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 140 | * @param timeout - (optional) timeout for IK 141 | * @param joint_seed (optional) - seed to use 142 | * @return 143 | */ 144 | bool isInCollision(const Eigen::Affine3d &pose, 145 | const geometry_msgs::TransformStamped &frame_to_robot_base, 146 | double timeout = 10.0, std::vector joint_seed = std::vector()) const; 147 | 148 | /** 149 | * @brief isReachable - check if point is reacheable. 150 | * @param name - name of point to check 151 | * @param joint_seed - (optional) tries to find joint solutions closest to seed 152 | * @param timeout - (optional) timeout for IK 153 | * @return 154 | */ 155 | bool isReachable(const std::string &name, double timeout = 10.0, 156 | std::vector joint_seed = std::vector()) const; 157 | 158 | /** 159 | * @brief isReachable - check if pose (relative to named frame) is reacheable. 160 | * @param pose - pose to check 161 | * @param frame - frame in which pose is expressed 162 | * @param timeout - (optional) timeout for IK 163 | * @param joint_seed - seed to use 164 | * @return 165 | */ 166 | bool isReachable(const Eigen::Affine3d &pose, const std::string &frame, 167 | const std::string &joint_seed, double timeout = 10.0) const; 168 | 169 | /** 170 | * @brief isReachable - check if pose (relative to named frame) is reacheable. 171 | * @param pose - pose to check 172 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 173 | * @param timeout - (optional) timeout for IK 174 | * @param joint_seed - seed to use 175 | * @return 176 | */ 177 | bool isReachable(const Eigen::Affine3d &pose, 178 | const geometry_msgs::TransformStamped &frame_to_robot_base, 179 | const std::string &joint_seed, double timeout = 10.0) const; 180 | 181 | /** 182 | * @brief isReachable - check if pose (relative to named frame) is reacheable. 183 | * @param pose - pose to check 184 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 185 | * @param custom_tool_to_moveit_tool - transform from custom_tool frame to moveit_tool_frame 186 | * @param timeout - (optional) timeout for IK 187 | * @param joint_seed - seed to use 188 | * @return 189 | */ 190 | bool isReachable(const Eigen::Affine3d &pose, 191 | const geometry_msgs::TransformStamped &frame_to_robot_base, 192 | const geometry_msgs::TransformStamped &custom_tool_to_moveit_tool, 193 | const std::string &joint_seed, 194 | double timeout = 10.0) const; 195 | 196 | /** 197 | * @brief isReachable - check if pose (relative to named frame) is reacheable. 198 | * @param pose - pose to check 199 | * @param frame - frame in which pose is expressed 200 | * @param timeout - (optional) timeout for IK 201 | * @param joint_seed (optional) - named seed to use defined in srdf 202 | * @return 203 | */ 204 | bool isReachable(const Eigen::Affine3d &pose, const std::string &frame, 205 | double timeout = 10.0, std::vector joint_seed = std::vector()) const; 206 | 207 | /** 208 | * @brief isReachable - check if pose (relative to named frame) is reacheable. 209 | * @param pose - pose to check 210 | * @param frame_to_robot_base - transform from frame of pose to robot base frame 211 | * @param timeout - (optional) timeout for IK 212 | * @param joint_seed (optional) - named seed to use defined in srdf 213 | * @return 214 | */ 215 | bool isReachable(const Eigen::Affine3d &pose, 216 | const geometry_msgs::TransformStamped &frame_to_robot_base, 217 | double timeout = 10.0, std::vector joint_seed = std::vector()) const; 218 | 219 | /** 220 | * @brief addTrajPoint - add point to trajectory. 221 | * @param traj_name - name of trajectory buffer to add point to 222 | * @param point_name - name of point to add 223 | * @param time - time from start of trajectory to reach point 224 | * @param type - Type of interpolation from last point to this point 225 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 226 | * @param num_steps - number of points to be interpolated 227 | * By default, it is set to 0 and only adds the given point to the Trajectory 228 | * @throws (point_name is not found) 229 | * @throws (transform if TF named point_name fails) 230 | */ 231 | void addTrajPoint(const std::string &traj_name, const std::string &point_name, 232 | double time, const InterpolationType &type = interpolation_type::JOINT, 233 | const unsigned int num_steps = 0); 234 | 235 | /** 236 | * @brief Add trajectory point to motion buffer. 237 | * @param traj_name - name of trajectory buffer to add point to 238 | * @param pose - pose of point to add 239 | * @param frame - frame (must be a TF accessible frame) in which pose is defined 240 | * @param time - time from start of trajectory to reach point 241 | * @param type - Type of interpolation from last point to this point 242 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 243 | * @param num_steps - number of points to be interpolated 244 | * By default, it is set to 0 and only adds the given point to the Trajectory 245 | * @param point_name - (optional) name of point (used in log messages) 246 | * @throws (Transform from frame to robot base failed) 247 | */ 248 | void addTrajPoint(const std::string &traj_name, const Eigen::Affine3d pose, 249 | const std::string &frame, double time, const InterpolationType &type = interpolation_type::JOINT, 250 | const unsigned int num_steps = 0, const std::string &point_name = std::string()); 251 | 252 | /** 253 | * @brief addTrajPoint - add point to trajectory. 254 | * @brief This function supports custom tool frame for adding traj points. 255 | * @param traj_name - name of trajectory buffer to add point to 256 | * @param point_name - name of point to add 257 | * @param tool_name - frame (must be a TF accessible frame) in which pose is defined 258 | * @param time - time from start of trajectory to reach point 259 | * @param type - Type of interpolation from last point to this point 260 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 261 | * @param num_steps - number of points to be interpolated 262 | * By default, it is set to 0 and only adds the given point to the Trajectory 263 | * @throws (point_name is not found) 264 | * @throws (transform if TF named point_name fails) 265 | */ 266 | void addTrajPoint(const std::string &traj_name, const std::string &point_name, 267 | const std::string &tool_name, double time, const InterpolationType &type = interpolation_type::JOINT, 268 | const unsigned int num_steps = 0); 269 | 270 | /** 271 | * @brief Add trajectory point to motion buffer. 272 | * @brief This function supports custom tool frame for adding traj points. 273 | * @param traj_name - name of trajectory buffer to add point to 274 | * @param pose - pose of point to add 275 | * @param pose_frame - frame (must be a TF accessible frame) in which pose is defined 276 | * @param tool_name - frame (must be a TF accessible frame) in which pose is defined 277 | * @param time - time from start of trajectory to reach point 278 | * @param type - Type of interpolation from last point to this point 279 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 280 | * @param num_steps - number of points to be interpolated 281 | * By default, it is set to 0 and only adds the given point to the Trajectory 282 | * @param point_name - (optional) name of point (used in log messages) 283 | * @throws (Transform from frame to robot base failed) 284 | */ 285 | void addTrajPoint(const std::string &traj_name, const Eigen::Affine3d pose, 286 | const std::string &pose_frame, const std::string &tool_name, double time, 287 | const InterpolationType &type = interpolation_type::JOINT, const unsigned int num_steps = 0, 288 | const std::string &point_name = std::string()); 289 | 290 | /** 291 | * @brief addTrajPoint - add point to trajectory. 292 | * @param traj_name - name of trajectory buffer to add point to 293 | * @param point_name - name of point to add 294 | * @param time - time from start of trajectory to reach point 295 | * @param type - Type of interpolation from last point to this point 296 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 297 | * @param num_steps - number of points to be interpolated 298 | * By default, it is set to 0 and only adds the given point to the Trajectory 299 | * @param options (optional) - Locks the joints when moving from the previous point 300 | * @throws (point_name is not found) 301 | * @throws (transform if TF named point_name fails) 302 | */ 303 | void addTrajPointJointLock(const std::string &traj_name, const std::string &point_name, 304 | double time, const InterpolationType &type = interpolation_type::JOINT, 305 | const unsigned int num_steps = 0, JointLockOptions options = JointLockOptions::LOCK_NONE); 306 | 307 | /** 308 | * @brief Add trajectory point to motion buffer. 309 | * @brief This function supports custom tool frame for adding traj points. 310 | * @param traj_name - name of trajectory buffer to add point to 311 | * @param pose - pose of point to add 312 | * @param pose_frame - frame (must be a TF accessible frame) in which pose is defined 313 | * @param tool_name - frame (must be a TF accessible frame) in which pose is defined 314 | * @param time - time from start of trajectory to reach point 315 | * @param type - Type of interpolation from last point to this point 316 | * By default, it is set to JOINT. Can be set to "CARTESIAN" for cartesian Interpolation 317 | * @param num_steps - number of points to be interpolated 318 | * By default, it is set to 0 and only adds the given point to the Trajectory 319 | * @param point_name - (optional) name of point (used in log messages) 320 | * @param options - (optional) - Locks the joints when moving from the previous point 321 | * @throws (Transform from frame to robot base failed) 322 | */ 323 | void addTrajPointJointLock(const std::string &traj_name, const Eigen::Affine3d pose, 324 | const std::string &pose_frame, const std::string &tool_name, double time, 325 | const InterpolationType &type = interpolation_type::JOINT, const unsigned int num_steps = 0, 326 | const std::string &point_name = std::string(), JointLockOptions options = JointLockOptions::LOCK_NONE); 327 | 328 | /** 329 | * @brief getJointSolution returns joint solution for cartesian pose. 330 | * @param pose - desired pose 331 | * @param timeout - ik solver timeout 332 | * @param attempts - number of IK solve attem 333 | * @param seed - ik seed 334 | * @param joint_point - joint solution for pose 335 | * @return true if joint solution found 336 | */ 337 | bool getJointSolution(const Eigen::Affine3d &pose, double timeout, const std::vector &seed, 338 | std::vector &joint_point) const; 339 | 340 | /** 341 | * @brief getJointSolution returns joint solution for cartesian pose. 342 | * @brief This function supports custom tool frame for solving IK. 343 | * @param pose - desired pose 344 | * @param tool_name - frame (must be a TF accessible frame) in which pose is defined 345 | * @param timeout - ik solver timeout 346 | * @param attempts - number of IK solve attem 347 | * @param seed - ik seed 348 | * @param joint_point - joint solution for pose 349 | * @return true if joint solution found 350 | */ 351 | bool getJointSolution(const Eigen::Affine3d &pose, const std::string &tool_name, 352 | double timeout, const std::vector &seed, std::vector &joint_point) const; 353 | 354 | /** 355 | * @brief getPose finds cartesian pose for the given joint positions. 356 | * @param joint_point - joint positions 357 | * @param pose - pose corresponding to joint_pose 358 | * @return true if pose is found 359 | */ 360 | bool getPose(const std::vector &joint_point, Eigen::Affine3d &pose) const; 361 | 362 | /** 363 | * @brief getPose finds cartesian pose for the given joint positions. 364 | * @brief This function supports custom tool frame for solving FK. 365 | * @param joint_point - joint positions 366 | * @param pose - pose corresponding to joint_pose 367 | * @param tool_name - frame (must be a TF accessible frame) in which pose is defined 368 | * @return true if pose is found 369 | */ 370 | bool getPose(const std::vector &joint_point, const std::string &tool_name, 371 | Eigen::Affine3d &pose) const; 372 | 373 | /** 374 | * @brief clearTrajectory - clears stored trajectory 375 | * @param traj_name - trajectory to clear 376 | */ 377 | void clearTrajectory(const ::std::string traj_name); 378 | 379 | /** 380 | * @brief plan out a given trajectory 381 | * @param traj_name - name of trajectory to be executed (must be filled with 382 | * prior calls to "addTrajPoint". 383 | * @param collision_check - bool to turn check for collision on\off 384 | * @throws (Conversion to joint trajectory failed) 385 | * @throws (Trajectory "traj_name" not found) 386 | * @throws (One of interpolated point is 387 | * in Collision with itself or environment) 388 | */ 389 | std::vector plan(const std::string traj_name, 390 | bool collision_check = false); 391 | 392 | /** 393 | * @brief interpolate - returns the pose interpolated from \e from pose towards \e to 394 | * pose at time t in [0,1]. 395 | * @param from: initial pose 396 | * @param to: final pose 397 | * @param t: parameteric time 398 | */ 399 | Eigen::Affine3d interpolate(const Eigen::Affine3d &from, const Eigen::Affine3d &to, 400 | double t) const; 401 | 402 | /** 403 | * @brief interpolate - returns the joint point interpolated from \e from joint point 404 | * towards \e to joint point at time t in [0,1]. 405 | * @param from: initial joint point 406 | * @param to: final joint point 407 | * @param t: parameteric time 408 | */ 409 | std::vector interpolate(const std::vector &from, const std::vector &to, 410 | double t) const; 411 | 412 | /** 413 | * @brief isNearSingular: returns true if joint_point results in robot config that 414 | * is near a singularity configuration. 415 | * @param joint_point(optional) - joint position of the robot to check 416 | * If no joint point is provided current position is checked for singularity 417 | * @throws (joint_group is not a chain) 418 | */ 419 | bool isNearSingular(const std::vector &joint_point = std::vector()) const; 420 | 421 | /** 422 | * @brief setSpeedModifier - Setter method for the execution speed modifier of the 423 | * execute method. 424 | * @param speed_modifier 425 | * @return 426 | */ 427 | void setSpeedModifier(const double speed_modifier); 428 | 429 | /** 430 | * @brief setSpeedModifier - Getter method for the execution speed modifier of the 431 | * execute method. 432 | * @return speed_modifier_ 433 | */ 434 | double getSpeedModifier(void) const; 435 | 436 | void updateRvizRobotState(const Eigen::Affine3d &pose, const std::string &in_frame, 437 | const std::string &joint_seed, double timeout = 10.0) const; 438 | 439 | void updateRvizRobotState(const Eigen::Affine3d &pose, const std::string &in_frame, 440 | std::vector joint_seed = std::vector(), double timeout = 10.0) const; 441 | 442 | void updateRvizRobotState(const Eigen::Affine3d &pose, 443 | const geometry_msgs::TransformStamped &frame_to_robot_base, 444 | const std::string &joint_seed, double timeout = 10.0) const; 445 | 446 | void updateRvizRobotState(const Eigen::Affine3d &pose, 447 | const geometry_msgs::TransformStamped &frame_to_robot_base, 448 | std::vector joint_seed = std::vector(), 449 | double timeout = 10.0) const; 450 | 451 | geometry_msgs::TransformStamped lookupTransformMoveitToolAndCustomTool(const std::string tool_frame); 452 | 453 | geometry_msgs::TransformStamped lookupTransformBetweenFrames(const std::string &target_frame, 454 | const std::string &source_frame) const; 455 | 456 | geometry_msgs::TransformStamped lookupTransformToBase(const std::string &in_frame) const; 457 | 458 | protected: 459 | Robot(); 460 | 461 | Eigen::Affine3d transformToBase(const Eigen::Affine3d &in, const std::string &in_frame) const; 462 | 463 | Eigen::Affine3d transformToBase(const Eigen::Affine3d &in, 464 | const geometry_msgs::TransformStamped &transform_msg) const; 465 | 466 | /** 467 | * @brief transformPoseBetweenFrames transforms frame_in to frame_out. 468 | * @param target_pose - goal pose for IK 469 | * @param frame_in - Current Frame of Reference as Input 470 | * @param frame_out - Target Frame of Reference for Transform 471 | * @param 472 | */ 473 | Eigen::Affine3d transformPoseBetweenFrames(const Eigen::Affine3d &target_pose, 474 | const std::string &frame_in, const std::string &frame_out) const; 475 | 476 | std::vector toJointTrajectoryPoint( 477 | std::vector &ROS_joint_trajectory_points) const; 478 | 479 | control_msgs::FollowJointTrajectoryGoal toFollowJointTrajectoryGoal( 480 | const std::vector &joint_trajectory_points) const; 481 | 482 | bool toJointTrajectory(const std::string traj_name, std::vector &points, 483 | bool collision_check = false); 484 | 485 | /** 486 | * @brief computeIKTransforms - Computes the transforms between the base/tip 487 | * frames defined in the URDF and the base/tip frames defined for the IK solver. 488 | */ 489 | void computeIKSolverTransforms(); 490 | 491 | /** 492 | * @brief jointInterpolation - joint Interpolation from last added point to 493 | * current trajectory point(traj_point). 494 | * @param traj_point: target traj_point for joint interpolation 495 | * @param points: Vector of Joint Trajectory Point to be executed 496 | * @param num_steps: number of steps to be interpolated between current point and traj_point 497 | * @param collision_check - bool to turn check for collision on\off 498 | * @return true if all the points including traj_point are added to the points. 499 | */ 500 | bool jointInterpolation(const std::unique_ptr &traj_point, 501 | std::vector &points, const unsigned int num_steps, 502 | bool collision_check = false); 503 | 504 | /** 505 | * @brief cartesianInterpolation - Cartesian Interpolation from last added point to 506 | * current trajectory point(traj_point). 507 | * @param traj_point: target traj_point for cartesian interpolation 508 | * @param points: Vector of Joint Trajectory Point to be executed 509 | * @param num_steps: number of steps to be interpolated between current point and traj_point 510 | * @param collision_check - bool to turn check for collision on\off 511 | * @return true if all the points including traj_point are added to the points. 512 | */ 513 | bool cartesianInterpolation(const std::unique_ptr &traj_point, 514 | std::vector &points, const unsigned int num_steps, 515 | bool collision_check = false); 516 | 517 | /** 518 | * @brief interpolate - Cartesian interpolation from \e from point towards \e to 519 | * point at time t in [0,1] and stores it in \e point. 520 | * @param from: initial Cartesian point 521 | * @param to: final Cartesian point 522 | * @param t: parameteric time 523 | * @param point: interpolated Cartesian point 524 | */ 525 | void interpolate(const std::unique_ptr &from, 526 | const std::unique_ptr &to, double t, 527 | std::unique_ptr &point) const; 528 | 529 | /** 530 | * @brief interpolate - Joint interpolation from \e from point towards \e to 531 | * point at time t in [0,1] and stores it in \e point. 532 | * @param from: initial Joint point 533 | * @param to: final Joint point 534 | * @param t: parameteric time 535 | * @param point: interpolated Joint point 536 | */ 537 | void interpolate(const std::unique_ptr &from, 538 | const std::unique_ptr &to, double t, 539 | std::unique_ptr &point) const; 540 | 541 | void addTrajPoint(const std::string &traj_name, std::unique_ptr &point, 542 | const InterpolationType &type = interpolation_type::JOINT, 543 | const unsigned int num_steps = 0); 544 | 545 | bool isReachable(std::unique_ptr &point, double timeout, 546 | std::vector joint_seed = std::vector()) const; 547 | 548 | int trajCollisionCheck(control_msgs::FollowJointTrajectoryGoal &goal, 549 | bool collision_check = false); 550 | 551 | /** 552 | * @brief getJointState - Returns a vector of the 553 | * robot position from updated from last IK call. 554 | * 555 | * @return std::vector current_joint_position 556 | */ 557 | virtual std::vector getJointState(void) const; 558 | 559 | bool getIK(const Eigen::Affine3d pose, const std::vector &seed, 560 | std::vector &joint_point, double timeout = 1, unsigned int attempts = 1) const; 561 | 562 | bool getIK(const Eigen::Affine3d pose, std::vector &joint_point, 563 | double timeout = 1, unsigned int attempts = 1) const; 564 | 565 | bool getFK(const std::vector &joint_point, Eigen::Affine3d &pose) const; 566 | 567 | std::unique_ptr lookupTrajectoryPoint(const std::string &name, 568 | double time) const; 569 | 570 | bool isConfigChange(const std::vector jp1, const std::vector jp2) const; 571 | 572 | /** 573 | * @brief toJointTrajPtMsg - Converts native joint point (vector + time) to ROS joint 574 | * trajectory point message type. 575 | * @param joint_point 576 | * @param time 577 | * @return 578 | */ 579 | static trajectory_msgs::JointTrajectoryPoint toJointTrajPtMsg(const std::vector &joint_point, 580 | double time); 581 | 582 | trajectory_msgs::JointTrajectoryPoint toJointTrajPtMsg(const JointTrajectoryPoint &joint_point) const; 583 | 584 | // Dynamic Reconfigure callback 585 | void reconfigureRequest(moveit_simple_dynamic_reconfigure_Config &config, uint32_t level); 586 | 587 | // Robot internal objects 588 | std::map traj_info_map_; 589 | 590 | // MoveIt objects 591 | mutable moveit::core::RobotStatePtr virtual_robot_state_; // for IK calls 592 | planning_scene::PlanningScenePtr planning_scene_; 593 | const moveit::core::JointModelGroup *joint_group_; 594 | robot_model::RobotModelConstPtr robot_model_ptr_; 595 | robot_model_loader::RobotModelLoaderPtr robot_model_loader_; 596 | 597 | // Visualizations 598 | moveit_visual_tools::MoveItVisualToolsPtr virtual_visual_tools_; 599 | 600 | // TF for looking up waypoints 601 | // TODO: Consider swapping out the listener for an action based lookup - much 602 | // less overhead 603 | tf2_ros::Buffer tf_buffer_; 604 | tf2_ros::TransformListener tf_listener_; 605 | 606 | // Service Server to Lookup Waypoints 607 | mutable ros::ServiceClient lookup_wp_client_; 608 | 609 | // ROS objects 610 | ros::NodeHandle nh_; 611 | mutable std::recursive_mutex m_; 612 | 613 | // Dynamic Reconfigure 614 | double speed_modifier_; 615 | double joint_equality_tolerance_; 616 | double service_client_timeout_; 617 | moveit_simple_dynamic_reconfigure_Parameters params_; 618 | std::shared_ptr> dynamic_reconfig_server_; 619 | 620 | // Kinematics 621 | std::string ik_base_frame_; 622 | std::string ik_tip_frame_; 623 | Eigen::Affine3d ik_tip_to_srdf_tip_; 624 | Eigen::Affine3d srdf_base_to_ik_base_; 625 | 626 | std::string planning_group_; 627 | std::string robot_description_; 628 | }; 629 | } // namespace moveit_simple 630 | #endif // ROBOT_H 631 | -------------------------------------------------------------------------------- /moveit_simple/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_simple 4 | 0.2.1 5 | Includes simple wrappers for calling MoveIt as a ROS based library. Similar function as MoveGroup but all calls are made locally using direct MoveIt objects. This library assumes that MoveIt is being used in a broader ROS system (i.e. certain parameters, like robot_description exist) 6 | 7 | Apache 2.0 8 | Boost Software License 9 | 10 | Shaun Edwards 11 | Shaun Edwards 12 | 13 | catkin 14 | 15 | cmake_modules 16 | dynamic_reconfigure 17 | control_msgs 18 | eigen 19 | eigen_conversions 20 | geometry_msgs 21 | moveit_core 22 | moveit_ros_planning 23 | moveit_visual_tools 24 | roscpp 25 | rosparam_handler 26 | sensor_msgs 27 | tf2 28 | tf2_geometry_msgs 29 | tf2_ros 30 | trajectory_msgs 31 | moveit_simple_msgs 32 | 33 | control_msgs 34 | dynamic_reconfigure 35 | eigen 36 | eigen_conversions 37 | geometry_msgs 38 | industrial_robot_client 39 | industrial_robot_simulator 40 | moveit_core 41 | moveit_ros_planning 42 | robot_state_publisher 43 | roscpp 44 | rosparam_handler 45 | sensor_msgs 46 | tf 47 | tf2 48 | tf2_geometry_msgs 49 | tf2_ros 50 | trajectory_msgs 51 | moveit_simple_msgs 52 | 53 | moveit_kinematics 54 | rostest 55 | rosunit 56 | 57 | -------------------------------------------------------------------------------- /moveit_simple/src/joint_lock_options.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | namespace moveit_simple 22 | { 23 | 24 | JointLockOptions operator&(JointLockOptions lhs, JointLockOptions rhs) 25 | { 26 | return static_cast(static_cast(lhs) & static_cast(rhs)); 27 | } 28 | 29 | JointLockOptions operator|(JointLockOptions lhs, JointLockOptions rhs) 30 | { 31 | return static_cast(static_cast(lhs) | static_cast(rhs)); 32 | } 33 | 34 | } // namespace moveit_simple 35 | -------------------------------------------------------------------------------- /moveit_simple/src/joint_locker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | namespace moveit_simple 22 | { 23 | 24 | void JointLocker::lockJoints(const std::vector &prev_joints, std::vector ¤t_joints, JointLockOptions options) 25 | { 26 | for (auto i = 0UL; i < sizeof(JointLockOptions) * 8UL; i++) 27 | { 28 | if ((static_cast(options) & (1 << i)) != 0) 29 | { 30 | current_joints[i] = prev_joints[i]; 31 | } 32 | } 33 | } 34 | 35 | std::string JointLocker::resolveToString(const JointLockOptions options) 36 | { 37 | std::string result; 38 | const std::string prefix = "LOCK_JOINT_"; 39 | 40 | for (auto i = 0UL; i < sizeof(options) * 8UL; i++) 41 | { 42 | if ((static_cast(options) & (1 << i)) != 0) 43 | { 44 | auto joint_string = prefix + std::to_string(i + 1); 45 | if (!result.empty()) 46 | { 47 | result += ", "; 48 | } 49 | 50 | result += joint_string; 51 | } 52 | } 53 | 54 | if (result == "") 55 | { 56 | return "LOCK_NONE"; 57 | } 58 | 59 | return result; 60 | } 61 | 62 | } // namespace moveit_simple 63 | -------------------------------------------------------------------------------- /moveit_simple/src/online_robot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace moveit_simple 30 | { 31 | OnlineRobot::OnlineRobot(const ros::NodeHandle &nh, const std::string &robot_description, 32 | const std::string &group_name) : Robot(nh, robot_description, group_name), 33 | action_("joint_trajectory_action", true) 34 | { 35 | current_robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_)); 36 | current_robot_state_->setToDefaultValues(); 37 | 38 | online_visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(robot_model_ptr_->getRootLinkName(), 39 | nh_.getNamespace() + "/rviz_visual_tools", robot_model_ptr_)); 40 | online_visual_tools_->loadRobotStatePub(nh_.getNamespace() + "/display_robot_state"); 41 | 42 | ROS_INFO_STREAM("Waiting for action servers"); 43 | action_.waitForServer(ros::Duration(30.0)); 44 | ROS_INFO_STREAM("Done waiting for action servers"); 45 | 46 | ROS_INFO_STREAM("Loading ROS pubs/subs"); 47 | j_state_sub_ = nh_.subscribe("joint_states", 1, &OnlineRobot::updateCurrentState, this); 48 | 49 | // TODO: How to handle action server and other failures in the constructor 50 | // Perhaps move any items that can fail our of the constructor into an init 51 | // function with a proper return 52 | if (!action_.isServerConnected()) 53 | { 54 | ROS_ERROR_STREAM("Failed to connect to joint trajectory action server: "); 55 | } 56 | 57 | return; 58 | } 59 | 60 | OnlineRobot::OnlineRobot(const ros::NodeHandle &nh, const std::string &robot_description, 61 | const std::string &group_name, const std::string &ik_base_frame, const std::string &ik_tip_frame) 62 | : OnlineRobot(nh, robot_description, group_name) 63 | { 64 | try 65 | { 66 | ik_base_frame_ = ik_base_frame; 67 | ik_tip_frame_ = ik_tip_frame; 68 | this->computeIKSolverTransforms(); 69 | } 70 | catch (tf2::TransformException &ex) 71 | { 72 | ROS_ERROR_STREAM("Failed to compute transforms between the base/tip frames defined" 73 | << " in the SRDF and the base/tip frames defined for the IK solver"); 74 | throw IKSolverTransformException("Failed to compute transforms between the base/tip" 75 | " frame defined in the SRDF and the base/tip frames defined for the IK solver"); 76 | } 77 | } 78 | 79 | void OnlineRobot::execute(const std::string traj_name, bool collision_check) 80 | { 81 | std::lock_guard guard(m_); 82 | 83 | try 84 | { 85 | std::vector goal; 86 | 87 | goal = plan(traj_name, collision_check); 88 | execute(goal); 89 | } 90 | catch (IKFailException &ik) 91 | { 92 | ROS_ERROR_STREAM("IK Failed for trajectory: [" << traj_name << "]"); 93 | throw ik; 94 | } 95 | catch (CollisionDetected &cd) 96 | { 97 | ROS_ERROR_STREAM("Collision detected in trajectory"); 98 | throw cd; 99 | } 100 | catch (std::invalid_argument &ia) 101 | { 102 | ROS_ERROR_STREAM("Invalid trajectory name: [" << traj_name << "]"); 103 | throw ia; 104 | } 105 | catch (ExecutionFailureException &ef) 106 | { 107 | ROS_ERROR_STREAM("Trajectory [" << traj_name << "] failed to execute"); 108 | throw ef; 109 | } 110 | } 111 | 112 | void OnlineRobot::execute(std::vector &joint_trajectory_points, 113 | bool collision_check) 114 | { 115 | std::lock_guard guard(m_); 116 | 117 | const double TIMEOUT_SCALE = 1.25; // scales time to wait for action timeout. 118 | 119 | control_msgs::FollowJointTrajectoryGoal goal = toFollowJointTrajectoryGoal(joint_trajectory_points); 120 | 121 | int collision_points = trajCollisionCheck(goal, collision_check); 122 | 123 | if (collision_points == 0) 124 | { 125 | ros::Duration traj_time = 126 | goal.trajectory.points[goal.trajectory.points.size() - 1].time_from_start; 127 | 128 | ros::Duration timeout(TIMEOUT_SCALE * traj_time.toSec()); 129 | 130 | actionlib::SimpleClientGoalState result = action_.sendGoalAndWait(goal, timeout); 131 | if (result == actionlib::SimpleClientGoalState::SUCCEEDED) 132 | { 133 | std::vector joint_state = getJointState(); 134 | std::vector joint_state_goal = joint_trajectory_points.back().jointPoint(); 135 | 136 | bool reached_goal = true; 137 | for (size_t i = 0; i < joint_state.size(); ++i) 138 | { 139 | // Use the dynamic param joint_equality_tolerance_ to check whether we 140 | // are close enough to the goal in joint space. 141 | if (std::abs(joint_state[i] - joint_state_goal[i]) > joint_equality_tolerance_) 142 | { 143 | ROS_ERROR("Joint %zu angle (%lf) not within tolerance (%lf) of goal " 144 | "angle (%lf)", 145 | i, joint_state[i], joint_equality_tolerance_, joint_state_goal[i]); 146 | reached_goal = false; 147 | break; 148 | } 149 | } 150 | 151 | if (!reached_goal) 152 | { 153 | ROS_ERROR("Trajectory failed to reach goal"); 154 | throw ExecutionFailureException("Trajectory failed to reach goal"); 155 | } 156 | } 157 | else 158 | { 159 | ROS_ERROR("Joint Trajectory failed to execute. Action server responded " 160 | "with goal state: %s.", result.toString().c_str()); 161 | throw ExecutionFailureException("Execution failed for Joint Trajectory"); 162 | } 163 | } 164 | else 165 | { 166 | ROS_ERROR_STREAM("Collision detected at " << collision_points 167 | << " points for Joint Trajectory"); 168 | throw CollisionDetected("Collision detected while interpolating "); 169 | } 170 | } 171 | 172 | void OnlineRobot::startExecution(const std::string &traj_name, bool collision_check) 173 | { 174 | std::lock_guard guard(m_); 175 | 176 | try 177 | { 178 | std::vector joint_trajectory_points; 179 | joint_trajectory_points = plan(traj_name, collision_check); 180 | 181 | const double TIMEOUT_SCALE = 1.25; 182 | control_msgs::FollowJointTrajectoryGoal goal = this->toFollowJointTrajectoryGoal(joint_trajectory_points); 183 | 184 | auto collision_points = this->trajCollisionCheck(goal, collision_check); 185 | 186 | if (collision_points == 0) 187 | { 188 | auto traj_time = goal.trajectory.points[goal.trajectory.points.size() - 1].time_from_start; 189 | auto timeout = ros::Time::now() + ros::Duration(TIMEOUT_SCALE*traj_time.toSec()); 190 | 191 | action_.sendGoal(goal); 192 | is_timed_out_ = false; 193 | std::thread execution_thread(&OnlineRobot::executing, this, timeout); 194 | execution_thread.detach(); 195 | } 196 | else 197 | { 198 | ROS_ERROR_STREAM("Collision detected in trajectory: " << traj_name); 199 | throw CollisionDetected("Collision detected while interpolating "); 200 | } 201 | } 202 | catch (IKFailException &ik) 203 | { 204 | ROS_ERROR_STREAM("IK Failed for trajectory: [" << traj_name << "]"); 205 | throw ik; 206 | } 207 | catch (std::invalid_argument &ia) 208 | { 209 | ROS_ERROR_STREAM("Invalid trajectory name: [" << traj_name << "]"); 210 | throw ia; 211 | } 212 | catch (ExecutionFailureException &ef) 213 | { 214 | ROS_ERROR_STREAM("Trajectory [" << traj_name << "] failed to execute"); 215 | throw ef; 216 | } 217 | } 218 | 219 | bool OnlineRobot::isExecuting() 220 | { 221 | std::lock_guard guard(m_); 222 | 223 | if (!is_timed_out_) 224 | { 225 | auto action_state = action_.getState(); 226 | if (action_state == actionlib::SimpleClientGoalState::PENDING || action_state == actionlib::SimpleClientGoalState::ACTIVE) 227 | { 228 | return true; 229 | } 230 | else 231 | { 232 | this->stopExecution(); 233 | return false; 234 | } 235 | } 236 | else 237 | { 238 | this->stopExecution(); 239 | ROS_ERROR_STREAM("Execution timed out"); 240 | return false; 241 | } 242 | } 243 | 244 | void OnlineRobot::stopExecution() 245 | { 246 | std::lock_guard guard(m_); 247 | 248 | action_.cancelGoal(); 249 | auto action_state = action_.getState(); 250 | while (action_state != actionlib::SimpleClientGoalState::RECALLED 251 | && action_state != actionlib::SimpleClientGoalState::PREEMPTED 252 | && action_state != actionlib::SimpleClientGoalState::ABORTED 253 | && action_state != actionlib::SimpleClientGoalState::SUCCEEDED 254 | && action_state != actionlib::SimpleClientGoalState::REJECTED) 255 | { 256 | action_state = action_.getState(); 257 | } 258 | } 259 | 260 | std::vector OnlineRobot::getJointState(void) const 261 | { 262 | std::lock_guard guard(m_); 263 | 264 | ros::spinOnce(); 265 | std::vector current_joint_positions; 266 | 267 | current_robot_state_->update(); 268 | current_robot_state_->copyJointGroupPositions(joint_group_->getName(), current_joint_positions); 269 | 270 | return current_joint_positions; 271 | } 272 | 273 | void OnlineRobot::executing(const ros::Time &timeout) 274 | { 275 | auto action_state = action_.getState(); 276 | while (ros::Time::now() <= timeout && action_state == actionlib::SimpleClientGoalState::ACTIVE) 277 | { 278 | is_timed_out_ = false; 279 | action_state = action_.getState(); 280 | } 281 | 282 | if (action_state == actionlib::SimpleClientGoalState::ACTIVE) 283 | is_timed_out_ = true; 284 | } 285 | 286 | void OnlineRobot::updateCurrentState(const sensor_msgs::JointStateConstPtr &msg) 287 | { 288 | current_robot_state_->setVariablePositions(msg->name, msg->position); 289 | } 290 | } // namespace moveit_simple 291 | -------------------------------------------------------------------------------- /moveit_simple/src/point_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace moveit_simple 27 | { 28 | void TrajectoryPoint::setJointLockOptions(const JointLockOptions &options) 29 | { 30 | joint_lock_options_ = options; 31 | } 32 | 33 | JointLockOptions TrajectoryPoint::getJointLockOptions() 34 | { 35 | return joint_lock_options_; 36 | } 37 | 38 | std::unique_ptr JointTrajectoryPoint::toJointTrajPoint(const Robot &robot, double timeout, 39 | const std::vector &seed, 40 | JointLockOptions options) const 41 | { 42 | ROS_DEBUG_STREAM("JointTrajectoryPoint: passing through joint trajectory point"); 43 | return std::unique_ptr(new JointTrajectoryPoint(*this)); 44 | } 45 | 46 | std::unique_ptr JointTrajectoryPoint::toCartTrajPoint(const Robot &robot) const 47 | { 48 | Eigen::Affine3d pose; 49 | 50 | ROS_DEBUG_STREAM("JointTrajectoryPoint: Calculating FK for Cartesian trajectory point"); 51 | if (robot.getPose(joint_point_, pose)) 52 | { 53 | return std::unique_ptr(new CartTrajectoryPoint(pose, time(), name())); 54 | } 55 | else 56 | { 57 | ROS_WARN_STREAM("Failed to find FK for point: " << name_); 58 | return std::unique_ptr(nullptr); 59 | } 60 | } 61 | 62 | std::unique_ptr CartTrajectoryPoint::toJointTrajPoint(const Robot &robot, double timeout, 63 | const std::vector &seed, 64 | JointLockOptions options) const 65 | { 66 | std::vector joints; 67 | 68 | ROS_DEBUG_STREAM("CartTrajectoryPoint: Calculating IK for joint trajectory point"); 69 | if (robot.getJointSolution(pose_, timeout, seed, joints)) 70 | { 71 | return std::unique_ptr(new JointTrajectoryPoint(joints, time(), name(), options)); 72 | } 73 | else 74 | { 75 | ROS_WARN_STREAM("Failed to find joint solution for point: " << name_); 76 | return std::unique_ptr(nullptr); 77 | } 78 | } 79 | 80 | std::unique_ptr CartTrajectoryPoint::toCartTrajPoint(const Robot &robot) const 81 | { 82 | ROS_DEBUG_STREAM("CartTrajectoryPoint: passing through cartesian trajectory point"); 83 | return std::unique_ptr(new CartTrajectoryPoint(*this)); 84 | } 85 | 86 | std::unique_ptr CombinedTrajectoryPoint::toJointTrajPoint(const Robot &robot, double timeout, 87 | const std::vector &seed, 88 | JointLockOptions options) const 89 | { 90 | std::vector joints; 91 | std::copy(joint_point_.begin(), joint_point_.end(), std::back_inserter(joints)); 92 | if (compareJointAndCart(robot, timeout)) 93 | { 94 | return std::unique_ptr(new JointTrajectoryPoint(joints, time(), name(), options)); 95 | } 96 | else if(this->type() == PointType::JOINT) 97 | { 98 | return std::unique_ptr(new JointTrajectoryPoint(joints, time(), name(), options)); 99 | } 100 | else 101 | { 102 | auto point = std::unique_ptr(new CartTrajectoryPoint(pose(), time(), name(), jointLockOptions())); 103 | return point->toJointTrajPoint(robot, timeout, joints, options); 104 | } 105 | } 106 | 107 | std::unique_ptr CombinedTrajectoryPoint::toCartTrajPoint(const Robot &robot) const 108 | { 109 | if(compareJointAndCart(robot, this->timeout())) 110 | { 111 | return std::unique_ptr(new CartTrajectoryPoint(pose(), time(), name(), jointLockOptions())); 112 | } 113 | else if(this->type() == PointType::JOINT) 114 | { 115 | std::vector joints; 116 | std::copy(joint_point_.begin(), joint_point_.end(), std::back_inserter(joints)); 117 | auto point = std::unique_ptr(new JointTrajectoryPoint(joints, time(), name(), jointLockOptions())); 118 | return point->toCartTrajPoint(robot); 119 | } 120 | else 121 | { 122 | return std::unique_ptr(new CartTrajectoryPoint(pose(), time(), name(), jointLockOptions())); 123 | } 124 | } 125 | 126 | std::string CombinedTrajectoryPoint::pointVecToString(const std::vector &vec) const 127 | { 128 | std::stringstream ss; 129 | ss << "[ "; 130 | for_each(vec.begin(), vec.end(), [&ss](const double &point){ss << point << " ";}); 131 | ss << " ]"; 132 | return ss.str(); 133 | } 134 | 135 | bool CombinedTrajectoryPoint::compareJointAndCart(const Robot &robot, double timeout) const 136 | { 137 | std::vector cart_point; 138 | bool in_tol = false; 139 | double tol = tol_; 140 | 141 | if (robot.getJointSolution(pose_, timeout, joint_point_, cart_point)) 142 | { 143 | if (joint_point_.size() == cart_point.size()) 144 | { 145 | in_tol = std::equal(joint_point_.begin(), joint_point_.end(), cart_point.begin(), 146 | [&tol](const double &joint, const double &cart) { return std::abs(joint - cart) <= tol; }); 147 | 148 | if (!in_tol) 149 | { 150 | std::stringstream ss; 151 | ss << "CombinedTrajectoryPoint: Cartesian and Joint representations are out of tolerance. " << std::endl; 152 | ss << "Using " << ((this->type() == PointType::JOINT) ? "joint" : "cartesian") 153 | << " representation, per preference." << std::endl; 154 | ss << "Joint Representation Joints: " << pointVecToString(joint_point_) << std::endl; 155 | ss << "Cartesian Representation Joints: " << pointVecToString(cart_point) << std::endl; 156 | ROS_WARN_STREAM(ss.str()); 157 | } 158 | } 159 | else 160 | { 161 | ROS_WARN_STREAM("Joint and Cartesian representations are not the same size. Cannot compare."); 162 | } 163 | } 164 | else 165 | { 166 | ROS_WARN_STREAM("Failed to find joint solution for point: " << name_); 167 | } 168 | 169 | return in_tol; 170 | } 171 | 172 | } // namespace moveit_simple 173 | -------------------------------------------------------------------------------- /moveit_simple/test/config/test_display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Tree1 10 | Splitter Ratio: 0.5 11 | Tree Height: 558 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.0299999993 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | All Links Enabled: true 58 | Expand Joint Details: false 59 | Expand Link Details: false 60 | Expand Tree: false 61 | Link Tree Style: Links in Alphabetic Order 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | ee_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | link_b: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | link_l: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | link_r: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | link_s: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | link_t: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | link_u: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | tool0: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | tool_custom: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | world: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | wp1: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | wp2: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | wp3: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | wp4: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Name: RobotModel 131 | Robot Description: robot_description 132 | TF Prefix: "" 133 | Update Interval: 0 134 | Value: true 135 | Visual Enabled: true 136 | - Class: rviz/TF 137 | Enabled: true 138 | Frame Timeout: 15 139 | Frames: 140 | All Enabled: false 141 | base_link: 142 | Value: false 143 | ee_link: 144 | Value: false 145 | link_b: 146 | Value: false 147 | link_l: 148 | Value: false 149 | link_r: 150 | Value: false 151 | link_s: 152 | Value: false 153 | link_t: 154 | Value: false 155 | link_u: 156 | Value: false 157 | tf_pub1: 158 | Value: true 159 | tool0: 160 | Value: false 161 | tool_custom: 162 | Value: true 163 | world: 164 | Value: true 165 | wp1: 166 | Value: true 167 | wp2: 168 | Value: true 169 | wp3: 170 | Value: true 171 | wp4: 172 | Value: true 173 | Marker Scale: 1 174 | Name: TF 175 | Show Arrows: true 176 | Show Axes: true 177 | Show Names: true 178 | Tree: 179 | base_link: 180 | link_s: 181 | link_l: 182 | link_u: 183 | link_r: 184 | link_b: 185 | link_t: 186 | tool0: 187 | ee_link: 188 | tool_custom: 189 | {} 190 | tf_pub1: 191 | {} 192 | world: 193 | wp1: 194 | {} 195 | wp2: 196 | {} 197 | wp3: 198 | {} 199 | wp4: 200 | {} 201 | Update Interval: 0 202 | Value: true 203 | Enabled: true 204 | Global Options: 205 | Background Color: 48; 48; 48 206 | Default Light: true 207 | Fixed Frame: world 208 | Frame Rate: 30 209 | Name: root 210 | Tools: 211 | - Class: rviz/Interact 212 | Hide Inactive Objects: true 213 | - Class: rviz/MoveCamera 214 | - Class: rviz/Select 215 | - Class: rviz/FocusCamera 216 | - Class: rviz/Measure 217 | - Class: rviz/SetInitialPose 218 | Topic: /initialpose 219 | - Class: rviz/SetGoal 220 | Topic: /move_base_simple/goal 221 | - Class: rviz/PublishPoint 222 | Single click: true 223 | Topic: /clicked_point 224 | Value: true 225 | Views: 226 | Current: 227 | Class: rviz/Orbit 228 | Distance: 3.11921096 229 | Enable Stereo Rendering: 230 | Stereo Eye Separation: 0.0599999987 231 | Stereo Focal Distance: 1 232 | Swap Stereo Eyes: false 233 | Value: false 234 | Focal Point: 235 | X: 0.126767859 236 | Y: -0.028345149 237 | Z: 0.110510007 238 | Focal Shape Fixed Size: true 239 | Focal Shape Size: 0.0500000007 240 | Invert Z Axis: false 241 | Name: Current View 242 | Near Clip Distance: 0.00999999978 243 | Pitch: 0.635398209 244 | Target Frame: 245 | Value: Orbit (rviz) 246 | Yaw: 6.2185812 247 | Saved: ~ 248 | Window Geometry: 249 | Displays: 250 | collapsed: false 251 | Height: 846 252 | Hide Left Dock: false 253 | Hide Right Dock: true 254 | QMainWindow State: 000000ff00000000fd00000004000000000000016b000002c1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c1000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c1000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003ffc0100000002fb0000000800540069006d00650100000000000004b00000031700fffffffb0000000800540069006d006501000000000000045000000000000000000000033f000002c100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 255 | Selection: 256 | collapsed: false 257 | Time: 258 | collapsed: false 259 | Tool Properties: 260 | collapsed: false 261 | Views: 262 | collapsed: true 263 | Width: 1200 264 | X: 468 265 | Y: 318 266 | -------------------------------------------------------------------------------- /moveit_simple/test/joint_lock_options.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | using namespace moveit_simple; 23 | using testing::Types; 24 | 25 | namespace moveit_simple_test 26 | { 27 | 28 | std::vector checkOptions(const JointLockOptions &options) 29 | { 30 | std::vector result = {false, false, false, false, false, false}; 31 | 32 | if ((options & JointLockOptions::LOCK_JOINT_1) != JointLockOptions::LOCK_NONE) 33 | { 34 | result[0] = true; 35 | } 36 | 37 | if ((options & JointLockOptions::LOCK_JOINT_2) != JointLockOptions::LOCK_NONE) 38 | { 39 | result[1] = true; 40 | } 41 | 42 | if ((options & JointLockOptions::LOCK_JOINT_3) != JointLockOptions::LOCK_NONE) 43 | { 44 | result[2] = true; 45 | } 46 | 47 | if ((options & JointLockOptions::LOCK_JOINT_4) != JointLockOptions::LOCK_NONE) 48 | { 49 | result[3] = true; 50 | } 51 | 52 | if ((options & JointLockOptions::LOCK_JOINT_5) != JointLockOptions::LOCK_NONE) 53 | { 54 | result[4] = true; 55 | } 56 | 57 | if ((options & JointLockOptions::LOCK_JOINT_6) != JointLockOptions::LOCK_NONE) 58 | { 59 | result[5] = true; 60 | } 61 | 62 | return result; 63 | } 64 | 65 | TEST(MoveitSimpleTest, joint_locker_options_none) 66 | { 67 | auto r0 = checkOptions(JointLockOptions::LOCK_NONE); 68 | EXPECT_FALSE(r0[0]); 69 | EXPECT_FALSE(r0[1]); 70 | EXPECT_FALSE(r0[2]); 71 | EXPECT_FALSE(r0[3]); 72 | EXPECT_FALSE(r0[4]); 73 | EXPECT_FALSE(r0[5]); 74 | } 75 | 76 | TEST(MoveitSimpleTest, joint_locker_options_j1_j2) 77 | { 78 | auto r1 = checkOptions(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2); 79 | EXPECT_TRUE(r1[0]); 80 | EXPECT_TRUE(r1[1]); 81 | EXPECT_FALSE(r1[2]); 82 | EXPECT_FALSE(r1[3]); 83 | EXPECT_FALSE(r1[4]); 84 | EXPECT_FALSE(r1[5]); 85 | } 86 | 87 | TEST(MoveitSimpleTest, joint_locker_options_none_j3) 88 | { 89 | auto r2 = checkOptions(JointLockOptions::LOCK_NONE | JointLockOptions::LOCK_JOINT_3); 90 | EXPECT_FALSE(r2[0]); 91 | EXPECT_FALSE(r2[1]); 92 | EXPECT_TRUE(r2[2]); 93 | EXPECT_FALSE(r2[3]); 94 | EXPECT_FALSE(r2[4]); 95 | EXPECT_FALSE(r2[5]); 96 | } 97 | 98 | TEST(MoveitSimpleTest, joint_locker_options_all) 99 | { 100 | auto r3 = checkOptions(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2 | 101 | JointLockOptions::LOCK_JOINT_3 | JointLockOptions::LOCK_JOINT_4 | 102 | JointLockOptions::LOCK_JOINT_5 | JointLockOptions::LOCK_JOINT_6); 103 | EXPECT_TRUE(r3[0]); 104 | EXPECT_TRUE(r3[1]); 105 | EXPECT_TRUE(r3[2]); 106 | EXPECT_TRUE(r3[3]); 107 | EXPECT_TRUE(r3[4]); 108 | EXPECT_TRUE(r3[5]); 109 | } 110 | 111 | TEST(MoveitSimpleTest, joint_locker_options_all_none) 112 | { 113 | auto r4 = checkOptions(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2 | 114 | JointLockOptions::LOCK_JOINT_3 | JointLockOptions::LOCK_JOINT_4 | 115 | JointLockOptions::LOCK_JOINT_5 | JointLockOptions::LOCK_JOINT_6 | 116 | JointLockOptions::LOCK_NONE); 117 | EXPECT_TRUE(r4[0]); 118 | EXPECT_TRUE(r4[1]); 119 | EXPECT_TRUE(r4[2]); 120 | EXPECT_TRUE(r4[3]); 121 | EXPECT_TRUE(r4[4]); 122 | EXPECT_TRUE(r4[5]); 123 | } 124 | 125 | } // namespace moveit_simple_test 126 | -------------------------------------------------------------------------------- /moveit_simple/test/joint_locker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | using namespace moveit_simple; 23 | using testing::Types; 24 | 25 | namespace moveit_simple_test 26 | { 27 | 28 | static const std::vector wp1 = {1.5, 1.5, 1.5, 1.5, 1.5, 1.5}; 29 | static const std::vector wp2 = {2.5, 2.5, 2.5, 2.5, 2.5, 2.5}; 30 | 31 | TEST(MoveitSimpleTest, joint_locker_default) 32 | { 33 | auto default_locking(wp2); 34 | auto default_locking_expected(wp2); 35 | JointLocker::lockJoints(wp1, default_locking); 36 | ASSERT_EQ(default_locking, default_locking_expected); 37 | } 38 | 39 | TEST(MoveitSimpleTest, joint_locker_no_locking) 40 | { 41 | auto no_locking(wp2); 42 | auto no_locking_expected(wp2); 43 | JointLocker::lockJoints(wp1, no_locking, JointLockOptions::LOCK_NONE); 44 | ASSERT_EQ(no_locking, no_locking_expected); 45 | } 46 | 47 | TEST(MoveitSimpleTest, joint_locker_lock_j1) 48 | { 49 | auto lock_j1(wp2); 50 | auto lock_j1_expected(wp2); 51 | lock_j1_expected[0] = 1.5; 52 | JointLocker::lockJoints(wp1, lock_j1, JointLockOptions::LOCK_JOINT_1); 53 | ASSERT_EQ(lock_j1, lock_j1_expected); 54 | } 55 | 56 | TEST(MoveitSimpleTest, joint_locker_lock_j2) 57 | { 58 | auto lock_j2(wp2); 59 | auto lock_j2_expected(wp2); 60 | lock_j2_expected[1] = 1.5; 61 | JointLocker::lockJoints(wp1, lock_j2, JointLockOptions::LOCK_JOINT_2); 62 | ASSERT_EQ(lock_j2, lock_j2_expected); 63 | } 64 | 65 | TEST(MoveitSimpleTest, joint_locker_lock_j3) 66 | { 67 | auto lock_j3(wp2); 68 | auto lock_j3_expected(wp2); 69 | lock_j3_expected[2] = 1.5; 70 | JointLocker::lockJoints(wp1, lock_j3, JointLockOptions::LOCK_JOINT_3); 71 | ASSERT_EQ(lock_j3, lock_j3_expected); 72 | } 73 | 74 | TEST(MoveitSimpleTest, joint_locker_lock_j4) 75 | { 76 | auto lock_j4(wp2); 77 | auto lock_j4_expected(wp2); 78 | lock_j4_expected[3] = 1.5; 79 | JointLocker::lockJoints(wp1, lock_j4, JointLockOptions::LOCK_JOINT_4); 80 | ASSERT_EQ(lock_j4, lock_j4_expected); 81 | } 82 | 83 | TEST(MoveitSimpleTest, joint_locker_lock_j5) 84 | { 85 | auto lock_j5(wp2); 86 | auto lock_j5_expected(wp2); 87 | lock_j5_expected[4] = 1.5; 88 | JointLocker::lockJoints(wp1, lock_j5, JointLockOptions::LOCK_JOINT_5); 89 | ASSERT_EQ(lock_j5, lock_j5_expected); 90 | } 91 | 92 | TEST(MoveitSimpleTest, joint_locker_lock_j6) 93 | { 94 | auto lock_j6(wp2); 95 | auto lock_j6_expected(wp2); 96 | lock_j6_expected[5] = 1.5; 97 | JointLocker::lockJoints(wp1, lock_j6, JointLockOptions::LOCK_JOINT_6); 98 | ASSERT_EQ(lock_j6, lock_j6_expected); 99 | } 100 | 101 | TEST(MoveitSimpleTest, joint_locker_lock_j1_j3) 102 | { 103 | auto lock_j1_j3(wp2); 104 | auto lock_j1_j3_expected(wp2); 105 | lock_j1_j3_expected[0] = 1.5; 106 | lock_j1_j3_expected[2] = 1.5; 107 | JointLocker::lockJoints(wp1, lock_j1_j3, JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_3); 108 | ASSERT_EQ(lock_j1_j3, lock_j1_j3_expected); 109 | } 110 | 111 | TEST(MoveitSimpleTest, joint_locker_lock_j1_j3_j5) 112 | { 113 | auto lock_j1_j3_j5(wp2); 114 | auto lock_j1_j3_j5_expected(wp2); 115 | lock_j1_j3_j5_expected[0] = 1.5; 116 | lock_j1_j3_j5_expected[2] = 1.5; 117 | lock_j1_j3_j5_expected[4] = 1.5; 118 | JointLocker::lockJoints(wp1, lock_j1_j3_j5, 119 | JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_3 | JointLockOptions::LOCK_JOINT_5); 120 | ASSERT_EQ(lock_j1_j3_j5, lock_j1_j3_j5_expected); 121 | } 122 | 123 | TEST(MoveitSimpleTest, joint_locker_lock_j4_j6) 124 | { 125 | auto lock_j4_j6(wp2); 126 | auto lock_j4_j6_expected(wp2); 127 | lock_j4_j6_expected[3] = 1.5; 128 | lock_j4_j6_expected[5] = 1.5; 129 | JointLocker::lockJoints(wp1, lock_j4_j6, JointLockOptions::LOCK_JOINT_4 | JointLockOptions::LOCK_JOINT_6); 130 | ASSERT_EQ(lock_j4_j6, lock_j4_j6_expected); 131 | } 132 | 133 | TEST(MoveitSimpleTest, joint_locker_lock_all) 134 | { 135 | auto lock_all(wp2); 136 | auto lock_all_expected(wp1); 137 | JointLocker::lockJoints(wp1, lock_all, JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2 | JointLockOptions::LOCK_JOINT_3 | 138 | JointLockOptions::LOCK_JOINT_4 | JointLockOptions::LOCK_JOINT_5 | JointLockOptions::LOCK_JOINT_6); 139 | ASSERT_EQ(lock_all, lock_all_expected) ; 140 | } 141 | 142 | TEST(MoveitSimpleTest, resolve_to_string_empty) 143 | { 144 | auto str = JointLocker::resolveToString(); 145 | auto str_expected = std::string("LOCK_NONE"); 146 | ASSERT_EQ(str, str_expected); 147 | } 148 | 149 | TEST(MoveitSimpleTest, resolve_to_string_none) 150 | { 151 | auto str = JointLocker::resolveToString(); 152 | auto str_expected = std::string("LOCK_NONE"); 153 | ASSERT_EQ(str, str_expected); 154 | } 155 | 156 | TEST(MoveitSimpleTest, resolve_to_string_single_j1) 157 | { 158 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_1); 159 | auto str_expected = std::string("LOCK_JOINT_1"); 160 | ASSERT_EQ(str, str_expected); 161 | } 162 | 163 | TEST(MoveitSimpleTest, resolve_to_string_single_j2) 164 | { 165 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_2); 166 | auto str_expected = std::string("LOCK_JOINT_2"); 167 | ASSERT_EQ(str, str_expected); 168 | } 169 | 170 | TEST(MoveitSimpleTest, resolve_to_string_single_j3) 171 | { 172 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_3); 173 | auto str_expected = std::string("LOCK_JOINT_3"); 174 | ASSERT_EQ(str, str_expected); 175 | } 176 | 177 | TEST(MoveitSimpleTest, resolve_to_string_single_j4) 178 | { 179 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_4); 180 | auto str_expected = std::string("LOCK_JOINT_4"); 181 | ASSERT_EQ(str, str_expected); 182 | } 183 | 184 | TEST(MoveitSimpleTest, resolve_to_string_single_j5) 185 | { 186 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_5); 187 | auto str_expected = std::string("LOCK_JOINT_5"); 188 | ASSERT_EQ(str, str_expected); 189 | } 190 | 191 | TEST(MoveitSimpleTest, resolve_to_string_single_j6) 192 | { 193 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_6); 194 | auto str_expected = std::string("LOCK_JOINT_6"); 195 | ASSERT_EQ(str, str_expected); 196 | } 197 | 198 | TEST(MoveitSimpleTest, resolve_to_string_double) 199 | { 200 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_3); 201 | auto str_expected = std::string("LOCK_JOINT_1, LOCK_JOINT_3"); 202 | ASSERT_EQ(str, str_expected); 203 | } 204 | 205 | TEST(MoveitSimpleTest, resolve_to_string_triple) 206 | { 207 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2 | 208 | JointLockOptions::LOCK_JOINT_4); 209 | auto str_expected = std::string("LOCK_JOINT_1, LOCK_JOINT_2, LOCK_JOINT_4"); 210 | ASSERT_EQ(str, str_expected); 211 | } 212 | 213 | TEST(MoveitSimpleTest, resolve_to_string_quad) 214 | { 215 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_3 | 216 | JointLockOptions::LOCK_JOINT_5 | JointLockOptions::LOCK_JOINT_6); 217 | auto str_expected = std::string("LOCK_JOINT_1, LOCK_JOINT_3, LOCK_JOINT_5, LOCK_JOINT_6"); 218 | ASSERT_EQ(str, str_expected); 219 | } 220 | 221 | TEST(MoveitSimpleTest, resolve_to_string_all) 222 | { 223 | auto str = JointLocker::resolveToString(JointLockOptions::LOCK_JOINT_1 | JointLockOptions::LOCK_JOINT_2 | 224 | JointLockOptions::LOCK_JOINT_3 | JointLockOptions::LOCK_JOINT_4 | 225 | JointLockOptions::LOCK_JOINT_5 | JointLockOptions::LOCK_JOINT_6); 226 | auto str_expected = std::string("LOCK_JOINT_1, LOCK_JOINT_2, LOCK_JOINT_3, LOCK_JOINT_4, LOCK_JOINT_5, LOCK_JOINT_6"); 227 | ASSERT_EQ(str, str_expected); 228 | } 229 | 230 | } // namespace moveit_simple_test 231 | -------------------------------------------------------------------------------- /moveit_simple/test/kuka_kr210_utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | int main(int argc, char **argv) 25 | { 26 | testing::InitGoogleTest(&argc, argv); 27 | ros::init(argc, argv, "kuka_kr210_utest"); 28 | boost::thread ros_thread(boost::bind(&ros::spin)); 29 | 30 | int res = RUN_ALL_TESTS(); 31 | ros_thread.interrupt(); 32 | ros_thread.join(); 33 | 34 | return res; 35 | } 36 | -------------------------------------------------------------------------------- /moveit_simple/test/launch/kuka_kr210.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /moveit_simple/test/launch/kuka_kr210_utest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | -------------------------------------------------------------------------------- /moveit_simple/test/launch/motoman_mh5.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /moveit_simple/test/launch/motoman_mh5_utest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | -------------------------------------------------------------------------------- /moveit_simple/test/launch/test_display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /moveit_simple/test/motoman_mh5_utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2016 Shaun Edwards 5 | * Copyright (c) 2018 Plus One Robotics 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * you may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | int main(int argc, char **argv) 25 | { 26 | testing::InitGoogleTest(&argc, argv); 27 | ros::init(argc, argv, "motoman_mh5_utest"); 28 | boost::thread ros_thread(boost::bind(&ros::spin)); 29 | 30 | int res = RUN_ALL_TESTS(); 31 | ros_thread.interrupt(); 32 | ros_thread.join(); 33 | 34 | return res; 35 | } -------------------------------------------------------------------------------- /moveit_simple/test/moveit_simple_utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2018 Plus One Robotics 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char **argv) 22 | { 23 | testing::InitGoogleTest(&argc, argv); 24 | return RUN_ALL_TESTS(); 25 | } 26 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | joint_4: 3 | has_velocity_limits: true 4 | max_velocity: 3.124139447 5 | has_acceleration_limits: false 6 | max_acceleration: 0 7 | joint_1: 8 | has_velocity_limits: true 9 | max_velocity: 2.146755039 10 | has_acceleration_limits: false 11 | max_acceleration: 0 12 | joint_3: 13 | has_velocity_limits: true 14 | max_velocity: 1.954768816 15 | has_acceleration_limits: false 16 | max_acceleration: 0 17 | joint_2: 18 | has_velocity_limits: true 19 | max_velocity: 2.007128695 20 | has_acceleration_limits: false 21 | max_acceleration: 0 22 | joint_6: 23 | has_velocity_limits: true 24 | max_velocity: 3.822271167 25 | has_acceleration_limits: false 26 | max_acceleration: 0 27 | joint_5: 28 | has_velocity_limits: true 29 | max_velocity: 3.001966396 30 | has_acceleration_limits: false 31 | max_acceleration: 0 -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | ## IKFast plugin not returning "closest" solution 3 | ## kinematics_solver: kuka_kr210_manipulator_kinematics/IKFastKinematicsPlugin 4 | kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin 5 | kinematics_solver_search_resolution: 0.01 6 | kinematics_solver_timeout: 0.01 7 | kinematics_solver_attempts: 10 8 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/kr210l150.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/kuka_custom_tool.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/kuka_kr210.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/base_link.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_1.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_2.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_3.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_4.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_5.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/collision/link_6.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/base_link.STL -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_1.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_2.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_3.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_4.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_5.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/kuka_kr210/meshes/visual/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/kuka_kr210/meshes/visual/link_6.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - joint_s 5 | - joint_l 6 | - joint_u 7 | - joint_r 8 | - joint_b 9 | - joint_t -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | joint_b: 6 | has_velocity_limits: true 7 | max_velocity: 3.48 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | joint_l: 11 | has_velocity_limits: true 12 | max_velocity: 2.96 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | joint_r: 16 | has_velocity_limits: true 17 | max_velocity: 3.48 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | joint_s: 21 | has_velocity_limits: true 22 | max_velocity: 2.96 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | joint_t: 26 | has_velocity_limits: true 27 | max_velocity: 6.97 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | joint_u: 31 | has_velocity_limits: true 32 | max_velocity: 2.96 33 | has_acceleration_limits: false 34 | max_acceleration: 0 -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/joint_names.yaml: -------------------------------------------------------------------------------- 1 | # This file is only required if your robot uses non-standard joint names 2 | controller_joint_names: [joint_s, joint_l, joint_u, joint_r, joint_b, joint_t] 3 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: motoman_mh5_manipulator_kinematics/IKFastKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/motoman_mh5_robot.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | FMTkConfigDefault: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMTkConfigDefault: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDSTkConfigDefault: 74 | type: geometric::PDST 75 | STRIDEkConfigDefault: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRTkConfigDefault: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRTkConfigDefault: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiESTkConfigDefault: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjESTkConfigDefault: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRMkConfigDefault: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstarkConfigDefault: 110 | type: geometric::LazyPRMstar 111 | SPARSkConfigDefault: 112 | type: geometric::SPARS 113 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwokConfigDefault: 118 | type: geometric::SPARStwo 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | manipulator: 124 | planner_configs: 125 | - SBLkConfigDefault 126 | - ESTkConfigDefault 127 | - LBKPIECEkConfigDefault 128 | - BKPIECEkConfigDefault 129 | - KPIECEkConfigDefault 130 | - RRTkConfigDefault 131 | - RRTConnectkConfigDefault 132 | - RRTstarkConfigDefault 133 | - TRRTkConfigDefault 134 | - PRMkConfigDefault 135 | - PRMstarkConfigDefault 136 | - FMTkConfigDefault 137 | - BFMTkConfigDefault 138 | - PDSTkConfigDefault 139 | - STRIDEkConfigDefault 140 | - BiTRRTkConfigDefault 141 | - LBTRRTkConfigDefault 142 | - BiESTkConfigDefault 143 | - ProjESTkConfigDefault 144 | - LazyPRMkConfigDefault 145 | - LazyPRMstarkConfigDefault 146 | - SPARSkConfigDefault 147 | - SPARStwokConfigDefault -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_BASE_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_BASE_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_B_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_B_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_L_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_L_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_R_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_R_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_S_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_S_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_T_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_T_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_U_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/collision/MH5_U_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_BASE_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_BASE_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_B_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_B_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_L_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_L_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_R_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_R_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_S_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_S_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_T_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_T_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_U_AXIS.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusone-robotics/moveit_simple/a802af8fba663ec9bbbd78dc196763fcb5e70c28/moveit_simple/test/resources/motoman_mh5/meshes/mh5/visual/MH5_U_AXIS.stl -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/urdf/mh5_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/urdf/motoman_mh5_robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /moveit_simple/test/resources/motoman_mh5/urdf/motoman_mh5_robot_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /moveit_simple_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_simple_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2020-01-28) 6 | ------------------ 7 | * Fix version discrepancy `#100 `_ 8 | 9 | 0.2.0 (2019-07-03) 10 | ------------------ 11 | 12 | 0.1.0 (2019-04-30) 13 | ------------------ 14 | * [feature] trajectory training service interface `#73 `_, request to re-load from config, diagnostic info, lookupTrajectoryPoint. 15 | * Contributors: Aaron Wood, Geoffrey Chiou, Mike Lautman, Shaun Edwards 16 | -------------------------------------------------------------------------------- /moveit_simple_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_simple_msgs) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | message_generation 8 | roscpp 9 | sensor_msgs 10 | std_msgs 11 | tf2 12 | tf2_geometry_msgs 13 | tf2_ros 14 | ) 15 | 16 | add_message_files( 17 | FILES 18 | CombinedJointPoint.msg 19 | ) 20 | 21 | add_service_files( 22 | FILES 23 | LookupTrajectory.srv 24 | LookupWaypoint.srv 25 | ) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | geometry_msgs 31 | ) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS 35 | LIBRARIES 36 | CATKIN_DEPENDS 37 | message_runtime 38 | roscpp 39 | sensor_msgs 40 | std_msgs 41 | tf2 42 | tf2_geometry_msgs 43 | tf2_ros 44 | ) 45 | 46 | -------------------------------------------------------------------------------- /moveit_simple_msgs/msg/CombinedJointPoint.msg: -------------------------------------------------------------------------------- 1 | # Combined Joint Point Message 2 | string waypoint_name 3 | string[] joint_names 4 | float64[] joint_point 5 | geometry_msgs/TransformStamped transform_stamped -------------------------------------------------------------------------------- /moveit_simple_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_simple_msgs 4 | 0.2.1 5 | The moveit_simple_msgs package 6 | 7 | Aaron Wood 8 | 9 | Apache 2.0 10 | Boost Software License 11 | 12 | Aaron Wood 13 | 14 | catkin 15 | message_generation 16 | roscpp 17 | sensor_msgs 18 | std_msgs 19 | tf2 20 | tf2_geometry_msgs 21 | tf2_ros 22 | roscpp 23 | sensor_msgs 24 | std_msgs 25 | tf2 26 | tf2_geometry_msgs 27 | tf2_ros 28 | message_runtime 29 | roscpp 30 | sensor_msgs 31 | std_msgs 32 | tf2 33 | tf2_geometry_msgs 34 | tf2_ros 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /moveit_simple_msgs/srv/LookupTrajectory.srv: -------------------------------------------------------------------------------- 1 | # Trajectory Lookup 2 | # Requests all waypoints associated with a trajectory name 3 | 4 | string trajectory_name 5 | --- 6 | bool success 7 | CombinedJointPoint[] waypoints -------------------------------------------------------------------------------- /moveit_simple_msgs/srv/LookupWaypoint.srv: -------------------------------------------------------------------------------- 1 | # Waypoint Lookup 2 | 3 | string waypoint_name # Name of waypoints to look up 4 | --- 5 | bool success # True if waypoint found, False otherwise 6 | CombinedJointPoint waypoint # Requested waypoint 7 | --------------------------------------------------------------------------------