├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── admittance_params.yaml ├── arm_admittance_params_real.yaml ├── cobot_admittance_params.yaml ├── ur10_admittance_params.yaml └── ur10_admittance_params_real.yaml ├── include └── AdmittanceController.h ├── launch ├── arm_admittance_controller.launch ├── cobot_admittance_controller.launch ├── ur10_admittance_controller.launch └── ur10_admittance_controller_real.launch ├── package.xml └── src ├── AdmittanceController.cpp └── admittance_controller_node.cpp /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 11 | roscpp 12 | geometry_msgs 13 | sensor_msgs 14 | tf 15 | tf_conversions 16 | ) 17 | 18 | set(CMAKE_CXX_FLAGS "-O2 -O3 -std=c++11 -Wall") 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # std_msgs # Or other packages containing msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS include 111 | # LIBRARIES cpr_closed_loop_controller 112 | # CATKIN_DEPENDS cartesian_state_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS}) 123 | include_directories(${EIGEN3_INCLUDE_DIR}) 124 | 125 | set(INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include) 126 | set(SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) 127 | 128 | 129 | add_executable(admittance_controller_node 130 | ${SRC_DIR}/admittance_controller_node.cpp 131 | ${SRC_DIR}/AdmittanceController.cpp 132 | ${INCLUDE_DIR}/AdmittanceController.h) 133 | 134 | # add_executable(cpr_record_demo_node 135 | # src/demonstration_recorder_node.cpp) 136 | 137 | 138 | ## Declare a C++ library 139 | 140 | ## Add cmake target dependencies of the library 141 | ## as an example, code may need to be generated before libraries 142 | ## either from message generation or dynamic reconfigure 143 | #add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Declare a C++ executable 146 | ## With catkin_make all packages are built within a single CMake context 147 | ## The recommended prefix ensures that target names across packages don't collide 148 | # add_executable(${PROJECT_NAME}_node src/cpr_closed_loop_controller_node.cpp) 149 | 150 | ## Rename C++ executable without prefix 151 | ## The above recommended prefix causes long target names, the following renames the 152 | ## target back to the shorter version for ease of user use 153 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 154 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 155 | 156 | ## Add cmake target dependencies of the executable 157 | ## same as for the library above 158 | add_dependencies(admittance_controller_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 159 | 160 | ## Specify libraries to link a library or executable target against 161 | target_link_libraries(admittance_controller_node 162 | ${catkin_LIBRARIES} 163 | ) 164 | 165 | 166 | ############# 167 | ## Install ## 168 | ############# 169 | 170 | # all install targets should use catkin DESTINATION variables 171 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 172 | 173 | ## Mark executable scripts (Python etc.) for installation 174 | ## in contrast to setup.py, you can choose the destination 175 | # install(PROGRAMS 176 | # scripts/my_python_script 177 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark executables and/or libraries for installation 181 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 182 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 185 | # ) 186 | 187 | ## Mark cpp header files for installation 188 | # install(DIRECTORY include/${PROJECT_NAME}/ 189 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 190 | # FILES_MATCHING PATTERN "*.h" 191 | # PATTERN ".svn" EXCLUDE 192 | # ) 193 | 194 | ## Mark other files for installation (e.g. launch and bag files, etc.) 195 | # install(FILES 196 | # # myfile1 197 | # # myfile2 198 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 199 | # ) 200 | 201 | ############# 202 | ## Testing ## 203 | ############# 204 | 205 | ## Add gtest based cpp test target and link libraries 206 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_cpr_closed_loop_controller.cpp) 207 | # if(TARGET ${PROJECT_NAME}-test) 208 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 209 | # endif() 210 | 211 | ## Add folders to be run by python nosetests 212 | # catkin_add_nosetests(test) 213 | 214 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # robot_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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config/cobot_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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | // NOTE: This node should be ideally a ROS 35 | // controller, but the current implementation of ROS control in 36 | // indigo requires writing a specific interface for this matter. 37 | // The kinetic version of ROS control enables controllers with 38 | // multiple interfaces (FT sensor, platform and arm), but a proper 39 | // ROS control implementation of this controller remains future work. 40 | // 41 | // USAGE EXAMPLE; 42 | // ros::NodeHandle nh; 43 | // double frequency = 1000.0; 44 | // std::string state_topic_arm, cmd_topic_arm, topic_arm_twist_world, 45 | // topic_wrench_u_e, topic_wrench_u_c, cmd_topic_platform, 46 | // state_topic_platform, wrench_topic, wrench_control_topic, 47 | // laser_front_topic, laser_rear_topic; 48 | // std::vector M_p, M_a, D, D_p, D_a, K, d_e; 49 | // double wrench_filter_factor, force_dead_zone_thres, 50 | // torque_dead_zone_thres, obs_distance_thres, self_detect_thres; 51 | // 52 | // 53 | // // Fill in values 54 | // ... 55 | // 56 | // 57 | // AdmittanceController admittance_controller(nh, frequency, 58 | // cmd_topic_platform, 59 | // state_topic_platform, 60 | // cmd_topic_arm, 61 | // topic_arm_twist_world, 62 | // topic_wrench_u_e, 63 | // topic_wrench_u_c, 64 | // state_topic_arm, 65 | // wrench_topic, 66 | // wrench_control_topic, 67 | // laser_front_topic, 68 | // laser_rear_topic, 69 | // M_p, M_a, D, D_p, D_a, K, d_e, 70 | // wrench_filter_factor, 71 | // force_dead_zone_thres, 72 | // torque_dead_zone_thres, 73 | // obs_distance_thres, 74 | // self_detect_thres); 75 | // admittance_controller.run(); 76 | 77 | using namespace Eigen; 78 | 79 | typedef Matrix Vector7d; 80 | typedef Matrix Vector6d; 81 | typedef Matrix Matrix6d; 82 | 83 | class AdmittanceController 84 | { 85 | protected: 86 | // --- ROS VARIABLES --- // 87 | // A handle to the node in ros 88 | ros::NodeHandle nh_; 89 | // Rate of the run loop 90 | ros::Rate loop_rate_; 91 | 92 | 93 | // --- Subscribers --- // 94 | 95 | // Subscriber for the arm state 96 | ros::Subscriber sub_arm_pose_; 97 | ros::Subscriber sub_arm_pose_arm_; 98 | ros::Subscriber sub_arm_twist_; 99 | // Subscriber for the ft sensor at the end-effector 100 | ros::Subscriber sub_wrench_external_; 101 | // Subscriber for the control input (wrench) 102 | ros::Subscriber sub_wrench_control_; 103 | 104 | 105 | // --- Publishers --- // 106 | // Publisher for the twist of arm endeffector 107 | ros::Publisher pub_arm_cmd_; 108 | 109 | 110 | // --- INPUT SIGNAL --- // 111 | // external wrench (force/torque sensor) in "robotiq_force_torque_frame_id" frame 112 | Vector6d wrench_external_; 113 | // control wrench (from any controller) expected to be in "ur5_arm_base_link" frame 114 | Vector6d wrench_control_; 115 | 116 | 117 | // --- ADMITTANCE PARAMETERS --- // 118 | // M_a_ -> Desired mass of arm 119 | // D_a_ -> Desired damping of arm 120 | Matrix6d M_a_, D_a_; 121 | 122 | // --- OUTPUT COMMANDS --- // 123 | // final arm desired velocity 124 | Vector6d arm_desired_twist_; 125 | 126 | // limiting the workspace of the arm 127 | Vector6d workspace_limits_; 128 | double arm_max_vel_; 129 | double arm_max_acc_; 130 | 131 | // --- STATE VARIABLES -- // 132 | // Arm state: position, orientation, and twist (in "arm_base_link") 133 | Vector3d arm_real_position_; 134 | Quaterniond arm_real_orientation_; 135 | Vector6d arm_real_twist_; 136 | 137 | // extra variables (this can be made more general; i.e. include in yaml file!) 138 | Vector3d arm_real_position_arm_; 139 | Quaterniond arm_real_orientation_arm_; 140 | Vector3d base_position_; 141 | 142 | // End-effector state: pose and twist (in "world" frame) 143 | Vector7d ee_pose_world_; 144 | Vector6d ee_twist_world_; 145 | 146 | // Transform from base_link to world 147 | Matrix6d rotation_base_; 148 | Matrix6d rotation_tool_; 149 | 150 | // TF: 151 | // Listeners 152 | tf::TransformListener listener_ft_; 153 | tf::TransformListener listener_control_; 154 | tf::TransformListener listener_arm_; 155 | 156 | // Guards 157 | bool ft_arm_ready_; 158 | bool arm_world_ready_; 159 | bool world_arm_ready_; 160 | 161 | // Initialization 162 | void wait_for_transformations(); 163 | 164 | // Control 165 | void compute_admittance(); 166 | 167 | // Callbacks 168 | void pose_arm_callback(const geometry_msgs::PoseConstPtr msg); 169 | void pose_arm_arm_callback(const geometry_msgs::PoseConstPtr msg); 170 | void twist_arm_callback(const geometry_msgs::TwistConstPtr msg); 171 | void wrench_external_callback(const geometry_msgs::WrenchStampedConstPtr msg); 172 | void wrench_control_callback(const geometry_msgs::WrenchStampedConstPtr msg); 173 | 174 | 175 | // Util 176 | bool get_rotation_matrix(Matrix6d & rotation_matrix, 177 | tf::TransformListener & listener, 178 | std::string from_frame, std::string to_frame, bool getT); 179 | 180 | void limit_to_workspace(); 181 | 182 | // void publish_debuggings_signals(); 183 | 184 | void send_commands_to_robot(); 185 | 186 | 187 | public: 188 | AdmittanceController(ros::NodeHandle &n, double frequency, 189 | std::string topic_arm_pose, 190 | std::string topic_arm_twist, 191 | std::string wrench_external_topic, 192 | std::string wrench_control_topic, 193 | std::string cmd_topic_arm, 194 | std::vector M_a, 195 | std::vector D_a, 196 | std::vector workspace_limits, 197 | double arm_max_vel, 198 | double arm_max_acc); 199 | void run(); 200 | }; 201 | 202 | #endif // ADMITTANCECONTROLLER_H 203 | 204 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /launch/cobot_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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | tf 32 | cmake_modules 33 | eigen 34 | 35 | sensor_msgs 36 | geometry_msgs 37 | tf 38 | eigen 39 | catkin 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------