├── .gitattributes ├── README.md ├── ati_sensor ├── CMakeLists.txt ├── include │ └── ft_sensor.h ├── launch │ └── ft_sensor.launch ├── package.xml └── src │ ├── ft_sensor.cpp │ └── ft_sensor_node.cpp ├── ur5_admittance_control ├── CMakeLists.txt ├── config │ └── admittance_params.yaml ├── include │ └── AdmittanceController.h ├── launch │ └── admittance_controller.launch ├── msg │ └── PoseTwist.msg ├── package.xml └── src │ ├── AdmittanceController.cpp │ └── admittance_controller_node.cpp ├── ur5_bringup ├── CMakeLists.txt ├── config │ └── initial_pose.yaml ├── controllers │ ├── joint_state_controller.yaml │ └── ur5_vel_controller.yaml ├── launch │ ├── init_pose_start_controllers.launch │ ├── robot_bringup.launch │ ├── ur5_bringup.launch │ └── ur_force_control.launch ├── package.xml ├── rviz │ └── admittance_fake_wrench.rviz ├── src │ ├── fake_wrench.py │ ├── init_pose_start_controllers.py │ └── timed_roslaunch.sh ├── urdf │ ├── ur5.urdf.xacro │ └── ur5_description.urdf.xacro └── worlds │ ├── bifurcation.world │ └── empty.world └── ur5_cartesian_velocity_control ├── CMakeLists.txt ├── config └── controllers.yaml ├── include └── ur5_cartesian_velocity_control │ ├── cartesian_state_controller.h │ ├── cartesian_velocity_controller.h │ └── kinematic_chain_controller_base.h ├── launch └── load_controller.launch ├── package.xml ├── src ├── cartesian_state_controller.cpp └── cartesian_velocity_controller.cpp └── ur5_cartesian_velocity_control_plugin.xml /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Compliation and build 2 | 3 | You also need the following ros packages 4 | ```bash 5 | $ sudo apt-get install ros-indigo-ridgeback-* 6 | $ sudo apt-get install ros-indigo-universal-robot 7 | ``` 8 | if you are getting error for broken packages (most probably due to a wrong version of gazebo), you can use 'aptitude' instead of 'apt-get' which propose a solution and resolve the conflict. 9 | 10 | 11 | Finally complie 12 | ```bash 13 | $ cd ~/catkin_ws 14 | $ catkin_make 15 | $ source devel/setup.bash 16 | $ catkin_make 17 | ``` 18 | * you might need the source the bash file and compie again if the first compliation could not find some of in house dependencies. 19 | 20 | 21 | For simulator, you can use gazebo7 22 | ```bash 23 | $ sudo apt-get install ros-indigo-gazebo7-* 24 | ``` 25 | You might need to follow [these instructions](http://gazebosim.org/tutorials?tut=install_ubuntu#Alternativeinstallation:step-by-step). 26 | 27 | --- 28 | --- 29 | 30 | 31 | 32 | # Control Architecture 33 | 34 | ## Kinematics and transformations 35 | 36 | Here is a short list of important frames and their usage. 37 | 38 | | frame id | Usage | 39 | |---------------|-----------------------------------| 40 | | world | Odometry and navigation | 41 | | ur5_arm_base_link | Arm pose and twist | 42 | | base_link | Platform pose and twist | 43 | | robotiq_force_torque_frame_id | External force applied to the end-effector | 44 | 45 | 46 | 47 | 48 | 49 | ## Adamittance dynamics 50 | The following figure shows the controller architecture for the admittance control on the robot. 51 | 52 | ![alt text](fig_control_schematics.png "Control architecture") 53 | 54 | The two equations in the center describe the admittance dynamics which compute the desired accelaration for the arm and the platform. These accelerations are integrated in time to acheive the desired velocities for the robot. The low-level velocity controller fullfills these velocities. In the case of platform, the computed velocities can be modified accroding to obstacle avoidance node. 55 | 56 | 57 | 58 | The admittance parameters (shown in purple) are as follows: 59 | 60 | | Variable | Parameter | 61 | |---------------|-----------------------------------| 62 | | Ma | Desired mass of the arm | 63 | | Da | Desired damping of the arm | 64 | | Dc | Desired damping of the coupling | 65 | | Kc | Desired Stiffness of the coupling | 66 | | Mp | Desired mass of the platform | 67 | | Dp | Desired damping of the platform | 68 | 69 | These parameters are load from a yaml through the launch file. 70 | 71 | 72 | ### External force 73 | The external is initially measured by the force/torque sensor in its own frame reference. In admittance controller this force is transformed to "ur5_arm_base_link" where the control of arm takes place. To avoid reacting to small forces a deadzone is considered. Moreover, a low-pass filter is used to smooth the measurements. The parameters of the deadzone and the filter can be set from the launch file. 74 | 75 | ### Higher-level control (Motion planning ) 76 | Through a higher level controller, the position of the equilibrium can be can be changed to acheive a desired behavior. Also, Fc can be used for control purposes; e.g., to compensate for the mass of an object carried by the arm. 77 | 78 | [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/7BjHhV-BkwE/0.jpg)](https://youtu.be/7BjHhV-BkwE) 79 | 80 | 81 | 82 | # 这里面的用的controller其实和UR之前下载的controller没有什么本质的区别,仅仅是控制模式不一样而已,一个是velocity,而另一个是joint_trajectory,以后需要用到cartesian space的速度可以考虑用这个包! 83 | 84 | 85 | # 仿真程序 86 | roslaunch ur5_admittance_control admittance_controller.launch 87 | 88 | # 真实程序 89 | roslaunch ur5_admittance_control admittance_controller.launch sim:=false F/T_sensor:=true 90 | 91 | # 安装错误 92 | 因为这个包很多ur的东西,所以很容易产生错误，如果错误无法解决，controller之类的错误，就把catkin_ws环境下的其他包移除，只留force_control的几个包然后，catkin_make就可以了 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /ati_sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.2) 2 | project(ati_sensor) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_srvs 7 | std_msgs 8 | geometry_msgs 9 | rospy 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS geometry_msgs std_msgs 14 | INCLUDE_DIRS include 15 | LIBRARIES ati_sensor 16 | ) 17 | 18 | include_directories(include 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | add_library(ati_sensor src/ft_sensor.cpp) 23 | 24 | find_package(LibXml2 REQUIRED) 25 | include_directories(${LIBXML2_INCLUDE_DIR}) 26 | target_link_libraries(ati_sensor ${LIBXML2_LIBRARIES} ${XENOMAI_RTDM_LIBRARIES}) 27 | 28 | add_executable(ft_sensor_node src/ft_sensor_node.cpp) 29 | target_link_libraries(ft_sensor_node ati_sensor ${catkin_LIBRARIES}) 30 | 31 | -------------------------------------------------------------------------------- /ati_sensor/include/ft_sensor.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define MAX_XML_SIZE 35535 18 | #define RDT_RECORD_SIZE 36 19 | 20 | namespace ati{ 21 | static const std::string default_ip = "192.168.1.1"; 22 | static const int current_calibration=-1; 23 | // 数据相应结构体 24 | typedef struct response_struct { 25 | uint32_t rdt_sequence; 26 | uint32_t ft_sequence; 27 | uint32_t status; 28 | int32_t Fx; 29 | int32_t Fy; 30 | int32_t Fz; 31 | int32_t Tx; 32 | int32_t Ty; 33 | int32_t Tz; 34 | uint32_t cpt; 35 | uint32_t cpf; 36 | } response_s; 37 | 38 | // 发送指令 39 | typedef struct command_struct{ 40 | static const uint16_t command_header = 0x1234; 41 | uint16_t command; 42 | uint32_t sample_count; 43 | static const uint16_t STOP = 0x0000; 44 | static const uint16_t REALTIME = 0x0002; 45 | static const uint16_t BUFFERED = 0x0003; 46 | static const uint16_t MULTIUNIT = 0x0004; 47 | static const uint16_t RESET_THRESHOLD_LATCH = 0x0041; 48 | static const uint16_t SET_SOFWARE_BIAS = 0x0042; 49 | static const int DEFAULT_PORT = 49152; 50 | } command_s; 51 | 52 | class FTSensor{ 53 | public: 54 | FTSensor(); 55 | ~FTSensor(); 56 | 57 | enum settings_error_t 58 | { 59 | NO_SETTINGS_ERROR, 60 | SETTINGS_REQUEST_ERROR, 61 | CALIB_PARSE_ERROR, 62 | GAUGE_PARSE_ERROR, 63 | RDTRATE_PARSE_ERROR 64 | }; 65 | 66 | // 初始化,从XML中读取配置参数 67 | bool init(std::string ip, int calibration_index = ati::current_calibration, 68 | uint16_t cmd = ati::command_s::REALTIME, int sample_count = -1); 69 | 70 | // 读取力和力矩counts数据值 71 | const double getCountsperForce(){return resp_.cpf;}; 72 | const double getCountsperTorque(){return resp_.cpt;}; 73 | 74 | // 读取真实数据值 75 | template 76 | void getMeasurements(T measurements[6]) 77 | { 78 | doComm(); 79 | 80 | measurements[0]=static_cast( resp_.Fx ) / static_cast(resp_.cpf); 81 | measurements[1]=static_cast( resp_.Fy ) / static_cast(resp_.cpf); 82 | measurements[2]=static_cast( resp_.Fz ) / static_cast(resp_.cpf); 83 | 84 | measurements[3]=static_cast( resp_.Tx ) / static_cast(resp_.cpt); 85 | measurements[4]=static_cast( resp_.Ty ) / static_cast(resp_.cpt); 86 | measurements[5]=static_cast( resp_.Tz ) / static_cast(resp_.cpt); 87 | } 88 | 89 | 90 | template 91 | void getMeasurements(T measurements[6],uint32_t& rdt_sequence) 92 | { 93 | getMeasurements(measurements); 94 | rdt_sequence = resp_.rdt_sequence; 95 | } 96 | 97 | template 98 | void getMeasurements(T measurements[6],uint32_t& rdt_sequence,uint32_t& ft_sequence) 99 | { 100 | getMeasurements(measurements,rdt_sequence); 101 | ft_sequence = resp_.ft_sequence; 102 | } 103 | 104 | // 写入IP地址 105 | const std::string getIP(){return this->ip;} 106 | const uint16_t getPort(){return this->port;} 107 | std::string message_header(){ 108 | std::stringstream ss; 109 | ss << "[ft_sensor " << this->ip << ":" << this->port << "] "; 110 | return ss.str(); 111 | } 112 | 113 | // 数据传输频率 114 | const int getRDTRate(){return this->rdt_rate_;} 115 | 116 | // 归零 117 | void setBias(); 118 | void setTimeout(float sec); 119 | bool isInitialized(); 120 | bool getCalibrationData(); 121 | settings_error_t getSettings(); 122 | bool setRDTOutputRate(unsigned int rate); 123 | std::vector getGaugeBias(); 124 | bool setGaugeBias(unsigned int gauge_idx, int gauge_bias); 125 | bool setGaugeBias(std::map &gauge_map); 126 | bool setGaugeBias(std::vector &gauge_vect); 127 | 128 | 129 | protected: 130 | // 套接字信息 131 | bool startRealTimeStreaming(uint32_t sample_count=1); 132 | bool startBufferedStreaming(uint32_t sample_count=100); 133 | bool startMultiUnitStreaming(uint32_t sample_count=100); 134 | bool resetThresholdLatch(); 135 | bool setSoftwareBias(); 136 | bool stopStreaming(); 137 | bool startStreaming(); 138 | bool startStreaming(int nb_samples); 139 | bool openSockets(); 140 | void openSocket(int& handle, const std::string ip, const uint16_t port, const int option); 141 | bool closeSockets(); 142 | int closeSocket(const int& handle); 143 | void setCommand(uint16_t cmd); 144 | void setSampleCount(uint32_t sample_count); 145 | bool sendCommand(); 146 | bool sendCommand(uint16_t cmd); 147 | bool getResponse(); 148 | bool sendTCPrequest(std::string &request_cmd); 149 | void doComm(); 150 | std::string ip; 151 | uint16_t port; 152 | int calibration_index; 153 | int rdt_rate_; 154 | int *setbias_; 155 | int socketHandle_; 156 | int socketHTTPHandle_; 157 | struct sockaddr_in addr_; 158 | socklen_t addr_len_; 159 | struct hostent *hePtr_; 160 | 161 | // 通信协议 162 | response_s resp_; 163 | command_s cmd_; 164 | unsigned char request_[8]; 165 | unsigned char response_[RDT_RECORD_SIZE]; 166 | bool initialized_; 167 | bool timeout_set_; 168 | struct timeval timeval_; 169 | int response_ret_; 170 | char xml_c_[MAX_XML_SIZE]; 171 | std::string xml_s_; 172 | 173 | }; 174 | } 175 | -------------------------------------------------------------------------------- /ati_sensor/launch/ft_sensor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ati_sensor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ati_sensor 3 | 1.1.0 4 | The package for FT sensors using the ATI net box. 5 | 6 | Antoine Hoarau 7 | 8 | CeCILL-C 9 | 10 | catkin 11 | roscpp 12 | rospy 13 | std_srvs 14 | std_msgs 15 | geometry_msgs 16 | tf 17 | libxml2 18 | 19 | roscpp 20 | libxml2 21 | std_srvs 22 | geometry_msgs 23 | std_msgs 24 | tf 25 | rospy 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ati_sensor/src/ft_sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "ft_sensor.h" 2 | #include 3 | #include 4 | 5 | // XML 相关的库 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define rt_dev_socket socket 15 | #define rt_dev_setsockopt setsockopt 16 | #define rt_dev_bind bind 17 | #define rt_dev_recvfrom recvfrom 18 | #define rt_dev_sendto sendto 19 | #define rt_dev_close close 20 | #define rt_dev_connect connect 21 | #define rt_dev_recv recv 22 | #define rt_dev_send send 23 | #define RT_SO_TIMEOUT SO_RCVTIMEO 24 | 25 | static std::string getStringInXml(const std::string& xml_s,const std::string& tag) 26 | { 27 | const std::string tag_open = "<"+tag+">"; 28 | const std::string tag_close = ""; 29 | const std::size_t n_start = xml_s.find(tag_open); 30 | const std::size_t n_end = xml_s.find(tag_close); 31 | return xml_s.substr(n_start+tag_open.length(),n_end); 32 | } 33 | template 34 | static T getNumberInXml(const std::string& xml_s,const std::string& tag) 35 | { 36 | const std::string num = getStringInXml(xml_s,tag); 37 | double r = ::atof(num.c_str()); 38 | return static_cast(r); 39 | } 40 | template 41 | static bool getArrayFromString(const std::string& str,const char delim,T *data,size_t len) 42 | { 43 | size_t start = str.find_first_not_of(delim), end=start; 44 | size_t idx = 0; 45 | while (start != std::string::npos && idx < len){ 46 | end = str.find(delim, start); 47 | std::string token = str.substr(start, end-start); 48 | if (token.empty()) 49 | token = "0.0"; 50 | double r = ::atof(token.c_str()); 51 | data[idx] = static_cast(r); 52 | ++idx; 53 | start = str.find_first_not_of(delim, end); 54 | } 55 | return (idx == len); 56 | } 57 | template 58 | static bool getArrayFromXml(const std::string& xml_s,const std::string& tag,const char delim,T *data,size_t len) 59 | { 60 | const std::string str = getStringInXml(xml_s,tag); 61 | return getArrayFromString(str,delim,data,len); 62 | } 63 | 64 | 65 | using namespace ati; 66 | 67 | FTSensor::FTSensor() 68 | { 69 | // 初始化参数 70 | initialized_ = false; 71 | ip = ati::default_ip; 72 | port = command_s::DEFAULT_PORT; 73 | cmd_.command = command_s::STOP; 74 | cmd_.sample_count = 1; 75 | calibration_index = ati::current_calibration; 76 | socketHandle_ = -1; 77 | resp_.cpf = 1000000; 78 | resp_.cpt = 1000000; 79 | rdt_rate_ = 0; 80 | timeval_.tv_sec = 2; 81 | timeval_.tv_usec = 0; 82 | xml_s_.reserve(MAX_XML_SIZE); 83 | setbias_ = new int[6]; 84 | } 85 | 86 | FTSensor::~FTSensor() 87 | { 88 | stopStreaming(); 89 | if(!closeSockets()) 90 | std::cerr << message_header() << "Sensor did not shutdown correctly" << std::endl; 91 | delete setbias_; 92 | } 93 | 94 | bool FTSensor::startStreaming(int nb_samples) 95 | { 96 | if (nb_samples < 0) { 97 | // use default sample_count 98 | return startStreaming(); 99 | } 100 | else { 101 | // use given sample count 102 | uint32_t sample_count = static_cast(nb_samples); 103 | switch(cmd_.command){ 104 | case command_s::REALTIME: 105 | //std::cout << "Starting realtime streaming" << std::endl; 106 | return startRealTimeStreaming(sample_count); 107 | case command_s::BUFFERED: 108 | //std::cout << "Starting buffered streaming" << std::endl; 109 | return startBufferedStreaming(sample_count); 110 | case command_s::MULTIUNIT: 111 | //std::cout << "Starting multi-unit streaming" << std::endl; 112 | return startMultiUnitStreaming(sample_count); 113 | default: 114 | std::cout << message_header() << cmd_.command << ": command mode not allowed" << std::endl; 115 | return false; 116 | } 117 | } 118 | } 119 | 120 | // Initialization read from XML file 121 | bool FTSensor::startStreaming() 122 | { 123 | switch(cmd_.command){ 124 | case command_s::REALTIME: 125 | //std::cout << "Starting realtime streaming" << std::endl; 126 | return startRealTimeStreaming(); 127 | case command_s::BUFFERED: 128 | //std::cout << "Starting buffered streaming" << std::endl; 129 | return startBufferedStreaming(); 130 | case command_s::MULTIUNIT: 131 | //std::cout << "Starting multi-unit streaming" << std::endl; 132 | return startMultiUnitStreaming(); 133 | default: 134 | std::cout << message_header() << cmd_.command<< ": command mode not allowed" << std::endl; 135 | return false; 136 | } 137 | } 138 | 139 | bool FTSensor::init(std::string ip, int calibration_index, uint16_t cmd, int sample_count) 140 | { 141 | // Re-Initialize parameters 142 | initialized_ = true; 143 | this->ip = ip; 144 | this->port = command_s::DEFAULT_PORT; 145 | cmd_.command = command_s::STOP; 146 | cmd_.sample_count = 1; 147 | this->calibration_index = calibration_index; 148 | 149 | // Open Socket 150 | if(!ip.empty() && openSockets()) 151 | { 152 | if(!stopStreaming()) // if previously launched 153 | std::cerr << "\033[33m" << message_header() << "Could not stop streaming\033[0m" << std::endl; 154 | setCommand(cmd); // Setting cmd mode 155 | 156 | initialized_ &= startStreaming(sample_count); // Starting streaming 157 | 158 | if (!initialized_) 159 | { 160 | std::cerr << "\033[1;31m" << message_header() << "Could not start streaming\033[0m" << std::endl; 161 | return initialized_; 162 | } 163 | initialized_ &= getResponse(); 164 | 165 | // Parse Calibration from web server 166 | if(initialized_) 167 | getCalibrationData(); 168 | }else 169 | initialized_ = false; 170 | 171 | if (!initialized_) 172 | std::cerr << "\033[1;31m" << message_header() << "Error during initialization, FT sensor NOT started\033[0m" << std::endl; 173 | 174 | return initialized_; 175 | } 176 | bool FTSensor::openSockets() 177 | { 178 | try{ 179 | // To get the online configuration (need to build rtnet with TCP option) 180 | openSocket(socketHTTPHandle_,getIP(),80,IPPROTO_TCP); 181 | // The data socket 182 | openSocket(socketHandle_,getIP(),getPort(),IPPROTO_UDP); 183 | } 184 | catch (std::exception &ex) { 185 | std::cerr << "\033[1;31m" << message_header() << "openSockets error: " << ex.what() <<"\033[0m" << std::endl; 186 | return false; 187 | } 188 | return true; 189 | } 190 | void FTSensor::openSocket(int& handle,const std::string ip,const uint16_t port,const int option) 191 | { 192 | // create the socket 193 | if (handle != -1) 194 | rt_dev_close(handle); 195 | std::string proto = ""; 196 | if(option == IPPROTO_UDP) 197 | { 198 | proto = "UDP"; 199 | handle = rt_dev_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 200 | } 201 | else if(option == IPPROTO_TCP) 202 | { 203 | proto = "TCP"; 204 | handle = rt_dev_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 205 | } 206 | else 207 | handle = rt_dev_socket(AF_INET, SOCK_DGRAM, 0); 208 | 209 | if (handle < 0) { 210 | std::cerr << "\033[31m" << message_header() << "Could not init sensor socket for proto [" << proto 211 | << "], please make sure your can ping the sensor\033[0m" << std::endl; 212 | throw std::runtime_error("failed to init sensor socket"); 213 | } 214 | 215 | // re-use address in case it's still binded 216 | rt_dev_setsockopt(handle, SOL_SOCKET, SO_REUSEADDR, 0, 0); 217 | 218 | // set the socket parameters 219 | struct sockaddr_in addr = {0}; 220 | hostent * hePtr = NULL; 221 | hePtr = gethostbyname(ip.c_str()); 222 | memcpy(&addr.sin_addr, hePtr->h_addr_list[0], hePtr->h_length); 223 | 224 | //addr_.sin_addr.s_addr = INADDR_ANY; 225 | addr.sin_family = AF_INET; 226 | addr.sin_port = htons(port); 227 | 228 | // connect 229 | if (rt_dev_connect(handle, (struct sockaddr*) &addr, sizeof(addr)) < 0) 230 | { 231 | std::cerr << "\033[31m" << message_header() << "Could not connect to " << ip << "\033[0m" << std::endl ; 232 | throw std::runtime_error("failed to connect to socket" ); 233 | } 234 | return; 235 | } 236 | bool FTSensor::closeSockets() 237 | { 238 | return closeSocket(socketHandle_) == 0 && closeSocket(socketHTTPHandle_) == 0; 239 | } 240 | int FTSensor::closeSocket(const int& handle) 241 | { 242 | if(handle < 0) 243 | return 0; // closing a non-open socket is considered successful 244 | return rt_dev_close(handle); 245 | } 246 | 247 | bool FTSensor::getCalibrationData() 248 | { 249 | FTSensor::settings_error_t err = getSettings(); 250 | if(err!=SETTINGS_REQUEST_ERROR && err!=CALIB_PARSE_ERROR) 251 | { 252 | std::cout << message_header() << "Sucessfully retrieved counts per force : " << resp_.cpf << std::endl; 253 | std::cout << message_header() << "Sucessfully retrieved counts per torque : " << resp_.cpt << std::endl; 254 | } 255 | else 256 | { 257 | std::cerr << message_header() << "Using default counts per force : " << resp_.cpf << std::endl; 258 | std::cerr << message_header() << "Using default counts per torque : " << resp_.cpt << std::endl; 259 | } 260 | } 261 | 262 | FTSensor::settings_error_t FTSensor::getSettings() 263 | { 264 | std::string index(""); 265 | if(calibration_index != ati::current_calibration) 266 | { 267 | std::stringstream ss; 268 | ss << calibration_index; 269 | index = "?index=" + ss.str(); 270 | std::cout << message_header() << "Using calibration index "<(xml_s_,"cfgcpf"); 301 | const uint32_t cfgcpt_r = getNumberInXml(xml_s_,"cfgcpt"); 302 | const int cfgcomrdtrate = getNumberInXml(xml_s_,"comrdtrate"); 303 | rdt_rate_ = cfgcomrdtrate; 304 | 305 | // 6 tokens separated by semi-colon 306 | if (!getArrayFromXml(xml_s_,"setbias",';',setbias_, 6)) 307 | { 308 | return GAUGE_PARSE_ERROR; 309 | } 310 | 311 | if(cfgcpf_r && cfgcpt_r) 312 | { 313 | resp_.cpf = cfgcpf_r; 314 | resp_.cpt = cfgcpt_r; 315 | return NO_SETTINGS_ERROR; 316 | } 317 | std::cerr << message_header() << "Could not parse file " << filename << std::endl; 318 | return SETTINGS_REQUEST_ERROR; 319 | } 320 | 321 | bool FTSensor::sendTCPrequest(std::string &request_cmd) 322 | { 323 | if (request_cmd.empty() ) 324 | { 325 | std::cerr << message_header() << "Empty TCP command, not sending" << std::endl; 326 | return false; 327 | } 328 | else 329 | { 330 | static const uint32_t chunkSize = 4; // Every chunk of data will be of this size 331 | static const uint32_t maxSize = 65536; // The maximum file size to receive 332 | std::string host = getIP(); 333 | 334 | std::string request_s = "GET "+request_cmd+" HTTP/1.0\r\nHost: "+host+"\r\n\r\n"; 335 | 336 | if (rt_dev_send(socketHTTPHandle_, request_s.c_str(),request_s.length(), 0) < 0) 337 | { 338 | std::cerr << message_header() << "Could not send GET request to " << host 339 | << ":80. Please make sure that RTnet TCP protocol is installed" << std::endl; 340 | return false; 341 | } 342 | 343 | //empty the buffer but we don't care about the result 344 | int recvLength=0; 345 | int posBuff = 0; 346 | while(posBuff < maxSize) // Just a security to avoid infinity loop 347 | { 348 | recvLength = rt_dev_recv(socketHTTPHandle_, &xml_c_[posBuff],chunkSize, 0); 349 | posBuff += recvLength; 350 | if(recvLength <= 0) // The last chunk returns 0 351 | break; 352 | } 353 | if (posBuff > 4) 354 | { 355 | const char *awaited_response = "HTTP/1.0 302 Found"; 356 | if (strncmp(xml_c_, awaited_response, 18 )==0) 357 | { 358 | return true; 359 | } 360 | else 361 | { 362 | std::cerr << message_header() << "Bad response from set command. Response is :" << xml_c_ << std::endl; 363 | return false; 364 | } 365 | } 366 | else 367 | { 368 | std::cerr << message_header() << "Bad response from set command. Response is :" << xml_c_ << std::endl; 369 | return false; 370 | } 371 | } 372 | } 373 | 374 | 375 | bool FTSensor::setRDTOutputRate(unsigned int rate) 376 | { 377 | if (rate > 0 && rate <= 7000) 378 | { 379 | std::stringstream cfgcomrdtrate_ss; 380 | cfgcomrdtrate_ss << rate; 381 | std::string cmd = "/comm.cgi?comrdtrate=" + cfgcomrdtrate_ss.str(); 382 | 383 | if(sendTCPrequest(cmd)) 384 | { 385 | // we consider the rate was set and don't read it back 386 | rdt_rate_ = rate; 387 | return true; 388 | } 389 | else 390 | return false; 391 | } 392 | else 393 | { 394 | std::cerr << message_header() << "RDT rate must be in range [1-7000]" << std::endl; 395 | return false; 396 | } 397 | } 398 | 399 | 400 | bool FTSensor::setGaugeBias(unsigned int gauge_idx, int gauge_bias) 401 | { 402 | std::map map; 403 | map[gauge_idx] = gauge_bias; 404 | return setGaugeBias(map); 405 | } 406 | 407 | std::vector FTSensor::getGaugeBias() 408 | { 409 | FTSensor::settings_error_t err = getSettings(); 410 | if(err!=SETTINGS_REQUEST_ERROR && err!=GAUGE_PARSE_ERROR) 411 | { 412 | std::vector bias(setbias_, setbias_ + 6); 413 | return bias; 414 | } 415 | else 416 | { 417 | std::cerr << message_header() << "Could not get gauge bias values" << std::endl; 418 | return std::vector(); 419 | } 420 | } 421 | 422 | 423 | bool FTSensor::setGaugeBias(std::vector &gauge_vect) 424 | { 425 | std::map map; 426 | for (size_t i=0; i < gauge_vect.size(); ++i) 427 | { 428 | map[i] = gauge_vect[i]; 429 | } 430 | return setGaugeBias(map); 431 | } 432 | 433 | bool FTSensor::setGaugeBias(std::map &gauge_map) 434 | { 435 | std::stringstream setbias_ss; 436 | std::map::iterator it; 437 | bool first_element = true; 438 | //prepare the query 439 | for (it=gauge_map.begin(); it!=gauge_map.end(); ++it) 440 | { 441 | if( it->first < 6) 442 | { 443 | if(first_element) 444 | { 445 | setbias_ss << "?"; 446 | first_element = false; 447 | } 448 | else 449 | { 450 | setbias_ss << "&"; 451 | } 452 | setbias_ss << "setbias" << it->first << "=" << it->second; 453 | } 454 | else 455 | { 456 | std::cerr << message_header() << "Invalid gauge number "<< it->first << std::endl; 457 | return false; 458 | } 459 | } 460 | 461 | std::string host = getIP(); 462 | std::string cmd = "/setting.cgi" + setbias_ss.str(); 463 | return sendTCPrequest(cmd); 464 | } 465 | 466 | bool FTSensor::sendCommand() 467 | { 468 | return sendCommand(cmd_.command); 469 | } 470 | 471 | bool FTSensor::sendCommand(uint16_t cmd) 472 | { 473 | *reinterpret_cast(&request_[0]) = htons(command_s::command_header); 474 | *reinterpret_cast(&request_[2]) = htons(cmd); 475 | *reinterpret_cast(&request_[4]) = htonl(cmd_.sample_count); 476 | //return rt_dev_sendto(socketHandle_, (void*) &request_, sizeof(request_), 0, (sockaddr*) &addr_, addr_len_ ) == 8; 477 | return rt_dev_send(socketHandle_, (void*) &request_, sizeof(request_), 0) == sizeof(request_);//, (sockaddr*) &addr_, addr_len_ ) == 8; 478 | } 479 | 480 | bool FTSensor::getResponse() 481 | { 482 | 483 | //response_ret_ = rt_dev_recvfrom(socketHandle_, (void*) &response_, sizeof(response_), 0, (sockaddr*) &addr_, &addr_len_ ); 484 | response_ret_ = rt_dev_recv(socketHandle_, (void*) &response_, sizeof(response_), 0);//, (sockaddr*) &addr_, &addr_len_ ); 485 | resp_.rdt_sequence = ntohl(*reinterpret_cast(&response_[0])); 486 | resp_.ft_sequence = ntohl(*reinterpret_cast(&response_[4])); 487 | resp_.status = ntohl(*reinterpret_cast(&response_[8])); 488 | resp_.Fx = static_cast(ntohl(*reinterpret_cast(&response_[12 + 0 * 4]))); 489 | resp_.Fy = static_cast(ntohl(*reinterpret_cast(&response_[12 + 1 * 4]))); 490 | resp_.Fz = static_cast(ntohl(*reinterpret_cast(&response_[12 + 2 * 4]))); 491 | resp_.Tx = static_cast(ntohl(*reinterpret_cast(&response_[12 + 3 * 4]))); 492 | resp_.Ty = static_cast(ntohl(*reinterpret_cast(&response_[12 + 4 * 4]))); 493 | resp_.Tz = static_cast(ntohl(*reinterpret_cast(&response_[12 + 5 * 4]))); 494 | if (response_ret_ < 0) 495 | { 496 | std::cerr << "\033[1;31m" << message_header() << "Error while receiving: " << strerror(errno) << "\033[0m" << std::endl; 497 | } 498 | if (response_ret_!=RDT_RECORD_SIZE) 499 | std::cerr << message_header() << "Error of package.xml size " <setSoftwareBias(); 519 | } 520 | bool FTSensor::isInitialized() 521 | { 522 | return initialized_; 523 | } 524 | 525 | void FTSensor::setTimeout(float sec) 526 | { 527 | if (sec <= 0) { 528 | std::cerr << message_header() << "Can't set timeout <= 0 sec" << std::endl; 529 | return; 530 | } 531 | 532 | if (isInitialized()) { 533 | std::cerr << message_header() << "Can't set timeout if socket is initialized, call this before init()." << std::endl; 534 | return; 535 | } 536 | timeval_.tv_sec = static_cast(sec); 537 | timeval_.tv_usec = static_cast(sec/1.e6); 538 | } 539 | 540 | bool FTSensor::resetThresholdLatch() 541 | { 542 | if(! sendCommand(command_s::RESET_THRESHOLD_LATCH)){ 543 | std::cerr << message_header() << "Could not start reset threshold latch" << std::endl; 544 | return false; 545 | } 546 | return true; 547 | } 548 | bool FTSensor::setSoftwareBias() 549 | { 550 | //if(!stopStreaming()) 551 | //std::cerr << "Could not stop streaming" << std::endl; 552 | if(! sendCommand(command_s::SET_SOFWARE_BIAS)){ 553 | ;//std::cerr << "Could not set software bias" << std::endl; 554 | return false; 555 | } 556 | //if(!startStreaming()) 557 | //std::cerr << "Could not restart streaming" << std::endl; 558 | return true; 559 | } 560 | bool FTSensor::stopStreaming() 561 | { 562 | return sendCommand(command_s::STOP); 563 | } 564 | 565 | bool FTSensor::startBufferedStreaming(uint32_t sample_count) 566 | { 567 | setSampleCount(sample_count); 568 | setCommand(command_s::BUFFERED); 569 | if(! sendCommand()){ 570 | std::cerr << message_header() << "Could not start buffered streaming" << std::endl; 571 | return false; 572 | } 573 | return true; 574 | } 575 | bool FTSensor::startMultiUnitStreaming(uint32_t sample_count) 576 | { 577 | setSampleCount(sample_count); 578 | setCommand(command_s::MULTIUNIT); 579 | if(! sendCommand()){ 580 | std::cerr << message_header() << "Could not start multi-unit streaming" << std::endl; 581 | return false; 582 | } 583 | return true; 584 | } 585 | bool FTSensor::startRealTimeStreaming(uint32_t sample_count) 586 | { 587 | setSampleCount(sample_count); 588 | setCommand(command_s::REALTIME); 589 | if(! sendCommand()){ 590 | std::cerr << message_header() << "Could not start realtime streaming" << std::endl; 591 | return false; 592 | } 593 | std::cout << message_header() << "Start realtime streaming with " << sample_count <<" samples " << std::endl; 594 | return true; 595 | } 596 | 597 | void FTSensor::setCommand(uint16_t cmd) 598 | { 599 | this->cmd_.command = cmd; 600 | } 601 | 602 | void FTSensor::setSampleCount(uint32_t sample_count) 603 | { 604 | this->cmd_.sample_count = sample_count; 605 | } 606 | 607 | -------------------------------------------------------------------------------- /ati_sensor/src/ft_sensor_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "ft_sensor.h" 13 | 14 | int count = 0; 15 | FILE *f; 16 | 17 | 18 | namespace ftsensor { 19 | 20 | class FTSensorPublisher 21 | { 22 | private: 23 | //! The node handle 24 | ros::NodeHandle nh_; 25 | //! Node handle in the private namespace 26 | ros::NodeHandle priv_nh_; 27 | 28 | //! The sensor 29 | boost::shared_ptr ftsensor_; 30 | std::string ip_; 31 | std::string frame_ft_; 32 | 33 | //! Publisher for sensor readings 34 | ros::Publisher pub_sensor_readings_; 35 | 36 | //! Service for setting the bias 37 | ros::ServiceServer srv_set_bias_; 38 | 39 | public: 40 | //------------------ Callbacks ------------------- 41 | // Callback for setting bias 42 | bool setBiasCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response); 43 | 44 | // Publish the measurements 45 | void publishMeasurements(); 46 | 47 | //! Subscribes to and advertises topics 48 | FTSensorPublisher(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") 49 | { 50 | priv_nh_.param("frame", frame_ft_, "/ati_ft_link"); 51 | priv_nh_.param("ip", ip_, "192.168.1.1"); 52 | 53 | ROS_INFO_STREAM("ATISensor IP : "<< ip_); 54 | ROS_INFO_STREAM("ATISensor frame : "<< frame_ft_); 55 | 56 | // Create a new sensor 57 | ftsensor_ = boost::shared_ptr(new ati::FTSensor()); 58 | 59 | // Init FT Sensor 60 | if (ftsensor_->init(ip_)) 61 | { 62 | // Set bias 63 | ftsensor_->setBias(); 64 | 65 | ROS_INFO_STREAM("ATISensor RDT Rate : "<< ftsensor_->getRDTRate()); 66 | 67 | // Advertise topic where readings are published 68 | pub_sensor_readings_ = priv_nh_.advertise("data", 10); 69 | 70 | // Advertise service for setting the bias 71 | srv_set_bias_ = priv_nh_.advertiseService("set_bias", &FTSensorPublisher::setBiasCallback, this); 72 | } 73 | else 74 | { 75 | throw std::runtime_error("FT sensor could not initialize"); 76 | } 77 | } 78 | 79 | //! Empty stub 80 | ~FTSensorPublisher() {} 81 | 82 | }; 83 | 84 | /* 每次启动自动设置bias vetor,用来消除运动时候的偏差 */ 85 | bool FTSensorPublisher::setBiasCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response) 86 | { 87 | ftsensor_->setBias(); 88 | } 89 | 90 | void FTSensorPublisher::publishMeasurements() 91 | { 92 | // Recall that this has to be transformed using the stewart platform 93 | //tf_broadcaster_.sendTransform(tf::StampedTransform(nano_top_frame_, ros::Time::now(), "/world", "/nano_top_frame")); 94 | geometry_msgs::WrenchStamped ftreadings; 95 | float measurements[6]; 96 | ftsensor_->getMeasurements(measurements); 97 | 98 | ftreadings.wrench.force.x = measurements[0]; 99 | ftreadings.wrench.force.y = measurements[1]; 100 | ftreadings.wrench.force.z = measurements[2]; 101 | ftreadings.wrench.torque.x = measurements[3]; 102 | ftreadings.wrench.torque.y = measurements[4]; 103 | ftreadings.wrench.torque.z = measurements[5]; 104 | 105 | ftreadings.header.stamp = ros::Time::now(); 106 | ftreadings.header.frame_id = frame_ft_; 107 | 108 | pub_sensor_readings_.publish(ftreadings); 109 | 110 | // ROS_INFO("Measured Force: %f %f %f Measured Torque: %f %f %f", measurements[0], measurements[1], measurements[2], measurements[3], measurements[4], measurements[5]); 111 | } 112 | 113 | } // namespace ftsensor 114 | 115 | int main(int argc, char **argv) 116 | { 117 | ros::init(argc, argv, "ft_sensor"); 118 | ros::NodeHandle nh; 119 | ros::Rate loop(100); 120 | try 121 | { 122 | ftsensor::FTSensorPublisher node(nh); 123 | while(ros::ok()) 124 | { 125 | node.publishMeasurements(); 126 | ros::spinOnce(); 127 | 128 | loop.sleep(); 129 | } 130 | } 131 | catch (std::exception &ex) { 132 | ROS_FATAL_STREAM("Failed with error : " << ex.what()); 133 | return -1; 134 | } 135 | return 0; 136 | } 137 | -------------------------------------------------------------------------------- /ur5_admittance_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ur5_admittance_control) 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 | sensor_msgs 13 | tf 14 | tf_conversions 15 | rospy 16 | std_msgs 17 | geometry_msgs 18 | message_generation 19 | ) 20 | 21 | set(CMAKE_CXX_FLAGS "-O2 -O3 -std=c++11 -Wall") 22 | 23 | add_message_files( 24 | FILES 25 | PoseTwist.msg 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | geometry_msgs 31 | ) 32 | 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | # LIBRARIES cpr_closed_loop_controller 37 | CATKIN_DEPENDS geometry_msgs message_runtime 38 | # DEPENDS system_lib 39 | ) 40 | 41 | ########### 42 | ## Build ## 43 | ########### 44 | 45 | ## Specify additional locations of header files 46 | ## Your package locations should be listed before other locations 47 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS}) 48 | include_directories(${EIGEN3_INCLUDE_DIR}) 49 | 50 | set(INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include) 51 | set(SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) 52 | 53 | 54 | add_executable(admittance_controller_node 55 | ${SRC_DIR}/admittance_controller_node.cpp 56 | ${SRC_DIR}/AdmittanceController.cpp 57 | ${INCLUDE_DIR}/AdmittanceController.h) 58 | 59 | ## Add cmake target dependencies of the executable 60 | ## same as for the library above 61 | add_dependencies(admittance_controller_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 62 | 63 | #add_dependencies(cpr_record_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 64 | # 65 | #add_dependencies(cpr_ee_state_splitter_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 66 | 67 | 68 | ## Specify libraries to link a library or executable target against 69 | target_link_libraries(admittance_controller_node 70 | ${catkin_LIBRARIES} 71 | ) 72 | 73 | -------------------------------------------------------------------------------- /ur5_admittance_control/config/admittance_params.yaml: -------------------------------------------------------------------------------- 1 | # Admittance parameters for two mass damper systems 2 | # (the arm and the platform) with params (mass_arm, damping_arm) 3 | # and (mass_platform, damping_platform) respectively, connected by a 4 | # spring-damper （弹簧减震器）with parameters (damping_coupling耦合阻尼, stiffness_coupling耦合刚度, 5 | # equilibrium_point_spring) 6 | mass_arm: [2,0,0,0,0,0, 7 | 0,2,0,0,0,0, 8 | 0,0,2,0,0,0, 9 | 0,0,0,2,0,0, 10 | 0,0,0,0,2,0, 11 | 0,0,0,0,0,2] 12 | damping_arm: [12,0,0,0,0,0, 13 | 0,12,0,0,0,0, 14 | 0,0,12,0,0,0, 15 | 0,0,0,10,0,0, 16 | 0,0,0,0,10,0, 17 | 0,0,0,0,0,10] 18 | stiffness_arm: [10,0,0,0,0,0, 19 | 0,20,0,0,0,0, 20 | 0,0,10,0,0,0, 21 | 0,0,0,10,0,0, 22 | 0,0,0,0,10,0, 23 | 0,0,0,0,0,10] 24 | # [position (x, y, z) orientation (x, y, z, w)] 25 | # 就是设该点为力平衡点,此时在外界没有作用力的时候机械臂是平衡的，不给自动耷拉下来（耷拉说明平衡点为0） 26 | # 通过指定平衡点应该就可以实现东南的第二个视频 27 | # 因为平衡点的是（0.1,0.4）,要平衡力, 所以fake foce 的力肯定是相反方向的，也就是(0.4,-0.1) 28 | # 上条驳回,其实就是（0.1,0.4,0.5）,前面是关节rpy转角的问题，见ur5.urdf.xacro 29 | # equilibrium_point_spring: [0.1,0.4,0.5,-0.593599, -0.566706, -0.369419, 0.435908] 30 | equilibrium_point_spring: [-0.341,-0.103,0.657,-0.49, -0.1520, 0.8077, -0.2835] 31 | 32 | 33 | workspace_limits: [-0.50, 0.50, -0.5, 0.5, 0.30, 0.75] 34 | 35 | arm_max_vel: 1.5 36 | arm_max_acc: 1.0 -------------------------------------------------------------------------------- /ur5_admittance_control/include/AdmittanceController.h: -------------------------------------------------------------------------------- 1 | #ifndef ADMITTANCECONTROLLER_H 2 | #define ADMITTANCECONTROLLER_H 3 | 4 | #include "ros/ros.h" 5 | #include "ur5_admittance_control/PoseTwist.h" 6 | #include "geometry_msgs/WrenchStamped.h" 7 | #include "geometry_msgs/TwistStamped.h" 8 | #include 9 | #include 10 | #include 11 | 12 | #include "eigen3/Eigen/Core" 13 | #include "eigen3/Eigen/Geometry" 14 | #include "eigen3/Eigen/Dense" 15 | 16 | #include "std_msgs/Float32.h" 17 | 18 | using namespace Eigen; 19 | 20 | typedef Matrix Vector7d; 21 | typedef Matrix Vector6d; 22 | typedef Matrix Matrix6d; 23 | 24 | class AdmittanceController 25 | { 26 | protected: 27 | ////// ROS VARIABLES: 28 | // A handle to the node in ros 29 | ros::NodeHandle nh_; 30 | // Rate of the run loop 31 | ros::Rate loop_rate_; 32 | 33 | 34 | ///// Subscribers: 35 | // Subscriber for the arm state 36 | ros::Subscriber sub_arm_state_; 37 | // Subscriber for the ft sensor at the endeffector 38 | ros::Subscriber sub_wrench_external_; 39 | 40 | ////// Publishers: 41 | // Publisher for the twist of arm endeffector 42 | ros::Publisher pub_arm_cmd_; 43 | 44 | ////// INPUT SIGNAL 45 | // external wrench (force/torque sensor) in "robotiq_force_torque_frame_id" frame 46 | Vector6d wrench_external_; 47 | 48 | 49 | ////// FORCE/TORQUE-SENSOR FILTER: 50 | // Parameters for the noisy wrench 51 | double wrench_filter_factor_; 52 | double force_dead_zone_thres_; 53 | double torque_dead_zone_thres_; 54 | double admittance_ratio_; 55 | 56 | 57 | /////// ADMITTANCE PARAMETERS: 58 | // M_a_ -> Desired mass of arm 59 | // D_a_ -> Desired damping of arm 60 | // K_ -> Desired Stiffness of the coupling 61 | Matrix6d M_a_, D_a_, K_a_; 62 | // equilibrium position of the coupling spring 63 | Vector3d equilibrium_position_; 64 | // equilibrium orientation of the coupling spring 65 | Quaterniond equilibrium_orientation_; 66 | 67 | // OUTPUT COMMANDS 68 | // final arm desired velocity 69 | Vector6d arm_desired_twist_final_; 70 | 71 | // limiting the workspace of the arm 72 | Vector6d workspace_limits_; 73 | double arm_max_vel_; 74 | double arm_max_acc_; 75 | 76 | 77 | ////// STATE VARIABLES: 78 | // Arm state: position, orientation, and twist (in "ur5_arm_base_link") 79 | Vector3d arm_real_position_; 80 | Quaterniond arm_real_orientation_; 81 | Vector6d arm_real_twist_; 82 | 83 | 84 | // Transform from base_link to world 85 | Matrix6d rotation_base_; 86 | 87 | // TF: 88 | // Listeners 89 | tf::TransformListener listener_ft_; 90 | tf::TransformListener listener_control_; 91 | tf::TransformListener listener_arm_; 92 | 93 | // 判断是否所有坐标系矩阵建立 94 | bool transformation; 95 | 96 | // Initialization 97 | void wait_for_transformations(); 98 | 99 | // Control 100 | void compute_admittance(); 101 | 102 | 103 | 104 | // Callbacks 105 | void state_arm_callback(const ur5_admittance_control::PoseTwistConstPtr msg); 106 | void wrench_callback(const geometry_msgs::WrenchStampedConstPtr msg); 107 | 108 | // Util 109 | bool get_rotation_matrix(Matrix6d & rotation_matrix,tf::TransformListener & listener,std::string from_frame, std::string to_frame); 110 | 111 | void limit_to_workspace(); 112 | 113 | void send_commands_to_robot(); 114 | 115 | 116 | public: 117 | AdmittanceController(ros::NodeHandle &n, double frequency, 118 | std::string cmd_topic_arm, 119 | std::string state_topic_arm, 120 | std::string wrench_topic, 121 | std::vector M_a, 122 | std::vector D_a, 123 | std::vector K_a, 124 | std::vector d_e, 125 | std::vector workspace_limits, 126 | double arm_max_vel, 127 | double arm_max_acc, 128 | double wrench_filter_factor, 129 | double force_dead_zone_thres, 130 | double torque_dead_zone_thres); 131 | void run(); 132 | }; 133 | 134 | #endif // ADMITTANCECONTROLLER_H 135 | 136 | -------------------------------------------------------------------------------- /ur5_admittance_control/launch/admittance_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ur5_admittance_control/msg/PoseTwist.msg: -------------------------------------------------------------------------------- 1 | # The pose in this message should be specified in the coordinate frame given by header.frame_id. 2 | # The twist in this message should be specified in the coordinate frame given by the child_frame_id 3 | Header header 4 | string child_frame_id 5 | geometry_msgs/Pose pose 6 | geometry_msgs/Twist twist 7 | -------------------------------------------------------------------------------- /ur5_admittance_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ur5_admittance_control 4 | 0.0.0 5 | The admittance_control 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 | sensor_msgs 31 | tf 32 | cmake_modules 33 | eigen 34 | message_generation 35 | geometry_msgs 36 | message_runtime 37 | geometry_msgs 38 | sensor_msgs 39 | tf 40 | eigen 41 | catkin 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /ur5_admittance_control/src/AdmittanceController.cpp: -------------------------------------------------------------------------------- 1 | #include "AdmittanceController.h" 2 | 3 | AdmittanceController::AdmittanceController(ros::NodeHandle &n, 4 | double frequency, 5 | std::string topic_arm_command, 6 | std::string topic_arm_state, 7 | std::string topic_external_wrench, 8 | std::vector M_a, 9 | std::vector D_a, 10 | std::vector K_a, 11 | std::vector d_e, 12 | std::vector workspace_limits, 13 | double arm_max_vel, 14 | double arm_max_acc, 15 | double wrench_filter_factor, 16 | double force_dead_zone_thres, 17 | double torque_dead_zone_thres) : 18 | nh_(n), loop_rate_(frequency), 19 | wrench_filter_factor_(wrench_filter_factor), 20 | force_dead_zone_thres_(force_dead_zone_thres), 21 | torque_dead_zone_thres_(torque_dead_zone_thres), 22 | M_a_(M_a.data()),D_a_(D_a.data()), K_a_(K_a.data()), 23 | workspace_limits_(workspace_limits.data()), 24 | arm_max_vel_(arm_max_vel), 25 | arm_max_acc_(arm_max_acc) 26 | { 27 | ///// Subscribers 28 | sub_arm_state_ = nh_.subscribe(topic_arm_state, 10, 29 | &AdmittanceController::state_arm_callback, this, 30 | ros::TransportHints().reliable().tcpNoDelay()); 31 | 32 | sub_wrench_external_ = nh_.subscribe(topic_external_wrench, 5, 33 | &AdmittanceController::wrench_callback, this, 34 | ros::TransportHints().reliable().tcpNoDelay()); 35 | 36 | ////// Publishers 37 | pub_arm_cmd_ = nh_.advertise(topic_arm_command, 5); 38 | 39 | /// initializing the class variables 40 | wrench_external_.setZero(); 41 | 42 | // setting the equilibrium position and orientation and Make sure the orientation goal is normalized 43 | Vector7d equilibrium_full(d_e.data()); 44 | equilibrium_position_ << equilibrium_full.topRows(3); 45 | equilibrium_orientation_.coeffs() << equilibrium_full.bottomRows(4) /equilibrium_full.bottomRows(4).norm(); 46 | // starting from a state that does not create movements on the robot 47 | arm_real_orientation_ = equilibrium_orientation_; 48 | arm_real_position_ = equilibrium_position_ ; 49 | 50 | while (nh_.ok() && !arm_real_position_(0)) { 51 | ROS_WARN_THROTTLE(1, "Waiting for the state of the arm..."); 52 | ros::spinOnce(); 53 | loop_rate_.sleep(); 54 | } 55 | 56 | // Init integrator 57 | arm_desired_twist_final_.setZero(); 58 | 59 | transformation = false; 60 | 61 | // between zero and one that modulated the input force as to control the stiffness(刚度) 62 | admittance_ratio_ = 1; 63 | 64 | wait_for_transformations(); 65 | } 66 | 67 | /////////////////////////////////////////////////////////////// 68 | ////////////////////////// Callbacks ////////////////////////// 69 | /////////////////////////////////////////////////////////////// 70 | void AdmittanceController::state_arm_callback(const ur5_admittance_control::PoseTwistConstPtr msg) { 71 | arm_real_position_ << msg->pose.position.x, msg->pose.position.y, 72 | msg->pose.position.z; 73 | 74 | arm_real_orientation_.coeffs() << msg->pose.orientation.x, 75 | msg->pose.orientation.y, 76 | msg->pose.orientation.z, 77 | msg->pose.orientation.w; 78 | 79 | arm_real_twist_ << msg->twist.linear.x, msg->twist.linear.y, 80 | msg->twist.linear.z, msg->twist.angular.x, msg->twist.angular.y, 81 | msg->twist.angular.z; 82 | } 83 | 84 | void AdmittanceController::wrench_callback(const geometry_msgs::WrenchStampedConstPtr msg) { 85 | Vector6d wrench_ft_frame; 86 | Matrix6d rotation_ft_base; 87 | if (transformation) { 88 | 89 | // Reading the FT-sensor in its own frame (robotiq_force_torque_frame_id) 90 | wrench_ft_frame << msg->wrench.force.x, msg->wrench.force.y, 91 | msg->wrench.force.z, msg->wrench.torque.x, 92 | msg->wrench.torque.y, msg->wrench.torque.z; 93 | 94 | for (int i = 0; i < 3; i++) { 95 | if (abs(wrench_ft_frame(i)) < force_dead_zone_thres_) { 96 | wrench_ft_frame(i) = 0; 97 | } 98 | if (abs(wrench_ft_frame(i + 3)) < torque_dead_zone_thres_) { 99 | wrench_ft_frame(i + 3) = 0; 100 | } 101 | } 102 | // Get transform from arm base link to platform base link 103 | get_rotation_matrix(rotation_ft_base, listener_ft_, 104 | "base_link", "FT_link"); 105 | 106 | // Filter and update 107 | wrench_external_ << (1 - wrench_filter_factor_) * wrench_external_ + 108 | wrench_filter_factor_ * rotation_ft_base * wrench_ft_frame; 109 | } 110 | } 111 | 112 | /////////////////////////////////////////////////////////////// 113 | ///////////////////// Control Loop //////////////////////////// 114 | /////////////////////////////////////////////////////////////// 115 | void AdmittanceController::run() { 116 | 117 | ROS_INFO("Running the admittance control loop ................."); 118 | 119 | while (nh_.ok()) { 120 | // Admittance Dynamics computation 121 | compute_admittance(); 122 | 123 | // sum the vel from admittance to DS in this function 124 | // limit the the movement of the arm to the permitted workspace 125 | limit_to_workspace(); 126 | 127 | // Copy commands to messages 128 | send_commands_to_robot(); 129 | 130 | // publishing visualization/debugging info 131 | // publish_debuggings_signals(); 132 | 133 | ros::spinOnce(); 134 | loop_rate_.sleep(); 135 | } 136 | 137 | 138 | } 139 | 140 | /////////////////////////////////////////////////////////////// 141 | ///////////////////// Admittance Dynamics ///////////////////// 142 | /////////////////////////////////////////////////////////////// 143 | void AdmittanceController::compute_admittance() { 144 | 145 | Vector6d arm_desired_accelaration; 146 | 147 | Vector6d error; 148 | // Orientation error w.r.t. desired equilibriums 149 | if (equilibrium_orientation_.coeffs().dot(arm_real_orientation_.coeffs()) < 0.0) { 150 | arm_real_orientation_.coeffs() << -arm_real_orientation_.coeffs(); 151 | } 152 | 153 | Eigen::Quaterniond quat_rot_err(arm_real_orientation_* equilibrium_orientation_.inverse()); 154 | 155 | if (quat_rot_err.coeffs().norm() > 1e-3) { 156 | // Normalize error quaternion 157 | quat_rot_err.coeffs() << quat_rot_err.coeffs() / quat_rot_err.coeffs().norm(); 158 | } 159 | 160 | Eigen::AngleAxisd err_arm_des_orient(quat_rot_err); 161 | error.bottomRows(3) << err_arm_des_orient.axis() *err_arm_des_orient.angle(); 162 | // Translation error w.r.t.（with respect to） desipared equilibrium 163 | error.topRows(3) = arm_real_position_ - equilibrium_position_; 164 | 165 | arm_desired_accelaration = M_a_.inverse() * ( - K_a_ * error - D_a_ * arm_desired_twist_final_ + admittance_ratio_ * wrench_external_); 166 | 167 | // limiting the accelaration for better stability and safety 168 | // x,y,z for the arm 169 | double a_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm(); 170 | if (a_acc_norm > arm_max_acc_) { 171 | ROS_WARN_STREAM_THROTTLE(1, "Admittance generates high arm accelaration!"<< " norm: " << a_acc_norm); 172 | arm_desired_accelaration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm); 173 | } 174 | 175 | // Integrate for velocity based interface 176 | ros::Duration duration = loop_rate_.expectedCycleTime(); 177 | arm_desired_twist_final_ += arm_desired_accelaration * duration.toSec(); 178 | } 179 | 180 | /////////////////////////////////////////////////////////////// 181 | //////////////////// Limit to workspace//////////////////////// 182 | /////////////////////////////////////////////////////////////// 183 | void AdmittanceController::limit_to_workspace() { 184 | 185 | if (arm_real_position_(0) < workspace_limits_(0) || arm_real_position_(0) > workspace_limits_(1)) { 186 | ROS_WARN_STREAM_THROTTLE (1, "Out of permitted workspace. x = " 187 | << arm_real_position_(0) << " not in [" << workspace_limits_(0) << " , " 188 | << workspace_limits_(1) << "]"); 189 | } 190 | 191 | if (arm_real_position_(1) < workspace_limits_(2) || arm_real_position_(1) > workspace_limits_(3)) { 192 | ROS_WARN_STREAM_THROTTLE (1, "Out of permitted workspace. y = " 193 | << arm_real_position_(1) << " not in [" << workspace_limits_(2) << " , " 194 | << workspace_limits_(3) << "]"); 195 | } 196 | 197 | if (arm_real_position_(2) < workspace_limits_(4) || arm_real_position_(2) > workspace_limits_(5)) { 198 | ROS_WARN_STREAM_THROTTLE (1, "Out of permitted workspace. z = " 199 | << arm_real_position_(2) << " not in [" << workspace_limits_(4) << " , " 200 | << workspace_limits_(5) << "]"); 201 | } 202 | 203 | if (arm_desired_twist_final_(0) < 0 && arm_real_position_(0) < workspace_limits_(0)) { 204 | arm_desired_twist_final_(0) = 0; 205 | } 206 | 207 | if (arm_desired_twist_final_(0) > 0 && arm_real_position_(0) > workspace_limits_(1)) { 208 | arm_desired_twist_final_(0) = 0; 209 | } 210 | 211 | if (arm_desired_twist_final_(1) < 0 && arm_real_position_(1) < workspace_limits_(2)) { 212 | arm_desired_twist_final_(1) = 0; 213 | } 214 | 215 | if (arm_desired_twist_final_(1) > 0 && arm_real_position_(1) > workspace_limits_(3)) { 216 | arm_desired_twist_final_(1) = 0; 217 | } 218 | 219 | if (arm_desired_twist_final_(2) < 0 && arm_real_position_(2) < workspace_limits_(4)) { 220 | arm_desired_twist_final_(2) = 0; 221 | } 222 | 223 | if (arm_desired_twist_final_(2) > 0 && arm_real_position_(2) > workspace_limits_(5)) { 224 | arm_desired_twist_final_(2) = 0; 225 | } 226 | 227 | // velocity of the arm along x, y, and z axis 228 | double norm_vel_des = (arm_desired_twist_final_.segment(0, 3)).norm(); 229 | 230 | if (norm_vel_des > arm_max_vel_) { 231 | ROS_WARN_STREAM_THROTTLE(1, "Admittance generate fast arm movements! velocity norm: " << norm_vel_des); 232 | arm_desired_twist_final_.segment(0, 3) *= (arm_max_vel_ / norm_vel_des); 233 | } 234 | 235 | } 236 | 237 | /////////////////////////////////////////////////////////////// 238 | //////////////////// COMMANDING THE ROBOT ///////////////////// 239 | /////////////////////////////////////////////////////////////// 240 | void AdmittanceController::send_commands_to_robot() { 241 | geometry_msgs::Twist arm_twist_cmd; 242 | 243 | arm_twist_cmd.linear.x = arm_desired_twist_final_(0); 244 | arm_twist_cmd.linear.y = arm_desired_twist_final_(1); 245 | arm_twist_cmd.linear.z = arm_desired_twist_final_(2); 246 | arm_twist_cmd.angular.x = arm_desired_twist_final_(3); 247 | arm_twist_cmd.angular.y = arm_desired_twist_final_(4); 248 | arm_twist_cmd.angular.z = arm_desired_twist_final_(5); 249 | 250 | pub_arm_cmd_.publish(arm_twist_cmd); 251 | } 252 | 253 | 254 | ////////////////////// 255 | /// INITIALIZATION /// 256 | ////////////////////// 257 | void AdmittanceController::wait_for_transformations() { 258 | tf::TransformListener listener; 259 | Matrix6d rot_matrix; 260 | rotation_base_.setZero(); 261 | 262 | while (!get_rotation_matrix(rot_matrix, listener, "world", "base_link")) { 263 | sleep(1); 264 | } 265 | 266 | while (!get_rotation_matrix(rotation_base_, listener, "base_link", "base_link")) { 267 | sleep(1); 268 | } 269 | 270 | while (!get_rotation_matrix(rot_matrix, listener, "base_link", "FT_link")) { 271 | sleep(1); 272 | } 273 | transformation = true; 274 | 275 | ROS_INFO("The Force/Torque sensor is ready to use."); 276 | } 277 | 278 | //////////// 279 | /// UTIL /// 280 | //////////// 281 | 282 | bool AdmittanceController::get_rotation_matrix(Matrix6d & rotation_matrix, 283 | tf::TransformListener & listener, 284 | std::string from_frame, 285 | std::string to_frame) 286 | { 287 | tf::StampedTransform transform; 288 | Matrix3d rotation_from_to; 289 | try { 290 | listener.lookupTransform(from_frame, to_frame, 291 | ros::Time(0), transform); 292 | tf::matrixTFToEigen(transform.getBasis(), rotation_from_to); 293 | rotation_matrix.setZero(); 294 | // 因为力和力矩是六维的，所以用两个相同的 R 295 | rotation_matrix.topLeftCorner(3, 3) = rotation_from_to; 296 | rotation_matrix.bottomRightCorner(3, 3) = rotation_from_to; 297 | } 298 | catch (tf::TransformException ex) { 299 | rotation_matrix.setZero(); 300 | ROS_WARN_STREAM_THROTTLE(1, "Waiting for TF from: " << from_frame << " to: " << to_frame ); 301 | return false; 302 | } 303 | 304 | return true; 305 | } 306 | 307 | 308 | -------------------------------------------------------------------------------- /ur5_admittance_control/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 = 100.0; 10 | 11 | // Parameters 12 | std::string topic_arm_state; 13 | std::string topic_arm_command; 14 | std::string topic_external_wrench; 15 | 16 | std::vector M_a; 17 | std::vector D_a; 18 | std::vector K_a; 19 | std::vector d_e; 20 | std::vector workspace_limits; 21 | 22 | double arm_max_vel; 23 | double arm_max_acc; 24 | 25 | double wrench_filter_factor; 26 | double force_dead_zone_thres; 27 | double torque_dead_zone_thres; 28 | 29 | 30 | /// LOADING PARAMETERS FROM THE ROS SERVER 31 | if (!nh.getParam("topic_arm_state", topic_arm_state)) { 32 | ROS_ERROR("Couldn't retrieve the topic name for the state of the arm."); 33 | return -1; 34 | } 35 | if (!nh.getParam("topic_arm_command", topic_arm_command)) { 36 | ROS_ERROR("Couldn't retrieve the topic name for commanding the arm."); 37 | return -1; 38 | } 39 | if (!nh.getParam("topic_external_wrench", topic_external_wrench)) { 40 | ROS_ERROR("Couldn't retrieve the topic name for the force/torque sensor."); 41 | return -1; 42 | } 43 | 44 | /// ADMITTANCE PARAMETERS 45 | if (!nh.getParam("mass_arm", M_a)) { 46 | ROS_ERROR("Couldn't retrieve the desired mass of the arm."); 47 | return -1; 48 | } 49 | if (!nh.getParam("damping_arm", D_a)) { 50 | ROS_ERROR("Couldn't retrieve the desired damping of the arm."); 51 | return -1; 52 | } 53 | if (!nh.getParam("stiffness_arm", K_a)) { 54 | ROS_ERROR("Couldn't retrieve the desired stiffness of the coupling."); 55 | return -1; 56 | } 57 | if (!nh.getParam("equilibrium_point_spring", d_e)) { 58 | ROS_ERROR("Couldn't retrieve the desired equilibrium of the spring."); 59 | return -1; 60 | } 61 | 62 | /// SAFETY PARAMETERS 63 | if (!nh.getParam("workspace_limits", workspace_limits)) { 64 | ROS_ERROR("Couldn't retrieve the limits of the workspace."); 65 | return -1; 66 | } 67 | if (!nh.getParam("arm_max_vel", arm_max_vel)) { 68 | ROS_ERROR("Couldn't retrieve the max velocity for the arm."); 69 | return -1; 70 | } 71 | if (!nh.getParam("arm_max_acc", arm_max_acc)) { 72 | ROS_ERROR("Couldn't retrieve the max acceleration for the arm."); 73 | return -1; 74 | } 75 | 76 | /// FORCE/TORQUE-SENSOR PARAMETERS 77 | if (!nh.getParam("wrench_filter_factor", wrench_filter_factor)) { 78 | ROS_ERROR("Couldn't retrieve the desired wrench filter factor."); 79 | return -1; 80 | } 81 | if (!nh.getParam("force_dead_zone_thres", force_dead_zone_thres)) { 82 | ROS_ERROR("Couldn't retrieve the desired force_dead_zone threshold."); 83 | return -1; 84 | } 85 | if (!nh.getParam("torque_dead_zone_thres", torque_dead_zone_thres)) { 86 | ROS_ERROR("Couldn't retrieve the desired torque_dead_zone threshold. "); 87 | return -1; 88 | } 89 | 90 | // Constructing the controller 91 | AdmittanceController admittance_controller( 92 | nh, 93 | frequency, 94 | topic_arm_command, 95 | topic_arm_state, 96 | topic_external_wrench, 97 | M_a, D_a, K_a, d_e, 98 | workspace_limits, 99 | arm_max_vel, arm_max_acc, 100 | wrench_filter_factor, 101 | force_dead_zone_thres, 102 | torque_dead_zone_thres); 103 | 104 | // Running the controller 105 | admittance_controller.run(); 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /ur5_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ur5_bringup) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS roslaunch) 7 | 8 | catkin_package() 9 | 10 | install(DIRECTORY launch urdf 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 12 | ) 13 | 14 | -------------------------------------------------------------------------------- /ur5_bringup/config/initial_pose.yaml: -------------------------------------------------------------------------------- 1 | initial_pose: 2 | joint_names: ["elbow_joint", 3 | "shoulder_lift_joint", 4 | "shoulder_pan_joint", 5 | "wrist_1_joint", 6 | "wrist_2_joint", 7 | "wrist_3_joint"] 8 | joint_values: [2.137181473386596, 9 | -1.4998039803657388, 10 | 1.549433454173772, 11 | -2.0415906290824704, 12 | 1.652389958375986, 13 | 0.0786922321771] 14 | -------------------------------------------------------------------------------- /ur5_bringup/controllers/joint_state_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 150 4 | -------------------------------------------------------------------------------- /ur5_bringup/controllers/ur5_vel_controller.yaml: -------------------------------------------------------------------------------- 1 | hardware_control_loop: 2 | loop_hz: 125 3 | 4 | # Settings for ros_control hardware interface 5 | hardware_interface: 6 | joints: 7 | - shoulder_pan_joint 8 | - shoulder_lift_joint 9 | - elbow_joint 10 | - wrist_1_joint 11 | - wrist_2_joint 12 | - wrist_3_joint 13 | 14 | ur5_cartesian_velocity_controller: 15 | type: cartesian_controller/CartesianVelocityController 16 | publish_rate: 125 17 | root_name: base_link 18 | tip_name: ee_link 19 | 20 | ur5_cartesian_velocity_controller_sim: 21 | type: cartesian_controller/CartesianVelocityControllerSim 22 | publish_rate: 125 23 | root_name: base_link 24 | tip_name: ee_link 25 | 26 | ur5_cartesian_state_controller: 27 | type: cartesian_controller/CartesianStateController 28 | publish_rate: 125 29 | root_name: base_link 30 | tip_name: ee_link 31 | 32 | #Publish wrench ---------------------------------- 33 | force_torque_sensor_controller: 34 | type: force_torque_sensor_controller/ForceTorqueSensorController 35 | publish_rate: 125 36 | -------------------------------------------------------------------------------- /ur5_bringup/launch/init_pose_start_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ur5_bringup/launch/robot_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 40 | 41 | 43 | 44 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 75 | 76 | 77 | 79 | 80 | 81 | 82 | 83 | 85 | 86 | 88 | 89 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /ur5_bringup/launch/ur5_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /ur5_bringup/launch/ur_force_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /ur5_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ur5_bringup 4 | 0.0.0 5 | The ur5_bringup package 6 | 7 | medina 8 | 9 | TODO 10 | 11 | 12 | catkin 13 | roslaunch 14 | 15 | gazebo_msgs 16 | controller_manager_msgs 17 | robot_state_publisher 18 | urdf 19 | xacro 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ur5_bringup/rviz/admittance_fake_wrench.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1/Offset1 8 | Splitter Ratio: 0.675000012 9 | Tree Height: 522 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | - Class: redwall_arm_panel/Teleop 30 | Name: Teleop 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.899999976 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 1000 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | FT_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | Link Tree Style: Links in Alphabetic Order 69 | base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | ee_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | forearm_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | shoulder_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | upper_arm_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | wrist_1_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | wrist_2_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | wrist_3_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | Name: ur5 110 | Robot Description: robot_description 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Class: rviz/TF 116 | Enabled: true 117 | Frame Timeout: 15 118 | Frames: 119 | All Enabled: false 120 | FT_link: 121 | Value: true 122 | base_link: 123 | Value: false 124 | ee_link: 125 | Value: true 126 | fake_force_pose: 127 | Value: false 128 | forearm_link: 129 | Value: true 130 | shoulder_link: 131 | Value: true 132 | upper_arm_link: 133 | Value: true 134 | world: 135 | Value: false 136 | wrist_1_link: 137 | Value: true 138 | wrist_2_link: 139 | Value: true 140 | wrist_3_link: 141 | Value: true 142 | Marker Scale: 1 143 | Name: TF 144 | Show Arrows: false 145 | Show Axes: false 146 | Show Names: false 147 | Tree: 148 | world: 149 | base_link: 150 | shoulder_link: 151 | upper_arm_link: 152 | forearm_link: 153 | wrist_1_link: 154 | wrist_2_link: 155 | wrist_3_link: 156 | ee_link: 157 | FT_link: 158 | {} 159 | fake_force_pose: 160 | {} 161 | Update Interval: 0 162 | Value: true 163 | - Class: rviz/InteractiveMarkers 164 | Enable Transparency: false 165 | Enabled: true 166 | Name: foce_torque_marker 167 | Show Axes: false 168 | Show Descriptions: true 169 | Show Visual Aids: false 170 | Update Topic: /fake_force_sensor/update 171 | Value: true 172 | - Alpha: 1 173 | Arrow Width: 0.5 174 | Class: rviz/WrenchStamped 175 | Enabled: true 176 | Force Arrow Scale: 2 177 | Force Color: 204; 51; 51 178 | History Length: 1 179 | Name: External_wrench 180 | Topic: /ft_sensor/data 181 | Torque Arrow Scale: 2 182 | Torque Color: 204; 204; 51 183 | Unreliable: false 184 | Value: true 185 | - Class: rviz/Image 186 | Enabled: true 187 | Image Topic: /camera/color/image_raw 188 | Max Value: 1 189 | Median window: 5 190 | Min Value: 0 191 | Name: Image 192 | Normalize Range: true 193 | Queue Size: 2 194 | Transport Hint: raw 195 | Unreliable: false 196 | Value: true 197 | - Class: octomap_rviz_plugin/OccupancyGrid 198 | Enabled: true 199 | Max. Height Display: 3.40282347e+38 200 | Max. Octree Depth: 16 201 | Min. Height Display: -3.40282347e+38 202 | Name: OccupancyGrid 203 | Octomap Topic: /octomap_full 204 | Queue Size: 5 205 | Value: true 206 | Voxel Alpha: 1 207 | Voxel Coloring: Z-Axis 208 | Voxel Rendering: All Voxels 209 | Enabled: true 210 | Global Options: 211 | Background Color: 48; 48; 48 212 | Default Light: true 213 | Fixed Frame: world 214 | Frame Rate: 30 215 | Name: root 216 | Tools: 217 | - Class: rviz/Interact 218 | Hide Inactive Objects: true 219 | - Class: rviz/MoveCamera 220 | - Class: rviz/Select 221 | - Class: rviz/FocusCamera 222 | - Class: rviz/Measure 223 | - Class: rviz/SetInitialPose 224 | Topic: /initialpose 225 | - Class: rviz/SetGoal 226 | Topic: /move_base_simple/goal 227 | - Class: rviz/PublishPoint 228 | Single click: true 229 | Topic: /clicked_point 230 | Value: true 231 | Views: 232 | Current: 233 | Class: rviz/XYOrbit 234 | Distance: 3.07725 235 | Enable Stereo Rendering: 236 | Stereo Eye Separation: 0.0599999987 237 | Stereo Focal Distance: 1 238 | Swap Stereo Eyes: false 239 | Value: false 240 | Focal Point: 241 | X: 0 242 | Y: 0 243 | Z: 0 244 | Focal Shape Fixed Size: false 245 | Focal Shape Size: 0.0500000007 246 | Invert Z Axis: false 247 | Name: Current View 248 | Near Clip Distance: 0.00999999978 249 | Pitch: 0.0252030715 250 | Target Frame: ur5_arm_base 251 | Value: XYOrbit (rviz) 252 | Yaw: 2.36222696 253 | Saved: ~ 254 | Window Geometry: 255 | Displays: 256 | collapsed: false 257 | Height: 1056 258 | Hide Left Dock: false 259 | Hide Right Dock: false 260 | Image: 261 | collapsed: false 262 | QMainWindow State: 000000ff00000000fd00000004000000000000017a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000002800000299000000d700fffffffa000000010100000002fb0000000c00430061006d0065007200610000000000ffffffff0000000000000000fb000000100044006900730070006c00610079007301000000000000017a0000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002520000009c0000000000000000fb0000000a0049006d00610067006501000002c7000000f70000001600ffffff000000010000016200000396fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000c00540065006c0065006f00700100000028000003960000001600fffffffb0000000a0056006900650077007300000000440000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004980000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 263 | Selection: 264 | collapsed: false 265 | Teleop: 266 | collapsed: false 267 | Time: 268 | collapsed: false 269 | Tool Properties: 270 | collapsed: false 271 | Views: 272 | collapsed: false 273 | Width: 1920 274 | X: 0 275 | Y: 24 276 | -------------------------------------------------------------------------------- /ur5_bringup/src/fake_wrench.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | import copy 5 | import tf 6 | import numpy 7 | 8 | from interactive_markers.interactive_marker_server import * 9 | from interactive_markers.menu_handler import * 10 | from visualization_msgs.msg import * 11 | from geometry_msgs.msg import Point 12 | from geometry_msgs.msg import WrenchStamped 13 | from tf.broadcaster import TransformBroadcaster 14 | 15 | marker_pose = geometry_msgs.msg.Transform() 16 | listener = None 17 | wrench_pub = None 18 | 19 | def publisherCallback( msg ): 20 | try: 21 | listener.waitForTransform("/world", "/fake_force_pose", rospy.Time(0), rospy.Duration(10.0)) 22 | (trans1,rot1) = listener.lookupTransform("/world", "/fake_force_pose", rospy.Time(0)) 23 | (trans2,rot2) = listener.lookupTransform("/world", "/FT_link", rospy.Time(0)) 24 | (trans3,rot3) = listener.lookupTransform("/FT_link", "/world",rospy.Time(0)) 25 | # Publish the fake force 26 | fake_wrench = geometry_msgs.msg.WrenchStamped() 27 | trans1_mat = tf.transformations.translation_matrix(trans1) 28 | trans2_mat = tf.transformations.translation_matrix(trans2) 29 | rot3_mat = tf.transformations.quaternion_matrix(rot3) 30 | mat1 = numpy.dot(rot3_mat, trans1_mat-trans2_mat) 31 | force_at_ft_link = tf.transformations.translation_from_matrix(mat1) 32 | # print(marker_pose) 33 | fake_wrench.wrench.force.x = force_at_ft_link[0] 34 | fake_wrench.wrench.force.y = force_at_ft_link[1] 35 | fake_wrench.wrench.force.z = force_at_ft_link[2] 36 | rot = (marker_pose.rotation.x, marker_pose.rotation.y, marker_pose.rotation.z, marker_pose.rotation.w) 37 | euler = tf.transformations.euler_from_quaternion(rot) 38 | fake_wrench.wrench.torque.x = euler[0] 39 | fake_wrench.wrench.torque.y = euler[1] 40 | fake_wrench.wrench.torque.z = euler[2] 41 | fake_wrench.header.frame_id = "FT_link" 42 | fake_wrench.header.stamp = rospy.Time.now() 43 | wrench_pub.publish(fake_wrench) 44 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 45 | print "Coudn't find transforms!" 46 | 47 | def transformCallback( msg ): 48 | br = TransformBroadcaster() 49 | br.sendTransform((marker_pose.translation.x, 50 | marker_pose.translation.y, 51 | marker_pose.translation.z), 52 | (0,0,0,1), 53 | rospy.Time.now(), 54 | "fake_force_pose", 55 | "world") 56 | 57 | def makeBox( msg ): 58 | marker = Marker() 59 | 60 | marker.type = Marker.SPHERE 61 | marker.scale.x = msg.scale * 0.02 62 | marker.scale.y = msg.scale * 0.02 63 | marker.scale.z = msg.scale * 0.02 64 | marker.color.r = 0.0 65 | marker.color.g = 1.0 66 | marker.color.b = 0.0 67 | marker.color.a = 1.0 68 | 69 | return marker 70 | 71 | def makeBoxControl( msg ): 72 | control = InteractiveMarkerControl() 73 | control.always_visible = True 74 | control.markers.append( makeBox(msg) ) 75 | msg.controls.append( control ) 76 | return control 77 | 78 | def processFeedback(feedback, br): 79 | s = "Feedback from marker '" + feedback.marker_name 80 | s += "' / control '" + feedback.control_name + "'" 81 | 82 | mp = "" 83 | if feedback.mouse_point_valid: 84 | mp = " at " + str(feedback.mouse_point.x) 85 | mp += ", " + str(feedback.mouse_point.y) 86 | mp += ", " + str(feedback.mouse_point.z) 87 | mp += " in frame " + feedback.header.frame_id 88 | 89 | if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: 90 | # rospy.loginfo(s + ": button click" + mp + ".") 91 | pass 92 | elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: 93 | # rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") 94 | pass 95 | elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: 96 | # rospy.loginfo(s + ": pose changed") 97 | marker_pose.translation.x = feedback.pose.position.x 98 | marker_pose.translation.y = feedback.pose.position.y 99 | marker_pose.translation.z = feedback.pose.position.z 100 | marker_pose.rotation.x = feedback.pose.orientation.x 101 | marker_pose.rotation.y = feedback.pose.orientation.y 102 | marker_pose.rotation.z = feedback.pose.orientation.z 103 | marker_pose.rotation.w = feedback.pose.orientation.w 104 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: 105 | pass 106 | # rospy.loginfo(s + ": mouse down" + mp + ".") 107 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: 108 | # rospy.loginfo(s + ": mouse up" + mp + ".") 109 | pass 110 | server.applyChanges() 111 | 112 | 113 | if __name__ == "__main__": 114 | rospy.init_node("fake_force_sensor") 115 | br = TransformBroadcaster() 116 | listener = tf.TransformListener() 117 | wrench_pub = rospy.Publisher('/ft_sensor/data', geometry_msgs.msg.WrenchStamped, queue_size=4) 118 | 119 | # for default trans and rotation same with postion 120 | marker_pose.translation.x = 0.101201 121 | marker_pose.translation.y = 0.40755 122 | marker_pose.translation.z = 0.51814 123 | marker_pose.rotation.x = 0 124 | marker_pose.rotation.y = 0 125 | marker_pose.rotation.z = 0 126 | marker_pose.rotation.w = 1 127 | 128 | # Publisher for the topic 129 | rospy.Timer(rospy.Duration(0.02), publisherCallback) 130 | # Publisher for the TF 131 | rospy.Timer(rospy.Duration(0.02), transformCallback) 132 | 133 | server = InteractiveMarkerServer("fake_force_sensor") 134 | menu_handler = MenuHandler() 135 | pf_wrap = lambda fb: processFeedback(fb, br) 136 | 137 | menu_handler.insert("First Entry", callback=pf_wrap) 138 | menu_handler.insert("Second Entry", callback=pf_wrap) 139 | sub_menu_handle = menu_handler.insert("Submenu") 140 | menu_handler.insert("First Entry", parent=sub_menu_handle, callback=pf_wrap) 141 | menu_handler.insert("Second Entry", parent=sub_menu_handle, callback=pf_wrap) 142 | 143 | # for force and torque 144 | # position = Point(0, 0, 0) 145 | # beacause admittance_control config file 146 | # for force to zero 147 | position = Point(0.101201, 0.40755, 0.51814) 148 | int_marker = InteractiveMarker() 149 | int_marker.header.frame_id = "world" 150 | int_marker.pose.position = position 151 | int_marker.scale = 0.3 152 | int_marker.name = "fake_wrench" 153 | int_marker.description = "Simulation: \n Force = the distance vector from UR5_ee_link to marker \n Torque = the angle vector from initial orientation to marker" 154 | # insert a box 155 | makeBoxControl(int_marker) 156 | 157 | 158 | fixed = False 159 | control = InteractiveMarkerControl() 160 | control.orientation.w = 1 161 | control.orientation.x = 1 162 | control.orientation.y = 0 163 | control.orientation.z = 0 164 | control.name = "rotate_x" 165 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 166 | if fixed: 167 | control.orientation_mode = InteractiveMarkerControl.FIXED 168 | int_marker.controls.append(control) 169 | 170 | control = InteractiveMarkerControl() 171 | control.orientation.w = 1 172 | control.orientation.x = 1 173 | control.orientation.y = 0 174 | control.orientation.z = 0 175 | control.name = "move_x" 176 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 177 | if fixed: 178 | control.orientation_mode = InteractiveMarkerControl.FIXED 179 | int_marker.controls.append(control) 180 | 181 | control = InteractiveMarkerControl() 182 | control.orientation.w = 1 183 | control.orientation.x = 0 184 | control.orientation.y = 1 185 | control.orientation.z = 0 186 | control.name = "rotate_y" 187 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 188 | if fixed: 189 | control.orientation_mode = InteractiveMarkerControl.FIXED 190 | int_marker.controls.append(control) 191 | 192 | control = InteractiveMarkerControl() 193 | control.orientation.w = 1 194 | control.orientation.x = 0 195 | control.orientation.y = 1 196 | control.orientation.z = 0 197 | control.name = "move_y" 198 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 199 | if fixed: 200 | control.orientation_mode = InteractiveMarkerControl.FIXED 201 | int_marker.controls.append(control) 202 | 203 | control = InteractiveMarkerControl() 204 | control.orientation.w = 1 205 | control.orientation.x = 0 206 | control.orientation.y = 0 207 | control.orientation.z = 1 208 | control.name = "rotate_z" 209 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 210 | if fixed: 211 | control.orientation_mode = InteractiveMarkerControl.FIXED 212 | int_marker.controls.append(control) 213 | 214 | control = InteractiveMarkerControl() 215 | control.orientation.w = 1 216 | control.orientation.x = 0 217 | control.orientation.y = 0 218 | control.orientation.z = 1 219 | control.name = "move_z" 220 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 221 | if fixed: 222 | control.orientation_mode = InteractiveMarkerControl.FIXED 223 | int_marker.controls.append(control) 224 | 225 | server.insert(int_marker, pf_wrap) 226 | menu_handler.apply(server, int_marker.name) 227 | 228 | server.applyChanges() 229 | 230 | rospy.spin() 231 | -------------------------------------------------------------------------------- /ur5_bringup/src/init_pose_start_controllers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import sys 5 | import subprocess 6 | 7 | from gazebo_msgs.srv import SetModelConfiguration, SetModelConfigurationRequest 8 | from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest 9 | 10 | SET_MODEL_CONFIGURATION = "/gazebo/set_model_configuration" 11 | SWITCH_CONTROLLER = "/controller_manager/switch_controller" 12 | 13 | def set_initial_pose_gazebo(): 14 | set_model_config_client = rospy.ServiceProxy(SET_MODEL_CONFIGURATION, SetModelConfiguration) 15 | req = SetModelConfigurationRequest() 16 | req.model_name = "ur5" 17 | req.urdf_param_name = "robot_description" 18 | req.joint_names = rospy.get_param("/initial_pose/joint_names") 19 | req.joint_positions = rospy.get_param("/initial_pose/joint_values") 20 | rospy.wait_for_service(SET_MODEL_CONFIGURATION) 21 | res = set_model_config_client(req) 22 | 23 | def pause_gazebo(): 24 | subprocess.check_call("rosservice call /gazebo/pause_physics", shell=True) 25 | 26 | def unpause_gazebo(): 27 | subprocess.check_call("rosservice call /gazebo/unpause_physics", shell=True) 28 | 29 | def clear_wrenches(): 30 | subprocess.check_call("rosservice call /gazebo/clear_body_wrenches \"body_name: 'ur5'\"", shell=True) 31 | 32 | def reset_world(): 33 | subprocess.check_call("rosservice call /gazebo/reset_world", shell=True) 34 | 35 | 36 | def start_ros_controllers(): 37 | start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER, SwitchController) 38 | req = SwitchControllerRequest() 39 | # this two controller is same as the ur joint trajectory controller 40 | req.start_controllers.append("ur5_cartesian_velocity_controller_sim") 41 | req.start_controllers.append("joint_state_controller") 42 | req.strictness = req.BEST_EFFORT 43 | rospy.wait_for_service(SWITCH_CONTROLLER) 44 | res = start_controllers_client(req) 45 | 46 | if __name__ == "__main__": 47 | pause_gazebo() 48 | set_initial_pose_gazebo() 49 | clear_wrenches() 50 | unpause_gazebo() 51 | start_ros_controllers() 52 | 53 | -------------------------------------------------------------------------------- /ur5_bringup/src/timed_roslaunch.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Script to delay the launch of a roslaunch file 4 | # 5 | # Koen Lekkerkerker 6 | # Thu 24 Apr 2014 7 | # 8 | # Use: ./timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] 9 | # 10 | 11 | function showHelp(){ 12 | echo 13 | echo "This script can delay the launch of a roslaunch file" 14 | echo "Place it in the 'scripts' folder of your catkin package" 15 | echo "and make sure that the file is executable (chmod +x timed_roslaunch.sh)" 16 | echo 17 | echo "Run it from command line:" 18 | echo 19 | echo "Use: ./timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 20 | echo "Or: rosrun [yourpackage] time_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 21 | echo "Example: ./timed_roslaunch.sh 2 turtlebot_navigation amcl_demo.launch initial_pose_x:=17.0 initial_pose_y:=17.0" 22 | echo 23 | echo "Or run it from another roslaunch file:" 24 | echo 25 | echo '' 26 | echo ' ' 27 | echo ' ' 30 | echo ' ' 31 | echo '' 32 | } 33 | 34 | if [ "$1" = "-h" ]; then 35 | showHelp 36 | else 37 | echo "start wait for $1 seconds" 38 | sleep $1 39 | echo "end wait for $1 seconds" 40 | shift 41 | echo "now running 'roslaunch $@'" 42 | roslaunch $@ 43 | fi 44 | -------------------------------------------------------------------------------- /ur5_bringup/urdf/ur5.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ur5_bringup/urdf/ur5_description.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 40 | 41 | 42 | 43 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 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 | 358 | 362 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | true 396 | 397 | 398 | true 399 | 400 | 401 | true 402 | 403 | 404 | true 405 | 406 | 407 | true 408 | 409 | 410 | true 411 | 412 | 413 | true 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | transmission_interface/SimpleTransmission 422 | 423 | ${hw_interface} 424 | 425 | 426 | 1 427 | 428 | 429 | 430 | 431 | transmission_interface/SimpleTransmission 432 | 433 | ${hw_interface} 434 | 435 | 436 | 1 437 | 438 | 439 | 440 | 441 | transmission_interface/SimpleTransmission 442 | 443 | ${hw_interface} 444 | 445 | 446 | 1 447 | 448 | 449 | 450 | 451 | transmission_interface/SimpleTransmission 452 | 453 | ${hw_interface} 454 | 455 | 456 | 1 457 | 458 | 459 | 460 | 461 | transmission_interface/SimpleTransmission 462 | 463 | ${hw_interface} 464 | 465 | 466 | 1 467 | 468 | 469 | 470 | 471 | transmission_interface/SimpleTransmission 472 | 473 | ${hw_interface} 474 | 475 | 476 | 1 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | / 485 | true 486 | 487 | 488 | 489 | -------------------------------------------------------------------------------- /ur5_bringup/worlds/bifurcation.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 0 60 | 0 61 | 1 62 | 63 | 64 | 65 | 0.001 66 | 1 67 | 1000 68 | 0 0 -9.8 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | EARTH_WGS84 77 | 0 78 | 0 79 | 0 80 | 0 81 | 82 | 83 | 1 84 | 85 | 86 | 0 0 1 0 -0 0 87 | 88 | 89 | 1.5 0.8 0.03 90 | 91 | 92 | 93 | 94 | 95 | 0.6 96 | 0.6 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 10 105 | 106 | 107 | 0 0 1 0 -0 0 108 | 109 | 110 | 1.5 0.8 0.03 111 | 112 | 113 | 114 | 118 | 119 | 120 | 121 | 0.68 0.38 0.5 0 -0 0 122 | 123 | 124 | 0.02 125 | 1 126 | 127 | 128 | 10 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 0.68 0.38 0.5 0 -0 0 141 | 142 | 143 | 0.02 144 | 1 145 | 146 | 147 | 148 | 152 | 153 | 154 | 155 | 0.68 -0.38 0.5 0 -0 0 156 | 157 | 158 | 0.02 159 | 1 160 | 161 | 162 | 10 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 0.68 -0.38 0.5 0 -0 0 175 | 176 | 177 | 0.02 178 | 1 179 | 180 | 181 | 182 | 186 | 187 | 188 | 189 | -0.68 -0.38 0.5 0 -0 0 190 | 191 | 192 | 0.02 193 | 1 194 | 195 | 196 | 10 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | -0.68 -0.38 0.5 0 -0 0 209 | 210 | 211 | 0.02 212 | 1 213 | 214 | 215 | 216 | 220 | 221 | 222 | 223 | -0.68 0.38 0.5 0 -0 0 224 | 225 | 226 | 0.02 227 | 1 228 | 229 | 230 | 10 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | -0.68 0.38 0.5 0 -0 0 243 | 244 | 245 | 0.02 246 | 1 247 | 248 | 249 | 250 | 254 | 255 | 256 | 257 | 0 258 | 0 259 | 260 | 0 261 | 0 262 | 1 263 | 264 | 0 1.45575 0 0 -0 0 265 | 266 | 267 | true 268 | 269 | 270 | 0 0 1 0 -0 0 271 | 272 | 273 | 1.5 0.8 0.03 274 | 275 | 276 | 277 | 278 | 279 | 0.6 280 | 0.6 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 10 289 | 290 | 291 | 0 0 1 0 -0 0 292 | 293 | 294 | 1.5 0.8 0.03 295 | 296 | 297 | 298 | 302 | 303 | 304 | 305 | 0.68 0.38 0.5 0 -0 0 306 | 307 | 308 | 0.02 309 | 1 310 | 311 | 312 | 10 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 0.68 0.38 0.5 0 -0 0 325 | 326 | 327 | 0.02 328 | 1 329 | 330 | 331 | 332 | 336 | 337 | 338 | 339 | 0.68 -0.38 0.5 0 -0 0 340 | 341 | 342 | 0.02 343 | 1 344 | 345 | 346 | 10 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 0.68 -0.38 0.5 0 -0 0 359 | 360 | 361 | 0.02 362 | 1 363 | 364 | 365 | 366 | 370 | 371 | 372 | 373 | -0.68 -0.38 0.5 0 -0 0 374 | 375 | 376 | 0.02 377 | 1 378 | 379 | 380 | 10 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | -0.68 -0.38 0.5 0 -0 0 393 | 394 | 395 | 0.02 396 | 1 397 | 398 | 399 | 400 | 404 | 405 | 406 | 407 | -0.68 0.38 0.5 0 -0 0 408 | 409 | 410 | 0.02 411 | 1 412 | 413 | 414 | 10 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | -0.68 0.38 0.5 0 -0 0 427 | 428 | 429 | 0.02 430 | 1 431 | 432 | 433 | 434 | 438 | 439 | 440 | 441 | 0 442 | 0 443 | 444 | 0 445 | 0 446 | 1 447 | 448 | 6 1.48898 0 0 -0 0 449 | 450 | 451 | 452 | 453 | 454 | 455 | model://stop_sign/meshes/stop_sign.dae 456 | 457 | 458 | 10 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | model://stop_sign/meshes/stop_sign.dae 473 | 474 | 475 | 476 | 481 | 482 | 483 | 484 | 0 485 | 0 486 | 487 | 0 488 | 489 | 490 | 1 491 | 0 492 | 0 493 | 1 494 | 0 495 | 1 496 | 497 | 1 498 | 499 | 0 500 | 1 501 | 502 | 3 -2 0 0 -0 0 503 | 1 504 | 505 | 506 | 507 | 3 -2 0 1.5e-05 7e-06 0 508 | 509 | 3 -2 0 1.5e-05 7e-06 0 510 | 0 0 0 0 -0 0 511 | -0 0 -0.010602 -0.107938 -0.303544 0 512 | -0 0 -0.010602 0 -0 0 513 | 514 | 515 | 516 | 0 0 0 0 -0 0 517 | 518 | 0 0 0 0 -0 0 519 | 0 0 0 0 -0 0 520 | 0 0 0 0 -0 0 521 | 0 0 0 0 -0 0 522 | 523 | 524 | 525 | 0 1.45575 0 0 -0 0 526 | 527 | 0 1.45575 0 0 -0 0 528 | 0 0 0 0 -0 0 529 | 0 0 0 0 -0 0 530 | 0 0 0 0 -0 0 531 | 532 | 533 | 534 | 6 1.48898 0 0 -0 0 535 | 536 | 6 1.48898 0 0 -0 0 537 | 0 0 0 0 -0 0 538 | 0 0 0 0 -0 0 539 | 0 0 0 0 -0 0 540 | 541 | 542 | 543 | 544 | 545 | 9.83224 -8.50019 7.46455 0 0.589424 2.35245 546 | orbit 547 | 548 | 549 | 550 | 551 | -------------------------------------------------------------------------------- /ur5_bringup/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ur5_cartesian_velocity_control) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | controller_interface 10 | control_msgs 11 | forward_command_controller 12 | control_toolbox 13 | realtime_tools 14 | urdf 15 | roscpp 16 | kdl_parser 17 | kdl_conversions 18 | ur5_admittance_control 19 | ) 20 | set(CMAKE_CXX_FLAGS "-O2 -O3 -std=c++11 -mtune=native -fPIC -Wall -Wno-deprecated-declarations") 21 | 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES ur5_cartesian_velocity_control 26 | DEPENDS ur5_admittance_control 27 | ) 28 | 29 | include_directories(include ${catkin_INCLUDE_DIRS}) 30 | 31 | 32 | ## Declare a C++ library 33 | # add_library(${PROJECT_NAME} 34 | # src/${PROJECT_NAME}/ur5_cartesian_velocity_control.cpp 35 | # ) 36 | add_library(${PROJECT_NAME} 37 | include/ur5_cartesian_velocity_control/kinematic_chain_controller_base.h 38 | include/ur5_cartesian_velocity_control/cartesian_velocity_controller.h 39 | include/ur5_cartesian_velocity_control/cartesian_state_controller.h 40 | src/cartesian_velocity_controller.cpp 41 | src/cartesian_state_controller.cpp 42 | ) 43 | 44 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 45 | 46 | ## Specify libraries to link a library or executable target against 47 | # target_link_libraries(${PROJECT_NAME}_node 48 | # ${catkin_LIBRARIES} 49 | # ) 50 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 51 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | ur5: 2 | # Controller for the real hardware (velocity interface) 3 | cartesian_velocity_controller: 4 | type: cartesian_controller/CartesianVelocityController 5 | publish_rate: 125 6 | root_name: base_link 7 | tip_name: ee_link 8 | 9 | # Controller for gazebo (velocity interface on top of a position interface) 10 | cartesian_velocity_controller_sim: 11 | type: cartesian_controller/CartesianVelocityControllerSim 12 | publish_rate: 125 13 | root_name: base_link 14 | tip_name: ee_link 15 | 16 | # Cartesian state publisher 17 | cartesian_state_controller: 18 | type: cartesian_controller/CartesianStateController 19 | publish_rate: 125 20 | root_name: base_link 21 | tip_name: ee_link 22 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/include/ur5_cartesian_velocity_control/cartesian_state_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIAN_STATE_CONTROLLER_H 2 | #define CARTESIAN_STATE_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "ur5_cartesian_velocity_control/kinematic_chain_controller_base.h" 11 | #include 12 | 13 | namespace controller_interface 14 | { 15 | 16 | /** \brief This class implements a cartesian state publisher 17 | * for ROS control. 18 | */ 19 | 20 | class CartesianStateController: 21 | public KinematicChainControllerBase< 22 | hardware_interface::JointStateInterface> 23 | { 24 | public: 25 | CartesianStateController() {} 26 | ~CartesianStateController() {} 27 | 28 | /** \brief The init function is called to initialize the controller from a 29 | * non-realtime thread with a pointer to the hardware interface, itself, 30 | * instead of a pointer to a RobotHW. 31 | *s 32 | * \param robot The specific hardware interface used by this controller. 33 | * 34 | * \param n A NodeHandle in the namespace from which the controller 35 | * should read its configuration, and where it should set up its ROS 36 | * interface. 37 | * 38 | * \returns True if initialization was successful and the controller 39 | * is ready to be started. 40 | */ 41 | bool init(hardware_interface::JointStateInterface *robot, ros::NodeHandle &n); 42 | 43 | /** \brief This is called from within the realtime thread just before the 44 | * first call to \ref update 45 | * 46 | * \param time The current time 47 | */ 48 | void starting(const ros::Time& time); 49 | 50 | /*! 51 | * \brief Issues commands to the joint. Called at regular intervals 52 | */ 53 | void update(const ros::Time& time, const ros::Duration& period); 54 | 55 | protected: 56 | ros::Publisher pub_state_; 57 | 58 | KDL::FrameVel x_dot_; 59 | KDL::Frame x_; 60 | 61 | boost::shared_ptr fk_vel_solver_; 62 | boost::shared_ptr fk_pos_solver_; 63 | 64 | boost::shared_ptr > realtime_pub_; 66 | 67 | ros::Time last_publish_time_; 68 | double publish_rate_; 69 | }; 70 | 71 | } // namespace controller_interface 72 | 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/include/ur5_cartesian_velocity_control/cartesian_velocity_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIAN_VELOCITY_CONTROLLER_H 2 | #define CARTESIAN_VELOCITY_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "ur5_cartesian_velocity_control/kinematic_chain_controller_base.h" 15 | #include 16 | 17 | namespace controller_interface 18 | { 19 | 20 | /** \brief This class implements a ROS control cartesian velocity 21 | * controller. Its base class implements the core 22 | * of the controller. 23 | */ 24 | 25 | template 26 | class CartesianVelocityControllerBase: 27 | public KinematicChainControllerBase 28 | { 29 | public: 30 | CartesianVelocityControllerBase() {} 31 | ~CartesianVelocityControllerBase() {} 32 | 33 | /** \brief The init function is called to initialize the controller from a 34 | * non-realtime thread with a pointer to the hardware interface, itself, 35 | * instead of a pointer to a RobotHW. 36 | *s 37 | * \param robot The specific hardware interface used by this controller. 38 | * 39 | * \param n A NodeHandle in the namespace from which the controller 40 | * should read its configuration, and where it should set up its ROS 41 | * interface. 42 | * 43 | * \returns True if initialization was successful and the controller 44 | * is ready to be started. 45 | */ 46 | bool init(T *robot, ros::NodeHandle &n); 47 | 48 | /** \brief This is called from within the realtime thread just before the 49 | * first call to \ref update 50 | * 51 | * \param time The current time 52 | */ 53 | void starting(const ros::Time& time); 54 | 55 | /*! 56 | * \brief Issues commands to the joint. Called at regular intervals 57 | */ 58 | void update(const ros::Time& time, const ros::Duration& period); 59 | 60 | /*! 61 | * \brief Subscriber's callback function 62 | */ 63 | void command_cart_vel(const geometry_msgs::TwistConstPtr &msg); 64 | 65 | /** \brief Write current commands to the hardware interface 66 | */ 67 | virtual void writeVelocityCommands(const ros::Duration& period) = 0; 68 | 69 | protected: 70 | ros::Subscriber sub_command_; // Interface to external commands 71 | 72 | KDL::Twist x_dt_des_; // Desired end-effector velocity 73 | KDL::JntArray q_dt_cmd_; // Desired joint velocity 74 | 75 | boost::shared_ptr ik_vel_solver_; 76 | 77 | boost::shared_ptr > realtime_pub_; 79 | 80 | ros::Time last_publish_time_; 81 | double publish_rate_; 82 | 83 | KDL::FrameVel x_dot_; 84 | KDL::Frame x_; 85 | ur5_admittance_control::PoseTwist msg_state_; 86 | 87 | boost::shared_ptr fk_vel_solver_; 88 | boost::shared_ptr fk_pos_solver_; 89 | }; 90 | 91 | /** \brief Two different instantiations of the CartesianVelocityControllerBase 92 | * are provided here: 93 | * - CartesianVelocityController: with a VelocityJointInterface 94 | * - CartesianVelocityControllerSim: with a PositionJointInterface 95 | * (for gazebo) 96 | */ 97 | 98 | /** The controller instance for the real hardware 99 | * */ 100 | class CartesianVelocityController : 101 | public CartesianVelocityControllerBase< 102 | hardware_interface::VelocityJointInterface> { 103 | /** 104 | * Write velocity commands through a velocity interface 105 | */ 106 | void writeVelocityCommands(const ros::Duration& period); 107 | }; 108 | 109 | /** 110 | * The controller instance for simulationd in gazebo. As of now 111 | * gazebo does not provide a VelocityJointInterface and therefore we simulate 112 | * it here through a PositionJointInterface. 113 | */ 114 | class CartesianVelocityControllerSim : 115 | public CartesianVelocityControllerBase< 116 | hardware_interface::PositionJointInterface> { 117 | 118 | /** 119 | * Write velocity commands through a position interface 120 | */ 121 | void writeVelocityCommands(const ros::Duration& period); 122 | }; 123 | 124 | } // namespace controller_interface 125 | 126 | 127 | #endif 128 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/include/ur5_cartesian_velocity_control/kinematic_chain_controller_base.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATIC_CHAIN_CONTROLLER_BASE_H 2 | #define KINEMATIC_CHAIN_CONTROLLER_BASE_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include //this to compute the gravity vector 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace controller_interface 23 | { 24 | template 25 | class KinematicChainControllerBase: public Controller 26 | { 27 | public: 28 | KinematicChainControllerBase() {} 29 | ~KinematicChainControllerBase() {} 30 | 31 | bool init(JI *robot, ros::NodeHandle &n); 32 | 33 | protected: 34 | ros::NodeHandle nh_; 35 | KDL::Chain kdl_chain_; 36 | KDL::JntArrayVel joint_msr_; 37 | 38 | struct limits_ 39 | { 40 | KDL::JntArray min; 41 | KDL::JntArray max; 42 | KDL::JntArray center; 43 | } joint_limits_; 44 | 45 | std::vector joint_handles_; 46 | }; 47 | 48 | template 49 | bool KinematicChainControllerBase::init(JI *robot, ros::NodeHandle &n) 50 | { 51 | nh_ = n; 52 | 53 | // get URDF and name of root and tip from the parameter server 54 | std::string robot_description, root_name, tip_name; 55 | 56 | std::string name_space = nh_.getNamespace(); 57 | std::cout<< "--------------------> name_space: " << name_space << std::endl; 58 | 59 | if (!ros::param::search(name_space,"robot_description", robot_description)) 60 | { 61 | ROS_ERROR_STREAM("KinematicChainControllerBase: No robot description (URDF)" 62 | "found on parameter server (" << n.getNamespace() << 63 | "/robot_description)"); 64 | return false; 65 | } 66 | 67 | if (!nh_.getParam( name_space + "/root_name", root_name)) 68 | { 69 | ROS_ERROR_STREAM("KinematicChainControllerBase: No root name found on " 70 | "parameter server ("< "< link_ = model.getLink(tip_name); 151 | boost::shared_ptr joint_; 152 | joint_limits_.min.resize(kdl_chain_.getNrOfJoints()); 153 | joint_limits_.max.resize(kdl_chain_.getNrOfJoints()); 154 | joint_limits_.center.resize(kdl_chain_.getNrOfJoints()); 155 | int index; 156 | 157 | for (std::size_t i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++) 158 | { 159 | joint_ = model.getJoint(link_->parent_joint->name); 160 | ROS_INFO("Getting limits for joint: %s", joint_->name.c_str()); 161 | index = kdl_chain_.getNrOfJoints() - i - 1; 162 | 163 | if(joint_->limits){ 164 | joint_limits_.min(index) = joint_->limits->lower; 165 | joint_limits_.max(index) = joint_->limits->upper; 166 | joint_limits_.center(index) = (joint_limits_.min(index) + 167 | joint_limits_.max(index))/2; 168 | }else{ 169 | joint_limits_.min(index) = 0; 170 | joint_limits_.max(index) = 0; 171 | joint_limits_.center(index) = 0; 172 | ROS_INFO("joint_->limits is NULL %s",joint_->name.c_str()); 173 | } 174 | 175 | link_ = model.getLink(link_->getParent()->name); 176 | } 177 | 178 | ROS_INFO("Getting joint handles"); 179 | // Get joint handles for all of the joints in the chain 180 | int count=0; 181 | for(std::vector::const_iterator it = 182 | kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it) 183 | { 184 | 185 | ROS_INFO("%s type: %s", it->getJoint().getName().c_str(), 186 | it->getJoint().getTypeName().c_str() ); 187 | if(it->getJoint().getTypeName() != "None" && count < 7) { 188 | joint_handles_.push_back(robot->getHandle(it->getJoint().getName())); 189 | } 190 | count++; 191 | } 192 | 193 | ROS_INFO("Number of joints in handle = %lu", joint_handles_.size() ); 194 | ROS_INFO_STREAM("kdl_chain.getNrOfJoints: " << kdl_chain_.getNrOfJoints()); 195 | 196 | ROS_INFO("Finished Kinematic Base init"); 197 | 198 | return true; 199 | } 200 | 201 | } 202 | 203 | #endif 204 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/launch/load_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 　　　　　　　 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ur5_cartesian_velocity_control 4 | 0.0.1 5 | The ur5_cartesian_velocity_control package 6 | 7 | 8 | 9 | 10 | Jose Medina 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | catkin 19 | controller_interface 20 | control_msgs 21 | control_toolbox 22 | realtime_tools 23 | urdf 24 | forward_command_controller 25 | kdl_parser 26 | kdl_conversions 27 | cmake_modules 28 | eigen 29 | ur5_admittance_control 30 | 31 | controller_interface 32 | control_msgs 33 | control_toolbox 34 | realtime_tools 35 | urdf 36 | forward_command_controller 37 | kdl_parser 38 | kdl_conversions 39 | eigen 40 | ur5_admittance_control 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/src/cartesian_state_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ur5_cartesian_velocity_control/kinematic_chain_controller_base.h" 3 | #include "ur5_cartesian_velocity_control/cartesian_state_controller.h" 4 | #include "kdl_conversions/kdl_msg.h" 5 | 6 | 7 | namespace controller_interface 8 | { 9 | /** \brief Initialize the kinematic chain for kinematics-based computation. 10 | * 11 | */ 12 | 13 | bool CartesianStateController::init( 14 | hardware_interface::JointStateInterface *robot, ros::NodeHandle &n) { 15 | 16 | KinematicChainControllerBase:: 17 | init(robot, n); 18 | 19 | fk_vel_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_)); 20 | fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); 21 | 22 | // get publishing period 23 | if (!n.getParam("publish_rate", publish_rate_)){ 24 | ROS_ERROR("Parameter 'publish_rate' not set"); 25 | return false; 26 | } 27 | realtime_pub_.reset(new realtime_tools::RealtimePublisher 28 | (n, "ee_state", 4)); 29 | 30 | x_.p.Zero(); 31 | x_.M.Identity(); 32 | x_dot_.p.Zero(); 33 | x_dot_.M.Identity(); 34 | 35 | return true; 36 | } 37 | 38 | /** \brief This is called from within the realtime thread just before the 39 | * first call to \ref update 40 | * 41 | * \param time The current time 42 | */ 43 | void CartesianStateController::starting(const ros::Time& time) { 44 | last_publish_time_ = time; 45 | } 46 | 47 | /*! 48 | * \brief Issues commands to the joint. Should be called at regular intervals 49 | */ 50 | void CartesianStateController::update(const ros::Time& time, 51 | const ros::Duration& period) { 52 | 53 | // Get joint positions 54 | for(std::size_t i=0; i < joint_handles_.size(); i++) 55 | { 56 | joint_msr_.q(i) = joint_handles_[i].getPosition(); 57 | joint_msr_.qdot(i) = joint_handles_[i].getVelocity(); 58 | } 59 | 60 | // Compute forward kinematics 61 | fk_vel_solver_->JntToCart(joint_msr_, x_dot_); 62 | fk_pos_solver_->JntToCart(joint_msr_.q, x_); 63 | 64 | // Limit rate of publishing 65 | if (publish_rate_ > 0.0 && last_publish_time_ 66 | + ros::Duration(1.0/publish_rate_) < time) { 67 | 68 | // try to publish 69 | if (realtime_pub_->trylock()) { 70 | // we're actually publishing, so increment time 71 | last_publish_time_ = last_publish_time_ 72 | + ros::Duration(1.0/publish_rate_); 73 | 74 | // populate message 75 | realtime_pub_->msg_.header.stamp = time; 76 | tf::poseKDLToMsg(x_, realtime_pub_->msg_.pose); 77 | tf::twistKDLToMsg(x_dot_.GetTwist(), realtime_pub_->msg_.twist); 78 | 79 | realtime_pub_->unlockAndPublish(); 80 | } 81 | } 82 | 83 | } 84 | 85 | } // controller_interface 86 | 87 | // Register controllers with the PLUGINLIB_EXPORT_CLASS macro to enable dynamic 88 | // loading with the controller manager 89 | PLUGINLIB_EXPORT_CLASS(controller_interface::CartesianStateController, controller_interface::ControllerBase) 90 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/src/cartesian_velocity_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ur5_cartesian_velocity_control/kinematic_chain_controller_base.h" 3 | #include "ur5_cartesian_velocity_control/cartesian_velocity_controller.h" 4 | #include "kdl_conversions/kdl_msg.h" 5 | 6 | namespace controller_interface 7 | { 8 | /** \brief Initialize the kinematic chain for kinematics-based computation. 9 | * 10 | */ 11 | template 12 | bool CartesianVelocityControllerBase::init( 13 | T *robot, ros::NodeHandle &n) { 14 | 15 | // KDL 16 | KinematicChainControllerBase::init(robot, n); 17 | ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv_givens(this->kdl_chain_)); 18 | fk_vel_solver_.reset(new KDL::ChainFkSolverVel_recursive(this->kdl_chain_)); 19 | fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(this->kdl_chain_)); 20 | 21 | // get publishing period 22 | if (!n.getParam("publish_rate", publish_rate_)){ 23 | ROS_ERROR("Parameter 'publish_rate' not set"); 24 | return false; 25 | } 26 | realtime_pub_.reset(new realtime_tools::RealtimePublisher 27 | (n, "ee_state", 4)); 28 | 29 | 30 | // Topics 31 | sub_command_ = n.subscribe("command_cart_vel", 5, 32 | &CartesianVelocityControllerBase::command_cart_vel, 33 | this,ros::TransportHints().reliable().tcpNoDelay()); 34 | 35 | // Variable init 36 | this->joint_msr_.resize(this->kdl_chain_.getNrOfJoints()); 37 | q_dt_cmd_.resize(this->kdl_chain_.getNrOfJoints()); 38 | x_dt_des_ = KDL::Twist::Zero(); 39 | x_.p.Zero(); 40 | x_.M.Identity(); 41 | x_dot_.p.Zero(); 42 | x_dot_.M.Identity(); 43 | 44 | return true; 45 | } 46 | 47 | /** \brief This is called from within the realtime thread just before the 48 | * first call to \ref update 49 | * 50 | * \param time The current time 51 | */ 52 | template 53 | void CartesianVelocityControllerBase::starting(const ros::Time& time){ 54 | for(std::size_t i=0; i < this->joint_handles_.size(); i++) { 55 | q_dt_cmd_(i) = 0.0; 56 | } 57 | x_dt_des_ = KDL::Twist::Zero(); 58 | last_publish_time_ = time; 59 | } 60 | 61 | /*! 62 | * \brief Issues commands to the joint. Should be called at regular intervals 63 | */ 64 | template 65 | void CartesianVelocityControllerBase::update(const ros::Time& time, 66 | const ros::Duration& period) { 67 | 68 | // Get joint positions 69 | for(std::size_t i=0; i < this->joint_handles_.size(); i++) 70 | { 71 | this->joint_msr_.q(i) = this->joint_handles_[i].getPosition(); 72 | this->joint_msr_.qdot(i) = this->joint_handles_[i].getVelocity(); 73 | } 74 | 75 | // Compute inverse kinematics velocity solver 76 | ik_vel_solver_->CartToJnt(this->joint_msr_.q, x_dt_des_, q_dt_cmd_); 77 | writeVelocityCommands(period); 78 | 79 | // Forward kinematics 80 | fk_vel_solver_->JntToCart(this->joint_msr_, x_dot_); 81 | fk_pos_solver_->JntToCart(this->joint_msr_.q, x_); 82 | 83 | // Limit rate of publishing 84 | if (publish_rate_ > 0.0 && last_publish_time_ 85 | + ros::Duration(1.0/publish_rate_) < time) { 86 | 87 | // try to publish 88 | if (realtime_pub_->trylock()) { 89 | // we're actually publishing, so increment time 90 | last_publish_time_ = last_publish_time_ 91 | + ros::Duration(1.0/publish_rate_); 92 | 93 | // populate message 94 | realtime_pub_->msg_.header.stamp = time; 95 | tf::poseKDLToMsg(x_, realtime_pub_->msg_.pose); 96 | tf::twistKDLToMsg(x_dot_.GetTwist(), realtime_pub_->msg_.twist); 97 | 98 | realtime_pub_->unlockAndPublish(); 99 | } 100 | } 101 | } 102 | 103 | /*! 104 | * \brief Subscriber's callback: copies twist commands 105 | */ 106 | template 107 | void CartesianVelocityControllerBase::command_cart_vel( 108 | const geometry_msgs::TwistConstPtr &msg) { 109 | x_dt_des_.vel(0) = msg->linear.x; 110 | x_dt_des_.vel(1) = msg->linear.y; 111 | x_dt_des_.vel(2) = msg->linear.z; 112 | x_dt_des_.rot(0) = msg->angular.x; 113 | x_dt_des_.rot(1) = msg->angular.y; 114 | x_dt_des_.rot(2) = msg->angular.z; 115 | } 116 | 117 | 118 | /********************************************/ 119 | /**FUNCTIONS OF INSTANCES OF THE BASE CLASS**/ 120 | /********************************************/ 121 | 122 | /** \brief write the desired velocity command in the hardware interface input 123 | * for a VelocityJointInterface 124 | * \param period The duration of an update cycle 125 | */ 126 | void CartesianVelocityController::writeVelocityCommands( 127 | const ros::Duration& period) { 128 | for(std::size_t i=0; i < this->joint_handles_.size(); i++) { 129 | this->joint_handles_[i].setCommand(q_dt_cmd_(i)); 130 | } 131 | } 132 | 133 | /** \brief write the desired velocity command in the hardware interface input 134 | * for a PosititionJointInterface 135 | * \param period The duration of an update cycle 136 | */ 137 | void CartesianVelocityControllerSim::writeVelocityCommands( 138 | const ros::Duration& period) { 139 | for(std::size_t i=0; i < this->joint_handles_.size(); i++) { 140 | this->joint_handles_[i].setCommand(this->joint_msr_.q(i) 141 | + q_dt_cmd_(i)*period.toSec()); 142 | } 143 | } 144 | 145 | } // controller_interface namespace 146 | 147 | // Register controllers with the PLUGINLIB_EXPORT_CLASS macro to enable dynamic 148 | // loading with the controller manager 149 | PLUGINLIB_EXPORT_CLASS(controller_interface::CartesianVelocityController, 150 | controller_interface::ControllerBase) 151 | PLUGINLIB_EXPORT_CLASS(controller_interface::CartesianVelocityControllerSim, 152 | controller_interface::ControllerBase) 153 | -------------------------------------------------------------------------------- /ur5_cartesian_velocity_control/ur5_cartesian_velocity_control_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cartesian velocity control 5 | 6 | 7 | 8 | 9 | 10 | Cartesian state observer 11 | 12 | 13 | 14 | 15 | 16 | Cartesian velocity control using a position interface. This is used in conjunction with Gazebo which, at the moment of writing this code, does not fully support a joint velocity interface. 17 | 18 | 19 | 20 | --------------------------------------------------------------------------------