├── README.md └── robot_admittance_controller ├── CMakeLists.txt ├── LICENSE ├── README.md ├── Scripts └── readForceSensor.py ├── config ├── admittance_params.yaml ├── arm_admittance_params_real.yaml ├── ur10_admittance_params.yaml └── ur10_admittance_params_real.yaml ├── include └── AdmittanceController.h ├── launch ├── arm_admittance_controller.launch ├── ur10_admittance_controller.launch ├── ur10_admittance_controller_real.launch ├── ur5_ft_upload.launch └── view_ur5_ft.launch ├── package.xml ├── src ├── AdmittanceController.cpp ├── admittance_controller_node.cpp └── readForce.cpp └── ur_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg └── view_robot.rviz ├── launch ├── ur10_upload.launch ├── ur5_ft_upload.launch ├── ur5_upload.launch ├── view_ur5.launch └── view_ur5_ft.launch ├── meshes └── ur5 │ ├── collision │ ├── base.stl │ ├── forearm.stl │ ├── shoulder.stl │ ├── upperarm.stl │ ├── wrist1.stl │ ├── wrist2.stl │ └── wrist3.stl │ └── visual │ ├── base.dae │ ├── forearm.dae │ ├── shoulder.dae │ ├── upperarm.dae │ ├── wrist1.dae │ ├── wrist2.dae │ └── wrist3.dae ├── model.pdf ├── package.xml └── urdf ├── common.gazebo.xacro ├── robotiq_ft300.urdf.xacro ├── ur.gazebo.xacro ├── ur.transmission.xacro ├── ur5.urdf.xacro ├── ur5_ft_robot.urdf.xacro ├── ur5_joint_limited_robot.urdf.xacro └── ur5_robot.urdf.xacro /README.md: -------------------------------------------------------------------------------- 1 | # admittance-controller_UR5 2 | UR5 with FT-300 robotiq force sensor(platform) to implement admittance control 3 | 4 | codes partly from https://github.com/nbfigueroa/robot_admittance_controller 5 | -------------------------------------------------------------------------------- /robot_admittance_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(arm_admittance_controller) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 roscpp geometry_msgs sensor_msgs tf tf_conversions robotiq_ft_sensor) 11 | 12 | set(CMAKE_CXX_FLAGS "-O2 -O3 -std=c++11 -Wall") 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | INCLUDE_DIRS include 105 | # LIBRARIES cpr_closed_loop_controller 106 | # CATKIN_DEPENDS cartesian_state_msgs 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS}) 117 | include_directories(${EIGEN3_INCLUDE_DIR}) 118 | 119 | set(INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include) 120 | set(SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) 121 | 122 | 123 | add_executable(admittance_controller_node 124 | ${SRC_DIR}/admittance_controller_node.cpp 125 | ${SRC_DIR}/AdmittanceController.cpp 126 | ${INCLUDE_DIR}/AdmittanceController.h) 127 | 128 | # add_executable(cpr_record_demo_node 129 | # src/demonstration_recorder_node.cpp) 130 | 131 | 132 | ## Declare a C++ library 133 | 134 | ## Add cmake target dependencies of the library 135 | ## as an example, code may need to be generated before libraries 136 | ## either from message generation or dynamic reconfigure 137 | #add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | ## With catkin_make all packages are built within a single CMake context 141 | ## The recommended prefix ensures that target names across packages don't collide 142 | # add_executable(${PROJECT_NAME}_node src/cpr_closed_loop_controller_node.cpp) 143 | 144 | ## Rename C++ executable without prefix 145 | ## The above recommended prefix causes long target names, the following renames the 146 | ## target back to the shorter version for ease of user use 147 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 148 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 149 | 150 | ## Add cmake target dependencies of the executable 151 | ## same as for the library above 152 | add_dependencies(admittance_controller_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 153 | 154 | ## Specify libraries to link a library or executable target against 155 | target_link_libraries(admittance_controller_node 156 | ${catkin_LIBRARIES} 157 | ) 158 | 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_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_cpr_closed_loop_controller.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 | 208 | 209 | 210 | add_executable(readForce 211 | src/readForce.cpp 212 | ) 213 | add_dependencies(readForce ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 214 | target_link_libraries(readForce 215 | ${catkin_LIBRARIES} 216 | ) -------------------------------------------------------------------------------- /robot_admittance_controller/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Nadia Figueroa, Interactive Robotics Group, MIT 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 | -------------------------------------------------------------------------------- /robot_admittance_controller/README.md: -------------------------------------------------------------------------------- 1 | # arm_admittance_controller 2 | 3 | Admittance control law to generate desired motion of an end-effector (twist), given a desired control wrench and external wrench for a robotic arm that is not torque-controlled (i.e. velocity or position controlled). 4 | 5 | Such a controller is necessary to use impedance-control-like laws and provide compliant human-robot-interaction when the velocity/position controlled robot arm is equipped with an external force/torque sensor. 6 | 7 | * The current implementation has been tested on a UR10 **velocity-controlled robot** equipped with a robotiq FT 300 force torque sensor. The arm is part of the Robbie Yuri robot of the Interactive Robotics Group (IRG), MIT which is an older version of the [Care-O-Bot](http://www.care-o-bot.org) platform series. 8 | 9 | * The current implementation is being tested on a Mitsubishi **position-controlled robot** equipped with a Mitsubishi force torque sensor. The robot is part of the MIT-Mitsubishi collaboration. 10 | 11 | **Disclaimer**: Parts of this code was originally forked from [ridgeback_ur5_controller](https://github.com/epfl-lasa/ridgeback_ur5_controller) which is a repo used to control a Clearpath robotic platform with a UR5 from the LASA laboratory at EPFL. Plenty modifications have been made in order to work with a standalone robotic arm. 12 | 13 | ## Installation 14 | ... 15 | 16 | ## Usage 17 | ... 18 | 19 | ## Contact 20 | **Maintainer**: [Nadia Figueroa](https://nbfigueroa.github.io/) (nadiafig AT mit dot edu) 21 | 22 | ## License 23 | This package is licensed under MIT license. 24 | -------------------------------------------------------------------------------- /robot_admittance_controller/Scripts/readForceSensor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # license removed for brevity 3 | import rospy 4 | ##from std_msgs.msg import String 5 | from std_msgs.msg import Float32 6 | import socket 7 | from time import gmtime, strftime 8 | import ConfigParser 9 | import time 10 | 11 | 12 | 13 | class socket_connection (): 14 | 15 | def __init__ (self, publisher_object, rate): 16 | 17 | try: 18 | 19 | config = ConfigParser.ConfigParser() 20 | config.read("config.ini") 21 | 22 | self.host_ip = config.get('Information', 'ip_address_ur5') 23 | self.port = int (config.get ('Information', 'port')) 24 | self.output_file_location = config.get ('Information', 'output_file_location') 25 | 26 | self.write_object = True 27 | 28 | if self.output_file_location is "": 29 | self.output_file_location = "" 30 | elif self.output_file_location is "None": 31 | self.write_object = False 32 | else: 33 | self.output_file_location = self.output_file_location + "/" 34 | 35 | 36 | except: 37 | 38 | print ("Warning: Config.ini file problem, please check Config.ini") 39 | 40 | self.socket_object = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 41 | self.publisher_object = publisher_object 42 | self.publisher_rate = rate 43 | self.is_published = False 44 | 45 | 46 | def connect (self): 47 | 48 | try: 49 | 50 | print("Warning: Connecting to Ur5 ip adress: " + self.host_ip) 51 | self.socket_object.connect(( self.host_ip, self.port )) 52 | 53 | if self.write_object is True: 54 | print("Warning: File Write Location: " + self.output_file_location) 55 | f = open (self.output_file_location + "DataStream.csv", "w") 56 | 57 | try: 58 | print("Writing in DataStream.csv, Press ctrl + c to stop") 59 | while (1): 60 | 61 | deneme = self.socket_object.recv(1024) 62 | denemeF = float(deneme.split(",")[2]) 63 | 64 | ## force_torque_values = (self.socket_object.recv(1024).replace("(","*")).replace(")","*") 65 | 66 | ## value_list = force_torque_values.split("*") 67 | 68 | ## rospy.loginfo(value_list [1]) 69 | rospy.loginfo(denemeF) 70 | 71 | 72 | 73 | ## self.publisher_object.publish(value_list[1]) 74 | self.publisher_object.publish(denemeF) 75 | self.publisher_rate.sleep() 76 | 77 | ## if self.write_object is True: 78 | ## f.write(strftime("%a, %d %b %Y %H:%M:%S +0000", gmtime()) + ": " +force_torque_values) 79 | 80 | except KeyboardInterrupt: 81 | 82 | f.close 83 | self.socket_object.close 84 | 85 | return False 86 | 87 | except Exception as e: 88 | 89 | print("Error: No Connection!! Please check your ethernet cable :)" + str(e)) 90 | 91 | return False 92 | 93 | 94 | 95 | def main(): 96 | 97 | 98 | ## pub = rospy.Publisher('ft300_force_torque', String, queue_size=10) 99 | 100 | pub = rospy.Publisher('ft300_force_torque', Float32, queue_size=10) 101 | rospy.init_node('torque_force_sensor_data', anonymous=True) 102 | 103 | rate = rospy.Rate(10) 104 | 105 | socket_connection_obj = socket_connection(pub, rate) 106 | socket_connection_obj.connect() 107 | 108 | 109 | if __name__ == "__main__": 110 | 111 | try: 112 | main() 113 | except rospy.ROSInterruptException: 114 | pass 115 | 116 | 117 | -------------------------------------------------------------------------------- /robot_admittance_controller/config/admittance_params.yaml: -------------------------------------------------------------------------------- 1 | # Admittance parameters for mass damper system 2 | mass_arm: [2,0,0,0,0,0, 3 | 0,2,0,0,0,0, 4 | 0,0,2,0,0,0, 5 | 0,0,0,1,0,0, 6 | 0,0,0,0,1,0, 7 | 0,0,0,0,0,1] 8 | damping_arm: [12,0,0,0,0,0, 9 | 0,12,0,0,0,0, 10 | 0,0,12,0,0,0, 11 | 0,0,0,7,0,0, 12 | 0,0,0,0,7,0, 13 | 0,0,0,0,0,7] 14 | # not that position is defined in "ur5_arm_base_link" 15 | # with x pointing right 16 | # y pointing forward 17 | # z pointing up 18 | 19 | # this will limit the workspace ([x_min x_max y_min y_max z_min z_max]) 20 | # This is in the world reference frame!! Might need to change to arm_base_link 21 | workspace_limits: [0.15, 1.75, -1.75, 1.75, 0.30, 1.4] 22 | arm_max_vel: 0.5 23 | arm_max_acc: 1.0 24 | -------------------------------------------------------------------------------- /robot_admittance_controller/config/arm_admittance_params_real.yaml: -------------------------------------------------------------------------------- 1 | # Admittance parameters for mass damper systems 2 | mass_arm: [6,0,0,0,0,0, 3 | 0,6,0,0,0,0, 4 | 0,0,6,0,0,0, 5 | 0,0,0,1,0,0, 6 | 0,0,0,0,1,0, 7 | 0,0,0,0,0,.5] 8 | damping_arm: [60,0,0,0,0,0, 9 | 0,60,0,0,0,0, 10 | 0,0,60,0,0,0, 11 | 0,0,0,15,0,0, 12 | 0,0,0,0,15,0, 13 | 0,0,0,0,0,15] 14 | 15 | 16 | # not that position is defined in "ur5_arm_base_link" 17 | # with x pointing right 18 | # y pointing forward 19 | # z pointing up 20 | 21 | # this will limit the workspace ([x_min x_max y_min y_max z_min z_max]) 22 | # This is in the world reference frame!! Might need to change to arm_base_link 23 | workspace_limits: [0.45, 1.5, -1.3, 1.3, 0.10, 1.4] 24 | arm_max_vel: 0.3 25 | arm_max_acc: 1.0 26 | -------------------------------------------------------------------------------- /robot_admittance_controller/config/ur10_admittance_params.yaml: -------------------------------------------------------------------------------- 1 | # Admittance parameters for mass damper system 2 | mass_arm: [2,0,0,0,0,0, 3 | 0,2,0,0,0,0, 4 | 0,0,2,0,0,0, 5 | 0,0,0,1,0,0, 6 | 0,0,0,0,1,0, 7 | 0,0,0,0,0,1] 8 | damping_arm: [12,0,0,0,0,0, 9 | 0,12,0,0,0,0, 10 | 0,0,12,0,0,0, 11 | 0,0,0,7,0,0, 12 | 0,0,0,0,7,0, 13 | 0,0,0,0,0,7] 14 | # not that position is defined in "ur5_arm_base_link" 15 | # with x pointing right 16 | # y pointing forward 17 | # z pointing up 18 | 19 | # this will limit the workspace ([x_min x_max y_min y_max z_min z_max]) 20 | # This is in the world reference frame!! Might need to change to arm_base_link 21 | workspace_limits: [0.45, 1.5, -1.3, 1.3, 0.10, 1.4] 22 | arm_max_vel: 0.3 23 | arm_max_acc: 0.3 24 | -------------------------------------------------------------------------------- /robot_admittance_controller/config/ur10_admittance_params_real.yaml: -------------------------------------------------------------------------------- 1 | # Admittance parameters for mass damper systems 2 | mass_arm: [6,0,0,0,0,0, 3 | 0,6,0,0,0,0, 4 | 0,0,6,0,0,0, 5 | 0,0,0,2,0,0, 6 | 0,0,0,0,2,0, 7 | 0,0,0,0,0,2] 8 | damping_arm: [40,0,0,0,0,0, 9 | 0,40,0,0,0,0, 10 | 0,0,40,0,0,0, 11 | 0,0,0,15,0,0, 12 | 0,0,0,0,15,0, 13 | 0,0,0,0,0,15] 14 | 15 | 16 | # not that position is defined in "ur5_arm_base_link" 17 | # with x pointing right 18 | # y pointing forward 19 | # z pointing up 20 | 21 | # this will limit the workspace ([x_min x_max y_min y_max z_min z_max]) 22 | # This is in the world reference frame!! Might need to change to arm_base_link 23 | workspace_limits: [0.45, 1.5, -1.3, 1.3, 0.10, 1.4] 24 | arm_max_vel: 0.3 25 | arm_max_acc: 0.3 26 | -------------------------------------------------------------------------------- /robot_admittance_controller/include/AdmittanceController.h: -------------------------------------------------------------------------------- 1 | #ifndef ADMITTANCECONTROLLER_H //防止一个源文件两次包含同一个头文件 2 | #define ADMITTANCECONTROLLER_H 3 | 4 | #include "ros/ros.h" 5 | 6 | // #include "cartesian_state_msgs/PoseTwist.h" 7 | #include "geometry_msgs/Pose.h" 8 | #include "geometry_msgs/Twist.h" 9 | #include "geometry_msgs/WrenchStamped.h" 10 | #include "geometry_msgs/TwistStamped.h" 11 | #include 12 | #include 13 | #include 14 | 15 | #include "eigen3/Eigen/Core" 16 | #include "eigen3/Eigen/Geometry" 17 | #include "eigen3/Eigen/Dense" 18 | 19 | #include "std_msgs/Float32.h" 20 | 21 | 22 | // AdmittanceController class // 23 | // A simple class implementing an admittance 24 | // controller for the ridgeback + UR5 platform at the LASA lab. An 25 | // AdmittanceController is a while loop that: 26 | // 1) Reads the robot state (arm+platform) 27 | // 1) Sends a desired twist to the robot platform 28 | // 2) Sends a desired twist to the robot arm 29 | // 30 | // Communication interfaces: 31 | // - The desired twists are both sent through ROS topics. 32 | // The low level controllers of both the arm and the platform are 33 | // implemented in ROS control. 34 | 35 | // NOTE: This node should be ideally a ROS 36 | // controller, but the current implementation of ROS control in 37 | // indigo requires writing a specific interface for this matter. 38 | 39 | // The kinetic version of ROS control enables controllers with 40 | // multiple interfaces (FT sensor, platform and arm), but a proper 41 | // ROS control implementation of this controller remains future work. 42 | // 43 | // USAGE EXAMPLE; 44 | // ros::NodeHandle nh; 45 | // double frequency = 1000.0; 46 | // std::string state_topic_arm, cmd_topic_arm, topic_arm_twist_world, 47 | // topic_wrench_u_e, topic_wrench_u_c, cmd_topic_platform, 48 | // state_topic_platform, wrench_topic, wrench_control_topic, 49 | // laser_front_topic, laser_rear_topic; 50 | // std::vector M_p, M_a, D, D_p, D_a, K, d_e; 51 | // double wrench_filter_factor, force_dead_zone_thres, 52 | // torque_dead_zone_thres, obs_distance_thres, self_detect_thres; 53 | // 54 | // 55 | // // Fill in values 56 | // ... 57 | // 58 | // 59 | // AdmittanceController admittance_controller(nh, frequency, 60 | // cmd_topic_platform, 61 | // state_topic_platform, 62 | // cmd_topic_arm, 63 | // topic_arm_twist_world, 64 | // topic_wrench_u_e, 65 | // topic_wrench_u_c, 66 | // state_topic_arm, 67 | // wrench_topic, 68 | // wrench_control_topic, 69 | // laser_front_topic, 70 | // laser_rear_topic, 71 | // M_p, M_a, D, D_p, D_a, K, d_e, 72 | // wrench_filter_factor, 73 | // force_dead_zone_thres, 74 | // torque_dead_zone_thres, 75 | // obs_distance_thres, 76 | // self_detect_thres); 77 | // admittance_controller.run(); 78 | 79 | using namespace Eigen; 80 | 81 | typedef Matrix Vector7d; //使用typedef为现有类型创建别名，定义易于记忆的类型名 82 | typedef Matrix Vector6d; 83 | typedef Matrix Matrix6d; 84 | 85 | class AdmittanceController 86 | { 87 | protected: 88 | // --- ROS VARIABLES --- // 89 | // A handle to the node in ros 90 | ros::NodeHandle nh_; 91 | // Rate of the run loop 92 | ros::Rate loop_rate_; 93 | 94 | 95 | // --- Subscribers --- // 96 | 97 | // Subscriber for the arm state 98 | ros::Subscriber sub_arm_pose_; 99 | ros::Subscriber sub_arm_pose_arm_; 100 | ros::Subscriber sub_arm_twist_; 101 | // Subscriber for the ft sensor at the end-effector 102 | ros::Subscriber sub_wrench_external_; 103 | // Subscriber for the control input (wrench) 104 | ros::Subscriber sub_wrench_control_; 105 | 106 | 107 | // --- Publishers --- // 108 | // Publisher for the twist of arm endeffector 109 | ros::Publisher pub_arm_cmd_; 110 | 111 | 112 | // --- INPUT SIGNAL --- // 113 | // external wrench (force/torque sensor) in "robotiq_force_torque_frame_id" frame 114 | Vector6d wrench_external_; 115 | // control wrench (from any controller) expected to be in "ur5_arm_base_link" frame 116 | Vector6d wrench_control_; 117 | 118 | 119 | // --- ADMITTANCE PARAMETERS --- // 120 | // M_a_ -> Desired mass of arm 121 | // D_a_ -> Desired damping of arm 122 | Matrix6d M_a_, D_a_; 123 | 124 | // --- OUTPUT COMMANDS --- // 125 | // final arm desired velocity 126 | Vector6d arm_desired_twist_; 127 | 128 | // limiting the workspace of the arm 129 | Vector6d workspace_limits_; 130 | double arm_max_vel_; 131 | double arm_max_acc_; 132 | 133 | // --- STATE VARIABLES -- // 134 | // Arm state: position, orientation, and twist (in "arm_base_link") 135 | Vector3d arm_real_position_; 136 | Quaterniond arm_real_orientation_; 137 | Vector6d arm_real_twist_; 138 | 139 | // extra variables (this can be made more general; i.e. include in yaml file!) 140 | Vector3d arm_real_position_arm_; 141 | Quaterniond arm_real_orientation_arm_; 142 | Vector3d base_position_; 143 | 144 | // End-effector state: pose and twist (in "world" frame) 145 | Vector7d ee_pose_world_; 146 | Vector6d ee_twist_world_; 147 | 148 | // Transform from base_link to world 149 | Matrix6d rotation_base_; 150 | Matrix6d rotation_tool_; 151 | 152 | // TF: 153 | // Listeners 154 | tf::TransformListener listener_ft_; 155 | tf::TransformListener listener_control_; 156 | tf::TransformListener listener_arm_; 157 | 158 | // Guards 159 | bool ft_arm_ready_; 160 | bool arm_world_ready_; 161 | bool world_arm_ready_; 162 | 163 | // Initialization 164 | void wait_for_transformations(); 165 | 166 | // Control 167 | void compute_admittance(); 168 | 169 | // Callbacks 170 | void pose_arm_callback(const geometry_msgs::PoseConstPtr msg); 171 | void pose_arm_arm_callback(const geometry_msgs::PoseConstPtr msg); 172 | void twist_arm_callback(const geometry_msgs::TwistConstPtr msg); 173 | void wrench_external_callback(const geometry_msgs::WrenchStampedConstPtr msg); 174 | void wrench_control_callback(const geometry_msgs::WrenchStampedConstPtr msg); 175 | 176 | 177 | // Util 178 | bool get_rotation_matrix(Matrix6d & rotation_matrix, 179 | tf::TransformListener & listener, 180 | std::string from_frame, std::string to_frame, bool getT); 181 | 182 | void limit_to_workspace(); 183 | 184 | // void publish_debuggings_signals(); 185 | 186 | void send_commands_to_robot(); 187 | 188 | 189 | public: 190 | AdmittanceController(ros::NodeHandle &n, double frequency, 191 | std::string topic_arm_pose, 192 | std::string topic_arm_twist, 193 | std::string wrench_external_topic, 194 | std::string wrench_control_topic, 195 | std::string cmd_topic_arm, 196 | std::vector M_a, 197 | std::vector D_a, 198 | std::vector workspace_limits, 199 | double arm_max_vel, 200 | double arm_max_acc); 201 | void run(); 202 | }; 203 | 204 | #endif // ADMITTANCECONTROLLER_H 205 | 206 | -------------------------------------------------------------------------------- /robot_admittance_controller/launch/arm_admittance_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robot_admittance_controller/launch/ur10_admittance_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robot_admittance_controller/launch/ur10_admittance_controller_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robot_admittance_controller/launch/ur5_ft_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_admittance_controller/launch/view_ur5_ft.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /robot_admittance_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | arm_admittance_controller 4 | 0.0.0 5 | The arm_admittance_controller package 6 | 7 | 8 | 9 | 10 | Jose Medina 11 | 12 | 13 | MIT 14 | 15 | 16 | 17 | 18 | 19 | tf 20 | cmake_modules 21 | eigen 22 | 23 | sensor_msgs 24 | geometry_msgs 25 | tf 26 | eigen 27 | catkin 28 | 29 | 30 | 31 | 32 | roscpp 33 | geometry_msgs 34 | sensor_msgs 35 | tf_conversions 36 | robotiq_ft_sensor 37 | roscpp 38 | tf_conversions 39 | robotiq_ft_sensor 40 | -------------------------------------------------------------------------------- /robot_admittance_controller/src/AdmittanceController.cpp: -------------------------------------------------------------------------------- 1 | #include "AdmittanceController.h" 2 | 3 | AdmittanceController::AdmittanceController(ros::NodeHandle &n, 4 | double frequency, 5 | std::string topic_arm_pose, 6 | std::string topic_arm_twist, 7 | std::string topic_external_wrench, 8 | std::string topic_control_wrench, 9 | std::string topic_arm_command, 10 | std::vector M_a, 11 | std::vector D_a, 12 | std::vector workspace_limits, 13 | double arm_max_vel, 14 | double arm_max_acc) : 15 | nh_(n), loop_rate_(frequency), 16 | M_a_(M_a.data()), 17 | D_a_(D_a.data()), 18 | workspace_limits_(workspace_limits.data()), 19 | arm_max_vel_(arm_max_vel), 20 | arm_max_acc_(arm_max_acc){ 21 | 22 | 23 | // --- Subscribers --- // 24 | sub_arm_pose_ = nh_.subscribe(topic_arm_pose, 10, 25 | &AdmittanceController::pose_arm_callback, this, 26 | ros::TransportHints().reliable().tcpNoDelay()); 27 | std::string topic_arm_pose_arm = "/UR10arm/ee_pose_arm"; 28 | sub_arm_pose_arm_ = nh_.subscribe(topic_arm_pose_arm, 10, 29 | &AdmittanceController::pose_arm_arm_callback, this, 30 | ros::TransportHints().reliable().tcpNoDelay()); 31 | 32 | sub_arm_twist_ = nh_.subscribe(topic_arm_twist, 10, 33 | &AdmittanceController::twist_arm_callback, this, 34 | ros::TransportHints().reliable().tcpNoDelay()); 35 | 36 | sub_wrench_external_ = nh_.subscribe(topic_external_wrench, 5, 37 | &AdmittanceController::wrench_external_callback, this, 38 | ros::TransportHints().reliable().tcpNoDelay()); 39 | sub_wrench_control_ = nh_.subscribe(topic_control_wrench, 5, 40 | &AdmittanceController::wrench_control_callback, this, 41 | ros::TransportHints().reliable().tcpNoDelay()); 42 | 43 | // --- Publishers --- // 44 | pub_arm_cmd_ = nh_.advertise(topic_arm_command, 5); 45 | 46 | // pub_wrench_external_ = nh_.advertise( 47 | // topic_external_wrench_arm_frame, 5); 48 | // pub_wrench_control_ = nh_.advertise( 49 | // topic_control_wrench_arm_frame, 5); 50 | 51 | ROS_INFO_STREAM("Arm max vel:" << arm_max_vel_ << " max acc:" << arm_max_acc_); 52 | 53 | 54 | // initializing the class variables 55 | wrench_external_.setZero(); 56 | wrench_control_.setZero(); 57 | 58 | ee_pose_world_.setZero(); 59 | ee_twist_world_.setZero(); 60 | 61 | // setting the robot state to zero and wait for data 62 | arm_real_position_.setZero(); 63 | 64 | while (nh_.ok() && !arm_real_position_(0)) { 65 | ROS_WARN_THROTTLE(1, "Waiting for the state of the arm..."); 66 | ros::spinOnce(); 67 | loop_rate_.sleep(); 68 | } 69 | 70 | // Init integrator 71 | arm_desired_twist_.setZero(); 72 | ft_arm_ready_ = false; 73 | arm_world_ready_ = false; 74 | world_arm_ready_ = false; 75 | 76 | wait_for_transformations(); 77 | } 78 | 79 | /////////////////////////////////////////////////////////////// 80 | ///////////////////// Control Loop //////////////////////////// 81 | /////////////////////////////////////////////////////////////// 82 | void AdmittanceController::run() { 83 | 84 | ROS_INFO("Running the admittance control loop ................."); 85 | 86 | while (nh_.ok()) { 87 | // Admittance Dynamics computation 88 | compute_admittance(); 89 | 90 | // sum the vel from admittance to DS in this function 91 | limit_to_workspace(); 92 | // Here I can do the "workspace-modulation" idea 93 | 94 | // Copy commands to messages 95 | send_commands_to_robot(); 96 | 97 | ros::spinOnce(); 98 | loop_rate_.sleep(); 99 | } 100 | 101 | 102 | } 103 | 104 | 105 | /////////////////////////////////////////////////////////////// 106 | ///////////////////// Admittance Dynamics ///////////////////// 107 | /////////////////////////////////////////////////////////////// 108 | void AdmittanceController::compute_admittance() { 109 | 110 | // Vector6d platform_desired_acceleration; 111 | Vector6d arm_desired_accelaration; 112 | 113 | arm_desired_accelaration = M_a_.inverse() * ( - D_a_ * arm_desired_twist_ 114 | + wrench_external_ + wrench_control_); 115 | 116 | // limiting the accelaration for better stability and safety 117 | double a_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm(); 118 | if (a_acc_norm > arm_max_acc_) { 119 | ROS_WARN_STREAM_THROTTLE(1, "Admittance generates high arm accelaration!" 120 | << " norm: " << a_acc_norm); 121 | arm_desired_accelaration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm); 122 | } 123 | 124 | // Integrate for velocity based interface 125 | ros::Duration duration = loop_rate_.expectedCycleTime(); 126 | arm_desired_twist_ += arm_desired_accelaration * duration.toSec(); 127 | 128 | } 129 | 130 | /////////////////////////////////////////////////////////////// 131 | ////////////////////////// Callbacks ////////////////////////// 132 | /////////////////////////////////////////////////////////////// 133 | void AdmittanceController::pose_arm_callback( 134 | const geometry_msgs::PoseConstPtr msg) { 135 | arm_real_position_ << msg->position.x, msg->position.y, 136 | msg->position.z; 137 | 138 | arm_real_orientation_.coeffs() << msg->orientation.x, 139 | msg->orientation.y, 140 | msg->orientation.z, 141 | msg->orientation.w; 142 | } 143 | 144 | void AdmittanceController::pose_arm_arm_callback( 145 | const geometry_msgs::PoseConstPtr msg) { 146 | arm_real_position_arm_ << msg->position.x, msg->position.y, 147 | msg->position.z; 148 | 149 | arm_real_orientation_arm_.coeffs() << msg->orientation.x, 150 | msg->orientation.y, 151 | msg->orientation.z, 152 | msg->orientation.w; 153 | } 154 | 155 | void AdmittanceController::twist_arm_callback( 156 | const geometry_msgs::TwistConstPtr msg) { 157 | 158 | arm_real_twist_ << msg->linear.x, msg->linear.y, 159 | msg->linear.z, msg->angular.x, msg->angular.y, 160 | msg->angular.z; 161 | } 162 | 163 | void AdmittanceController::wrench_external_callback( 164 | const geometry_msgs::WrenchStampedConstPtr msg) { 165 | Vector6d wrench_ft_frame; 166 | if (ft_arm_ready_) { 167 | 168 | // Reading the FT-sensor in its own frame (robotiq_force_torque_frame_id) 169 | wrench_ft_frame << msg->wrench.force.x, msg->wrench.force.y, 170 | msg->wrench.force.z, msg->wrench.torque.x, 171 | msg->wrench.torque.y, msg->wrench.torque.z; 172 | 173 | 174 | // --- This can be a callback! --- // 175 | get_rotation_matrix(rotation_tool_, listener_ft_, "base_link", "robotiq_force_torque_frame_id", 0 ); 176 | wrench_external_ << rotation_tool_ * wrench_ft_frame; 177 | } 178 | } 179 | 180 | void AdmittanceController::wrench_control_callback( 181 | const geometry_msgs::WrenchStampedConstPtr msg) { 182 | 183 | if (msg->header.frame_id == "arm_base_link") { 184 | wrench_control_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, 185 | msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z; 186 | } 187 | else { 188 | ROS_WARN_THROTTLE(5, "wrench_control_callback: The frame_id is not specified as ur5_arm_base_link"); 189 | } 190 | } 191 | 192 | /////////////////////////////////////////////////////////////// 193 | //////////////////// COMMANDING THE ROBOT ///////////////////// 194 | /////////////////////////////////////////////////////////////// 195 | void AdmittanceController::send_commands_to_robot() { 196 | 197 | // for the arm 198 | geometry_msgs::Twist arm_twist_cmd; 199 | 200 | arm_twist_cmd.linear.x = arm_desired_twist_(0); 201 | arm_twist_cmd.linear.y = arm_desired_twist_(1); 202 | arm_twist_cmd.linear.z = arm_desired_twist_(2); 203 | arm_twist_cmd.angular.x = arm_desired_twist_(3); 204 | arm_twist_cmd.angular.y = arm_desired_twist_(4); 205 | arm_twist_cmd.angular.z = arm_desired_twist_(5); 206 | 207 | ROS_WARN_STREAM_THROTTLE(0.1,"Desired linear velocity: " << arm_twist_cmd.linear.x << " " << arm_twist_cmd.linear.y << " " << arm_twist_cmd.linear.z); 208 | ROS_WARN_STREAM_THROTTLE(0.1,"Desired angular velocity: " << arm_twist_cmd.angular.x << " " << arm_twist_cmd.angular.y << " " << arm_twist_cmd.angular.z); 209 | 210 | pub_arm_cmd_.publish(arm_twist_cmd); 211 | } 212 | 213 | 214 | void AdmittanceController::limit_to_workspace() { 215 | 216 | // velocity of the arm along x, y, and z axis 217 | double norm_vel_des = (arm_desired_twist_.segment(0, 3)).norm(); 218 | 219 | if (norm_vel_des > arm_max_vel_) { 220 | ROS_WARN_STREAM_THROTTLE(1, "Admittance generate fast arm movements! velocity norm: " << norm_vel_des); 221 | 222 | arm_desired_twist_.segment(0, 3) *= (arm_max_vel_ / norm_vel_des); 223 | 224 | } 225 | 226 | if (norm_vel_des < 1e-5) 227 | arm_desired_twist_.segment(0,3).setZero(); 228 | 229 | if (arm_desired_twist_(3) < 1e-5) 230 | arm_desired_twist_(3) = 0; 231 | if (arm_desired_twist_(4)< 1e-5) 232 | arm_desired_twist_(4) = 0; 233 | if (arm_desired_twist_(5) < 1e-5) 234 | arm_desired_twist_(5) = 0; 235 | 236 | // velocity of the arm along x, y, and z angles 237 | if (arm_desired_twist_(3) > 0.3) 238 | arm_desired_twist_(3) = 0.3; 239 | if (arm_desired_twist_(4) > 0.3) 240 | arm_desired_twist_(4) = 0.3; 241 | if (arm_desired_twist_(5) > 0.3) 242 | arm_desired_twist_(5) = 0.3; 243 | 244 | 245 | // Impose workspace constraints on desired velocities 246 | double ee_base_norm = (arm_real_position_arm_).norm(); 247 | double rec_operating_limit = 1.15; // simulated robot 248 | // double rec_operating_limit = 1; // real robot 249 | double dist_limit = rec_operating_limit - ee_base_norm; 250 | ROS_WARN_STREAM_THROTTLE(0.1, "||x_ee-w_limit||: " << dist_limit) ; 251 | if (ee_base_norm >= rec_operating_limit){ 252 | ROS_WARN_STREAM_THROTTLE(0.1, "Out of operational workspace limit!") ; 253 | base_position_ << 0.33000, 0.00000, 0.48600; 254 | Vector3d repulsive_field = -(arm_real_position_- base_position_); 255 | arm_desired_twist_.segment(0,3) = 0.05*(repulsive_field/ee_base_norm); 256 | ROS_WARN_STREAM_THROTTLE(0.1, "Bringing robot back slowly with uniform repulsive velocity field!") ; 257 | } 258 | 259 | if (arm_real_position_(2) < workspace_limits_(4)) 260 | arm_desired_twist_(2) += 0.3; 261 | 262 | 263 | // Continuous modulation of velocity to follow the surface (try walid/sina's modulation) 264 | double workspace_fct = dist_limit; 265 | if (dist_limit > 0.05) 266 | workspace_fct = 1; 267 | 268 | // workspace_fct *= norm_vel_des; 269 | // repulsive_field = workspace_fct * (repulsive_field/ee_base_norm); 270 | // ROS_WARN_STREAM_THROTTLE(0.1,"Repulsive velocity field: " << repulsive_field(0) << " " << repulsive_field(1) << " " << repulsive_field(2)); 271 | ROS_WARN_STREAM_THROTTLE(0.1,"Workspace Scaling function: " << workspace_fct); 272 | // arm_desired_twist_.segment(0,3) = workspace_fct*arm_desired_twist_.segment(0,3); 273 | 274 | } 275 | 276 | 277 | ////////////////////// 278 | /// INITIALIZATION /// 279 | ////////////////////// 280 | void AdmittanceController::wait_for_transformations() { 281 | tf::TransformListener listener; 282 | Matrix6d rot_matrix; 283 | rotation_base_.setZero(); 284 | rotation_tool_.setZero(); 285 | 286 | // Makes sure all TFs exists before enabling all transformations in the callbacks 287 | while (!get_rotation_matrix(rotation_base_, listener, 288 | "base_link", "arm_base_link", 1)) { 289 | sleep(1); 290 | } 291 | arm_world_ready_ = true; 292 | 293 | while (!get_rotation_matrix(rot_matrix, listener, 294 | "arm_base_link", "base_link", 0)) { 295 | sleep(1); 296 | } 297 | world_arm_ready_ = true; 298 | 299 | while (!get_rotation_matrix(rotation_tool_, listener, 300 | "base_link", "robotiq_force_torque_frame_id", 0)) { 301 | sleep(1); 302 | } 303 | ft_arm_ready_ = true; 304 | ROS_INFO("The Force/Torque sensor is ready to use."); 305 | } 306 | 307 | 308 | 309 | 310 | //////////// 311 | /// UTIL /// 312 | //////////// 313 | 314 | // Add a "get transformation" 315 | bool AdmittanceController::get_rotation_matrix(Matrix6d & rotation_matrix, 316 | tf::TransformListener & listener, 317 | std::string from_frame, 318 | std::string to_frame, bool getT) { 319 | tf::StampedTransform transform; 320 | Matrix3d rotation_from_to; 321 | try { 322 | listener.lookupTransform(from_frame, to_frame, 323 | ros::Time(0), transform); 324 | tf::matrixTFToEigen(transform.getBasis(), rotation_from_to); 325 | rotation_matrix.setZero(); 326 | rotation_matrix.topLeftCorner(3, 3) = rotation_from_to; 327 | rotation_matrix.bottomRightCorner(3, 3) = rotation_from_to; 328 | 329 | if (getT==1) 330 | base_position_ << transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z() ; 331 | } 332 | catch (tf::TransformException ex) { 333 | rotation_matrix.setZero(); 334 | ROS_WARN_STREAM_THROTTLE(1, "Waiting for TF from: " << from_frame << " to: " << to_frame ); 335 | return false; 336 | } 337 | 338 | return true; 339 | } 340 | -------------------------------------------------------------------------------- /robot_admittance_controller/src/admittance_controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "AdmittanceController.h" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "admittance_controller_node"); 7 | 8 | ros::NodeHandle nh; 9 | double frequency = 150.0; 10 | 11 | // Parameters 12 | std::string topic_arm_pose; 13 | std::string topic_arm_twist; 14 | std::string topic_arm_command; 15 | std::string topic_external_wrench; 16 | std::string topic_control_wrench; 17 | std::string topic_ds_velocity; 18 | std::string topic_external_wrench_arm_frame; 19 | std::string topic_control_external_arm_frame; 20 | std::string topic_admittance_ratio; 21 | 22 | std::vector M_a; 23 | std::vector D_a; 24 | std::vector workspace_limits; 25 | 26 | double arm_max_vel; 27 | double arm_max_acc; 28 | 29 | 30 | // LOADING PARAMETERS FROM THE ROS SERVER 31 | 32 | // Topic names 33 | if (!nh.getParam("topic_arm_pose", topic_arm_pose)) { 34 | ROS_ERROR("Couldn't retrieve the topic name for the EE pose in the world frame."); 35 | return -1; 36 | } 37 | 38 | if (!nh.getParam("topic_arm_twist", topic_arm_twist)) { 39 | ROS_ERROR("Couldn't retrieve the topic name for the EE twist in the world frame."); 40 | return -1; 41 | } 42 | 43 | if (!nh.getParam("topic_external_wrench", topic_external_wrench)) { 44 | ROS_ERROR("Couldn't retrieve the topic name for the force/torque sensor."); 45 | return -1; 46 | } 47 | 48 | if (!nh.getParam("topic_control_wrench", topic_control_wrench)) { 49 | ROS_ERROR("Couldn't retrieve the topic name for the control wrench."); 50 | return -1; 51 | } 52 | 53 | if (!nh.getParam("topic_arm_command", topic_arm_command)) { 54 | ROS_ERROR("Couldn't retrieve the topic name for commanding the arm."); 55 | return -1; 56 | } 57 | 58 | // ADMITTANCE PARAMETERS 59 | if (!nh.getParam("mass_arm", M_a)) { 60 | ROS_ERROR("Couldn't retrieve the desired mass of the arm."); 61 | return -1; 62 | } 63 | 64 | if (!nh.getParam("damping_arm", D_a)) { 65 | ROS_ERROR("Couldn't retrieve the desired damping of the arm."); 66 | return -1; 67 | } 68 | 69 | 70 | // SAFETY PARAMETERS 71 | if (!nh.getParam("workspace_limits", workspace_limits)) { 72 | ROS_ERROR("Couldn't retrieve the limits of the workspace."); 73 | return -1; 74 | } 75 | 76 | if (!nh.getParam("arm_max_vel", arm_max_vel)) { 77 | ROS_ERROR("Couldn't retrieve the max velocity for the arm."); 78 | return -1; 79 | } 80 | 81 | if (!nh.getParam("arm_max_acc", arm_max_acc)) { 82 | ROS_ERROR("Couldn't retrieve the max acceleration for the arm."); 83 | return -1; 84 | } 85 | 86 | 87 | // Constructing the controller 88 | AdmittanceController admittance_controller( 89 | nh, 90 | frequency, 91 | topic_arm_pose, 92 | topic_arm_twist, 93 | topic_external_wrench, 94 | topic_control_wrench, 95 | topic_arm_command, 96 | M_a, D_a, 97 | workspace_limits, 98 | arm_max_vel, arm_max_acc); 99 | 100 | // Running the controller 101 | admittance_controller.run(); 102 | 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /robot_admittance_controller/src/readForce.cpp: -------------------------------------------------------------------------------- 1 | /* Software License Agreement (BSD License) 2 | * 3 | * Copyright (c) 2014, Robotiq, Inc. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * * Neither the name of Robotiq, Inc. nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * Copyright (c) 2014, Robotiq, Inc 34 | */ 35 | 36 | /** 37 | * \file rq_test_sensor.cpp 38 | * 39 | * \author Jonathan Savoie 40 | * \maintainer Jean-Philippe Roberge 41 | */ 42 | 43 | #include "ros/ros.h" 44 | #include "std_msgs/String.h" 45 | #include "robotiq_ft_sensor/ft_sensor.h" 46 | #include "robotiq_ft_sensor/sensor_accessor.h" 47 | 48 | #include 49 | 50 | /*void receiveCallback(const std_msgs::String::ConstPtr& msg) 51 | { 52 | ROS_INFO("I heard: [%s]", msg->data.c_str()); 53 | }*/ 54 | 55 | void reCallback(const robotiq_ft_sensor::ft_sensor& msg) 56 | { 57 | ROS_INFO("I heard: FX[%f] FY[%f] FZ[%f] MX[%f] MY[%f] MZ[%f]", msg.Fx,msg.Fy,msg.Fz,msg.Mx,msg.My,msg.Mz); 58 | } 59 | 60 | /** 61 | * This tutorial demonstrates simple sending of messages over the ROS system. 62 | */ 63 | int main(int argc, char **argv) 64 | { 65 | 66 | ros::init(argc, argv, "rq_test_sensor"); 67 | 68 | 69 | ros::NodeHandle n; 70 | 71 | ros::ServiceClient client = n.serviceClient("robotiq_ft_sensor_acc"); 72 | ros::Subscriber sub1 = n.subscribe("robotiq_ft_sensor",100,reCallback); //订阅方式获取数据 73 | 74 | robotiq_ft_sensor::sensor_accessor srv; 75 | 76 | int count = 0; 77 | while (ros::ok()) 78 | { 79 | if(count == 10000000){ 80 | 81 | /// Deprecated Interface 82 | // srv.request.command = "SET ZRO"; 83 | 84 | /// New Interface with numerical commands 85 | srv.request.command_id = srv.request.COMMAND_SET_ZERO; 86 | 87 | if(client.call(srv)){ 88 | ROS_INFO("ret: %s", srv.response.res.c_str()); 89 | } 90 | count = 0; 91 | } 92 | 93 | ros::spinOnce(); 94 | 95 | ++count; 96 | } 97 | 98 | 99 | return 0; 100 | } -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ur_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.7 (2019-11-23) 6 | ------------------ 7 | 8 | 1.2.6 (2019-11-19) 9 | ------------------ 10 | * Add optional safety_controller tags to all joints in xacro macros (`#437 `_) 11 | * Migrated all package.xml files to format=2 (`#439 `_) 12 | * Corrected dimensions and positions of inertias (`#426 `_) 13 | * Add description view launch files for all descriptions to easily check them (`#435 `_) 14 | * Contributors: Felix Mauch, JeremyZoss, Miguel Prada, Qiang Qiu, gavanderhoorn 15 | 16 | 1.2.5 (2019-04-05) 17 | ------------------ 18 | * Add transmission_hw_interface to UR xacro and expose everywhere (`#392 `_) 19 | * Update maintainer listing: add Miguel (`#410 `_) 20 | * Updated xacro namespace. 21 | * Update maintainer and author information. 22 | * Updated mesh ambience so the model isn't so dark in Gazebo 23 | * Fix overlapping variable names between robot definition files (`#356 `_) 24 | * Improve meshes shading (`#233 `_) 25 | * Added run_depend for xacro 26 | * Using the 'doc' attribute on 'arg' elements. 27 | * Enable self collision in gazebo 28 | * Contributors: Dave Niewinski, Felix von Drigalski, Harsh Deshpande, Joe, Marcel Schnirring, Miguel Prada, MonteroJJ, ipa-fxm 29 | 30 | 1.2.1 (2018-01-06) 31 | ------------------ 32 | * Merge pull request `#329 `_ from tecnalia-medical-robotics/joint_limits 33 | Homogenize xacro macro arguments. 34 | * Merge pull request `#332 `_ from davetcoleman/kinetic_hw_iface_warning 35 | Remove UR3 ROS Control Hardware Interface warning 36 | * Remove UR3 ROS Control Hardware Interface warning 37 | * Extend changes to '_robot.urdf.xacro' variants as well. 38 | * Homogenize xacro macro arguments. 39 | Joint limits for the limited version could be set using arguments for the UR10 40 | but not for the UR3 and UR5. Same lower and upper limit arguments are added to 41 | the UR3 and UR5 xacro macros. 42 | * Fix elbow joint limits (`#268 `_) 43 | * Remove warning 'redefining global property: pi' (Jade+) (`#315 `_) 44 | * Contributors: Beatriz Leon, Dave Coleman, Felix Messmer, Miguel Prada 45 | 46 | 1.2.0 (2017-08-04) 47 | ------------------ 48 | 49 | 1.1.9 (2017-01-02) 50 | ------------------ 51 | * reintroduce 'pi', unbrake dependent xacros. 52 | * use '--inorder' to trigger use of jade+ xacro on Indigo. 53 | * Contributors: gavanderhoorn 54 | 55 | 1.1.8 (2016-12-30) 56 | ------------------ 57 | * all: update maintainers. 58 | * Contributors: gavanderhoorn 59 | 60 | 1.1.7 (2016-12-29) 61 | ------------------ 62 | * Fix xacro warnings in Jade (`#251 `_) 63 | * added default values to xacro macro 64 | * tested joint limits modification 65 | * Contributors: Dave Coleman, G.A. vd. Hoorn, philip 14.04 66 | 67 | 1.1.6 (2016-04-01) 68 | ------------------ 69 | * unify mesh names 70 | * add color to avoid default color 'red' for collision meshes 71 | * use correct DH parameter + colored meshes 72 | * introducing urdf for ur3 - first draft 73 | * unify common xacro files 74 | * remove obsolete urdf files 75 | * description: add '_joint' suffix to newly introduced joint tags. 76 | This is more in-line with naming of existing joint tags. 77 | * description: add ROS-I base and tool0 frames. Fix `#49 `_ and `#95 `_. 78 | Note that 'base' is essentially 'base_link' but rotated by 180 79 | degrees over the Z-axis. This is necessary as the visual and 80 | collision geometries appear to also have their origins rotated 81 | 180 degrees wrt the real robot. 82 | 'tool0' is similar to 'ee_link', but with its orientation such 83 | that it coincides with an all-zeros TCP setting on the UR 84 | controller. Users are expected to attach their own TCP frames 85 | to this frame, instead of updating it (see also [1]). 86 | [1] http://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages#Standardised_links\_.2BAC8_frames 87 | * description: minor whitespace cleanup of UR5 & 10 xacros. 88 | * regenerate urdf files 89 | * use PositionJointInterface as hardwareInterface in transmissions - affects simulation only 90 | * Contributors: gavanderhoorn, ipa-fxm 91 | 92 | 1.0.2 (2014-03-31) 93 | ------------------ 94 | 95 | 1.0.1 (2014-03-31) 96 | ------------------ 97 | * changes due to file renaming 98 | * generate urdfs from latest xacros 99 | * file renaming 100 | * adapt launch files in order to be able to use normal/limited xacro 101 | * fixed typo in limits 102 | * add joint_limited urdf.xacros for both robots 103 | * (re-)add ee_link for both robots 104 | * updates for latest gazebo under hydro 105 | * remove ee_link - as in ur10 106 | * use same xacro params as ur10 107 | * use new transmission interfaces 108 | * update xml namespaces for hydro 109 | * remove obsolete urdf file 110 | * remove obsolete urdf file 111 | * Contributors: ipa-fxm 112 | 113 | * Update ur10.urdf.xacro 114 | Corrected UR10's urdf to faithfully represent joint effort thresholds, velocity limits, and dynamics parameters. 115 | * Update ur5.urdf.xacro 116 | Corrected effort thresholds and friction values for UR5 urdf. 117 | * added corrected mesh file 118 | * Added definitions for adding tergets in install folder. Issue `#10 `_. 119 | * Corrected warning on xacro-files in hydro. 120 | * Added definitions for adding tergets in install folder. Issue `#10 `_. 121 | * Updated to catkin. ur_driver's files were added to nested Python directory for including in other packages. 122 | * fixed name of ur5 transmissions 123 | * patched gazebo.urdf.xacro to be compatible with gazebo 1.5 124 | * fixed copy&paste error (?) 125 | * prefix versions of gazebo and transmission macros 126 | * Added joint limited urdf and associated moveit package. The joint limited package is friendlier to the default KLD IK solution 127 | * Added ur5 moveit library. The Kinematics used by the ur5 move it library is unreliable and should be replaced with the ur_kinematics 128 | * Updated urdf files use collision/visual models. 129 | * Reorganized meshes to include both collision and visual messhes (like other ROS-I robots). Modified urdf xacro to include new models. Removed extra robot pedestal link from urdf (urdfs should only include the robot itself). 130 | * minor changes on ur5 xacro files 131 | * Removed extra stl files and fixed indentions 132 | * Renamed packages and new groovy version 133 | * Added ur10 and renamed packages 134 | * Contributors: Denis Štogl, IPR-SR2, Kelsey, Mathias Lüdtke, Shaun Edwards, ipa-nhg, jrgnicho, kphawkins, robot 135 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ur_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | # add_message_files( 24 | # FILES 25 | # Message1.msg 26 | # Message2.msg 27 | # ) 28 | 29 | ## Generate services in the 'srv' folder 30 | # add_service_files( 31 | # FILES 32 | # Service1.srv 33 | # Service2.srv 34 | # ) 35 | 36 | ## Generate added messages and services with any dependencies listed here 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # std_msgs # Or other packages containing msgs 40 | # ) 41 | 42 | ################################### 43 | ## catkin specific configuration ## 44 | ################################### 45 | ## The catkin_package macro generates cmake config files for your package 46 | ## Declare things to be passed to dependent projects 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 49 | ## DEPENDS: system dependencies of this project that dependent projects also need 50 | catkin_package( 51 | # INCLUDE_DIRS include 52 | # LIBRARIES ur_description 53 | # CATKIN_DEPENDS urdf 54 | # DEPENDS system_lib 55 | ) 56 | 57 | ########### 58 | ## Build ## 59 | ########### 60 | 61 | ## Specify additional locations of header files 62 | ## Your package locations should be listed before other locations 63 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 64 | 65 | ## Declare a cpp library 66 | # add_library(ur_description 67 | # src/${PROJECT_NAME}/ur_description.cpp 68 | # ) 69 | 70 | ## Declare a cpp executable 71 | # add_executable(ur_description_node src/ur_description_node.cpp) 72 | 73 | ## Add cmake target dependencies of the executable/library 74 | ## as an example, message headers may need to be generated before nodes 75 | # add_dependencies(ur_description_node ur_description_generate_messages_cpp) 76 | 77 | ## Specify libraries to link a library or executable target against 78 | # target_link_libraries(ur_description_node 79 | # ${catkin_LIBRARIES} 80 | # ) 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | 86 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 87 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 88 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 89 | 90 | # all install targets should use catkin DESTINATION variables 91 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 92 | 93 | ## Mark executable scripts (Python etc.) for installation 94 | ## in contrast to setup.py, you can choose the destination 95 | # install(PROGRAMS 96 | # scripts/my_python_script 97 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | # ) 99 | 100 | ## Mark executables and/or libraries for installation 101 | # install(TARGETS ur_description ur_description_node 102 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 105 | # ) 106 | 107 | ## Mark cpp header files for installation 108 | # install(DIRECTORY include/${PROJECT_NAME}/ 109 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 110 | # FILES_MATCHING PATTERN "*.h" 111 | # PATTERN ".svn" EXCLUDE 112 | # ) 113 | 114 | ## Mark other files for installation (e.g. launch and bag files, etc.) 115 | # install(FILES 116 | # # myfile1 117 | # # myfile2 118 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | # ) 120 | 121 | ############# 122 | ## Testing ## 123 | ############# 124 | 125 | ## Add gtest based cpp test target and link libraries 126 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_description.cpp) 127 | # if(TARGET ${PROJECT_NAME}-test) 128 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 129 | # endif() 130 | 131 | ## Add folders to be run by python nosetests 132 | # catkin_add_nosetests(test) 133 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/cfg/view_robot.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 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 787 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 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.03 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 | base_link: 58 | Alpha: 1 59 | Show Axes: false 60 | Show Trail: false 61 | Value: true 62 | link_1: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | link_2: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | link_3: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | link_4: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | link_5: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | link_6: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | Name: RobotModel 93 | Robot Description: robot_description 94 | TF Prefix: "" 95 | Update Interval: 0 96 | Value: true 97 | Visual Enabled: true 98 | - Class: rviz/TF 99 | Enabled: true 100 | Frame Timeout: 15 101 | Frames: 102 | /base_link: 103 | Value: true 104 | /link_1: 105 | Value: true 106 | /link_2: 107 | Value: true 108 | /link_3: 109 | Value: true 110 | /link_4: 111 | Value: true 112 | /link_5: 113 | Value: true 114 | /link_6: 115 | Value: true 116 | /tool0: 117 | Value: true 118 | All Enabled: true 119 | Marker Scale: 1 120 | Name: TF 121 | Show Arrows: true 122 | Show Axes: true 123 | Show Names: true 124 | Tree: 125 | /base_link: 126 | /link_1: 127 | /link_2: 128 | /link_3: 129 | /link_4: 130 | /link_5: 131 | /link_6: 132 | /tool0: 133 | {} 134 | Update Interval: 0 135 | Value: true 136 | Enabled: true 137 | Global Options: 138 | Background Color: 48; 48; 48 139 | Fixed Frame: /base_link 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/Measure 147 | - Class: rviz/SetInitialPose 148 | Topic: /initialpose 149 | - Class: rviz/SetGoal 150 | Topic: /move_base_simple/goal 151 | Value: true 152 | Views: 153 | Current: 154 | Class: rviz/Orbit 155 | Distance: 10 156 | Focal Point: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Name: Current View 161 | Near Clip Distance: 0.01 162 | Pitch: 0.115398 163 | Target Frame: 164 | Value: Orbit (rviz) 165 | Yaw: 0.930399 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 1000 171 | Hide Left Dock: false 172 | Hide Right Dock: false 173 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000003a2fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003a2000000dd00ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f700fffffffb0000000800540069006d00650100000000000004500000000000000000000002a9000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 174 | Selection: 175 | collapsed: false 176 | Time: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: false 182 | Width: 1280 183 | X: 60 184 | Y: 60 185 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/launch/ur10_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/launch/ur5_ft_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/launch/ur5_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/launch/view_ur5.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/launch/view_ur5_ft.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/base.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/forearm.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/shoulder.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/upperarm.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/wrist1.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/wrist2.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/meshes/ur5/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/meshes/ur5/collision/wrist3.stl -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/model.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LWZ55/admittance-controller_UR5/0d83cba143080227ce63ac6b7899ed7976cb7b71/robot_admittance_controller/ur_description/model.pdf -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ur_description 4 | 1.2.7 5 | 6 | URDF description for Universal UR5/10 robot arms 7 | 8 | 9 | Wim Meeussen 10 | Kelsey Hawkins 11 | Mathias Ludtke 12 | Felix Messmer 13 | G.A. vd. Hoorn 14 | Miguel Prada Sarasola 15 | Nadia Hammoudeh Garcia 16 | 17 | BSD 18 | 19 | http://ros.org/wiki/ur_description 20 | 21 | catkin 22 | joint_state_publisher 23 | robot_state_publisher 24 | rviz 25 | urdf 26 | xacro 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/common.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/robotiq_ft300.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | true 8 | 9 | 10 | true 11 | 12 | 13 | true 14 | 15 | 16 | true 17 | 18 | 19 | true 20 | 21 | 22 | true 23 | 24 | 25 | true 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | transmission_interface/SimpleTransmission 8 | 9 | ${hw_interface} 10 | 11 | 12 | 1 13 | 14 | 15 | 16 | 17 | transmission_interface/SimpleTransmission 18 | 19 | ${hw_interface} 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | transmission_interface/SimpleTransmission 28 | 29 | ${hw_interface} 30 | 31 | 32 | 1 33 | 34 | 35 | 36 | 37 | transmission_interface/SimpleTransmission 38 | 39 | ${hw_interface} 40 | 41 | 42 | 1 43 | 44 | 45 | 46 | 47 | transmission_interface/SimpleTransmission 48 | 49 | ${hw_interface} 50 | 51 | 52 | 1 53 | 54 | 55 | 56 | 57 | transmission_interface/SimpleTransmission 58 | 59 | ${hw_interface} 60 | 61 | 62 | 1 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur5.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 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 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur5_ft_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur5_joint_limited_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /robot_admittance_controller/ur_description/urdf/ur5_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | --------------------------------------------------------------------------------