├── .gitignore ├── .vscode └── settings.json ├── CMakeLists.txt ├── Config.cmake.in ├── demos ├── ati.py └── plot_ati_log.py ├── doc ├── 20170925_angebot_schunk_ati_sensor.PDF ├── 20170925_schreiben_exklusivvertrieb_schunk.pdf ├── 9610-05-1022 Quick Start.pdf ├── images │ ├── ati_mini_40.jpg │ └── editing_wired_connection.png ├── mini_40_datasheet.pdf ├── mini_40_drawing.pdf └── net_box_9105_a.jpg ├── include └── AtiFTSensor.h ├── license.txt ├── package.xml ├── python └── ati_ft_sensor │ └── __init__.py ├── readme.md ├── src └── AtiFTSensor.cpp ├── srcpy └── ati_ft_sensor_cpp.cpp └── tests ├── test_sensor.cpp └── test_sensor_stream.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.swp 3 | build/ 4 | docs/ 5 | tags 6 | *.user 7 | *build* -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "core": "cpp" 4 | } 5 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019, New York University and Max Planck Gesellschaft. 3 | # 4 | # License BSD-3 clause 5 | # 6 | 7 | # 8 | # set up the project 9 | # 10 | cmake_minimum_required(VERSION 3.10.2) 11 | 12 | project(ati_ft_sensor) 13 | 14 | # Using C++17 15 | if(NOT CMAKE_C_STANDARD) 16 | set(CMAKE_C_STANDARD 99) 17 | endif() 18 | if(NOT CMAKE_CXX_STANDARD) 19 | set(CMAKE_CXX_STANDARD 17) 20 | endif() 21 | 22 | # 23 | # Dependencies 24 | # 25 | 26 | # depend on ament macros 27 | find_package(mpi_cmake_modules REQUIRED) 28 | find_package(real_time_tools REQUIRED) 29 | find_package(Boost REQUIRED COMPONENTS thread) 30 | find_package(Eigen3 REQUIRED) 31 | find_package(pybind11 REQUIRED) 32 | 33 | set(Xenomai_additionnal_INCLUDE_DIR) 34 | if(${CURRENT_OS} MATCHES "xenomai") 35 | set(Xenomai_additionnal_INCLUDE_DIR /usr/local/rtnet/include 36 | /usr/src/rtnet/stack/include) 37 | endif() 38 | 39 | 40 | set(all_targets) 41 | 42 | # 43 | # Add the main library 44 | # 45 | # Create the CMake target 46 | add_library(${PROJECT_NAME} SHARED src/AtiFTSensor.cpp) 47 | # Add the include dependencies 48 | target_include_directories( 49 | ${PROJECT_NAME} 50 | PUBLIC $ 51 | $ ${Xenomai_INCLUDE_DIR} 52 | ${Xenomai_additionnal_INCLUDE_DIR}) 53 | # Link the dependencies 54 | target_link_libraries(${PROJECT_NAME} Boost::boost) 55 | target_link_libraries(${PROJECT_NAME} Boost::thread) 56 | target_link_libraries(${PROJECT_NAME} real_time_tools::real_time_tools) 57 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) 58 | target_link_libraries(${PROJECT_NAME} ${Xenomai_LIBS}) 59 | # For the installation 60 | list(APPEND all_targets ${PROJECT_NAME}) 61 | 62 | # 63 | # Building some utilities 64 | # 65 | if(Xenomai_FOUND) 66 | add_executable(ati_ft_sensor_test_sensor_stream tests/test_sensor_stream.cpp) 67 | target_link_libraries(ati_ft_sensor_test_sensor_stream ${PROJECT_NAME}) 68 | list(APPEND all_targets ati_ft_sensor_test_sensor_stream) 69 | endif() 70 | 71 | add_executable(ati_ft_sensor_test_sensor tests/test_sensor.cpp) 72 | target_link_libraries(ati_ft_sensor_test_sensor ${PROJECT_NAME}) 73 | list(APPEND all_targets ati_ft_sensor_test_sensor) 74 | 75 | # 76 | # building documentation 77 | # 78 | add_documentation() 79 | 80 | # python bindings 81 | set(py_ati_core_SRC_FILES 82 | srcpy/ati_ft_sensor_cpp.cpp 83 | ) 84 | 85 | pybind11_add_module(ati_ft_sensor_cpp MODULE ${py_ati_core_SRC_FILES}) 86 | target_link_libraries(ati_ft_sensor_cpp PRIVATE pybind11::module) 87 | target_link_libraries(ati_ft_sensor_cpp PRIVATE ati_ft_sensor) 88 | 89 | # install the bindings 90 | get_python_install_dir(python_install_dir) 91 | install(TARGETS ati_ft_sensor_cpp DESTINATION ${python_install_dir}) 92 | 93 | # install python package 94 | install( 95 | DIRECTORY python 96 | DESTINATION "${python_install_dir}" 97 | PATTERN "*.pyc" EXCLUDE 98 | PATTERN "__pycache__" EXCLUDE 99 | ) 100 | 101 | # 102 | # Install the package 103 | # 104 | 105 | # install the include directory 106 | install(DIRECTORY include/ DESTINATION include) 107 | 108 | # command to install the library and binaries 109 | install( 110 | TARGETS ${all_targets} 111 | EXPORT ${PROJECT_NAME}Targets 112 | LIBRARY DESTINATION lib 113 | ARCHIVE DESTINATION lib 114 | RUNTIME DESTINATION bin 115 | INCLUDES 116 | DESTINATION include) 117 | 118 | # Export this package as a cmake package. 119 | generate_cmake_package() -------------------------------------------------------------------------------- /Config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 4 | 5 | include(CMakeFindDependencyMacro) 6 | 7 | # we do not add the other dependencies because these are header files lib 8 | if(${CMAKE_VERSION} VERSION_LESS "3.15.0") 9 | find_package(real_time_tools REQUIRED) 10 | find_package(Eigen3 REQUIRED) 11 | find_package(Boost REQUIRED COMPONENTS thread) 12 | else() 13 | find_dependency(real_time_tools REQUIRED) 14 | find_dependency(Eigen3 REQUIRED) 15 | find_dependency(Boost REQUIRED COMPONENTS thread) 16 | endif() 17 | 18 | check_required_components(@PROJECT_NAME@) -------------------------------------------------------------------------------- /demos/ati.py: -------------------------------------------------------------------------------- 1 | # Demo to get ati sensor readings in python 2 | 3 | from ati_ft_sensor_cpp import AtiFTSensor 4 | 5 | sensor = AtiFTSensor() 6 | 7 | sensor.initialize() 8 | 9 | while True: 10 | ft = sensor.getFT() 11 | print(ft) -------------------------------------------------------------------------------- /demos/plot_ati_log.py: -------------------------------------------------------------------------------- 1 | import os,sys 2 | from pylab import * 3 | 4 | data = loadtxt('log_file.dat') 5 | 6 | t = data[:,0]/1000.0 7 | 8 | figure() 9 | for i in range(3): 10 | subplot(3,1,i+1) 11 | plot(t,data[:,4+i]) 12 | title('Force [N]') 13 | figure() 14 | for i in range(3): 15 | subplot(3,1,i+1) 16 | plot(t,data[:,7+i]) 17 | title('torque [Nm]') 18 | figure() 19 | subplot(4,1,1) 20 | plot(data[:,0]) 21 | xlabel('sequence') 22 | subplot(4,1,2) 23 | plot(diff(data[:,0])) 24 | xlabel('diff sequence') 25 | subplot(4,1,3) 26 | plot(data[:,2]) 27 | xlabel('status') 28 | subplot(4,1,4) 29 | plot(data[:,3]) 30 | xlabel('dt [s]') 31 | 32 | 33 | show() -------------------------------------------------------------------------------- /doc/20170925_angebot_schunk_ati_sensor.PDF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/20170925_angebot_schunk_ati_sensor.PDF -------------------------------------------------------------------------------- /doc/20170925_schreiben_exklusivvertrieb_schunk.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/20170925_schreiben_exklusivvertrieb_schunk.pdf -------------------------------------------------------------------------------- /doc/9610-05-1022 Quick Start.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/9610-05-1022 Quick Start.pdf -------------------------------------------------------------------------------- /doc/images/ati_mini_40.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/images/ati_mini_40.jpg -------------------------------------------------------------------------------- /doc/images/editing_wired_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/images/editing_wired_connection.png -------------------------------------------------------------------------------- /doc/mini_40_datasheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/mini_40_datasheet.pdf -------------------------------------------------------------------------------- /doc/mini_40_drawing.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/mini_40_drawing.pdf -------------------------------------------------------------------------------- /doc/net_box_9105_a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/doc/net_box_9105_a.jpg -------------------------------------------------------------------------------- /include/AtiFTSensor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file AtiFTSensor.h 3 | * @author Ludovic Righetti 4 | * @license License BSD-3-Clause 5 | * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft. 6 | * @date 2013-10-22 7 | */ 8 | 9 | #ifndef ATIFTSENSOR_H_ 10 | #define ATIFTSENSOR_H_ 11 | #include 12 | #include 13 | #include 14 | #ifdef XENOMAI 15 | #include 16 | #include 17 | #include 18 | #else 19 | #include // std::mutex 20 | #endif 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace ati_ft_sensor 28 | { 29 | 30 | class AtiFTSensor{ 31 | public: 32 | AtiFTSensor(); 33 | ~AtiFTSensor(); 34 | 35 | bool initialize(); 36 | 37 | void getStatus(uint32_t& rdt_seq, uint32_t& ft_seq, uint32_t& status); 38 | void getFT(double* force, double* torque); 39 | const Eigen::Ref> getFT_vector(); 40 | void stream(bool stream); 41 | void stop(); 42 | void setBias(); 43 | void setBias(double* force, double* torque); 44 | void resetBias(); 45 | 46 | double F_[3]; 47 | double T_[3]; 48 | uint32_t rdt_sequence_; 49 | uint32_t ft_sequence_; 50 | uint32_t status_; 51 | Eigen::Matrix force_torque_; 52 | 53 | // Streaming support is only available on xenomai. 54 | #ifdef XENOMAI 55 | struct steaming_msg 56 | { 57 | uint32_t rdt_seq; 58 | uint32_t ft_seq; 59 | uint32_t status; 60 | double time; 61 | double F[3]; 62 | double T[3]; 63 | }; 64 | #endif 65 | 66 | private: 67 | 68 | static constexpr double count_per_force_ = 1000000.0; 69 | static constexpr double count_per_torque_ = 1000000.0; 70 | 71 | struct received_msg 72 | { 73 | uint32_t rdt_sequence; // RDT sequence number of this packet. 74 | uint32_t ft_sequence; // The record’s internal sequence number 75 | uint32_t status; // System status code 76 | // Force and torque readings use counts values 77 | int32_t Fx; // X-axis force 78 | int32_t Fy; // Y-axis force 79 | int32_t Fz; // Z-axis force 80 | int32_t Tx; // X-axis torque 81 | int32_t Ty; // Y-axis torque 82 | int32_t Tz; // Z-axis torque 83 | }; 84 | 85 | struct send_msg 86 | { 87 | uint16_t command_header; // = 0x1234 // Required 88 | uint16_t command; // Command to execute 89 | uint32_t sample_count; // Samples to output (0 = infinite) 90 | 91 | }; 92 | 93 | static THREAD_FUNCTION_RETURN_TYPE read_ft(void* instance_pointer) 94 | { 95 | ((AtiFTSensor*)(instance_pointer))->read_ft(); 96 | return THREAD_FUNCTION_RETURN_VALUE; 97 | } 98 | 99 | void read_ft(); 100 | 101 | sockaddr_in local_address_, remote_address_; 102 | int socket_; 103 | real_time_tools::RealTimeThread reading_thread_; 104 | 105 | double F_bias_[3], T_bias_[3]; 106 | 107 | #ifdef XENOMAI 108 | RT_MUTEX mutex_; 109 | RT_PIPE stream_pipe_; 110 | #else 111 | std::mutex mutex_; 112 | #endif 113 | 114 | bool initialized_; 115 | bool going_; 116 | bool streaming_; 117 | }; 118 | 119 | } 120 | 121 | 122 | #endif /* ATIFTSENSOR_H_ */ 123 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, New York University and Max Planck Gesellschaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | ati_ft_sensor 10 | 1.0.0 11 | 12 | 13 | Provide drivers for the ATI force torque sensor. 14 | 15 | 16 | Ludovic Righetti 17 | Vincent Berenz 18 | 19 | BSD 3-clause 20 | 21 | 22 | ament_cmake 23 | 24 | mpi_cmake_modules 25 | 26 | 27 | Boost 28 | 29 | real_time_tools 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /python/ati_ft_sensor/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/ati_ft_sensor/73ec8288cfd3fc207686ce50cf59cda85a2b98f6/python/ati_ft_sensor/__init__.py -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # ati_ft_sensor 2 |
3 | Interface using internet connexion to a ATI force torque sensor. 4 | 5 | ### Installation 6 | 7 | #### Standard dependencies 8 | 9 | This package is based on [ROS2](https://docs.ros.org/). 10 | Currently based on ROS2 dashing release. 11 | 12 | #### Download the pacakge 13 | 14 | Use the `git clone` or [treep](https://gitlab.is.tue.mpg.de/amd-clmc/treep) 15 | using the [treep_machines_in_motion](https://github.com/machines-in-motion/treep_machines_in_motion) configuration. 16 | 17 | In both case do not forget to **register your ssh public key in your settings**: 18 | 19 | In short you can either: 20 | - use git directly: 21 | ``` 22 | mkdir -p ~/devel/workspace/src 23 | cd ~/devel/workspace/src 24 | git clone git@github.com:open-dynamic-robot-initiative/ati_ft_sensor.git 25 | ``` 26 | - or use treep: 27 | ``` 28 | mkdir -p ~/devel 29 | pip install treep 30 | cd ~/devel 31 | git clone git@github.com:machines-in-motion/treep_machines_in_motion.git 32 | treep --clone ati_ft_sensor 33 | ``` 34 | 35 | #### Build the package 36 | 37 | We use [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) 38 | to build this package: 39 | ``` 40 | cd mkdir -p ~/devel/workspace 41 | colcon build 42 | ``` 43 | 44 | ### Usage 45 | 46 | #### Usage with rt_preempt 47 | 48 | The ATI FT sensor is connected to the control PC via ethernet. To access the ATI sensor from the computer, the network connection to the ATI sensor must be configured. 49 | 50 | - On Ubuntu, open the application "Network connections" 51 | - Note: Please use ifconfig to identify the port to which the ethernet cable is connected. To verify if you have identified the right port check if the Hwaddress in Ifconfig matches with the number in the General tab in "Network connection". 52 | - If you configure the wrong port, the computer will not be able to connect with the internet. In which case, undo your changes and run "sudo service network-manager restart". 53 | - Select the network connection which connects to the ATI sensor. For this, you might want to use the command ifconfig on the terminal to see the mac addresses of all network devices. 54 | - In the "IPv4 Settings" tab, specify the network connection parameters manually by switching "Method" to "Manual" and put in the values as shown in the screenshot 55 |
56 | 57 | ##### Note 58 | > :warning: In case you installed an LDAP account on the computer be aware that configuring the network card as described above might cause problems when rebooting the PC. Particularly, Ubuntu tries to connect to LDAP using the network settings from the hardcoded network connection, which will not work. In case you experience such issues at the first logging after a reboot, remove the network connection to the ATI sensor and restart the computer. Once you manged to login, connect the ATI sensor again. 59 | 60 | > :warning: In case after restart you are unable to login to you LDAP account, please login to administrator(if you do not know the login credentials, please ask Vincent), and run "sudo service network-manager restart" and then restart the computer if necessary. 61 | 62 | > :warning: In case of socket error: ```cannot bind socket, error: 98, Address already in usehardware communication loop started``` 63 | Make sure all of the processes for dynamic_graph and ATI sensor are killed. 64 | 65 | #### Demos/Examples 66 | You find examples of how to stream data in the tests/ folder. 67 | 68 | You can run it via 69 | ``` 70 | ait_ft_sensor_test_sensor 71 | ``` 72 | or (with ROS2) 73 | ``` 74 | ros2 run ati_ft_sensor ati_ft_sensor_test_sensor 75 | ``` 76 | You also can run it with Xenomai by adding `_stream` to the previous commands. 77 | 78 | The output would be like 79 | ``` 80 | rdt_seq=153674, ft_seq=674807363, status=0 81 | -0.03 0.03 -0.01 0.00 0.00 -0.00 82 | ``` 83 | 84 | ### Hardware specs 85 | You can find more detail in below links: 86 | - [Datasheet](doc/mini_40_datasheet.pdf) 87 | - [Technical Drawing Sensor](doc/mini_40_drawing.pdf) 88 | - [Thecnical Drawing Net Box](doc/net_box_9105_a.jpg) 89 | - [Quote](20170925_angebot_schunk_ati_sensor.PDF) 90 | - [Schunk - Exclusive Distributor](20170925_schreiben_exklusivvertrieb_schunk.pdf) 91 | - [Quick Start Guide](9610-05-1022%20Quick%20Start.pdf) 92 | These information is downloaded from the manufacturer's website, and we are not responsible for any information that they contain. 93 | 94 | ### Original Authors 95 | 96 | - Ludovic Righetti 97 | - Alexander Herzog 98 | 99 | ### License and Copyrights 100 | 101 | License BSD-3-Clause 102 | Copyright (c) 2019, New York University and Max Planck Gesellschaft. 103 | -------------------------------------------------------------------------------- /src/AtiFTSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AtiFTSensor.cpp 3 | * 4 | * Created on: Oct 22, 2013 5 | * Author: righetti 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #ifdef XENOMAI 13 | #include 14 | #include 15 | #else 16 | // Define xenomai like API for normal linux using defines. 17 | #define rt_dev_socket socket 18 | #define rt_dev_ioctl ioctl 19 | #define rt_dev_close close 20 | #define rt_dev_setsockopt setsockopt 21 | #define rt_dev_bind bind 22 | #define rt_dev_recv recv 23 | #define rt_dev_recvmsg recvmsg 24 | #define rt_dev_send send 25 | #define rt_dev_sendto sendto 26 | #define rt_dev_connect connect 27 | 28 | #define rt_mutex_acquire(mutex, d) ((mutex)->lock()) 29 | #define rt_mutex_release(mutex) ((mutex)->unlock()) 30 | 31 | #endif 32 | #include 33 | 34 | namespace ati_ft_sensor 35 | { 36 | AtiFTSensor::AtiFTSensor() 37 | { 38 | initialized_ = false; 39 | } 40 | 41 | bool AtiFTSensor::initialize() 42 | { 43 | if(initialized_) 44 | { 45 | printf("warning already initialized\n"); 46 | return true; 47 | } 48 | printf("initializing\n"); 49 | 50 | //init some values 51 | for(int i=0; i<3; ++i) 52 | { 53 | F_bias_[i] = 0.0; 54 | T_bias_[i] = 0.0; 55 | } 56 | initialized_ = false; 57 | going_ = true; 58 | streaming_ = false; 59 | 60 | //setup the networking sockets 61 | memset(&local_address_, 0, sizeof(struct sockaddr_in)); 62 | memset(&remote_address_, 0, sizeof(struct sockaddr_in)); 63 | local_address_.sin_family = AF_INET; 64 | local_address_.sin_addr.s_addr = INADDR_ANY; 65 | local_address_.sin_port = htons(49152); 66 | 67 | remote_address_.sin_family = AF_INET; 68 | inet_aton("192.168.4.1",&(remote_address_.sin_addr)); 69 | remote_address_.sin_port = htons(49152); 70 | 71 | socket_ = rt_dev_socket(AF_INET, SOCK_DGRAM, 0); 72 | if(socket_<0) 73 | { 74 | printf("cannot create socket, error %d, %s\n",errno, strerror(errno)); 75 | return false; 76 | } 77 | if(rt_dev_bind(socket_, (struct sockaddr *) &local_address_, sizeof(struct sockaddr_in)) < 0) 78 | { 79 | printf("cannot bind socket, error: %d, %s\n", errno, strerror(errno)); 80 | return false; 81 | } 82 | if(rt_dev_connect(socket_, (struct sockaddr *) &remote_address_, sizeof(struct sockaddr_in)) < 0) 83 | { 84 | printf("cannot connect socket, error: %d, %s\n", errno, strerror(errno)); 85 | return false; 86 | } 87 | 88 | printf("created sockets\n"); 89 | 90 | #ifdef XENOMAI 91 | if(rt_pipe_create(&stream_pipe_, "ati_ft_stream",P_MINOR_AUTO,0)) 92 | { 93 | printf("cannot create pipe, error: %d, %s\n", errno, strerror(errno)); 94 | return false; 95 | } 96 | 97 | //create mutex for the threads 98 | rt_mutex_create(&mutex_,NULL); 99 | #endif 100 | 101 | //now create the reading thread 102 | reading_thread_.create_realtime_thread( 103 | &ati_ft_sensor::AtiFTSensor::read_ft, this); 104 | 105 | initialized_ = true; 106 | return initialized_; 107 | } 108 | 109 | void AtiFTSensor::read_ft() 110 | { 111 | //send message to start streaming 112 | send_msg msg; 113 | msg.command_header = htons(0x1234); 114 | msg.command = htons(0x0002); 115 | msg.sample_count = 0; 116 | rt_dev_send(socket_, &msg, sizeof(msg), 0); 117 | 118 | //some internal variables 119 | bool internal_going = true; 120 | 121 | #ifdef XENOMAI 122 | RTIME time1, time2; 123 | time2 = rt_timer_read(); 124 | #endif 125 | 126 | //the main reading loop 127 | while(internal_going) 128 | { 129 | #ifdef XENOMAI 130 | time1 = rt_timer_read(); 131 | #endif 132 | 133 | //read the socket 134 | received_msg rcv_msg; 135 | ssize_t response = rt_dev_recv(socket_, &rcv_msg, sizeof(rcv_msg), 0); 136 | if(response != sizeof(rcv_msg)) 137 | { 138 | printf("Received message of unexpected length %ld\n", response); 139 | } 140 | 141 | //update state / first get the byte order right 142 | rt_mutex_acquire(&mutex_,TM_INFINITE); 143 | rdt_sequence_ = ntohl(rcv_msg.rdt_sequence); 144 | ft_sequence_ = ntohl(rcv_msg.ft_sequence); 145 | status_ = ntohl(rcv_msg.status); 146 | rcv_msg.Fx = ntohl(rcv_msg.Fx); 147 | rcv_msg.Fy = ntohl(rcv_msg.Fy); 148 | rcv_msg.Fz = ntohl(rcv_msg.Fz); 149 | rcv_msg.Tx = ntohl(rcv_msg.Tx); 150 | rcv_msg.Ty = ntohl(rcv_msg.Ty); 151 | rcv_msg.Tz = ntohl(rcv_msg.Tz); 152 | F_[0] = double(rcv_msg.Fx)/count_per_force_ - F_bias_[0]; 153 | F_[1] = double(rcv_msg.Fy)/count_per_force_ - F_bias_[1]; 154 | F_[2] = double(rcv_msg.Fz)/count_per_force_ - F_bias_[2]; 155 | T_[0] = double(rcv_msg.Tx)/count_per_torque_ - T_bias_[0]; 156 | T_[1] = double(rcv_msg.Ty)/count_per_torque_ - T_bias_[1]; 157 | T_[2] = double(rcv_msg.Tz)/count_per_torque_ - T_bias_[2]; 158 | 159 | //if streaming mode, copy to pipe 160 | if(streaming_) 161 | { 162 | #ifdef XENOMAI 163 | steaming_msg log; 164 | log.rdt_seq = rdt_sequence_; 165 | log.ft_seq = ft_sequence_; 166 | log.status = status_; 167 | for(int i=0; i<3; ++i) 168 | { 169 | log.F[i] = F_[i]; 170 | log.T[i] = T_[i]; 171 | } 172 | log.time = double(time1-time2)/10e9; 173 | time2 = time1; 174 | rt_pipe_write(&stream_pipe_,&log,sizeof(log), P_NORMAL); 175 | #else 176 | printf("Streaming is only supported on xenomai.\n"); 177 | exit(-1); 178 | #endif 179 | } 180 | internal_going = going_; 181 | rt_mutex_release(&mutex_); 182 | } 183 | //stop streaming 184 | msg.command = 0x0000; 185 | rt_dev_send(socket_, &msg, sizeof(msg), 0); 186 | } 187 | 188 | void AtiFTSensor::setBias() 189 | { 190 | double* force = NULL; 191 | double* torque = NULL; 192 | setBias(force, torque); 193 | } 194 | 195 | void AtiFTSensor::setBias(double* force, double* torque) 196 | { 197 | rt_mutex_acquire(&mutex_,TM_INFINITE); 198 | for(int i=0; i<3; ++i) { 199 | if (force == NULL) { 200 | F_bias_[i] = F_[i]; 201 | } else { 202 | F_bias_[i] = force[i]; 203 | } 204 | if (torque == NULL) { 205 | T_bias_[i] = T_[i]; 206 | } else { 207 | T_bias_[i] = torque[i]; 208 | } 209 | } 210 | rt_mutex_release(&mutex_); 211 | } 212 | 213 | void AtiFTSensor::resetBias() 214 | { 215 | rt_mutex_acquire(&mutex_,TM_INFINITE); 216 | for(int i=0; i<3; ++i) 217 | { 218 | F_bias_[i] = 0.; 219 | T_bias_[i] = 0.; 220 | } 221 | rt_mutex_release(&mutex_); 222 | } 223 | 224 | 225 | void AtiFTSensor::getStatus(uint32_t& rdt_seq, uint32_t& ft_seq, uint32_t& status) 226 | { 227 | rt_mutex_acquire(&mutex_,TM_INFINITE); 228 | 229 | rdt_seq = rdt_sequence_; 230 | ft_seq = ft_sequence_; 231 | status = status_; 232 | 233 | rt_mutex_release(&mutex_); 234 | } 235 | 236 | void AtiFTSensor::getFT(double* force, double* torque) 237 | { 238 | rt_mutex_acquire(&mutex_,TM_INFINITE); 239 | 240 | for(int i=0; i<3; ++i) 241 | { 242 | force[i] = F_[i]; 243 | torque[i] = T_[i]; 244 | } 245 | 246 | rt_mutex_release(&mutex_); 247 | } 248 | 249 | const Eigen::Ref> AtiFTSensor::getFT_vector() 250 | { 251 | rt_mutex_acquire(&mutex_,TM_INFINITE); 252 | 253 | for(int i=0; i<3; ++i) 254 | { 255 | force_torque_[i] = F_[i]; 256 | force_torque_[i+3] = T_[i]; 257 | } 258 | 259 | rt_mutex_release(&mutex_); 260 | return force_torque_; 261 | } 262 | 263 | void AtiFTSensor::stop() 264 | { 265 | if(initialized_) 266 | { 267 | rt_mutex_acquire(&mutex_,TM_INFINITE); 268 | going_ = false; 269 | rt_mutex_release(&mutex_); 270 | reading_thread_.join(); 271 | rt_dev_close(socket_); 272 | #ifdef XENOMAI 273 | rt_pipe_delete(&stream_pipe_); 274 | #endif 275 | initialized_ = false; 276 | } 277 | } 278 | 279 | void AtiFTSensor::stream(bool stream) 280 | { 281 | rt_mutex_acquire(&mutex_,TM_INFINITE); 282 | streaming_ = stream; 283 | rt_mutex_release(&mutex_); 284 | } 285 | 286 | AtiFTSensor::~AtiFTSensor() 287 | { 288 | stop(); 289 | } 290 | 291 | } 292 | -------------------------------------------------------------------------------- /srcpy/ati_ft_sensor_cpp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace py = pybind11; 8 | using namespace ati_ft_sensor; 9 | 10 | PYBIND11_MODULE(ati_ft_sensor_cpp, m){ 11 | py::class_(m, "AtiFTSensor") 12 | .def(py::init<>()) 13 | .def("initialize", &AtiFTSensor::initialize) 14 | // .def("setBias", &AtiFTSensor::setBias) 15 | .def("resetBias", &AtiFTSensor::resetBias) 16 | .def("stop", &AtiFTSensor::stop) 17 | .def("getFT", &AtiFTSensor::getFT_vector) 18 | .def("stream", &AtiFTSensor::stream) 19 | ; 20 | } -------------------------------------------------------------------------------- /tests/test_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_sensor.cpp 3 | * 4 | * Created on: 1 Dec, 2018 5 | * Author: jviereck 6 | */ 7 | 8 | #ifdef XENOMAI 9 | #include 10 | #include 11 | #include 12 | #include 13 | #endif 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | ati_ft_sensor::AtiFTSensor sensor; 32 | bool going = true; 33 | 34 | void waitForEnter() 35 | { 36 | std::string line; 37 | std::getline(std::cin, line); 38 | std::cout << line << std::endl; 39 | } 40 | 41 | void warnOnSwitchToSecondaryMode(int) 42 | { 43 | std::cerr << "WARNING: Switched out of RealTime. Stack-trace in syslog.\n"; 44 | } 45 | 46 | THREAD_FUNCTION_RETURN_TYPE logTask(void*) 47 | { 48 | sensor.initialize(); 49 | std::this_thread::sleep_for(std::chrono::seconds(1)); 50 | sensor.setBias(); 51 | 52 | double F[3]; 53 | double T[3]; 54 | uint32_t rdt_seq, ft_seq, status; 55 | 56 | 57 | while(true) { 58 | sensor.getStatus(rdt_seq, ft_seq, status); 59 | sensor.getFT(&F[0], &T[0]); 60 | 61 | printf("rdt_seq=%d, ft_seq=%d, status=%d\n", rdt_seq, ft_seq, status); 62 | printf("%4.2f \t %4.2f \t %4.2f \t %4.2f \t %4.2f \t %4.2f \n", 63 | F[0], F[1], F[2], T[0], T[1], T[2]); 64 | 65 | std::this_thread::sleep_for(std::chrono::seconds(1)); 66 | } 67 | } 68 | 69 | 70 | 71 | int main() 72 | { 73 | real_time_tools::RealTimeThread thread_; 74 | thread_.create_realtime_thread(&logTask, nullptr); 75 | thread_.join(); 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /tests/test_sensor_stream.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_sensor_stream.cpp 3 | * 4 | * Created on: Oct 22, 2013 5 | * Author: righetti 6 | */ 7 | 8 | 9 | 10 | 11 | /*!============================================================================= 12 | ============================================================================== 13 | 14 | \file test_communication_loop.cpp 15 | 16 | \author righetti 17 | \date May 14, 2012 18 | 19 | ============================================================================== 20 | \brief This program runs a communication loop at a user defined frequency 21 | \brief and logs the number of received messages at each loop in a file 22 | 23 | ============================================================================*/ 24 | 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | RT_PIPE log_pipe; 45 | ati_ft_sensor::AtiFTSensor sensor; 46 | bool going = true; 47 | 48 | void waitForEnter() 49 | { 50 | std::string line; 51 | std::getline(std::cin, line); 52 | std::cout << line << std::endl; 53 | } 54 | 55 | void warnOnSwitchToSecondaryMode(int) 56 | { 57 | std::cerr << "WARNING: Switched out of RealTime. Stack-trace in syslog.\n"; 58 | } 59 | 60 | void logTask() 61 | { 62 | int fd; 63 | FILE *log_file = fopen("log_file.dat","w"); 64 | 65 | ati_ft_sensor::AtiFTSensor::steaming_msg log; 66 | 67 | fd = open("/proc/xenomai/registry/native/pipes/ati_ft_stream", O_RDONLY); 68 | 69 | if (fd < 0) { 70 | printf("cannot open log pipe - ERROR %d\n",fd); 71 | return; 72 | } 73 | 74 | 75 | size_t size = 0; 76 | bool reading = true; 77 | 78 | if(!((size = read(fd,&log,sizeof(log))) == sizeof(log))) 79 | reading = false; 80 | 81 | 82 | while ( going ) 83 | { 84 | if(reading) 85 | { 86 | fprintf(log_file, "%u \t", log.rdt_seq); 87 | fprintf(log_file, "%u \t", log.ft_seq); 88 | fprintf(log_file, "%u \t", log.status); 89 | fprintf(log_file, "%2.6f \t", log.time); 90 | fprintf(log_file, "%4.2f \t %4.2f \t %4.2f \t %4.2f \t %4.2f \t %4.2f \n", 91 | log.F[0],log.F[1],log.F[2],log.T[0],log.T[1],log.T[2]); 92 | } 93 | reading = true; 94 | if(!((size = read(fd,&log,sizeof(log))) == sizeof(log))) 95 | reading = false; 96 | 97 | 98 | } 99 | printf("stop logging\n"); 100 | close(fd); 101 | fclose(log_file); 102 | 103 | return; 104 | } 105 | 106 | 107 | 108 | int main(int argc, char* argv[]) 109 | { 110 | mlockall(MCL_CURRENT | MCL_FUTURE); 111 | rt_task_shadow(NULL, "test_communication_loop", 50, 0); 112 | 113 | sensor.initialize(); 114 | sensor.stream(true); 115 | rt_task_sleep(1e6); 116 | 117 | boost::thread log_thread(logTask); 118 | 119 | std::cout << "Press [Enter] to exit.\n"; 120 | waitForEnter(); 121 | std::cout << "exiting\n"; 122 | 123 | going = false; 124 | log_thread.join(); 125 | sensor.stream(false); 126 | sensor.stop(); 127 | 128 | return 0; 129 | } 130 | --------------------------------------------------------------------------------