├── .env ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.assets ├── image-20210323222536538.png ├── image-20210323222712987.png ├── image-20210323222920761.png └── image-20210323223249480.png ├── README.md ├── launch ├── check_and_grasp.launch ├── go_grasp.launch └── state_checker.launch ├── package.xml └── scripts ├── get_panda_state.py └── panda_grasp.py /.env: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/panda_go_grasp/57b597bf99d5ebb28aba09130d07648cb8b9c9f9/.env -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(panda_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/get_panda_state.py 163 | scripts/panda_grasp.py 164 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_panda_go_grasp.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /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/panda_go_grasp/57b597bf99d5ebb28aba09130d07648cb8b9c9f9/README.assets/image-20210323222536538.png -------------------------------------------------------------------------------- /README.assets/image-20210323222712987.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/panda_go_grasp/57b597bf99d5ebb28aba09130d07648cb8b9c9f9/README.assets/image-20210323222712987.png -------------------------------------------------------------------------------- /README.assets/image-20210323222920761.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/panda_go_grasp/57b597bf99d5ebb28aba09130d07648cb8b9c9f9/README.assets/image-20210323222920761.png -------------------------------------------------------------------------------- /README.assets/image-20210323223249480.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hymwgk/panda_go_grasp/57b597bf99d5ebb28aba09130d07648cb8b9c9f9/README.assets/image-20210323223249480.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # panda_go_grasp 2 | 使用panda机械臂接收grasp pose，执行抓取和一些其他操作 3 | 4 | 接收以**桌面参考系为父坐标系**的GraspConfigList类型数据 5 | 6 | 读取列表的第一个GraspConfig类型数据（最优grasp config） 7 | 8 | 9 | 10 | ## 使用 11 | 12 | 1. 解锁机械臂 13 | 14 | ```bash 15 | cd ~/catkin_ws #where your "panda_client.sh" is 16 | source panda_client.sh -r 17 | ``` 18 | 19 | 2. 启动moveit 20 | 21 | ```bash 22 | roslaunch panda_moveit_config moveit_rviz.launch 23 | ``` 24 | 25 | 3. 未完待续 26 | 27 | 28 | 29 | 关于末端几个坐标系位置关系如下图所示： 30 | 31 | image-20210323222536538image-20210323222712987image-20210323222920761image-20210323223249480 32 | 33 | ## 测试模式 34 | 35 | 测试模式中（命令行参数`--test`），机械臂将会执行以下操作： 36 | 37 | > 1. 移动到设定好的初始姿态 38 | > 1. 移动到预备抓取姿态 39 | > 3. 沿着approach轴移动到抓取姿态，并合并夹爪 40 | > 4. 返回预备抓取姿态 41 | > 5. 移动到设定好的放置姿态，并打开夹爪 42 | > 6. 循环执行2到5步骤 43 | 人为给定的抓取姿态是坐标系`panda_EE`(典范抓取坐标系形式)相对于`panda_link0`的位姿，记为$^BT_E$，由于使用`movegroup.go()`命令给规划组`panda_arm`设置的目标姿态，是坐标系`panda_link8`相对于坐标系`panda_link0`的位姿$^BT_L$，而不是$^BT_E$，因此需要分别计算出两个状态下的夹爪位姿数据： 44 | 45 | - 预备抓取状态，坐标系`panda_link8`相对于坐标系`panda_link0`的位姿，记为$^BT_{L_p}$（脚标$p$代表预备抓取状态） 46 | - 最终抓取状态下，坐标系`panda_link8`相对于坐标系`panda_link0`的位姿，记为$^BT_{L_g}$（脚标代表最终抓取状态） 47 | 48 | 其中，预备抓取状态，仅仅是保持抓取姿态的同时，沿抓取姿态的approach轴，向负方向后撤15cm。 49 | 50 | 计算过程为，重记给定的抓取姿态为$^BT_{E_g}$，对应的抓取预备姿态为$^BT_{E_p}$，则有 51 | $$ 52 | ^BT_{L_g}=^BT_{Eg}\times^{E_g}T_{L_g}\\ 53 | ^BT_{L_p}=^BT_{Eg}\times^{E_g}T_{E_p}\times^{E_p}T_{L_p} 54 | $$ 55 | 其中，$^{E_g}T_{E_p}$代表预备抓取坐标系相对于最终抓取坐标系的位姿（代表了后撤的变换关系），且$^{E_g}T_{L_g}=^{E_p}T_{L_p}=^ET_L$，是固定机械臂参数，代表`panda_link8`坐标系相对于`panda_EE`的位姿。 56 | 57 | 58 | 59 | ## 抓取模式 60 | 61 | -------------------------------------------------------------------------------- /launch/check_and_grasp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/go_grasp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/state_checker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_go_grasp 4 | 0.0.0 5 | The panda_go_grasp package 6 | 7 | 8 | 9 | 10 | wgk 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/get_panda_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # Author : Hongzhuo Liang 4 | # E-mail : liang@informatik.uni-hamburg.de 5 | # Description: 主要是为了获取机械臂当前的状态，通过ros实时获取夹爪当前的姿态，看夹爪是否位于home状态 6 | #如果使用panda等机器人，就需要直接在shell中运行这个程序，不能在anconda中运行，其实也不是不可以，但是每个都要提前运行一个 7 | #panda_clinet.sh脚本 8 | # Date : 08/09/2018 12:00 AM 9 | # File Name : get_ur5_robot_state.py 10 | #python 2 11 | 12 | import rospy 13 | import numpy as np 14 | import moveit_commander 15 | import sys 16 | 17 | def get_robot_state_moveit(): 18 | # moveit_commander.roscpp_initialize(sys.argv) 19 | # robot = moveit_commander.RobotCommander() 20 | try: 21 | #获取当前机械臂的每个关节的角度，构建成为一个ndarry 22 | current_joint_values = np.array(group.get_current_joint_values()) 23 | #计算每个关节的角度的所有的距离差值的绝对值之和 24 | diff = abs(current_joint_values - home_joint_values)*180/np.pi 25 | #要和关节数量相同，共7个关节 26 | if np.sum(diff<1) ==7: # if current joint - home position < 1 degree, we think it is at home 27 | return 1 # robot at home 28 | else: 29 | print(np.sum(diff<1)) 30 | return 2 # robot is moving 31 | 32 | except: 33 | return 3 # robot state unknow 34 | rospy.loginfo("Get robot state failed") 35 | 36 | if __name__ == '__main__': 37 | 38 | #joint_state_topic = ['joint_states:=/robot/joint_states'] 39 | rospy.init_node('panda_state_checker', anonymous=True) 40 | rate = rospy.Rate(10) 41 | moveit_commander.roscpp_initialize(sys.argv) 42 | #想要使用moveit，就必须要提前上传ros关于机械臂的参数等等 43 | group = moveit_commander.MoveGroupCommander("panda_arm") 44 | home_joint_values = np.array([0.0399, -0.6999, 0.1798, -2.8000, 0.1897, 2.1302, 0.9199]) 45 | while not rospy.is_shutdown(): 46 | #获取机械臂当前状态 47 | at_home = get_robot_state_moveit() 48 | print(group.get_current_joint_values()) 49 | if at_home == 1: 50 | rospy.set_param("/robot_at_home", "true") 51 | rospy.loginfo("robot at home") 52 | elif at_home == 2: 53 | rospy.set_param("/robot_at_home", "false") 54 | rospy.loginfo("robot is moving") 55 | elif at_home == 3: 56 | rospy.loginfo("robot state unknow") 57 | rate.sleep() 58 | -------------------------------------------------------------------------------- /scripts/panda_grasp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | """ 4 | moveit_ik_demo.py - Version 0.1 2014-01-14 5 | 使得机械臂，先在初始状态，然后移动一下机械臂，然后再回到初始状态，停止 6 | Use inverse kinemtatics to move the end effector to a specified pose 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyleft (c) 2014 Patrick Goebel. All lefts reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | """ 23 | 24 | import rospy, sys 25 | import moveit_commander 26 | import tf 27 | import argparse 28 | import math 29 | import numpy as np 30 | from math import pi 31 | import time 32 | import copy 33 | from moveit_msgs.msg import RobotTrajectory,DisplayTrajectory 34 | from trajectory_msgs.msg import JointTrajectoryPoint 35 | 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 | try: 41 | from gpd_grasp_msgs.msg import GraspConfig,GraspConfigList 42 | except ImportError: 43 | print("Please install grasp msgs from https://github.com/TAMS-Group/gpd_grasp_msgs in your ROS workspace") 44 | exit() 45 | 46 | #解析命令行参数 47 | parser = argparse.ArgumentParser(description='Panda go grasp') 48 | parser.add_argument('--test',action='store_true',default=True) #设置同时处理几个场景 49 | parameters = parser.parse_args() 50 | 51 | 52 | class MoveItDemo: 53 | def __init__(self): 54 | #初始化moveit的 API接口 55 | moveit_commander.roscpp_initialize(sys.argv) 56 | #初始化ros节点 名为panda_grasp 57 | rospy.init_node('panda_grasp', anonymous=True) 58 | rospy.set_param("/robot_state", "Initializing") 59 | rospy.loginfo("Robot initializing") 60 | 61 | #构建tf发布器 62 | self.tf_broadcaster=tf.TransformBroadcaster() 63 | 64 | self.grasp_config=GraspConfig() 65 | 66 | #创建多用途的TF监听器 67 | self.tf_listener = tf.TransformListener() 68 | #变换关系正确读取的标志位 69 | get_transform=False 70 | #等待并获取正确的tf变换关系 71 | while not get_transform: 72 | try: 73 | if parameters.test: 74 | get_transform = True 75 | rospy.loginfo("Test mode") 76 | else: 77 | #相机坐标系相对于base坐标系的位姿 78 | self.btc_trans, self.btc_quater = self.tf_listener.lookupTransform('/kinect', '/panda_link0', rospy.Time(0)) 79 | #将trans转换成为ndarry 80 | self.btc_trans=np.array(self.btc_trans) 81 | self.btc_quater= np.array(self.btc_quater) 82 | get_transform = True 83 | rospy.loginfo("got transform complete") 84 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 85 | raise SystemError("got transform failed") 86 | 87 | 88 | 89 | # 初始化场景对象 90 | self.scene = moveit_commander.PlanningSceneInterface() 91 | #为场景添加桌子，防止机械臂碰撞桌面 92 | self.add_table() 93 | rospy.sleep(2) 94 | # 创建机械臂规划组对象 95 | self.panda_arm = moveit_commander.MoveGroupCommander('panda_arm') 96 | #创建机械手规划对象 97 | self.panda_hand=moveit_commander.MoveGroupCommander('hand') 98 | # 99 | self.panda_arm.set_max_acceleration_scaling_factor(0.2) 100 | self.panda_arm.set_max_velocity_scaling_factor(0.5) 101 | #通过此发布器发布规划的轨迹 102 | display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', 103 | DisplayTrajectory, 104 | queue_size=20) 105 | # 获取末端执行器名称 106 | self.end_effector_link = self.panda_arm.get_end_effector_link() 107 | rospy.loginfo("End effector detected {}".format(self.end_effector_link)) 108 | 109 | # 设置允许机械臂末位姿的错误余量 110 | self.panda_arm.set_goal_position_tolerance(0.01)#1cm 111 | self.panda_arm.set_goal_orientation_tolerance(0.05)# 112 | 113 | #不允许规划失败重规划,规划时间只允许5秒钟,否则很浪费时间 114 | self.panda_arm.allow_replanning(False) 115 | self.panda_arm.set_planning_time(5) 116 | 117 | # 设置panda的初始姿态，和预备姿态 118 | self.initial_joints = [0.04, -0.70, 0.18, -2.80, 0.19, 2.13, 0.92] 119 | self.ready_joints = [1.544, -0.755, 0.190, -2.713, 0.149, 2.027, 0.799] 120 | #移动到home 121 | self.move_to_joints(self.panda_arm,self.initial_joints,tag="initial pose") 122 | #张开夹爪 123 | self.set_gripper(0.08)#张开8cm 124 | rospy.set_param("/robot_state", "ready") 125 | rospy.loginfo("Ready to grasp, initial pose") 126 | 127 | ######################开始等待接收夹爪姿态######################### 128 | rospy.loginfo("Waiting for gripper pose") 129 | self.callback_done=False 130 | 131 | if parameters.test:#测试模式 132 | self.grasp_test() 133 | else: 134 | rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, self.Callback,queue_size=1) 135 | 136 | #######################执行抓取#################################### 137 | while not rospy.is_shutdown(): 138 | #等待回调函数处理完 139 | if self.callback_done: 140 | self.callback_done=False 141 | else: 142 | rospy.sleep(0.5) 143 | continue 144 | 145 | 146 | 147 | #移动至预抓取姿态 148 | rospy.set_param("/robot_state", "moving") 149 | rospy.loginfo('Move to pre_grasp pose') 150 | self.panda_arm.set_start_state_to_current_state() #以当前姿态作为规划起始点 151 | success=self.panda_arm.go(self.pre_grasp_pose_link8,wait=True) 152 | self.panda_arm.stop() 153 | self.panda_arm.clear_pose_targets() 154 | 155 | if not success: 156 | raise SystemError('Failed to move to pre_grasp pose!') 157 | rospy.loginfo('Succeed') 158 | 159 | rospy.sleep(1)#等待机械臂稳定 160 | 161 | 162 | #再设置当前姿态为起始姿态 163 | self.panda_arm.set_start_state_to_current_state() 164 | # 165 | waypoints = [] 166 | wpose=self.panda_arm.get_current_pose().pose 167 | wpose.position.x= self.grasp_pose_link8.position.x 168 | wpose.position.y= self.grasp_pose_link8.position.y 169 | wpose.position.z= self.grasp_pose_link8.position.z 170 | waypoints.append(copy.deepcopy(wpose)) 171 | 172 | #规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径 173 | (plan, fraction) = self.panda_arm.compute_cartesian_path( 174 | waypoints, # waypoints to follow 175 | 0.01, # eef_step 176 | 0.0) # jump_threshold 177 | ##显示轨迹 178 | display_trajectory = DisplayTrajectory() 179 | display_trajectory.trajectory_start = self.panda_arm.get_current_state() 180 | display_trajectory.trajectory.append(plan) 181 | display_trajectory_publisher.publish(display_trajectory) 182 | 183 | #执行,并等待这个轨迹执行成功 184 | new_plan=self.scale_trajectory_speed(plan,0.3) 185 | self.panda_arm.execute(new_plan,wait=True) 186 | 187 | #执行抓取 188 | rospy.loginfo("Start to grasp") 189 | self.set_gripper(0.03)#张开3cm 190 | rospy.sleep(1) 191 | 192 | ####################抓取完后撤#################### 193 | waypoints = [] 194 | wpose=self.panda_arm.get_current_pose().pose 195 | wpose.position.x= self.pre_grasp_pose_link8.position.x 196 | wpose.position.y= self.pre_grasp_pose_link8.position.y 197 | wpose.position.z= self.pre_grasp_pose_link8.position.z 198 | waypoints.append(copy.deepcopy(wpose)) 199 | 200 | #规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径 201 | (plan, fraction) = self.panda_arm.compute_cartesian_path( 202 | waypoints, # waypoints to follow 203 | 0.01, # eef_step 204 | 0.0) # jump_threshold 205 | #显示轨迹 206 | display_trajectory = DisplayTrajectory() 207 | display_trajectory.trajectory_start = self.panda_arm.get_current_state() 208 | display_trajectory.trajectory.append(plan) 209 | display_trajectory_publisher.publish(display_trajectory) 210 | #执行,并等待后撤成功 211 | new_plan=self.scale_trajectory_speed(plan,0.6) 212 | self.panda_arm.execute(new_plan,wait=True) 213 | 214 | ######################移动到预备姿态############################ 215 | self.move_to_joints(self.panda_arm,self.ready_joints,tag="ready pose") 216 | self.set_gripper(0.08)#张开8cm 217 | rospy.set_param("/robot_state", "ready") 218 | rospy.loginfo("Ready to grasp, ready pose") 219 | rospy.sleep(2) 220 | if parameters.test:#测试模式 221 | self.callback_done=True 222 | rospy.set_param("/robot_state", "moving") 223 | self.move_to_joints(self.panda_arm,self.initial_joints) 224 | rospy.set_param("/robot_state", "ready") 225 | 226 | 227 | # Shut down MoveIt cleanly 228 | moveit_commander.roscpp_shutdown() 229 | 230 | # Exit MoveIt 231 | moveit_commander.os._exit(0) 232 | 233 | def lookupTransform(self,tf_listener, target, source): 234 | tf_listener.waitForTransform(target, source, rospy.Time(), rospy.Duration(4.0)) #等待时间为4秒 235 | 236 | trans, rot = tf_listener.lookupTransform(target, source, rospy.Time()) 237 | euler = tf.transformations.euler_from_quaternion(rot) 238 | 239 | source_target = tf.transformations.compose_matrix(translate = trans, angles = euler) 240 | return source_target 241 | def getTfFromMatrix(self,matrix): 242 | scale, shear, angles, trans, persp = tf.transformations.decompose_matrix(matrix) 243 | return trans, tf.transformations.quaternion_from_euler(*angles), angles 244 | 245 | 246 | def quater_multi_vec(self,quater,vec): 247 | quater_=tf.transformations.quaternion_inverse(quater) 248 | vec_quater=np.c_[vec,[0]] 249 | temp=quaternion_multiply(quater,vec_quater) 250 | temp=quaternion_multiply(temp,quater_) 251 | return temp[:3] 252 | 253 | def move_to_joints(self,group,joints,tag="initial pose"): 254 | #先从Initial 移动到HOME 255 | case,plan = self.planJointGoal(group,joints)#返回真 就是找到轨迹 256 | if case==2: 257 | rospy.loginfo("Move to {}".format(tag)) 258 | group.execute(plan,wait=True) 259 | elif case==1: 260 | rospy.loginfo("Already at {}".format(tag)) 261 | 262 | else: 263 | raise SystemError("Home pose trajectory not found") 264 | 265 | def planJointGoal(self,movegroup,joint_goal,lable='Next'): 266 | current_joint = movegroup.get_current_joint_values() 267 | dis_pose =np.linalg.norm(np.array(joint_goal)-np.array(current_joint)) 268 | #print(current_joint) 269 | #print(joint_goal) 270 | if dis_pose<0.008: 271 | return 1,None #已经到位 272 | else: 273 | movegroup.set_joint_value_target(joint_goal) 274 | plan = movegroup.plan() 275 | if not plan.joint_trajectory.points: 276 | return 0,plan 277 | else:#执行规划 278 | return 2,plan 279 | 280 | 281 | def Callback(self,data): 282 | """根据接收的夹爪抓取姿态，计算预抓取夹爪的位置姿态 283 | 接收的抓取默认以相机坐标系为参考系 284 | 使用的抓取坐标系为典范抓取坐标系(抓取中心点位于两指中心) 285 | """ 286 | 287 | #data是GraspConfigList,data.grasps是GraspConfig[]类型, 288 | #data.grasps[0]是list中第一个GraspConfig类型的数据，代表的最优的那个抓取配置 289 | self.grasp_config=data.grasps[0] 290 | #最终抓取姿态 291 | self.grasp_pose_link8=Pose() 292 | #预抓取姿态 293 | self.pre_grasp_pose_link8=Pose() 294 | 295 | #以下是读取grasp的pose，需要注意的是，此时pose的参考系是谁？是桌面标签参考坐标系，并不是panda_link0 296 | #读取grasp pose的三个方向向量，转换为ndarray形式 297 | approach=np.array([self.grasp_config.approach.x,\ 298 | self.grasp_config.approach.y,self.grasp_config.approach.z])#接近轴 299 | binormal=np.array([self.grasp_config.binormal.x,\ 300 | self.grasp_config.binormal.y,self.grasp_config.binormal.z])#合并轴 301 | axis=np.array([self.grasp_config.axis.x,\ 302 | self.grasp_config.axis.y,self.grasp_config.axis.z])# 303 | #进行方向向量归一化 304 | approach=approach/np.linalg.norm(approach) 305 | binormal=binormal/np.linalg.norm(binormal) 306 | axis=axis/np.linalg.norm(axis) 307 | #读取典范抓取坐标系原点在参考系中的坐标（默认参考系为相机坐标系） 308 | ctg_trans=np.array([self.grasp_config.top.x,self.grasp_config.top.y,self.grasp_config.top.z])#[3,] 309 | ctg_rot=np.hstack([approach,binormal,axis]).reshape(3,3).T #[3,3] 310 | #设置后撤距离(m) 311 | dis =0.05 312 | 313 | #计算出目标典范抓取坐标系在基座base中的位置姿态 314 | btg_rot = self.btc_rot.dot(ctg_rot)#旋转矩阵[3,3] 315 | btg_quater=quaternion_from_matrix(btg_rot)#将姿态转换为四元数形式 316 | btg_trans = self.btc_rot.dot(ctg_trans.reshape(3,1))+self.btc_trans.reshape(3,1) #[3,1] 317 | btg_trans=btg_trans.reshape(3,)#[3,] 318 | 319 | self.grasp_pose_link8.position.x = btg_trans[0] 320 | self.grasp_pose_link8.position.y = btg_trans[1] 321 | self.grasp_pose_link8.position.z = btg_trans[2] 322 | self.grasp_pose_link8.orientation.x = btg_quater[0] 323 | self.grasp_pose_link8.orientation.y = btg_quater[1] 324 | self.grasp_pose_link8.orientation.z = btg_quater[2] 325 | self.grasp_pose_link8.orientation.w = btg_quater[3] 326 | 327 | #计算预抓取与后撤抓取坐标系在基座坐标系中的位置姿态 328 | self.pre_grasp_pose_link8 = copy.deepcopy(self.grasp_pose_link8) 329 | #计算pre抓取在相机坐标系中的位置 330 | btp_trans = btg_trans - btg_rot[0]*dis #[3,] 331 | self.pre_grasp_pose_link8.position.x = btp_trans[0] 332 | self.pre_grasp_pose_link8.position.y = btp_trans[1] 333 | self.pre_grasp_pose_link8.position.z = btp_trans[2] 334 | 335 | 336 | #发布目标抓取姿态在base坐标系的位置 337 | self.tf_broadcaster.sendTransform( 338 | btg_trans, 339 | btg_quater, 340 | rospy.Time.now(), 341 | "base2grasp", 342 | "panda_link0") 343 | #发布预备抓取姿态在base坐标系的位置 344 | self.tf_broadcaster.sendTransform( 345 | btp_trans, 346 | btg_quater,#与抓取姿态相同 347 | rospy.Time.now(), 348 | "base2pre", 349 | "panda_link0") 350 | 351 | #标志回调函数处理完毕 352 | self.callback_done=False 353 | 354 | def grasp_test(self): 355 | """ 356 | 给定panda_EE坐标系在panda_link0坐标系下的最终抓取姿态 BTEg 357 | 分别计算: 358 | panda_link8坐标系相对于panda_link0坐标系下的预抓取姿态 BTLp 359 | panda_link8坐标系相对于panda_link0坐标系下的最终抓取姿态 BTLg 360 | """ 361 | #设置后撤距离(10cm) 362 | dis =0.15 363 | 364 | #BTLg 最终抓取状态时，panda_link8相对于基座的位姿 365 | self.grasp_pose_link8=Pose() 366 | #BTLp 预抓取状态时，panda_link8相对于基座的位姿 367 | self.pre_grasp_pose_link8=Pose() 368 | 369 | 370 | #LTE panda_EE(夹爪)相对于link8坐标系的位姿(机械臂固定参数值) 371 | lte_trans=np.array([0.0000,0.0000,0.1034]) 372 | lte_quater = np.array([0.0000,0.0000,-0.38268,0.92388]) 373 | lte_rot = quaternion_matrix(lte_quater)#[4,4] 374 | #lte_rot = lte_rot[:3,:3] 375 | #BTEg 最终抓取状态时，panda_EE相对于基座的位姿 376 | bteg_quater=np.array([0.98609,0.16538,0.01226,-0.011129])#将姿态转换为四元数形式 377 | bteg_trans =np.array([0.55608,-0.04333,0.072476]) 378 | bteg_rot = quaternion_matrix(bteg_quater) 379 | #bteg_rot = bteg_rot[:3,:3]#截取旋转矩阵[3,3] 380 | #BTEp 预抓取状态时，panda_EE相对于基座的位姿 381 | btep_trans = bteg_trans - bteg_rot[:3,2]*dis #[3,] 382 | btep_rot = bteg_rot 383 | 384 | #BTLg 最终抓取状态时，panda_link8相对于基座的位姿 385 | btlg_rot = bteg_rot.dot(lte_rot.T)#姿态 386 | btlg_trans = bteg_trans - btlg_rot[:3,:3].dot(lte_trans)#位置 387 | btlg_quater=quaternion_from_matrix(btlg_rot) 388 | 389 | self.grasp_pose_link8.position.x = btlg_trans[0] 390 | self.grasp_pose_link8.position.y = btlg_trans[1] 391 | self.grasp_pose_link8.position.z = btlg_trans[2] 392 | self.grasp_pose_link8.orientation.x = btlg_quater[0] 393 | self.grasp_pose_link8.orientation.y = btlg_quater[1] 394 | self.grasp_pose_link8.orientation.z = btlg_quater[2] 395 | self.grasp_pose_link8.orientation.w = btlg_quater[3] 396 | 397 | #BTLp 预抓取状态时，panda_link8相对于基座的位姿 398 | self.pre_grasp_pose_link8 = copy.deepcopy(self.grasp_pose_link8) 399 | 400 | btlp_rot = btlg_rot 401 | btlp_trans = btep_trans - btlp_rot[:3,:3].dot(lte_trans) 402 | self.pre_grasp_pose_link8.position.x = btlp_trans[0] 403 | self.pre_grasp_pose_link8.position.y = btlp_trans[1] 404 | self.pre_grasp_pose_link8.position.z = btlp_trans[2] 405 | 406 | 407 | #发布BTEg 最终抓取状态时，panda_EE相对于基座的位姿 408 | self.tf_broadcaster.sendTransform( 409 | bteg_trans, 410 | bteg_quater, 411 | rospy.Time.now(), 412 | "base2grasp", 413 | "panda_link0") 414 | #发布BTEp 预抓取状态时，panda_EE相对于基座的位姿 415 | self.tf_broadcaster.sendTransform( 416 | btep_trans, 417 | bteg_quater,#与抓取姿态相同 418 | rospy.Time.now(), 419 | "base2pre", 420 | "panda_link0") 421 | 422 | #标志回调函数处理完毕 423 | self.callback_done=True 424 | 425 | 426 | def scale_trajectory_speed(self,traj,spd=0.1): 427 | new_traj = RobotTrajectory() 428 | new_traj = traj 429 | 430 | n_joints = len(traj.joint_trajectory.joint_names) 431 | n_points = len(traj.joint_trajectory.points) 432 | 433 | #spd = 3.0 434 | 435 | points = list(traj.joint_trajectory.points) 436 | 437 | for i in range(n_points): 438 | point = JointTrajectoryPoint() 439 | point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd 440 | point.velocities = list(traj.joint_trajectory.points[i].velocities) 441 | point.accelerations = list(traj.joint_trajectory.points[i].accelerations) 442 | point.positions = traj.joint_trajectory.points[i].positions 443 | 444 | for j in range(n_joints): 445 | point.velocities[j] = point.velocities[j] * spd 446 | point.accelerations[j] = point.accelerations[j] * spd 447 | 448 | points[i] = point 449 | 450 | new_traj.joint_trajectory.points = points 451 | return new_traj 452 | 453 | def add_table(self): 454 | """为场景中添加抓取桌面，防止机械臂与桌子发生碰撞 455 | """ 456 | #清除场景可能存在的遗留物体 457 | self.scene.remove_world_object('table') 458 | #设置桌面尺寸 x y z 459 | table_size = [0.6, 1.2, 0.01] 460 | #设置桌子的位置姿态 461 | table_pose = PoseStamped() 462 | table_pose.header.frame_id = 'panda_link0' 463 | table_pose.pose.position.x = 0.55 464 | table_pose.pose.position.y = 0.0 465 | table_pose.pose.position.z = 0.025 466 | table_pose.pose.orientation.w = 1.0 467 | # 将table加入场景当中 468 | self.scene.add_box('table', table_pose, table_size) 469 | 470 | def set_gripper(self,gripper_width): 471 | """设置panda 夹爪的开合大小 472 | gripper_width 最大0.08m 473 | """ 474 | if gripper_width>0.08 or gripper_width<0.0: 475 | raise Exception 476 | oneside=gripper_width/2 477 | joint_goal = self.panda_hand.get_current_joint_values() 478 | joint_goal[0] = oneside 479 | joint_goal[1] = oneside 480 | self.panda_hand.set_joint_value_target(joint_goal) 481 | plan=self.panda_hand.plan() 482 | self.panda_hand.execute(plan,wait=True) 483 | rospy.loginfo("Gripper action completed") 484 | 485 | 486 | 487 | if __name__ == "__main__": 488 | try: 489 | MoveItDemo() 490 | rospy.spin() 491 | except rospy.ROSInterruptException: 492 | rospy.loginfo("Arm tracker node terminated.") 493 | 494 | 495 | --------------------------------------------------------------------------------