├── .env ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.assets ├── image-20210323222536538.png ├── image-20210323222712987.png ├── image-20210323222920761.png ├── image-20210323223249480.png ├── image-20220617164001221.png ├── image-20220617164143378.png ├── image-20220617164603420.png ├── image-20220617185222262.png ├── image-20220618155407298.png ├── image-20220619145428179.png ├── image-20220619145856797.png ├── image-20220619153333535.png ├── image-20220619201211249.png ├── image-20220619210632902.png ├── image-20220619210635579.png ├── image-20220619222710468.png ├── image-20220619223223897.png ├── image-20220619223646039.png └── 坐标系示意图.png ├── README.md ├── config ├── default │ └── GTE.tf └── ur5 │ ├── armConfig.yaml │ └── gripper │ └── AG95 │ ├── GTEg.tf │ └── LTE.tf ├── launch ├── check_and_grasp.launch ├── go_grasp.launch ├── state_checker.launch └── ur5_moveit_rviz.launch ├── package.xml └── scripts ├── dh_hand_client.py └── go_grasp.py /.env: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/.env -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(go_grasp) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES panda_go_grasp 107 | # CATKIN_DEPENDS roscpp rospy 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/panda_go_grasp.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/panda_go_grasp_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | catkin_install_python(PROGRAMS 162 | scripts/go_grasp.py 163 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_panda_go_grasp.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Hymwgk 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.assets/image-20210323222536538.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20210323222536538.png -------------------------------------------------------------------------------- /README.assets/image-20210323222712987.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20210323222712987.png -------------------------------------------------------------------------------- /README.assets/image-20210323222920761.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20210323222920761.png -------------------------------------------------------------------------------- /README.assets/image-20210323223249480.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20210323223249480.png -------------------------------------------------------------------------------- /README.assets/image-20220617164001221.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220617164001221.png -------------------------------------------------------------------------------- /README.assets/image-20220617164143378.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220617164143378.png -------------------------------------------------------------------------------- /README.assets/image-20220617164603420.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220617164603420.png -------------------------------------------------------------------------------- /README.assets/image-20220617185222262.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220617185222262.png -------------------------------------------------------------------------------- /README.assets/image-20220618155407298.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220618155407298.png -------------------------------------------------------------------------------- /README.assets/image-20220619145428179.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619145428179.png -------------------------------------------------------------------------------- /README.assets/image-20220619145856797.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619145856797.png -------------------------------------------------------------------------------- /README.assets/image-20220619153333535.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619153333535.png -------------------------------------------------------------------------------- /README.assets/image-20220619201211249.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619201211249.png -------------------------------------------------------------------------------- /README.assets/image-20220619210632902.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619210632902.png -------------------------------------------------------------------------------- /README.assets/image-20220619210635579.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619210635579.png -------------------------------------------------------------------------------- /README.assets/image-20220619222710468.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619222710468.png -------------------------------------------------------------------------------- /README.assets/image-20220619223223897.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619223223897.png -------------------------------------------------------------------------------- /README.assets/image-20220619223646039.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/image-20220619223646039.png -------------------------------------------------------------------------------- /README.assets/坐标系示意图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/go_grasp/dddbd701837ef09aaed444d6a4054b9d845f1c30/README.assets/坐标系示意图.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # go_grasp 2 | 基于ROS & Moveit，订阅给定的抓取位姿话题，使用链式机械臂进行抓取，代码目前在UR5机械臂上已进行了测试，未来希望也能够适配于panda机械臂。 3 | 4 | ## 基本信息 5 | 6 | - 订阅接收数据类型为[`GraspConfigList`](https://github.com/Hymwgk/gpg/tree/master/msg)的抓取位姿话题 7 | - 默认执行所接受话题中`GraspConfigList`格式数据中包含的第一个`GraspConfig`数据所代表的抓取 8 | - 默认订阅的抓取位姿的形式，使用`典范抓取坐标系`形式表示 9 | 10 | 11 | 12 | ## 代码涉及的基础抓取信息 13 | 14 | 15 | 16 | image-20220619153333535 17 | 18 | ### 符号说明 19 | 20 | 以下符号基本延续在书本《机器人学导论》中的符号表示形式； 21 | 22 | 需要特别说明的是，我们设定机械臂在接收到指定的抓取位姿之后，不会直接将夹爪移动到对应的抓取位姿，而是将夹爪首先移动到一个`预备抓取姿态`，然后从该预备姿态，沿着抓取approach轴方向直线靠近抓取姿态，直至到达目标。 23 | 24 | 为了区分抓取状态和预备抓取状态时的机械臂所处的状态，我们通过对位置可变动坐标系的符号添加下标`g`（grasp抓取状态）与`p`（pre-grasp预备抓取状态）分别代表这两种不同的状态；例如，变换关系`BTE`代表坐标系{E}相对于坐标系{B}的普遍变换关系，而`BTEg`特指在抓取情形下，坐标系{E}相对于坐标系{B}的变换关系，特殊的，对于相对位置固定的坐标系`{E}`与`{L}`之间存在关系`LgTEg = LpTEp = LTE` 25 | 26 | 27 | 28 | 注意理解下文中： 情形，状态等词语的区别 29 | 30 | ### 涉及的连杆与物体（以UR5为例） 31 | 32 | `base_link` ：ur5机械臂规划组里的基座连杆 33 | 34 | `wrist_3_link`：ur5机械臂末端最后一个连杆 35 | 36 | `AG95` ：使用的机械手实物， 37 | 38 | `Realsense D435`：使用的相机实物 39 | 40 | `GRIPPER`：虚拟夹爪 41 | 42 | `Object`：桌面上的目标物体 43 | 44 | 45 | 46 | ### 坐标系 47 | 48 | 表征一个刚体在三维空间中的位置与姿态（位姿），可以通过在刚体上固连一个坐标系，并描述该坐标系相对于空间参考坐标系的变换关系来实现，下面解释本抓取例子中各个连杆与物体上固连的坐标系系统。 49 | 50 | 51 | 52 | **`{GC}`**：典范抓取坐标系，固连在虚拟夹爪`GRIPPER`上，它主要描述 **在抓取情形下** 一种典型的抓取坐标系与夹爪的相对固连方式 ，不用于描述具体的抓取姿态 53 | 54 | > - 定义，典范抓取坐标系的x轴为approach轴，y轴为闭合方向轴记为binormal，z轴为夹爪指向夹爪正面，记为minor_pc轴（使用Dex-net中定义的形式） 55 | > - 所谓虚拟夹爪，可以认为是物理夹爪在抓取情形下的表征，代表物理夹爪在抓取情形下的状态 56 | > - 虚拟夹爪在显示的时候，我们把它简化为图中所示的形式，它的重要的基本几何尺寸与实际夹爪形同，可以[参照这里的一些解释](https://github.com/Hymwgk/PointNetGPD) 57 | 58 | **`{G}`**：表征抓取位姿的坐标系，与实际夹爪`AG95`的固连形式 与 {GC}和`GRIPPER`固连形式相同 59 | 60 | > 一旦确定了该坐标系相对于一个父坐标系的位置姿态，**抓取情形下**，实际夹爪在父坐标系中的具体位置姿态也就确定了 61 | > 62 | > 仅在抓取情形下有意义，因此在变换关系中可省略下标`g` ，例如GgTEg=GTEg 63 | 64 | **`{O}`**：固连于物体`Object`之上的物体坐标系（或者叫Local frame） 65 | 66 | > 在计算过程中，我们通常无法直接计算抓取坐标系{G}相对于某个父坐标系的位姿，而是先计算出坐标系{O}的位姿，然后使用一个人为设定的变换关系OTG来推算出坐标系{G}的位姿 67 | 68 | **`{C}`**：相机坐标系，在我们UR5的例子中，使用realsense D435的 `camera_color_optical_frame`坐标系，固连于其rgb光学镜头 69 | 70 | > 通常，在进行抓取姿态检测时，得到的抓取位姿都是使用CTG来表示的，即，最初视觉抓取检测出的抓取一般都是以相机坐标系{C} 为父坐标系 71 | 72 | **`{E}`** ：实际夹爪的指尖中心固连（抓取）坐标系，在我们的UR5例子中，认为坐标系{E}固连在AG95夹爪指尖中心位置处 73 | 74 | > - 与{G} 不同的是，坐标系{E}任何时间均会随着实际夹爪移动而移动，坐标系{G}仅在抓取情形下有意义； 75 | > 76 | > - 坐标系{E}与实际夹爪的固连形态可能是由系统直接指定，也可能是由自己人为指定，例如本UR5例子中，为了增加例子说明度，我们采用{L}与类似`wrist_3_link` 连杆的固连形态，因此在本例子抓取情形时，{E} 与 {G}之间存在一个确定的变换关系G**T**Eg （当然你也可以直接使用坐标系{G}与虚拟夹爪之间相同的固连形态） 以下左侧第一个是我们人为指定的固连方式 77 | > 78 | > ![image-20220619223646039](README.assets/image-20220619223646039.png) 79 | 80 | **`{B}`**：机械臂基座连杆的固连坐标系，在UR5机械臂中，固连于`base_link`连杆 81 | 82 | **`{L}`**：机械臂末端连杆的固连坐标系，在UR5机械臂中，固连于`wrist_3_link` 连杆 83 | 84 | 85 | 86 | ### 坐标系间的变换关系 87 | 88 | 说明： 以下的变换中，符号`T`代表`to`的含义，表示从一坐标系到另一坐标系的变换关系（变换矩阵），也可以表述为，一个坐标系相对于另一个坐标系中的位姿（位置姿态），例如，`ATB`代表坐标系`{A}`到坐标系{B}的变换，也可表述为坐标系{B}相对于坐标系{A}的位姿，以下存在一定程度的说法混用，注意分辨。 89 | 90 | 91 | 92 | #### 基本变换关系 93 | 94 | **`BTC`**：相机坐标系{C}相对于基座坐标系{B}的位置姿态，手眼标定之后即可确定； 95 | 96 | > 实际上，如果是眼在手上的形式，手眼标定其实是确定了关系`LTC` ，关系`BTC`则由BTC=BTL*LTC 实时计算确定，其中`BTL`由正运动学计算得到 97 | > 98 | > 在ROS中，我们可以使用TF直接读取该变换关系 99 | 100 | **`CTG`**：抓取坐标系{G}在相机坐标系{C}中的位姿，即，所订阅话题中的目标抓取位姿的表示形式 101 | 102 | **`GTEg`**：**抓取情形下**，抓取坐标系`{G}`相对于实际夹爪指尖固连坐标系`{E}` 的变换关系 103 | 104 | **`LTE`**：指尖坐标系`{E}`与末端连杆固连坐标系`{L}`之间的变换关系，是一个由物理机械结构尺寸确定的固定变换关系 105 | 106 | > 例如本例子中，AG95夹爪结构尺寸确定，且固定连接在`wrist_3_link`连杆上，因此该变换是一个固定值 107 | 108 | **`EgTEp`**：坐标系{E}在预备抓取与抓取情形下 之间的变换关系，为人为设定的固定值 109 | 110 | 111 | 112 | #### 中间变换关系解释 113 | 114 | **`BTEg`**：特指抓取情形下，实际夹爪指尖抓取坐标系`{E}`，相对于机械臂基座固连坐标系`{B}`的位姿 115 | 116 | > 在代码中，我们使用以下表示形式，将变换关系拆分为平移和旋转 117 | > 118 | > - `BTEg_rot`：变换矩阵`BTEg`的旋转分量 119 | > 120 | > - `BTEg_trans`：变换矩阵`BTEg`的位置分量，以下变换同理，省略不写 121 | 122 | **`BTEp`**：特指预备抓取情形下，实际夹爪指尖抓取坐标系`{E}`，相对于机械臂基座固连坐标系`{B}`的位姿 123 | 124 | 125 | 126 | #### 求解的目标变换关系 127 | 128 | 在进行手眼标定之后，我们可以使用正运动学，实时读取关系**`BTC`**，并结合给出的抓取位姿变换CTG，使用上述**基本变换关系**计算出抓取与预抓取情形下，机械臂末端执行器的变换关系`BTEg`与`BTEp` ，进而计算出以下两个关系`BTLg`与`BTLp` ，最终使用Moveit进行机械臂各关节逆运动学解，并规划运动轨迹。 129 | 130 | **`BTLg`**：抓取情形下，机械臂末端连杆上的固连坐标系`{L}`，相对于机械臂基座固连坐标系`{B}`的位姿，例如抓取情形下ur5机械臂中的`wrist_3_link`坐标系，相对于`base_link`坐标系的位姿 131 | 132 | **`BTLp`**：预备抓取情形下，`wrist_3_link`固连坐标系`{L}`，相对于机械臂基座`base_link`固连的坐标系`{B}`的位姿 133 | 134 | 135 | 136 | ### 人为设定参数与条件 137 | 138 | #### 给定的约束条件 139 | 140 | - 我们人为指定抓取情形下，抓取坐标系{G}与虚拟夹爪的固连方式 采取 典范抓取坐标系形式 141 | 142 | > **该设定条件的必要性：**在抓取执行环节中，不同的机械臂的夹爪指尖中心固连坐标系{E}可能是不同的；为了将抓取检测与抓取执行环节脱耦，我们需要设定抓取检测环节产生的抓取坐标系与夹爪的统一固连方式，这个条件的设定通常是在抓取检测环节确定的（Dex-net与PointNetGPD均使用与本UR5例子相同的形式），抓取执行环节仅需根据关系`GTEg`即可得到抓取情形下，物理实际夹爪的位置姿态 143 | 144 | 145 | 146 | #### 根据硬件实际尺寸，给定参数 147 | 148 | - 变换关系**`LTE`**：在本UR5例子中，由于AG95是第三方厂商生产的，并非标准配件，因此我们需要自己去根据夹爪硬件尺寸参数指定该变换关系，为方便起见，我们以ROS系统中`ur5_ moveit_config`程序包中给出的坐标系{L}为参考系，设定{E}相对于{L}仅存在一个沿z轴正向的偏移量`offset = 0.1034`，姿态不发生旋转 149 | 150 | > 如果使用自带标准夹爪，一般情况下，在ROS的Moveit配置包中，会有一个厂家设定好的指尖中心抓取坐标系，例如使用panda机械臂的自带电动夹爪时，可以直接选定坐标系{E}为预设的`panda_EE`坐标系，可以使用TF直接读取变换关系**`LTE`**即可，而不需人为设定 151 | 152 | - 变换关系**`GTEg`**：坐标系{E}与实际夹爪的固连形式，以及坐标系{G}与虚拟夹爪的固连形式都是我们自己设定的，因此需要指定在抓取情形下，两者存在的此固定变换关系 153 | 154 | - 变换关系**`OTG`**：计算{G}的位姿，通常需要先计算出坐标系{O}的位姿，我们设定坐标系{G}相对于{O}仅在x轴正向存在一个偏移量，姿态不发生偏转 155 | - 变换关系**`EgTEp`**：坐标系{E}在预备抓取与抓取情形下 之间的变换关系，我们设定两种情形下，{Ep}相对于{Eg}仅存在z轴负方向的一个偏移`retreat = 0.15`，姿态不发生偏转 156 | 157 | #### 外部输入抓取表示形式 158 | 159 | 约定，外部输入的抓取姿态，统一使用`CTG`的形式表示，固连方式使用典范抓取坐标系形式。 160 | 161 | 162 | 163 | ### 目标变换计算过程 164 | 165 | 本代码中，接收的话题中的抓取位姿的表示，是使用CTG来表示的，**目标是**，根据坐标系之间的变换关系，求出在抓取情形和预备抓取情形下的变换关系`BTLg`与`BTLp`，通过间接对`wrist_3_link`连杆进行轨迹规划并移动，使得在预备抓取和抓取情况下，夹爪与基座满足变换关系`BTEg`，具体变换链条为 166 | 167 | image-20220619210635579 168 | 169 | 1. 根据手眼标定得到的BTC 计算抓取坐标系{G}在基座坐标系下的位姿`BTG=BTC*CTG` 170 | 2. 根据变换关系`GTEg`计算得到在抓取情形下，夹爪坐标系{E}相对于基座的位姿`BTEg=BTG*GTEg` 171 | 3. 根据变换关系`EgTEp`计算得到在预备抓取情形下，夹爪坐标系{E}相对于基座的位姿`BTEp=BTEg*EgTEp` 172 | 4. 对末端连杆与夹爪的相对位姿关系`LTE`求逆得到`ETL` 173 | 5. 根据`ETL`计算得到抓取情形下，末端连杆在基座坐标系下的位姿`BTLg=BTEg*ETL` 174 | 6. 根据`ETL`计算得到预备抓取情形下，末端连杆在基座坐标系下的位姿`BTLp=BTEp*ETL` 175 | 176 | 177 | 178 | image-20220617164603420 179 | 180 | 181 | 182 | ## UR5机械臂执行抓取 183 | 184 | ### 仿真环境测试抓取 185 | 186 | 此时并不真正抓取，仅仅执行一个固定动作 187 | 188 | 1. 启动仿真环境 189 | 190 | ``` 191 | roslaunch ur5_moveit_config demo.launch 192 | ``` 193 | 194 | 2. 启动go_grasp 节点 (python 2.7) 195 | 196 | ``` 197 | roslaunch go_grasp go_grasp.launch robot_name:='ur5' mode:='test' 198 | ``` 199 | 200 | 201 | 202 | ### 仿真环境抓取 203 | 204 | 订阅外部节点发布的抓取信息，并在虚拟环境中执行抓取； 需要提前发布 抓取话题 以及发布手眼位姿关系；这里以一个realsense D435眼在手上安装方式为例， 205 | 206 | 207 | 208 | 1. 对UR5进行[手眼标定](https://github.com/Hymwgk/ur5_hand_eye_calibrate) 209 | 210 | 2. 发布手眼标定关系 211 | 212 | ``` 213 | roslaunch ur5_hand_eye_calibrate publish_ur5_eoh.launch 214 | ``` 215 | 216 | 3. (python 3.8.5) 检测桌面目标物体并发布抓取信息，这里以一个简单的用YOLO v5检测目标并执行抓取的代码为例，注意，这里使用yolo_v5检测了3-Dof的平面抓取位姿，但是本仓库的代码可以执行6-DOF的抓取，仅需要替换掉这里使用检测节点，使用自己的GPD算法发布6-dof抓取位姿即可； 217 | 218 | **Tip：**如果想观察6-Dof的效果，可以查看我的[这个仓库](https://github.com/Hymwgk/gpg)，融合了gpg与PointNetGPD两部分的代码 219 | 220 | ``` 221 | roslaunch yolov5_ros yolo_v5.launch 222 | ``` 223 | 224 | 4. 安装UR5的Moveit相关程序包，并启动仿真环境 225 | 226 | ``` 227 | roslaunch ur5_moveit_config demo.launch 228 | ``` 229 | 230 | 2. (python 2.7) 启动go_grasp节点，观察抓取效果，在使用自己的gpd算法时，注意要根据自己的代码以及实际硬件修改`launch`文件中的参数，具体参数含义可以查看`go_grasp.py`中的详细注释 231 | 232 | ``` 233 | roslaunch go_grasp go_grasp.launch robot_name:='ur5' mode:='run' 234 | ``` 235 | 236 | 237 | 238 | ### 实机抓取 239 | 240 | 1. 解锁机械臂 241 | 242 | ```bash 243 | roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.1.110 244 | ``` 245 | 246 | 2. 在机械臂端启动底层接口 247 | 248 | 249 | 250 | 3. 启动moveit 251 | 252 | ``` 253 | roslaunch ur5_hand_eye_calibrate ur5_moveit_rviz.launch 254 | ``` 255 | 256 | 4. 启动 257 | 258 | 259 | 260 | 261 | 262 | ## 状态转移 263 | 264 | 265 | 266 | 眼在手外： 267 | 268 | 1. 启动 269 | 2. 移动到`初始化状态`，此时机械臂蜷缩，避免阻挡相机视线 270 | 3. 检测抓取位姿 271 | 4. 移动到`工作状态`，此时机械臂末端垂直与桌面（相机坐标系z轴近似垂直桌面） 272 | 5. 移动到`预备抓取位置姿态` 273 | 6. 移动到`抓取位姿` 274 | 7. 返回`预备抓取姿态` 275 | 8. 返回到`工作状态` 276 | 9. 移动到`放置状态` 277 | 10. 返回到`初始化状态` 278 | 11. 从步骤2-10循环直到没有目标剩余 279 | 280 | 281 | 282 | 眼在手上（目前代码实现的是这种状态） 283 | 284 | 1. 启动 285 | 2. 移动到`工作状态`，此时机械臂末端垂直与桌面（相机坐标系z轴近似垂直桌面）方便检测 286 | 3. 检测抓取位姿 287 | 4. 移动到`预备抓取位置姿态` 288 | 5. 移动到`抓取位姿` 289 | 6. 返回`预备抓取姿态` 290 | 7. 返回到`工作状态` 291 | 8. 移动到`放置状态` 292 | 9. 返回到`工作状态` 293 | 10. 从步骤2-9循环直到没有目标剩余 294 | -------------------------------------------------------------------------------- /config/default/GTE.tf: -------------------------------------------------------------------------------- 1 | grasp 2 | gripper 3 | 0.000000 0.000000 0.000000 4 | 1.000000 0.000000 0.000000 5 | 0.000000 1.000000 0.000000 6 | 0.000000 0.000000 1.000000 7 | -------------------------------------------------------------------------------- /config/ur5/armConfig.yaml: -------------------------------------------------------------------------------- 1 | retreat: 0.15 #预备后撤距离 2 | max_acceleration_scaling_factor: 0.15 #机械臂最大加速度比例因子 3 | max_velocity_scaling_factor: 0.25 #机械臂最大速度比例因子 4 | initial_joints: [1.1318244934082031, -1.5151599089251917, -0.310127083455221, -1.171316925679342, -1.6035016218768519, -0.34478789964784795] #打开机械臂之后的初始化姿态 5 | working_joints: [-0.19, -1.52, 1.37, -1.47, -1.52, -0.19] #打开相机检测目标时候的姿态（各个关节角） 6 | place_joints: [0.53, -1.74,1.98, -1.91, -1.52, -0.19] #放置状态，可以设置多个 7 | -------------------------------------------------------------------------------- /config/ur5/gripper/AG95/GTEg.tf: -------------------------------------------------------------------------------- 1 | grasp 2 | gripper 3 | 0.000000 0.000000 0.000000 4 | 0.000000 1.000000 0.000000 5 | 0.000000 0.000000 1.000000 6 | 1.000000 0.000000 0.000000 7 | -------------------------------------------------------------------------------- /config/ur5/gripper/AG95/LTE.tf: -------------------------------------------------------------------------------- 1 | final_link 2 | gripper 3 | 0.000000 0.000000 0.103400 4 | 1.000000 0.000000 0.000000 5 | 0.000000 1.000000 0.000000 6 | 0.000000 0.000000 1.000000 7 | -------------------------------------------------------------------------------- /launch/check_and_grasp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/go_grasp.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 | -------------------------------------------------------------------------------- /launch/state_checker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/ur5_moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | go_grasp 4 | 0.0.0 5 | The go_grasp package 6 | 7 | 8 | 9 | 10 | wgk_ZZU 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | roscpp 53 | rospy 54 | roscpp 55 | rospy 56 | roscpp 57 | rospy 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /scripts/dh_hand_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import actionlib 5 | from dh_hand_driver.msg import * 6 | from dh_hand_driver.srv import * 7 | 8 | 9 | def go_position(MotorID, force, position): 10 | # Creates the SimpleActionClient, passing the type of the action 11 | # (FibonacciAction) to the constructor. 12 | # client = actionlib.SimpleActionClient('actuate_hand', dh_hand_driver.action.ActuateHand) 13 | client = actionlib.SimpleActionClient('actuate_hand', ActuateHandAction) 14 | 15 | # Waits until the action server has started up and started 16 | # listening for goals. 17 | client.wait_for_server() 18 | 19 | # Creates a goal to send to the action server. 20 | # goal = dh_hand_driver.action.ActuateHandGoal() 21 | goal = ActuateHandGoal() 22 | goal.MotorID = MotorID 23 | goal.force = force 24 | goal.position = position 25 | 26 | # Sends the goal to the action server. 27 | client.send_goal(goal) 28 | 29 | # Waits for the server to finish performing the action. 30 | client.wait_for_result() 31 | 32 | # Prints out the result of executing the action 33 | return client.get_result() # A FibonacciResult 34 | 35 | 36 | def hand_close(force): 37 | res = go_position(1, int(force), 0) 38 | return res 39 | 40 | 41 | def hand_open(): 42 | res = go_position(1, 50, 95) 43 | return res 44 | 45 | 46 | if __name__ == '__main__': 47 | try: 48 | # Initializes a rospy node so that the SimpleActionClient can 49 | # publish and subscribe over ROS. 50 | rospy.init_node('actuate_hand_client') 51 | # result = go_position() 52 | result = hand_close(50) 53 | # result = hand_open() 54 | print("Result:", result.opration_done) 55 | except rospy.ROSInterruptException: 56 | print("program interrupted before completion", file=sys.stderr) 57 | -------------------------------------------------------------------------------- /scripts/go_grasp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | """ 4 | 已测试python环境：2.7.16 5 | author: wgk_ZZU 6 | date: 2022/06/18 7 | abcwgk@gmail.com 8 | """ 9 | 10 | #from sqlalchemy import false 11 | import rospy, sys 12 | import rospkg 13 | import rosparam 14 | import moveit_commander 15 | import os 16 | #print('Hello,'+os.environ.get('ROS_MASTER_URI')+'!') 17 | #import moveit_commander 18 | import tf 19 | import argparse 20 | import math 21 | import numpy as np 22 | from math import pi 23 | import time 24 | import copy 25 | from moveit_msgs.msg import RobotTrajectory,DisplayTrajectory 26 | from trajectory_msgs.msg import JointTrajectoryPoint 27 | from threading import Lock, Event 28 | 29 | #from franka_msgs.srv import SetCartesianImpedance, \ 30 | # SetCartesianImpedanceRequest, \ 31 | # SetCartesianImpedanceResponse 32 | 33 | #from controller_manager_msgs.srv import SwitchController,SwitchControllerRequest,SwitchControllerResponse 34 | 35 | import actionlib 36 | from geometry_msgs.msg import PoseStamped, Pose 37 | from tf.transformations import euler_from_quaternion, quaternion_from_euler,quaternion_multiply,quaternion_from_matrix,quaternion_matrix 38 | #from autolab_core import RigidTransform,transformations 39 | #from pyquaternion import Quaternion 40 | from gpg.msg import GraspConfig,GraspConfigList 41 | from autolab_core import RigidTransform 42 | #from franka_gripper.msg import GraspAction, GraspGoal 43 | #from franka_gripper.msg import GraspEpsilon 44 | #解析命令行参数 45 | #parser = argparse.ArgumentParser(description='Go grasp') 46 | #parser.add_argument('--test',type=int, default=1) #设置同时处理几个场景 47 | #parameters,unknow =parser.parse_known_args() 48 | 49 | 50 | 51 | class GoGrasp: 52 | def __init__(self): 53 | #初始化moveit的 API接口 54 | moveit_commander.roscpp_initialize(sys.argv) 55 | #初始化ros节点 名为ur_grasp 56 | rospy.init_node('go_grasp', anonymous=True) 57 | rospy.set_param("/robot_state", "Initializing") 58 | rospy.loginfo("Robot initializing") 59 | 60 | #获取当前程序包地址 61 | rospack = rospkg.RosPack() 62 | current_path=rospack.get_path('go_grasp') 63 | 64 | robot_name = rospy.get_param('~robot_name', 'ur5') 65 | #机械臂Moveit规划组名称 66 | move_group_name = rospy.get_param('~move_group_name', 'manipulator') 67 | #机械臂Moveit规划组的基座link坐标系 68 | base_frame = rospy.get_param('~base_frame', 'base_link') 69 | #订阅发布GraspConfigList的话题名称 70 | grasp_list_topic = rospy.get_param('~grasp_list_topic', '/yolov5/detection_grasps') 71 | #需要说明接收的抓取位姿，是以谁为父坐标系的 72 | grasp_father_frame = rospy.get_param('~grasp_father_frame', 'camera_color_optical_frame') 73 | #指定夹爪指尖固连坐标系E与点发抓取坐标系G之间的变换关系GTEg的配置文件 74 | gte_config_path = rospy.get_param('~gte_config_path',os.path.join(current_path,'config/ur5/gripper/AG95/GTEg.tf')) 75 | #机械臂末端link与夹爪指尖固连坐标系E变换关系LTE的配置文件 76 | lte_config_path = rospy.get_param('~lte_config_path', os.path.join(current_path,'config/ur5/gripper/AG95/LTE.tf')) 77 | #机械臂配置参数文件地址 78 | arm_config_path = rospy.get_param('~arm_config_path', os.path.join(current_path,'config/ur5/armConfig.yaml')) 79 | #获得模式，默认测试模式 80 | mode = rospy.get_param('~mode', 'test') 81 | 82 | 83 | #先从从 yaml读取关于机械臂的配置参数,[({参数名，参数值},命名空间),(),...] 84 | #并上传ros服务器,以robot_name为命名空间 85 | arm_config_list=rosparam.load_file(arm_config_path) 86 | for params, _ in arm_config_list: 87 | rosparam.upload_params(robot_name,params) 88 | 89 | 90 | 91 | #再从参数服务器读取刚上传的yaml配置参数 92 | self.retreat = rospy.get_param(robot_name+'/retreat', '0.15') 93 | self.max_acceleration_scaling_factor = rospy.get_param(robot_name+'/max_acceleration_scaling_factor', '0.1') 94 | self.max_velocity_scaling_factor = rospy.get_param(robot_name+'/max_velocity_scaling_factor', '0.2') 95 | 96 | self.initial_joints = rospy.get_param(robot_name+'/initial_joints', '') 97 | 98 | self.working_joints = rospy.get_param(robot_name+'/working_joints', '') 99 | self.place_joints = rospy.get_param(robot_name+'/place_joints', '') 100 | 101 | 102 | 103 | #读取GTEg变换 104 | self.GTEg = RigidTransform.load(gte_config_path) 105 | #读取LTE变换 (夹爪指尖固连坐标系在机械臂末端连杆中的位姿，一般用于非机械臂自带的夹爪) 106 | self.LTE = RigidTransform.load(lte_config_path) 107 | 108 | 109 | #BTLg 最终抓取状态时，机械臂末端Link相对于基座的位姿 110 | self.grasp_pose_wrist3=Pose() 111 | #BTLp 预抓取状态时，机械臂末端Link相对于基座的位姿 112 | self.pre_grasp_pose_wrist3=Pose() 113 | 114 | 115 | #构建tf发布器 116 | self.tf_broadcaster=tf.TransformBroadcaster() 117 | 118 | #self.grasp_config=GraspConfig() 119 | 120 | #创建多用途的TF监听器 121 | self.tf_listener = tf.TransformListener() 122 | #变换关系正确读取的标志位 123 | get_transform=False 124 | #等待并获取抓取父坐标系与机械臂基座之间的tf变换关系 125 | while not get_transform: 126 | try: 127 | if mode=='test': 128 | get_transform = True 129 | rospy.loginfo("==================Test mode====================") 130 | else: 131 | self.tf_listener.waitForTransform(grasp_father_frame,base_frame, rospy.Time(), rospy.Duration(5.0)) 132 | #抓取父坐标系相对于机械臂基座坐标系的位姿 133 | self.BTC_trans, self.BTC_quater = self.tf_listener.lookupTransform(base_frame, grasp_father_frame, rospy.Time(0)) 134 | #将trans转换成为ndarry 135 | self.BTC_trans=np.array(self.BTC_trans) 136 | #self.BTC_quater= np.array(self.BTC_quater) 137 | self.BTC_rot = quaternion_matrix(self.BTC_quater) 138 | #截取为3*3 139 | self.BTC_rot = self.BTC_rot[0:3,0:3] 140 | get_transform = True 141 | rospy.loginfo("got transform complete") 142 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 143 | raise SystemError("got transform failed") 144 | 145 | 146 | 147 | # 初始化场景对象 148 | self.scene = moveit_commander.PlanningSceneInterface() 149 | #为场景添加桌子，防止机械臂碰撞桌面 150 | self.add_table() 151 | rospy.sleep(2) 152 | # 创建机械臂规划组对象 153 | self.robot_arm = moveit_commander.MoveGroupCommander(move_group_name) 154 | #创建机械手规划对象 155 | #self.panda_hand=moveit_commander.MoveGroupCommander('hand') 156 | # 157 | self.robot_arm.set_max_acceleration_scaling_factor(self.max_acceleration_scaling_factor) 158 | self.robot_arm.set_max_velocity_scaling_factor(self.max_velocity_scaling_factor) 159 | #通过此发布器发布规划的轨迹 160 | display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', 161 | DisplayTrajectory, 162 | queue_size=20) 163 | # 获取末端执行器名称 164 | self.end_effector_link = self.robot_arm.get_end_effector_link() 165 | rospy.loginfo("End effector detected {}".format(self.end_effector_link)) 166 | 167 | # 设置允许机械臂末位姿的错误余量 168 | self.robot_arm.set_goal_position_tolerance(0.01)#1cm 169 | self.robot_arm.set_goal_orientation_tolerance(0.05)# 170 | 171 | #不允许规划失败重规划,规划时间只允许5秒钟,否则很浪费时间 172 | self.robot_arm.allow_replanning(False) 173 | self.robot_arm.set_planning_time(5) 174 | 175 | #移动到工作姿态 176 | self.move_to_joints(self.robot_arm,self.working_joints,tag="working pose") 177 | #张开夹爪 178 | #self.set_gripper(0.078,epsilon=0.01)#张开8cm 179 | rospy.set_param("/robot_state", "ready") 180 | rospy.loginfo("Ready to grasp, working pose") 181 | 182 | ######################开始等待接收夹爪姿态######################### 183 | rospy.loginfo("Waiting for gripper pose") 184 | self.callback_done=False 185 | #刚开始认为抓取数量为0 186 | self.No_grasp = True 187 | 188 | if mode=='test':#测试模式 189 | #pass 190 | self.grasp_test() 191 | else: 192 | rospy.Subscriber(grasp_list_topic, GraspConfigList, self.Callback,queue_size=1) 193 | 194 | #######################执行抓取#################################### 195 | while not rospy.is_shutdown(): 196 | 197 | #判断回调函数是否处理完 198 | if self.callback_done: 199 | self.callback_done=False 200 | #处理完发现抓取数量为0 201 | if self.No_grasp: 202 | rospy.sleep(1) 203 | rospy.loginfo("Get No Grasp !") 204 | continue 205 | else: 206 | #如果抓取数量不是0 207 | self.No_grasp =True 208 | else: 209 | rospy.sleep(0.5) 210 | continue 211 | 212 | 213 | 214 | #移动至预抓取姿态 215 | rospy.set_param("/robot_state", "moving") 216 | rospy.loginfo('Move to pre_grasp pose') 217 | self.robot_arm.set_start_state_to_current_state() #以当前姿态作为规划起始点 218 | success=self.robot_arm.go(self.pre_grasp_pose_wrist3,wait=True) 219 | self.robot_arm.stop() 220 | self.robot_arm.clear_pose_targets() 221 | 222 | if not success: 223 | rospy.loginfo('Failed to move to pre_grasp pose!') 224 | rospy.sleep(1) 225 | rospy.set_param("/robot_state", "ready") 226 | continue 227 | 228 | rospy.loginfo('Succeed') 229 | 230 | rospy.sleep(1)#等待机械臂稳定 231 | 232 | 233 | #再设置当前姿态为起始姿态 234 | self.robot_arm.set_start_state_to_current_state() 235 | # 236 | waypoints = [] 237 | wpose=self.robot_arm.get_current_pose().pose 238 | wpose.position.x= self.grasp_pose_wrist3.position.x 239 | wpose.position.y= self.grasp_pose_wrist3.position.y 240 | wpose.position.z= self.grasp_pose_wrist3.position.z 241 | waypoints.append(copy.deepcopy(wpose)) 242 | 243 | #规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径 244 | (plan, fraction) = self.robot_arm.compute_cartesian_path( 245 | waypoints, # waypoints to follow 246 | 0.01, # eef_step 247 | 0.0) # jump_threshold 248 | ##显示轨迹 249 | display_trajectory = DisplayTrajectory() 250 | display_trajectory.trajectory_start = self.robot_arm.get_current_state() 251 | display_trajectory.trajectory.append(plan) 252 | display_trajectory_publisher.publish(display_trajectory) 253 | 254 | #执行,并等待这个轨迹执行成功 255 | new_plan=self.scale_trajectory_speed(plan,0.2) 256 | self.robot_arm.execute(new_plan,wait=True) 257 | 258 | #执行抓取 259 | rospy.loginfo("Start to grasp") 260 | #self.set_gripper(0.01,epsilon=0.4)#张开3cm 261 | rospy.sleep(1) 262 | 263 | ####################返回到预备抓取状态#################### 264 | waypoints = [] 265 | wpose=self.robot_arm.get_current_pose().pose 266 | wpose.position.x= self.pre_grasp_pose_wrist3.position.x 267 | wpose.position.y= self.pre_grasp_pose_wrist3.position.y 268 | wpose.position.z= self.pre_grasp_pose_wrist3.position.z 269 | waypoints.append(copy.deepcopy(wpose)) 270 | 271 | #规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径 272 | (plan, fraction) = self.robot_arm.compute_cartesian_path( 273 | waypoints, # waypoints to follow 274 | 0.01, # eef_step 275 | 0.0) # jump_threshold 276 | #显示轨迹 277 | display_trajectory = DisplayTrajectory() 278 | display_trajectory.trajectory_start = self.robot_arm.get_current_state() 279 | display_trajectory.trajectory.append(plan) 280 | display_trajectory_publisher.publish(display_trajectory) 281 | #执行,并等待后撤成功 282 | new_plan=self.scale_trajectory_speed(plan,0.6) 283 | self.robot_arm.execute(new_plan,wait=True) 284 | ######################移动到工作状态############################ 285 | self.move_to_joints(self.robot_arm,self.working_joints,tag="working pose") 286 | #self.set_gripper(0.08,epsilon=0.4)#张开8cm 287 | rospy.loginfo("Working pose") 288 | 289 | #########################移动放到放置状态####################### 290 | self.move_to_joints(self.robot_arm,self.place_joints,tag="place pose") 291 | #self.set_gripper(0.08,epsilon=0.4)#张开8cm 292 | rospy.loginfo("place pose") 293 | rospy.sleep(1) 294 | ######################移动到工作状态############################ 295 | self.move_to_joints(self.robot_arm,self.working_joints,tag="working pose") 296 | #self.set_gripper(0.08,epsilon=0.4)#张开8cm 297 | rospy.set_param("/robot_state", "ready") 298 | rospy.loginfo("Ready to grasp, working pose") 299 | rospy.sleep(2) 300 | 301 | if mode=='test':#测试模式 302 | self.callback_done=True 303 | self.No_grasp = False 304 | rospy.set_param("/robot_state", "moving") 305 | self.move_to_joints(self.robot_arm,self.working_joints,tag="working pose") 306 | rospy.set_param("/robot_state", "ready") 307 | 308 | 309 | # Shut down MoveIt cleanly 310 | moveit_commander.roscpp_shutdown() 311 | 312 | # Exit MoveIt 313 | moveit_commander.os._exit(0) 314 | 315 | def lookupTransform(self,tf_listener, target, source): 316 | tf_listener.waitForTransform(target, source, rospy.Time(), rospy.Duration(4.0)) #等待时间为4秒 317 | 318 | trans, rot = tf_listener.lookupTransform(target, source, rospy.Time()) 319 | euler = tf.transformations.euler_from_quaternion(rot) 320 | 321 | source_target = tf.transformations.compose_matrix(translate = trans, angles = euler) 322 | return source_target 323 | def getTfFromMatrix(self,matrix): 324 | scale, shear, angles, trans, persp = tf.transformations.decompose_matrix(matrix) 325 | return trans, tf.transformations.quaternion_from_euler(*angles), angles 326 | 327 | 328 | def quater_multi_vec(self,quater,vec): 329 | quater_=tf.transformations.quaternion_inverse(quater) 330 | vec_quater=np.c_[vec,[0]] 331 | temp=quaternion_multiply(quater,vec_quater) 332 | temp=quaternion_multiply(temp,quater_) 333 | return temp[:3] 334 | 335 | def move_to_joints(self,group,joints,tag="initial pose"): 336 | #先从Initial 移动到HOME 337 | case,plan = self.planJointGoal(group,joints)#返回真 就是找到轨迹 338 | if case==2: 339 | rospy.loginfo("Move to {}".format(tag)) 340 | group.execute(plan,wait=True) 341 | elif case==1: 342 | rospy.loginfo("Already at {}".format(tag)) 343 | 344 | else: 345 | raise SystemError("Home pose trajectory not found") 346 | 347 | def planJointGoal(self,movegroup,joint_goal,lable='Next'): 348 | current_joint = movegroup.get_current_joint_values() 349 | dis_pose =np.linalg.norm(np.array(joint_goal)-np.array(current_joint)) 350 | #print(current_joint) 351 | #print(joint_goal) 352 | if dis_pose<0.008: 353 | return 1,None #已经到位 354 | else: 355 | movegroup.set_joint_value_target(joint_goal) 356 | plan = movegroup.plan() 357 | if not plan.joint_trajectory.points: 358 | return 0,plan 359 | else:#执行规划 360 | return 2,plan 361 | 362 | 363 | def Callback(self,grasp_config_list): 364 | """ 365 | 分别计算: 366 | 机械臂末端link坐标系相对于base_link坐标系下的预抓取姿态 BTLp 367 | 机械臂末端link坐标系相对于base_link坐标系下的最终抓取姿态 BTLg 368 | """ 369 | 370 | #如果机械臂正在移动，不再对接收到的抓取进行处理 371 | if rospy.get_param("/robot_state")=='moving': 372 | #标志回调函数处理完毕 373 | self.callback_done=True 374 | return 0 375 | #如果处于预备状态，判断话题中合法抓取数量 376 | if len(grasp_config_list.grasps)==0: 377 | self.callback_done=True 378 | return 0 379 | else: 380 | #话题中给定抓取数量不为0 381 | self.No_grasp =False 382 | rospy.loginfo('Get grasp num {}'.format(len(grasp_config_list.grasps))) 383 | 384 | 385 | #认为抓取列表中的第一个抓取是最优抓取，把它读出来 386 | best_grasp_config = grasp_config_list.grasps[0] 387 | #将最优抓取构建为矩阵形式，即CTG 388 | CTG_trans = np.array([best_grasp_config.position.x, 389 | best_grasp_config.position.y, best_grasp_config.position.z]) 390 | 391 | CTG_approach = np.array([best_grasp_config.approach.x, 392 | best_grasp_config.approach.y,best_grasp_config.approach.z]) 393 | CTG_binormal = np.array([best_grasp_config.binormal.x, 394 | best_grasp_config.binormal.y,best_grasp_config.binormal.z]) 395 | CTG_axis = np.array([best_grasp_config.axis.x, 396 | best_grasp_config.axis.y,best_grasp_config.axis.z]) 397 | 398 | CTG_rot = np.c_[CTG_approach,np.c_[CTG_binormal,CTG_axis]].reshape(3,3).swapaxes(0,1) 399 | 400 | 401 | 402 | #计算得到抓取坐标系G在基座坐标系B中的位姿 403 | BTG_rot = self.BTC_rot.dot(CTG_rot) 404 | BTG_trans = self.BTC_rot.dot(CTG_trans)+self.BTC_trans 405 | 406 | #读取 夹爪**指尖**固连坐标系E与抓取坐标系G之间的关系 GTEg 407 | GTEg_trans = self.GTEg.translation # 408 | GTEg_rot = self.GTEg.rotation #3*3 409 | 410 | #抓取情形下，夹爪指尖坐标系E 相对于基座的位姿 411 | BTEg_rot = BTG_rot.dot(GTEg_rot) 412 | BTEg_trans = BTG_rot.dot(GTEg_trans)+BTG_trans 413 | BTEg_rot_ = np.eye(4) 414 | BTEg_rot_[:3,:3] = BTEg_rot 415 | #用于显示 416 | BTEg_quater = quaternion_from_matrix(BTEg_rot_) 417 | 418 | #预备抓取情形下，夹爪指尖坐标系E 相对于基座的位姿 419 | BTEp_rot = BTEg_rot 420 | BTEp_trans = BTEg_trans - BTEg_rot[:,2]*self.retreat #[3,] 421 | #用于显示 422 | BTEp_quater = BTEg_quater 423 | 424 | #LTE 夹爪指尖固连坐标系与机械臂末端Link的变换关系 425 | LTE_trans=self.LTE.translation#偏移 426 | LTE_rot = self.LTE.rotation#[3,3] 427 | #ETL 428 | ETL_rot = LTE_rot.T 429 | ETL_trans = - ETL_rot.dot(LTE_trans) 430 | 431 | 432 | #抓取情形下，末端连杆坐标系L 相对基座坐标系的位姿 433 | BTLg_rot = BTEg_rot.dot(ETL_rot) 434 | BTLg_trans = BTEg_rot.dot(ETL_trans)+BTEg_trans 435 | 436 | #预备抓取情形下，末端连杆坐标系L 相对基座坐标系的位姿 437 | BTLp_rot = BTEp_rot.dot(ETL_rot) 438 | BTLp_trans = BTEp_rot.dot(ETL_trans)+BTLp_trans 439 | BTLg_rot_ = np.eye(4) 440 | BTLg_rot_[:3,:3] = BTLg_rot 441 | BTLg_quater=quaternion_from_matrix(BTLg_rot_) 442 | 443 | 444 | #BTLg 转换为GraspConfig形式 445 | self.grasp_pose_wrist3.position.x = BTLg_trans[0] 446 | self.grasp_pose_wrist3.position.y = BTLg_trans[1] 447 | self.grasp_pose_wrist3.position.z = BTLg_trans[2] 448 | self.grasp_pose_wrist3.orientation.x = BTLg_quater[0] 449 | self.grasp_pose_wrist3.orientation.y = BTLg_quater[1] 450 | self.grasp_pose_wrist3.orientation.z = BTLg_quater[2] 451 | self.grasp_pose_wrist3.orientation.w = BTLg_quater[3] 452 | 453 | #BTLp 拷贝 BTLg 仅仅修改位置参数即可 454 | self.pre_grasp_pose_wrist3 = copy.deepcopy(self.grasp_pose_wrist3) 455 | self.pre_grasp_pose_wrist3.position.x = BTLp_trans[0] 456 | self.pre_grasp_pose_wrist3.position.y = BTLp_trans[1] 457 | self.pre_grasp_pose_wrist3.position.z = BTLp_trans[2] 458 | 459 | #发布BTEg 最终抓取状态时，AG95相对于基座的位姿 460 | self.tf_broadcaster.sendTransform( 461 | BTEg_trans, 462 | BTEg_quater, 463 | rospy.Time.now(), 464 | "BTEg", 465 | "base_link") 466 | #发布BTEp 预抓取状态时，AG95相对于基座的位姿 467 | self.tf_broadcaster.sendTransform( 468 | BTEp_trans, 469 | BTEp_quater, #与抓取情形下相同 470 | rospy.Time.now(), 471 | "BTEp", 472 | "base_link") 473 | 474 | #标志回调函数处理完毕 475 | self.callback_done=True 476 | 477 | def grasp_test(self): 478 | """ 479 | 给定AG95坐标系在base坐标系下的最终抓取姿态 BTEg 480 | 分别计算: 481 | wrist_3_link坐标系相对于base_link坐标系下的预抓取姿态 BTLp 482 | wrist_3_link坐标系相对于base_link坐标系下的最终抓取姿态 BTLg 483 | """ 484 | 485 | #BTEg 最终抓取状态时，AG95相对于基座的位姿 486 | BTEg_trans =np.array([0.58166,0.08053,0.11798]) 487 | BTEg_quater=np.array([-0.69264,0.72096,-0.0053162,-0.020851])#将姿态转换为四元数形式 488 | BTEg_rot = quaternion_matrix(BTEg_quater) 489 | BTEg_rot = BTEg_rot[:3,:3] 490 | 491 | 492 | #LTE 夹爪指尖固连坐标系与机械臂末端Link的变换关系 493 | LTE_trans=self.LTE.translation#偏移 494 | LTE_rot = self.LTE.rotation#[3,3] 495 | 496 | #BTEp 预抓取状态时，AG95相对于基座的位姿 497 | BTEp_trans = BTEg_trans - BTEg_rot[:,2]*self.retreat #[3,] 498 | BTEp_rot = BTEg_rot 499 | 500 | #BTLg 最终抓取状态时，机械臂末端link相对于基座的位姿 501 | BTLg_rot = BTEg_rot.dot(LTE_rot.T)#姿态[3,3] 502 | BTLg_trans = BTEg_trans - BTLg_rot.dot(LTE_trans)#位置 503 | BTLg_rot_ = np.eye(4) 504 | BTLg_rot_[:3,:3] = BTLg_rot 505 | BTLg_quater=quaternion_from_matrix(BTLg_rot_) 506 | 507 | self.grasp_pose_wrist3.position.x = BTLg_trans[0] 508 | self.grasp_pose_wrist3.position.y = BTLg_trans[1] 509 | self.grasp_pose_wrist3.position.z = BTLg_trans[2] 510 | self.grasp_pose_wrist3.orientation.x = BTLg_quater[0] 511 | self.grasp_pose_wrist3.orientation.y = BTLg_quater[1] 512 | self.grasp_pose_wrist3.orientation.z = BTLg_quater[2] 513 | self.grasp_pose_wrist3.orientation.w = BTLg_quater[3] 514 | 515 | #BTLp 预抓取状态时，机械臂末端link相对于基座的位姿 516 | self.pre_grasp_pose_wrist3 = copy.deepcopy(self.grasp_pose_wrist3) 517 | 518 | BTLp_rot = BTLg_rot 519 | BTLp_trans = BTEp_trans - BTLp_rot.dot(LTE_trans) 520 | self.pre_grasp_pose_wrist3.position.x = BTLp_trans[0] 521 | self.pre_grasp_pose_wrist3.position.y = BTLp_trans[1] 522 | self.pre_grasp_pose_wrist3.position.z = BTLp_trans[2] 523 | 524 | 525 | #发布BTEg 最终抓取状态时，AG95相对于基座的位姿 526 | self.tf_broadcaster.sendTransform( 527 | BTEg_trans, 528 | BTEg_quater, 529 | rospy.Time.now(), 530 | "BTEg", 531 | "base_link") 532 | #发布BTEp 预抓取状态时，AG95相对于基座的位姿 533 | self.tf_broadcaster.sendTransform( 534 | BTEp_trans, 535 | BTEg_quater, #与抓取情况下相同 536 | rospy.Time.now(), 537 | "BTEp", 538 | "base_link") 539 | 540 | #标志回调函数处理完毕 541 | self.callback_done=True 542 | self.No_grasp = False 543 | 544 | 545 | def scale_trajectory_speed(self,traj,spd=0.1): 546 | new_traj = RobotTrajectory() 547 | new_traj = traj 548 | 549 | n_joints = len(traj.joint_trajectory.joint_names) 550 | n_points = len(traj.joint_trajectory.points) 551 | 552 | #spd = 3.0 553 | 554 | points = list(traj.joint_trajectory.points) 555 | 556 | for i in range(n_points): 557 | point = JointTrajectoryPoint() 558 | point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd 559 | point.velocities = list(traj.joint_trajectory.points[i].velocities) 560 | point.accelerations = list(traj.joint_trajectory.points[i].accelerations) 561 | point.positions = traj.joint_trajectory.points[i].positions 562 | 563 | for j in range(n_joints): 564 | point.velocities[j] = point.velocities[j] * spd 565 | point.accelerations[j] = point.accelerations[j] * spd 566 | 567 | points[i] = point 568 | 569 | new_traj.joint_trajectory.points = points 570 | return new_traj 571 | 572 | def add_table(self): 573 | """为场景中添加抓取桌面，防止机械臂与桌子发生碰撞 574 | """ 575 | #清除场景可能存在的遗留物体 576 | self.scene.remove_world_object('table') 577 | #设置桌面尺寸 x y z 578 | table_size = [0.6, 1.2, 0.01] 579 | #设置桌子的位置姿态 580 | table_pose = PoseStamped() 581 | table_pose.header.frame_id = 'panda_link0' 582 | table_pose.pose.position.x = 0.55 583 | table_pose.pose.position.y = 0.0 584 | table_pose.pose.position.z = 0.025 585 | table_pose.pose.orientation.w = 1.0 586 | # 将table加入场景当中 587 | self.scene.add_box('table', table_pose, table_size) 588 | 589 | def set_gripper(self,gripper_width,epsilon=0.0): 590 | """设置panda 夹爪的开合大小 591 | gripper_width 最大0.08m 592 | """ 593 | if gripper_width>0.08 or gripper_width<0.0: 594 | raise Exception 595 | #帮助维持夹爪力度 596 | grasp_epsilon = GraspEpsilon(epsilon,epsilon) 597 | goal = GraspGoal(width = gripper_width, speed = 0.08,epsilon=grasp_epsilon ,force=5.0) 598 | self.gripper_client.send_goal(goal ) 599 | self.gripper_client.wait_for_result(rospy.Duration.from_sec(5.0)) 600 | 601 | rospy.loginfo("Gripper action completed") 602 | 603 | 604 | 605 | if __name__ == "__main__": 606 | try: 607 | GoGrasp() 608 | rospy.spin() 609 | except rospy.ROSInterruptException: 610 | rospy.loginfo("Arm tracker node terminated.") 611 | 612 | 613 | --------------------------------------------------------------------------------