├── interbotix_ros_slate ├── CATKIN_IGNORE ├── interbotix_slate_driver │ ├── README.md │ ├── lib │ │ └── libchassis_driver.so │ ├── src │ │ ├── slate_base_node.cpp │ │ └── base_driver.cpp │ ├── include │ │ └── interbotix_slate_driver │ │ │ ├── serial_driver.h │ │ │ ├── base_driver.h │ │ │ ├── od_index.h │ │ │ └── slate_base.h │ ├── package.xml │ └── CMakeLists.txt ├── interbotix_slate_msgs │ ├── srv │ │ └── SetString.srv │ ├── CMakeLists.txt │ └── package.xml └── interbotix_ros_slate │ ├── README.md │ ├── CMakeLists.txt │ └── package.xml ├── interbotix_ros_uxarms ├── CATKIN_IGNORE ├── xarm_msgs │ ├── srv │ │ ├── GetErr.srv │ │ ├── ClearErr.srv │ │ ├── GripperState.srv │ │ ├── GripperConfig.srv │ │ ├── GripperMove.srv │ │ ├── SetDigitalIO.srv │ │ ├── GetDigitalIO.srv │ │ ├── GetAnalogIO.srv │ │ ├── GetControllerDigitalIO.srv │ │ ├── ConfigToolModbus.srv │ │ ├── SetToolModbus.srv │ │ ├── TCPOffset.srv │ │ ├── SetLoad.srv │ │ ├── SetInt16.srv │ │ ├── SetAxis.srv │ │ └── Move.srv │ ├── msg │ │ ├── IOState.msg │ │ └── RobotMsg.msg │ ├── package.xml │ └── ReadMe.md ├── images │ └── xarm_api_banner.png └── xarm_api │ ├── include │ ├── xarm │ │ ├── common │ │ │ ├── crc16.h │ │ │ ├── queue_memcpy.h │ │ │ └── data_type.h │ │ ├── linux │ │ │ ├── thread.h │ │ │ └── network.h │ │ ├── xarm_config.h │ │ ├── connect.h │ │ ├── debug │ │ │ └── debug_print.h │ │ ├── instruction │ │ │ ├── uxbus_cmd_ser.h │ │ │ ├── uxbus_cmd_tcp.h │ │ │ └── servo3_config.h │ │ ├── port │ │ │ ├── socket.h │ │ │ └── serial.h │ │ └── report_data.h │ └── xarm_ros_client.h │ ├── src │ └── xarm │ │ ├── linux │ │ ├── thread.cc │ │ └── network.cc │ │ ├── debug │ │ └── debug_print.cc │ │ ├── instruction │ │ ├── uxbus_cmd_ser.cc │ │ └── uxbus_cmd_tcp.cc │ │ ├── common │ │ ├── queue_memcpy.cc │ │ └── crc16.cc │ │ ├── connect.cc │ │ └── port │ │ └── socket.cc │ ├── test │ ├── test_tool_modbus.cpp │ ├── example1_report_norm.cc │ ├── servo_cartesian_test.cpp │ └── move_test.cpp │ ├── package.xml │ └── scripts │ └── servo_cartesian_test.py ├── interbotix_ros_xseries ├── CATKIN_IGNORE ├── dynamixel_workbench_toolbox │ ├── examples │ │ ├── CATKIN_IGNORE │ │ ├── src │ │ │ ├── b_Ping.cpp │ │ │ ├── a_Model_Scan.cpp │ │ │ ├── g_Reset.cpp │ │ │ ├── f_Reboot.cpp │ │ │ ├── d_BPS_Change.cpp │ │ │ ├── o_Find_Dynamixel.cpp │ │ │ ├── h_Position.cpp │ │ │ ├── i_Velocity.cpp │ │ │ ├── k_Read_Write.cpp │ │ │ ├── j_Current_Based_Position.cpp │ │ │ ├── c_ID_Change.cpp │ │ │ ├── l_Sync_Write.cpp │ │ │ ├── m_Sync_Read_Write.cpp │ │ │ ├── e_Mode_Change.cpp │ │ │ └── n_Bulk_Read_Write.cpp │ │ └── CMakeLists.txt │ ├── src │ │ └── DynamixelWorkbench.h │ ├── library.properties │ ├── README.md │ ├── package.xml │ ├── include │ │ └── dynamixel_workbench_toolbox │ │ │ ├── dynamixel_tool.h │ │ │ ├── dynamixel_item.h │ │ │ └── dynamixel_workbench.h │ └── CMakeLists.txt ├── interbotix_ros_xseries │ ├── CMakeLists.txt │ ├── README.md │ └── package.xml ├── interbotix_xs_sdk │ ├── images │ │ ├── wizard_options.png │ │ └── xs_sdk_banner.png │ ├── TROUBLESHOOTING.md │ ├── src │ │ └── xs_sdk.cpp │ ├── 99-interbotix-udev.rules │ ├── package.xml │ ├── config │ │ └── mode_configs_template.yaml │ └── CMakeLists.txt ├── interbotix_xs_msgs │ ├── msg │ │ ├── JointTemps.msg │ │ ├── JointSingleCommand.msg │ │ ├── JointGroupCommand.msg │ │ ├── JointTrajectoryCommand.msg │ │ ├── TurretJoy.msg │ │ ├── ArmJoy.msg │ │ ├── HexJoy.msg │ │ └── LocobotJoy.msg │ ├── srv │ │ ├── TorqueEnable.srv │ │ ├── RegisterValues.srv │ │ ├── Reboot.srv │ │ ├── MotorGains.srv │ │ ├── RobotInfo.srv │ │ └── OperatingModes.srv │ ├── package.xml │ ├── CMakeLists.txt │ └── README.md └── README.md ├── .github ├── CODEOWNERS ├── ISSUE_TEMPLATE │ ├── config.yml │ ├── feature_request.yml │ ├── question.yml │ └── bug_report.yml └── workflows │ ├── xs-rolling.yaml │ ├── xs-galactic.yaml │ ├── xs-humble.yaml │ ├── xs-noetic.yaml │ ├── slate-noetic.yaml │ ├── iron.yml │ ├── jazzy.yml │ ├── humble.yml │ └── rolling.yml ├── .gitignore ├── images ├── core_banner.png ├── core_irros_structure.png └── core_repo_structure.png └── LICENSE /interbotix_ros_slate/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # Default Reviewer 2 | 3 | * @LSinterbotix 4 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | __pycache__/ 3 | *.egg-info/ 4 | 5 | */matlab_msg_gen*/ 6 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/README.md: -------------------------------------------------------------------------------- 1 | # interbotix_slate_driver 2 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GetErr.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | 4 | int16 err 5 | 6 | string message -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/ClearErr.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | 4 | int16 ret 5 | 6 | string message -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GripperState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | 3 | float32 curr_pos 4 | int16 err_code -------------------------------------------------------------------------------- /images/core_banner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/images/core_banner.png -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv: -------------------------------------------------------------------------------- 1 | string data 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /images/core_irros_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/images/core_irros_structure.png -------------------------------------------------------------------------------- /images/core_repo_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/images/core_repo_structure.png -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/src/DynamixelWorkbench.h: -------------------------------------------------------------------------------- 1 | #include "../include/dynamixel_workbench_toolbox/dynamixel_workbench.h" 2 | 3 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/images/xarm_api_banner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/interbotix_ros_uxarms/images/xarm_api_banner.png -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_ros_slate/README.md: -------------------------------------------------------------------------------- 1 | # interbotix_ros_slate 2 | 3 | This metapackage groups together the core ROS Packages for the Interbotix Slate mobile base. 4 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GripperConfig.srv: -------------------------------------------------------------------------------- 1 | # Grip velocity configuration: range is from 1 to 5000 2 | float32 pulse_vel 3 | 4 | --- 5 | 6 | int16 ret 7 | string message 8 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GripperMove.srv: -------------------------------------------------------------------------------- 1 | # position command of gripper: range from 0(close) to 850 (open) 2 | float32 pulse_pos 3 | 4 | --- 5 | 6 | int16 ret 7 | string message -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(interbotix_ros_slate) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_ros_xseries/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(interbotix_ros_xseries) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_ros_xseries/README.md: -------------------------------------------------------------------------------- 1 | # interbotix_ros_xseries 2 | 3 | This metapackage groups together the core ROS Packages for the Interbotix X-Series family of products. 4 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/images/wizard_options.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/interbotix_ros_xseries/interbotix_xs_sdk/images/wizard_options.png -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/images/xs_sdk_banner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/interbotix_ros_xseries/interbotix_xs_sdk/images/xs_sdk_banner.png -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/lib/libchassis_driver.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Interbotix/interbotix_ros_core/HEAD/interbotix_ros_slate/interbotix_slate_driver/lib/libchassis_driver.so -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/msg/IOState.msg: -------------------------------------------------------------------------------- 1 | # for indicating 2 digital and 2 analog Input port state 2 | 3 | int32 digital_1 4 | 5 | int32 digital_2 6 | 7 | float32 analog_1 8 | 9 | float32 analog_2 10 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/SetDigitalIO.srv: -------------------------------------------------------------------------------- 1 | # Setting the 2 digital Output port at robot end connector 2 | 3 | int16 io_num 4 | 5 | int16 value 6 | 7 | --- 8 | 9 | int16 ret 10 | 11 | string message 12 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GetDigitalIO.srv: -------------------------------------------------------------------------------- 1 | # Getting the 2 digital Input port at robot end connector 2 | 3 | --- 4 | 5 | int32 digital_1 6 | 7 | int32 digital_2 8 | 9 | int16 ret 10 | 11 | string message 12 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GetAnalogIO.srv: -------------------------------------------------------------------------------- 1 | # Getting one of the 2 analog Input port at robot end connector 2 | 3 | int16 port_num 4 | 5 | --- 6 | 7 | float32 analog_value 8 | 9 | int16 ret 10 | 11 | string message 12 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/GetControllerDigitalIO.srv: -------------------------------------------------------------------------------- 1 | # Getting the controller DIGITAL input port status, io_num: from 1 to 8 2 | 3 | int16 io_num 4 | 5 | --- 6 | 7 | int16 ret 8 | 9 | int16 value 10 | 11 | string message 12 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/JointTemps.msg: -------------------------------------------------------------------------------- 1 | # This message is used specifically in the interbotix_xsarm_diagnostic_tool package 2 | 3 | # Holds the temperatures [C] for the specified joints 4 | 5 | string[] names 6 | int32[] temps 7 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/TROUBLESHOOTING.md: -------------------------------------------------------------------------------- 1 | # Troubleshooting a Dynamixel-based Robot 2 | 3 | See the [Troubleshooting Guide](https://docs.trossenrobotics.com/interbotix_xsarms_docs/troubleshooting.html) on the Interbotix X-Series Arms documentation site. 4 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/ConfigToolModbus.srv: -------------------------------------------------------------------------------- 1 | # configure the tool modbus communication baud rate, in bps: 2 | int32 baud_rate 3 | 4 | # configure the timeout parameter in modbus communication, in milliseconds 5 | int32 timeout_ms 6 | 7 | --- 8 | 9 | string message 10 | int16 ret -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/SetToolModbus.srv: -------------------------------------------------------------------------------- 1 | # unsigned char data to be sent to tool device through modbus 2 | uint8[] send_data 3 | 4 | # Specify the anticipated maximum respond data length in bytes 5 | int16 respond_len 6 | 7 | --- 8 | 9 | int16 ret 10 | uint8[] respond_data 11 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/TCPOffset.srv: -------------------------------------------------------------------------------- 1 | # to give robot floating point number Cartesian TCP offset, based on initial Tool Frame located at flange center. 2 | 3 | float32 x 4 | float32 y 5 | float32 z 6 | 7 | float32 roll 8 | float32 pitch 9 | float32 yaw 10 | 11 | ----- 12 | 13 | int16 ret 14 | string message 15 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/SetLoad.srv: -------------------------------------------------------------------------------- 1 | # to set robot load parameters, based on initial Tool Frame located at flange center. 2 | # Load mass (kg) 3 | float32 mass 4 | 5 | # Load Center of Mass (mm), with respect to intial Tool Frame 6 | float32 xc 7 | float32 yc 8 | float32 zc 9 | 10 | ----- 11 | 12 | int16 ret 13 | string message 14 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/JointSingleCommand.msg: -------------------------------------------------------------------------------- 1 | # Command a desired joint. Note that the command is processed differently based on the joint's operating mode. 2 | # For example, if a joint's operating mode is set to 'position', the command is interpreted as a position in radians 3 | 4 | string name # Name of joint 5 | float32 cmd # Joint command 6 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/library.properties: -------------------------------------------------------------------------------- 1 | name=DynamixelWorkbench 2 | version=0.1.0 3 | author=Darby Lim (thlim@robotis.com) 4 | maintainer=Pyo (pyo@robotis.com) 5 | sentence=DynamixelWorkbench using DynamixelSDK 6 | paragraph=Tools for Dynamixel 7 | category=Other 8 | url=https://github.com/ROBOTIS-GIT/dynamixel-workbench 9 | architectures=OpenCM904 10 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/config.yml: -------------------------------------------------------------------------------- 1 | blank_issues_enabled: true 2 | contact_links: 3 | - name: Contact Trossen Robotics Support 4 | url: https://www.trossenrobotics.com/contact.aspx 5 | about: If you have any questions about missing or damaged parts. 6 | - name: Discussions 7 | url: https://github.com/orgs/Interbotix/discussions 8 | about: Ask and answer questions about Interbotix products. 9 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/srv/TorqueEnable.srv: -------------------------------------------------------------------------------- 1 | # Torque joints on/off 2 | 3 | string cmd_type # set to 'group' if commanding a joint group or 'single' if commanding a single joint 4 | string name # name of the group if commanding a joint group or joint if commanding a single joint 5 | bool enable # set to 'true' to torque on or 'false' to torque off 6 | --- 7 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/SetInt16.srv: -------------------------------------------------------------------------------- 1 | # request: set robot state or mode. 2 | # data: int value indicating command robot state or execution mode. 3 | 4 | int16 data 5 | 6 | ----- 7 | 8 | # response: 9 | # ret is 0 for successful execution and others for errors or warnings occured 10 | # message is a string returned by function, indicating execution status. 11 | 12 | int16 ret 13 | string message 14 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp: -------------------------------------------------------------------------------- 1 | #include "interbotix_slate_driver/slate_base.h" 2 | 3 | int main(int argc, char ** argv) 4 | { 5 | ros::init(argc, argv, "slate_base"); 6 | ros::NodeHandle node = ros::NodeHandle(); 7 | auto driver = slate_base::SlateBase(&node); 8 | 9 | auto r = ros::Rate(20); 10 | while (ros::ok()) { 11 | ros::spinOnce(); 12 | driver.update(); 13 | r.sleep(); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.0) 2 | project(interbotix_slate_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | ) 8 | 9 | add_service_files( 10 | FILES 11 | SetString.srv 12 | ) 13 | 14 | generate_messages( 15 | DEPENDENCIES 16 | std_msgs 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | message_runtime 22 | std_msgs 23 | ) 24 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/common/crc16.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef COMMON_CRC16_H_ 8 | #define COMMON_CRC16_H_ 9 | 10 | #include "xarm/common/data_type.h" 11 | 12 | int modbus_crc(unsigned char *data, int len); 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/SetAxis.srv: -------------------------------------------------------------------------------- 1 | # request: for enabling / disabling motion control of one(or all) joint. 2 | # id: 1-7 for target joint number, or 8 for all joints 3 | # data: 0 for disabling servo control, 1 for enabling servo control. 4 | 5 | int16 id 6 | int16 data 7 | 8 | --- 9 | 10 | # response: 11 | # ret is 0 for successful execution and others for errors or warnings occured 12 | # message is a string returned by function, indicating execution status. 13 | 14 | int16 ret 15 | string message -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/JointGroupCommand.msg: -------------------------------------------------------------------------------- 1 | # Command the joints in the specified joint group. Note that the commands are processed differently based on the group's operating mode. 2 | # For example, if a group's operating mode is set to 'position', the commands are interpreted as positions in radians 3 | 4 | string name # Name of joint group 5 | float32[] cmd # List of joint commands; order is dictated by the index of each joint name for the given group in the 'groups' section of a 'motor_config' yaml file 6 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/linux/thread.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef LINUX_THREAD_H_ 8 | #define LINUX_THREAD_H_ 9 | 10 | #include 11 | 12 | typedef void *(*fun_point_t)(void *); 13 | 14 | void thread_delete(pthread_t id); 15 | pthread_t thread_init(fun_point_t fun_point, void *arg); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk.cpp: -------------------------------------------------------------------------------- 1 | #include "interbotix_xs_sdk/xs_sdk_obj.h" 2 | 3 | int main( int argc, char** argv ) 4 | { 5 | ros::init(argc, argv, "xs_sdk"); 6 | ros::NodeHandle n; 7 | bool success = true; 8 | InterbotixRobotXS bot(&n, success); 9 | if (success) 10 | ros::spin(); 11 | else 12 | { 13 | ROS_FATAL( 14 | "[xs_sdk] For troubleshooting, please see " 15 | "'https://docs.trossenrobotics.com/interbotix_xsarms_docs/troubleshooting.html'"); 16 | ros::shutdown(); 17 | } 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/linux/network.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef LINUX_NETWORK_H_ 8 | #define LINUX_NETWORK_H_ 9 | 10 | #include "xarm/common/data_type.h" 11 | 12 | int socket_init(char *local_ip, int port, int is_server); 13 | int socket_send_data(int client_fp, unsigned char *data, int len); 14 | int socket_connect_server(int *socket, char server_ip[], int server_port); 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /.github/workflows/xs-rolling.yaml: -------------------------------------------------------------------------------- 1 | name: build-xs-rolling 2 | 3 | on: 4 | push: 5 | branches: 6 | - rolling 7 | pull_request: 8 | branches: 9 | - rolling 10 | workflow_dispatch: 11 | 12 | defaults: 13 | run: 14 | shell: bash 15 | 16 | jobs: 17 | xs-rolling: 18 | strategy: 19 | matrix: 20 | env: 21 | - {ROS_DISTRO: rolling, ROS_REPO: main} 22 | runs-on: ubuntu-24.04 23 | steps: 24 | - uses: actions/checkout@v3 25 | with: 26 | submodules: recursive 27 | path: src 28 | - uses: 'ros-industrial/industrial_ci@master' 29 | env: ${{matrix.env}} 30 | -------------------------------------------------------------------------------- /.github/workflows/xs-galactic.yaml: -------------------------------------------------------------------------------- 1 | name: build-xs-galactic 2 | 3 | on: 4 | push: 5 | branches: 6 | - galactic 7 | pull_request: 8 | branches: 9 | - galactic 10 | workflow_dispatch: 11 | 12 | defaults: 13 | run: 14 | shell: bash 15 | 16 | jobs: 17 | xs-galactic: 18 | strategy: 19 | matrix: 20 | env: 21 | - {ROS_DISTRO: galactic, ROS_REPO: main} 22 | runs-on: ubuntu-20.04 23 | steps: 24 | - uses: actions/checkout@v3 25 | with: 26 | submodules: recursive 27 | path: src 28 | - uses: 'ros-industrial/industrial_ci@master' 29 | env: ${{matrix.env}} 30 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/README.md: -------------------------------------------------------------------------------- 1 | # Notes 2 | 3 | The [interbotix_xs_sdk](interbotix_xs_sdk/) ROS wrapper builds on-top of the [dynamixel_workbench_toolbox](http://wiki.ros.org/dynamixel_workbench_toolbox) C++ API, which in turn builds on-top of the [dynamixel_sdk](http://wiki.ros.org/dynamixel_sdk) C++ API. As the *dynamixel_sdk* ROS wrapper version on 'apt' is kept up-to-date, the prebuilt binary version is installed. However, the *dynamixel_workbench_toolbox* ROS wrapper available on 'apt' is a bit behind the times so it is built from source here. Both the *dynamixel_sdk* and *dynamixel_workbench_toolbox* packages were created by [ROBOTIS](https://github.com/ROBOTIS-GIT). 4 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/JointTrajectoryCommand.msg: -------------------------------------------------------------------------------- 1 | # Command a joint trajectory to the desired joint(s). Note that the commands are processed differently based on the currently set operating mode. 2 | # For example, if the operating mode is set to 'position', the commands are interpreted as positions in radians 3 | 4 | string cmd_type # set to 'single' for a single joint or 'group' for a group of joints 5 | string name # joint group name if 'cmd_type' is set to 'group' or joint name if 'cmd_type' is set to 'single' 6 | trajectory_msgs/JointTrajectory traj # ROS trajectory message 7 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_ros_slate/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interbotix_ros_slate 4 | 0.0.0 5 | The interbotix_ros_slate metapackage 6 | Trossen Robotics 7 | BSD-3-Clause 8 | 9 | Trossen Robotics 10 | 11 | catkin 12 | 13 | interbotix_slate_driver 14 | interbotix_slate_msgs 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_ros_xseries/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interbotix_ros_xseries 4 | 0.0.0 5 | The interbotix_ros_xseries metapackage 6 | Luke Schmitt 7 | BSD 8 | 9 | Luke Schmitt 10 | 11 | catkin 12 | dynamixel_workbench_toolbox 13 | interbotix_xs_msgs 14 | interbotix_xs_sdk 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interbotix_slate_msgs 4 | 0.0.0 5 | The interbotix_slate_msgs package 6 | 7 | Trossen Robotics 8 | 9 | BSD-3-Clause 10 | 11 | catkin 12 | 13 | message_generation 14 | std_msgs 15 | 16 | std_msgs 17 | 18 | message_runtime 19 | std_msgs 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | xarm_msgs 4 | 0.0.0 5 | The xarm_msgs package 6 | Jason Peng 7 | BSD 8 | catkin 9 | std_msgs 10 | message_generation 11 | message_runtime 12 | 13 | message_generation 14 | message_runtime 15 | 16 | std_msgs 17 | std_msgs 18 | 19 | 20 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/xarm_config.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | class XARM_CONF { 8 | public: 9 | XARM_CONF() {} 10 | ~XARM_CONF() {} 11 | 12 | static const int AXIS_NUM = 7; 13 | static const int GRIPPER_ID = 8; 14 | static const int GPIO_ID = 9; 15 | static const int SERIAL_BAUD = 921600; 16 | static const int TCP_PORT_CONTROL = 502; 17 | static const int TCP_PORT_REPORT_NORM = 30001; 18 | static const int TCP_PORT_REPORT_RICH = 30002; 19 | static const int TCP_PORT_REPORT_DEVL = 30003; 20 | }; 21 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/connect.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef XARM_CONNECT_H_ 8 | #define XARM_CONNECT_H_ 9 | 10 | #include "xarm/instruction/uxbus_cmd_ser.h" 11 | #include "xarm/instruction/uxbus_cmd_tcp.h" 12 | 13 | UxbusCmdSer *connect_rs485_control(const char *com); 14 | UxbusCmdTcp *connect_tcp_control(char *server_ip); 15 | SocketPort *connext_tcp_report_norm(char *server_ip); 16 | SocketPort *connext_tcp_report_rich(char *server_ip); 17 | SocketPort *connext_tcp_report_devl(char *server_ip); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/README.md: -------------------------------------------------------------------------------- 1 | How to run dynamixel_workbench in linux without ROS 2 | 3 | 1. Downloads DynamixelSDK 4 | 5 | ``` 6 | $ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK 7 | ``` 8 | 9 | 2. Build DynamixelSDK 10 | 11 | http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/library_setup/cpp_linux/#build-the-library 12 | 13 | 14 | 3. Downloads Dynamixel-Workbench 15 | 16 | ``` 17 | $ git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench 18 | ``` 19 | 20 | 4. Build Dynamixel-Workbench and Run examples 21 | 22 | ``` 23 | $ cd ${YOUR_DOWNLOAD_PATH}/dynamixel_workbench/dynamixel_workbench_toolbox/examples 24 | $ mkdir build && cd build 25 | $ cmake .. 26 | $ make 27 | $ sudo chmod a+rw ${USB_PORT} 28 | $ ./monitor 29 | ``` -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/debug/debug_print.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef DEBUG_DEBUG_PRINT_H_ 8 | #define DEBUG_DEBUG_PRINT_H_ 9 | 10 | #include "xarm/common/data_type.h" 11 | 12 | void print_log(const char *format, ...); 13 | void print_nvect(const char *str, double vect[], int n); 14 | void print_nvect(const char *str, float *vect, int n); 15 | void print_nvect(const char *str, unsigned char vect[], int n); 16 | void print_nvect(const char *str, int vect[], int n); 17 | void print_hex(const char *str, unsigned char *hex, int len); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/linux/thread.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/linux/thread.h" 8 | 9 | #include 10 | 11 | #define PRINT_ERR printf 12 | 13 | pthread_t thread_init(fun_point_t fun_point, void *arg) { 14 | pthread_t id; 15 | pthread_attr_t attr; 16 | 17 | pthread_attr_init(&attr); 18 | pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); 19 | char ret = pthread_create(&id, &attr, fun_point, arg); 20 | if (0 != ret) PRINT_ERR("error: pthread create failes\n"); 21 | 22 | return id; 23 | } 24 | 25 | void thread_delete(pthread_t id) { pthread_cancel(id); } 26 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/test/test_tool_modbus.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "xarm_ros_client.h" 3 | #include 4 | 5 | // Please run "export ROS_NAMESPACE=/xarm" first 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "xarm_modbus"); 10 | ros::NodeHandle nh; 11 | 12 | xarm_api::XArmROSClient client; 13 | client.init(nh); 14 | 15 | int recv_bytes = 7; 16 | unsigned char send_data[6] = {0x01,0x06,0x00,0x0A,0x00,0x03}, recv_data[recv_bytes]={0}; 17 | 18 | int ret = 0; 19 | ros::Rate rate(10); 20 | 21 | ret = client.send_tool_modbus(send_data, 6, recv_data, recv_bytes); 22 | fprintf(stderr, "ret = %d, recv_data: ", ret); 23 | 24 | for(int i=0; i 6 | ============================================================================*/ 7 | #ifndef INSTRUCTION_UXBUS_CMD_SER_H_ 8 | #define INSTRUCTION_UXBUS_CMD_SER_H_ 9 | 10 | #include "xarm/instruction/uxbus_cmd.h" 11 | #include "xarm/port/serial.h" 12 | 13 | class UxbusCmdSer : public UxbusCmd { 14 | public: 15 | UxbusCmdSer(SerialPort *arm_port); 16 | ~UxbusCmdSer(void); 17 | 18 | int check_xbus_prot(unsigned char *datas, int funcode); 19 | int send_pend(int funcode, int num, int timeout, unsigned char *ret_data); 20 | int send_xbus(int funcode, unsigned char *datas, int num); 21 | void close(void); 22 | 23 | private: 24 | SerialPort *arm_port_; 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/srv/Reboot.srv: -------------------------------------------------------------------------------- 1 | # Reboot motors 2 | # 3 | # Note that if a dual-joint is selected, both motors will be rebooted. Also note 4 | # that motors will NOT hold position during the reboot process. Additionally, only 5 | # EEPROM registers will retain their values, but RAM registers will not. See details 6 | # at https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#area-eeprom-ram 7 | 8 | string cmd_type # set to 'group' if commanding a joint group or 'single' if commanding a single joint 9 | string name # name of the group if commanding a joint group or joint if commanding a single joint 10 | bool enable # whether to torque the selected joints on after reboot 11 | bool smart_reboot # set to true to only reboot motors in a specified group that are in an error state 12 | # (as opposed to a blanket reboot of all motors in said group) 13 | --- 14 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/common/queue_memcpy.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef COMMON_QUEUE_MEMCPY_H_ 8 | #define COMMON_QUEUE_MEMCPY_H_ 9 | 10 | #include 11 | 12 | class QueueMemcpy { 13 | public: 14 | QueueMemcpy(long n, long n_size); 15 | ~QueueMemcpy(void); 16 | char flush(void); 17 | char push(void *data); 18 | char pop(void *data); 19 | char get(void *data); 20 | long size(void); 21 | long node_size(void); 22 | int is_full(void); 23 | 24 | protected: 25 | private: 26 | long total_; 27 | long annode_size_; 28 | 29 | long cnt_; 30 | long head_; 31 | long tail_; 32 | char *buf_; 33 | pthread_mutex_t mutex_; 34 | }; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | interbotix_xs_msgs 3 | 0.0.1 4 | 5 | interbotix_xs_msgs provides messages for the Interbotix X-Series family 6 | of products. 7 | 8 | Luke Schmitt 9 | Luke Schmitt 10 | BSD 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | trajectory_msgs 17 | geometry_msgs 18 | 19 | message_runtime 20 | std_msgs 21 | trajectory_msgs 22 | geometry_msgs 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ 2 | #define INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ 3 | 4 | #include 5 | #define ASSERT(expr, fail) (static_cast(expr) ? void(0) : (fail)) 6 | 7 | class SerialDriver 8 | { 9 | public: 10 | bool init(const std::string& portname, std::string& dev, int baudRate); 11 | void close(); 12 | 13 | bool setEntry(int index, const float* data_address); 14 | bool setEntry(int index, const int* data_address); 15 | bool setEntry(int index, const char* data_address); 16 | 17 | bool getEntry(int index, float* data_address); 18 | bool getEntry(int index, int* data_address); 19 | bool getEntry(int index, char* data_address); 20 | 21 | private: 22 | bool handleTimeout(int); 23 | 24 | int fd_; 25 | bool isOpened_; 26 | int conn_timeout_cnt_; 27 | int baud_rate_ = 0; 28 | std::string portname_; 29 | }; 30 | 31 | #endif // INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ 32 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/port/socket.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef PORT_SOCKET_H_ 8 | #define PORT_SOCKET_H_ 9 | 10 | #include 11 | 12 | #include "xarm/common/data_type.h" 13 | #include "xarm/common/queue_memcpy.h" 14 | 15 | class SocketPort { 16 | public: 17 | SocketPort(char *server_ip, int server_port, int que_num, int que_maxlen); 18 | ~SocketPort(void); 19 | int is_ok(void); 20 | void flush(void); 21 | void recv_proc(void); 22 | int write_frame(unsigned char *data, int len); 23 | int read_frame(unsigned char *data); 24 | void close_port(void); 25 | int que_maxlen_; 26 | 27 | private: 28 | int fp_; 29 | int state_; 30 | int que_num_; 31 | QueueMemcpy *rx_que_; 32 | pthread_t thread_id_; 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /.github/workflows/xs-humble.yaml: -------------------------------------------------------------------------------- 1 | name: build-xs-humble 2 | 3 | on: 4 | push: 5 | branches: 6 | - humble 7 | pull_request: 8 | branches: 9 | - humble 10 | workflow_dispatch: 11 | 12 | defaults: 13 | run: 14 | shell: bash 15 | 16 | jobs: 17 | xs-humble: 18 | strategy: 19 | matrix: 20 | env: 21 | - {ROS_DISTRO: humble, ROS_REPO: main} 22 | runs-on: ubuntu-22.04 23 | steps: 24 | - uses: actions/checkout@v4 25 | with: 26 | submodules: recursive 27 | path: src/interbotix_ros_core 28 | - name: Prepare Workspace 29 | run: | 30 | rm src/interbotix_ros_core/interbotix_ros_common_drivers/COLCON_IGNORE 31 | rm src/interbotix_ros_core/interbotix_ros_xseries/COLCON_IGNORE 32 | - name: Install Dependencies 33 | run: | 34 | sudo apt-get install --reinstall pkg-config cmake-data 35 | - uses: 'ros-industrial/industrial_ci@master' 36 | with: 37 | config: ${{toJSON(matrix.env)}} 38 | -------------------------------------------------------------------------------- /.github/workflows/xs-noetic.yaml: -------------------------------------------------------------------------------- 1 | name: build-xs-noetic 2 | 3 | on: 4 | push: 5 | paths-ignore: 6 | - 'interbotix_ros_slate/**' 7 | - 'interbotix_ros_uxarms/**' 8 | branches: 9 | - main 10 | - devel 11 | pull_request: 12 | paths-ignore: 13 | - 'interbotix_ros_slate/**' 14 | - 'interbotix_ros_uxarms/**' 15 | branches: 16 | - main 17 | - devel 18 | workflow_dispatch: 19 | 20 | defaults: 21 | run: 22 | shell: bash 23 | 24 | jobs: 25 | xs-noetic: 26 | strategy: 27 | matrix: 28 | env: 29 | - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_tools} 30 | - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_make} 31 | runs-on: ubuntu-latest 32 | steps: 33 | - uses: actions/checkout@v4 34 | - name: Create src directory for xs 35 | run: | 36 | rm interbotix_ros_xseries/CATKIN_IGNORE 37 | mkdir src 38 | mv interbotix_ros_xseries src 39 | - uses: 'ros-industrial/industrial_ci@master' 40 | env: ${{matrix.env}} 41 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(interbotix_xs_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | trajectory_msgs 8 | geometry_msgs 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | JointGroupCommand.msg 14 | JointSingleCommand.msg 15 | JointTrajectoryCommand.msg 16 | # xsarm_diagnostic_tool 17 | JointTemps.msg 18 | # xsarm_joy 19 | ArmJoy.msg 20 | #xshexapod_joy 21 | HexJoy.msg 22 | #xslocobot_joy 23 | LocobotJoy.msg 24 | #xsturret_joy 25 | TurretJoy.msg 26 | ) 27 | 28 | add_service_files( 29 | FILES 30 | Reboot.srv 31 | RobotInfo.srv 32 | MotorGains.srv 33 | TorqueEnable.srv 34 | OperatingModes.srv 35 | RegisterValues.srv 36 | ) 37 | 38 | generate_messages( 39 | DEPENDENCIES 40 | std_msgs 41 | trajectory_msgs 42 | geometry_msgs 43 | ) 44 | 45 | catkin_package( 46 | CATKIN_DEPENDS 47 | message_runtime 48 | std_msgs 49 | trajectory_msgs 50 | ) 51 | -------------------------------------------------------------------------------- /.github/workflows/slate-noetic.yaml: -------------------------------------------------------------------------------- 1 | name: build-slate-noetic 2 | 3 | on: 4 | push: 5 | paths-ignore: 6 | - 'interbotix_ros_uxarms/**' 7 | - 'interbotix_ros_xseries/**' 8 | branches: 9 | - main 10 | - devel 11 | - noetic 12 | pull_request: 13 | paths-ignore: 14 | - 'interbotix_ros_uxarms/**' 15 | - 'interbotix_ros_xseries/**' 16 | branches: 17 | - main 18 | - devel 19 | - noetic 20 | workflow_dispatch: 21 | 22 | defaults: 23 | run: 24 | shell: bash 25 | 26 | jobs: 27 | slate-noetic: 28 | strategy: 29 | matrix: 30 | env: 31 | - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_tools} 32 | - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_make} 33 | runs-on: ubuntu-latest 34 | steps: 35 | - uses: actions/checkout@v4 36 | - name: Create src directory for slate 37 | run: | 38 | rm interbotix_ros_slate/CATKIN_IGNORE 39 | mkdir src 40 | mv interbotix_ros_slate src 41 | - uses: 'ros-industrial/industrial_ci@master' 42 | env: ${{matrix.env}} 43 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/srv/MotorGains.srv: -------------------------------------------------------------------------------- 1 | # Set PID gains 2 | # 3 | # To get familiar with the various PID gains, go to... 4 | # http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ 5 | # ...click on a motor model, and scroll down to the 'PID' section. 6 | 7 | string cmd_type # set to 'group' if commanding a joint group or 'single' if commanding a single joint 8 | string name # name of the group if commanding a joint group or joint if commanding a single joint 9 | int32 kp_pos # acts as a pass-through to the Position_P_Gain register 10 | int32 ki_pos # acts as a pass-through to the Position_I_Gain register 11 | int32 kd_pos # acts as a pass-through to the Position_D_Gain register 12 | int32 k1 # acts as a pass-through to the Feedforward_1st_Gain register 13 | int32 k2 # acts as a pass-through to the Feedforward_2nd_Gain register 14 | int32 kp_vel # acts as a pass-through to the Velocity_P_Gain register 15 | int32 ki_vel # acts as a pass-through to the Velocity_I_Gain register 16 | --- 17 | -------------------------------------------------------------------------------- /.github/workflows/iron.yml: -------------------------------------------------------------------------------- 1 | name: build-iron 2 | 3 | on: 4 | push: 5 | branches: 6 | - iron 7 | pull_request: 8 | branches: 9 | - iron 10 | workflow_dispatch: 11 | 12 | # Limit jobs run by PRs or branches by cancelling ongoing jobs 13 | # https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior 14 | concurrency: 15 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} 16 | cancel-in-progress: true 17 | 18 | jobs: 19 | iron: 20 | strategy: 21 | matrix: 22 | env: 23 | - {ROS_DISTRO: iron, ROS_REPO: main} 24 | - {ROS_DISTRO: iron, ROS_REPO: testing} 25 | runs-on: ubuntu-22.04 26 | steps: 27 | - uses: actions/checkout@v4 28 | with: 29 | submodules: recursive 30 | - name: Prepare Workspace 31 | run: | 32 | rm \ 33 | interbotix_ros_common_drivers/COLCON_IGNORE \ 34 | interbotix_ros_slate/COLCON_IGNORE \ 35 | interbotix_ros_xseries/COLCON_IGNORE 36 | - uses: ros-industrial/industrial_ci@master 37 | env: ${{matrix.env}} 38 | -------------------------------------------------------------------------------- /.github/workflows/jazzy.yml: -------------------------------------------------------------------------------- 1 | name: build-jazzy 2 | 3 | on: 4 | push: 5 | branches: 6 | - jazzy 7 | pull_request: 8 | branches: 9 | - jazzy 10 | workflow_dispatch: 11 | 12 | # Limit jobs run by PRs or branches by cancelling ongoing jobs 13 | # https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior 14 | concurrency: 15 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} 16 | cancel-in-progress: true 17 | 18 | jobs: 19 | jazzy: 20 | strategy: 21 | matrix: 22 | env: 23 | - {ROS_DISTRO: jazzy, ROS_REPO: main} 24 | - {ROS_DISTRO: jazzy, ROS_REPO: testing} 25 | runs-on: ubuntu-24.04 26 | steps: 27 | - uses: actions/checkout@v4 28 | with: 29 | submodules: recursive 30 | - name: Prepare Workspace 31 | run: | 32 | rm \ 33 | interbotix_ros_common_drivers/COLCON_IGNORE \ 34 | interbotix_ros_slate/COLCON_IGNORE \ 35 | interbotix_ros_xseries/COLCON_IGNORE 36 | - uses: ros-industrial/industrial_ci@master 37 | env: ${{matrix.env}} 38 | -------------------------------------------------------------------------------- /.github/workflows/humble.yml: -------------------------------------------------------------------------------- 1 | name: build-humble 2 | 3 | on: 4 | push: 5 | branches: 6 | - humble 7 | pull_request: 8 | branches: 9 | - humble 10 | workflow_dispatch: 11 | 12 | # Limit jobs run by PRs or branches by cancelling ongoing jobs 13 | # https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior 14 | concurrency: 15 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} 16 | cancel-in-progress: true 17 | 18 | jobs: 19 | humble: 20 | strategy: 21 | matrix: 22 | env: 23 | - {ROS_DISTRO: humble, ROS_REPO: main} 24 | - {ROS_DISTRO: humble, ROS_REPO: testing} 25 | runs-on: ubuntu-22.04 26 | steps: 27 | - uses: actions/checkout@v4 28 | with: 29 | submodules: recursive 30 | - name: Prepare Workspace 31 | run: | 32 | rm \ 33 | interbotix_ros_common_drivers/COLCON_IGNORE \ 34 | interbotix_ros_slate/COLCON_IGNORE \ 35 | interbotix_ros_xseries/COLCON_IGNORE 36 | - uses: ros-industrial/industrial_ci@master 37 | env: ${{matrix.env}} 38 | -------------------------------------------------------------------------------- /.github/workflows/rolling.yml: -------------------------------------------------------------------------------- 1 | name: build-rolling 2 | 3 | on: 4 | push: 5 | branches: 6 | - rolling 7 | pull_request: 8 | branches: 9 | - rolling 10 | workflow_dispatch: 11 | 12 | # Limit jobs run by PRs or branches by cancelling ongoing jobs 13 | # https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior 14 | concurrency: 15 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} 16 | cancel-in-progress: true 17 | 18 | jobs: 19 | rolling: 20 | strategy: 21 | matrix: 22 | env: 23 | - {ROS_DISTRO: rolling, ROS_REPO: main} 24 | - {ROS_DISTRO: rolling, ROS_REPO: testing} 25 | runs-on: ubuntu-24.04 26 | steps: 27 | - uses: actions/checkout@v4 28 | with: 29 | submodules: recursive 30 | - name: Prepare Workspace 31 | run: | 32 | rm \ 33 | interbotix_ros_common_drivers/COLCON_IGNORE \ 34 | interbotix_ros_slate/COLCON_IGNORE \ 35 | interbotix_ros_xseries/COLCON_IGNORE 36 | - uses: ros-industrial/industrial_ci@master 37 | env: ${{matrix.env}} 38 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef INSTRUCTION_UXBUS_CMD_TCP_H_ 8 | #define INSTRUCTION_UXBUS_CMD_TCP_H_ 9 | 10 | #include "xarm/instruction/uxbus_cmd.h" 11 | #include "xarm/port/socket.h" 12 | 13 | class UxbusCmdTcp : public UxbusCmd { 14 | public: 15 | UxbusCmdTcp(SocketPort *arm_port); 16 | ~UxbusCmdTcp(void); 17 | 18 | int check_xbus_prot(unsigned char *datas, int funcode); 19 | int send_pend(int funcode, int num, int timeout, unsigned char *ret_data); 20 | int send_xbus(int funcode, unsigned char *datas, int num); 21 | void close(void); 22 | 23 | private: 24 | SocketPort *arm_port_; 25 | int bus_flag_; 26 | int prot_flag_; 27 | int TX2_PROT_CON_ = 2; // tcp cmd prot 28 | int TX2_PROT_HEAT_ = 1; // tcp heat prot 29 | int TX2_BUS_FLAG_MIN_ = 1; // cmd序号 起始值 30 | int TX2_BUS_FLAG_MAX_ = 5000; // cmd序号 最大值 31 | }; 32 | 33 | #endif -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ 2 | #define INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ 3 | 4 | #include 5 | 6 | #include "od_index.h" 7 | 8 | namespace base_driver 9 | { 10 | 11 | bool chassisInit(std::string &dev); 12 | bool chassisControl(float aim_x_vel, float aim_z_omega); 13 | bool getChassisInfo(float &x_vel, float &z_omega); 14 | bool getChassisOdom(float &odom_x, float &odom_y, float &odom_theta); 15 | 16 | bool getBatteryInfo(float &vol, float &cur, int &percent); 17 | bool getChassisState(SystemState &state); 18 | bool getChassisCollision(int &collision); 19 | bool getVersion(char *text); 20 | bool getJoyState(int &state); 21 | 22 | // Limited to 100 characters 23 | bool setText(const char *text); 24 | 25 | // 0 / 1 26 | bool setCharge(int charge); 27 | 28 | // 0 / 1 29 | bool setAlarm(int alarm); 30 | 31 | bool motorCtrl(int v); 32 | 33 | bool setIo(int io); 34 | 35 | bool getIo(int &io_state); 36 | 37 | // 0 ~ 100 38 | bool setLight(int light); 39 | 40 | bool setStateLight(int light); 41 | 42 | } // namespace base_driver 43 | 44 | #endif // INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ 45 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | xarm_api 4 | 0.0.0 5 | The xarm_api package 6 | 7 | Jason Peng 8 | 9 | BSD 10 | catkin 11 | 12 | std_srvs 13 | genmsg 14 | 15 | roscpp 16 | std_msgs 17 | xarm_msgs 18 | sensor_msgs 19 | 20 | roscpp 21 | std_msgs 22 | xarm_msgs 23 | sensor_msgs 24 | 25 | roscpp 26 | std_msgs 27 | xarm_msgs 28 | sensor_msgs 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/TurretJoy.msg: -------------------------------------------------------------------------------- 1 | # This message is used specifically in the interbotix_xsturret_simple_interface package 2 | # 3 | # Maps raw 'joy' commands to more specific ones to control an Interbotix turret 4 | 5 | # enum values that define the joystick controls for the robot 6 | 7 | ######################################################################################################### 8 | 9 | # Control the pan-and-tilt mechanism 10 | int8 PAN_CCW = 1 11 | int8 PAN_CW = 2 12 | int8 TILT_UP = 3 13 | int8 TILT_DOWN = 4 14 | int8 PAN_TILT_HOME = 5 15 | 16 | ######################################################################################################### 17 | 18 | # Customize configurations for the Interbotix Turret 19 | # Inc/Dec Joint speed 20 | int8 SPEED_INC = 6 21 | int8 SPEED_DEC = 7 22 | 23 | # Quickly toggle between a fast and slow speed setting 24 | int8 SPEED_COURSE = 8 25 | int8 SPEED_FINE = 9 26 | 27 | ######################################################################################################### 28 | 29 | # Control the motion of the pan-and-tilt mechanism 30 | int8 pan_cmd 31 | int8 tilt_cmd 32 | 33 | # Speed Configs 34 | int8 speed_cmd 35 | int8 speed_toggle_cmd 36 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/99-interbotix-udev.rules: -------------------------------------------------------------------------------- 1 | # Place this file in /etc/udev/rules.d/ 2 | # Then reload udev by typing 'udevadm control --reload-rules && udevadm trigger' 3 | # Sets up rules to give permanent names to devices 4 | 5 | # Allow serial devices to be read by anyone 6 | KERNEL=="ttyUSB*", MODE:="0666" 7 | KERNEL=="ttyACM*", MODE:="0666" 8 | KERNEL=="js*", MODE:="0666" 9 | KERNEL=="video*", MODE:="0666" 10 | 11 | # OpenCM9.04C board 12 | SUBSYSTEM=="tty", ATTRS{idVendor}=="fff1", ATTRS{idProduct}=="ff48", ENV{ID_MM_DEVICE_IGNORE}="1", SYMLINK+="ttyDXL" 13 | 14 | # U2D2 board (also sets latency timer to 1ms for faster communication) 15 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", ENV{ID_MM_DEVICE_IGNORE}="1", ATTR{device/latency_timer}="1", SYMLINK+="ttyDXL" 16 | 17 | # RPLidar 18 | SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="rplidar" 19 | 20 | # Kobuki base 21 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="kobuki*", ATTR{device/latency_timer}="1", MODE:="0666", GROUP:="dialout", SYMLINK+="kobuki" 22 | 23 | # LifeCam Cinema 24 | SUBSYSTEM=="video4linux", ATTRS{idVendor}=="045e", ATTRS{idProduct}=="0812", ATTR{index}=="0", SYMLINK+="lifecam" 25 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamixel_workbench_toolbox 4 | 2.2.0 5 | 6 | This package is composed of 'dynamixel_item', 'dynamixel_tool', 'dynamixel_driver' and 'dynamixel_workbench' class. 7 | The 'dynamixel_item' is saved as control table item and information of Dynamixels. 8 | The 'dynamixel_tool' class loads its by model number of Dynamixels. 9 | The 'dynamixel_driver' class includes wraped function used in DYNAMIXEL SDK. 10 | The 'dynamixel_workbench' class make simple to use Dynamixels 11 | 12 | Apache 2.0 13 | Darby Lim 14 | Ryan Shim 15 | Will Son 16 | http://wiki.ros.org/dynamixel_workbench_toolbox 17 | http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ 18 | https://github.com/ROBOTIS-GIT/dynamixel-workbench 19 | https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues 20 | catkin 21 | roscpp 22 | dynamixel_sdk 23 | 24 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/test/example1_report_norm.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include 8 | 9 | #include "xarm/connect.h" 10 | #include "xarm/report_data.h" 11 | 12 | int main(int argc, char **argv) { 13 | if (argc < 2) { 14 | printf("Please enter IP address\n"); 15 | return 0; 16 | } 17 | char *server_ip = argv[1]; 18 | SocketPort *arm_report = connext_tcp_report_norm(server_ip); 19 | if (arm_report == NULL) return 0; 20 | 21 | int rxcnt = 0; 22 | ReportDataNorm *norm_data = new ReportDataNorm(); 23 | 24 | unsigned char rx_data[1280]; 25 | int ret; 26 | int err_num = 0; 27 | while (1) { 28 | usleep(1000); 29 | 30 | ret = arm_report->read_frame(rx_data); 31 | if (ret != 0) continue; 32 | ret = norm_data->flush_data(rx_data); 33 | 34 | if (ret == 0) { 35 | rxcnt++; 36 | printf("\n【normal report】: len = %d, rxcnt = %d, err_num = %d\n", 37 | bin8_to_32(rx_data), rxcnt, err_num); 38 | norm_data->print_data(); 39 | } else { 40 | printf("Error: norm_data.flush_data failed, ret = %d\n", ret); 41 | err_num++; 42 | } 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/srv/Move.srv: -------------------------------------------------------------------------------- 1 | # request: command specification for motion executions. 2 | # Units: 3 | # joint space/angles: radian, radian/s and radian/s^2. 4 | # Cartesian space: mm, mm/s, and mm/s^2. 5 | # time: sec 6 | 7 | # pose： target coordinate. 8 | # For Joint Space target，pose dimention is the number of joints. element as each target joint position. 9 | # For Cartesian target: pose dimention is 6 for (x, y, z, roll, pitch, yaw) 10 | 11 | float32[] pose 12 | 13 | # mvvelo: specified maximum velocity during execution. linear or angular velocity 14 | 15 | float32 mvvelo 16 | 17 | # mvacc: specified maximum acceleration during execution. linear or angular acceleration. 18 | 19 | float32 mvacc 20 | 21 | # mvtime: currently do not have any special meaning, please just give it 0. 22 | # PLEASE NOTE: after firmware version 1.5, For servo_cartesian motion, mvtime will be used as indicator of coordinate system. (0 for BASE coordinate, non-zero for TOOL coordinate) 23 | 24 | float32 mvtime 25 | 26 | # mvradii: this is special for move_ineb service, meaning the blending radius between 2 straight path trajectories, 0 for no blend. 27 | 28 | float32 mvradii 29 | 30 | --- 31 | 32 | # response: 33 | # ret is 0 for successful execution and others for errors or warnings occured 34 | # message is a string returned by function, indicating execution status. 35 | 36 | int16 ret 37 | 38 | string message -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/od_index.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ 2 | #define INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ 3 | 4 | #define PORT "chassis" 5 | 6 | typedef enum 7 | { 8 | SYS_INIT = 0x00, 9 | SYS_NORMAL, 10 | SYS_REMOTE, 11 | SYS_ESTOP, 12 | SYS_CALIB, 13 | SYS_TEST, 14 | SYS_CHARGING, 15 | 16 | SYS_ERR = 0x10, 17 | SYS_ERR_ID, 18 | SYS_ERR_COM, 19 | SYS_ERR_ENC, 20 | SYS_ERR_COLLISION, 21 | SYS_ERR_LOW_VOLTAGE, 22 | SYS_ERR_OVER_VOLTAGE, 23 | SYS_ERR_OVER_CURRENT, 24 | SYS_ERR_OVER_TEMP, 25 | 26 | SYS_STATE_LEN, 27 | } SystemState; 28 | 29 | typedef enum 30 | { 31 | INDEX_SYS_STATE = 0, 32 | INDEX_SYS_POWER_PERCENTAGE = 1, 33 | 34 | INDEX_CHASSIS_VEL = 2, 35 | INDEX_CHASSIS_POS_OR_OMEGA = 3, 36 | INDEX_CHASSIS_ODOM_X = 4, 37 | INDEX_CHASSIS_ODOM_Y = 5, 38 | INDEX_CHASSIS_ODOM_THETA = 6, 39 | 40 | INDEX_SYS_VOLTAGE = 7, 41 | INDEX_SYS_CURRENT = 8, 42 | 43 | INDEX_AIM_CHASSIS_VEL = 9, 44 | INDEX_AIM_CHASSIS_POS_OR_OMEGA = 10, 45 | 46 | INDEX_SYS_CHARGE = 11, 47 | INDEX_SYS_ALARM = 12, 48 | INDEX_SYS_TEXT = 13, 49 | INDEX_SYS_LIGHT = 14, 50 | INDEX_SYS_COLLISION = 15, 51 | INDEX_SYS_VERSION = 16, 52 | INDEX_STATE_LIGHT = 17, 53 | INDEX_STATE_JOY = 18, 54 | INDEX_STATE_IO = 21, 55 | INDEX_SYS_CMD = 22, 56 | } OdIndex; 57 | 58 | #endif // INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ 59 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/msg/RobotMsg.msg: -------------------------------------------------------------------------------- 1 | # feedback information of the controlled robot 2 | 3 | # state of robot: 4 | # 1 – RUNNING, executing motion command. 5 | # 2 – SLEEPING, not in execution, but ready to move. 6 | # 3 – PAUSED, paused in the middle of unfinished motion. 7 | # 4 – STOPPED, not ready for any motion commands. 8 | # 5 – CONFIG_CHANGED, system configuration or mode changed, not ready for motion commands. 9 | int16 state 10 | 11 | # mode of robot: 12 | # 0 for POSITION mode.(position control by xarm controller box, execute api standard commands) 13 | # 1 for SERVOJ mode. (Immediate execution towards received joint space target, like a step response) 14 | # 2 for TEACHING_JOINT mode. (Gravity compensated mode, easy for teaching) 15 | int16 mode 16 | 17 | # cmdnum: number of commands waiting in the buffer. 18 | int16 cmdnum 19 | 20 | # mt_brake: if translated to binary digits, each bit represent one axis, 1 for brake enabled, 0 for brake disabled 21 | int16 mt_brake 22 | 23 | # mt_able: if translated to binary digits, each bit represent one axis, 1 for servo control enabled, 0 for servo disabled 24 | int16 mt_able 25 | 26 | # error code (if non-zero) 27 | int16 err 28 | 29 | # warning code (if non-zero) 30 | int16 warn 31 | 32 | # current joint angles expressed in radian. 33 | float32[] angle 34 | 35 | # current TCP Cartesian position expressed in mm(position), radian(orientation) 36 | float32[6] pose 37 | 38 | # TCP offset from center of flange, with respect to tool frame. 39 | float32[6] offset -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.yml: -------------------------------------------------------------------------------- 1 | name: Feature Request 2 | description: Suggest an idea for this project 3 | title: "[Enhancement]: " 4 | labels: ["enhancement"] 5 | assignees: 6 | - LSInterbotix 7 | body: 8 | - type: markdown 9 | attributes: 10 | value: | 11 | Thanks for taking the time to suggest a new feature! 12 | - type: textarea 13 | id: describe-feature 14 | attributes: 15 | label: Describe the solution you'd like 16 | description: A clear and concise description of what you want to happen. 17 | validations: 18 | required: true 19 | - type: textarea 20 | id: platform 21 | attributes: 22 | label: Platform 23 | description: Which platform would you like to see this feature on? 24 | placeholder: X-Series Arms, LoCoBots, etc. 25 | - type: textarea 26 | id: problem 27 | attributes: 28 | label: Is your feature request related to a problem? Please describe. 29 | description: A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 30 | - type: textarea 31 | id: logs 32 | attributes: 33 | label: Suggested code 34 | description: Please suggest any code or commands that you think would help with implementing the feature. This will be automatically formatted into code, so no need for backticks. 35 | - type: textarea 36 | id: other 37 | attributes: 38 | label: Additional Info 39 | description: Please enter anything else you'd like to share here. 40 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interbotix_slate_driver 4 | 0.0.0 5 | The interbotix_slate_driver package 6 | 7 | Trossen Robotics 8 | 9 | BSD-3-Clause 10 | 11 | catkin 12 | 13 | geometry_msgs 14 | interbotix_slate_msgs 15 | nav_msgs 16 | roscpp 17 | sensor_msgs 18 | std_srvs 19 | tf 20 | 21 | geometry_msgs 22 | interbotix_slate_msgs 23 | nav_msgs 24 | roscpp 25 | sensor_msgs 26 | std_srvs 27 | tf 28 | 29 | geometry_msgs 30 | interbotix_slate_msgs 31 | nav_msgs 32 | roscpp 33 | sensor_msgs 34 | std_srvs 35 | tf 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Trossen Robotics 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 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/port/serial.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef PORT_SERIAL_H_ 8 | #define PORT_SERIAL_H_ 9 | 10 | #include 11 | 12 | #include "xarm/common/data_type.h" 13 | #include "xarm/common/queue_memcpy.h" 14 | 15 | class SerialPort { 16 | public: 17 | SerialPort(const char *port, int baud, int que_num, int que_maxlen); 18 | ~SerialPort(void); 19 | int is_ok(void); 20 | void flush(void); 21 | void recv_proc(void); 22 | int write_frame(unsigned char *data, int len); 23 | int read_frame(unsigned char *data); 24 | void close_port(void); 25 | int que_maxlen_; 26 | int que_num_; 27 | 28 | private: 29 | int fp_; 30 | int state_; 31 | pthread_t thread_id_; 32 | QueueMemcpy *rx_que_; 33 | int init_serial(const char *port, int baud); 34 | int read_char(unsigned char *ch); 35 | int read_str(unsigned char *data, char eol, int len); 36 | int write_char(unsigned char ch); 37 | void parse_put(unsigned char *data, int len); 38 | 39 | typedef enum _UXBUS_RECV_STATE { 40 | UXBUS_START_FROMID = 0, 41 | UXBUS_START_TOOID = 1, 42 | UXBUS_STATE_LENGTH = 2, 43 | UXBUS_STATE_DATA = 3, 44 | UXBUS_STATE_CRC1 = 4, 45 | UXBUS_STATE_CRC2 = 5, 46 | } UXBUS_RECV_STATE; 47 | 48 | unsigned char UXBUS_PROT_FROMID_; 49 | unsigned char UXBUS_PROT_TOID_; 50 | 51 | int rx_data_idx_; 52 | int rx_state_; 53 | unsigned char rx_buf_[128]; 54 | int rx_length_; 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/question.yml: -------------------------------------------------------------------------------- 1 | name: Question 2 | description: Ask a question 3 | title: "[Question]: " 4 | labels: ["question"] 5 | assignees: 6 | - LSInterbotix 7 | body: 8 | - type: markdown 9 | attributes: 10 | value: | 11 | Thanks for taking the time to ask a question! 12 | - type: textarea 13 | id: question 14 | attributes: 15 | label: Question 16 | description: Ask your question here. 17 | validations: 18 | required: true 19 | - type: textarea 20 | id: robot_model 21 | attributes: 22 | label: Robot Model 23 | description: Which robot are you asking about (if any)? 24 | placeholder: wx250s, locobot_px100, etc. 25 | - type: dropdown 26 | id: operating_system 27 | attributes: 28 | label: Operating System 29 | description: Which operating system are you targeting when asking this question? 30 | multiple: true 31 | options: 32 | - Ubuntu 18.04 33 | - Ubuntu 20.04 34 | - Ubuntu 22.04 35 | - Other (Describe in "Additional Info") 36 | validations: 37 | required: true 38 | - type: dropdown 39 | id: ros_version 40 | attributes: 41 | label: ROS Version 42 | description: Which version of ROS are you targeting when asking this question? 43 | multiple: true 44 | options: 45 | - ROS 1 Melodic 46 | - ROS 1 Noetic 47 | - ROS 2 Galactic 48 | - ROS 2 Humble 49 | - ROS 2 Rolling 50 | - Other (Describe in "Additional Info") 51 | validations: 52 | required: true 53 | - type: textarea 54 | id: other 55 | attributes: 56 | label: Additional Info 57 | description: Please enter anything else you'd like to share here. 58 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/debug/debug_print.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/debug/debug_print.h" 8 | 9 | #include 10 | 11 | #include "xarm/common/data_type.h" 12 | 13 | #define DB_FLG "[deprint ] " 14 | #define PRINTF_NUM_MAX 128 15 | #define log_put printf 16 | 17 | void print_log(const char *format, ...) { 18 | char buffer[PRINTF_NUM_MAX] = {0}; 19 | va_list arg; 20 | va_start(arg, format); 21 | vsnprintf(buffer, PRINTF_NUM_MAX, format, arg); 22 | printf("%s", buffer); 23 | va_end(arg); 24 | } 25 | 26 | void print_nvect(const char *str, double vect[], int n) { 27 | print_log("%s", str); 28 | for (int i = 0; i < n; ++i) { print_log("%0.3f ", vect[i]); } 29 | print_log("\n"); 30 | } 31 | 32 | void print_nvect(const char *str, float *vect, int n) { 33 | print_log("%s", str); 34 | for (int i = 0; i < n; ++i) { print_log("%0.3f ", vect[i]); } 35 | print_log("\n"); 36 | } 37 | 38 | void print_nvect(const char *str, unsigned char vect[], int n) { 39 | print_log("%s", str); 40 | for (int i = 0; i < n; ++i) { print_log("%d ", vect[i]); } 41 | print_log("\n"); 42 | } 43 | 44 | void print_nvect(const char *str, int vect[], int n) { 45 | print_log("%s", str); 46 | for (int i = 0; i < n; ++i) { print_log("%d ", vect[i]); } 47 | print_log("\n"); 48 | } 49 | 50 | void print_hex(const char *str, unsigned char *hex, int len) { 51 | char buf[len * 3 + 1] = {'\0'}; 52 | long i; 53 | for (i = 0; i < len; ++i) { sprintf((char *)&buf[i * 3], "%02x ", hex[i]); } 54 | 55 | printf("%s %s\n", str, buf); 56 | } 57 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/srv/RobotInfo.srv: -------------------------------------------------------------------------------- 1 | # Get robot information 2 | # 3 | # Note that if a 'gripper' joint is specified, all information will be specified in terms of the 'left_finger' joint 4 | 5 | string cmd_type # set to 'group' if requesting info about a joint group or 'single' if requesting info about a single joint 6 | string name # the group name if requesting info about a group or the joint name if requesting info about a single joint 7 | --- 8 | string mode # the operating mode for the specified joint group or joint 9 | string profile_type # the profile type for the specified joint group or joint 10 | string[] joint_names # the name of each joint in a group or just the specified joint 11 | int16[] joint_ids # the Dynamixel ID for each joint in a group or for the specified joint 12 | float32[] joint_lower_limits # the lower limit [radians] for each joint in a group or for the specified joint (taken from URDF) 13 | float32[] joint_upper_limits # the upper limit [radians] for each joint in a group or for the specified joint (taken from URDF) 14 | float32[] joint_velocity_limits # the velocity limit [rad/s] for each joint in a group or for the specified joint (taken from URDF) 15 | float32[] joint_sleep_positions # the sleep position [rad] for each joint in a group or for the specified joint 16 | int16[] joint_state_indices # index for each joint in a group or for the specified joint in the published JointState message 17 | int16 num_joints # the number of joints in a group or 1 18 | string[] name # the name of the group or joint requested; if group was 'all', returns the names of all groups 19 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.0) 2 | project(interbotix_slate_driver) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-Wall -g -std=c++11 -O0 ") 6 | 7 | set(serial_driver "chassis_driver") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | geometry_msgs 11 | interbotix_slate_msgs 12 | nav_msgs 13 | roscpp 14 | sensor_msgs 15 | std_srvs 16 | tf 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | include 22 | LIBRARIES 23 | ${PROJECT_NAME} 24 | CATKIN_DEPENDS 25 | geometry_msgs 26 | interbotix_slate_msgs 27 | nav_msgs 28 | roscpp 29 | sensor_msgs 30 | std_srvs 31 | tf 32 | ) 33 | 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib) 40 | 41 | add_library(slate_base 42 | src/slate_base.cpp 43 | src/base_driver.cpp 44 | ) 45 | add_dependencies(slate_base ${slate_base_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 46 | 47 | target_link_libraries(slate_base 48 | ${serial_driver} 49 | ${catkin_LIBRARIES} 50 | ) 51 | 52 | add_executable(slate_base_node 53 | src/slate_base_node.cpp 54 | ) 55 | add_dependencies(slate_base_node ${slate_base_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 56 | 57 | target_link_libraries(slate_base_node 58 | slate_base 59 | ) 60 | 61 | add_custom_command( 62 | OUTPUT copy_lib 63 | COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/lib/* ${CATKIN_DEVEL_PREFIX}/lib 64 | ) 65 | add_custom_target(COPY_CHASSIS_LIB ALL DEPENDS copy_lib slate_base_node) 66 | 67 | install( 68 | TARGETS 69 | slate_base_node 70 | RUNTIME DESTINATION 71 | ${CATKIN_PACKAGE_BIN_DESTINATION} 72 | ) 73 | 74 | install( 75 | FILES 76 | lib/lib${serial_driver}.so 77 | DESTINATION 78 | ${CATKIN_PACKAGE_LIB_DESTINATION} 79 | ) 80 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/scripts/servo_cartesian_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import time 5 | import rospy 6 | from xarm_msgs.srv import * 7 | 8 | def servo_cartesian_tool(step, freq, time_secs): 9 | servo_cart = rospy.ServiceProxy('/xarm/move_servo_cart', Move) 10 | req = MoveRequest() 11 | req.pose = step 12 | req.mvvelo = 0 13 | req.mvacc = 0 14 | req.mvtime = 1 # tool coordinate motion 15 | loop_num = time_secs*float(freq) 16 | sleep_sec = 1.0/float(freq) 17 | ret = 0 18 | 19 | try: 20 | for i in range(int(loop_num)): 21 | res = servo_cart(req) 22 | if res.ret: 23 | print("Something Wrong happened calling servo_cart service, index is %d, ret = %d"%(i, res.ret)) 24 | ret = -1 25 | break 26 | time.sleep(sleep_sec) 27 | return ret 28 | 29 | except rospy.ServiceException as e: 30 | print("servo_cartesian (tool) Service call failed: %s"%e) 31 | return -1 32 | 33 | 34 | if __name__ == "__main__": 35 | 36 | rospy.wait_for_service('/xarm/move_servo_cart') 37 | rospy.set_param('/xarm/wait_for_finish', True) # return after motion service finish 38 | 39 | motion_en = rospy.ServiceProxy('/xarm/motion_ctrl', SetAxis) 40 | set_mode = rospy.ServiceProxy('/xarm/set_mode', SetInt16) 41 | set_state = rospy.ServiceProxy('/xarm/set_state', SetInt16) 42 | home = rospy.ServiceProxy('/xarm/go_home', Move) 43 | 44 | try: 45 | motion_en(8,1) 46 | set_mode(0) 47 | set_state(0) 48 | req = MoveRequest() # MoveRequest for go_home 49 | req.mvvelo = 0.7 50 | req.mvacc = 3.5 51 | req.mvtime = 0 52 | home(req) 53 | 54 | except rospy.ServiceException as e: 55 | print("Before servo_cartesian, service call failed: %s"%e) 56 | exit(-1) 57 | 58 | # configurations for servo_cartesian 59 | set_mode(1) 60 | set_state(0) 61 | 62 | # relative step for servo_cartesian in TOOL Coordinate (req.mvtime = 1) 63 | step = [0.3, 0, 0, 0, 0, 0] 64 | # publish frequency in Hz: 65 | freq = 100 66 | # publish time in seconds: 67 | time_secs = 5.0 68 | 69 | time.sleep(2.0) 70 | if servo_cartesian_tool(step, freq, time_secs) == 0: 71 | print("execution finished successfully!") -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeeded to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | return 0; 70 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_ser.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/instruction/uxbus_cmd_ser.h" 8 | 9 | #include 10 | 11 | #include "xarm/common/crc16.h" 12 | #include "xarm/debug/debug_print.h" 13 | #include "xarm/instruction/uxbus_cmd_config.h" 14 | 15 | UxbusCmdSer::UxbusCmdSer(SerialPort *arm_port) { arm_port_ = arm_port; } 16 | UxbusCmdSer::~UxbusCmdSer(void) {} 17 | 18 | int UxbusCmdSer::check_xbus_prot(unsigned char *datas, int funcode) { 19 | if (datas[3] & 0x40) 20 | { return UXBUS_STATE::ERR_CODE; } 21 | else 22 | if (datas[3] & 0x20) 23 | { return UXBUS_STATE::WAR_CODE; } 24 | else 25 | { return 0; } 26 | } 27 | 28 | int UxbusCmdSer::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) { 29 | int ret; 30 | unsigned char rx_data[arm_port_->que_maxlen_] = {0}; 31 | int times = timeout; 32 | while (times) { 33 | times -= 1; 34 | ret = arm_port_->read_frame(rx_data); 35 | if (ret != -1) { 36 | ret = check_xbus_prot(rx_data, funcode); 37 | for (int i = 0; i < num; i++) { ret_data[i] = rx_data[i + 4]; } 38 | return ret; 39 | } 40 | usleep(1000); 41 | } 42 | return UXBUS_STATE::ERR_TOUT; 43 | } 44 | 45 | int UxbusCmdSer::send_xbus(int funcode, unsigned char *datas, int num) { 46 | int i; 47 | unsigned char send_data[num + 4]; 48 | 49 | send_data[0] = UXBUS_CONF::MASTER_ID; 50 | send_data[1] = UXBUS_CONF::SLAVE_ID; 51 | send_data[2] = num + 1; 52 | send_data[3] = funcode; 53 | for (i = 0; i < num; i++) { send_data[4 + i] = datas[i]; } 54 | 55 | int crc = modbus_crc(send_data, 4 + num); 56 | send_data[4 + num] = (unsigned char)(crc & 0xFF); 57 | send_data[5 + num] = (unsigned char)((crc >> 8) & 0xFF); 58 | 59 | arm_port_->flush(); 60 | int ret = arm_port_->write_frame(send_data, num + 6); 61 | return ret; 62 | } 63 | 64 | void UxbusCmdSer::close(void) { arm_port_->close_port(); } 65 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/common/queue_memcpy.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/common/queue_memcpy.h" 8 | 9 | #include 10 | 11 | QueueMemcpy::QueueMemcpy(long n, long n_size) { 12 | total_ = n; 13 | annode_size_ = n_size; 14 | buf_ = new char[total_ * annode_size_]; 15 | pthread_mutex_init(&mutex_, NULL); 16 | flush(); 17 | } 18 | 19 | QueueMemcpy::~QueueMemcpy(void) { delete[] buf_; } 20 | 21 | char QueueMemcpy::flush(void) { 22 | cnt_ = 0; 23 | head_ = 0; 24 | tail_ = 0; 25 | memset(buf_, 0, annode_size_ * total_); 26 | 27 | return 0; 28 | } 29 | 30 | long QueueMemcpy::size(void) { return cnt_; } 31 | 32 | int QueueMemcpy::is_full(void) { 33 | if (total_ <= cnt_) 34 | return 1; 35 | else 36 | return 0; 37 | } 38 | 39 | long QueueMemcpy::node_size(void) { return annode_size_; } 40 | 41 | char QueueMemcpy::pop(void *data) { 42 | pthread_mutex_lock(&mutex_); 43 | if (0 >= cnt_) { 44 | pthread_mutex_unlock(&mutex_); 45 | return -1; 46 | } 47 | if (total_ <= tail_) tail_ = 0; 48 | 49 | memcpy(data, &buf_[tail_ * annode_size_], annode_size_); 50 | tail_++; 51 | cnt_--; 52 | pthread_mutex_unlock(&mutex_); 53 | return 0; 54 | } 55 | 56 | char QueueMemcpy::get(void *data) { 57 | pthread_mutex_lock(&mutex_); 58 | if (0 >= cnt_) { 59 | pthread_mutex_unlock(&mutex_); 60 | return -1; 61 | } 62 | if (total_ <= tail_) tail_ = 0; 63 | 64 | memcpy(data, &buf_[tail_ * annode_size_], annode_size_); 65 | pthread_mutex_unlock(&mutex_); 66 | 67 | return 0; 68 | } 69 | 70 | char QueueMemcpy::push(void *data) { 71 | pthread_mutex_lock(&mutex_); 72 | if (total_ <= cnt_) { 73 | pthread_mutex_unlock(&mutex_); 74 | return -1; 75 | } 76 | if (total_ <= head_) head_ = 0; 77 | 78 | memcpy(&buf_[head_ * annode_size_], data, annode_size_); 79 | head_++; 80 | cnt_++; 81 | pthread_mutex_unlock(&mutex_); 82 | return 0; 83 | } 84 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/a_Model_Scan.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | 26 | if (argc < 3) 27 | { 28 | printf("Please set '-port_name' and '-baud_rate' arguments for connected Dynamixels\n"); 29 | return 0; 30 | } 31 | else 32 | { 33 | port_name = argv[1]; 34 | baud_rate = atoi(argv[2]); 35 | } 36 | 37 | DynamixelWorkbench dxl_wb; 38 | 39 | const char *log; 40 | bool result = false; 41 | 42 | uint8_t scanned_id[16]; 43 | uint8_t dxl_cnt = 0; 44 | uint8_t range = 100; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | printf("Wait for scan...\n"); 58 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to scan\n"); 63 | } 64 | else 65 | { 66 | printf("Find %d Dynamixels\n", dxl_cnt); 67 | 68 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 69 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 70 | } 71 | 72 | return 0; 73 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/ArmJoy.msg: -------------------------------------------------------------------------------- 1 | # This message is used specifically in the interbotix_xsarm_joy package 2 | # 3 | # Maps raw 'joy' commands to more specific ones to control an Interbotix manipulator 4 | 5 | # enum values that define the joystick controls for the robot 6 | 7 | ######################################################################################################### 8 | 9 | # Control the motion of the virtual 'ee_gripper_link' or end effector using the modern_robotics_ik engine 10 | # Position Control 11 | int8 EE_X_INC = 1 12 | int8 EE_X_DEC = 2 13 | int8 EE_Y_INC = 3 14 | int8 EE_Y_DEC = 4 15 | int8 EE_Z_INC = 5 16 | int8 EE_Z_DEC = 6 17 | 18 | # Orientation Control 19 | int8 EE_ROLL_CCW = 7 20 | int8 EE_ROLL_CW = 8 21 | int8 EE_PITCH_UP = 9 22 | int8 EE_PITCH_DOWN = 10 23 | 24 | ######################################################################################################### 25 | 26 | # Control the motion of independent joints on the Arm or send predefined robot poses 27 | # Waist Joint Control 28 | int8 WAIST_CCW = 11 29 | int8 WAIST_CW = 12 30 | 31 | # Gripper Control 32 | int8 GRIPPER_OPEN = 13 33 | int8 GRIPPER_CLOSE = 14 34 | 35 | # Pose Control 36 | int8 HOME_POSE = 15 37 | int8 SLEEP_POSE = 16 38 | 39 | ######################################################################################################### 40 | 41 | # Customize configurations for the Interbotix Arm 42 | # Inc/Dec Joint speed 43 | int8 SPEED_INC = 17 44 | int8 SPEED_DEC = 18 45 | 46 | # Quickly toggle between a fast and slow speed setting 47 | int8 SPEED_COURSE = 19 48 | int8 SPEED_FINE = 20 49 | 50 | # Inc/Dec Gripper pressure 51 | int8 GRIPPER_PWM_INC = 21 52 | int8 GRIPPER_PWM_DEC = 22 53 | 54 | # Torque On/Off all servos 55 | int8 TORQUE_ON = 23 56 | int8 TORQUE_OFF = 24 57 | 58 | ######################################################################################################### 59 | 60 | # Control the motion of the Interbotix Arm 61 | int8 ee_x_cmd 62 | int8 ee_y_cmd 63 | int8 ee_z_cmd 64 | int8 ee_roll_cmd 65 | int8 ee_pitch_cmd 66 | 67 | # Independent Joint/Pose Control 68 | int8 waist_cmd 69 | int8 gripper_cmd 70 | int8 pose_cmd 71 | 72 | # Misc. Configs 73 | int8 speed_cmd 74 | int8 speed_toggle_cmd 75 | int8 gripper_pwm_cmd 76 | int8 torque_cmd 77 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interbotix_xs_sdk 4 | 0.0.0 5 | The interbotix_xs_sdk package 6 | Solomon Wiznitzer 7 | BSD 8 | 9 | Solomon Wiznitzer 10 | 11 | 12 | 13 | catkin 14 | actionlib 15 | control_msgs 16 | dynamixel_workbench_toolbox 17 | message_generation 18 | roscpp 19 | sensor_msgs 20 | interbotix_xs_msgs 21 | urdf 22 | yaml-cpp 23 | actionlib 24 | control_msgs 25 | dynamixel_workbench_toolbox 26 | message_generation 27 | roscpp 28 | sensor_msgs 29 | interbotix_xs_msgs 30 | urdf 31 | yaml-cpp 32 | actionlib 33 | control_msgs 34 | dynamixel_workbench_toolbox 35 | message_runtime 36 | roscpp 37 | sensor_msgs 38 | interbotix_xs_msgs 39 | urdf 40 | yaml-cpp 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.yml: -------------------------------------------------------------------------------- 1 | name: Bug Report 2 | description: File a bug report 3 | title: "[Bug]: " 4 | labels: ["bug"] 5 | assignees: 6 | - LSInterbotix 7 | body: 8 | - type: markdown 9 | attributes: 10 | value: | 11 | Thanks for taking the time to fill out this bug report! 12 | - type: textarea 13 | id: what-happened 14 | attributes: 15 | label: What happened? 16 | description: Also tell us what you expected to happen. 17 | placeholder: Tell us what you see! 18 | validations: 19 | required: true 20 | - type: textarea 21 | id: robot_model 22 | attributes: 23 | label: Robot Model 24 | description: Which robot are you using? 25 | placeholder: wx250s, locobot_px100, etc. 26 | validations: 27 | required: true 28 | - type: dropdown 29 | id: operating_system 30 | attributes: 31 | label: Operating System 32 | description: Which operating system are you seeing this problem on? 33 | multiple: true 34 | options: 35 | - Ubuntu 18.04 36 | - Ubuntu 20.04 37 | - Ubuntu 22.04 38 | - Other (Describe in "Additional Info") 39 | validations: 40 | required: true 41 | - type: dropdown 42 | id: ros_distro 43 | attributes: 44 | label: ROS Distro 45 | description: Which distribution of ROS are you seeing this problem on? 46 | multiple: true 47 | options: 48 | - ROS 1 Melodic 49 | - ROS 1 Noetic 50 | - ROS 2 Galactic 51 | - ROS 2 Humble 52 | - ROS 2 Rolling 53 | - Other (Describe in "Additional Info") 54 | validations: 55 | required: true 56 | - type: textarea 57 | id: steps 58 | attributes: 59 | label: Steps To Reproduce 60 | description: What were the steps you took immediately preceding your issue? 61 | - type: textarea 62 | id: logs 63 | attributes: 64 | label: Relevant log output 65 | description: Please copy and paste any relevant log output. This will be automatically formatted into code, so no need for backticks. 66 | render: shell 67 | - type: textarea 68 | id: other 69 | attributes: 70 | label: Additional Info 71 | description: Please enter anything else you'd like to share here. 72 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/config/mode_configs_template.yaml: -------------------------------------------------------------------------------- 1 | # This file is used to customize the initial operating modes of all joint groups and individual joints in a robot. 2 | # It should be placed in the 'config' directory of any ROS package that builds ontop of the interbotix_xs_sdk. 3 | # Example values are shown as well. All parameters are mandatory unless stated otherwise 4 | 5 | port: /dev/ttyDXL # Optional; specifies the USB port that connects to the U2D2; 6 | # if specified, it overwrites the port in the 'motor_configs' yaml file. 7 | # if the 'port' parameter and its value are left out, the port in the 'motor_configs' yaml file is used. 8 | # if the 'port' parameter is specified but the value is not, it defaults to '/dev/ttyDXL' 9 | 10 | groups: # Optional; specify initial Operating Modes for all joint groups (as detailed in the 'motor_configs' yaml file under 'groups') 11 | arm: # Example joint group name 12 | operating_mode: position # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for the seven modes; defaults to 'position' 13 | profile_type: time # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for the two profile types; defaults to 'velocity' 14 | profile_velocity: 2000 # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for a description; defaults to '0' 15 | profile_acceleration: 1000 # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for a description; defaults to '0' 16 | torque_enable: true # Optional; whether the motors in this group should be torqued on or off by default; defaults to 'true' 17 | 18 | singles: # Optional; specify the Operating Mode for individual joints (any joint specified in the 'motor_configs' yaml file); parameters follow the same structure as above 19 | gripper: 20 | operating_mode: pwm 21 | torque_enable: true 22 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.reset(dxl_id, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to reset\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to reset\n"); 78 | } 79 | 80 | return 0; 81 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.reboot(dxl_id, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to reboot\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to reboot\n"); 78 | } 79 | 80 | return 0; 81 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int dxl_id = 1; 25 | int baud_rate = 57600; 26 | int new_baud_rate = 1000000; 27 | 28 | if (argc < 5) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel_id', '-new_baud_rate' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | new_baud_rate = atoi(argv[4]); 39 | } 40 | 41 | DynamixelWorkbench dxl_wb; 42 | 43 | const char *log; 44 | bool result = false; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | uint16_t model_number = 0; 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | 64 | return 0; 65 | } 66 | else 67 | { 68 | printf("Succeeded to ping\n"); 69 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 70 | } 71 | 72 | result = dxl_wb.changeBaudrate(dxl_id, new_baud_rate, &log); 73 | if (result == false) 74 | { 75 | printf("%s\n", log); 76 | return 0; 77 | } 78 | else 79 | { 80 | printf("%s\n", log); 81 | } 82 | 83 | return 0; 84 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_msgs/ReadMe.md: -------------------------------------------------------------------------------- 1 | # Service Introduction 2 |   The services and messages defined in this package is based on the programming API. To offer a way for users to call them by ROS interface. Services provided are: 3 | 4 | ## motion_ctrl 5 |   To enable or disable the servo control of any joint.(message type: ***xarm_msgs::SetAxis***) 6 | ## set_mode 7 |   To set operation mode. (message type: ***xarm_msgs::SetInt16***) 8 |   * 0 for POSE mode, the robot will be position controlled. Trajectory will be planned by XArm Controller. 9 |   * 1 for SERVOJ mode, the robot will be commanded by servo_j function, fast & immediate execution like a step response, use this if user can generate properly interpolated trajectory. 10 |   * 2 for TEACH_JOINT mode, Gravity compensated mode, no position control. 11 | 12 | ## set_state 13 |   To set robot state. (message type: ***xarm_msgs::SetInt16***) 14 |   * 0 for READY/START state, robot must be in this state to perform any motion. 15 |   * 3 for PAUSE state, robot motion will be suspended. 16 |   * 4 for STOP state, if error occurs or configuration changes, robot will switch to this state. 17 | 18 | ## go_home 19 |   Robot will go to home position with specified velocity and acceleration.(message type: ***xarm_msgs::Move***) 20 | 21 | ## move_line 22 |   Robot TCP will move to Caetesian target point with a straight line trajectory. Under specified Cartesian velocity and acceleartion. (message type: ***xarm_msgs::Move***) 23 | 24 | ## move_lineb 25 |   Given a set of targets, robot will move to final target through middle points, at each middle point, 2 straight-line trajectory will be blended with specified radius. (message type: ***xarm_msgs::Move***) 26 | 27 | ## move_joint 28 |   Given all desired joint positions, and max joint angular velocity/acceleration, robot will move to joint space target. (message type: ***xarm_msgs::Move***) 29 | 30 | ## move_servoj: 31 |   Used in SERVOJ mode, and if user have trajectory planned, call this service with high enough rate. Each trajectory point will be executed fast and immediately. (message type: ***xarm_msgs::Move***) 32 | 33 | ***Please check the inside [srv](./srv/) files for detailed data information.*** 34 | 35 | # Feedback Status Message 36 | 37 | Refer to [RobotMsg](./msg/RobotMsg.msg) for robot feedback information contents published through topic "/xarm/xarm_states". -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/connect.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/connect.h" 8 | 9 | #include "xarm/instruction/uxbus_cmd.h" 10 | #include "xarm/instruction/uxbus_cmd_ser.h" 11 | #include "xarm/instruction/uxbus_cmd_tcp.h" 12 | #include "xarm/xarm_config.h" 13 | 14 | UxbusCmdSer *connect_rs485_control(const char *com) { 15 | SerialPort *arm_port = new SerialPort(com, XARM_CONF::SERIAL_BAUD, 3, 128); 16 | if (arm_port->is_ok() != 0) { 17 | printf("Error: Serial RS485 connection failed\n"); 18 | return NULL; 19 | } 20 | UxbusCmdSer *arm_cmd = new UxbusCmdSer(arm_port); 21 | printf("Serial RS485 connection successful\n"); 22 | return arm_cmd; 23 | } 24 | 25 | UxbusCmdTcp *connect_tcp_control(char *server_ip) { 26 | SocketPort *arm_port = 27 | new SocketPort(server_ip, XARM_CONF::TCP_PORT_CONTROL, 3, 128); 28 | if (arm_port->is_ok() != 0) { 29 | printf("Error: Tcp Control connection failed\n"); 30 | return NULL; 31 | } 32 | UxbusCmdTcp *arm_cmd = new UxbusCmdTcp(arm_port); 33 | printf("Tcp Control connection successful\n"); 34 | return arm_cmd; 35 | } 36 | 37 | SocketPort *connext_tcp_report_norm(char *server_ip) { 38 | SocketPort *arm_report = 39 | new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_NORM, 3, 512); 40 | if (arm_report->is_ok() != 0) { 41 | printf("Error: Tcp Report Norm connection failed, ip: %s\n", server_ip); 42 | return NULL; 43 | } 44 | printf("Tcp Report Norm connection successful\n"); 45 | return arm_report; 46 | } 47 | 48 | SocketPort *connext_tcp_report_rich(char *server_ip) { 49 | SocketPort *arm_report = 50 | new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_RICH, 3, 512); 51 | if (arm_report->is_ok() != 0) { 52 | printf("Error: Tcp Report Norm connection failed\n"); 53 | return NULL; 54 | } 55 | printf("Tcp Report Norm connection successful\n"); 56 | return arm_report; 57 | } 58 | 59 | SocketPort *connext_tcp_report_devl(char *server_ip) { 60 | SocketPort *arm_report = 61 | new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_DEVL, 3, 512); 62 | if (arm_report->is_ok() != 0) { 63 | printf("Error: Tcp Report develop connection failed\n"); 64 | return NULL; 65 | } 66 | printf("Tcp Report develop connection successful\n"); 67 | return arm_report; 68 | } 69 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/HexJoy.msg: -------------------------------------------------------------------------------- 1 | # This message is used specifically in the interbotix_xshexapod_joy package 2 | # 3 | # Maps raw 'joy' commands to more specific ones to control an Interbotix hexapod 4 | 5 | # enum values that define the joystick controls for the robot 6 | 7 | ######################################################################################################### 8 | 9 | # Command Options to move the Hexapod in the 'world' or in 'place' 10 | # World Control 11 | int8 WORLD_X_INC = 1 12 | int8 WORLD_X_DEC = 2 13 | int8 WORLD_Y_INC = 3 14 | int8 WORLD_Y_DEC = 4 15 | int8 WORLD_YAW_CCW = 5 16 | int8 WORLD_YAW_CW = 6 17 | 18 | # In Place Control 19 | int8 PLACE_X_INC = 7 20 | int8 PLACE_X_DEC = 8 21 | int8 PLACE_Y_INC = 9 22 | int8 PLACE_Y_DEC = 10 23 | int8 PLACE_Z_INC = 11 24 | int8 PLACE_Z_DEC = 12 25 | int8 PLACE_ROLL_CCW = 13 26 | int8 PLACE_ROLL_CW = 14 27 | int8 PLACE_PITCH_UP = 15 28 | int8 PLACE_PITCH_DOWN = 16 29 | 30 | # Move Type 31 | int8 MOVE_HEXAPOD = 17 32 | int8 MOVE_LEG = 18 33 | 34 | # Pose Control 35 | int8 HOME_POSE = 19 36 | int8 SLEEP_POSE = 20 37 | 38 | ######################################################################################################### 39 | 40 | # Customize configurations for the Interbotix Hexapod 41 | 42 | # Cycle through the various gaits when in 'move_hexapod' mode 43 | int8 GAIT_NEXT = 21 44 | int8 GAIT_PREVIOUS = 22 45 | 46 | # Cycle through the legs when in 'move_leg' mode 47 | int8 LEG_NEXT = 23 48 | int8 LEG_PREVIOUS = 24 49 | 50 | # Widen or narrow the hexapod's stance 51 | int8 WIDEN_STANCE = 25 52 | int8 NARROW_STANCE = 26 53 | 54 | # Reboot motors if necessary 55 | int8 REBOOT_MOTORS = 27 56 | 57 | # Set the current stance and hexapod height to be the new 'Home Pose' 58 | int8 SET_HOME_POSE = 28 59 | 60 | # Inc/Dec speed 61 | int8 SPEED_INC = 29 62 | int8 SPEED_DEC = 30 63 | 64 | # Quickly toggle between a fast and slow speed setting 65 | int8 SPEED_COURSE = 31 66 | int8 SPEED_FINE = 32 67 | 68 | ######################################################################################################### 69 | 70 | # Control the motion of the Interbotix Hexapod 71 | int8 world_x_cmd 72 | int8 world_y_cmd 73 | int8 world_yaw_cmd 74 | 75 | int8 place_x_cmd 76 | int8 place_y_cmd 77 | int8 place_z_cmd 78 | int8 place_roll_cmd 79 | int8 place_pitch_cmd 80 | 81 | int8 pose_cmd 82 | int8 move_type_cmd 83 | 84 | # Other Options 85 | int8 gait_toggle_cmd 86 | int8 leg_toggle_cmd 87 | int8 stance_cmd 88 | int8 reboot_cmd 89 | int8 set_home_pose_cmd 90 | 91 | # Speed Configs 92 | int8 speed_cmd 93 | int8 speed_toggle_cmd 94 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | #define BAUDRATE_NUM 7 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | const char* port_name = "/dev/ttyUSB0"; 26 | 27 | if (argc < 2) 28 | { 29 | printf("Please set '-port_name' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | } 36 | 37 | DynamixelWorkbench dxl_wb; 38 | 39 | const char *log; 40 | bool result = false; 41 | 42 | uint8_t scanned_id[100]; 43 | uint8_t dxl_cnt = 0; 44 | 45 | uint32_t baudrate[BAUDRATE_NUM] = {9600, 57600, 115200, 1000000, 2000000, 3000000, 4000000}; 46 | uint8_t range = 253; 47 | 48 | uint8_t index = 0; 49 | 50 | while (index < BAUDRATE_NUM) 51 | { 52 | result = dxl_wb.init(port_name, baudrate[index], &log); 53 | if (result == false) 54 | { 55 | printf("%s\n", log); 56 | printf("Failed to init\n"); 57 | } 58 | else 59 | printf("Succeed to init(%d)\n", baudrate[index]); 60 | 61 | dxl_cnt = 0; 62 | for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0; 63 | 64 | printf("Wait for scan...\n"); 65 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 66 | if (result == false) 67 | { 68 | printf("%s\n", log); 69 | printf("Failed to scan\n"); 70 | } 71 | else 72 | { 73 | printf("Find %d Dynamixels\n", dxl_cnt); 74 | 75 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 76 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 77 | } 78 | 79 | index++; 80 | } 81 | 82 | return 0; 83 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/h_Position.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.jointMode(dxl_id, 0, 0, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change joint mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change joint mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalPosition(dxl_id, (int32_t)0); 83 | sleep(3); 84 | 85 | dxl_wb.goalPosition(dxl_id, (int32_t)1023); 86 | sleep(3); 87 | } 88 | } 89 | 90 | return 0; 91 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/port/socket.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/port/socket.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "xarm/linux/network.h" 15 | #include "xarm/linux/thread.h" 16 | 17 | void SocketPort::recv_proc(void) { 18 | int num; 19 | unsigned char recv_data[que_maxlen_]; 20 | while (state_ == 0) { 21 | bzero(recv_data, que_maxlen_); 22 | num = recv(fp_, (void *)&recv_data[4], que_maxlen_ - 1, 0); 23 | if (num <= 0) { 24 | // in case recv() blocking call is interrupted by a system signal, this should not be considered as socket failure. 25 | if(errno == EINTR) 26 | { 27 | printf("EINTR occured\n"); 28 | continue; 29 | } 30 | 31 | close_port(); 32 | printf("SocketPort::recv_proc exit, %d\n", fp_); 33 | pthread_exit(0); 34 | } 35 | bin32_to_8(num, &recv_data[0]); 36 | rx_que_->push(recv_data); 37 | } 38 | } 39 | 40 | static void *recv_proc_(void *arg) { 41 | SocketPort *my_this = (SocketPort *)arg; 42 | 43 | my_this->recv_proc(); 44 | 45 | pthread_exit(0); 46 | } 47 | 48 | SocketPort::SocketPort(char *server_ip, int server_port, int que_num, 49 | int que_maxlen) { 50 | que_num_ = que_num; 51 | que_maxlen_ = que_maxlen; 52 | state_ = -1; 53 | rx_que_ = new QueueMemcpy(que_num_, que_maxlen_); 54 | fp_ = socket_init((char *)" ", 0, 0); 55 | if (fp_ == -1) { return; } 56 | 57 | int ret = socket_connect_server(&fp_, server_ip, server_port); 58 | if (ret == -1) { return; } 59 | 60 | state_ = 0; 61 | flush(); 62 | thread_id_ = thread_init(recv_proc_, this); 63 | } 64 | 65 | SocketPort::~SocketPort(void) { 66 | state_ = -1; 67 | delete rx_que_; 68 | } 69 | 70 | int SocketPort::is_ok(void) { return state_; } 71 | 72 | void SocketPort::flush(void) { rx_que_->flush(); } 73 | 74 | int SocketPort::read_frame(unsigned char *data) { 75 | if (state_ != 0) { return -1; } 76 | 77 | if (rx_que_->size() == 0) { return -1; } 78 | rx_que_->pop(data); 79 | return 0; 80 | } 81 | 82 | int SocketPort::write_frame(unsigned char *data, int len) { 83 | int ret = socket_send_data(fp_, data, len); 84 | return ret; 85 | } 86 | 87 | void SocketPort::close_port(void) { 88 | close(fp_); 89 | state_ = -1; 90 | delete rx_que_; 91 | } 92 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.wheelMode(dxl_id, 0, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change wheel mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change wheel mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalVelocity(dxl_id, (int32_t)-100); 83 | sleep(3); 84 | 85 | dxl_wb.goalVelocity(dxl_id, (int32_t)100); 86 | sleep(3); 87 | } 88 | 89 | dxl_wb.goalVelocity(dxl_id, (int32_t)0); 90 | } 91 | 92 | return 0; 93 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.itemWrite(dxl_id, "LED", 1, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to LED On\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to LED On\n"); 78 | } 79 | 80 | int32_t get_data = 0; 81 | result = dxl_wb.itemRead(dxl_id, "Present_Position", &get_data, &log); 82 | if (result == false) 83 | { 84 | printf("%s\n", log); 85 | printf("Failed to get present position\n"); 86 | } 87 | else 88 | { 89 | printf("Succeed to get present position(value : %d)\n", get_data); 90 | } 91 | 92 | return 0; 93 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.currentBasedPositionMode(dxl_id, 30, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change current based position mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change current based position mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalPosition(dxl_id, (int32_t)0); 83 | sleep(3); 84 | 85 | dxl_wb.goalPosition(dxl_id, (int32_t)2048); 86 | sleep(3); 87 | } 88 | } 89 | 90 | return 0; 91 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/report_data.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef XARM_REPORT_DATA_H_ 8 | #define XARM_REPORT_DATA_H_ 9 | 10 | #include "xarm/common/data_type.h" 11 | 12 | 13 | class ReportDataDevelop { 14 | public: 15 | ReportDataDevelop(void); 16 | ~ReportDataDevelop(void); 17 | 18 | int flush_data(unsigned char *rx_data); 19 | void print_data(void); 20 | 21 | private: 22 | int runing_; 23 | int mode_; 24 | int cmdnum_; 25 | float angle_[7]; 26 | float pose_[6]; 27 | float tau_[7]; 28 | int total_num_; 29 | }; 30 | 31 | 32 | 33 | class ReportDataNorm { 34 | public: 35 | ReportDataNorm(void); 36 | ~ReportDataNorm(void); 37 | 38 | int flush_data(unsigned char *rx_data); 39 | void print_data(void); 40 | 41 | // private: 42 | int runing_; 43 | int mode_; 44 | int cmdnum_; 45 | float angle_[7]; 46 | float pose_[6]; 47 | float tau_[7]; 48 | 49 | int mt_brake_; 50 | int mt_able_; 51 | int err_; 52 | int war_; 53 | float tcp_offset_[6]; 54 | float tcp_load_[4]; 55 | int collis_sens_; 56 | int teach_sens_; 57 | float gravity_dir_[3]; 58 | int total_num_; 59 | }; 60 | 61 | 62 | 63 | class ReportDataRich { 64 | public: 65 | ReportDataRich(void); 66 | ~ReportDataRich(void); 67 | 68 | int flush_data(unsigned char *rx_data); 69 | void print_data(void); 70 | 71 | private: 72 | int runing_; 73 | int mode_; 74 | int cmdnum_; 75 | float angle_[7]; 76 | float pose_[6]; 77 | float tau_[7]; 78 | 79 | int mt_brake_; 80 | int mt_able_; 81 | int err_; 82 | int war_; 83 | float tcp_offset_[6]; 84 | float tcp_load_[4]; 85 | int collis_sens_; 86 | int teach_sens_; 87 | float gravity_dir_[3]; 88 | int total_num_; 89 | 90 | int arm_type_; 91 | int axis_num_; 92 | int master_id_; 93 | int slave_id_; 94 | int motor_tid_; 95 | int motor_fid_; 96 | unsigned char versions_[30]; 97 | float trs_jerk_; 98 | float trs_accmin_; 99 | float trs_accmax_; 100 | float trs_velomin_; 101 | float trs_velomax_; 102 | float p2p_jerk_; 103 | float p2p_accmin_; 104 | float p2p_accmax_; 105 | float p2p_velomin_; 106 | float p2p_velomax_; 107 | float rot_jerk_; 108 | float rot_accmax_; 109 | int sv3msg_[16]; 110 | float trs_msg_[5]; 111 | float p2p_msg_[5]; 112 | float rot_msg_[2]; 113 | }; 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dynamixel_workbench) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | include_directories( 7 | ../src 8 | ) 9 | 10 | add_library(dynamixel_workbench 11 | ../src/dynamixel_workbench_toolbox/dynamixel_item.cpp 12 | ../src/dynamixel_workbench_toolbox/dynamixel_driver.cpp 13 | ../src/dynamixel_workbench_toolbox/dynamixel_tool.cpp 14 | ../src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp 15 | ) 16 | 17 | if(APPLE) 18 | target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_mac_cpp.dylib") 19 | else() 20 | target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_x64_cpp.so") 21 | endif() 22 | 23 | add_executable(model_scan src/a_Model_Scan.cpp) 24 | target_link_libraries(model_scan LINK_PUBLIC dynamixel_workbench) 25 | 26 | add_executable(ping src/b_Ping.cpp) 27 | target_link_libraries(ping LINK_PUBLIC dynamixel_workbench) 28 | 29 | add_executable(id_change src/c_ID_Change.cpp) 30 | target_link_libraries(id_change LINK_PUBLIC dynamixel_workbench) 31 | 32 | add_executable(bps_change src/d_BPS_Change.cpp) 33 | target_link_libraries(bps_change LINK_PUBLIC dynamixel_workbench) 34 | 35 | add_executable(mode_change src/e_Mode_Change.cpp) 36 | target_link_libraries(mode_change LINK_PUBLIC dynamixel_workbench) 37 | 38 | add_executable(reboot src/f_Reboot.cpp) 39 | target_link_libraries(reboot LINK_PUBLIC dynamixel_workbench) 40 | 41 | add_executable(reset src/g_Reset.cpp) 42 | target_link_libraries(reset LINK_PUBLIC dynamixel_workbench) 43 | 44 | add_executable(position src/h_Position.cpp) 45 | target_link_libraries(position LINK_PUBLIC dynamixel_workbench) 46 | 47 | add_executable(velocity src/i_Velocity.cpp) 48 | target_link_libraries(velocity LINK_PUBLIC dynamixel_workbench) 49 | 50 | add_executable(current_based_position src/j_Current_Based_Position.cpp) 51 | target_link_libraries(current_based_position LINK_PUBLIC dynamixel_workbench) 52 | 53 | add_executable(read_write src/k_Read_Write.cpp) 54 | target_link_libraries(read_write LINK_PUBLIC dynamixel_workbench) 55 | 56 | add_executable(sync_write src/l_Sync_Write.cpp) 57 | target_link_libraries(sync_write LINK_PUBLIC dynamixel_workbench) 58 | 59 | add_executable(sync_read_write src/m_Sync_Read_Write.cpp) 60 | target_link_libraries(sync_read_write LINK_PUBLIC dynamixel_workbench) 61 | 62 | add_executable(bulk_read_write src/n_Bulk_Read_Write.cpp) 63 | target_link_libraries(bulk_read_write LINK_PUBLIC dynamixel_workbench) 64 | 65 | add_executable(find_dynamixel src/o_Find_Dynamixel.cpp) 66 | target_link_libraries(find_dynamixel LINK_PUBLIC dynamixel_workbench) 67 | 68 | add_executable(monitor src/p_Monitor.cpp) 69 | target_link_libraries(monitor LINK_PUBLIC dynamixel_workbench) -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_tool.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #ifndef DYNAMIXEL_TOOL_H 20 | #define DYNAMIXEL_TOOL_H 21 | 22 | #include 23 | #include 24 | 25 | #include "dynamixel_item.h" 26 | 27 | class DynamixelTool 28 | { 29 | private: 30 | enum {DYNAMIXEL_BUFFER = 36}; 31 | uint8_t dxl_id_[DYNAMIXEL_BUFFER]; 32 | uint8_t dxl_cnt_; 33 | 34 | const char *model_name_; 35 | uint16_t model_number_; 36 | 37 | const ControlItem *control_table_; 38 | const ModelInfo *model_info_; 39 | 40 | uint16_t the_number_of_control_item_; 41 | 42 | public: 43 | DynamixelTool(); 44 | ~DynamixelTool(); 45 | 46 | void initTool(void); 47 | 48 | bool addTool(const char *model_name, uint8_t id, const char **log = NULL); 49 | bool addTool(uint16_t model_number, uint8_t id, const char **log = NULL); 50 | 51 | void addDXL(uint8_t id); 52 | 53 | const char *getModelName(void); 54 | uint16_t getModelNumber(void); 55 | 56 | const uint8_t* getID(void); 57 | uint8_t getDynamixelBuffer(void); 58 | uint8_t getDynamixelCount(void); 59 | 60 | float getRPM(void); 61 | 62 | int64_t getValueOfMinRadianPosition(void); 63 | int64_t getValueOfMaxRadianPosition(void); 64 | int64_t getValueOfZeroRadianPosition(void); 65 | 66 | float getMinRadian(void); 67 | float getMaxRadian(void); 68 | 69 | uint8_t getTheNumberOfControlItem(void); 70 | 71 | const ControlItem *getControlItem(const char *item_name, const char **log = NULL); 72 | const ControlItem *getControlTable(void); 73 | const ModelInfo *getModelInfo(void); 74 | 75 | private: 76 | bool setControlTable(const char *model_name, const char **log = NULL); 77 | bool setControlTable(uint16_t model_number, const char **log = NULL); 78 | 79 | bool setModelName(uint16_t model_number, const char **log = NULL); 80 | bool setModelNumber(const char *model_name, const char **log = NULL); 81 | }; 82 | #endif //DYNAMIXEL_TOOL_H 83 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/msg/LocobotJoy.msg: -------------------------------------------------------------------------------- 1 | # This message is used specifically in the interbotix_xslocobot_joy package 2 | # 3 | # Maps raw 'joy' commands to more specific ones to control an Interbotix LoCoBot 4 | 5 | # enum values that define the joystick controls for the robot 6 | 7 | ######################################################################################################### 8 | 9 | # Reset base odometry 10 | int8 RESET_ODOM = 1 11 | 12 | ######################################################################################################### 13 | 14 | # Control the pan-and-tilt mechanism 15 | int8 PAN_CCW = 2 16 | int8 PAN_CW = 3 17 | int8 TILT_UP = 4 18 | int8 TILT_DOWN = 5 19 | int8 PAN_TILT_HOME = 6 20 | 21 | ######################################################################################################### 22 | 23 | # Control the motion of the virtual 'ee_gripper_link' or end effector using the modern_robotics_ik engine 24 | # Position Control 25 | int8 EE_X_INC = 7 26 | int8 EE_X_DEC = 8 27 | int8 EE_Y_INC = 9 28 | int8 EE_Y_DEC = 10 29 | int8 EE_Z_INC = 11 30 | int8 EE_Z_DEC = 12 31 | 32 | # Orientation Control 33 | int8 EE_ROLL_CCW = 13 34 | int8 EE_ROLL_CW = 14 35 | int8 EE_PITCH_UP = 15 36 | int8 EE_PITCH_DOWN = 16 37 | 38 | ######################################################################################################### 39 | 40 | # Control the motion of independent joints on the Arm or send predefined robot poses 41 | # Waist Joint Control 42 | int8 WAIST_CCW = 17 43 | int8 WAIST_CW = 18 44 | 45 | # Gripper Control 46 | int8 GRIPPER_OPEN = 19 47 | int8 GRIPPER_CLOSE = 20 48 | 49 | # Pose Control 50 | int8 HOME_POSE = 21 51 | int8 SLEEP_POSE = 22 52 | 53 | ######################################################################################################### 54 | 55 | # Customize configurations for the Interbotix Arm 56 | # Inc/Dec Joint speed 57 | int8 SPEED_INC = 23 58 | int8 SPEED_DEC = 24 59 | 60 | # Quickly toggle between a fast and slow speed setting 61 | int8 SPEED_COURSE = 25 62 | int8 SPEED_FINE = 26 63 | 64 | # Inc/Dec Gripper pressure 65 | int8 GRIPPER_PWM_INC = 27 66 | int8 GRIPPER_PWM_DEC = 28 67 | 68 | ######################################################################################################### 69 | 70 | # Control the motion of the Kobuki base 71 | float64 base_x_cmd 72 | float64 base_theta_cmd 73 | int8 base_reset_odom_cmd 74 | 75 | # Control the motion of the camera pan-and-tilt mechanism 76 | int8 pan_cmd 77 | int8 tilt_cmd 78 | 79 | # Control the motion of the Interbotix Arm 80 | int8 ee_x_cmd 81 | int8 ee_y_cmd 82 | int8 ee_z_cmd 83 | int8 ee_roll_cmd 84 | int8 ee_pitch_cmd 85 | 86 | # Independent Joint/Pose Control 87 | int8 waist_cmd 88 | int8 gripper_cmd 89 | int8 pose_cmd 90 | 91 | # Arm Configs 92 | int8 speed_cmd 93 | int8 speed_toggle_cmd 94 | int8 gripper_pwm_cmd 95 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm_ros_client.h: -------------------------------------------------------------------------------- 1 | #ifndef __XARM_ROS_CLIENT_H__ 2 | #define __XARM_ROS_CLIENT_H__ 3 | 4 | #include "ros/ros.h" 5 | #include 6 | 7 | namespace xarm_api{ 8 | 9 | class XArmROSClient 10 | { 11 | public: 12 | XArmROSClient(){}; 13 | void init(ros::NodeHandle& nh); 14 | ~XArmROSClient(){}; 15 | 16 | int motionEnable(short en); 17 | int setState(short state); 18 | int setMode(short mode); 19 | int clearErr(void); 20 | int getErr(void); 21 | int setTCPOffset(const std::vector& tcp_offset); 22 | int setLoad(float mass, const std::vector& center_of_mass); 23 | int setServoJ(const std::vector& joint_cmd); 24 | int setServoCartisian(const std::vector& cart_cmd); 25 | int goHome(float jnt_vel_rad, float jnt_acc_rad=15); 26 | int moveJoint(const std::vector& joint_cmd, float jnt_vel_rad, float jnt_acc_rad=15); 27 | int moveLine(const std::vector& cart_cmd, float cart_vel_mm, float cart_acc_mm=500); 28 | int moveLineB(int num_of_pnts, const std::vector cart_cmds[], float cart_vel_mm, float cart_acc_mm=500, float radii=0); 29 | int getGripperState(float *curr_pulse, int *curr_err); 30 | int gripperConfig(float pulse_vel); 31 | int gripperMove(float pulse); 32 | 33 | int config_tool_modbus(int baud_rate, int time_out_ms); 34 | int send_tool_modbus(unsigned char* data, int send_len, unsigned char* recv_data=NULL, int recv_len=0); 35 | 36 | private: 37 | ros::ServiceClient motion_ctrl_client_; 38 | ros::ServiceClient set_mode_client_; 39 | ros::ServiceClient set_state_client_; 40 | ros::ServiceClient go_home_client_; 41 | ros::ServiceClient move_lineb_client_; 42 | ros::ServiceClient move_servoj_client_; 43 | ros::ServiceClient move_servo_cart_client_; 44 | ros::ServiceClient move_line_client_; 45 | ros::ServiceClient move_joint_client_; 46 | ros::ServiceClient set_tcp_offset_client_; 47 | ros::ServiceClient set_load_client_; 48 | ros::ServiceClient clear_err_client_; 49 | ros::ServiceClient get_err_client_; 50 | ros::ServiceClient config_modbus_client_; 51 | ros::ServiceClient send_modbus_client_; 52 | ros::ServiceClient gripper_move_client_; 53 | ros::ServiceClient gripper_config_client_; 54 | ros::ServiceClient gripper_state_client_; 55 | 56 | xarm_msgs::SetAxis set_axis_srv_; 57 | xarm_msgs::SetInt16 set_int16_srv_; 58 | xarm_msgs::TCPOffset offset_srv_; 59 | xarm_msgs::SetLoad set_load_srv_; 60 | xarm_msgs::ClearErr clear_err_srv_; 61 | xarm_msgs::GetErr get_err_srv_; 62 | xarm_msgs::Move move_srv_; 63 | xarm_msgs::Move servoj_msg_; 64 | xarm_msgs::Move servo_cart_msg_; 65 | xarm_msgs::ConfigToolModbus cfg_modbus_msg_; 66 | xarm_msgs::SetToolModbus set_modbus_msg_; 67 | xarm_msgs::GripperConfig gripper_config_msg_; 68 | xarm_msgs::GripperMove gripper_move_msg_; 69 | xarm_msgs::GripperState gripper_state_msg_; 70 | 71 | ros::NodeHandle nh_; 72 | }; 73 | 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2016 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | int new_dxl_id = 2; 27 | 28 | if (argc < 5) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id', '-new_dynamixel_id' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | new_dxl_id = atoi(argv[4]); 39 | } 40 | 41 | DynamixelWorkbench dxl_wb; 42 | 43 | const char *log; 44 | bool result = false; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | uint16_t model_number = 0; 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | 64 | return 0; 65 | } 66 | else 67 | { 68 | printf("Succeeded to ping\n"); 69 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 70 | } 71 | 72 | result = dxl_wb.changeID(dxl_id, new_dxl_id, &log); 73 | if (result == false) 74 | { 75 | printf("%s\n", log); 76 | return 0; 77 | } 78 | else 79 | { 80 | printf("%s\n", log); 81 | } 82 | 83 | uint8_t scanned_id[16]; 84 | uint8_t dxl_cnt = 0; 85 | uint8_t range = 100; 86 | 87 | printf("Wait for scan...\n"); 88 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 89 | if (result == false) 90 | { 91 | printf("%s\n", log); 92 | printf("Failed to scan\n"); 93 | } 94 | else 95 | { 96 | printf("Find %d Dynamixels\n", dxl_cnt); 97 | 98 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 99 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 100 | } 101 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/instruction/servo3_config.h: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Author: Jimy Zhang 7 | ============================================================================*/ 8 | #ifndef CORE_INSTRUCTION_SERVO3_CONFIG_H_ 9 | #define CORE_INSTRUCTION_SERVO3_CONFIG_H_ 10 | 11 | class SERVO3_RG { 12 | public: 13 | static const unsigned short CON_EN = 0x0100; 14 | static const unsigned short CON_MODE = 0x0101; 15 | static const unsigned short CON_DIR = 0x0102; 16 | static const unsigned short SV3MOD_POS = 0; 17 | static const unsigned short SV3MOD_SPD = 1; 18 | static const unsigned short SV3MOD_FOS = 2; 19 | static const unsigned short SV3_SAVE = 0x1000; 20 | static const unsigned short BRAKE = 0x0104; 21 | static const unsigned short GET_TEMP = 0x000E; 22 | static const unsigned short ERR_CODE = 0x000F; 23 | static const unsigned short OVER_TEMP = 0x0108; 24 | static const unsigned short CURR_CURR = 0x0001; 25 | static const unsigned short POS_KP = 0x0200; 26 | static const unsigned short POS_FWDKP = 0x0201; 27 | static const unsigned short POS_PWDTC = 0x0202; 28 | static const unsigned short SPD_KP = 0x0203; 29 | static const unsigned short SPD_KI = 0x0204; 30 | static const unsigned short CURR_KP = 0x090C; 31 | static const unsigned short CURR_KI = 0x090D; 32 | static const unsigned short SPD_IFILT = 0x030C; 33 | static const unsigned short SPD_OFILT = 0x030D; 34 | static const unsigned short POS_CMDILT = 0x030E; 35 | static const unsigned short CURR_IFILT = 0x0401; 36 | static const unsigned short POS_KD = 0x0205; 37 | static const unsigned short POS_ACCT = 0x0300; 38 | static const unsigned short POS_DECT = 0x0301; 39 | static const unsigned short POS_STHT = 0x0302; 40 | static const unsigned short POS_SPD = 0x0303; 41 | static const unsigned short MT_ID = 0x1600; 42 | static const unsigned short BAUDRATE = 0x0601; 43 | static const unsigned short SOFT_REBOOT = 0x0607; 44 | static const unsigned short TAGET_TOQ = 0x050a; 45 | static const unsigned short CURR_TOQ = 0x050c; 46 | static const unsigned short TOQ_SPD = 0x050e; 47 | static const unsigned short TAGET_POS = 0x0700; 48 | static const unsigned short CURR_POS = 0x0702; 49 | static const unsigned short HARD_VER = 0x0800; 50 | static const unsigned short SOFT_VER = 0x0801; 51 | static const unsigned short MT_TYPE = 0x0802; 52 | static const unsigned short MT_ZERO = 0x0817; 53 | static const unsigned short RESET_PVL = 0x0813; 54 | static const unsigned short CAL_ZERO = 0x080C; 55 | static const unsigned short ERR_SWITCH = 0x0910; 56 | static const unsigned short RESET_ERR = 0x0109; 57 | static const unsigned short SV3_BRO_ID = 0xFF; 58 | 59 | static const unsigned short MODBUS_BAUDRATE = 0x0A0B; 60 | static const unsigned short DIGITAL_IN = 0x0A14; 61 | static const unsigned short DIGITAL_OUT = 0x0A15; 62 | static const unsigned short ANALOG_IO1 = 0x0A16; 63 | static const unsigned short ANALOG_IO2 = 0x0A17; 64 | }; 65 | 66 | #endif -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(dynamixel_workbench_toolbox) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | dynamixel_sdk 16 | ) 17 | 18 | ################################################################################ 19 | # Setup for python modules and scripts 20 | ################################################################################ 21 | 22 | ################################################################################ 23 | # Declare ROS messages, services and actions 24 | ################################################################################ 25 | 26 | ################################################################################ 27 | # Declare ROS dynamic reconfigure parameters 28 | ################################################################################ 29 | 30 | ################################################################################ 31 | # Declare catkin specific configuration to be passed to dependent projects 32 | ################################################################################ 33 | catkin_package( 34 | INCLUDE_DIRS include 35 | LIBRARIES dynamixel_workbench_toolbox 36 | CATKIN_DEPENDS roscpp dynamixel_sdk 37 | ) 38 | 39 | ################################################################################ 40 | # Build 41 | ################################################################################ 42 | include_directories( 43 | include 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | add_library(dynamixel_workbench_toolbox 48 | src/${PROJECT_NAME}/dynamixel_item.cpp 49 | src/${PROJECT_NAME}/dynamixel_driver.cpp 50 | src/${PROJECT_NAME}/dynamixel_tool.cpp 51 | src/${PROJECT_NAME}/dynamixel_workbench.cpp 52 | ) 53 | add_dependencies(dynamixel_workbench_toolbox ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(dynamixel_workbench_toolbox ${catkin_LIBRARIES}) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS dynamixel_workbench_toolbox 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | ################################################################################ 70 | # Test 71 | ################################################################################ 72 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Interbotix X-Series Messages ROS Package 2 | 3 | ## Overview 4 | This package builds the common messages, services, and actions used by the Interbotix X-Series platforms. 5 | 6 | # Structure 7 | The *interbotix_xs_msgs* package contains no nodes, rather it defines a common location to store message, service, and action definitions used by X-Series platforms. 8 | 9 | # Usage 10 | Simply build this package and include any generated message in your own custom node or script. For example, if you wanted to control an X-Series manipulator using a joystick, you would include the ArmJoy message in your program's header. 11 | - C++ 12 | ```C++ 13 | #include "interbotix_xs_msgs/ArmJoy.h" 14 | ``` 15 | - Python 16 | ```python 17 | from interbotix_xs_msgs.msg import ArmJoy 18 | ``` 19 | 20 | The table below provides a list and use case for each type defined in this package. 21 | 22 | | Type | Description | Use Case | 23 | | :---------------------------: | :--------------------------------------------------------------------------------- | :----------------------- | 24 | | JointSingleCommand.msg | Command a desired joint | Common | 25 | | JointGroupCommand.msg | Command the joints in the specified joint group. | Common | 26 | | JointTrajectoryCommand.msg | Command a joint trajectory to the desired joint(s). | Common | 27 | | JointTemps.msg | Holds the temperatures [C] for the specified joints | Common | 28 | | ArmJoy.msg | Maps raw 'joy' commands to more specific ones to control an Interbotix manipulator | Joystick Arm Control | 29 | | LocobotJoy.msg | Maps raw 'joy' commands to more specific ones to control an Interbotix LoCoBot | Joystick LoCoBot Control | 30 | | HexJoy.msg | Maps raw 'joy' commands to more specific ones to control an Interbotix hexapod | Joystick Hexapod Control | 31 | | TurretJoy.msg | Maps raw 'joy' commands to more specific ones to control an Interbotix turret | Joystick Turret Control | 32 | | MotorGains.srv | Set PID gains | Common | 33 | | OperatingModes.srv | Set Operating Modes | Common | 34 | | Reboot.srv | Reboot motors | Common | 35 | | RegisterValues.srv | Set or get the register(s) value(s) from motor(s) | Common | 36 | | RobotInfo.srv | Get robot information | Common | 37 | | TorqueEnable.srv | Torque joints on/off | Common | 38 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(interbotix_xs_sdk) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | interbotix_xs_msgs 12 | actionlib 13 | control_msgs 14 | dynamixel_workbench_toolbox 15 | message_generation 16 | roscpp 17 | sensor_msgs 18 | urdf 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | # Resolve system dependency on yaml-cpp, which apparently does not 25 | # provide a CMake find_package() module. 26 | find_package(PkgConfig REQUIRED) 27 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 28 | find_path(YAML_CPP_INCLUDE_DIR 29 | NAMES yaml_cpp.h 30 | PATHS ${YAML_CPP_INCLUDE_DIRS} 31 | ) 32 | find_library(YAML_CPP_LIBRARY 33 | NAMES YAML_CPP 34 | PATHS ${YAML_CPP_LIBRARY_DIRS} 35 | ) 36 | 37 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 38 | add_definitions(-DHAVE_NEW_YAMLCPP) 39 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 40 | 41 | ################################### 42 | ## catkin specific configuration ## 43 | ################################### 44 | ## The catkin_package macro generates cmake config files for your package 45 | ## Declare things to be passed to dependent projects 46 | 47 | catkin_package( 48 | INCLUDE_DIRS include 49 | LIBRARIES ${PROJECT_NAME} 50 | CATKIN_DEPENDS interbotix_xs_msgs actionlib dynamixel_workbench_toolbox message_runtime roscpp sensor_msgs urdf 51 | ) 52 | 53 | ########### 54 | ## Build ## 55 | ########### 56 | 57 | ## Specify additional locations of header files 58 | ## Your package locations should be listed before other locations 59 | include_directories( 60 | include 61 | ${catkin_INCLUDE_DIRS} 62 | ${YAML_CPP_INCLUDE_DIRS} 63 | ) 64 | 65 | ## Declare a C++ library 66 | add_library(${PROJECT_NAME} 67 | src/xs_sdk_obj.cpp 68 | ) 69 | target_link_libraries(${PROJECT_NAME} PUBLIC ${YAML_CPP_LIBRARIES}) 70 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 71 | 72 | ## Declare a C++ executable 73 | ## Specify libraries to link a library or executable target against 74 | ## Add cmake target dependencies of the executable 75 | add_executable(xs_sdk src/xs_sdk.cpp src/xs_sdk_obj.cpp) 76 | add_dependencies(xs_sdk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 77 | target_link_libraries(xs_sdk ${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 78 | 79 | ############# 80 | ## Install ## 81 | ############# 82 | 83 | ## Mark executable scripts (Python etc.) for installation 84 | ## in contrast to setup.py, you can choose the destination 85 | catkin_install_python(PROGRAMS 86 | scripts/xs_sdk_sim 87 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 88 | ) 89 | 90 | ## Mark other files for installation (e.g. launch and bag files, etc.) 91 | install( 92 | DIRECTORY 93 | config 94 | DESTINATION 95 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 96 | ) 97 | 98 | install(TARGETS xs_sdk 99 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 100 | ) 101 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_tcp.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/instruction/uxbus_cmd_tcp.h" 8 | 9 | #include 10 | 11 | #include "xarm/debug/debug_print.h" 12 | #include "xarm/instruction/uxbus_cmd_config.h" 13 | 14 | UxbusCmdTcp::UxbusCmdTcp(SocketPort *arm_port) { 15 | arm_port_ = arm_port; 16 | bus_flag_ = TX2_BUS_FLAG_MIN_; 17 | prot_flag_ = TX2_PROT_CON_; 18 | } 19 | 20 | UxbusCmdTcp::~UxbusCmdTcp(void) {} 21 | 22 | int UxbusCmdTcp::check_xbus_prot(unsigned char *datas, int funcode) { 23 | unsigned char *data_fp = &datas[4]; 24 | 25 | int sizeof_data = bin8_to_32(datas); 26 | if (sizeof_data < 8 || sizeof_data >= arm_port_->que_maxlen_) 27 | { return UXBUS_STATE::ERR_LENG; } 28 | 29 | int num = bin8_to_16(&data_fp[0]); 30 | int prot = bin8_to_16(&data_fp[2]); 31 | int len = bin8_to_16(&data_fp[4]); 32 | int fun = data_fp[6]; 33 | int state = data_fp[7]; 34 | 35 | int bus_flag = bus_flag_; 36 | if (bus_flag == TX2_BUS_FLAG_MIN_) 37 | { bus_flag = TX2_BUS_FLAG_MAX_; } 38 | else 39 | { bus_flag -= 1; } 40 | 41 | if (num != bus_flag) { /*fprintf(stderr, "expect flag: %d, recv: %d, tar_fun: %d\n", bus_flag, num, funcode);*/ return UXBUS_STATE::ERR_NUM; } 42 | if (prot != TX2_PROT_CON_) { return UXBUS_STATE::ERR_PROT; } 43 | if (fun != funcode) { /*fprintf(stderr, "expect funcode: %d, recv: %d, tar_flag: %d\n", funcode, fun, bus_flag);*/ return UXBUS_STATE::ERR_FUN; } 44 | if (state & 0x40) { return UXBUS_STATE::ERR_CODE; } 45 | if (state & 0x20) { return UXBUS_STATE::WAR_CODE; } 46 | if (sizeof_data != len + 6) { return UXBUS_STATE::ERR_LENG; } 47 | return 0; 48 | } 49 | 50 | int UxbusCmdTcp::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) { 51 | int i; 52 | int ret; 53 | unsigned char rx_data[arm_port_->que_maxlen_] = {0}; 54 | int times = timeout; 55 | while (times) { 56 | times -= 1; 57 | ret = arm_port_->read_frame(rx_data); 58 | if (ret != -1) { 59 | ret = check_xbus_prot(rx_data, funcode); 60 | 61 | int n = num; 62 | if (num == -1) { 63 | n = rx_data[9] - 2; 64 | } 65 | for (i = 0; i < n; i++) { ret_data[i] = rx_data[i + 8 + 4]; } 66 | // print_hex(" 3", rx_data, num + 8 + 4); 67 | return ret; 68 | } 69 | usleep(1000); 70 | } 71 | return UXBUS_STATE::ERR_TOUT; 72 | } 73 | 74 | int UxbusCmdTcp::send_xbus(int funcode, unsigned char *datas, int num) { 75 | int len = num + 7; 76 | unsigned char send_data[len]; 77 | bin16_to_8(bus_flag_, &send_data[0]); 78 | bin16_to_8(prot_flag_, &send_data[2]); 79 | bin16_to_8(num + 1, &send_data[4]); 80 | send_data[6] = funcode; 81 | 82 | for (int i = 0; i < num; i++) { send_data[7 + i] = datas[i]; } 83 | arm_port_->flush(); 84 | // print_hex("send:", send_data, num + 7); 85 | int ret = arm_port_->write_frame(send_data, len); 86 | if (ret != len) { return -1; } 87 | 88 | bus_flag_ += 1; 89 | if (bus_flag_ > TX2_BUS_FLAG_MAX_) { bus_flag_ = TX2_BUS_FLAG_MIN_; } 90 | 91 | return 0; 92 | } 93 | 94 | void UxbusCmdTcp::close(void) { arm_port_->close_port(); } -------------------------------------------------------------------------------- /interbotix_ros_xseries/interbotix_xs_msgs/srv/OperatingModes.srv: -------------------------------------------------------------------------------- 1 | # Set Operating Modes 2 | # 3 | # To get familiar with the various operating modes, go to... 4 | # http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ 5 | # ...click on a motor model, and scroll down to the 'Operating Mode' section. 6 | # 7 | # There are 6 valid operating modes. They are... 8 | # "position" - allows up to 1 complete joint revolution (perfect for arm joints); units are in radians 9 | # "ext_position" - allows up to 512 joint revolutions; units are in radians 10 | # "velocity" - allows infinite number of rotations (perfect for wheeled robots); units are in rad/s 11 | # "current" - allows infinite number of rotations (perfect for grippers); units are in milliamps 12 | # "current_based_position" - allows up to 512 joint revolutions; units are in radians 13 | # "pwm" - allows infinite number of rotations (perfect for grippers); units are in PWM 14 | # 15 | # Note that the interbotix_xs_sdk offers one other 'pseudo' operating mode that can be useful in controlling Interbotix Grippers - called "linear_position". 16 | # Behind the scenes, it uses the "position" operating mode mentioned above. The main difference is that with this mode, a desired linear distance [m] 17 | # between the two gripper fingers can be commanded. In the "position" mode though, only the angular position of the motor can be commanded. 18 | # 19 | # There are 2 valid profile types - either 'time' or 'velocity'. Depending on which is chosen, the following parameters behave differently. 20 | # 21 | # 1) profile_velocity: acts as a pass-through to the Profile_Velocity register and operates in one of two ways. If 22 | # 'profile_type' is set to 'velocity', this parameter describes the max velocity limit for the specified joint(s); 23 | # for example, if doing 'position' control, setting this to '131' would be equivalent to a limit of 3.14 rad/s; if 24 | # 'profile_type' is set to 'time', this parameter sets the time span (in milliseconds) that it should take for the 25 | # specified joint(s) to move; to have an 'infinite' max limit, set to '0'. 26 | # 27 | # 2) profile_acceleration: acts as a pass-through to the Profile_Acceleration register and operates in one of two ways. If 28 | # 'profile_type' is set to 'velocity', this parameter describes the max acceleration limit for the specified joint(s); 29 | # for example, if doing 'position' or 'velocity' control, setting this to '15' would be equivalent to a limit of 5.6 rad/s^2; 30 | # if 'profile_type' is set to 'time', this parameter sets the time span (in milliseconds) that it should take for the 31 | # specified joint(s) to accelerate; to have an 'infinite' max limit, set to '0'. 32 | 33 | string cmd_type # set to 'group' if commanding a joint group or 'single' if commanding a single joint 34 | string name # name of the group if commanding a joint group or joint if commanding a single joint 35 | string mode # desired operating mode as described above 36 | string profile_type # desired 'profile' type - either 'time' or 'velocity' as described above 37 | int32 profile_velocity # desired velocity profile as explained above - only used in 'position' or the 'ext_position' control modes 38 | int32 profile_acceleration # desired acceleration profile as explained above - used in all modes except for 'current' and 'pwm' control 39 | --- 40 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | void swap(int32_t *array); 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | const char* port_name = "/dev/ttyUSB0"; 26 | int baud_rate = 57600; 27 | 28 | uint16_t model_number = 0; 29 | uint8_t dxl_id[2] = {0, 0}; 30 | 31 | if (argc < 5) 32 | { 33 | printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); 34 | return 0; 35 | } 36 | else 37 | { 38 | port_name = argv[1]; 39 | baud_rate = atoi(argv[2]); 40 | dxl_id[0] = atoi(argv[3]); 41 | dxl_id[1] = atoi(argv[4]); 42 | } 43 | 44 | DynamixelWorkbench dxl_wb; 45 | 46 | const char *log; 47 | bool result = false; 48 | 49 | result = dxl_wb.init(port_name, baud_rate, &log); 50 | if (result == false) 51 | { 52 | printf("%s\n", log); 53 | printf("Failed to init\n"); 54 | 55 | return 0; 56 | } 57 | else 58 | printf("Succeed to init(%d)\n", baud_rate); 59 | 60 | for (int cnt = 0; cnt < 2; cnt++) 61 | { 62 | result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); 63 | if (result == false) 64 | { 65 | printf("%s\n", log); 66 | printf("Failed to ping\n"); 67 | } 68 | else 69 | { 70 | printf("Succeeded to ping\n"); 71 | printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); 72 | } 73 | 74 | result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); 75 | if (result == false) 76 | { 77 | printf("%s\n", log); 78 | printf("Failed to change joint mode\n"); 79 | } 80 | else 81 | { 82 | printf("Succeed to change joint mode\n"); 83 | } 84 | } 85 | 86 | result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log); 87 | if (result == false) 88 | { 89 | printf("%s\n", log); 90 | printf("Failed to add sync write handler\n"); 91 | } 92 | 93 | int32_t goal_position[2] = {0, 1023}; 94 | 95 | const uint8_t handler_index = 0; 96 | 97 | while(1) 98 | { 99 | result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); 100 | if (result == false) 101 | { 102 | printf("%s\n", log); 103 | printf("Failed to sync write position\n"); 104 | } 105 | 106 | sleep(3); 107 | 108 | swap(goal_position); 109 | } 110 | 111 | return 0; 112 | } 113 | 114 | void swap(int32_t *array) 115 | { 116 | int32_t tmp = array[0]; 117 | array[0] = array[1]; 118 | array[1] = tmp; 119 | } -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/include/xarm/common/data_type.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #ifndef COMMON_DATA_TYPE_H_ 8 | #define COMMON_DATA_TYPE_H_ 9 | 10 | #include 11 | 12 | inline void bin64_to_8(long long a, unsigned char* b) { 13 | b[0] = (unsigned char)(a >> 56); 14 | b[1] = (unsigned char)(a >> 48); 15 | b[2] = (unsigned char)(a >> 40); 16 | b[3] = (unsigned char)(a >> 32); 17 | b[4] = (unsigned char)(a >> 24); 18 | b[5] = (unsigned char)(a >> 16); 19 | b[6] = (unsigned char)(a >> 8); 20 | b[7] = (unsigned char)a; 21 | } 22 | 23 | inline void bin32_to_8(int a, unsigned char *b) { 24 | b[0] = (unsigned char)(a >> 24); 25 | b[1] = (unsigned char)(a >> 16); 26 | b[2] = (unsigned char)(a >> 8); 27 | b[3] = (unsigned char)a; 28 | } 29 | 30 | inline void bin16_to_8(int a, unsigned char *b) { 31 | unsigned short temp = a; 32 | b[0] = (unsigned char)(temp >> 8); 33 | b[1] = (unsigned char)temp; 34 | } 35 | 36 | inline int bin8_to_32(unsigned char *a) { 37 | return ((a[0] << 24) + (a[1] << 16) + (a[2] << 8) + a[3]); 38 | } 39 | 40 | inline int bin8_to_16(unsigned char *a) { 41 | signed int tmp = (a[0] << 8) + a[1]; 42 | return tmp; 43 | } 44 | 45 | inline int bin8_to_s16(unsigned char *a) { 46 | return (int)(short)(a[0] << 8) + a[1]; 47 | } 48 | 49 | inline void bin8_to_ns16(unsigned char *a, int *data, int n) { 50 | for (int i = 0; i < n; ++i) { 51 | data[i] = bin8_to_s16(&a[i * 2]); 52 | } 53 | } 54 | 55 | inline void fp32_to_hex(double dataf, unsigned char datahex[4]) { 56 | union _fp32hex { 57 | float dataf; 58 | unsigned char datahex[4]; 59 | } fp32hex; 60 | fp32hex.dataf = (float)dataf; 61 | datahex[0] = fp32hex.datahex[0]; 62 | datahex[1] = fp32hex.datahex[1]; 63 | datahex[2] = fp32hex.datahex[2]; 64 | datahex[3] = fp32hex.datahex[3]; 65 | } 66 | 67 | inline float hex_to_fp32(unsigned char datahex[4]) { 68 | union _fp32hex { 69 | float dataf; 70 | unsigned char datahex[4]; 71 | } fp32hex; 72 | fp32hex.datahex[0] = datahex[0]; 73 | fp32hex.datahex[1] = datahex[1]; 74 | fp32hex.datahex[2] = datahex[2]; 75 | fp32hex.datahex[3] = datahex[3]; 76 | return (float)fp32hex.dataf; 77 | } 78 | 79 | inline void int32_to_hex(int data, unsigned char datahex[4]) { 80 | union _int32hex { 81 | int data; 82 | unsigned char datahex[4]; 83 | } int32hex; 84 | int32hex.data = data; 85 | datahex[0] = int32hex.datahex[0]; 86 | datahex[1] = int32hex.datahex[1]; 87 | datahex[2] = int32hex.datahex[2]; 88 | datahex[3] = int32hex.datahex[3]; 89 | } 90 | 91 | inline void hex_to_nfp32(unsigned char *datahex, float *dataf, int n) { 92 | for (int i = 0; i < n; ++i) 93 | { 94 | dataf[i] = hex_to_fp32(&datahex[i * 4]); 95 | } 96 | } 97 | 98 | inline void nfp32_to_hex(float *dataf, unsigned char *datahex, int n) { 99 | for (int i = 0; i < n; ++i) 100 | { 101 | fp32_to_hex(dataf[i], &datahex[i * 4]); 102 | } 103 | } 104 | 105 | inline void nint32_to_hex(int *data, unsigned char *datahex, int n) { 106 | for (int i = 0; i < n; ++i) 107 | { 108 | int32_to_hex(data[i], &datahex[i * 4]); 109 | } 110 | } 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/linux/network.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/linux/network.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define DB_FLG "[net work] " 19 | #define PRINT_ERR printf 20 | 21 | #define PERRNO(ret, db_flg, str) \ 22 | { \ 23 | if (-1 == ret) { \ 24 | PRINT_ERR("%s%s\n", db_flg, str); \ 25 | return -1; \ 26 | } \ 27 | \ 28 | } 29 | 30 | int socket_init(char *local_ip, int port, int is_server) { 31 | int sockfd = socket(AF_INET, SOCK_STREAM, 0); 32 | PERRNO(sockfd, DB_FLG, "error: socket"); 33 | 34 | int on = 1; 35 | int keepAlive = 1; // Turn on keepalive attribute 36 | int keepIdle = 1; // If there is no data in n seconds, probe 37 | int keepInterval = 1; // Detection interval,5 seconds 38 | int keepCount = 3; // 3 detection attempts 39 | struct timeval timeout = {2, 0}; 40 | 41 | int ret = 42 | setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void *)&on, sizeof(on)); 43 | PERRNO(ret, DB_FLG, "error: setsockopt"); 44 | ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, (void *)&keepAlive, 45 | sizeof(keepAlive)); 46 | PERRNO(ret, DB_FLG, "error: setsockopt"); 47 | ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPIDLE, (void *)&keepIdle, 48 | sizeof(keepIdle)); 49 | PERRNO(ret, DB_FLG, "error: setsockopt"); 50 | ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPINTVL, (void *)&keepInterval, 51 | sizeof(keepInterval)); 52 | PERRNO(ret, DB_FLG, "error: setsockopt"); 53 | ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPCNT, (void *)&keepCount, 54 | sizeof(keepCount)); 55 | PERRNO(ret, DB_FLG, "error: setsockopt"); 56 | ret = setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, 57 | sizeof(struct timeval)); 58 | PERRNO(ret, DB_FLG, "error: setsockopt"); 59 | 60 | if (is_server) { 61 | struct sockaddr_in local_addr; 62 | local_addr.sin_family = AF_INET; 63 | local_addr.sin_port = htons(port); 64 | local_addr.sin_addr.s_addr = inet_addr(local_ip); 65 | ret = bind(sockfd, (struct sockaddr *)&local_addr, sizeof(local_addr)); 66 | PERRNO(ret, DB_FLG, "error: bind"); 67 | 68 | int ret = listen(sockfd, 10); 69 | PERRNO(ret, DB_FLG, "error: listen"); 70 | } 71 | return sockfd; 72 | } 73 | 74 | int socket_connect_server(int *socket, char server_ip[], int server_port) { 75 | struct sockaddr_in server_addr; 76 | server_addr.sin_family = AF_INET; 77 | server_addr.sin_port = htons(server_port); 78 | inet_aton(server_ip, &server_addr.sin_addr); 79 | int ret = 80 | connect(*socket, (struct sockaddr *)&server_addr, sizeof(server_addr)); 81 | PERRNO(ret, DB_FLG, "error: connect"); 82 | return 0; 83 | } 84 | 85 | int socket_send_data(int client_fp, unsigned char *data, int len) { 86 | int ret = send(client_fp, (void *)data, len, 0); 87 | if (ret == -1) { PRINT_ERR(DB_FLG "error: socket_send_data\n"); } 88 | return ret; 89 | } 90 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby), Ryan Shim */ 18 | 19 | #ifndef DYNAMIXEL_ITEM_H 20 | #define DYNAMIXEL_ITEM_H 21 | 22 | #include 23 | #include 24 | 25 | #define AX_12A 12 26 | #define AX_12W 300 27 | #define AX_18A 18 28 | 29 | #define RX_10 10 30 | #define RX_24F 24 31 | #define RX_28 28 32 | #define RX_64 64 33 | 34 | #define EX_106 107 35 | 36 | #define MX_12W 360 37 | #define MX_28 29 38 | #define MX_28_2 30 39 | #define MX_64 310 40 | #define MX_64_2 311 41 | #define MX_106 320 42 | #define MX_106_2 321 43 | 44 | #define XL_320 350 45 | #define XL430_W250 1060 46 | 47 | #define XL430_W250_2 1090 // 2XL 48 | 49 | #define XC430_W150 1070 50 | #define XC430_W240 1080 51 | 52 | #define XM430_W210 1030 53 | #define XM430_W350 1020 54 | 55 | #define XM540_W150 1130 56 | #define XM540_W270 1120 57 | 58 | #define XH430_W210 1010 59 | #define XH430_W350 1000 60 | #define XH430_V210 1050 61 | #define XH430_V350 1040 62 | 63 | #define XH540_W150 1110 64 | #define XH540_W270 1100 65 | #define XH540_V150 1150 66 | #define XH540_V270 1140 67 | 68 | #define XW540_T260 1170 69 | #define XW540_T140 1180 70 | 71 | #define PRO_L42_10_S300_R 35072 72 | #define PRO_L54_30_S400_R 37928 73 | #define PRO_L54_30_S500_R 37896 74 | #define PRO_L54_50_S290_R 38176 75 | #define PRO_L54_50_S500_R 38152 76 | 77 | #define PRO_M42_10_S260_R 43288 78 | #define PRO_M54_40_S250_R 46096 79 | #define PRO_M54_60_S250_R 46352 80 | 81 | #define PRO_H42_20_S300_R 51200 82 | #define PRO_H54_100_S500_R 53768 83 | #define PRO_H54_200_S500_R 54024 84 | 85 | #define PRO_M42_10_S260_R_A 43289 86 | #define PRO_M54_40_S250_R_A 46097 87 | #define PRO_M54_60_S250_R_A 46353 88 | 89 | #define PRO_H42_20_S300_R_A 51201 90 | #define PRO_H54_100_S500_R_A 53769 91 | #define PRO_H54_200_S500_R_A 54025 92 | 93 | #define PRO_PLUS_M42P_010_S260_R 2100 94 | #define PRO_PLUS_M54P_040_S250_R 2110 95 | #define PRO_PLUS_M54P_060_S250_R 2120 96 | 97 | #define PRO_PLUS_H42P_020_S300_R 2000 98 | #define PRO_PLUS_H54P_100_S500_R 2010 99 | #define PRO_PLUS_H54P_200_S500_R 2020 100 | 101 | #define RH_P12_RN 35073 102 | #define RH_P12_RN_A 35074 103 | 104 | #define BYTE 1 105 | #define WORD 2 106 | #define DWORD 4 107 | 108 | typedef struct 109 | { 110 | const char *item_name; 111 | uint16_t address; 112 | uint8_t item_name_length; 113 | uint16_t data_length; 114 | } ControlItem; 115 | 116 | typedef struct 117 | { 118 | float rpm; 119 | 120 | int64_t value_of_min_radian_position; 121 | int64_t value_of_zero_radian_position; 122 | int64_t value_of_max_radian_position; 123 | 124 | float min_radian; 125 | float max_radian; 126 | } ModelInfo; 127 | 128 | // Public Functions 129 | namespace DynamixelItem 130 | { 131 | const ControlItem *getControlTable(uint16_t model_number); 132 | const ModelInfo *getModelInfo(uint16_t model_number); 133 | 134 | uint8_t getTheNumberOfControlItem(); 135 | } 136 | 137 | #endif //DYNAMIXEL_ITEM_H 138 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/src/xarm/common/crc16.cc: -------------------------------------------------------------------------------- 1 | /* Copyright 2017 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: Jimy Zhang 6 | ============================================================================*/ 7 | #include "xarm/common/crc16.h" 8 | 9 | const unsigned char crc_tableh[256] = { 10 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 11 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 12 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 13 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 14 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 15 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 16 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 17 | 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 18 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 19 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 20 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 21 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 22 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 23 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 24 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 25 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 26 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 27 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 28 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 29 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 30 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 31 | 0x00, 0xC1, 0x81, 0x40 32 | }; 33 | 34 | const unsigned char crc_tablel[256] = { 35 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 36 | 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 37 | 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 38 | 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 39 | 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 40 | 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 41 | 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 42 | 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 43 | 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 44 | 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 45 | 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 46 | 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 47 | 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 48 | 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 49 | 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 50 | 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 51 | 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 52 | 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 53 | 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 54 | 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 55 | 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 56 | 0x41, 0x81, 0x80, 0x40 57 | }; 58 | 59 | int modbus_crc(unsigned char *data, int len) { 60 | unsigned char init_crch = 0xFF; 61 | unsigned char init_crcl = 0xFF; 62 | int index; 63 | while (len--) { 64 | index = init_crcl ^ *data++; 65 | init_crcl = init_crch ^ crc_tableh[index]; 66 | init_crch = crc_tablel[index]; 67 | } 68 | return ((init_crch << 8) | init_crcl); 69 | } 70 | -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp: -------------------------------------------------------------------------------- 1 | #include "interbotix_slate_driver/base_driver.h" 2 | #include "interbotix_slate_driver/serial_driver.h" 3 | 4 | namespace base_driver 5 | { 6 | 7 | SerialDriver *serial_driver = nullptr; 8 | 9 | bool chassisInit(std::string &dev) 10 | { 11 | serial_driver = new SerialDriver(); 12 | return serial_driver->init(PORT, dev, 115200); 13 | } 14 | 15 | void exit() 16 | { 17 | serial_driver->close(); 18 | ::exit(-1); 19 | } 20 | 21 | bool getBatteryInfo(float &vol, float &cur, int &percent) 22 | { 23 | ASSERT(serial_driver != nullptr, exit()); 24 | return serial_driver->getEntry(INDEX_SYS_VOLTAGE, &vol) && 25 | serial_driver->getEntry(INDEX_SYS_CURRENT, &cur) && 26 | serial_driver->getEntry(INDEX_SYS_POWER_PERCENTAGE, &percent); 27 | } 28 | 29 | bool getChassisCollision(int &collision) 30 | { 31 | ASSERT(serial_driver != nullptr, exit()); 32 | return serial_driver->getEntry(INDEX_SYS_COLLISION, &collision); 33 | } 34 | 35 | bool getChassisState(SystemState &state) 36 | { 37 | ASSERT(serial_driver != nullptr, exit()); 38 | return serial_driver->getEntry(INDEX_SYS_STATE, (int *)&state); 39 | } 40 | 41 | bool getVersion(char *text) 42 | { 43 | ASSERT(serial_driver != nullptr, exit()); 44 | return serial_driver->getEntry(INDEX_SYS_VERSION, text); 45 | } 46 | 47 | bool getJoyState(int &state) 48 | { 49 | ASSERT(serial_driver != nullptr, exit()); 50 | return serial_driver->getEntry(INDEX_STATE_JOY, (int *)&state); 51 | } 52 | 53 | bool setText(const char *text) 54 | { 55 | ASSERT(serial_driver != nullptr, exit()); 56 | return serial_driver->setEntry(INDEX_SYS_TEXT, text); 57 | } 58 | 59 | bool setCharge(int charge) 60 | { 61 | ASSERT(serial_driver != nullptr, exit()); 62 | return serial_driver->setEntry(INDEX_SYS_CHARGE, &charge); 63 | } 64 | 65 | bool setAlarm(int alarm) 66 | { 67 | ASSERT(serial_driver != nullptr, exit()); 68 | return serial_driver->setEntry(INDEX_SYS_ALARM, &alarm); 69 | } 70 | 71 | bool motorCtrl(int v) 72 | { 73 | ASSERT(serial_driver != nullptr, exit()); 74 | return serial_driver->setEntry(INDEX_SYS_CMD, &v); 75 | } 76 | 77 | bool setIo(int io) 78 | { 79 | ASSERT(serial_driver != nullptr, exit()); 80 | return serial_driver->setEntry(INDEX_STATE_IO, &io); 81 | } 82 | 83 | bool getIo(int &io_state) 84 | { 85 | ASSERT(serial_driver != nullptr, exit()); 86 | return serial_driver->getEntry(INDEX_STATE_IO, &io_state); 87 | } 88 | 89 | bool setLight(int light) 90 | { 91 | ASSERT(serial_driver != nullptr, exit()); 92 | return serial_driver->setEntry(INDEX_SYS_LIGHT, &light); 93 | } 94 | 95 | bool setStateLight(int light) 96 | { 97 | ASSERT(serial_driver != nullptr, exit()); 98 | return serial_driver->setEntry(INDEX_STATE_LIGHT, &light); 99 | } 100 | 101 | /** 102 | * @brief Set the body velocity 103 | * @param aim_x_vel forward velocity [m/s] 104 | * @param aim_z_omega rotational velocity [rad/s] 105 | */ 106 | bool chassisControl(float aim_x_vel, float aim_z_omega) 107 | { 108 | ASSERT(serial_driver != nullptr, exit()); 109 | return serial_driver->setEntry(INDEX_AIM_CHASSIS_VEL, &aim_x_vel) && 110 | serial_driver->setEntry(INDEX_AIM_CHASSIS_POS_OR_OMEGA, &aim_z_omega); 111 | } 112 | /** 113 | * @brief Get body velocity 114 | * @param x_vel m/s 115 | * @param z_omega rad/s 116 | */ 117 | bool getChassisInfo(float &x_vel, float &z_omega) 118 | { 119 | ASSERT(serial_driver != nullptr, exit()); 120 | return serial_driver->getEntry(INDEX_CHASSIS_VEL, &x_vel) && 121 | serial_driver->getEntry(INDEX_CHASSIS_POS_OR_OMEGA, &z_omega); 122 | } 123 | /** 124 | * @brief Get chassis odometry 125 | * @param odom_x odom along the x-axis 126 | * @param odom_y odom along the y-axis 127 | * @param odom_theta odom around the z-axis 128 | */ 129 | bool getChassisOdom(float &odom_x, float &odom_y, float &odom_theta) 130 | { 131 | ASSERT(serial_driver != nullptr, exit()); 132 | return serial_driver->getEntry(INDEX_CHASSIS_ODOM_X, &odom_x) && 133 | serial_driver->getEntry(INDEX_CHASSIS_ODOM_Y, &odom_y) && 134 | serial_driver->getEntry(INDEX_CHASSIS_ODOM_THETA, &odom_theta); 135 | } 136 | 137 | } // namespace base_driver 138 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/test/servo_cartesian_test.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: jason 6 | ============================================================================*/ 7 | #include "ros/ros.h" 8 | #include 9 | 10 | int go_home_test(xarm_msgs::Move &srv, ros::ServiceClient &client) 11 | { 12 | srv.request.mvvelo = 20.0 / 57.0; 13 | srv.request.mvacc = 1000; 14 | srv.request.mvtime = 0; 15 | if(client.call(srv)) 16 | { 17 | ROS_INFO("%s\n", srv.response.message.c_str()); 18 | } 19 | else 20 | { 21 | ROS_ERROR("Failed to call service go home"); 22 | return 1; 23 | } 24 | return 0; 25 | } 26 | 27 | int servo_cart_test(xarm_msgs::Move &srv, ros::ServiceClient &client) 28 | { 29 | std::vector inc = {0.3, 0, 0, 0, 0, 0}; 30 | 31 | srv.request.mvvelo = 0; 32 | srv.request.mvacc = 0; 33 | srv.request.mvtime = 1; 34 | 35 | srv.request.pose = inc; 36 | 37 | for (int i = 0; i < 500; i++) 38 | { 39 | if(client.call(srv)) 40 | { 41 | ROS_INFO("%s\n", srv.response.message.c_str()); 42 | } 43 | else 44 | { 45 | ROS_ERROR("Failed to call service move_servoj"); 46 | return 1; 47 | } 48 | usleep(1e4); // 0.01s 49 | } 50 | return 0; 51 | } 52 | 53 | 54 | int main(int argc, char **argv) 55 | { 56 | ros::init(argc, argv, "xarm_move_test"); 57 | ros::NodeHandle nh; 58 | nh.setParam("/xarm/wait_for_finish", true); // return after motion service finish 59 | ros::ServiceClient motion_ctrl_client_ = nh.serviceClient("/xarm/motion_ctrl"); 60 | ros::ServiceClient set_mode_client_ = nh.serviceClient("/xarm/set_mode"); 61 | ros::ServiceClient set_state_client_ = nh.serviceClient("/xarm/set_state"); 62 | ros::ServiceClient go_home_client_ = nh.serviceClient("/xarm/go_home"); 63 | ros::ServiceClient servo_cart_client_ = nh.serviceClient("/xarm/move_servo_cart"); 64 | 65 | xarm_msgs::SetAxis set_axis_srv_; 66 | xarm_msgs::SetInt16 set_int16_srv_; 67 | xarm_msgs::Move move_srv_; 68 | 69 | 70 | // STEP 1: Motion Enable 71 | set_axis_srv_.request.id = 8; 72 | set_axis_srv_.request.data = 1; 73 | 74 | if(motion_ctrl_client_.call(set_axis_srv_)) 75 | { 76 | ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); 77 | } 78 | else 79 | { 80 | ROS_ERROR("Failed to call service motion_ctrl"); 81 | return 1; 82 | } 83 | 84 | // STEP 2: Set Mode to 0 85 | set_int16_srv_.request.data = 0; 86 | if(set_mode_client_.call(set_int16_srv_)) 87 | { 88 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 89 | } 90 | else 91 | { 92 | ROS_ERROR("Failed to call service set_mode"); 93 | return 1; 94 | } 95 | 96 | // STEP 3: Set State to 0 97 | set_int16_srv_.request.data = 0; 98 | if(set_state_client_.call(set_int16_srv_)) 99 | { 100 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 101 | } 102 | else 103 | { 104 | ROS_ERROR("Failed to call service set_state"); 105 | return 1; 106 | } 107 | 108 | // STEP 4: Go to Home Position 109 | if(go_home_test(move_srv_, go_home_client_) == 1) 110 | return 1; 111 | 112 | 113 | // STEP 5: Set Mode to 1 114 | set_int16_srv_.request.data = 1; 115 | if(set_mode_client_.call(set_int16_srv_)) 116 | { 117 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 118 | } 119 | else 120 | { 121 | ROS_ERROR("Failed to call service set_mode"); 122 | return 1; 123 | } 124 | 125 | // STEP 6: Set state to 0 126 | set_int16_srv_.request.data = 0; 127 | if(set_state_client_.call(set_int16_srv_)) 128 | { 129 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 130 | } 131 | else 132 | { 133 | ROS_ERROR("Failed to call service set_state"); 134 | return 1; 135 | } 136 | 137 | // STEP 7: Call SERVO_CARTESIAN service in a loop 138 | return servo_cart_test(move_srv_, servo_cart_client_); 139 | 140 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | void swap(int32_t *array); 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | const char* port_name = "/dev/ttyUSB0"; 26 | int baud_rate = 57600; 27 | 28 | uint16_t model_number = 0; 29 | uint8_t dxl_id[2] = {0, 0}; 30 | 31 | if (argc < 5) 32 | { 33 | printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); 34 | return 0; 35 | } 36 | else 37 | { 38 | port_name = argv[1]; 39 | baud_rate = atoi(argv[2]); 40 | dxl_id[0] = atoi(argv[3]); 41 | dxl_id[1] = atoi(argv[4]); 42 | } 43 | 44 | DynamixelWorkbench dxl_wb; 45 | 46 | const char *log; 47 | bool result = false; 48 | 49 | result = dxl_wb.init(port_name, baud_rate, &log); 50 | if (result == false) 51 | { 52 | printf("%s\n", log); 53 | printf("Failed to init\n"); 54 | 55 | return 0; 56 | } 57 | else 58 | printf("Succeed to init(%d)\n", baud_rate); 59 | 60 | for (int cnt = 0; cnt < 2; cnt++) 61 | { 62 | result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); 63 | if (result == false) 64 | { 65 | printf("%s\n", log); 66 | printf("Failed to ping\n"); 67 | } 68 | else 69 | { 70 | printf("Succeeded to ping\n"); 71 | printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); 72 | } 73 | 74 | result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); 75 | if (result == false) 76 | { 77 | printf("%s\n", log); 78 | printf("Failed to change joint mode\n"); 79 | } 80 | else 81 | { 82 | printf("Succeed to change joint mode\n"); 83 | } 84 | } 85 | 86 | result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log); 87 | if (result == false) 88 | { 89 | printf("%s\n", log); 90 | printf("Failed to add sync write handler\n"); 91 | } 92 | 93 | result = dxl_wb.addSyncReadHandler(dxl_id[0], "Present_Position", &log); 94 | if (result == false) 95 | { 96 | printf("%s\n", log); 97 | printf("Failed to add sync read handler\n"); 98 | } 99 | 100 | int32_t goal_position[2] = {0, 1023}; 101 | int32_t present_position[2] = {0, 0}; 102 | 103 | const uint8_t handler_index = 0; 104 | 105 | while(1) 106 | { 107 | result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); 108 | if (result == false) 109 | { 110 | printf("%s\n", log); 111 | printf("Failed to sync write position\n"); 112 | } 113 | 114 | do 115 | { 116 | result = dxl_wb.syncRead(handler_index, &log); 117 | if (result == false) 118 | { 119 | printf("%s\n", log); 120 | printf("Failed to sync read position\n"); 121 | } 122 | 123 | result = dxl_wb.getSyncReadData(handler_index, &present_position[0], &log); 124 | if (result == false) 125 | { 126 | printf("%s\n", log); 127 | } 128 | else 129 | { 130 | printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tGoal Position : %d\tPresent Position : %d\n" 131 | ,dxl_id[0], goal_position[0], present_position[0], dxl_id[1], goal_position[1], present_position[1]); 132 | } 133 | 134 | }while(abs(goal_position[0] - present_position[0]) > 15 && 135 | abs(goal_position[1] - present_position[1]) > 15); 136 | 137 | swap(goal_position); 138 | } 139 | 140 | return 0; 141 | } 142 | 143 | void swap(int32_t *array) 144 | { 145 | int32_t tmp = array[0]; 146 | array[0] = array[1]; 147 | array[1] = tmp; 148 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | int mode = 0; 27 | 28 | if (argc < 4) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | } 39 | 40 | DynamixelWorkbench dxl_wb; 41 | 42 | const char *log; 43 | bool result = false; 44 | 45 | uint16_t model_number = 0; 46 | 47 | result = dxl_wb.init(port_name, baud_rate, &log); 48 | if (result == false) 49 | { 50 | printf("%s\n", log); 51 | printf("Failed to init\n"); 52 | 53 | return 0; 54 | } 55 | else 56 | printf("Succeeded to init(%d)\n", baud_rate); 57 | 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | } 64 | else 65 | { 66 | printf("Succeeded to ping\n"); 67 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 68 | } 69 | 70 | printf("---> \n"); 71 | printf("Please insert the right number from 0 to 5 to set control mode \n"); 72 | printf("0 - current control mode\n"); 73 | printf("1 - velocity control mode\n"); 74 | printf("2 - position control mode\n"); 75 | printf("3 - extended position control mode\n"); 76 | printf("4 - current based position control mode\n"); 77 | printf("5 - pwm control mode\n"); 78 | printf("<--- \n\n"); 79 | 80 | scanf("%d", &mode); 81 | 82 | switch (mode) 83 | { 84 | case 0: 85 | result = dxl_wb.setCurrentControlMode(dxl_id, &log); 86 | if (result == false) 87 | { 88 | printf("%s\n", log); 89 | printf("Failed to set mode\n"); 90 | } 91 | else 92 | { 93 | printf("Succeeded to set mode\n"); 94 | } 95 | break; 96 | 97 | case 1: 98 | result = dxl_wb.setVelocityControlMode(dxl_id, &log); 99 | if (result == false) 100 | { 101 | printf("%s\n", log); 102 | printf("Failed to set mode\n"); 103 | } 104 | else 105 | { 106 | printf("Succeeded to set mode\n"); 107 | } 108 | break; 109 | 110 | case 2: 111 | result = dxl_wb.setPositionControlMode(dxl_id, &log); 112 | if (result == false) 113 | { 114 | printf("%s\n", log); 115 | printf("Failed to set mode\n"); 116 | } 117 | else 118 | { 119 | printf("Succeeded to set mode\n"); 120 | } 121 | break; 122 | 123 | case 3: 124 | result = dxl_wb.setExtendedPositionControlMode(dxl_id, &log); 125 | if (result == false) 126 | { 127 | printf("%s\n", log); 128 | printf("Failed to set mode\n"); 129 | } 130 | else 131 | { 132 | printf("Succeeded to set mode\n"); 133 | } 134 | break; 135 | 136 | case 4: 137 | result = dxl_wb.setCurrentBasedPositionControlMode(dxl_id, &log); 138 | if (result == false) 139 | { 140 | printf("%s\n", log); 141 | printf("Failed to set mode\n"); 142 | } 143 | else 144 | { 145 | printf("Succeeded to set mode\n"); 146 | } 147 | break; 148 | 149 | case 5: 150 | result = dxl_wb.setPWMControlMode(dxl_id, &log); 151 | if (result == false) 152 | { 153 | printf("%s\n", log); 154 | printf("Failed to set mode\n"); 155 | } 156 | else 157 | { 158 | printf("Succeeded to set mode\n"); 159 | } 160 | break; 161 | 162 | default: 163 | result = dxl_wb.setPositionControlMode(dxl_id, &log); 164 | if (result == false) 165 | { 166 | printf("%s\n", log); 167 | printf("Failed to set mode\n"); 168 | } 169 | else 170 | { 171 | printf("Succeeded to set mode\n"); 172 | } 173 | break; 174 | } 175 | 176 | return 0; 177 | } 178 | -------------------------------------------------------------------------------- /interbotix_ros_uxarms/xarm_api/test/move_test.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2018 UFACTORY Inc. All Rights Reserved. 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Author: waylon 6 | Jason 7 | ============================================================================*/ 8 | #include "ros/ros.h" 9 | #include 10 | 11 | int go_home_test(xarm_msgs::Move srv, ros::ServiceClient client) 12 | { 13 | srv.request.mvvelo = 20.0 / 57.0; 14 | srv.request.mvacc = 1000; 15 | srv.request.mvtime = 0; 16 | if(client.call(srv)) 17 | { 18 | ROS_INFO("%s\n", srv.response.message.c_str()); 19 | } 20 | else 21 | { 22 | ROS_ERROR("Failed to call service go home"); 23 | return 1; 24 | } 25 | return 0; 26 | } 27 | 28 | int servoj_test(xarm_msgs::Move srv, ros::ServiceClient client) 29 | { 30 | std::vector joint[2] = {{0, 0, 0, 0, 0, 0, 0}, 31 | {0, -0.3, 0, 0, 0, 0, 0}}; 32 | 33 | srv.request.mvvelo = 0; 34 | srv.request.mvacc = 0; 35 | srv.request.mvtime = 0; 36 | for (int i = 0; i < 2; i++) 37 | { 38 | srv.request.pose = joint[i]; 39 | if(client.call(srv)) 40 | { 41 | ROS_INFO("%s\n", srv.response.message.c_str()); 42 | } 43 | else 44 | { 45 | ROS_ERROR("Failed to call service move_servoj"); 46 | return 1; 47 | } 48 | sleep(2); 49 | } 50 | return 0; 51 | } 52 | 53 | int move_lineb_test(xarm_msgs::Move srv, ros::ServiceClient client) 54 | { 55 | std::vector pose[5] = { {300, 0, 100, -3.14, 0, 0}, 56 | {300, 100, 100, -3.14, 0, 0}, 57 | {400, 100, 100, -3.14, 0, 0}, 58 | {400, -100, 100, -3.14, 0, 0}, 59 | {300, -100, 100, -3.14, 0, 0}}; 60 | srv.request.mvvelo = 100; 61 | srv.request.mvacc = 1000; 62 | srv.request.mvtime = 0; 63 | srv.request.mvradii = 20; 64 | 65 | for(int k=0; k<3; k++) 66 | { 67 | for(int i = 0; i < 5; i++) 68 | { 69 | srv.request.pose = pose[i]; 70 | if(client.call(srv)) 71 | { 72 | ROS_INFO("%s\n", srv.response.message.c_str()); 73 | } 74 | else 75 | { 76 | ROS_ERROR("Failed to call service move_lineb"); 77 | return 1; 78 | } 79 | } 80 | } 81 | return 0; 82 | } 83 | 84 | int main(int argc, char **argv) 85 | { 86 | ros::init(argc, argv, "xarm_move_test"); 87 | ros::NodeHandle nh; 88 | 89 | nh.setParam("/xarm/wait_for_finish", true); // return after motion service finish 90 | ros::Publisher sleep_pub_ = nh.advertise("/xarm/sleep_sec", 1); 91 | ros::ServiceClient motion_ctrl_client_ = nh.serviceClient("/xarm/motion_ctrl"); 92 | ros::ServiceClient set_mode_client_ = nh.serviceClient("/xarm/set_mode"); 93 | ros::ServiceClient set_state_client_ = nh.serviceClient("/xarm/set_state"); 94 | ros::ServiceClient go_home_client_ = nh.serviceClient("/xarm/go_home"); 95 | ros::ServiceClient move_lineb_client_ = nh.serviceClient("/xarm/move_lineb"); 96 | ros::ServiceClient move_servoj_client_ = nh.serviceClient("/xarm/move_servoj"); 97 | 98 | xarm_msgs::SetAxis set_axis_srv_; 99 | xarm_msgs::SetInt16 set_int16_srv_; 100 | xarm_msgs::Move move_srv_; 101 | 102 | set_axis_srv_.request.id = 8; 103 | set_axis_srv_.request.data = 1; 104 | 105 | if(motion_ctrl_client_.call(set_axis_srv_)) 106 | { 107 | ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); 108 | } 109 | else 110 | { 111 | ROS_ERROR("Failed to call service motion_ctrl"); 112 | return 1; 113 | } 114 | 115 | set_int16_srv_.request.data = 0; 116 | if(set_mode_client_.call(set_int16_srv_)) 117 | { 118 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 119 | } 120 | else 121 | { 122 | ROS_ERROR("Failed to call service set_mode"); 123 | return 1; 124 | } 125 | 126 | set_int16_srv_.request.data = 0; 127 | if(set_state_client_.call(set_int16_srv_)) 128 | { 129 | ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); 130 | } 131 | else 132 | { 133 | ROS_ERROR("Failed to call service set_state"); 134 | return 1; 135 | } 136 | 137 | if(go_home_test(move_srv_, go_home_client_) == 1) 138 | return 1; 139 | 140 | // MOVE_LINEB need some additional configurations: wait=False, sleep before sending commands 141 | nh.setParam("/xarm/wait_for_finish", false); // This configuration is CRITICAL for move_lineb! 142 | std_msgs::Float32 sleep_msg; 143 | sleep_msg.data = 1.0; 144 | sleep_pub_.publish(sleep_msg); 145 | if(move_lineb_test(move_srv_, move_lineb_client_) == 1) 146 | return 1; 147 | nh.setParam("/xarm/wait_for_finish", true); 148 | 149 | } -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #ifndef DYNAMIXEL_WORKBENCH_H_ 20 | #define DYNAMIXEL_WORKBENCH_H_ 21 | 22 | #include "dynamixel_driver.h" 23 | 24 | class DynamixelWorkbench : public DynamixelDriver 25 | { 26 | public: 27 | DynamixelWorkbench(); 28 | ~DynamixelWorkbench(); 29 | 30 | bool torque(uint8_t id, int32_t onoff, const char **log = NULL); 31 | bool torqueOn(uint8_t id, const char **log = NULL); 32 | bool torqueOff(uint8_t id, const char **log = NULL); 33 | 34 | bool changeID(uint8_t id, uint8_t new_id, const char **log = NULL); 35 | bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log = NULL); 36 | bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log = NULL); 37 | 38 | bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); 39 | bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); 40 | 41 | bool led(uint8_t id, int32_t onoff, const char **log = NULL); 42 | bool ledOn(uint8_t id, const char **log = NULL); 43 | bool ledOff(uint8_t id, const char **log = NULL); 44 | 45 | bool setNormalDirection(uint8_t id, const char **log = NULL); 46 | bool setReverseDirection(uint8_t id, const char **log = NULL); 47 | 48 | bool setVelocityBasedProfile(uint8_t id, const char **log = NULL); 49 | bool setTimeBasedProfile(uint8_t id, const char **log = NULL); 50 | 51 | bool setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log = NULL); 52 | 53 | bool setCurrentControlMode(uint8_t id, const char **log = NULL); 54 | bool setTorqueControlMode(uint8_t id, const char **log = NULL); 55 | bool setVelocityControlMode(uint8_t id, const char **log = NULL); 56 | bool setPositionControlMode(uint8_t id, const char **log = NULL); 57 | bool setExtendedPositionControlMode(uint8_t id, const char **log = NULL); 58 | bool setMultiTurnControlMode(uint8_t id, const char **log = NULL); 59 | bool setCurrentBasedPositionControlMode(uint8_t id, const char **log = NULL); 60 | bool setPWMControlMode(uint8_t id, const char **log = NULL); 61 | 62 | bool setOperatingMode(uint8_t id, uint8_t index, const char **log = NULL); 63 | 64 | bool jointMode(uint8_t id, int32_t velocity = 0, int32_t acceleration = 0, const char **log = NULL); 65 | bool wheelMode(uint8_t id, int32_t acceleration = 0, const char **log = NULL); 66 | bool currentBasedPositionMode(uint8_t id, int32_t current = 0, const char **log = NULL); 67 | 68 | bool goalPosition(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 69 | // bool goalPosition(uint8_t id, int32_t value, const char **log = NULL); 70 | bool goalPosition(uint8_t id, float radian, const char **log = NULL); 71 | 72 | bool goalSpeed(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 73 | bool goalVelocity(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 74 | // bool goalVelocity(uint8_t id, int32_t value, const char **log = NULL); 75 | bool goalVelocity(uint8_t id, float velocity, const char **log = NULL); 76 | 77 | bool getPresentPositionData(uint8_t id, int32_t* data, const char **log = NULL); 78 | bool getRadian(uint8_t id, float* radian, const char **log = NULL); 79 | 80 | bool getPresentVelocityData(uint8_t id, int32_t* data, const char **log = NULL); 81 | bool getVelocity(uint8_t id, float* velocity, const char **log = NULL); 82 | 83 | int32_t convertRadian2Value(uint8_t id, float radian); 84 | float convertValue2Radian(uint8_t id, int32_t value); 85 | 86 | int32_t convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian); 87 | float convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian); 88 | 89 | int32_t convertVelocity2Value(uint8_t id, float velocity); 90 | float convertValue2Velocity(uint8_t id, int32_t value); 91 | 92 | int16_t convertCurrent2Value(uint8_t id, float current); 93 | int16_t convertCurrent2Value(float current); 94 | float convertValue2Current(uint8_t id, int16_t value); 95 | float convertValue2Current(int16_t value); 96 | 97 | float convertValue2Load(int16_t value); 98 | }; 99 | 100 | #endif /*DYNAMIXEL_WORKBENCH_H_*/ 101 | -------------------------------------------------------------------------------- /interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Taehun Lim (Darby) */ 18 | 19 | #include 20 | 21 | void swap(int32_t *array); 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | const char* port_name = "/dev/ttyUSB0"; 26 | int baud_rate = 57600; 27 | 28 | uint16_t model_number = 0; 29 | uint8_t dxl_id[2] = {0, 0}; 30 | 31 | if (argc < 5) 32 | { 33 | printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); 34 | return 0; 35 | } 36 | else 37 | { 38 | port_name = argv[1]; 39 | baud_rate = atoi(argv[2]); 40 | dxl_id[0] = atoi(argv[3]); 41 | dxl_id[1] = atoi(argv[4]); 42 | } 43 | 44 | DynamixelWorkbench dxl_wb; 45 | 46 | const char *log; 47 | bool result = false; 48 | 49 | result = dxl_wb.init(port_name, baud_rate, &log); 50 | if (result == false) 51 | { 52 | printf("%s\n", log); 53 | printf("Failed to init\n"); 54 | 55 | return 0; 56 | } 57 | else 58 | printf("Succeed to init(%d)\n", baud_rate); 59 | 60 | for (int cnt = 0; cnt < 2; cnt++) 61 | { 62 | result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); 63 | if (result == false) 64 | { 65 | printf("%s\n", log); 66 | printf("Failed to ping\n"); 67 | } 68 | else 69 | { 70 | printf("Succeeded to ping\n"); 71 | printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); 72 | } 73 | 74 | result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); 75 | if (result == false) 76 | { 77 | printf("%s\n", log); 78 | printf("Failed to change joint mode\n"); 79 | } 80 | else 81 | { 82 | printf("Succeed to change joint mode\n"); 83 | } 84 | } 85 | 86 | result = dxl_wb.initBulkWrite(&log); 87 | if (result == false) 88 | { 89 | printf("%s\n", log); 90 | } 91 | else 92 | { 93 | printf("%s\n", log); 94 | } 95 | 96 | result = dxl_wb.initBulkRead(&log); 97 | if (result == false) 98 | { 99 | printf("%s\n", log); 100 | } 101 | else 102 | { 103 | printf("%s\n", log); 104 | } 105 | 106 | result = dxl_wb.addBulkReadParam(dxl_id[0], "Present_Position", &log); 107 | if (result == false) 108 | { 109 | printf("%s\n", log); 110 | printf("Failed to add bulk read position param\n"); 111 | } 112 | else 113 | { 114 | printf("%s\n", log); 115 | } 116 | 117 | result = dxl_wb.addBulkReadParam(dxl_id[1], "LED", &log); 118 | if (result == false) 119 | { 120 | printf("%s\n", log); 121 | printf("Failed to add bulk read led param\n"); 122 | } 123 | else 124 | { 125 | printf("%s\n", log); 126 | } 127 | 128 | int32_t goal_position[2] = {0, 1023}; 129 | int32_t led[2] = {0, 1}; 130 | 131 | int32_t get_data[2] = {0, 0}; 132 | 133 | const uint8_t handler_index = 0; 134 | 135 | while(1) 136 | { 137 | result = dxl_wb.addBulkWriteParam(dxl_id[0], "Goal_Position", goal_position[0], &log); 138 | if (result == false) 139 | { 140 | printf("%s\n", log); 141 | printf("Failed to add bulk write position param\n"); 142 | } 143 | else 144 | { 145 | printf("%s\n", log); 146 | } 147 | 148 | result = dxl_wb.addBulkWriteParam(dxl_id[1], "LED", led[0], &log); 149 | if (result == false) 150 | { 151 | printf("%s\n", log); 152 | printf("Failed to add bulk write led param\n"); 153 | } 154 | else 155 | { 156 | printf("%s\n", log); 157 | } 158 | 159 | result = dxl_wb.bulkWrite(&log); 160 | if (result == false) 161 | { 162 | printf("%s\n", log); 163 | printf("Failed to bulk write\n"); 164 | } 165 | 166 | do 167 | { 168 | result = dxl_wb.bulkRead(&log); 169 | if (result == false) 170 | { 171 | printf("%s\n", log); 172 | printf("Failed to bulk read\n"); 173 | } 174 | 175 | result = dxl_wb.getBulkReadData(&get_data[0], &log); 176 | if (result == false) 177 | { 178 | printf("%s\n", log); 179 | } 180 | else 181 | { 182 | printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tLED : %d\n" 183 | ,dxl_id[0], goal_position[0], get_data[0], dxl_id[1], get_data[1]); 184 | } 185 | 186 | }while(abs(goal_position[0] - get_data[0]) > 15); 187 | 188 | swap(goal_position); 189 | swap(led); 190 | } 191 | 192 | return 0; 193 | } 194 | 195 | void swap(int32_t *array) 196 | { 197 | int32_t tmp = array[0]; 198 | array[0] = array[1]; 199 | array[1] = tmp; 200 | } -------------------------------------------------------------------------------- /interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ 2 | #define INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ 3 | 4 | #include "geometry_msgs/Twist.h" 5 | #include "interbotix_slate_driver/base_driver.h" 6 | #include "interbotix_slate_driver/serial_driver.h" 7 | #include "interbotix_slate_msgs/SetString.h" 8 | #include "nav_msgs/Odometry.h" 9 | #include "ros/ros.h" 10 | #include "sensor_msgs/BatteryState.h" 11 | #include "std_srvs/SetBool.h" 12 | #include "tf/transform_broadcaster.h" 13 | 14 | namespace slate_base 15 | { 16 | 17 | #define CMD_TIME_OUT 300 // ms 18 | #define PORT "chassis" 19 | 20 | class SlateBase 21 | { 22 | public: 23 | 24 | /** 25 | * @brief Constructor for the SlateBase 26 | * @param node_handle ROS NodeHandle 27 | */ 28 | explicit SlateBase(ros::NodeHandle * node_handle); 29 | 30 | /// @brief Destructor for the SlateBase 31 | ~SlateBase() {}; 32 | 33 | /// @brief Process velocity commands and update robot state 34 | void update(); 35 | 36 | private: 37 | // NodeHandle 38 | ros::NodeHandle node; 39 | 40 | // Linear velocity command 41 | float cmd_vel_x_; 42 | 43 | // Angular velocity command 44 | float cmd_vel_z_; 45 | 46 | // Time last velocity command was received 47 | ros::Time cmd_vel_time_last_update_; 48 | 49 | // Odometry publisher 50 | ros::Publisher pub_odom_; 51 | 52 | // BatteryState publisher 53 | ros::Publisher pub_battery_state_; 54 | 55 | // Twist command subscriber 56 | ros::Subscriber sub_cmd_vel_; 57 | 58 | // Screen info service server 59 | ros::ServiceServer srv_info_; 60 | 61 | // Motor status service server 62 | ros::ServiceServer srv_motor_torque_status_; 63 | 64 | // Set charge enable service server 65 | ros::ServiceServer srv_enable_charing_; 66 | 67 | // Name of odom frame 68 | std::string odom_frame_name_; 69 | 70 | // Name of base frame 71 | std::string base_frame_name_; 72 | 73 | // Update counter used to only update some values less frequently 74 | int cnt_; 75 | 76 | // Odometry translation in the x-direction in meters 77 | float x_; 78 | 79 | // Odometry translation in the y-direction in meters 80 | float y_; 81 | 82 | // Odometry rotation about the z-axis in radians 83 | float theta_; 84 | 85 | // Odometry forward velocity in meters per second 86 | float x_vel_; 87 | 88 | // Odometry rotational velocity about the z-axis in radians per second 89 | float z_omega_; 90 | 91 | // Whether or not we have received our first odometry update 92 | bool is_first_odom_; 93 | 94 | // Array containing x and y translation in meters and rotation in radians 95 | float pose_[3]; 96 | 97 | // Current of the right motor in Amps 98 | float right_motor_c_; 99 | 100 | // Current of the left motor in Amps 101 | float left_motor_c_; 102 | 103 | // The system state of the base 104 | SystemState chassis_state_; 105 | 106 | // Whether or not to publish base_link->odom TF 107 | bool publish_tf_; 108 | 109 | // Max linear velocity in the x-direction in meters per second 110 | float max_vel_x_ = 1.0; 111 | 112 | // Max angular velocity about the z-axis in radians per second 113 | float max_vel_z_ = 1.0; 114 | 115 | // If publish_tf_ is true, this is the broadcaster used to publish the odom->base_link TF 116 | tf::TransformBroadcaster tf_broadcaster_odom_; 117 | 118 | // Time of the current update 119 | ros::Time current_time_; 120 | 121 | // Time of the last update 122 | ros::Time last_time_; 123 | 124 | // Timeout for base velocity 125 | ros::Duration cmd_vel_timeout_; 126 | 127 | /** 128 | * @brief Process incoming Twist command message 129 | * @param msg Incoming Twist command message 130 | */ 131 | void cmd_vel_callback(const geometry_msgs::Twist & msg); 132 | 133 | /** 134 | * @brief Process incoming screen text service request 135 | * @param req Service request containing desired text 136 | * @param res[out] Service response containing a success indication and a message 137 | * @return True if service request succeeded, false otherwise 138 | */ 139 | bool set_text_callback( 140 | interbotix_slate_msgs::SetString::Request & req, 141 | interbotix_slate_msgs::SetString::Response & res); 142 | 143 | /** 144 | * @brief Process incoming motor torque status service request 145 | * @param req Service request containing desired motor torque status 146 | * @param res[out] Service response containing a success indication and a message 147 | * @return True if service request succeeded, false otherwise 148 | */ 149 | bool motor_torque_status_callback( 150 | std_srvs::SetBool::Request & req, 151 | std_srvs::SetBool::Response & res); 152 | 153 | /** 154 | * @brief Process incoming enable charing service request 155 | * @param req Service request containing desired desired charing enable status 156 | * @param res[out] Service response containing a success indication and a message 157 | * @return True if service succeeded, false otherwise 158 | */ 159 | bool enable_charing_callback( 160 | std_srvs::SetBool::Request & req, 161 | std_srvs::SetBool::Response & res); 162 | 163 | /** 164 | * @brief Wrap angle 165 | * @param angle Angle to wrap in radians 166 | * @return Wrapped angle 167 | */ 168 | float wrap_angle(float angle); 169 | }; 170 | 171 | } // namespace slate_base 172 | 173 | #endif // INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ 174 | --------------------------------------------------------------------------------