diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml new file mode 100644 index 0000000..c777cf7 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -0,0 +1,71 @@ +name: Bug Report +description: File a bug report +title: "[Bug]: " +labels: ["bug"] +assignees: + - LSInterbotix +body: + - type: markdown + attributes: + value: | + Thanks for taking the time to fill out this bug report! + - type: textarea + id: what-happened + attributes: + label: What happened? + description: Also tell us what you expected to happen. + placeholder: Tell us what you see! + validations: + required: true + - type: textarea + id: robot_model + attributes: + label: Robot Model + description: Which robot are you using? + placeholder: wx250s, locobot_px100, etc. + validations: + required: true + - type: dropdown + id: operating_system + attributes: + label: Operating System + description: Which operating system are you seeing this problem on? + multiple: true + options: + - Ubuntu 18.04 + - Ubuntu 20.04 + - Ubuntu 22.04 + - Other (Describe in "Additional Info") + validations: + required: true + - type: dropdown + id: ros_distro + attributes: + label: ROS Distro + description: Which distribution of ROS are you seeing this problem on? + multiple: true + options: + - ROS 1 Melodic + - ROS 1 Noetic + - ROS 2 Galactic + - ROS 2 Humble + - ROS 2 Rolling + - Other (Describe in "Additional Info") + validations: + required: true + - type: textarea + id: steps + attributes: + label: Steps To Reproduce + description: What were the steps you took immediately preceding your issue? + - type: textarea + id: logs + attributes: + label: Relevant log output + description: Please copy and paste any relevant log output. This will be automatically formatted into code, so no need for backticks. + render: shell + - type: textarea + id: other + attributes: + label: Additional Info + description: Please enter anything else you'd like to share here. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000..5c17b9a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,8 @@ +blank_issues_enabled: true +contact_links: + - name: Contact Trossen Robotics Support + url: https://www.trossenrobotics.com/contact.aspx + about: If you have any questions about missing or damaged parts. + - name: Discussions + url: https://github.com/orgs/Interbotix/discussions + about: Ask and answer questions about Interbotix products. diff --git a/.github/ISSUE_TEMPLATE/feature_request.yml b/.github/ISSUE_TEMPLATE/feature_request.yml new file mode 100644 index 0000000..a7d1b5b --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.yml @@ -0,0 +1,39 @@ +name: Feature Request +description: Suggest an idea for this project +title: "[Enhancement]: " +labels: ["enhancement"] +assignees: + - LSInterbotix +body: + - type: markdown + attributes: + value: | + Thanks for taking the time to suggest a new feature! + - type: textarea + id: describe-feature + attributes: + label: Describe the solution you'd like + description: A clear and concise description of what you want to happen. + validations: + required: true + - type: textarea + id: platform + attributes: + label: Platform + description: Which platform would you like to see this feature on? + placeholder: X-Series Arms, LoCoBots, etc. + - type: textarea + id: problem + attributes: + label: Is your feature request related to a problem? Please describe. + description: A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + - type: textarea + id: logs + attributes: + label: Suggested code + 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. + - type: textarea + id: other + attributes: + label: Additional Info + description: Please enter anything else you'd like to share here. diff --git a/.github/ISSUE_TEMPLATE/question.yml b/.github/ISSUE_TEMPLATE/question.yml new file mode 100644 index 0000000..85c95ea --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question.yml @@ -0,0 +1,57 @@ +name: Question +description: Ask a question +title: "[Question]: " +labels: ["question"] +assignees: + - LSInterbotix +body: + - type: markdown + attributes: + value: | + Thanks for taking the time to ask a question! + - type: textarea + id: question + attributes: + label: Question + description: Ask your question here. + validations: + required: true + - type: textarea + id: robot_model + attributes: + label: Robot Model + description: Which robot are you asking about (if any)? + placeholder: wx250s, locobot_px100, etc. + - type: dropdown + id: operating_system + attributes: + label: Operating System + description: Which operating system are you targeting when asking this question? + multiple: true + options: + - Ubuntu 18.04 + - Ubuntu 20.04 + - Ubuntu 22.04 + - Other (Describe in "Additional Info") + validations: + required: true + - type: dropdown + id: ros_version + attributes: + label: ROS Version + description: Which version of ROS are you targeting when asking this question? + multiple: true + options: + - ROS 1 Melodic + - ROS 1 Noetic + - ROS 2 Galactic + - ROS 2 Humble + - ROS 2 Rolling + - Other (Describe in "Additional Info") + validations: + required: true + - type: textarea + id: other + attributes: + label: Additional Info + description: Please enter anything else you'd like to share here. diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml new file mode 100644 index 0000000..9cd317d --- /dev/null +++ b/.github/workflows/humble.yml @@ -0,0 +1,37 @@ +name: build-humble + +on: + push: + branches: + - humble + pull_request: + branches: + - humble + workflow_dispatch: + +# Limit jobs run by PRs or branches by cancelling ongoing jobs +# https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + humble: + strategy: + matrix: + env: + - {ROS_DISTRO: humble, ROS_REPO: main} + - {ROS_DISTRO: humble, ROS_REPO: testing} + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Prepare Workspace + run: | + rm \ + interbotix_ros_common_drivers/COLCON_IGNORE \ + interbotix_ros_slate/COLCON_IGNORE \ + interbotix_ros_xseries/COLCON_IGNORE + - uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/iron.yml b/.github/workflows/iron.yml new file mode 100644 index 0000000..cf8237b --- /dev/null +++ b/.github/workflows/iron.yml @@ -0,0 +1,37 @@ +name: build-iron + +on: + push: + branches: + - iron + pull_request: + branches: + - iron + workflow_dispatch: + +# Limit jobs run by PRs or branches by cancelling ongoing jobs +# https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + iron: + strategy: + matrix: + env: + - {ROS_DISTRO: iron, ROS_REPO: main} + - {ROS_DISTRO: iron, ROS_REPO: testing} + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Prepare Workspace + run: | + rm \ + interbotix_ros_common_drivers/COLCON_IGNORE \ + interbotix_ros_slate/COLCON_IGNORE \ + interbotix_ros_xseries/COLCON_IGNORE + - uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/jazzy.yml b/.github/workflows/jazzy.yml new file mode 100644 index 0000000..8322bb3 --- /dev/null +++ b/.github/workflows/jazzy.yml @@ -0,0 +1,37 @@ +name: build-jazzy + +on: + push: + branches: + - jazzy + pull_request: + branches: + - jazzy + workflow_dispatch: + +# Limit jobs run by PRs or branches by cancelling ongoing jobs +# https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + jazzy: + strategy: + matrix: + env: + - {ROS_DISTRO: jazzy, ROS_REPO: main} + - {ROS_DISTRO: jazzy, ROS_REPO: testing} + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Prepare Workspace + run: | + rm \ + interbotix_ros_common_drivers/COLCON_IGNORE \ + interbotix_ros_slate/COLCON_IGNORE \ + interbotix_ros_xseries/COLCON_IGNORE + - uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml new file mode 100644 index 0000000..1b16fab --- /dev/null +++ b/.github/workflows/rolling.yml @@ -0,0 +1,37 @@ +name: build-rolling + +on: + push: + branches: + - rolling + pull_request: + branches: + - rolling + workflow_dispatch: + +# Limit jobs run by PRs or branches by cancelling ongoing jobs +# https://docs.github.com/en/actions/using-jobs/using-concurrency#example-using-concurrency-and-the-default-behavior +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + rolling: + strategy: + matrix: + env: + - {ROS_DISTRO: rolling, ROS_REPO: main} + - {ROS_DISTRO: rolling, ROS_REPO: testing} + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Prepare Workspace + run: | + rm \ + interbotix_ros_common_drivers/COLCON_IGNORE \ + interbotix_ros_slate/COLCON_IGNORE \ + interbotix_ros_xseries/COLCON_IGNORE + - uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/slate-humble.yaml b/.github/workflows/slate-humble.yaml new file mode 100644 index 0000000..914eb32 --- /dev/null +++ b/.github/workflows/slate-humble.yaml @@ -0,0 +1,31 @@ +name: build-slate-humble + +on: + push: + branches: + - humble + pull_request: + branches: + - humble + workflow_dispatch: + +defaults: + run: + shell: bash + +jobs: + slate-humble: + strategy: + matrix: + env: + - {ROS_DISTRO: humble, ROS_REPO: main} + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v4 + with: + path: src/interbotix_ros_core + - name: Prepare Workspace + run: | + rm src/interbotix_ros_core/interbotix_ros_slate/COLCON_IGNORE + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/xs-galactic.yaml b/.github/workflows/xs-galactic.yaml new file mode 100644 index 0000000..847b67b --- /dev/null +++ b/.github/workflows/xs-galactic.yaml @@ -0,0 +1,29 @@ +name: build-xs-galactic + +on: + push: + branches: + - galactic + pull_request: + branches: + - galactic + workflow_dispatch: + +defaults: + run: + shell: bash + +jobs: + xs-galactic: + strategy: + matrix: + env: + - {ROS_DISTRO: galactic, ROS_REPO: main} + runs-on: ubuntu-20.04 + steps: + - uses: actions/checkout@v3 + with: + submodules: recursive + path: src + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/xs-humble.yaml b/.github/workflows/xs-humble.yaml new file mode 100644 index 0000000..4bf02f1 --- /dev/null +++ b/.github/workflows/xs-humble.yaml @@ -0,0 +1,36 @@ +name: build-xs-humble + +on: + push: + branches: + - humble + pull_request: + branches: + - humble + workflow_dispatch: + +defaults: + run: + shell: bash + +jobs: + xs-humble: + strategy: + matrix: + env: + - {ROS_DISTRO: humble, ROS_REPO: main} + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + with: + submodules: recursive + path: src/interbotix_ros_core + - name: Prepare Workspace + run: | + # rm src/interbotix_ros_core/interbotix_ros_common_drivers/COLCON_IGNORE + rm src/interbotix_ros_core/interbotix_ros_xseries/COLCON_IGNORE + - name: Install Dependencies + run: | + sudo apt-get install --reinstall pkg-config cmake-data + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/xs-melodic.yaml b/.github/workflows/xs-melodic.yaml index 82bb937..36ecf03 100644 --- a/.github/workflows/xs-melodic.yaml +++ b/.github/workflows/xs-melodic.yaml @@ -21,9 +21,9 @@ jobs: env: - {ROS_DISTRO: melodic, ROS_REPO: main, BUILDER: catkin_tools} - {ROS_DISTRO: melodic, ROS_REPO: main, BUILDER: catkin_make} - runs-on: ubuntu-latest + runs-on: ubuntu-18.04 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - name: Create src directory for xs run: | rm interbotix_ros_xseries/CATKIN_IGNORE diff --git a/.github/workflows/xs-noetic.yaml b/.github/workflows/xs-noetic.yaml index 5881bed..59839ba 100644 --- a/.github/workflows/xs-noetic.yaml +++ b/.github/workflows/xs-noetic.yaml @@ -21,9 +21,9 @@ jobs: env: - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_tools} - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_make} - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - name: Create src directory for xs run: | rm interbotix_ros_xseries/CATKIN_IGNORE diff --git a/.github/workflows/xs-rolling.yaml b/.github/workflows/xs-rolling.yaml new file mode 100644 index 0000000..f8c7892 --- /dev/null +++ b/.github/workflows/xs-rolling.yaml @@ -0,0 +1,32 @@ +name: build-xs-rolling + +on: + push: + branches: + - rolling + pull_request: + branches: + - rolling + workflow_dispatch: + +defaults: + run: + shell: bash + +jobs: + xs-rolling: + strategy: + matrix: + env: + - {ROS_DISTRO: rolling, ROS_REPO: main} + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + with: + submodules: recursive + path: src/interbotix_ros_core + - name: Prepare Workspace + run: | + rm src/interbotix_ros_core/interbotix_ros_xseries/COLCON_IGNORE + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.gitignore b/.gitignore index 6aa876a..5e9b9ba 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ *.pyc +__pycache__/ +*.egg-info/ + */matlab_msg_gen*/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..e49f002 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,8 @@ +[submodule "interbotix_ros_xseries/dynamixel_workbench_toolbox"] + path = interbotix_ros_xseries/dynamixel_workbench_toolbox + url = https://github.com/Interbotix/dynamixel-workbench.git + branch = ros2 +[submodule "interbotix_ros_xseries/interbotix_xs_driver"] + path = interbotix_ros_xseries/interbotix_xs_driver + url = https://github.com/Interbotix/interbotix_xs_driver.git + branch = v0.3.3 diff --git a/LICENSE b/LICENSE index 52be755..8ad75d2 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ -BSD 2-Clause License +BSD 3-Clause License -Copyright (c) 2020-2022, Interbotix +Copyright (c) 2023, Trossen Robotics All rights reserved. Redistribution and use in source and binary forms, with or without @@ -13,6 +13,10 @@ modification, are permitted provided that the following conditions are met: this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/README.md b/README.md index d623d7d..36a10ce 100644 --- a/README.md +++ b/README.md @@ -15,12 +15,17 @@ Coming soon... Support-level software can be found in the [interbotix_ros_toolboxes](https://github.com/Interbotix/interbotix_ros_toolboxes) repository -### Build Status +### CI Status -#### X-Series Core Build Status +#### X-Series Core CI -![build-xs-melodic Status](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-melodic.yaml/badge.svg) -![build-xs-noetic Status](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-noetic.yaml/badge.svg) +| ROS Distro | X-Series ROS Core Build | +| :------- | :------- | +| ROS 1 Melodic | [![build-xs-melodic](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-melodic.yaml/badge.svg)](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-melodic.yaml) | +| ROS 1 Noetic | [![build-xs-noetic](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-noetic.yaml/badge.svg)](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-noetic.yaml) | +| ROS 2 Galactic | [![build-xs-galactic](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-galactic.yaml/badge.svg)](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-galactic.yaml) | +| ROS 2 Humble | [![build-xs-humble](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-humble.yaml/badge.svg)](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-humble.yaml) | +| ROS 2 Rolling | [![build-xs-rolling](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-rolling.yaml/badge.svg)](https://github.com/Interbotix/interbotix_ros_core/actions/workflows/xs-rolling.yaml) | ## Repo Structure ``` diff --git a/interbotix_ros_uxarms/CATKIN_IGNORE b/interbotix_ros_common_drivers/COLCON_IGNORE similarity index 100% rename from interbotix_ros_uxarms/CATKIN_IGNORE rename to interbotix_ros_common_drivers/COLCON_IGNORE diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/README.md b/interbotix_ros_common_drivers/interbotix_ros_footswitch/README.md new file mode 100644 index 0000000..7f069d4 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/README.md @@ -0,0 +1,4 @@ +# Interbotix ROS Footswitch + +This subrepository contains a driver and interfaces for the three-pedal footswitch from PCsensor. +It requires the use of a device with Vendor ID `3553` and Product ID `b001`. diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/CMakeLists.txt b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/CMakeLists.txt new file mode 100644 index 0000000..1fa7c2b --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.12.0) +project(interbotix_footswitch_driver) + +list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake") + +find_package(ament_cmake REQUIRED) +find_package(interbotix_footswitch_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(HIDAPI REQUIRED) +find_package(LIBUSB 1 REQUIRED) + +include_directories( + include + ${HIDAPI_INCLUDE_DIRS} + ${LibUSB_INCLUDE_DIRS} +) + +set(ROS_DEPENDENCIES + interbotix_footswitch_msgs + rclcpp + rclcpp_components +) + +add_library(footswitch_driver + src/footswitch_driver.cpp +) + +ament_target_dependencies(footswitch_driver ${ROS_DEPENDENCIES}) + +target_link_libraries(footswitch_driver + ${HIDAPI_LIBRARIES} + ${LibUSB_LIBRARIES} +) + +add_executable(footswitch_driver_node + src/footswitch_driver_node.cpp +) + +target_link_libraries(footswitch_driver_node + footswitch_driver +) + +install( + TARGETS footswitch_driver + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + TARGETS + footswitch_driver_node + RUNTIME DESTINATION + lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindHIDAPI.cmake b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindHIDAPI.cmake new file mode 100644 index 0000000..a320ca8 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindHIDAPI.cmake @@ -0,0 +1,234 @@ +# https://github.com/rpavlik/cmake-modules/blob/1b450496c5d11fdcad8b000843d0c516e1eaa59f/FindHIDAPI.cmake + +#.rst: +# FindHIDAPI +# ---------- +# +# Try to find HIDAPI library, from http://www.signal11.us/oss/hidapi/ +# +# Cache Variables: (probably not for direct use in your scripts) +# HIDAPI_INCLUDE_DIR +# HIDAPI_LIBRARY +# +# Non-cache variables you might use in your CMakeLists.txt: +# HIDAPI_FOUND +# HIDAPI_INCLUDE_DIRS +# HIDAPI_LIBRARIES +# +# COMPONENTS +# ^^^^^^^^^^ +# +# This module respects several COMPONENTS specifying the backend you prefer: +# ``any`` (the default), ``libusb``, and ``hidraw``. +# The availablility of the latter two depends on your platform. +# +# +# IMPORTED Targets +# ^^^^^^^^^^^^^^^^ + +# This module defines :prop_tgt:`IMPORTED` target ``HIDAPI::hidapi`` (in all cases or +# if no components specified), ``HIDAPI::hidapi-libusb`` (if you requested the libusb component), +# and ``HIDAPI::hidapi-hidraw`` (if you requested the hidraw component), +# +# Result Variables +# ^^^^^^^^^^^^^^^^ +# +# ``HIDAPI_FOUND`` +# True if HIDAPI or the requested components (if any) were found. +# +# We recommend using the imported targets instead of the following. +# +# ``HIDAPI_INCLUDE_DIRS`` +# ``HIDAPI_LIBRARIES`` +# +# Original Author: +# 2009-2010, 2019 Ryan Pavlik +# http://academic.cleardefinition.com +# +# Copyright Iowa State University 2009-2010. +# Copyright Collabora, Ltd. 2019. +# Distributed under the Boost Software License, Version 1.0. +# (See accompanying file LICENSE_1_0.txt or copy at +# http://www.boost.org/LICENSE_1_0.txt) + +cmake_policy(SET CMP0045 NEW) +cmake_policy(SET CMP0053 NEW) +cmake_policy(SET CMP0054 NEW) + +set(HIDAPI_ROOT_DIR + "${HIDAPI_ROOT_DIR}" + CACHE PATH "Root to search for HIDAPI") + +# Clean up components +if("${HIDAPI_FIND_COMPONENTS}") + if(WIN32 OR APPLE) + # This makes no sense on Windows or Mac, which have native APIs + list(REMOVE "${HIDAPI_FIND_COMPONENTS}" libusb) + endif() + + if(NOT ${CMAKE_SYSTEM} MATCHES "Linux") + # hidraw is only on linux + list(REMOVE "${HIDAPI_FIND_COMPONENTS}" hidraw) + endif() +endif() +# if(NOT "${HIDAPI_FIND_COMPONENTS}") +# # Default to any +# set("${HIDAPI_FIND_COMPONENTS}" any) +# endif() + +# Ask pkg-config for hints +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + set(_old_prefix_path "${CMAKE_PREFIX_PATH}") + # So pkg-config uses HIDAPI_ROOT_DIR too. + if(HIDAPI_ROOT_DIR) + list(APPEND CMAKE_PREFIX_PATH ${HIDAPI_ROOT_DIR}) + endif() + pkg_check_modules(PC_HIDAPI_LIBUSB QUIET hidapi-libusb) + pkg_check_modules(PC_HIDAPI_HIDRAW QUIET hidapi-hidraw) + # Restore + set(CMAKE_PREFIX_PATH "${_old_prefix_path}") +endif() + +# Actually search +find_library( + HIDAPI_UNDECORATED_LIBRARY + NAMES hidapi + PATHS "${HIDAPI_ROOT_DIR}") + +find_library( + HIDAPI_LIBUSB_LIBRARY + NAMES hidapi hidapi-libusb + PATHS "${HIDAPI_ROOT_DIR}" + HINTS ${PC_HIDAPI_LIBUSB_LIBRARY_DIRS}) + +if(CMAKE_SYSTEM MATCHES "Linux") + find_library( + HIDAPI_HIDRAW_LIBRARY + NAMES hidapi-hidraw + HINTS ${PC_HIDAPI_HIDRAW_LIBRARY_DIRS}) +endif() + +find_path( + HIDAPI_INCLUDE_DIR + NAMES hidapi.h + PATHS "${HIDAPI_ROOT_DIR}" + PATH_SUFFIXES hidapi include include/hidapi + HINTS ${PC_HIDAPI_HIDRAW_INCLUDE_DIRS} ${PC_HIDAPI_LIBUSB_INCLUDE_DIRS}) + +find_package(Threads QUIET) + +### +# Compute the "I don't care which backend" library +### +set(HIDAPI_LIBRARY) + +# First, try to use a preferred backend if supplied +if("${HIDAPI_FIND_COMPONENTS}" MATCHES "libusb" + AND HIDAPI_LIBUSB_LIBRARY + AND NOT HIDAPI_LIBRARY) + set(HIDAPI_LIBRARY ${HIDAPI_LIBUSB_LIBRARY}) +endif() +if("${HIDAPI_FIND_COMPONENTS}" MATCHES "hidraw" + AND HIDAPI_HIDRAW_LIBRARY + AND NOT HIDAPI_LIBRARY) + set(HIDAPI_LIBRARY ${HIDAPI_HIDRAW_LIBRARY}) +endif() + +# Then, if we don't have a preferred one, settle for anything. +if(NOT HIDAPI_LIBRARY) + if(HIDAPI_LIBUSB_LIBRARY) + set(HIDAPI_LIBRARY ${HIDAPI_LIBUSB_LIBRARY}) + elseif(HIDAPI_HIDRAW_LIBRARY) + set(HIDAPI_LIBRARY ${HIDAPI_HIDRAW_LIBRARY}) + elseif(HIDAPI_UNDECORATED_LIBRARY) + set(HIDAPI_LIBRARY ${HIDAPI_UNDECORATED_LIBRARY}) + endif() +endif() + +### +# Determine if the various requested components are found. +### +set(_hidapi_component_required_vars) + +foreach(_comp IN LISTS HIDAPI_FIND_COMPONENTS) + if("${_comp}" STREQUAL "any") + list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR + HIDAPI_LIBRARY) + if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_LIBRARY}") + set(HIDAPI_any_FOUND TRUE) + mark_as_advanced(HIDAPI_INCLUDE_DIR) + else() + set(HIDAPI_any_FOUND FALSE) + endif() + + elseif("${_comp}" STREQUAL "libusb") + list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR + HIDAPI_LIBUSB_LIBRARY) + if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_LIBUSB_LIBRARY}") + set(HIDAPI_libusb_FOUND TRUE) + mark_as_advanced(HIDAPI_INCLUDE_DIR HIDAPI_LIBUSB_LIBRARY) + else() + set(HIDAPI_libusb_FOUND FALSE) + endif() + + elseif("${_comp}" STREQUAL "hidraw") + list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR + HIDAPI_HIDRAW_LIBRARY) + if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_HIDRAW_LIBRARY}") + set(HIDAPI_hidraw_FOUND TRUE) + mark_as_advanced(HIDAPI_INCLUDE_DIR HIDAPI_HIDRAW_LIBRARY) + else() + set(HIDAPI_hidraw_FOUND FALSE) + endif() + + else() + message(WARNING "${_comp} is not a recognized HIDAPI component") + set(HIDAPI_${_comp}_FOUND FALSE) + endif() +endforeach() +unset(_comp) + +### +# FPHSA call +### +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args( + HIDAPI REQUIRED_VARS ${_hidapi_component_required_vars} THREADS_FOUND + HANDLE_COMPONENTS) + +if(HIDAPI_FOUND) + set(HIDAPI_LIBRARIES "${HIDAPI_LIBRARY}") + set(HIDAPI_INCLUDE_DIRS "${HIDAPI_INCLUDE_DIR}") + if(NOT TARGET HIDAPI::hidapi) + add_library(HIDAPI::hidapi UNKNOWN IMPORTED) + set_target_properties( + HIDAPI::hidapi + PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "C" + IMPORTED_LOCATION ${HIDAPI_LIBRARY}) + set_property( + TARGET HIDAPI::hidapi PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES + Threads::Threads) + endif() +endif() + +if(HIDAPI_libusb_FOUND AND NOT TARGET HIDAPI::hidapi-libusb) + add_library(HIDAPI::hidapi-libusb UNKNOWN IMPORTED) + set_target_properties( + HIDAPI::hidapi-libusb + PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES "C" IMPORTED_LOCATION + ${HIDAPI_LIBUSB_LIBRARY}) + set_property(TARGET HIDAPI::hidapi-libusb + PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES Threads::Threads) +endif() + +if(HIDAPI_hidraw_FOUND AND NOT TARGET HIDAPI::hidapi-hidraw) + add_library(HIDAPI::hidapi-hidraw UNKNOWN IMPORTED) + set_target_properties( + HIDAPI::hidapi-hidraw + PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES "C" IMPORTED_LOCATION + ${HIDAPI_HIDRAW_LIBRARY}) + set_property(TARGET HIDAPI::hidapi-hidraw + PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES Threads::Threads) +endif() diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindLIBUSB.cmake b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindLIBUSB.cmake new file mode 100644 index 0000000..4de4c4c --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/cmake/FindLIBUSB.cmake @@ -0,0 +1,100 @@ +# https://github.com/ryanbinns/ttwatch/blob/f449ce60818454415a4a667b39654f0e5446a6b3/cmake_modules/FindLibUSB.cmake#L62 + +# - Try to find libusb-1.0 +# Once done this will define +# +# LIBUSB_1_FOUND - system has libusb +# LIBUSB_1_INCLUDE_DIRS - the libusb include directory +# LIBUSB_1_LIBRARIES - Link these to use libusb +# LIBUSB_1_DEFINITIONS - Compiler switches required for using libusb +# +# Adapted from cmake-modules Google Code project +# +# Copyright (c) 2006 Andreas Schneider +# +# (Changes for libusb) Copyright (c) 2008 Kyle Machulis +# +# Redistribution and use is allowed according to the terms of the New BSD license. +# +# CMake-Modules Project New BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the CMake-Modules Project nor the names of its +# contributors may be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# + + +if (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS) + # in cache already + set(LIBUSB_FOUND TRUE) +else (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS) + find_path(LIBUSB_1_INCLUDE_DIR + NAMES + libusb.h + PATHS + /usr/include + /usr/local/include + /opt/local/include + /sw/include + PATH_SUFFIXES + libusb-1.0 + ) + + find_library(LIBUSB_1_LIBRARY + NAMES + usb-1.0 usb + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + set(LIBUSB_1_INCLUDE_DIRS + ${LIBUSB_1_INCLUDE_DIR} + ) + set(LIBUSB_1_LIBRARIES + ${LIBUSB_1_LIBRARY} +) + + if (LIBUSB_1_INCLUDE_DIRS AND LIBUSB_1_LIBRARIES) + set(LIBUSB_1_FOUND TRUE) + endif (LIBUSB_1_INCLUDE_DIRS AND LIBUSB_1_LIBRARIES) + + if (LIBUSB_1_FOUND) + if (NOT libusb_1_FIND_QUIETLY) + message(STATUS "Found libusb-1.0:") + message(STATUS " - Includes: ${LIBUSB_1_INCLUDE_DIRS}") + message(STATUS " - Libraries: ${LIBUSB_1_LIBRARIES}") + endif (NOT libusb_1_FIND_QUIETLY) + else (LIBUSB_1_FOUND) + if (libusb_1_FIND_REQUIRED) + message(FATAL_ERROR "Could not find libusb") + endif (libusb_1_FIND_REQUIRED) + endif (LIBUSB_1_FOUND) + + # show the LIBUSB_1_INCLUDE_DIRS and LIBUSB_1_LIBRARIES variables only in the advanced view + mark_as_advanced(LIBUSB_1_INCLUDE_DIRS LIBUSB_1_LIBRARIES) + +endif (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS) diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/include/interbotix_footswitch_driver/footswitch_driver.hpp b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/include/interbotix_footswitch_driver/footswitch_driver.hpp new file mode 100644 index 0000000..2f329ad --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/include/interbotix_footswitch_driver/footswitch_driver.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_ +#define INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_ + +#include +#include +#include +#include +#include + +#include "hidapi/hidapi.h" +#include "interbotix_footswitch_msgs/msg/footswitch_state.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace footswitch_driver +{ + +const uint16_t VEND_ID = 0x3553; +const uint16_t PROD_ID = 0xb001; + +using interbotix_footswitch_msgs::msg::FootswitchState; +class FootSwitch : public rclcpp::Node +{ +public: + explicit FootSwitch(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~FootSwitch(); + +private: + rclcpp::TimerBase::SharedPtr tmr_update_state_; + hid_device * handle_; + rclcpp::Publisher::SharedPtr pub_footswitch_state_; + int update_period_ms_{100}; // ms + void update_state(); +}; + +} // namespace footswitch_driver + +#endif // INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_ diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/package.xml b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/package.xml new file mode 100644 index 0000000..15e0440 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/package.xml @@ -0,0 +1,26 @@ + + + + interbotix_footswitch_driver + 0.0.0 + + The interbotix_footswitch_driver package provides a ROS interface for three-pedal footswitches from PCsensor. + + Trossen Robotics + BSD-3-Clause + Trossen Robotics + + BSD-3-Clause + + ament_cmake + + interbotix_footswitch_msgs + libhidapi-dev + libusb-1.0-dev + rclcpp_components + rclcpp + + + ament_cmake + + diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver.cpp b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver.cpp new file mode 100644 index 0000000..e83846c --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver.cpp @@ -0,0 +1,141 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "interbotix_footswitch_driver/footswitch_driver.hpp" + +namespace footswitch_driver +{ + +FootSwitch::FootSwitch(const rclcpp::NodeOptions & options) +: rclcpp::Node("footswitch_node") +{ + // Initialize the hidapi library + hid_init(); + + // Open the device using the VID, PID + handle_ = hid_open(VEND_ID, PROD_ID, NULL); + if (!handle_) { + RCLCPP_FATAL( + get_logger(), + "Unable to open device with VID '%x' and PID '%x'. Exiting...", + VEND_ID, PROD_ID); + hid_exit(); + ::exit(1); + } + + // Set the hid_read() function to be non-blocking. + hid_set_nonblocking(handle_, 1); + + int update_rate; + declare_parameter("update_rate", 10); + get_parameter("update_rate", update_rate); + + update_period_ms_ = static_cast((1.0 / static_cast(update_rate)) * 1'000); + + pub_footswitch_state_ = create_publisher("state", 10); + + RCLCPP_INFO( + get_logger(), + "Publishing footswitch state on topic '%s'.", + pub_footswitch_state_->get_topic_name()); + + tmr_update_state_ = create_wall_timer( + std::chrono::milliseconds(update_period_ms_), + std::bind(&FootSwitch::update_state, this)); + + RCLCPP_INFO(get_logger(), "Initialized Footswitch driver."); +} + +FootSwitch::~FootSwitch() +{ + RCLCPP_INFO(get_logger(), "Exiting Footswitch driver."); + // Close the device + if (handle_) { + hid_close(handle_); + } + + // Finalize the hidapi library + hid_exit(); +} + +void FootSwitch::update_state() +{ + int res = 0; + unsigned char buf[65]; + int i = 0; + + // Continue to poll if no readings have been received + while (res == 0 && rclcpp::ok()) { + res = hid_read(handle_, buf, sizeof(buf)); + if (res < 0) { + RCLCPP_ERROR(get_logger(), "Unable to read from footswitch: %ls", hid_error(handle_)); + break; + } + + i++; + // Break after update period + if (i >= update_period_ms_) { + break; + } + + get_clock()->sleep_for(std::chrono::milliseconds(1)); + } + + FootswitchState state; + + if (res > 0) { + for (int i = 3; i < 6; i++) { + switch (buf[i]) { + case 0: + break; + case 4: + state.state[0] = true; + break; + case 5: + state.state[1] = true; + break; + case 6: + state.state[2] = true; + break; + + default: + break; + } + } + + state.header.stamp = now(); + pub_footswitch_state_->publish(state); + } +} + +} // namespace footswitch_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(footswitch_driver::FootSwitch) diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver_node.cpp b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver_node.cpp new file mode 100644 index 0000000..81d9e13 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver_node.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "interbotix_footswitch_driver/footswitch_driver.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto footswitch_node = std::make_shared(); + rclcpp::spin(footswitch_node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/CMakeLists.txt b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/CMakeLists.txt new file mode 100644 index 0000000..af27383 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(interbotix_footswitch_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +set(MSG_FILES + msg/FootswitchState.msg +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_FILES} + DEPENDENCIES std_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/msg/FootswitchState.msg b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/msg/FootswitchState.msg new file mode 100644 index 0000000..937df38 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/msg/FootswitchState.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +bool[3] state diff --git a/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/package.xml b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/package.xml new file mode 100644 index 0000000..d03dc46 --- /dev/null +++ b/interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/package.xml @@ -0,0 +1,29 @@ + + + + interbotix_footswitch_msgs + 0.0.0 + + interbotix_footswitch_msgs provides messages for three-pedal footswitches from PCsensor. + + Luke Schmitt + BSD-3-Clause + Luke Schmitt + + ament_cmake + + std_msgs + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_cmake + + + ament_cmake + + diff --git a/interbotix_ros_xseries/CATKIN_IGNORE b/interbotix_ros_slate/COLCON_IGNORE similarity index 100% rename from interbotix_ros_xseries/CATKIN_IGNORE rename to interbotix_ros_slate/COLCON_IGNORE diff --git a/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt b/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt new file mode 100644 index 0000000..a0c408b --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(interbotix_ros_slate) +find_package(ament_cmake REQUIRED) +unset(CATKIN_INSTALL_INTO_PREFIX_ROOT) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_slate/interbotix_ros_slate/README.md b/interbotix_ros_slate/interbotix_ros_slate/README.md new file mode 100644 index 0000000..eb17938 --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/README.md @@ -0,0 +1 @@ +# interbotix_ros_slate diff --git a/interbotix_ros_slate/interbotix_ros_slate/package.xml b/interbotix_ros_slate/interbotix_ros_slate/package.xml new file mode 100644 index 0000000..c1cd2ea --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/package.xml @@ -0,0 +1,21 @@ + + + + interbotix_ros_slate + 0.0.0 + The interbotix_ros_slate metapackage + Trossen Robotics + BSD-3-Clause + + ament_cmake + + interbotix_slate_driver + interbotix_slate_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt new file mode 100644 index 0000000..691bf11 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt @@ -0,0 +1,96 @@ +cmake_minimum_required(VERSION 3.10.0) +project(interbotix_slate_driver) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_BUILD_TYPE "Release") + +set(serial_driver "chassis_driver") + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interbotix_slate_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +include_directories(include) + +set(ROS_DEPENDENCIES + geometry_msgs + interbotix_slate_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2_geometry_msgs + tf2_ros +) + +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") + set(ARCH "x86_64") +elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") + set(ARCH "aarch64") +else() + message(FATAL_ERROR "Unknown System Architecture: ${CMAKE_SYSTEM_PROCESSOR}") +endif() + +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib/${ARCH}) + +add_library(slate_base + src/slate_base.cpp + src/base_driver.cpp +) + +ament_target_dependencies(slate_base ${ROS_DEPENDENCIES}) + +target_link_libraries(slate_base + ${serial_driver} +) + +add_executable(slate_base_node + src/slate_base_node.cpp +) + +target_link_libraries(slate_base_node + slate_base +) + +install( + TARGETS slate_base + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + TARGETS + slate_base_node + RUNTIME DESTINATION + lib/${PROJECT_NAME} +) + +install( + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/lib/${ARCH}/lib${serial_driver}.so + DESTINATION + lib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_slate/interbotix_slate_driver/README.md b/interbotix_ros_slate/interbotix_slate_driver/README.md new file mode 100644 index 0000000..b75584f --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/README.md @@ -0,0 +1 @@ +# interbotix_slate_driver diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp new file mode 100644 index 0000000..6a2aedd --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp @@ -0,0 +1,71 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ +#define INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ + +#include + +#include "interbotix_slate_driver/serial_driver.hpp" + + +namespace base_driver +{ + +typedef struct +{ + float cmd_vel_x; + float cmd_vel_y; + float cmd_vel_z; + uint32_t light_state; + + uint32_t system_state; + uint32_t charge; + float voltage; + float current; + float vel_x; + float vel_y; + float vel_z; + float odom_x; + float odom_y; + float odom_z; + uint32_t cmd; + uint32_t io; + uint32_t err; +} ChassisData; + +bool chassisInit(std::string &dev); +bool getVersion(char *data); +bool setText(const char *text); +bool updateChassisInfo(ChassisData *data); +bool setSysCmd(uint32_t cmd); +bool setIo(uint32_t io); + +} // namespace base_driver + +#endif // INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp new file mode 100644 index 0000000..3171d32 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp @@ -0,0 +1,84 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ +#define INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ + +#include + +#include +#include + +typedef enum +{ + SYS_INIT = 0x00, + SYS_NORMAL, + SYS_REMOTE, + SYS_ESTOP, + SYS_CALIB, + SYS_TEST, + SYS_CHARGING, + + SYS_ERR = 0x10, + SYS_ERR_ID, + SYS_ERR_COM, + SYS_ERR_ENC, + SYS_ERR_COLLISION, + SYS_ERR_LOW_VOLTAGE, + SYS_ERR_OVER_VOLTAGE, + SYS_ERR_OVER_CURRENT, + SYS_ERR_OVER_TEMP, + + SYS_STATE_LEN, +} SystemState; + +class SerialDriver +{ + public: + ~SerialDriver(); + bool init(std::string& port, uint8_t id, int baud = B115200); + + bool getVersion(char* version); + bool setText(const char* text); + + int readHoldingRegs(uint16_t addr, uint16_t cnt, uint16_t* data); + int writeHoldingRegs(uint16_t addr, uint16_t cnt, uint16_t* data); + int readWriteHoldingRegs(uint16_t raddr, uint16_t rcnt, uint16_t* rdata, + uint16_t waddr, uint16_t wcnt, uint16_t* wdata); + + int send(const void* data, int len, int timeout = 0); + int recv(uint8_t* data, int maxlen, int timeout = 0); + + private: + void* m = nullptr; + int fd_ = -1; + fd_set read_set_; + std::mutex lock; +}; + +#endif // INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp new file mode 100644 index 0000000..8cd3353 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp @@ -0,0 +1,241 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ +#define INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ + +#include +#include + +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "interbotix_slate_msgs/srv/set_light_state.hpp" +#include "interbotix_slate_msgs/srv/set_string.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "interbotix_slate_driver/base_driver.hpp" +#include "interbotix_slate_driver/serial_driver.hpp" + +namespace slate_base +{ + +#define CMD_TIME_OUT 300 // ms +#define PORT "chassis" + +using geometry_msgs::msg::Quaternion; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Twist; +using interbotix_slate_msgs::srv::SetLightState; +using interbotix_slate_msgs::srv::SetString; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::BatteryState; +using std_srvs::srv::SetBool; + +class SlateBase : public rclcpp::Node +{ +public: + /** + * @brief Constructor for the SlateBase + * @param options ROS NodeOptions + */ + explicit SlateBase(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// @brief Destructor for the SlateBase + ~SlateBase() {} + + /// @brief Process velocity commands and update robot state + void update(); + +private: + // Linear velocity command + float cmd_vel_x_; + + // Angular velocity command + float cmd_vel_z_; + + // Time last velocity command was received + rclcpp::Time cmd_vel_time_last_update_; + + // Odometry publisher + rclcpp::Publisher::SharedPtr pub_odom_; + + // BatteryState publisher + rclcpp::Publisher::SharedPtr pub_battery_state_; + + // Twist command subscriber + rclcpp::Subscription::SharedPtr sub_cmd_vel_; + + // Set screen text service server + rclcpp::Service::SharedPtr srv_set_text_; + + // Motor status service server + rclcpp::Service::SharedPtr srv_motor_torque_status_; + + // Set charge enable service server + rclcpp::Service::SharedPtr srv_enable_charging_; + + // Set light state service server + rclcpp::Service::SharedPtr srv_set_light_state_; + + // Name of odom frame + std::string odom_frame_name_; + + // Name of base frame + std::string base_frame_name_; + + // Update counter used to only update some values less frequently + int cnt_; + + // Odometry translation in the x-direction in meters + float x_; + + // Odometry translation in the y-direction in meters + float y_; + + // Odometry rotation about the z-axis in radians + float theta_; + + // Odometry forward velocity in meters per second + float x_vel_; + + // Odometry rotational velocity about the z-axis in radians per second + float z_omega_; + + // Whether or not we have received our first odometry update + bool is_first_odom_; + + // Array containing x and y translation in meters and rotation in radians + float pose_[3]; + + // Current of the right motor in Amps + float right_motor_c_; + + // Current of the left motor in Amps + float left_motor_c_; + + // The system state of the base + SystemState chassis_state_; + + // Whether or not to publish TF from base_link->odom + bool publish_tf_; + + // Max linear velocity in the x-direction in meters per second + float max_vel_x_ = 1.0; + + // Max angular velocity about the z-axis in radians per second + float max_vel_z_ = 1.0; + + // Base command bytes containing data about charging and motor torque enabling + uint32_t sys_cmd_ = 0; + + // Base light state - see interbotix_slate_msgs/srv/SetLightState for details + uint32_t light_state_ = 0; + + // If publish_tf_ is true, this is the broadcaster used to publish the odom->base_link TF + tf2_ros::TransformBroadcaster tf_broadcaster_odom_; + + // Time of the current update + rclcpp::Time current_time_; + + // Time of the last update + rclcpp::Time last_time_; + + // Timeout for base velocity + rclcpp::Duration cmd_vel_timeout_; + + /** + * @brief Process incoming Twist command message + * @param msg Incoming Twist command message + */ + void cmd_vel_callback(const Twist::SharedPtr msg); + + /** + * @brief Process incoming screen text service request + * @param request_header Incoming RMW request identifier + * @param req Service request containing desired text + * @param res[out] Service response containing a success indication and a message + * @return true if service succeeded, false otherwise + */ + bool set_text_callback( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /** + * @brief Process incoming motor torque status service request + * @param request_header Incoming RMW request identifier + * @param req Service request containing desired motor torque status + * @param res[out] Service response containing a success indication and a message + * @return true if service succeeded, false otherwise + */ + bool motor_torque_status_callback( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /** + * @brief Process incoming enable charging service request + * @param request_header Incoming RMW request identifier + * @param req Service request containing desired charging enable status + * @param res[out] Service response containing a success indication and a message + * @return true if service succeeded, false otherwise + */ + bool enable_charging_callback( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /** + * @brief Process incoming set light state service request + * @param request_header Incoming RMW request identifier + * @param req Service request containing desired light state + * @param res[out] Service response containing a success indication and a message + * @return true if service succeeded, false otherwise + */ + bool set_light_state_callback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + const std::shared_ptr res); + + /** + * @brief Wrap angle + * @param angle Angle to wrap in radians + * @return Wrapped angle + */ + float wrap_angle(float angle); +}; + +} // namespace slate_base + +#endif // INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so b/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so new file mode 100644 index 0000000..30779ed Binary files /dev/null and b/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so differ diff --git a/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so b/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so new file mode 100644 index 0000000..b2f592b Binary files /dev/null and b/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so differ diff --git a/interbotix_ros_slate/interbotix_slate_driver/package.xml b/interbotix_ros_slate/interbotix_slate_driver/package.xml new file mode 100644 index 0000000..7214c39 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/package.xml @@ -0,0 +1,33 @@ + + + + interbotix_slate_driver + 0.0.0 + The interbotix_slate_driver package + + Trossen Robotics + + BSD-3-Clause + + ament_cmake + + geometry_msgs + interbotix_slate_msgs + libudev-dev + nav_msgs + rclcpp + sensor_msgs + std_srvs + tf2_geometry_msgs + tf2_ros + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp new file mode 100644 index 0000000..9111648 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp @@ -0,0 +1,74 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "rclcpp/rclcpp.hpp" + +#include "interbotix_slate_driver/base_driver.hpp" +#include "interbotix_slate_driver/serial_driver.hpp" + +namespace base_driver +{ + +#define MAX_TIMEOUT_CNT 50 + +uint32_t err_cnt = 0; +SerialDriver driver; + +bool chassisInit(std::string &dev) { return driver.init(dev, 1, B115200); } + +bool check(int ret) +{ + err_cnt += (ret == 0 ? 0 : 1); + if (err_cnt > MAX_TIMEOUT_CNT) + { + std::string dev; + err_cnt = 0; + } + return !ret; +} + +bool getVersion(char *data) { return driver.getVersion(data); } +bool setText(const char *text) { return driver.setText(text); } + +bool updateChassisInfo(ChassisData *data) +{ + return check(driver.readWriteHoldingRegs(0x00, 26, (uint16_t *)&(data->system_state), + 0x40, 8, (uint16_t *)data)); +} + +bool setSysCmd(uint32_t cmd) +{ + return check(driver.writeHoldingRegs(0x14, 2, (uint16_t *)&cmd)); +} + +bool setIo(uint32_t io) +{ + return check(driver.writeHoldingRegs(0x16, 2, (uint16_t *)&io)); +} + +} // namespace base_driver diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp new file mode 100644 index 0000000..90da2f2 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp @@ -0,0 +1,271 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "interbotix_slate_driver/slate_base.hpp" + +#include + +namespace slate_base +{ + +SlateBase::SlateBase(const rclcpp::NodeOptions & options) +: rclcpp::Node("slate_base", "", options), cmd_vel_x_(0.0), cmd_vel_z_(0.0), cnt_(0), x_(0.0), + y_(0.0), theta_(0.0), x_vel_(0.0), z_omega_(0.0), is_first_odom_(true), pose_{0}, + right_motor_c_(0.0), left_motor_c_(0.0), chassis_state_(SystemState::SYS_INIT), + publish_tf_(false), max_vel_x_(1.0), max_vel_z_(1.0), current_time_(get_clock()->now()), + last_time_(get_clock()->now()), tf_broadcaster_odom_(this), + cmd_vel_time_last_update_(get_clock()->now()), + cmd_vel_timeout_(rclcpp::Duration(std::chrono::milliseconds(CMD_TIME_OUT))) +{ + using std::placeholders::_1, std::placeholders::_2, std::placeholders::_3; + + declare_parameter("publish_tf", false); + declare_parameter("odom_frame_name", "odom"); + declare_parameter("base_frame_name", "base_link"); + + get_parameter("publish_tf", publish_tf_); + get_parameter("odom_frame_name", odom_frame_name_); + get_parameter("base_frame_name", base_frame_name_); + + pub_odom_ = create_publisher("odom", 50); + pub_battery_state_ = create_publisher("battery_state", 1); + + sub_cmd_vel_ = create_subscription( + "cmd_vel", + 1, + std::bind(&SlateBase::cmd_vel_callback, this, _1)); + + srv_set_text_ = create_service( + "set_text", + std::bind(&SlateBase::set_text_callback, this, _1, _2, _3)); + + srv_motor_torque_status_ = create_service( + "set_motor_torque_status", + std::bind(&SlateBase::motor_torque_status_callback, this, _1, _2, _3)); + + srv_enable_charging_ = create_service( + "enable_charging", + std::bind(&SlateBase::enable_charging_callback, this, _1, _2, _3)); + + srv_set_light_state_ = create_service( + "set_light_state", + std::bind(&SlateBase::set_light_state_callback, this, _1, _2, _3)); + + std::string dev; + if (!base_driver::chassisInit(dev)) { + RCLCPP_ERROR(get_logger(), "Failed to initialize base port."); + ::exit(EXIT_FAILURE); + } + RCLCPP_INFO(get_logger(), "Initalized base at port '%s'.", dev.c_str()); + char version[32] = {0}; + if (base_driver::getVersion(version)) { + RCLCPP_INFO(get_logger(), "Base version: %s", version); + } +} + +void SlateBase::update() +{ + rclcpp::spin_some(get_node_base_interface()); + current_time_ = get_clock()->now(); + + // time out velocity commands + if (current_time_ - cmd_vel_time_last_update_ > cmd_vel_timeout_) { + cmd_vel_x_ = 0.0f; + cmd_vel_z_ = 0.0f; + } + + // limit velocity commands + cmd_vel_x_ = std::min(max_vel_x_, std::max(-max_vel_x_, cmd_vel_x_)); + cmd_vel_z_ = std::min(max_vel_z_, std::max(-max_vel_z_, cmd_vel_z_)); + + // initialize chassis data and use it to update the driver + base_driver::ChassisData data = { + .cmd_vel_x=cmd_vel_x_, + .cmd_vel_y=0.0, + .cmd_vel_z=cmd_vel_z_, + .light_state=light_state_, + .system_state=0}; + + if (!base_driver::updateChassisInfo(&data)) { + return; + } + + // extract and update base system command bytes + sys_cmd_ = data.cmd; + + // update battery state every 10 iterations + cnt_++; + auto battery_state = BatteryState(); + if (cnt_ % 10 == 0) { + battery_state.header.stamp = current_time_; + battery_state.voltage = data.voltage; + battery_state.current = data.current; + battery_state.percentage = data.charge; + battery_state.power_supply_status = data.system_state; + pub_battery_state_->publish(battery_state); + } + + // update odometry values + x_vel_ = data.vel_x; + z_omega_ = data.vel_z; + + x_ = data.odom_x; + y_ = data.odom_y; + theta_ = data.odom_z; + + if (is_first_odom_) { + pose_[0] = x_; + pose_[1] = y_; + pose_[2] = theta_; + is_first_odom_ = false; + } + + // create transform + tf2::Quaternion q; + q.setRPY(0, 0, wrap_angle(theta_ - pose_[2])); + auto odom_quat = tf2::toMsg(q); + auto odom_trans = TransformStamped(); + odom_trans.header.stamp = current_time_; + odom_trans.header.frame_id = odom_frame_name_; + odom_trans.child_frame_id = base_frame_name_; + + odom_trans.transform.translation.x = + cos(-pose_[2]) * (x_ - pose_[0]) - sin(-pose_[2]) * (y_ - pose_[1]); + odom_trans.transform.translation.y = + sin(-pose_[2]) * (x_ - pose_[0]) + cos(-pose_[2]) * (y_ - pose_[1]); + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + // send the transform + if (publish_tf_) { + tf_broadcaster_odom_.sendTransform(odom_trans); + } + + // publish odometry + auto odom = Odometry(); + odom.header.stamp = current_time_; + odom.header.frame_id = odom_frame_name_; + + // set position + odom.pose.pose.position.x = odom_trans.transform.translation.x; + odom.pose.pose.position.y = odom_trans.transform.translation.y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + odom.pose.covariance[0] = (data.system_state == SystemState::SYS_ESTOP) ? -1 : 1; + + // set velocity + odom.child_frame_id = base_frame_name_; + odom.twist.twist.linear.x = x_vel_; + odom.twist.twist.linear.y = 0; + odom.twist.twist.angular.z = z_omega_; + + pub_odom_->publish(odom); + last_time_ = current_time_; +} + +void SlateBase::cmd_vel_callback(const Twist::SharedPtr msg) +{ + cmd_vel_x_ = msg->linear.x; + cmd_vel_z_ = msg->angular.z; + cmd_vel_time_last_update_ = get_clock()->now(); +} + +bool SlateBase::set_text_callback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + const std::shared_ptr res) +{ + res->message = "Set base screen text to: '" + req->data + "'."; + base_driver::setText(req->data.c_str()); + RCLCPP_INFO(get_logger(), res->message.c_str()); + res->success = true; + return true; +} + +bool SlateBase::motor_torque_status_callback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + const std::shared_ptr res) +{ + req->data ? sys_cmd_ &= ~(1): sys_cmd_ |= 1; + res->success = base_driver::setSysCmd(sys_cmd_); + + std::string enabled_disabled = req->data ? "enable" : "disable"; + if (res->success) { + res->message = "Successfully " + enabled_disabled + "d motor torque."; + RCLCPP_INFO(get_logger(), res->message.c_str()); + } else { + res->message = "Failed to " + enabled_disabled + " motor torque."; + RCLCPP_ERROR(get_logger(), res->message.c_str()); + } + return res->success; +} + +bool SlateBase::enable_charging_callback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + const std::shared_ptr res) +{ + req->data ? sys_cmd_ |= 2 : sys_cmd_ &= ~(2); + res->success = base_driver::setSysCmd(sys_cmd_); + + std::string enabled_disabled = req->data ? "enable" : "disable"; + if (res->success) { + res->message = "Successfully " + enabled_disabled + "d charging."; + RCLCPP_INFO(get_logger(), res->message.c_str()); + } else { + res->message = "Failed to " + enabled_disabled + " charging."; + RCLCPP_ERROR(get_logger(), res->message.c_str()); + } + return res->success; +} + +bool SlateBase::set_light_state_callback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + const std::shared_ptr res) +{ + res->message = "Set light state to: '" + std::to_string(req->light_state) + "'."; + light_state_ = req->light_state; + RCLCPP_INFO(get_logger(), res->message.c_str()); + res->success = true; + return true; +} + +float SlateBase::wrap_angle(float angle) +{ + if (angle > M_PI) { + angle = angle - 2.0 * M_PI; + } else if (angle < -M_PI) { + angle = angle + 2.0 * M_PI; + } + return angle; +} + +} // namespace slate_base diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp new file mode 100644 index 0000000..3000759 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp @@ -0,0 +1,43 @@ +// Copyright 2024 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "interbotix_slate_driver/slate_base.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + auto r = rclcpp::Rate(20); + while (rclcpp::ok()) { + node->update(); + r.sleep(); + } +} diff --git a/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt b/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt new file mode 100644 index 0000000..6dacddc --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.10.0) +project(interbotix_slate_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +set(SRV_FILES + srv/SetLightState.srv + srv/SetString.srv +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${SRV_FILES} + DEPENDENCIES + std_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_slate/interbotix_slate_msgs/README.md b/interbotix_ros_slate/interbotix_slate_msgs/README.md new file mode 100644 index 0000000..8d2db31 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/README.md @@ -0,0 +1 @@ +# interbotix_slate_msgs diff --git a/interbotix_ros_slate/interbotix_slate_msgs/package.xml b/interbotix_ros_slate/interbotix_slate_msgs/package.xml new file mode 100644 index 0000000..0014d18 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/package.xml @@ -0,0 +1,23 @@ + + + + interbotix_slate_msgs + 0.0.0 + The interbotix_slate_msgs package + + Trossen Robotics + + BSD-3-Clause + + ament_cmake + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/interbotix_ros_slate/interbotix_slate_msgs/srv/SetLightState.srv b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetLightState.srv new file mode 100644 index 0000000..ce58a80 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetLightState.srv @@ -0,0 +1,22 @@ +uint32 LIGHT_STATE_OFF=0 +uint32 LIGHT_STATE_RED=1 +uint32 LIGHT_STATE_GREEN=2 +uint32 LIGHT_STATE_YELLOW=3 +uint32 LIGHT_STATE_BLUE=4 +uint32 LIGHT_STATE_PURPLE=5 +uint32 LIGHT_STATE_CYAN=6 +uint32 LIGHT_STATE_WHITE=7 + +uint32 LIGHT_STATE_RED_FLASH=9 +uint32 LIGHT_STATE_GREEN_FLASH=10 +uint32 LIGHT_STATE_YELLOW_FLASH=11 +uint32 LIGHT_STATE_BLUE_FLASH=12 +uint32 LIGHT_STATE_PURPLE_FLASH=13 +uint32 LIGHT_STATE_CYAN_FLASH=14 +uint32 LIGHT_STATE_WHITE_FLASH=15 + +# Desired light state +uint32 light_state 0 +--- +bool success +string message diff --git a/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv new file mode 100644 index 0000000..b0de4d8 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv @@ -0,0 +1,4 @@ +string data +--- +bool success +string message diff --git a/interbotix_ros_uxarms/README.md b/interbotix_ros_uxarms/README.md deleted file mode 100644 index 857291e..0000000 --- a/interbotix_ros_uxarms/README.md +++ /dev/null @@ -1,48 +0,0 @@ -# UFactory xArm Driver ROS Packages -![xarm_api_banner](images/xarm_api_banner.png) - -## Overview -This folder contains the actual driver package and custom message descriptions needed for controlling the UFactory xArm platforms sold by Interbotix. These packages were copied straight from the [xarm_ros](https://github.com/xArm-Developer/xarm_ros) repository currently maintained by the xArm Developers. The [xarm_driver_node](xarm_api/src/xarm_driver.cpp) was written in C++ and is responsible for publishing joint states and advertising ROS Service Servers to control the arm. The [xarm_ros_client](xarm_api/src/xam_ros_client.cpp) was also written in C++ and presents C++ wrappers around many ROS Service Clients. Thus, there are two ways to control the robot. One is to command the robot via the ROS topics and/or services directly. In this manner, the developer can code in any language that is capable of sending a ROS message. The other approach is to 'skip' the ROS topic layer and use the publicly available functions directly. All the user would need to do is create an instance of the 'XArmROSClient' class as shown [here](xarm_api/src/xarm_ros_client.cpp) to take advantage of these functions. - -## Structure - -The *xarm_api* package only contains one driver node called **xarm_driver_node**. As mentioned previously, this node is responsible for controlling the physical robot. Please look below for a description of the ROS topics, services, and parameters available to the user. - -##### Publishers -- `//joint_states` - publishes ROS [JointState](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/JointState.html) messages at a set rate of 5 Hz; note that only the 'position' field is valid and is given in radians. -- `//xarm_states` - publishes custom [RobotMsg](xarm_msgs/msg/RobotMsg.msg) messages describing the control state of the robot. Refer to the message description for details. - -##### Subscribers -- `//sleep_sec` - subscribes to [Float32](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Float32.html) messages; it is unclear what this topic is used for... - -##### Services -- `//motion_ctrl` - service to enable/disable the specified joint or all joints; refer to the [SetAxis](xarm_msgs/srv/SetAxis.srv) service description for implementation details. -- `//set_mode` - service to set the operating mode - can either be 0 (Pose Mode), 1 (Servo Mode), or 2 (Teach Mode); Pose Mode allows the firmware in the Control Box to do motion planning when moving joints; Servo Mode bypasses the internal motion planning in the firmware, which is useful when a user wants to design their own motion planning system; Teach Mode makes it easy to manually move the robot; refer to the [SetInt16](xarm_msgs/srv/SetInt16.srv) service description for implementation details. -- `//set_state` - service to set the robot state - can either be 0 (Start State), 3 (Pause State), or 4 (Stop State); refer to the [SetInt16](xarm_msgs/srv/SetInt16.srv) service description for implementation details. -- `//set_tcp_offset` - service to set an end-effector offset relative to the last link on the arm; any Service that moves the end-effector in Cartesian space will take this offset into account; refer to the [TCPOffset](xarm_msgs/srv/TCPOffset.srv) service description for implementation details. -- `//set_load` - service to set the load seen at the end-effector; refer to the [SetLoad](xarm_msgs/srv/SetLoad.srv) service description for implementation details. -- `//go_home` - service to make the arm go to its Home Pose (all joints have a value of 0 rad); only works in Mode 0; refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details; note that the 'pose' field should be left blank. -- `//move_joint` - service to move all the robot's joints to user-specified positions; only works in Mode 0; see more at [5.7 xarm_api/xarm_msgs](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs); refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details. -- `//move_line` - service to move the end-effector of the arm in a straight line to a specified pose; only works in Mode 0; see more at [5.7 xarm_api/xarm_msgs](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs); refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details. -- `//move_lineb` - service to move the end-effector of the arm in Cartesian Space through a list of user-specified via points; only works in Mode 0; see more at [5.7 xarm_api/xarm_msgs](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs); refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details. -- `//move_servoj` - service to move all the robot's joints rapidly (with no motion planning) to user-specified positions; only works in Mode 1; see more at [5.7 xarm_api/xarm_msgs](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs); refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details. -- `//move_servo_cart` - service to move the end-effector of the arm in a straight line to a specified pose rapidly (with no motion planning); only works in Mode 1; see more at [5.7 xarm_api/xarm_msgs](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs); refer to the [Move](xarm_msgs/srv/Move.srv) service description for implementation details. -- `//get_err` - service to get the current error state; refer to the [GetErr](xarm_msgs/srv/GetErr.srv) service description for implementation details; note that a value of 0 means that the robot is not in an error state. -- `//clear_err` - service to reset the error state to 0; refer to the [ClearErr](xarm_msgs/srv/ClearErr.srv) service description for implementation details. -- `//gripper_config` - service to configure the gripper's speed; refer to the [GripperConfig](xarm_msgs/srv/GripperConfig.srv) service description for implementation details. -- `//gripper_move` - service to move the gripper; refer to the [GripperMove](xarm_msgs/srv/GripperMove.srv) service description for implementation details. -- `//gripper_state` - service to get the current gripper's state (range from 0 - 850); refer to the [GripperState](xarm_msgs/srv/GripperState.srv) service description for implementation details. - -Note that there are quite a few other services besides these that deal with reading digital or analog pins on the control box and the like. See more info at the [xarm_ros repository](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs). Note that any ROS Service defined there will also work here, but top layer packages (like their version of MoveIt or xArm Planner packages will not). - -##### Parameters - -- `//DOF` - degrees of freedom of the arm -- `//joint_names` - list of joint names for the robot -- `//xarm_robot_ip` - IP Address of the Control Box; typically 192.168.1.XXX -- `//wait_for_finish` - For ROS Services that operate in Mode 0, setting this to `True` will make those Services block until the motion is complete - - -## Usage - -To comply with the IRROS standard, the driver package contains no launch or config files (to keep it robot agnostic). The **xarm_driver_node** within it is meant to be included within the launch file located in any *interbotix_XXXXX_control* package (and passed the robot-specific config files). While it's obvious that this node can only be used to control arms, the decision was made to keep the node here and not in the *interbotix_ros_uxarms* sub-repo for consistency purposes. Also, the packages were not modified in any way so that updating them will be much easier. diff --git a/interbotix_ros_uxarms/images/xarm_api_banner.png b/interbotix_ros_uxarms/images/xarm_api_banner.png deleted file mode 100644 index 5e4e829..0000000 Binary files a/interbotix_ros_uxarms/images/xarm_api_banner.png and /dev/null differ diff --git a/interbotix_ros_uxarms/xarm_api/CMakeLists.txt b/interbotix_ros_uxarms/xarm_api/CMakeLists.txt deleted file mode 100755 index 4f38020..0000000 --- a/interbotix_ros_uxarms/xarm_api/CMakeLists.txt +++ /dev/null @@ -1,274 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(xarm_api) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - std_srvs - genmsg - xarm_msgs - sensor_msgs -) - -set(XARM_API_DIR src/xarm) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES xarm_api xarm_ros_client - CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(xarm_api - ${XARM_API_DIR}/connect.cc - ${XARM_API_DIR}/report_data.cc - - ${XARM_API_DIR}/common/crc16.cc - ${XARM_API_DIR}/common/queue_memcpy.cc - - ${XARM_API_DIR}/debug/debug_print.cc - - ${XARM_API_DIR}/instruction/uxbus_cmd.cc - ${XARM_API_DIR}/instruction/uxbus_cmd_ser.cc - ${XARM_API_DIR}/instruction/uxbus_cmd_tcp.cc - - ${XARM_API_DIR}/linux/network.cc - ${XARM_API_DIR}/linux/thread.cc - - ${XARM_API_DIR}/port/serial.cc - ${XARM_API_DIR}/port/socket.cc - - ) - -add_library(xarm_ros_client - src/xarm_ros_client.cpp -) -# Make sure xarm_msgs header files are generated before building xarm_ros_client -add_dependencies(xarm_ros_client xarm_msgs_generate_messages_cpp) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(xarm_api ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(example1_report_norm_node - test/example1_report_norm.cc -) - -add_executable(xarm_driver_node - src/xarm_driver_node.cpp - src/xarm_driver.cpp -) - -add_executable(move_test - test/move_test.cpp -) - -add_executable(test_tool_modbus - test/test_tool_modbus.cpp -) - -add_executable(servo_cart_test - test/servo_cartesian_test.cpp -) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - -## Specify libraries to link a library or executable target against - target_link_libraries(example1_report_norm_node - xarm_api - ${catkin_LIBRARIES} - ) - - target_link_libraries(xarm_driver_node - xarm_api - ${catkin_LIBRARIES} - ) - -target_link_libraries(move_test - xarm_api - ${catkin_LIBRARIES} - ) - -target_link_libraries(test_tool_modbus - xarm_api - ${catkin_LIBRARIES} - xarm_ros_client - ) - -target_link_libraries(servo_cart_test - xarm_api - ${catkin_LIBRARIES} - ) - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -catkin_install_python(PROGRAMS scripts/servo_cartesian_test.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark executables and/or libraries for installation -install(TARGETS xarm_api xarm_ros_client - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -## Mark cpp header files for installation -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE - ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_xarm_api.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/common/crc16.h b/interbotix_ros_uxarms/xarm_api/include/xarm/common/crc16.h deleted file mode 100755 index 5367667..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/common/crc16.h +++ /dev/null @@ -1,14 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef COMMON_CRC16_H_ -#define COMMON_CRC16_H_ - -#include "xarm/common/data_type.h" - -int modbus_crc(unsigned char *data, int len); - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/common/data_type.h b/interbotix_ros_uxarms/xarm_api/include/xarm/common/data_type.h deleted file mode 100755 index 5cdc451..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/common/data_type.h +++ /dev/null @@ -1,112 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef COMMON_DATA_TYPE_H_ -#define COMMON_DATA_TYPE_H_ - -#include - -inline void bin64_to_8(long long a, unsigned char* b) { - b[0] = (unsigned char)(a >> 56); - b[1] = (unsigned char)(a >> 48); - b[2] = (unsigned char)(a >> 40); - b[3] = (unsigned char)(a >> 32); - b[4] = (unsigned char)(a >> 24); - b[5] = (unsigned char)(a >> 16); - b[6] = (unsigned char)(a >> 8); - b[7] = (unsigned char)a; -} - -inline void bin32_to_8(int a, unsigned char *b) { - b[0] = (unsigned char)(a >> 24); - b[1] = (unsigned char)(a >> 16); - b[2] = (unsigned char)(a >> 8); - b[3] = (unsigned char)a; -} - -inline void bin16_to_8(int a, unsigned char *b) { - unsigned short temp = a; - b[0] = (unsigned char)(temp >> 8); - b[1] = (unsigned char)temp; -} - -inline int bin8_to_32(unsigned char *a) { - return ((a[0] << 24) + (a[1] << 16) + (a[2] << 8) + a[3]); -} - -inline int bin8_to_16(unsigned char *a) { - signed int tmp = (a[0] << 8) + a[1]; - return tmp; -} - -inline int bin8_to_s16(unsigned char *a) { - return (int)(short)(a[0] << 8) + a[1]; -} - -inline void bin8_to_ns16(unsigned char *a, int *data, int n) { - for (int i = 0; i < n; ++i) { - data[i] = bin8_to_s16(&a[i * 2]); - } -} - -inline void fp32_to_hex(double dataf, unsigned char datahex[4]) { - union _fp32hex { - float dataf; - unsigned char datahex[4]; - } fp32hex; - fp32hex.dataf = (float)dataf; - datahex[0] = fp32hex.datahex[0]; - datahex[1] = fp32hex.datahex[1]; - datahex[2] = fp32hex.datahex[2]; - datahex[3] = fp32hex.datahex[3]; -} - -inline float hex_to_fp32(unsigned char datahex[4]) { - union _fp32hex { - float dataf; - unsigned char datahex[4]; - } fp32hex; - fp32hex.datahex[0] = datahex[0]; - fp32hex.datahex[1] = datahex[1]; - fp32hex.datahex[2] = datahex[2]; - fp32hex.datahex[3] = datahex[3]; - return (float)fp32hex.dataf; -} - -inline void int32_to_hex(int data, unsigned char datahex[4]) { - union _int32hex { - int data; - unsigned char datahex[4]; - } int32hex; - int32hex.data = data; - datahex[0] = int32hex.datahex[0]; - datahex[1] = int32hex.datahex[1]; - datahex[2] = int32hex.datahex[2]; - datahex[3] = int32hex.datahex[3]; -} - -inline void hex_to_nfp32(unsigned char *datahex, float *dataf, int n) { - for (int i = 0; i < n; ++i) - { - dataf[i] = hex_to_fp32(&datahex[i * 4]); - } -} - -inline void nfp32_to_hex(float *dataf, unsigned char *datahex, int n) { - for (int i = 0; i < n; ++i) - { - fp32_to_hex(dataf[i], &datahex[i * 4]); - } -} - -inline void nint32_to_hex(int *data, unsigned char *datahex, int n) { - for (int i = 0; i < n; ++i) - { - int32_to_hex(data[i], &datahex[i * 4]); - } -} - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/common/queue_memcpy.h b/interbotix_ros_uxarms/xarm_api/include/xarm/common/queue_memcpy.h deleted file mode 100755 index 1744c5d..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/common/queue_memcpy.h +++ /dev/null @@ -1,36 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef COMMON_QUEUE_MEMCPY_H_ -#define COMMON_QUEUE_MEMCPY_H_ - -#include - -class QueueMemcpy { - public: - QueueMemcpy(long n, long n_size); - ~QueueMemcpy(void); - char flush(void); - char push(void *data); - char pop(void *data); - char get(void *data); - long size(void); - long node_size(void); - int is_full(void); - - protected: - private: - long total_; - long annode_size_; - - long cnt_; - long head_; - long tail_; - char *buf_; - pthread_mutex_t mutex_; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/connect.h b/interbotix_ros_uxarms/xarm_api/include/xarm/connect.h deleted file mode 100755 index 3ebcbad..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/connect.h +++ /dev/null @@ -1,19 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef XARM_CONNECT_H_ -#define XARM_CONNECT_H_ - -#include "xarm/instruction/uxbus_cmd_ser.h" -#include "xarm/instruction/uxbus_cmd_tcp.h" - -UxbusCmdSer *connect_rs485_control(const char *com); -UxbusCmdTcp *connect_tcp_control(char *server_ip); -SocketPort *connext_tcp_report_norm(char *server_ip); -SocketPort *connext_tcp_report_rich(char *server_ip); -SocketPort *connext_tcp_report_devl(char *server_ip); - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/debug/debug_print.h b/interbotix_ros_uxarms/xarm_api/include/xarm/debug/debug_print.h deleted file mode 100755 index b8f21ab..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/debug/debug_print.h +++ /dev/null @@ -1,19 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef DEBUG_DEBUG_PRINT_H_ -#define DEBUG_DEBUG_PRINT_H_ - -#include "xarm/common/data_type.h" - -void print_log(const char *format, ...); -void print_nvect(const char *str, double vect[], int n); -void print_nvect(const char *str, float *vect, int n); -void print_nvect(const char *str, unsigned char vect[], int n); -void print_nvect(const char *str, int vect[], int n); -void print_hex(const char *str, unsigned char *hex, int len); - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/servo3_config.h b/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/servo3_config.h deleted file mode 100755 index 5820f15..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/servo3_config.h +++ /dev/null @@ -1,66 +0,0 @@ - -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef CORE_INSTRUCTION_SERVO3_CONFIG_H_ -#define CORE_INSTRUCTION_SERVO3_CONFIG_H_ - -class SERVO3_RG { -public: - static const unsigned short CON_EN = 0x0100; - static const unsigned short CON_MODE = 0x0101; - static const unsigned short CON_DIR = 0x0102; - static const unsigned short SV3MOD_POS = 0; - static const unsigned short SV3MOD_SPD = 1; - static const unsigned short SV3MOD_FOS = 2; - static const unsigned short SV3_SAVE = 0x1000; - static const unsigned short BRAKE = 0x0104; - static const unsigned short GET_TEMP = 0x000E; - static const unsigned short ERR_CODE = 0x000F; - static const unsigned short OVER_TEMP = 0x0108; - static const unsigned short CURR_CURR = 0x0001; - static const unsigned short POS_KP = 0x0200; - static const unsigned short POS_FWDKP = 0x0201; - static const unsigned short POS_PWDTC = 0x0202; - static const unsigned short SPD_KP = 0x0203; - static const unsigned short SPD_KI = 0x0204; - static const unsigned short CURR_KP = 0x090C; - static const unsigned short CURR_KI = 0x090D; - static const unsigned short SPD_IFILT = 0x030C; - static const unsigned short SPD_OFILT = 0x030D; - static const unsigned short POS_CMDILT = 0x030E; - static const unsigned short CURR_IFILT = 0x0401; - static const unsigned short POS_KD = 0x0205; - static const unsigned short POS_ACCT = 0x0300; - static const unsigned short POS_DECT = 0x0301; - static const unsigned short POS_STHT = 0x0302; - static const unsigned short POS_SPD = 0x0303; - static const unsigned short MT_ID = 0x1600; - static const unsigned short BAUDRATE = 0x0601; - static const unsigned short SOFT_REBOOT = 0x0607; - static const unsigned short TAGET_TOQ = 0x050a; - static const unsigned short CURR_TOQ = 0x050c; - static const unsigned short TOQ_SPD = 0x050e; - static const unsigned short TAGET_POS = 0x0700; - static const unsigned short CURR_POS = 0x0702; - static const unsigned short HARD_VER = 0x0800; - static const unsigned short SOFT_VER = 0x0801; - static const unsigned short MT_TYPE = 0x0802; - static const unsigned short MT_ZERO = 0x0817; - static const unsigned short RESET_PVL = 0x0813; - static const unsigned short CAL_ZERO = 0x080C; - static const unsigned short ERR_SWITCH = 0x0910; - static const unsigned short RESET_ERR = 0x0109; - static const unsigned short SV3_BRO_ID = 0xFF; - - static const unsigned short MODBUS_BAUDRATE = 0x0A0B; - static const unsigned short DIGITAL_IN = 0x0A14; - static const unsigned short DIGITAL_OUT = 0x0A15; - static const unsigned short ANALOG_IO1 = 0x0A16; - static const unsigned short ANALOG_IO2 = 0x0A17; -}; - -#endif \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd.h b/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd.h deleted file mode 100644 index 8e6e63b..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd.h +++ /dev/null @@ -1,169 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef CORE_INSTRUCTION_UXBUS_CMD_H_ -#define CORE_INSTRUCTION_UXBUS_CMD_H_ - -#include -#include "xarm/common/data_type.h" - -class UxbusCmd { -public: - UxbusCmd(void); - ~UxbusCmd(void); - - int get_version(unsigned char rx_data[40]); - int get_robot_sn(unsigned char rx_data[40]); - int check_verification(int *rx_data); - int shutdown_system(int value); - int set_record_traj(int value); - int save_traj(char filename[81]); - int load_traj(char filename[81]); - int playback_traj(int times, int spdx = 1); - int playback_traj_old(int times); - int get_traj_rw_status(int *rx_data); - int set_reduced_mode(int on_off); - int set_reduced_linespeed(float lspd_mm); - int set_reduced_jointspeed(float jspd_rad); - int get_reduced_mode(int *rx_data); - int get_reduced_states(int *on, int xyz_list[6], float *tcp_speed, float *joint_speed, float jrange_rad[14] = NULL, int *fense_is_on = NULL, int *collision_rebound_is_on = NULL, int length = 21); - int set_xyz_limits(int xyz_list[6]); - int set_world_offset(float pose_offset[6]); - int cnter_reset(void); - int cnter_plus(void); - int set_reduced_jrange(float jrange_rad[14]); - int set_fense_on(int on_off); - int set_collis_reb(int on_off); - int motion_en(int id, int value); - int set_state(int value); - int get_state(int *rx_data); - int get_cmdnum(int *rx_data); - int get_err_code(int *rx_data); - int get_hd_types(int *rx_data); - int reload_dynamics(void); - int clean_err(void); - int clean_war(void); - int set_brake(int axis, int en); - int set_mode(int value); - int move_line(float mvpose[6], float mvvelo, float mvacc, float mvtime); - int move_lineb(float mvpose[6], float mvvelo, float mvacc, float mvtime, - float mvradii); - int move_joint(float mvjoint[7], float mvvelo, float mvacc, float mvtime); - int move_line_tool(float mvpose[6], float mvvelo, float mvacc, float mvtime); - int move_gohome(float mvvelo, float mvacc, float mvtime); - int move_servoj(float mvjoint[7], float mvvelo, float mvacc, float mvtime); - int move_servo_cartesian(float mvpose[6], float mvvelo, float mvacc, float mvtime); - int set_servot(float jnt_taus[7]); - int get_joint_tau(float jnt_taus[7]); - int set_safe_level(int level); - int get_safe_level(int *level); - int sleep_instruction(float sltime); - int move_circle(float pose1[6], float pose2[6], float mvvelo, float mvacc, float mvtime, float percent); - int set_tcp_jerk(float jerk); - int set_tcp_maxacc(float maxacc); - int set_joint_jerk(float jerk); - int set_joint_maxacc(float maxacc); - int set_tcp_offset(float pose_offset[6]); - int set_tcp_load(float mass, float load_offset[3]); - int set_collis_sens(int value); - int set_teach_sens(int value); - int set_gravity_dir(float gravity_dir[3]); - int clean_conf(void); - int save_conf(void); - - int get_tcp_pose(float pose[6]); - int get_joint_pose(float angles[7]); - int get_ik(float pose[6], float angles[7]); - int get_fk(float angles[7], float pose[6]); - int is_joint_limit(float joint[7], int *value); - int is_tcp_limit(float pose[6], int *value); - int gripper_addr_w16(int addr, float value); - int gripper_addr_r16(int addr, float *value); - int gripper_addr_w32(int addr, float value); - int gripper_addr_r32(int addr, float *value); - int gripper_set_en(int value); - int gripper_set_mode(int value); - int gripper_set_zero(void); - int gripper_get_pos(float *pulse); - int gripper_set_pos(float pulse); - int gripper_set_posspd(float speed); - int gripper_get_errcode(int rx_data[2]); - int gripper_clean_err(void); - - int tgpio_addr_w16(int addr, float value); - int tgpio_addr_r16(int addr, float *value); - int tgpio_addr_w32(int addr, float value); - int tgpio_addr_r32(int addr, float *value); - int tgpio_get_digital(int *io1, int *io2); - int tgpio_set_digital(int ionum, int value); - int tgpio_get_analog1(float *value); - int tgpio_get_analog2(float *value); - - int set_modbus_timeout(int value); - int set_modbus_baudrate(int baud); - int tgpio_set_modbus(unsigned char *send_data, int length, unsigned char *recv_data); - int gripper_modbus_w16s(int addr, float value, int len); - int gripper_modbus_r16s(int addr, int len, unsigned char *rx_data); - int gripper_modbus_set_en(int value); - int gripper_modbus_set_mode(int value); - int gripper_modbus_set_zero(void); - int gripper_modbus_get_pos(float *pulse); - int gripper_modbus_set_pos(float pulse); - int gripper_modbus_set_posspd(float speed); - int gripper_modbus_get_errcode(int *err); - int gripper_modbus_clean_err(void); - - int servo_set_zero(int id); - int servo_get_dbmsg(int rx_data[16]); - int servo_addr_w16(int id, int addr, float value); - int servo_addr_r16(int id, int addr, float *value); - int servo_addr_w32(int id, int addr, float value); - int servo_addr_r32(int id, int addr, float *value); - - - int cgpio_get_auxdigit(int *value); - int cgpio_get_analog1(float *value); - int cgpio_get_analog2(float *value); - int cgpio_set_auxdigit(int ionum, int value); - int cgpio_set_analog1(int value); - int cgpio_set_analog2(int value); - int cgpio_set_infun(int ionum, int fun); - int cgpio_set_outfun(int ionum, int fun); - int cgpio_get_state(int *state, int *digit_io, float *analog, int *input_conf, int *output_conf); - - int get_pose_offset(float pose1[6], float pose2[6], float offset[6], int orient_type_in=0, int orient_type_out=0); - int get_position_aa(float pose[6]); - int move_line_aa(float mvpose[6], float mvvelo, float mvacc, float mvtime, int mvcoord=0, int relative=0); - int move_servo_cart_aa(float mvpose[6], float mvvelo, float mvacc, int tool_coord=0, int relative=0); - - int tgpio_delay_set_digital(int ionum, int value, float delay_sec); - int cgpio_delay_set_digital(int ionum, int value, float delay_sec); - int tgpio_position_set_digital(int ionum, int value, float xyz[3], float tol_r); - int cgpio_position_set_digital(int ionum, int value, float xyz[3], float tol_r); - int config_io_stop_reset(int io_type, int val); - - virtual void close(void); - -private: - virtual int check_xbus_prot(unsigned char *data, int funcode); - virtual int send_pend(int funcode, int num, int timeout, unsigned char *rx_data); - virtual int send_xbus(int funcode, unsigned char *txdata, int num); - int set_nu8(int funcode, int *datas, int num); - int get_nu8(int funcode, int *rx_data, int num); - int get_nu8(int funcode, unsigned char *rx_data, int num); - int set_nu16(int funcode, int *datas, int num); - int get_nu16(int funcode, int *rx_data, int num); - int set_nfp32(int funcode, float *datas, int num); - int set_nint32(int funcode, int *datas, int num); - int get_nfp32(int funcode, float *rx_data, int num); - int swop_nfp32(int funcode, float tx_datas[], int txn, float *rx_data, int rxn); - int is_nfp32(int funcode, float datas[], int txn, int *value); - int set_nfp32_with_bytes(int funcode, float *datas, int num, char *additional, int n); - -private: - std::mutex mutex_; -}; -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_config.h b/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_config.h deleted file mode 100644 index 15ae9d5..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_config.h +++ /dev/null @@ -1,204 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef CORE_INSTRUCTION_UXBUS_CMD_CONFIG_H_ -#define CORE_INSTRUCTION_UXBUS_CMD_CONFIG_H_ - -class UXBUS_RG { -public: - UXBUS_RG(void) {} - ~UXBUS_RG(void) {} - - static const unsigned char GET_VERSION = 1; - static const unsigned char GET_ROBOT_SN = 2; - static const unsigned char CHECK_VERIFY = 3; - static const unsigned char RELOAD_DYNAMICS = 4; - static const unsigned char SHUTDOWN_SYSTEM = 10; - - static const unsigned char MOTION_EN = 11; - static const unsigned char SET_STATE = 12; - static const unsigned char GET_STATE = 13; - static const unsigned char GET_CMDNUM = 14; - static const unsigned char GET_ERROR = 15; - static const unsigned char CLEAN_ERR = 16; - static const unsigned char CLEAN_WAR = 17; - static const unsigned char SET_BRAKE = 18; - static const unsigned char SET_MODE = 19; - - static const unsigned char MOVE_LINE = 21; - static const unsigned char MOVE_LINEB = 22; - static const unsigned char MOVE_JOINT = 23; - static const unsigned char MOVE_HOME = 25; - static const unsigned char SLEEP_INSTT = 26; - static const unsigned char MOVE_CIRCLE = 27; - static const unsigned char MOVE_LINE_TOOL = 28; - static const unsigned char MOVE_SERVOJ = 29; - static const unsigned char MOVE_SERVO_CART = 30; - - static const unsigned char SET_TCP_JERK = 31; - static const unsigned char SET_TCP_MAXACC = 32; - static const unsigned char SET_JOINT_JERK = 33; - static const unsigned char SET_JOINT_MAXACC = 34; - static const unsigned char SET_TCP_OFFSET = 35; - static const unsigned char SET_LOAD_PARAM = 36; - static const unsigned char SET_COLLIS_SENS = 37; - static const unsigned char SET_TEACH_SENS = 38; - static const unsigned char CLEAN_CONF = 39; - static const unsigned char SAVE_CONF = 40; - - static const unsigned char GET_TCP_POSE = 41; - static const unsigned char GET_JOINT_POS = 42; - static const unsigned char GET_IK = 43; - static const unsigned char GET_FK = 44; - static const unsigned char IS_JOINT_LIMIT = 45; - static const unsigned char IS_TCP_LIMIT = 46; - - static const unsigned char SET_REDUCED_TRSV = 47; - static const unsigned char SET_REDUCED_P2PV = 48; - static const unsigned char GET_REDUCED_MODE = 49; - static const unsigned char SET_REDUCED_MODE = 50; - static const unsigned char SET_GRAVITY_DIR = 51; - static const unsigned char SET_LIMIT_XYZ = 52; - static const unsigned char GET_REDUCED_STATE = 53; - - static const unsigned char SET_SERVOT = 54; - static const unsigned char GET_JOINT_TAU = 55; - static const unsigned char SET_SAFE_LEVEL = 56; - static const unsigned char GET_SAFE_LEVEL = 57; - - static const unsigned char SET_REDUCED_JRANGE = 58; - static const unsigned char SET_FENSE_ON = 59; - static const unsigned char SET_COLLIS_REB = 60; - - static const unsigned char SET_TRAJ_RECORD = 61; - static const unsigned char SAVE_TRAJ = 62; - static const unsigned char LOAD_TRAJ = 63; - static const unsigned char PLAY_TRAJ = 64; - static const unsigned char GET_TRAJ_RW_STATUS = 65; - - static const unsigned char SET_TIMER = 71; - static const unsigned char CANCEL_TIMER = 72; - static const unsigned char SET_WORLD_OFFSET = 73; - static const unsigned char CNTER_RESET = 74; - static const unsigned char CNTER_PLUS = 75; - - static const unsigned char CAL_POSE_OFFSET = 76; - static const unsigned char GET_TCP_POSE_AA = 91; - static const unsigned char MOVE_LINE_AA = 92; - static const unsigned char MOVE_SERVO_CART_AA = 93; - - static const unsigned char SERVO_W16B = 101; - static const unsigned char SERVO_R16B = 102; - static const unsigned char SERVO_W32B = 103; - static const unsigned char SERVO_R32B = 104; - static const unsigned char SERVO_ZERO = 105; - static const unsigned char SERVO_DBMSG = 106; - - static const unsigned char TGPIO_MB_TIOUT = 123; - static const unsigned char TGPIO_MODBUS = 124; - static const unsigned char TGPIO_ERR = 125; - static const unsigned char TGPIO_W16B = 127; - static const unsigned char TGPIO_R16B = 128; - static const unsigned char TGPIO_W32B = 129; - static const unsigned char TGPIO_R32B = 130; - - static const unsigned char CGPIO_GET_DIGIT = 131; - static const unsigned char CGPIO_GET_ANALOG1 = 132; - static const unsigned char CGPIO_GET_ANALOG2 = 133; - static const unsigned char CGPIO_SET_DIGIT = 134; - static const unsigned char CGPIO_SET_ANALOG1 = 135; - static const unsigned char CGPIO_SET_ANALOG2 = 136; - static const unsigned char CGPIO_SET_IN_FUN = 137; - static const unsigned char CGPIO_SET_OUT_FUN = 138; - static const unsigned char CGPIO_GET_STATE = 139; - - static const unsigned char GET_HD_TYPES = 141; - static const unsigned char DELAYED_CGPIO_SET = 142; - static const unsigned char DELAYED_TGPIO_SET = 143; - static const unsigned char POSITION_CGPIO_SET = 144; - static const unsigned char POSITION_TGPIO_SET = 145; - static const unsigned char SET_IO_STOP_RESET = 146; -}; - -class UXBUS_STATE { -public: - UXBUS_STATE(void) {} - ~UXBUS_STATE(void) {} - static const int NOT_CONNECTED = -1; - static const int NOT_READY = -2; - static const int API_EXCEPTION = -3; - static const int CMD_NOT_EXIST = -4; - static const int TCP_LIMIT = -6; - static const int JOINT_LIMIT = -7; - static const int OUT_OF_RANGE = -8; - static const int EMERGENCY_STOP = -9; - static const int SERVO_NOT_EXIST = -10; - static const int CONVERT_FAILED = -11; - static const int ERR_CODE = 1; - static const int WAR_CODE = 2; - static const int ERR_TOUT = 3; - static const int ERR_LENG = 4; - static const int ERR_NUM = 5; - static const int ERR_PROT = 6; - static const int ERR_FUN = 7; - static const int ERR_NOTTCP = 8; - static const int ERR_OTHER = 11; - static const int ERR_PARAM = 12; - static const int TRAJ_RW_FAILED = 31; - static const int TRAJ_RW_TOUT = 32; - static const int TRAJ_PLAYBACK_TOUT = 33; - static const int SUCTION_CUP_TOUT = 41; -}; - -class TRAJ_STATE { -public: - TRAJ_STATE(void) {} - ~TRAJ_STATE(void) {} - - static const int IDLE = 0; - static const int LOADING = 1; - static const int LOAD_SUCCESS = 2; - static const int LOAD_FAIL = 3; - static const int SAVING = 4; - static const int SAVE_SUCCESS = 5; - static const int SAVE_FAIL = 6; -}; - -class UXBUS_CONF { -public: - UXBUS_CONF(void) {} - ~UXBUS_CONF(void) {} - - static const int SET_TIMEOUT = 1000; // ms - static const int GET_TIMEOUT = 1000; // ms - static const int GRIPPER_ID = 8; - static const int TGPIO_ID = 9; - static const int MASTER_ID = 0xAA; - static const int SLAVE_ID = 0x55; -}; - -class XARM_MODE { -public: - XARM_MODE(void) {} - ~XARM_MODE(void) {} - - static const int POSE = 0; - static const int SERVO = 1; - static const int TEACH_JOINT = 2; - static const int TEACH_CART = 3; -}; - -class XARM_STATE { -public: - XARM_STATE(void) {} - ~XARM_STATE(void) {} - - static const int START = 0; - static const int PAUSE = 3; - static const int STOP = 4; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h b/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h deleted file mode 100755 index a8396b2..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h +++ /dev/null @@ -1,27 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef INSTRUCTION_UXBUS_CMD_SER_H_ -#define INSTRUCTION_UXBUS_CMD_SER_H_ - -#include "xarm/instruction/uxbus_cmd.h" -#include "xarm/port/serial.h" - -class UxbusCmdSer : public UxbusCmd { - public: - UxbusCmdSer(SerialPort *arm_port); - ~UxbusCmdSer(void); - - int check_xbus_prot(unsigned char *datas, int funcode); - int send_pend(int funcode, int num, int timeout, unsigned char *ret_data); - int send_xbus(int funcode, unsigned char *datas, int num); - void close(void); - - private: - SerialPort *arm_port_; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h b/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h deleted file mode 100755 index e05d0de..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h +++ /dev/null @@ -1,33 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef INSTRUCTION_UXBUS_CMD_TCP_H_ -#define INSTRUCTION_UXBUS_CMD_TCP_H_ - -#include "xarm/instruction/uxbus_cmd.h" -#include "xarm/port/socket.h" - -class UxbusCmdTcp : public UxbusCmd { - public: - UxbusCmdTcp(SocketPort *arm_port); - ~UxbusCmdTcp(void); - - int check_xbus_prot(unsigned char *datas, int funcode); - int send_pend(int funcode, int num, int timeout, unsigned char *ret_data); - int send_xbus(int funcode, unsigned char *datas, int num); - void close(void); - - private: - SocketPort *arm_port_; - int bus_flag_; - int prot_flag_; - int TX2_PROT_CON_ = 2; // tcp cmd prot - int TX2_PROT_HEAT_ = 1; // tcp heat prot - int TX2_BUS_FLAG_MIN_ = 1; // cmd序号 起始值 - int TX2_BUS_FLAG_MAX_ = 5000; // cmd序号 最大值 -}; - -#endif \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/linux/network.h b/interbotix_ros_uxarms/xarm_api/include/xarm/linux/network.h deleted file mode 100755 index a08870d..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/linux/network.h +++ /dev/null @@ -1,16 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef LINUX_NETWORK_H_ -#define LINUX_NETWORK_H_ - -#include "xarm/common/data_type.h" - -int socket_init(char *local_ip, int port, int is_server); -int socket_send_data(int client_fp, unsigned char *data, int len); -int socket_connect_server(int *socket, char server_ip[], int server_port); - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/linux/thread.h b/interbotix_ros_uxarms/xarm_api/include/xarm/linux/thread.h deleted file mode 100755 index b856e06..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/linux/thread.h +++ /dev/null @@ -1,17 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef LINUX_THREAD_H_ -#define LINUX_THREAD_H_ - -#include - -typedef void *(*fun_point_t)(void *); - -void thread_delete(pthread_t id); -pthread_t thread_init(fun_point_t fun_point, void *arg); - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/port/serial.h b/interbotix_ros_uxarms/xarm_api/include/xarm/port/serial.h deleted file mode 100755 index cb48326..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/port/serial.h +++ /dev/null @@ -1,57 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef PORT_SERIAL_H_ -#define PORT_SERIAL_H_ - -#include - -#include "xarm/common/data_type.h" -#include "xarm/common/queue_memcpy.h" - -class SerialPort { - public: - SerialPort(const char *port, int baud, int que_num, int que_maxlen); - ~SerialPort(void); - int is_ok(void); - void flush(void); - void recv_proc(void); - int write_frame(unsigned char *data, int len); - int read_frame(unsigned char *data); - void close_port(void); - int que_maxlen_; - int que_num_; - - private: - int fp_; - int state_; - pthread_t thread_id_; - QueueMemcpy *rx_que_; - int init_serial(const char *port, int baud); - int read_char(unsigned char *ch); - int read_str(unsigned char *data, char eol, int len); - int write_char(unsigned char ch); - void parse_put(unsigned char *data, int len); - - typedef enum _UXBUS_RECV_STATE { - UXBUS_START_FROMID = 0, - UXBUS_START_TOOID = 1, - UXBUS_STATE_LENGTH = 2, - UXBUS_STATE_DATA = 3, - UXBUS_STATE_CRC1 = 4, - UXBUS_STATE_CRC2 = 5, - } UXBUS_RECV_STATE; - - unsigned char UXBUS_PROT_FROMID_; - unsigned char UXBUS_PROT_TOID_; - - int rx_data_idx_; - int rx_state_; - unsigned char rx_buf_[128]; - int rx_length_; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/port/socket.h b/interbotix_ros_uxarms/xarm_api/include/xarm/port/socket.h deleted file mode 100755 index e3a6ccb..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/port/socket.h +++ /dev/null @@ -1,35 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef PORT_SOCKET_H_ -#define PORT_SOCKET_H_ - -#include - -#include "xarm/common/data_type.h" -#include "xarm/common/queue_memcpy.h" - -class SocketPort { - public: - SocketPort(char *server_ip, int server_port, int que_num, int que_maxlen); - ~SocketPort(void); - int is_ok(void); - void flush(void); - void recv_proc(void); - int write_frame(unsigned char *data, int len); - int read_frame(unsigned char *data); - void close_port(void); - int que_maxlen_; - - private: - int fp_; - int state_; - int que_num_; - QueueMemcpy *rx_que_; - pthread_t thread_id_; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/report_data.h b/interbotix_ros_uxarms/xarm_api/include/xarm/report_data.h deleted file mode 100755 index 7d7b4b6..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/report_data.h +++ /dev/null @@ -1,115 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#ifndef XARM_REPORT_DATA_H_ -#define XARM_REPORT_DATA_H_ - -#include "xarm/common/data_type.h" - - -class ReportDataDevelop { - public: - ReportDataDevelop(void); - ~ReportDataDevelop(void); - - int flush_data(unsigned char *rx_data); - void print_data(void); - - private: - int runing_; - int mode_; - int cmdnum_; - float angle_[7]; - float pose_[6]; - float tau_[7]; - int total_num_; -}; - - - -class ReportDataNorm { - public: - ReportDataNorm(void); - ~ReportDataNorm(void); - - int flush_data(unsigned char *rx_data); - void print_data(void); - - // private: - int runing_; - int mode_; - int cmdnum_; - float angle_[7]; - float pose_[6]; - float tau_[7]; - - int mt_brake_; - int mt_able_; - int err_; - int war_; - float tcp_offset_[6]; - float tcp_load_[4]; - int collis_sens_; - int teach_sens_; - float gravity_dir_[3]; - int total_num_; -}; - - - -class ReportDataRich { - public: - ReportDataRich(void); - ~ReportDataRich(void); - - int flush_data(unsigned char *rx_data); - void print_data(void); - - private: - int runing_; - int mode_; - int cmdnum_; - float angle_[7]; - float pose_[6]; - float tau_[7]; - - int mt_brake_; - int mt_able_; - int err_; - int war_; - float tcp_offset_[6]; - float tcp_load_[4]; - int collis_sens_; - int teach_sens_; - float gravity_dir_[3]; - int total_num_; - - int arm_type_; - int axis_num_; - int master_id_; - int slave_id_; - int motor_tid_; - int motor_fid_; - unsigned char versions_[30]; - float trs_jerk_; - float trs_accmin_; - float trs_accmax_; - float trs_velomin_; - float trs_velomax_; - float p2p_jerk_; - float p2p_accmin_; - float p2p_accmax_; - float p2p_velomin_; - float p2p_velomax_; - float rot_jerk_; - float rot_accmax_; - int sv3msg_[16]; - float trs_msg_[5]; - float p2p_msg_[5]; - float rot_msg_[2]; -}; - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm/xarm_config.h b/interbotix_ros_uxarms/xarm_api/include/xarm/xarm_config.h deleted file mode 100755 index f2ab067..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm/xarm_config.h +++ /dev/null @@ -1,20 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -class XARM_CONF { -public: - XARM_CONF() {} - ~XARM_CONF() {} - - static const int AXIS_NUM = 7; - static const int GRIPPER_ID = 8; - static const int GPIO_ID = 9; - static const int SERIAL_BAUD = 921600; - static const int TCP_PORT_CONTROL = 502; - static const int TCP_PORT_REPORT_NORM = 30001; - static const int TCP_PORT_REPORT_RICH = 30002; - static const int TCP_PORT_REPORT_DEVL = 30003; -}; diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm_driver.h b/interbotix_ros_uxarms/xarm_api/include/xarm_driver.h deleted file mode 100755 index ff6e2d6..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm_driver.h +++ /dev/null @@ -1,128 +0,0 @@ -#ifndef __XARM_DRIVER_H -#define __XARM_DRIVER_H - -#include "ros/ros.h" -#include "std_msgs/Float32.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "xarm/connect.h" -#include "xarm/report_data.h" - -namespace xarm_api -{ - class XARMDriver - { - public: - XARMDriver():spinner(4){spinner.start();}; - ~XARMDriver(); - void XARMDriverInit(ros::NodeHandle& root_nh, char *server_ip); - void Heartbeat(void); - bool isConnectionOK(void); - - // provide a list of services: - bool MotionCtrlCB(xarm_msgs::SetAxis::Request &req, xarm_msgs::SetAxis::Response &res); - bool SetModeCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res); - bool SetStateCB(xarm_msgs::SetInt16::Request &req, xarm_msgs::SetInt16::Response &res); - bool SetTCPOffsetCB(xarm_msgs::TCPOffset::Request &req, xarm_msgs::TCPOffset::Response &res); - bool SetLoadCB(xarm_msgs::SetLoad::Request &req, xarm_msgs::SetLoad::Response &res); - bool SetDigitalIOCB(xarm_msgs::SetDigitalIO::Request &req, xarm_msgs::SetDigitalIO::Response &res); - bool GetDigitalIOCB(xarm_msgs::GetDigitalIO::Request &req, xarm_msgs::GetDigitalIO::Response &res); - bool GetAnalogIOCB(xarm_msgs::GetAnalogIO::Request &req, xarm_msgs::GetAnalogIO::Response &res); - bool ClearErrCB(xarm_msgs::ClearErr::Request& req, xarm_msgs::ClearErr::Response& res); - bool MoveitClearErrCB(xarm_msgs::ClearErr::Request& req, xarm_msgs::ClearErr::Response& res); - bool GetErrCB(xarm_msgs::GetErr::Request & req, xarm_msgs::GetErr::Response & res); - bool GoHomeCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveJointCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveLinebCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveLineCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveLineToolCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveServoJCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool MoveServoCartCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); - bool GripperConfigCB(xarm_msgs::GripperConfig::Request &req, xarm_msgs::GripperConfig::Response &res); - bool GripperMoveCB(xarm_msgs::GripperMove::Request &req, xarm_msgs::GripperMove::Response &res); - bool GripperStateCB(xarm_msgs::GripperState::Request &req, xarm_msgs::GripperState::Response &res); - bool VacuumGripperCB(xarm_msgs::SetInt16::Request &req, xarm_msgs::SetInt16::Response &res); - bool SetModbusCB(xarm_msgs::SetToolModbus::Request &req, xarm_msgs::SetToolModbus::Response &res); - bool ConfigModbusCB(xarm_msgs::ConfigToolModbus::Request &req, xarm_msgs::ConfigToolModbus::Response &res); - bool SetControllerDOutCB(xarm_msgs::SetDigitalIO::Request &req, xarm_msgs::SetDigitalIO::Response &res); - bool GetControllerDInCB(xarm_msgs::GetControllerDigitalIO::Request &req, xarm_msgs::GetControllerDigitalIO::Response &res); - void SleepTopicCB(const std_msgs::Float32ConstPtr& msg); - - void pub_robot_msg(xarm_msgs::RobotMsg &rm_msg); - void pub_joint_state(sensor_msgs::JointState &js_msg); - void pub_io_state(); - - int get_frame(void); - int get_rich_data(ReportDataNorm &norm_data); - - private: - SocketPort *arm_report_; - ReportDataNorm norm_data_; - UxbusCmd *arm_cmd_; - unsigned char rx_data_[1280]; - std::string ip; - pthread_t thread_id_; - ros::AsyncSpinner spinner; - int dof_; - int curr_state_; - int curr_err_; - xarm_msgs::IOState io_msg; - - ros::NodeHandle nh_; - ros::ServiceServer go_home_server_; - ros::ServiceServer move_joint_server_; - ros::ServiceServer motion_ctrl_server_; - ros::ServiceServer set_state_server_; - ros::ServiceServer set_mode_server_; - ros::ServiceServer move_lineb_server_; - ros::ServiceServer move_line_server_; - ros::ServiceServer move_line_tool_server_; - ros::ServiceServer move_servoj_server_; - ros::ServiceServer move_servo_cart_server_; - ros::ServiceServer set_tcp_offset_server_; - ros::ServiceServer set_load_server_; - ros::ServiceServer set_end_io_server_; - ros::ServiceServer get_digital_in_server_; - ros::ServiceServer get_analog_in_server_; - ros::ServiceServer clear_err_server_; - ros::ServiceServer moveit_clear_err_server_; - ros::ServiceServer get_err_server_; - ros::ServiceServer gripper_config_server_; - ros::ServiceServer gripper_move_server_; - ros::ServiceServer gripper_state_server_; - ros::ServiceServer set_vacuum_gripper_server_; - ros::ServiceServer set_modbus_server_; - ros::ServiceServer config_modbus_server_; - ros::ServiceServer set_controller_dout_server_; - ros::ServiceServer get_controller_din_server_; - - ros::Publisher joint_state_; - ros::Publisher robot_rt_state_; - ros::Publisher end_input_state_; - - ros::Subscriber sleep_sub_; - - int wait_for_finish(); - }; -} - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/include/xarm_ros_client.h b/interbotix_ros_uxarms/xarm_api/include/xarm_ros_client.h deleted file mode 100755 index 4bfd19f..0000000 --- a/interbotix_ros_uxarms/xarm_api/include/xarm_ros_client.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef __XARM_ROS_CLIENT_H__ -#define __XARM_ROS_CLIENT_H__ - -#include "ros/ros.h" -#include - -namespace xarm_api{ - -class XArmROSClient -{ -public: - XArmROSClient(){}; - void init(ros::NodeHandle& nh); - ~XArmROSClient(){}; - - int motionEnable(short en); - int setState(short state); - int setMode(short mode); - int clearErr(void); - int getErr(void); - int setTCPOffset(const std::vector& tcp_offset); - int setLoad(float mass, const std::vector& center_of_mass); - int setServoJ(const std::vector& joint_cmd); - int setServoCartisian(const std::vector& cart_cmd); - int goHome(float jnt_vel_rad, float jnt_acc_rad=15); - int moveJoint(const std::vector& joint_cmd, float jnt_vel_rad, float jnt_acc_rad=15); - int moveLine(const std::vector& cart_cmd, float cart_vel_mm, float cart_acc_mm=500); - int moveLineB(int num_of_pnts, const std::vector cart_cmds[], float cart_vel_mm, float cart_acc_mm=500, float radii=0); - int getGripperState(float *curr_pulse, int *curr_err); - int gripperConfig(float pulse_vel); - int gripperMove(float pulse); - - int config_tool_modbus(int baud_rate, int time_out_ms); - int send_tool_modbus(unsigned char* data, int send_len, unsigned char* recv_data=NULL, int recv_len=0); - -private: - ros::ServiceClient motion_ctrl_client_; - ros::ServiceClient set_mode_client_; - ros::ServiceClient set_state_client_; - ros::ServiceClient go_home_client_; - ros::ServiceClient move_lineb_client_; - ros::ServiceClient move_servoj_client_; - ros::ServiceClient move_servo_cart_client_; - ros::ServiceClient move_line_client_; - ros::ServiceClient move_joint_client_; - ros::ServiceClient set_tcp_offset_client_; - ros::ServiceClient set_load_client_; - ros::ServiceClient clear_err_client_; - ros::ServiceClient get_err_client_; - ros::ServiceClient config_modbus_client_; - ros::ServiceClient send_modbus_client_; - ros::ServiceClient gripper_move_client_; - ros::ServiceClient gripper_config_client_; - ros::ServiceClient gripper_state_client_; - - xarm_msgs::SetAxis set_axis_srv_; - xarm_msgs::SetInt16 set_int16_srv_; - xarm_msgs::TCPOffset offset_srv_; - xarm_msgs::SetLoad set_load_srv_; - xarm_msgs::ClearErr clear_err_srv_; - xarm_msgs::GetErr get_err_srv_; - xarm_msgs::Move move_srv_; - xarm_msgs::Move servoj_msg_; - xarm_msgs::Move servo_cart_msg_; - xarm_msgs::ConfigToolModbus cfg_modbus_msg_; - xarm_msgs::SetToolModbus set_modbus_msg_; - xarm_msgs::GripperConfig gripper_config_msg_; - xarm_msgs::GripperMove gripper_move_msg_; - xarm_msgs::GripperState gripper_state_msg_; - - ros::NodeHandle nh_; -}; - -} - -#endif diff --git a/interbotix_ros_uxarms/xarm_api/package.xml b/interbotix_ros_uxarms/xarm_api/package.xml deleted file mode 100755 index 3f8367d..0000000 --- a/interbotix_ros_uxarms/xarm_api/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - xarm_api - 0.0.0 - The xarm_api package - - Jason Peng - - BSD - catkin - - std_srvs - genmsg - - roscpp - std_msgs - xarm_msgs - sensor_msgs - - roscpp - std_msgs - xarm_msgs - sensor_msgs - - roscpp - std_msgs - xarm_msgs - sensor_msgs - - - - - - - diff --git a/interbotix_ros_uxarms/xarm_api/scripts/servo_cartesian_test.py b/interbotix_ros_uxarms/xarm_api/scripts/servo_cartesian_test.py deleted file mode 100644 index 6c893de..0000000 --- a/interbotix_ros_uxarms/xarm_api/scripts/servo_cartesian_test.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -from xarm_msgs.srv import * - -def servo_cartesian_tool(step, freq, time_secs): - servo_cart = rospy.ServiceProxy('/xarm/move_servo_cart', Move) - req = MoveRequest() - req.pose = step - req.mvvelo = 0 - req.mvacc = 0 - req.mvtime = 1 # tool coordinate motion - loop_num = time_secs*float(freq) - sleep_sec = 1.0/float(freq) - ret = 0 - - try: - for i in range(int(loop_num)): - res = servo_cart(req) - if res.ret: - print("Something Wrong happened calling servo_cart service, index is %d, ret = %d"%(i, res.ret)) - ret = -1 - break - time.sleep(sleep_sec) - return ret - - except rospy.ServiceException as e: - print("servo_cartesian (tool) Service call failed: %s"%e) - return -1 - - -if __name__ == "__main__": - - rospy.wait_for_service('/xarm/move_servo_cart') - rospy.set_param('/xarm/wait_for_finish', True) # return after motion service finish - - motion_en = rospy.ServiceProxy('/xarm/motion_ctrl', SetAxis) - set_mode = rospy.ServiceProxy('/xarm/set_mode', SetInt16) - set_state = rospy.ServiceProxy('/xarm/set_state', SetInt16) - home = rospy.ServiceProxy('/xarm/go_home', Move) - - try: - motion_en(8,1) - set_mode(0) - set_state(0) - req = MoveRequest() # MoveRequest for go_home - req.mvvelo = 0.7 - req.mvacc = 3.5 - req.mvtime = 0 - home(req) - - except rospy.ServiceException as e: - print("Before servo_cartesian, service call failed: %s"%e) - exit(-1) - - # configurations for servo_cartesian - set_mode(1) - set_state(0) - - # relative step for servo_cartesian in TOOL Coordinate (req.mvtime = 1) - step = [0.3, 0, 0, 0, 0, 0] - # publish frequency in Hz: - freq = 100 - # publish time in seconds: - time_secs = 5.0 - - time.sleep(2.0) - if servo_cartesian_tool(step, freq, time_secs) == 0: - print("execution finished successfully!") \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/common/crc16.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/common/crc16.cc deleted file mode 100755 index 6708b16..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/common/crc16.cc +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/common/crc16.h" - -const unsigned char crc_tableh[256] = { - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40 -}; - -const unsigned char crc_tablel[256] = { - 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, - 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, - 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, - 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, - 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, - 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, - 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, - 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, - 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, - 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, - 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, - 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, - 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, - 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, - 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, - 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, - 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, - 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, - 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, - 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, - 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, - 0x41, 0x81, 0x80, 0x40 -}; - -int modbus_crc(unsigned char *data, int len) { - unsigned char init_crch = 0xFF; - unsigned char init_crcl = 0xFF; - int index; - while (len--) { - index = init_crcl ^ *data++; - init_crcl = init_crch ^ crc_tableh[index]; - init_crch = crc_tablel[index]; - } - return ((init_crch << 8) | init_crcl); -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/common/queue_memcpy.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/common/queue_memcpy.cc deleted file mode 100755 index 31562df..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/common/queue_memcpy.cc +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/common/queue_memcpy.h" - -#include - -QueueMemcpy::QueueMemcpy(long n, long n_size) { - total_ = n; - annode_size_ = n_size; - buf_ = new char[total_ * annode_size_]; - pthread_mutex_init(&mutex_, NULL); - flush(); -} - -QueueMemcpy::~QueueMemcpy(void) { delete[] buf_; } - -char QueueMemcpy::flush(void) { - cnt_ = 0; - head_ = 0; - tail_ = 0; - memset(buf_, 0, annode_size_ * total_); - - return 0; -} - -long QueueMemcpy::size(void) { return cnt_; } - -int QueueMemcpy::is_full(void) { - if (total_ <= cnt_) - return 1; - else - return 0; -} - -long QueueMemcpy::node_size(void) { return annode_size_; } - -char QueueMemcpy::pop(void *data) { - pthread_mutex_lock(&mutex_); - if (0 >= cnt_) { - pthread_mutex_unlock(&mutex_); - return -1; - } - if (total_ <= tail_) tail_ = 0; - - memcpy(data, &buf_[tail_ * annode_size_], annode_size_); - tail_++; - cnt_--; - pthread_mutex_unlock(&mutex_); - return 0; -} - -char QueueMemcpy::get(void *data) { - pthread_mutex_lock(&mutex_); - if (0 >= cnt_) { - pthread_mutex_unlock(&mutex_); - return -1; - } - if (total_ <= tail_) tail_ = 0; - - memcpy(data, &buf_[tail_ * annode_size_], annode_size_); - pthread_mutex_unlock(&mutex_); - - return 0; -} - -char QueueMemcpy::push(void *data) { - pthread_mutex_lock(&mutex_); - if (total_ <= cnt_) { - pthread_mutex_unlock(&mutex_); - return -1; - } - if (total_ <= head_) head_ = 0; - - memcpy(&buf_[head_ * annode_size_], data, annode_size_); - head_++; - cnt_++; - pthread_mutex_unlock(&mutex_); - return 0; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/connect.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/connect.cc deleted file mode 100755 index b7c2d32..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/connect.cc +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/connect.h" - -#include "xarm/instruction/uxbus_cmd.h" -#include "xarm/instruction/uxbus_cmd_ser.h" -#include "xarm/instruction/uxbus_cmd_tcp.h" -#include "xarm/xarm_config.h" - -UxbusCmdSer *connect_rs485_control(const char *com) { - SerialPort *arm_port = new SerialPort(com, XARM_CONF::SERIAL_BAUD, 3, 128); - if (arm_port->is_ok() != 0) { - printf("Error: Serial RS485 connection failed\n"); - return NULL; - } - UxbusCmdSer *arm_cmd = new UxbusCmdSer(arm_port); - printf("Serial RS485 connection successful\n"); - return arm_cmd; -} - -UxbusCmdTcp *connect_tcp_control(char *server_ip) { - SocketPort *arm_port = - new SocketPort(server_ip, XARM_CONF::TCP_PORT_CONTROL, 3, 128); - if (arm_port->is_ok() != 0) { - printf("Error: Tcp Control connection failed\n"); - return NULL; - } - UxbusCmdTcp *arm_cmd = new UxbusCmdTcp(arm_port); - printf("Tcp Control connection successful\n"); - return arm_cmd; -} - -SocketPort *connext_tcp_report_norm(char *server_ip) { - SocketPort *arm_report = - new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_NORM, 3, 512); - if (arm_report->is_ok() != 0) { - printf("Error: Tcp Report Norm connection failed, ip: %s\n", server_ip); - return NULL; - } - printf("Tcp Report Norm connection successful\n"); - return arm_report; -} - -SocketPort *connext_tcp_report_rich(char *server_ip) { - SocketPort *arm_report = - new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_RICH, 3, 512); - if (arm_report->is_ok() != 0) { - printf("Error: Tcp Report Norm connection failed\n"); - return NULL; - } - printf("Tcp Report Norm connection successful\n"); - return arm_report; -} - -SocketPort *connext_tcp_report_devl(char *server_ip) { - SocketPort *arm_report = - new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_DEVL, 3, 512); - if (arm_report->is_ok() != 0) { - printf("Error: Tcp Report develop connection failed\n"); - return NULL; - } - printf("Tcp Report develop connection successful\n"); - return arm_report; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/debug/debug_print.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/debug/debug_print.cc deleted file mode 100755 index ccdc672..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/debug/debug_print.cc +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/debug/debug_print.h" - -#include - -#include "xarm/common/data_type.h" - -#define DB_FLG "[deprint ] " -#define PRINTF_NUM_MAX 128 -#define log_put printf - -void print_log(const char *format, ...) { - char buffer[PRINTF_NUM_MAX] = {0}; - va_list arg; - va_start(arg, format); - vsnprintf(buffer, PRINTF_NUM_MAX, format, arg); - printf("%s", buffer); - va_end(arg); -} - -void print_nvect(const char *str, double vect[], int n) { - print_log("%s", str); - for (int i = 0; i < n; ++i) { print_log("%0.3f ", vect[i]); } - print_log("\n"); -} - -void print_nvect(const char *str, float *vect, int n) { - print_log("%s", str); - for (int i = 0; i < n; ++i) { print_log("%0.3f ", vect[i]); } - print_log("\n"); -} - -void print_nvect(const char *str, unsigned char vect[], int n) { - print_log("%s", str); - for (int i = 0; i < n; ++i) { print_log("%d ", vect[i]); } - print_log("\n"); -} - -void print_nvect(const char *str, int vect[], int n) { - print_log("%s", str); - for (int i = 0; i < n; ++i) { print_log("%d ", vect[i]); } - print_log("\n"); -} - -void print_hex(const char *str, unsigned char *hex, int len) { - char buf[len * 3 + 1] = {'\0'}; - long i; - for (i = 0; i < len; ++i) { sprintf((char *)&buf[i * 3], "%02x ", hex[i]); } - - printf("%s %s\n", str, buf); -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd.cc deleted file mode 100644 index 3c3d8b9..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd.cc +++ /dev/null @@ -1,1110 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include -#include "xarm/instruction/uxbus_cmd.h" -#include "xarm/instruction/servo3_config.h" -#include "xarm/instruction/uxbus_cmd_config.h" -#include "xarm/debug/debug_print.h" - -static int BAUDRATES[13] = { 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600, 1000000, 1500000, 2000000, 2500000 }; - -static int get_baud_inx(int baud) { - for (int i = 0; i < 13; i++) { if (BAUDRATES[i] == baud) return i; } - return -1; -} - -UxbusCmd::UxbusCmd(void) {} - -UxbusCmd::~UxbusCmd(void) {} - -int UxbusCmd::check_xbus_prot(unsigned char *data, int funcode) { return -11; } - -int UxbusCmd::send_pend(int funcode, int num, int timeout, unsigned char *rx_data) { - return -11; -} - -int UxbusCmd::send_xbus(int funcode, unsigned char *txdata, int num) { return -11; } - -void UxbusCmd::close(void) {} - - -/******************************************************* - * Uxbus generic protocol function - *******************************************************/ - -int UxbusCmd::set_nu8(int funcode, int *datas, int num) { - std::lock_guard locker(mutex_); - //unsigned char send_data[num]; - unsigned char *send_data = new unsigned char[num]; - for (int i = 0; i < num; i++) - { - send_data[i] = (unsigned char)datas[i]; - } - - int ret = send_xbus(funcode, send_data, num); - delete send_data; - if (ret != 0) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, 0, funcode == UXBUS_RG::MOTION_EN ? UXBUS_CONF::SET_TIMEOUT * 2 : UXBUS_CONF::SET_TIMEOUT, NULL); - return ret; -} - -int UxbusCmd::get_nu8(int funcode, int *rx_data, int num) { - // unsigned char secv_data[num]; - unsigned char *send_data = new unsigned char[num]; - int ret = get_nu8(funcode, send_data, num); - - for (int i = 0; i < num; i++) { - rx_data[i] = send_data[i]; - } - delete send_data; - return ret; -} - -int UxbusCmd::get_nu8(int funcode, unsigned char *rx_data, int num) { - std::lock_guard locker(mutex_); - int ret = send_xbus(funcode, 0, 0); - if (ret != 0) { return UXBUS_STATE::ERR_NOTTCP; } - return send_pend(funcode, num, UXBUS_CONF::GET_TIMEOUT, rx_data); -} - -int UxbusCmd::set_nu16(int funcode, int *datas, int num) { - std::lock_guard locker(mutex_); - //unsigned char send_data[num * 2]; - unsigned char *send_data = new unsigned char[num * 2]; - for (int i = 0; i < num; i++) { - bin16_to_8(datas[i], &send_data[i * 2]); - } - int ret = send_xbus(funcode, send_data, num * 2); - delete send_data; - if (ret != 0) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); - - return ret; -} -int UxbusCmd::get_nu16(int funcode, int *rx_data, int num) { - std::lock_guard locker(mutex_); - //unsigned char datas[num * 2]; - unsigned char *datas = new unsigned char[num * 2]; - int ret = send_xbus(funcode, 0, 0); - if (ret != 0) { - delete datas; - return UXBUS_STATE::ERR_NOTTCP; - } - - ret = send_pend(funcode, num * 2, UXBUS_CONF::GET_TIMEOUT, datas); - for (int i = 0; i < num; i++) { - rx_data[i] = bin8_to_16(&datas[i * 2]); - } - delete datas; - - return ret; -} - -int UxbusCmd::set_nfp32(int funcode, float *datas, int num) { - std::lock_guard locker(mutex_); - // unsigned char hexdata[num * 4] = {0}; - unsigned char *hexdata = new unsigned char[num * 4]; - nfp32_to_hex(datas, hexdata, num); - int ret = send_xbus(funcode, hexdata, num * 4); - delete hexdata; - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); - - return ret; -} - -int UxbusCmd::set_nint32(int funcode, int *datas, int num) { - std::lock_guard locker(mutex_); - //unsigned char hexdata[num * 4] = {0}; \\?? - unsigned char *hexdata = new unsigned char[num * 4]; - nint32_to_hex(datas, hexdata, num); - int ret = send_xbus(funcode, hexdata, num * 4); - delete hexdata; - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); - - return ret; -} - -int UxbusCmd::get_nfp32(int funcode, float *rx_data, int num) { - std::lock_guard locker(mutex_); - //unsigned char datas[num * 4] = {0}; - unsigned char *datas = new unsigned char[num * 4]; - int ret = send_xbus(funcode, 0, 0); - if (0 != ret) { - delete datas; - return UXBUS_STATE::ERR_NOTTCP; - } - ret = send_pend(funcode, num * 4, UXBUS_CONF::GET_TIMEOUT, datas); - hex_to_nfp32(datas, rx_data, num); - delete datas; - return ret; -} - -int UxbusCmd::swop_nfp32(int funcode, float tx_datas[], int txn, float *rx_data, int rxn) { - std::lock_guard locker(mutex_); - // unsigned char hexdata[128] = { 0 }; - unsigned char *hexdata = new unsigned char[128]; - - nfp32_to_hex(tx_datas, hexdata, txn); - int ret = send_xbus(funcode, hexdata, txn * 4); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, rxn * 4, UXBUS_CONF::GET_TIMEOUT, hexdata); - hex_to_nfp32(hexdata, rx_data, rxn); - - return ret; -} - -int UxbusCmd::is_nfp32(int funcode, float datas[], int txn, int *value) { - std::lock_guard locker(mutex_); - //unsigned char hexdata[txn * 4] = {0}; - unsigned char *hexdata = new unsigned char[txn * 4]; - - nfp32_to_hex(datas, hexdata, txn); - int ret = send_xbus(funcode, hexdata, txn * 4); - if (0 != ret) { - delete hexdata; - return UXBUS_STATE::ERR_NOTTCP; - } - ret = send_pend(funcode, 1, UXBUS_CONF::GET_TIMEOUT, hexdata); - *value = hexdata[0]; - delete hexdata; - - return ret; -} - -int UxbusCmd::set_nfp32_with_bytes(int funcode, float *datas, int num, char *additional, int len) { - std::lock_guard locker(mutex_); - unsigned char *hexdata = new unsigned char[num * 4 + len]; - nfp32_to_hex(datas, hexdata, num); - for (int i = 0; i < len; i++) { hexdata[num * 4 + i] = additional[i]; } - int ret = send_xbus(funcode, hexdata, num * 4 + len); - delete hexdata; - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); - - return ret; -} - -/******************************************************* - * controler setting - *******************************************************/ -int UxbusCmd::get_version(unsigned char rx_data[40]) { - return get_nu8(UXBUS_RG::GET_VERSION, rx_data, 40); -} - -int UxbusCmd::get_robot_sn(unsigned char rx_data[40]) { - return get_nu8(UXBUS_RG::GET_ROBOT_SN, rx_data, 40); -} - -int UxbusCmd::check_verification(int *rx_data) { - return get_nu8(UXBUS_RG::CHECK_VERIFY, rx_data, 1); -} - -int UxbusCmd::shutdown_system(int value) { - return set_nu8(UXBUS_RG::SHUTDOWN_SYSTEM, &value, 1); -} - -int UxbusCmd::set_record_traj(int value) { - int txdata[1] = { value }; - return set_nu8(UXBUS_RG::SET_TRAJ_RECORD, txdata, 1); -} - -int UxbusCmd::playback_traj(int times, int spdx) { - int txdata[2] = { times, spdx }; - return set_nint32(UXBUS_RG::PLAY_TRAJ, txdata, 2); -} - -int UxbusCmd::playback_traj_old(int times) { - int txdata[1] = { times }; - return set_nint32(UXBUS_RG::PLAY_TRAJ, txdata, 1); -} - -int UxbusCmd::save_traj(char filename[81]) { - return set_nu8(UXBUS_RG::SAVE_TRAJ, (int *)filename, 81); -} - -int UxbusCmd::load_traj(char filename[81]) { - return set_nu8(UXBUS_RG::LOAD_TRAJ, (int *)filename, 81); -} - -int UxbusCmd::get_traj_rw_status(int *rx_data) { - return get_nu8(UXBUS_RG::GET_TRAJ_RW_STATUS, rx_data, 1); -} - -int UxbusCmd::set_reduced_mode(int on_off) { - int txdata[1] = { on_off }; - return set_nu8(UXBUS_RG::SET_REDUCED_MODE, txdata, 1); -} - -int UxbusCmd::set_reduced_linespeed(float lspd_mm) { - float txdata[1] = { lspd_mm }; - return set_nfp32(UXBUS_RG::SET_REDUCED_TRSV, txdata, 1); -} - -int UxbusCmd::set_reduced_jointspeed(float jspd_rad) { - float txdata[1] = { jspd_rad }; - return set_nfp32(UXBUS_RG::SET_REDUCED_P2PV, txdata, 1); -} - -int UxbusCmd::get_reduced_mode(int *rx_data) { - return get_nu8(UXBUS_RG::GET_REDUCED_MODE, rx_data, 1); -} - -int UxbusCmd::get_reduced_states(int *on, int xyz_list[6], float *tcp_speed, float *joint_speed, float jrange_rad[14], int *fense_is_on, int *collision_rebound_is_on, int length) { - //unsigned char rx_data[length] = {0}; - unsigned char *rx_data = new unsigned char[length]; - int ret = get_nu8(UXBUS_RG::GET_REDUCED_STATE, rx_data, length); - *on = rx_data[0]; - bin8_to_ns16(&rx_data[1], xyz_list, 6); - *tcp_speed = hex_to_fp32(&rx_data[13]); - *joint_speed = hex_to_fp32(&rx_data[17]); - if (length == 79) { - if (jrange_rad != NULL) { hex_to_nfp32(&rx_data[21], jrange_rad, 14); } - if (fense_is_on != NULL) { *fense_is_on = rx_data[77]; } - if (collision_rebound_is_on != NULL) { *collision_rebound_is_on = rx_data[78]; } - } - delete rx_data; - return ret; -} - -int UxbusCmd::set_xyz_limits(int xyz_list[6]) { - return set_nint32(UXBUS_RG::SET_LIMIT_XYZ, xyz_list, 6); -} - -int UxbusCmd::set_world_offset(float pose_offset[6]) { - return set_nfp32(UXBUS_RG::SET_WORLD_OFFSET, pose_offset, 6); -} - -int UxbusCmd::cnter_reset(void) { - return set_nu8(UXBUS_RG::CNTER_RESET, 0, 0); -} - -int UxbusCmd::cnter_plus(void) { - return set_nu8(UXBUS_RG::CNTER_PLUS, 0, 0); -} - -int UxbusCmd::set_reduced_jrange(float jrange_rad[14]) { - return set_nfp32(UXBUS_RG::SET_REDUCED_JRANGE, jrange_rad, 14); -} - -int UxbusCmd::set_fense_on(int on_off) { - int txdata[1] = { on_off }; - return set_nu8(UXBUS_RG::SET_FENSE_ON, txdata, 1); -} - -int UxbusCmd::set_collis_reb(int on_off) { - int txdata[1] = { on_off }; - return set_nu8(UXBUS_RG::SET_COLLIS_REB, txdata, 1); -} - -int UxbusCmd::motion_en(int id, int value) { - int txdata[2] = { id, value }; - return set_nu8(UXBUS_RG::MOTION_EN, txdata, 2); -} - -int UxbusCmd::set_state(int value) { - return set_nu8(UXBUS_RG::SET_STATE, &value, 1); -} - -int UxbusCmd::get_state(int *rx_data) { - return get_nu8(UXBUS_RG::GET_STATE, rx_data, 1); -} - -int UxbusCmd::get_cmdnum(int *rx_data) { - return get_nu16(UXBUS_RG::GET_CMDNUM, rx_data, 1); -} - -int UxbusCmd::get_err_code(int * rx_data) { - return get_nu8(UXBUS_RG::GET_ERROR, rx_data, 2); -} - -int UxbusCmd::get_hd_types(int *rx_data) { - return get_nu8(UXBUS_RG::GET_HD_TYPES, rx_data, 2); -} - -int UxbusCmd::reload_dynamics(void) { - int txdata[1] = { 0 }; - return set_nu8(UXBUS_RG::RELOAD_DYNAMICS, txdata, 0); -} - -int UxbusCmd::clean_err(void) { - int txdata[1] = { 0 }; - return set_nu8(UXBUS_RG::CLEAN_ERR, txdata, 0); -} - -int UxbusCmd::clean_war(void) { - int txdata[1] = { 0 }; - return set_nu8(UXBUS_RG::CLEAN_WAR, txdata, 0); -} - -int UxbusCmd::set_brake(int axis, int en) { - int txdata[2] = { axis, en }; - return set_nu8(UXBUS_RG::SET_BRAKE, txdata, 2); -} - -int UxbusCmd::set_mode(int value) { - int txdata[1] = { value }; - return set_nu8(UXBUS_RG::SET_MODE, txdata, 1); -} - -/******************************************************* - * controler motion - *******************************************************/ -int UxbusCmd::move_line(float mvpose[6], float mvvelo, float mvacc, float mvtime) { - float txdata[9] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_LINE, txdata, 9); -} - -int UxbusCmd::move_lineb(float mvpose[6], float mvvelo, float mvacc, float mvtime, - float mvradii) { - float txdata[10] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = mvtime; - txdata[9] = mvradii; - - return set_nfp32(UXBUS_RG::MOVE_LINEB, txdata, 10); -} - -int UxbusCmd::move_joint(float mvjoint[7], float mvvelo, float mvacc, - float mvtime) { - float txdata[10] = { 0 }; - for (int i = 0; i < 7; i++) { txdata[i] = mvjoint[i]; } - txdata[7] = mvvelo; - txdata[8] = mvacc; - txdata[9] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_JOINT, txdata, 10); -} - -int UxbusCmd::move_line_tool(float mvpose[6], float mvvelo, float mvacc, float mvtime) { - float txdata[9] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_LINE_TOOL, txdata, 9); -} - -int UxbusCmd::move_gohome(float mvvelo, float mvacc, float mvtime) { - float txdata[3] = { 0 }; - txdata[0] = mvvelo; - txdata[1] = mvacc; - txdata[2] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_HOME, txdata, 3); -} - -int UxbusCmd::move_servoj(float mvjoint[7], float mvvelo, float mvacc, float mvtime) { - float txdata[10] = { 0 }; - for (int i = 0; i < 7; i++) { txdata[i] = mvjoint[i]; } - txdata[7] = mvvelo; - txdata[8] = mvacc; - txdata[9] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_SERVOJ, txdata, 10); -} - -int UxbusCmd::move_servo_cartesian(float mvpose[6], float mvvelo, float mvacc, float mvtime) { - float txdata[9] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = mvtime; - return set_nfp32(UXBUS_RG::MOVE_SERVO_CART, txdata, 9); -} - -int UxbusCmd::set_servot(float jnt_taus[7]) { - float txdata[7] = { 0 }; - for (int i = 0; i < 7; i++) { txdata[i] = jnt_taus[i]; } - return set_nfp32(UXBUS_RG::SET_SERVOT, txdata, 7); -} - -int UxbusCmd::get_joint_tau(float jnt_taus[7]) { - return get_nfp32(UXBUS_RG::GET_JOINT_TAU, jnt_taus, 7); -} - -int UxbusCmd::set_safe_level(int level) { - int txdata[1] = { level }; - return set_nu8(UXBUS_RG::SET_SAFE_LEVEL, txdata, 1); -} - -int UxbusCmd::get_safe_level(int *level) { - return get_nu8(UXBUS_RG::GET_SAFE_LEVEL, level, 1); -} - -int UxbusCmd::sleep_instruction(float sltime) { - float txdata[1] = { sltime }; - return set_nfp32(UXBUS_RG::SLEEP_INSTT, txdata, 1); -} - -int UxbusCmd::move_circle(float pose1[6], float pose2[6], float mvvelo, float mvacc, float mvtime, float percent) { - - float txdata[16] = { 0 }; - for (int i = 0; i < 6; i++) { - txdata[i] = pose1[i]; - txdata[6 + i] = pose2[i]; - } - txdata[12] = mvvelo; - txdata[13] = mvacc; - txdata[14] = mvtime; - txdata[15] = percent; - - return set_nfp32(UXBUS_RG::MOVE_CIRCLE, txdata, 16); -} - -int UxbusCmd::set_tcp_jerk(float jerk) { - float txdata[1] = { jerk }; - return set_nfp32(UXBUS_RG::SET_TCP_JERK, txdata, 1); -} - -int UxbusCmd::set_tcp_maxacc(float maxacc) { - float txdata[1] = { maxacc }; - return set_nfp32(UXBUS_RG::SET_TCP_MAXACC, txdata, 1); -} - -int UxbusCmd::set_joint_jerk(float jerk) { - float txdata[1] = { jerk }; - return set_nfp32(UXBUS_RG::SET_JOINT_JERK, txdata, 1); -} - -int UxbusCmd::set_joint_maxacc(float maxacc) { - float txdata[1] = { maxacc }; - return set_nfp32(UXBUS_RG::SET_JOINT_MAXACC, txdata, 1); -} - -int UxbusCmd::set_tcp_offset(float pose_offset[6]) { - return set_nfp32(UXBUS_RG::SET_TCP_OFFSET, pose_offset, 6); -} - -int UxbusCmd::set_tcp_load(float mass, float load_offset[3]) { - float txdata[4] = { mass, load_offset[0], load_offset[1], load_offset[2] }; - return set_nfp32(UXBUS_RG::SET_LOAD_PARAM, txdata, 4); -} - -int UxbusCmd::set_collis_sens(int value) { - return set_nu8(UXBUS_RG::SET_TEACH_SENS, &value, 1); -} - -int UxbusCmd::set_teach_sens(int value) { - return set_nu8(UXBUS_RG::SET_TEACH_SENS, &value, 1); -} - -int UxbusCmd::set_gravity_dir(float gravity_dir[3]) { - return set_nfp32(UXBUS_RG::SET_GRAVITY_DIR, gravity_dir, 3); -} - -int UxbusCmd::clean_conf() { - return set_nu8(UXBUS_RG::CLEAN_CONF, 0, 0); -} - -int UxbusCmd::save_conf() { - return set_nu8(UXBUS_RG::SAVE_CONF, 0, 0); -} - -int UxbusCmd::get_tcp_pose(float pose[6]) { - return get_nfp32(UXBUS_RG::GET_TCP_POSE, pose, 6); -} - -int UxbusCmd::get_joint_pose(float angles[7]) { - return get_nfp32(UXBUS_RG::GET_JOINT_POS, angles, 7); -} - -int UxbusCmd::get_ik(float pose[6], float angles[7]) { - return swop_nfp32(UXBUS_RG::GET_IK, pose, 6, angles, 7); -} - -int UxbusCmd::get_fk(float angles[7], float pose[6]) { - return swop_nfp32(UXBUS_RG::GET_FK, angles, 7, pose, 6); -} - -int UxbusCmd::is_joint_limit(float joint[7], int *value) { - return is_nfp32(UXBUS_RG::IS_JOINT_LIMIT, joint, 7, value); -} - -int UxbusCmd::is_tcp_limit(float pose[6], int *value) { - return is_nfp32(UXBUS_RG::IS_TCP_LIMIT, pose, 6, value); -} - -/******************************************************* - * gripper - *******************************************************/ -int UxbusCmd::gripper_addr_w16(int addr, float value) { - // unsigned char txdata[7]; - std::lock_guard locker(mutex_); - unsigned char *txdata = new unsigned char[7]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - int ret = send_xbus(UXBUS_RG::TGPIO_W16B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_W16B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); -} - -int UxbusCmd::gripper_addr_r16(int addr, float *value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - bin16_to_8(addr, &txdata[1]); - int ret = send_xbus(UXBUS_RG::TGPIO_R16B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_R16B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} - -int UxbusCmd::gripper_addr_w32(int addr, float value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[7]; - unsigned char *txdata = new unsigned char[7]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - int ret = send_xbus(UXBUS_RG::TGPIO_W32B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_W32B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); -} - -int UxbusCmd::gripper_addr_r32(int addr, float *value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - bin16_to_8(addr, &txdata[1]); - int ret = send_xbus(UXBUS_RG::TGPIO_R32B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - ret = send_pend(UXBUS_RG::TGPIO_R32B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} - -int UxbusCmd::gripper_set_en(int value) { - return gripper_addr_w16(SERVO3_RG::CON_EN, (float)value); -} - -int UxbusCmd::gripper_set_mode(int value) { - return gripper_addr_w16(SERVO3_RG::CON_MODE, (float)value); -} - -int UxbusCmd::gripper_set_zero() { - return gripper_addr_w16(SERVO3_RG::MT_ZERO, 1); -} - -int UxbusCmd::gripper_get_pos(float *pulse) { - return gripper_addr_r32(SERVO3_RG::CURR_POS, pulse); -} - -int UxbusCmd::gripper_set_pos(float pulse) { - return gripper_addr_w32(SERVO3_RG::TAGET_POS, pulse); -} - -int UxbusCmd::gripper_set_posspd(float speed) { - return gripper_addr_w16(SERVO3_RG::POS_SPD, speed); -} - -int UxbusCmd::gripper_get_errcode(int rx_data[2]) { - return get_nu8(UXBUS_RG::TGPIO_ERR, rx_data, 2); -} - -int UxbusCmd::gripper_clean_err() { - return gripper_addr_w16(SERVO3_RG::RESET_ERR, 1); -} - -/******************************************************* - * tool gpio - *******************************************************/ -int UxbusCmd::tgpio_addr_w16(int addr, float value) { - // unsigned char txdata[7]; - unsigned char *txdata = new unsigned char[7]; - txdata[0] = UXBUS_CONF::TGPIO_ID; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::TGPIO_W16B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_W16B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); -} - -int UxbusCmd::tgpio_addr_r16(int addr, float *value) { - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = UXBUS_CONF::TGPIO_ID; - bin16_to_8(addr, &txdata[1]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::TGPIO_R16B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - ret = send_pend(UXBUS_RG::TGPIO_R16B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} -int UxbusCmd::tgpio_addr_w32(int addr, float value) { - // unsigned char txdata[7]; - unsigned char *txdata = new unsigned char[7]; - txdata[0] = UXBUS_CONF::TGPIO_ID; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::TGPIO_W32B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_W32B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); -} - -int UxbusCmd::tgpio_addr_r32(int addr, float *value) { - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = UXBUS_CONF::TGPIO_ID; - bin16_to_8(addr, &txdata[1]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::TGPIO_R32B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::TGPIO_R32B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} - -int UxbusCmd::tgpio_get_digital(int *io1, int *io2) { - float tmp; - int ret = tgpio_addr_r16(SERVO3_RG::DIGITAL_IN, &tmp); - - *io1 = (int)tmp & 0x0001; - *io2 = ((int)tmp & 0x0002) >> 1; - return ret; -} - -int UxbusCmd::tgpio_set_digital(int ionum, int value) { - int tmp = 0; - if (ionum == 1) { - tmp = tmp | 0x0100; - if (value) - { - tmp = tmp | 0x0001; - } - } - else if (ionum == 2) { - tmp = tmp | 0x0200; - if (value) - { - tmp = tmp | 0x0002; - } - } - else { - return -1; - } - return tgpio_addr_w16(SERVO3_RG::DIGITAL_OUT, (float)tmp); -} - -int UxbusCmd::tgpio_get_analog1(float * value) { - float tmp; - int ret = tgpio_addr_r16(SERVO3_RG::ANALOG_IO1, &tmp); - *value = (float)(tmp * 3.3 / 4096.0); - return ret; -} - -int UxbusCmd::tgpio_get_analog2(float * value) { - float tmp; - int ret = tgpio_addr_r16(SERVO3_RG::ANALOG_IO2, &tmp); - // printf("tmp = %f\n", tmp); - *value = (float)(tmp * 3.3 / 4096.0); - return ret; -} - -/******************************************************* - * tgpio modbus - *******************************************************/ - -int UxbusCmd::set_modbus_timeout(int value) { - return set_nu16(UXBUS_RG::TGPIO_MB_TIOUT, &value, 1); -} - -int UxbusCmd::set_modbus_baudrate(int baud) { - float val; - int baud_inx = get_baud_inx(baud); - if (baud_inx == -1) return -1; - int ret = tgpio_addr_r16(SERVO3_RG::MODBUS_BAUDRATE & 0x0FFF, &val); - if (ret == 0) { - int baud_i = (int)val; - if (baud_i != baud_inx) { - tgpio_addr_w16(SERVO3_RG::MODBUS_BAUDRATE, baud_inx); - tgpio_addr_w16((0x1000 | SERVO3_RG::MODBUS_BAUDRATE), baud_inx); - usleep(1e4); // 10ms - return tgpio_addr_w16(SERVO3_RG::SOFT_REBOOT, 1); - } - } - return ret; -} - -int UxbusCmd::tgpio_set_modbus(unsigned char *modbus_t, int len_t, unsigned char *rx_data) { - //unsigned char txdata[len_t + 1]; - unsigned char *txdata = new unsigned char[len_t + 1]; - txdata[0] = UXBUS_CONF::TGPIO_ID; - for (int i = 0; i < len_t; i++) { txdata[i + 1] = modbus_t[i]; } - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::TGPIO_MODBUS, txdata, len_t + 1); - delete txdata; - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - ret = send_pend(UXBUS_RG::TGPIO_MODBUS, -1, UXBUS_CONF::SET_TIMEOUT, rx_data); - return ret; -} - -int UxbusCmd::gripper_modbus_w16s(int addr, float value, int len) { - // unsigned char txdata[9], rx_data[254]; - unsigned char *txdata = new unsigned char[9]; - unsigned char *rx_data = new unsigned char[254]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - txdata[1] = 0x10; - bin16_to_8(addr, &txdata[2]); - bin16_to_8(len, &txdata[4]); - txdata[6] = len * 2; - fp32_to_hex(value, &txdata[7]); - return tgpio_set_modbus(txdata, len * 2 + 7, rx_data); -} - -int UxbusCmd::gripper_modbus_r16s(int addr, int len, unsigned char *rx_data) { - // unsigned char txdata[6]; - unsigned char *txdata = new unsigned char[6]; - txdata[0] = UXBUS_CONF::GRIPPER_ID; - txdata[1] = 0x03; - bin16_to_8(addr, &txdata[2]); - bin16_to_8(len, &txdata[4]); - return tgpio_set_modbus(txdata, 6, rx_data); -} - -int UxbusCmd::gripper_modbus_set_en(int value) { - // unsigned char txdata[2] = { 0 }; - unsigned char *txdata = new unsigned char[2]; - bin16_to_8(value, &txdata[0]); - float _value = hex_to_fp32(txdata); - return gripper_modbus_w16s(SERVO3_RG::CON_EN, _value, 1); -} - -int UxbusCmd::gripper_modbus_set_mode(int value) { - // unsigned char txdata[2]; - unsigned char *txdata = new unsigned char[2]; - bin16_to_8(value, &txdata[0]); - float _value = hex_to_fp32(txdata); - return gripper_modbus_w16s(SERVO3_RG::CON_MODE, _value, 1); -} - -int UxbusCmd::gripper_modbus_set_zero(void) { - return gripper_modbus_w16s(SERVO3_RG::MT_ZERO, 1, 1); -} - -int UxbusCmd::gripper_modbus_get_pos(float *pulse) { - // unsigned char rx_data[254]; - unsigned char *rx_data = new unsigned char[254]; - int ret = gripper_modbus_r16s(SERVO3_RG::CURR_POS, 2, rx_data); - *pulse = (float)bin8_to_32(&rx_data[4]); - return ret; -} - -int UxbusCmd::gripper_modbus_set_pos(float pulse) { - // unsigned char txdata[4]; - unsigned char *txdata = new unsigned char[4]; - txdata[0] = ((int)pulse >> 24) & 0xFF; - txdata[1] = ((int)pulse >> 16) & 0xFF; - txdata[2] = ((int)pulse >> 8) & 0xFF; - txdata[3] = (int)pulse & 0xFF; - float value = hex_to_fp32(txdata); - return gripper_modbus_w16s(SERVO3_RG::TAGET_POS, value, 2); -} - -int UxbusCmd::gripper_modbus_set_posspd(float speed) { - // unsigned char txdata[2]; - unsigned char *txdata = new unsigned char[2]; - bin16_to_8((int)speed, &txdata[0]); - float value = hex_to_fp32(txdata); - return gripper_modbus_w16s(SERVO3_RG::POS_SPD, value, 1); -} - -int UxbusCmd::gripper_modbus_get_errcode(int *err) { - // unsigned char rx_data[254]; - unsigned char *rx_data = new unsigned char[254]; - int ret = gripper_modbus_r16s(SERVO3_RG::ERR_CODE, 1, rx_data); - *err = bin8_to_16(&rx_data[4]); - return ret; -} - -int UxbusCmd::gripper_modbus_clean_err(void) { - return gripper_modbus_w16s(SERVO3_RG::RESET_ERR, 1, 1); -} - -/******************************************************* - * uservo - *******************************************************/ -int UxbusCmd::servo_set_zero(int id) { - return set_nu8(UXBUS_RG::SERVO_ZERO, &id, 1); -} - -int UxbusCmd::servo_get_dbmsg(int rx_data[16]) { - return get_nu8(UXBUS_RG::SERVO_DBMSG, rx_data, 16); -} - -int UxbusCmd::servo_addr_w16(int id, int addr, float value) { - // unsigned char txdata[7]; - std::lock_guard locker(mutex_); - unsigned char *txdata = new unsigned char[7]; - txdata[0] = id; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - int ret = send_xbus(UXBUS_RG::SERVO_W16B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::SERVO_W16B, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::servo_addr_r16(int id, int addr, float *value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = id; - bin16_to_8(addr, &txdata[1]); - int ret = send_xbus(UXBUS_RG::SERVO_R16B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - ret = send_pend(UXBUS_RG::SERVO_R16B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} - -int UxbusCmd::servo_addr_w32(int id, int addr, float value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[7]; - unsigned char *txdata = new unsigned char[7]; - txdata[0] = id; - bin16_to_8(addr, &txdata[1]); - fp32_to_hex(value, &txdata[3]); - int ret = send_xbus(UXBUS_RG::SERVO_W32B, txdata, 7); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::SERVO_W32B, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::servo_addr_r32(int id, int addr, float *value) { - std::lock_guard locker(mutex_); - // unsigned char txdata[3], rx_data[4]; - unsigned char *txdata = new unsigned char[3]; - unsigned char *rx_data = new unsigned char[4]; - txdata[0] = id; - bin16_to_8(addr, &txdata[1]); - int ret = send_xbus(UXBUS_RG::SERVO_R32B, txdata, 3); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - ret = send_pend(UXBUS_RG::SERVO_R32B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); - *value = (float)bin8_to_32(rx_data); - return ret; -} - - - -/******************************************************* - * controler gpio - *******************************************************/ -int UxbusCmd::cgpio_get_auxdigit(int *value) { - return get_nu16(UXBUS_RG::CGPIO_GET_DIGIT, value, 1); -} -int UxbusCmd::cgpio_get_analog1(float *value) { - int tmp; - int ret = get_nu16(UXBUS_RG::CGPIO_GET_ANALOG1, &tmp, 1); - *value = (float)(tmp * 10.0 / 4096.0); - return ret; -} -int UxbusCmd::cgpio_get_analog2(float *value) { - int tmp; - int ret = get_nu16(UXBUS_RG::CGPIO_GET_ANALOG2, &tmp, 1); - *value = (float)(tmp * 10.0 / 4096.0); - return ret; -} -/** - * - * @method cgpio_set_auxdigit - * @param ionum [0~7] - * @param value [0 / 1] - * @return [description] - */ - -int UxbusCmd::cgpio_set_auxdigit(int ionum, int value) { - if (ionum > 7) - { - return -99; - } - - int tmp = 0; - tmp = tmp | (0x0100 << ionum); - if (value) - { - tmp = tmp | (0x0001 << ionum); - } - return set_nu16(UXBUS_RG::CGPIO_SET_DIGIT, &tmp, 1); -} -int UxbusCmd::cgpio_set_analog1(int value) { - value = (int)(value / 10.0 * 4096.0); - return set_nu16(UXBUS_RG::CGPIO_SET_ANALOG1, &value, 1); -} -int UxbusCmd::cgpio_set_analog2(int value) { - value = (int)(value / 10.0 * 4096.0); - return set_nu16(UXBUS_RG::CGPIO_SET_ANALOG2, &value, 1); -} - -int UxbusCmd::cgpio_set_infun(int num, int fun) { - int txdata[2] = { num, fun }; - return set_nu8(UXBUS_RG::CGPIO_SET_IN_FUN, txdata, 2); -} -int UxbusCmd::cgpio_set_outfun(int num, int fun) { - int txdata[2] = { num, fun }; - return set_nu8(UXBUS_RG::CGPIO_SET_OUT_FUN, txdata, 2); -} - -/** - * get controler gpio all state infomation - * @method UxbusCmd::cgpio_get_state - * @param state [description] - * @param digit_io [digital input functional gpio state, - digital input configuring gpio state, - digital output functional gpio state, - digital output configuring gpio state] - * @param analog [analog-1 input value, - analog-2 input value, - analog-1 output value, - analog-2 output value] - * @param input_conf [digital input functional info] - * @param output_conf [digital output functional info] - * @return [description] - */ - -int UxbusCmd::cgpio_get_state(int *state, int *digit_io, float *analog, int *input_conf, int *output_conf) { - // unsigned char rx_data[34] = { 0 }; - unsigned char *rx_data = new unsigned char[34]; - int ret = get_nu8(UXBUS_RG::CGPIO_GET_STATE, rx_data, 34); - - state[0] = rx_data[0]; - state[1] = rx_data[1]; - for (int i = 0; i < 4; i++) { - digit_io[i] = bin8_to_16(&rx_data[2 + i * 2]); - analog[i] = (float)(bin8_to_16(&rx_data[10 + i * 2]) / 4096.0 * 10.0); - } - for (int i = 0; i < 8; i++) { - input_conf[i] = rx_data[18 + i]; - output_conf[i] = rx_data[26 + i]; - } - return ret; -} - -int UxbusCmd::get_pose_offset(float pose1[6], float pose2[6], float offset[6], int orient_type_in, int orient_type_out) { - float txdata[14] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = pose1[i]; } - for (int i = 0; i < 6; i++) { txdata[6+i] = pose2[i]; } - unsigned char *hexdata = new unsigned char[50]; - nfp32_to_hex(txdata, hexdata, 12); - hexdata[48] = orient_type_in; - hexdata[49] = orient_type_out; - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::MOVE_LINE_AA, hexdata, 50); - delete hexdata; - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - unsigned char *datas = new unsigned char[24]; - ret = send_pend(UXBUS_RG::MOVE_LINE_AA, 24, UXBUS_CONF::GET_TIMEOUT, datas); - hex_to_nfp32(datas, offset, 6); - delete datas; - return ret; -} - -int UxbusCmd::get_position_aa(float pose[6]) { - return get_nfp32(UXBUS_RG::GET_TCP_POSE_AA, pose, 6); -} - -int UxbusCmd::move_line_aa(float mvpose[6], float mvvelo, float mvacc, float mvtime, int mvcoord, int relative) { - float txdata[9] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = mvtime; - char additional[2] = { (char)mvcoord, (char)relative }; - return set_nfp32_with_bytes(UXBUS_RG::MOVE_LINE_AA, txdata, 9, additional, 2); -} - -int UxbusCmd::move_servo_cart_aa(float mvpose[6], float mvvelo, float mvacc, int tool_coord, int relative) { - float txdata[9] = { 0 }; - for (int i = 0; i < 6; i++) { txdata[i] = mvpose[i]; } - txdata[6] = mvvelo; - txdata[7] = mvacc; - txdata[8] = (char)tool_coord; - char additional[1] = { (char)relative }; - return set_nfp32_with_bytes(UXBUS_RG::MOVE_SERVO_CART_AA, txdata, 9, additional, 1); -} - -int UxbusCmd::tgpio_delay_set_digital(int ionum, int value, float delay_sec) { - unsigned char *txdata = new unsigned char[6]; - txdata[0] = ionum; - txdata[1] = value; - fp32_to_hex(delay_sec, &txdata[2]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::DELAYED_TGPIO_SET, txdata, 6); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::DELAYED_TGPIO_SET, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::cgpio_delay_set_digital(int ionum, int value, float delay_sec) { - unsigned char *txdata = new unsigned char[6]; - txdata[0] = ionum; - txdata[1] = value; - fp32_to_hex(delay_sec, &txdata[2]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::DELAYED_CGPIO_SET, txdata, 6); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::DELAYED_CGPIO_SET, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::tgpio_position_set_digital(int ionum, int value, float xyz[3], float tol_r) { - unsigned char *txdata = new unsigned char[18]; - txdata[0] = ionum; - txdata[1] = value; - nfp32_to_hex(xyz, &txdata[2], 3); - fp32_to_hex(tol_r, &txdata[14]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::POSITION_TGPIO_SET, txdata, 6); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::POSITION_TGPIO_SET, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::cgpio_position_set_digital(int ionum, int value, float xyz[3], float tol_r) { - unsigned char *txdata = new unsigned char[18]; - txdata[0] = ionum; - txdata[1] = value; - nfp32_to_hex(xyz, &txdata[2], 3); - fp32_to_hex(tol_r, &txdata[14]); - std::lock_guard locker(mutex_); - int ret = send_xbus(UXBUS_RG::POSITION_CGPIO_SET, txdata, 6); - if (0 != ret) { return UXBUS_STATE::ERR_NOTTCP; } - - return send_pend(UXBUS_RG::POSITION_CGPIO_SET, 0, UXBUS_CONF::SET_TIMEOUT, NULL); -} - -int UxbusCmd::config_io_stop_reset(int io_type, int val) { - int txdata[2] = { io_type, val }; - return set_nu8(UXBUS_RG::SET_IO_STOP_RESET, txdata, 2); -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_ser.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_ser.cc deleted file mode 100755 index f215a1b..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_ser.cc +++ /dev/null @@ -1,64 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/instruction/uxbus_cmd_ser.h" - -#include - -#include "xarm/common/crc16.h" -#include "xarm/debug/debug_print.h" -#include "xarm/instruction/uxbus_cmd_config.h" - -UxbusCmdSer::UxbusCmdSer(SerialPort *arm_port) { arm_port_ = arm_port; } -UxbusCmdSer::~UxbusCmdSer(void) {} - -int UxbusCmdSer::check_xbus_prot(unsigned char *datas, int funcode) { - if (datas[3] & 0x40) - { return UXBUS_STATE::ERR_CODE; } - else - if (datas[3] & 0x20) - { return UXBUS_STATE::WAR_CODE; } - else - { return 0; } -} - -int UxbusCmdSer::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) { - int ret; - unsigned char rx_data[arm_port_->que_maxlen_] = {0}; - int times = timeout; - while (times) { - times -= 1; - ret = arm_port_->read_frame(rx_data); - if (ret != -1) { - ret = check_xbus_prot(rx_data, funcode); - for (int i = 0; i < num; i++) { ret_data[i] = rx_data[i + 4]; } - return ret; - } - usleep(1000); - } - return UXBUS_STATE::ERR_TOUT; -} - -int UxbusCmdSer::send_xbus(int funcode, unsigned char *datas, int num) { - int i; - unsigned char send_data[num + 4]; - - send_data[0] = UXBUS_CONF::MASTER_ID; - send_data[1] = UXBUS_CONF::SLAVE_ID; - send_data[2] = num + 1; - send_data[3] = funcode; - for (i = 0; i < num; i++) { send_data[4 + i] = datas[i]; } - - int crc = modbus_crc(send_data, 4 + num); - send_data[4 + num] = (unsigned char)(crc & 0xFF); - send_data[5 + num] = (unsigned char)((crc >> 8) & 0xFF); - - arm_port_->flush(); - int ret = arm_port_->write_frame(send_data, num + 6); - return ret; -} - -void UxbusCmdSer::close(void) { arm_port_->close_port(); } diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_tcp.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_tcp.cc deleted file mode 100755 index b4a0e33..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/instruction/uxbus_cmd_tcp.cc +++ /dev/null @@ -1,94 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/instruction/uxbus_cmd_tcp.h" - -#include - -#include "xarm/debug/debug_print.h" -#include "xarm/instruction/uxbus_cmd_config.h" - -UxbusCmdTcp::UxbusCmdTcp(SocketPort *arm_port) { - arm_port_ = arm_port; - bus_flag_ = TX2_BUS_FLAG_MIN_; - prot_flag_ = TX2_PROT_CON_; -} - -UxbusCmdTcp::~UxbusCmdTcp(void) {} - -int UxbusCmdTcp::check_xbus_prot(unsigned char *datas, int funcode) { - unsigned char *data_fp = &datas[4]; - - int sizeof_data = bin8_to_32(datas); - if (sizeof_data < 8 || sizeof_data >= arm_port_->que_maxlen_) - { return UXBUS_STATE::ERR_LENG; } - - int num = bin8_to_16(&data_fp[0]); - int prot = bin8_to_16(&data_fp[2]); - int len = bin8_to_16(&data_fp[4]); - int fun = data_fp[6]; - int state = data_fp[7]; - - int bus_flag = bus_flag_; - if (bus_flag == TX2_BUS_FLAG_MIN_) - { bus_flag = TX2_BUS_FLAG_MAX_; } - else - { bus_flag -= 1; } - - if (num != bus_flag) { /*fprintf(stderr, "expect flag: %d, recv: %d, tar_fun: %d\n", bus_flag, num, funcode);*/ return UXBUS_STATE::ERR_NUM; } - if (prot != TX2_PROT_CON_) { return UXBUS_STATE::ERR_PROT; } - if (fun != funcode) { /*fprintf(stderr, "expect funcode: %d, recv: %d, tar_flag: %d\n", funcode, fun, bus_flag);*/ return UXBUS_STATE::ERR_FUN; } - if (state & 0x40) { return UXBUS_STATE::ERR_CODE; } - if (state & 0x20) { return UXBUS_STATE::WAR_CODE; } - if (sizeof_data != len + 6) { return UXBUS_STATE::ERR_LENG; } - return 0; -} - -int UxbusCmdTcp::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) { - int i; - int ret; - unsigned char rx_data[arm_port_->que_maxlen_] = {0}; - int times = timeout; - while (times) { - times -= 1; - ret = arm_port_->read_frame(rx_data); - if (ret != -1) { - ret = check_xbus_prot(rx_data, funcode); - - int n = num; - if (num == -1) { - n = rx_data[9] - 2; - } - for (i = 0; i < n; i++) { ret_data[i] = rx_data[i + 8 + 4]; } - // print_hex(" 3", rx_data, num + 8 + 4); - return ret; - } - usleep(1000); - } - return UXBUS_STATE::ERR_TOUT; -} - -int UxbusCmdTcp::send_xbus(int funcode, unsigned char *datas, int num) { - int len = num + 7; - unsigned char send_data[len]; - bin16_to_8(bus_flag_, &send_data[0]); - bin16_to_8(prot_flag_, &send_data[2]); - bin16_to_8(num + 1, &send_data[4]); - send_data[6] = funcode; - - for (int i = 0; i < num; i++) { send_data[7 + i] = datas[i]; } - arm_port_->flush(); - // print_hex("send:", send_data, num + 7); - int ret = arm_port_->write_frame(send_data, len); - if (ret != len) { return -1; } - - bus_flag_ += 1; - if (bus_flag_ > TX2_BUS_FLAG_MAX_) { bus_flag_ = TX2_BUS_FLAG_MIN_; } - - return 0; -} - -void UxbusCmdTcp::close(void) { arm_port_->close_port(); } \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/linux/network.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/linux/network.cc deleted file mode 100755 index 25ce082..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/linux/network.cc +++ /dev/null @@ -1,89 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/linux/network.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#define DB_FLG "[net work] " -#define PRINT_ERR printf - -#define PERRNO(ret, db_flg, str) \ - { \ - if (-1 == ret) { \ - PRINT_ERR("%s%s\n", db_flg, str); \ - return -1; \ - } \ - \ -} - -int socket_init(char *local_ip, int port, int is_server) { - int sockfd = socket(AF_INET, SOCK_STREAM, 0); - PERRNO(sockfd, DB_FLG, "error: socket"); - - int on = 1; - int keepAlive = 1; // Turn on keepalive attribute - int keepIdle = 1; // If there is no data in n seconds, probe - int keepInterval = 1; // Detection interval,5 seconds - int keepCount = 3; // 3 detection attempts - struct timeval timeout = {2, 0}; - - int ret = - setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void *)&on, sizeof(on)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, (void *)&keepAlive, - sizeof(keepAlive)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPIDLE, (void *)&keepIdle, - sizeof(keepIdle)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPINTVL, (void *)&keepInterval, - sizeof(keepInterval)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPCNT, (void *)&keepCount, - sizeof(keepCount)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - ret = setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, - sizeof(struct timeval)); - PERRNO(ret, DB_FLG, "error: setsockopt"); - - if (is_server) { - struct sockaddr_in local_addr; - local_addr.sin_family = AF_INET; - local_addr.sin_port = htons(port); - local_addr.sin_addr.s_addr = inet_addr(local_ip); - ret = bind(sockfd, (struct sockaddr *)&local_addr, sizeof(local_addr)); - PERRNO(ret, DB_FLG, "error: bind"); - - int ret = listen(sockfd, 10); - PERRNO(ret, DB_FLG, "error: listen"); - } - return sockfd; -} - -int socket_connect_server(int *socket, char server_ip[], int server_port) { - struct sockaddr_in server_addr; - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(server_port); - inet_aton(server_ip, &server_addr.sin_addr); - int ret = - connect(*socket, (struct sockaddr *)&server_addr, sizeof(server_addr)); - PERRNO(ret, DB_FLG, "error: connect"); - return 0; -} - -int socket_send_data(int client_fp, unsigned char *data, int len) { - int ret = send(client_fp, (void *)data, len, 0); - if (ret == -1) { PRINT_ERR(DB_FLG "error: socket_send_data\n"); } - return ret; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/linux/thread.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/linux/thread.cc deleted file mode 100755 index ccdc827..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/linux/thread.cc +++ /dev/null @@ -1,25 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/linux/thread.h" - -#include - -#define PRINT_ERR printf - -pthread_t thread_init(fun_point_t fun_point, void *arg) { - pthread_t id; - pthread_attr_t attr; - - pthread_attr_init(&attr); - pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); - char ret = pthread_create(&id, &attr, fun_point, arg); - if (0 != ret) PRINT_ERR("error: pthread create failes\n"); - - return id; -} - -void thread_delete(pthread_t id) { pthread_cancel(id); } diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/port/serial.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/port/serial.cc deleted file mode 100755 index 5fb6e54..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/port/serial.cc +++ /dev/null @@ -1,229 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/port/serial.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "xarm/common/crc16.h" -#include "xarm/linux/thread.h" - -void SerialPort::recv_proc(void) { - unsigned char ch; - int ret; - while (state_ == 0) { - ret = read_char(&ch); - - if (ret >= 0) { - parse_put(&ch, 1); - continue; - } - usleep(1000); - } -} - -static void *recv_proc_(void *arg) { - SerialPort *my_this = (SerialPort *)arg; - - my_this->recv_proc(); - - pthread_exit(0); -} - -SerialPort::SerialPort(const char *port, int baud, int que_num, - int que_maxlen) { - que_num_ = que_num; - que_maxlen_ = que_maxlen; - rx_que_ = new QueueMemcpy(que_num_, que_maxlen_); - - int ret = init_serial(port, baud); - if (ret == -1) - { state_ = -1; } - else - { state_ = 0; } - - if (state_ == -1) { return; } - - UXBUS_PROT_FROMID_ = 0x55; - UXBUS_PROT_TOID_ = 0xAA; - flush(); - thread_id_ = thread_init(recv_proc_, this); -} - -SerialPort::~SerialPort(void) { - state_ = -1; - delete rx_que_; -} - -int SerialPort::is_ok(void) { return state_; } - -void SerialPort::flush(void) { - rx_que_->flush(); - rx_data_idx_ = 0; - rx_state_ = UXBUS_START_FROMID; -} - -int SerialPort::read_char(unsigned char *ch) { return (read(fp_, ch, 1) == 1) ? 0 : -1; } - -int SerialPort::read_frame(unsigned char *data) { - if (state_ != 0) { return -1; } - - if (rx_que_->size() == 0) { return -1; } - - rx_que_->pop(data); - return 0; -} - -int SerialPort::write_char(unsigned char ch) { - return ((write(fp_, &ch, 1) == 1) ? 0 : -1); -} - -int SerialPort::write_frame(unsigned char *data, int len) { - if (write(fp_, data, len) != len) { return -1; } - return 0; -} - -void SerialPort::close_port(void) { - state_ = -1; - close(fp_); - delete rx_que_; -} - -void SerialPort::parse_put(unsigned char *data, int len) { - unsigned char ch; - - for (int i = 0; i < len; i++) { - ch = data[i]; - // printf("---state = %d, ch = %x\n", rx_state_, ch); - switch (rx_state_) { - case UXBUS_START_FROMID: - if (UXBUS_PROT_FROMID_ == ch) { - rx_buf_[0] = ch; - rx_state_ = UXBUS_START_TOOID; - } - break; - - case UXBUS_START_TOOID: - if (UXBUS_PROT_TOID_ == ch) { - rx_buf_[1] = ch; - rx_state_ = UXBUS_STATE_LENGTH; - } else { - rx_state_ = UXBUS_START_FROMID; - } - break; - - case UXBUS_STATE_LENGTH: - if (0 < ch && ch < (que_maxlen_ - 5)) { - rx_buf_[2] = ch; - rx_length_ = ch; - rx_data_idx_ = 3; - rx_state_ = UXBUS_STATE_DATA; - } else { - rx_state_ = UXBUS_START_FROMID; - } - break; - - case UXBUS_STATE_DATA: - if (rx_data_idx_ < rx_length_ + 3) { - rx_buf_[rx_data_idx_++] = ch; - if (rx_data_idx_ == rx_length_ + 3) { - rx_state_ = UXBUS_STATE_CRC1; - } - } else { - rx_state_ = UXBUS_START_FROMID; - } - break; - - case UXBUS_STATE_CRC1: - rx_buf_[rx_length_ + 3] = ch; - rx_state_ = UXBUS_STATE_CRC2; - break; - - case UXBUS_STATE_CRC2: - int crc, crc_r; - rx_buf_[rx_length_ + 4] = ch; - crc = modbus_crc(rx_buf_, rx_length_ + 3); - crc_r = (rx_buf_[rx_length_ + 4] << 8) + rx_buf_[rx_length_ + 3]; - if (crc == crc_r) { - rx_que_->push(rx_buf_); - } - rx_state_ = UXBUS_START_FROMID; - break; - - default: - rx_state_ = UXBUS_START_FROMID; - break; - } - } -} - -int SerialPort::init_serial(const char *port, int baud) { - speed_t speed; - struct termios options; - - fp_ = open((const char *)port, O_RDWR | O_NOCTTY); - if (-1 == fp_) { return -1; } - - fcntl(fp_, F_SETFL, FNDELAY); - tcgetattr(fp_, &options); - bzero(&options, sizeof(options)); - - switch (baud) { - case 110: - speed = B110; - break; - case 300: - speed = B300; - break; - case 600: - speed = B600; - break; - case 1200: - speed = B1200; - break; - case 2400: - speed = B2400; - break; - case 4800: - speed = B4800; - break; - case 9600: - speed = B9600; - break; - case 19200: - speed = B19200; - break; - case 38400: - speed = B38400; - break; - case 57600: - speed = B57600; - break; - case 115200: - speed = B115200; - break; - case 921600: - speed = B921600; - break; - } - - cfsetispeed(&options, speed); - cfsetospeed(&options, speed); - - options.c_oflag &= ~OPOST; - options.c_cc[VTIME] = 200; - options.c_cc[VMIN] = 10; - tcsetattr(fp_, TCSANOW, &options); - return 0; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/port/socket.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/port/socket.cc deleted file mode 100755 index e75c8f6..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/port/socket.cc +++ /dev/null @@ -1,91 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/port/socket.h" - -#include -#include -#include -#include - -#include "xarm/linux/network.h" -#include "xarm/linux/thread.h" - -void SocketPort::recv_proc(void) { - int num; - unsigned char recv_data[que_maxlen_]; - while (state_ == 0) { - bzero(recv_data, que_maxlen_); - num = recv(fp_, (void *)&recv_data[4], que_maxlen_ - 1, 0); - if (num <= 0) { - // in case recv() blocking call is interrupted by a system signal, this should not be considered as socket failure. - if(errno == EINTR) - { - printf("EINTR occured\n"); - continue; - } - - close_port(); - printf("SocketPort::recv_proc exit, %d\n", fp_); - pthread_exit(0); - } - bin32_to_8(num, &recv_data[0]); - rx_que_->push(recv_data); - } -} - -static void *recv_proc_(void *arg) { - SocketPort *my_this = (SocketPort *)arg; - - my_this->recv_proc(); - - pthread_exit(0); -} - -SocketPort::SocketPort(char *server_ip, int server_port, int que_num, - int que_maxlen) { - que_num_ = que_num; - que_maxlen_ = que_maxlen; - state_ = -1; - rx_que_ = new QueueMemcpy(que_num_, que_maxlen_); - fp_ = socket_init((char *)" ", 0, 0); - if (fp_ == -1) { return; } - - int ret = socket_connect_server(&fp_, server_ip, server_port); - if (ret == -1) { return; } - - state_ = 0; - flush(); - thread_id_ = thread_init(recv_proc_, this); -} - -SocketPort::~SocketPort(void) { - state_ = -1; - delete rx_que_; -} - -int SocketPort::is_ok(void) { return state_; } - -void SocketPort::flush(void) { rx_que_->flush(); } - -int SocketPort::read_frame(unsigned char *data) { - if (state_ != 0) { return -1; } - - if (rx_que_->size() == 0) { return -1; } - rx_que_->pop(data); - return 0; -} - -int SocketPort::write_frame(unsigned char *data, int len) { - int ret = socket_send_data(fp_, data, len); - return ret; -} - -void SocketPort::close_port(void) { - close(fp_); - state_ = -1; - delete rx_que_; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm/report_data.cc b/interbotix_ros_uxarms/xarm_api/src/xarm/report_data.cc deleted file mode 100755 index 6281aad..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm/report_data.cc +++ /dev/null @@ -1,249 +0,0 @@ -/* Copyright 2017 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jimy Zhang - ============================================================================*/ -#include "xarm/report_data.h" - -#include -#include - -#include "xarm/debug/debug_print.h" - -ReportDataDevelop::ReportDataDevelop(void) { - runing_ = 0; - mode_ = 0; - cmdnum_ = 0; - total_num_ = 0; - - memset(angle_, 0, sizeof(angle_)); - memset(pose_, 0, sizeof(pose_)); - memset(tau_, 0, sizeof(tau_)); -} - -ReportDataDevelop::~ReportDataDevelop(void) {} - -int ReportDataDevelop::flush_data(unsigned char *rx_data) { - unsigned char *data_fp = &rx_data[4]; - int sizeof_data = bin8_to_32(rx_data); - if (sizeof_data < 87) { return -1; } - - total_num_ = bin8_to_32(data_fp); - if (total_num_ != 87) { return -2; } - - runing_ = data_fp[4] & 0x0F; - mode_ = data_fp[4] >> 4; - cmdnum_ = bin8_to_16(&data_fp[5]); - - hex_to_nfp32(&data_fp[7], angle_, 7); - hex_to_nfp32(&data_fp[35], pose_, 6); - hex_to_nfp32(&data_fp[59], tau_, 7); - return 0; -} - -void ReportDataDevelop::print_data(void) { - printf("total = %d\n", total_num_); - printf("runing = %d\n", runing_); - printf("mode = %d\n", mode_); - printf("cmdnum = %d\n", cmdnum_); - print_nvect("angle = ", angle_, 7); - print_nvect("pose = ", pose_, 6); - print_nvect("tau = ", tau_, 7); -} - - - -ReportDataNorm::ReportDataNorm(void) { - runing_ = 0; - mode_ = 0; - cmdnum_ = 0; - memset(angle_, 0, sizeof(angle_)); - memset(pose_, 0, sizeof(pose_)); - memset(tau_, 0, sizeof(tau_)); - - mt_brake_ = 0; - mt_able_ = 0; - err_ = 0; - war_ = 0; - memset(tcp_offset_, 0, sizeof(tcp_offset_)); - memset(tcp_load_, 0, sizeof(tcp_load_)); - collis_sens_ = 0; - teach_sens_ = 0; - total_num_ = 0; -} - -ReportDataNorm::~ReportDataNorm(void) {} - -int ReportDataNorm::flush_data(unsigned char *rx_data) { - unsigned char *data_fp = &rx_data[4]; - int sizeof_data = bin8_to_32(rx_data); - if (sizeof_data < 133) { - printf("sizeof_data = %d\n", sizeof_data); - return -1; - } - - total_num_ = bin8_to_32(data_fp); - // 133: legacy bug, actuall total number (data part) is 145 bytes - if (total_num_ != 133 && total_num_ != 145) { return -2; } - - runing_ = data_fp[4] & 0x0F; - mode_ = data_fp[4] >> 4; - cmdnum_ = bin8_to_16(&data_fp[5]); - hex_to_nfp32(&data_fp[7], angle_, 7); - hex_to_nfp32(&data_fp[35], pose_, 6); - hex_to_nfp32(&data_fp[59], tau_, 7); - - mt_brake_ = data_fp[87]; - mt_able_ = data_fp[88]; - err_ = data_fp[89]; - war_ = data_fp[90]; - - hex_to_nfp32(&data_fp[91], tcp_offset_, 6); - hex_to_nfp32(&data_fp[115], tcp_load_, 4); - - collis_sens_ = data_fp[131]; - teach_sens_ = data_fp[132]; - hex_to_nfp32(&data_fp[133], gravity_dir_, 3); // length is 12, not taken into account in legacy version.(before firmware v1.1) - - return 0; -} - -void ReportDataNorm::print_data(void) { - printf("total = %d\n", total_num_); - printf("runing = %d\n", runing_); - printf("mode = %d\n", mode_); - printf("cmdnum = %d\n", cmdnum_); - print_nvect("angle = ", angle_, 7); - print_nvect("pose = ", pose_, 6); - print_nvect("tau = ", tau_, 7); - - - printf("mode = %d\n", mode_); - printf("mt_brake= %x\n", mt_brake_); - printf("mt_able = %x\n", mt_able_); - printf("err&war = %d %d\n", err_, war_); - print_nvect("tcp_off = ", tcp_offset_, 6); - print_nvect("tap_load= ", tcp_load_, 4); - printf("coll_sen= %d\n", collis_sens_); - printf("teac_sen= %d\n", teach_sens_); - print_nvect("gravity_dir_= ", gravity_dir_, 3); -} - - - -ReportDataRich::ReportDataRich(void) { - runing_ = 0; - mode_ = 0; - cmdnum_ = 0; - memset(angle_, 0, sizeof(angle_)); - memset(pose_, 0, sizeof(pose_)); - memset(tau_, 0, sizeof(tau_)); - - mt_brake_ = 0; - mt_able_ = 0; - err_ = 0; - war_ = 0; - memset(tcp_offset_, 0, sizeof(tcp_offset_)); - memset(tcp_load_, 0, sizeof(tcp_load_)); - collis_sens_ = 0; - teach_sens_ = 0; - - total_num_ = 0; -} - -ReportDataRich::~ReportDataRich(void) {} - -int ReportDataRich::flush_data(unsigned char *rx_data) { - unsigned char *data_fp = &rx_data[4]; - int sizeof_data = bin8_to_32(rx_data); - if (sizeof_data < 233) { - printf("sizeof_data = %d\n", sizeof_data); - return -1; - } - - total_num_ = bin8_to_32(data_fp); - if (total_num_ != 233) { return -2; } - - runing_ = data_fp[4] & 0x0F; - mode_ = data_fp[4] >> 4; - cmdnum_ = bin8_to_16(&data_fp[5]); - hex_to_nfp32(&data_fp[7], angle_, 7); - hex_to_nfp32(&data_fp[35], pose_, 6); - hex_to_nfp32(&data_fp[59], tau_, 7); - - mt_brake_ = data_fp[87]; - mt_able_ = data_fp[88]; - err_ = data_fp[89]; - war_ = data_fp[90]; - hex_to_nfp32(&data_fp[91], tcp_offset_, 6); - hex_to_nfp32(&data_fp[115], tcp_load_, 4); - collis_sens_ = data_fp[131]; - teach_sens_ = data_fp[132]; - hex_to_nfp32(&data_fp[133], gravity_dir_, 3); - - arm_type_ = data_fp[145]; - axis_num_ = data_fp[146]; - master_id_ = data_fp[147]; - slave_id_ = data_fp[148]; - motor_tid_ = data_fp[149]; - motor_fid_ = data_fp[150]; - memcpy(versions_, &data_fp[151], 30); - - hex_to_nfp32(&data_fp[181], trs_msg_, 5); - trs_jerk_ = trs_msg_[0]; - trs_accmin_ = trs_msg_[1]; - trs_accmax_ = trs_msg_[2]; - trs_velomin_ = trs_msg_[3]; - trs_velomax_ = trs_msg_[4]; - - hex_to_nfp32(&data_fp[201], p2p_msg_, 5); - p2p_jerk_ = p2p_msg_[0]; - p2p_accmin_ = p2p_msg_[1]; - p2p_accmax_ = p2p_msg_[2]; - p2p_velomin_ = p2p_msg_[3]; - p2p_velomax_ = p2p_msg_[4]; - - hex_to_nfp32(&data_fp[221], rot_msg_, 2); - rot_jerk_ = rot_msg_[0]; - rot_accmax_ = rot_msg_[1]; - - for (int i = 0; i < 17; i++) { sv3msg_[i] = data_fp[229 + i]; } - - return 0; -} - -void ReportDataRich::print_data(void) { - printf("versions= %s\n", versions_); - printf("total = %d\n", total_num_); - printf("runing = %d\n", runing_); - printf("mode = %d\n", mode_); - printf("cmdnum = %d\n", cmdnum_); - print_nvect("angle = ", angle_, 7); - print_nvect("pose = ", pose_, 6); - print_nvect("tau = ", tau_, 7); - - printf("mode = %d\n", mode_); - printf("mt_brake= %x\n", mt_brake_); - printf("mt_able = %x\n", mt_able_); - printf("err&war = %d %d\n", err_, war_); - print_nvect("tcp_off = ", tcp_offset_, 6); - print_nvect("tap_load= ", tcp_load_, 4); - printf("coll_sen= %d\n", collis_sens_); - printf("teac_sen= %d\n", teach_sens_); - print_nvect("gravity_dir_= ", gravity_dir_, 3); - - printf("xarm_type = %d(axis%d)\n", arm_type_, axis_num_); - printf("xarm_msid = 0x%X 0x%X\n", master_id_, slave_id_); - printf("motor_tfid = 0x%X 0x%X\n", motor_tid_, motor_fid_); - - print_nvect("trs_msg = ", trs_msg_, 5); - print_nvect("p2p_msg = ", p2p_msg_, 5); - print_nvect("ros_msg = ", rot_msg_, 2); - - printf("ID 执行状态 错误代码\n"); - for (int i = 0; i < 8; i++) { - printf("%d %d 0x%X\n", i + 1, sv3msg_[i * 2], - sv3msg_[i * 2 + 1]); - } -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm_driver.cpp b/interbotix_ros_uxarms/xarm_api/src/xarm_driver.cpp deleted file mode 100755 index 9fbb081..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm_driver.cpp +++ /dev/null @@ -1,681 +0,0 @@ -/* Copyright 2018 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jason Peng - ============================================================================*/ -#include -#include "xarm/instruction/uxbus_cmd_config.h" -#include "xarm/linux/thread.h" - -#define CMD_HEARTBEAT_US 3e7 // 30s - -void* cmd_heart_beat(void* args) -{ - xarm_api::XARMDriver *my_driver = (xarm_api::XARMDriver *) args; - while(true) - { - usleep(CMD_HEARTBEAT_US); // non-realtime - my_driver->Heartbeat(); - } - pthread_exit(0); -} - -namespace xarm_api -{ - XARMDriver::~XARMDriver() - { - // pthread_cancel(thread_id_); // heartbeat related - arm_cmd_->set_mode(XARM_MODE::POSE); - arm_cmd_->close(); - spinner.stop(); - } - - void XARMDriver::XARMDriverInit(ros::NodeHandle& root_nh, char *server_ip) - { - nh_ = root_nh; - nh_.getParam("DOF",dof_); - - arm_report_ = connext_tcp_report_norm(server_ip); - // ReportDataNorm norm_data_; - arm_cmd_ = connect_tcp_control(server_ip); - if (arm_cmd_ == NULL) - ROS_ERROR("Xarm Connection Failed!"); - else // clear unimportant errors - { - // thread_id_ = thread_init(cmd_heart_beat, this); // heartbeat related - int dbg_msg[16] = {0}; - arm_cmd_->servo_get_dbmsg(dbg_msg); - - for(int i=0; iclean_err(); - ROS_WARN("Cleared low-voltage error of joint %d", i+1); - } - else if((dbg_msg[i*2]==1)) - { - arm_cmd_->clean_err(); - ROS_WARN("There is servo error code:(0x%x) in joint %d, trying to clear it..", dbg_msg[i*2+1], i+1); - } - } - - } - - // api command services: - motion_ctrl_server_ = nh_.advertiseService("motion_ctrl", &XARMDriver::MotionCtrlCB, this); - set_mode_server_ = nh_.advertiseService("set_mode", &XARMDriver::SetModeCB, this); - set_state_server_ = nh_.advertiseService("set_state", &XARMDriver::SetStateCB, this); - set_tcp_offset_server_ = nh_.advertiseService("set_tcp_offset", &XARMDriver::SetTCPOffsetCB, this); - set_load_server_ = nh_.advertiseService("set_load", &XARMDriver::SetLoadCB, this); - - go_home_server_ = nh_.advertiseService("go_home", &XARMDriver::GoHomeCB, this); - move_joint_server_ = nh_.advertiseService("move_joint", &XARMDriver::MoveJointCB, this); - move_lineb_server_ = nh_.advertiseService("move_lineb", &XARMDriver::MoveLinebCB, this); - move_line_server_ = nh_.advertiseService("move_line", &XARMDriver::MoveLineCB, this); - move_line_tool_server_ = nh_.advertiseService("move_line_tool", &XARMDriver::MoveLineToolCB, this); - move_servoj_server_ = nh_.advertiseService("move_servoj", &XARMDriver::MoveServoJCB, this); - move_servo_cart_server_ = nh_.advertiseService("move_servo_cart", &XARMDriver::MoveServoCartCB, this); - clear_err_server_ = nh_.advertiseService("clear_err", &XARMDriver::ClearErrCB, this); - moveit_clear_err_server_ = nh_.advertiseService("moveit_clear_err", &XARMDriver::MoveitClearErrCB, this); - get_err_server_ = nh_.advertiseService("get_err", &XARMDriver::GetErrCB, this); - - // tool io: - set_end_io_server_ = nh_.advertiseService("set_digital_out", &XARMDriver::SetDigitalIOCB, this); - get_digital_in_server_ = nh_.advertiseService("get_digital_in", &XARMDriver::GetDigitalIOCB, this); - get_analog_in_server_ = nh_.advertiseService("get_analog_in", &XARMDriver::GetAnalogIOCB, this); - config_modbus_server_ = nh_.advertiseService("config_tool_modbus", &XARMDriver::ConfigModbusCB, this); - set_modbus_server_ = nh_.advertiseService("set_tool_modbus", &XARMDriver::SetModbusCB, this); - - gripper_config_server_ = nh_.advertiseService("gripper_config", &XARMDriver::GripperConfigCB, this); - gripper_move_server_ = nh_.advertiseService("gripper_move", &XARMDriver::GripperMoveCB, this); - gripper_state_server_ = nh_.advertiseService("gripper_state", &XARMDriver::GripperStateCB, this); - - set_vacuum_gripper_server_ = nh_.advertiseService("vacuum_gripper_set", &XARMDriver::VacuumGripperCB, this); - - // controller_io (digital): - set_controller_dout_server_ = nh_.advertiseService("set_controller_dout", &XARMDriver::SetControllerDOutCB, this); - get_controller_din_server_ = nh_.advertiseService("get_controller_din", &XARMDriver::GetControllerDInCB, this); - - // state feedback topics: - joint_state_ = nh_.advertise("joint_states", 10, true); - robot_rt_state_ = nh_.advertise("xarm_states", 10, true); - // end_input_state_ = nh_.advertise("xarm_input_states", 10, true); - - // subscribed topics - sleep_sub_ = nh_.subscribe("sleep_sec", 1, &XARMDriver::SleepTopicCB, this); - - } - - void XARMDriver::Heartbeat(void) - { - int cmd_num; - int ret = arm_cmd_->get_cmdnum(&cmd_num); - // if(ret) - // { - // ROS_ERROR("xArm Heartbeat error! ret = %d", ret); - // } - // ROS_INFO("xArm Heartbeat! %d", cmd_num); - } - - bool XARMDriver::isConnectionOK(void) - { - return !arm_report_->is_ok(); // is_ok will return 0 if connection is normal - } - - void XARMDriver::SleepTopicCB(const std_msgs::Float32ConstPtr& msg) - { - if(msg->data>0) - arm_cmd_->sleep_instruction(msg->data); - } - - bool XARMDriver::ClearErrCB(xarm_msgs::ClearErr::Request& req, xarm_msgs::ClearErr::Response& res) - { - // First clear controller warning and error: - int ret1 = arm_cmd_->gripper_modbus_clean_err(); - int ret2 = arm_cmd_->clean_war(); - int ret3 = arm_cmd_->clean_err(); - - // Then try to enable motor again: - res.ret = arm_cmd_->motion_en(8, 1); - - if(res.ret) - { - res.message = "clear err, ret = " + std::to_string(res.ret); - } - return true; - - // After calling this service, user should check '/xarm_states' again to make sure 'err' field is 0, to confirm success. - } - - bool XARMDriver::MoveitClearErrCB(xarm_msgs::ClearErr::Request& req, xarm_msgs::ClearErr::Response& res) - { - if(ClearErrCB(req, res)) - { - arm_cmd_->set_mode(1); - int ret = arm_cmd_->set_state(0); - - if(!ret) - return true; - return false; - } - return false; - - // After calling this service, user should check '/xarm_states' again to make sure 'err' field is 0, to confirm success. - } - - bool XARMDriver::GetErrCB(xarm_msgs::GetErr::Request & req, xarm_msgs::GetErr::Response & res) - { - res.err = curr_err_; - res.message = "current error code = " + std::to_string(res.err); - return true; - } - - bool XARMDriver::MotionCtrlCB(xarm_msgs::SetAxis::Request& req, xarm_msgs::SetAxis::Response& res) - { - res.ret = arm_cmd_->motion_en(req.id, req.data); - if(req.data == 1) - { - res.message = "motion enable, ret = " + std::to_string(res.ret); - } - else - { - res.message = "motion disable, ret = " + std::to_string(res.ret); - } - return true; - } - - bool XARMDriver::SetModeCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res) - { - res.ret = arm_cmd_->set_mode(req.data); - switch(req.data) - { - case XARM_MODE::POSE: - { - res.message = "pose mode, ret = " + std::to_string(res.ret); - }break; - case XARM_MODE::SERVO: - { - res.message = "servo mode, ret = " + std::to_string(res.ret); - }break; - case XARM_MODE::TEACH_CART: - { - res.message = "cartesian teach, ret = " + std::to_string(res.ret); - } break; - case XARM_MODE::TEACH_JOINT: - { - res.message = "joint teach , ret = " + std::to_string(res.ret); - } break; - default: - { - res.message = "the failed mode, ret = " + std::to_string(res.ret); - } - } - - return true; - } - - bool XARMDriver::SetStateCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res) - { - res.ret = arm_cmd_->set_state(req.data); - switch(req.data) - { - case XARM_STATE::START: - { - res.message = "start, ret = " + std::to_string(res.ret); - }break; - case XARM_STATE::PAUSE: - { - res.message = "pause, ret = " + std::to_string(res.ret); - }break; - case XARM_STATE::STOP: - { - res.message = "stop, ret = " + std::to_string(res.ret); - }break; - default: - { - res.message = "the failed state, ret = " + std::to_string(res.ret); - } - } - - return true; - } - - bool XARMDriver::SetTCPOffsetCB(xarm_msgs::TCPOffset::Request &req, xarm_msgs::TCPOffset::Response &res) - { - float offsets[6] = {req.x, req.y, req.z, req.roll, req.pitch, req.yaw}; - res.ret = arm_cmd_->set_tcp_offset(offsets) | arm_cmd_->save_conf(); - res.message = "set tcp offset: ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::SetLoadCB(xarm_msgs::SetLoad::Request &req, xarm_msgs::SetLoad::Response &res) - { - float Mass = req.mass; - float CoM[3] = {req.xc, req.yc, req.zc}; - res.ret = arm_cmd_->set_tcp_load(Mass, CoM) | arm_cmd_->save_conf(); - res.message = "set load: ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::SetControllerDOutCB(xarm_msgs::SetDigitalIO::Request &req, xarm_msgs::SetDigitalIO::Response &res) - { - if(req.io_num>=1 && req.io_num<=8) - { - res.ret = arm_cmd_->cgpio_set_auxdigit(req.io_num-1, req.value); - res.message = "set Controller digital Output "+ std::to_string(req.io_num) +" to "+ std::to_string(req.value) + " : ret = " + std::to_string(res.ret); - return true; - } - ROS_WARN("Controller IO io_num: from 1 to 8"); - return false; - } - - bool XARMDriver::GetControllerDInCB(xarm_msgs::GetControllerDigitalIO::Request &req, xarm_msgs::GetControllerDigitalIO::Response &res) - { - if(req.io_num>=1 && req.io_num<=8) - { - int all_status; - res.ret = arm_cmd_->cgpio_get_auxdigit(&all_status); - res.value = (all_status >> (req.io_num - 1)) & 0x0001; - res.message = "get Controller digital Input ret = " + std::to_string(res.ret); - return true; - } - ROS_WARN("Controller IO io_num: from 1 to 8"); - return false; - } - - bool XARMDriver::SetDigitalIOCB(xarm_msgs::SetDigitalIO::Request &req, xarm_msgs::SetDigitalIO::Response &res) - { - res.ret = arm_cmd_->tgpio_set_digital(req.io_num, req.value); - res.message = "set Digital port "+ std::to_string(req.io_num) +" to "+ std::to_string(req.value) + " : ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::GetDigitalIOCB(xarm_msgs::GetDigitalIO::Request &req, xarm_msgs::GetDigitalIO::Response &res) - { - res.ret = arm_cmd_->tgpio_get_digital(&res.digital_1, &res.digital_2); - res.message = "get Digital port ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::GetAnalogIOCB(xarm_msgs::GetAnalogIO::Request &req, xarm_msgs::GetAnalogIO::Response &res) - { - switch (req.port_num) - { - case 1: - res.ret = arm_cmd_->tgpio_get_analog1(&res.analog_value); - break; - case 2: - res.ret = arm_cmd_->tgpio_get_analog2(&res.analog_value); - break; - - default: - res.message = "GetAnalogIO Fail: port number incorrect !"; - return false; - } - res.message = "get analog port ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::SetModbusCB(xarm_msgs::SetToolModbus::Request &req, xarm_msgs::SetToolModbus::Response &res) - { - int send_len = req.send_data.size(); - int recv_len = req.respond_len; - unsigned char tx_data[send_len]={0}, rx_data[recv_len]={0}; - - for(int i=0; itgpio_set_modbus(tx_data, send_len, rx_data); - for(int i=0; iset_state(0); - ROS_WARN("Cleared Existing Error Code %d", curr_err_); - } - - int ret = arm_cmd_->set_modbus_baudrate(req.baud_rate); - if(ret) - { - res.message = "set baud_rate, ret = "+ std::to_string(ret); - if(ret==1) - res.message += " controller error exists, please check and run clear_err service first!"; - } - ret = arm_cmd_->set_modbus_timeout(req.timeout_ms); - if(ret) - { - res.message += (std::string(" set baud_rate, ret = ") + std::to_string(ret)); - if(ret==1) - res.message += " controller error exists, please check and run clear_err service first!"; - } - - if(res.message.size()) - res.ret = -1; - else - res.message = "Modbus configuration OK"; - - return true; - } - - bool XARMDriver::GoHomeCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - res.ret = arm_cmd_->move_gohome(req.mvvelo, req.mvacc, req.mvtime); - if(!res.ret) - { - res.ret = wait_for_finish(); - } - res.message = "go home, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveJointCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float joint[1][7]={0}; - int index = 0; - if(req.pose.size() != dof_) - { - res.ret = req.pose.size(); - res.message = "pose parameters incorrect! Expected: "+std::to_string(dof_); - return true; - } - else - { - for(index = 0; index < 7; index++) // should always send 7 joint commands, whatever current DOF is. - { - // joint[0][index] = req.pose[index]; - if(indexmove_joint(joint[0], req.mvvelo, req.mvacc, req.mvtime); - if(!res.ret) - { - res.ret = wait_for_finish(); - } - res.message = "move joint, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveLineCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float pose[1][6]; - int index = 0; - if(req.pose.size() != 6) - { - res.ret = -1; - res.message = "parameters incorrect!"; - return true; - } - else - { - for(index = 0; index < 6; index++) - { - pose[0][index] = req.pose[index]; - } - } - - res.ret = arm_cmd_->move_line(pose[0], req.mvvelo, req.mvacc, req.mvtime); - if(!res.ret) - { - res.ret = wait_for_finish(); - } - res.message = "move line, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveLineToolCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float pose[1][6]; - int index = 0; - if(req.pose.size() != 6) - { - res.ret = -1; - res.message = "parameters incorrect!"; - return true; - } - else - { - for(index = 0; index < 6; index++) - { - pose[0][index] = req.pose[index]; - } - } - - res.ret = arm_cmd_->move_line_tool(pose[0], req.mvvelo, req.mvacc, req.mvtime); - if(!res.ret) - { - res.ret = wait_for_finish(); - } - res.message = "move line tool, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveLinebCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float pose[1][6]; - int index = 0; - if(req.pose.size() != 6) - { - res.ret = -1; - res.message = "parameters incorrect!"; - return true; - } - else - { - for(index = 0; index < 6; index++) - { - pose[0][index] = req.pose[index]; - } - } - - res.ret = arm_cmd_->move_lineb(pose[0], req.mvvelo, req.mvacc, req.mvtime, req.mvradii); - res.message = "move lineb, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveServoJCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float pose[1][7]={0}; - int index = 0; - if(req.pose.size() != dof_) - { - res.ret = req.pose.size(); - res.message = "pose parameters incorrect! Expected: "+std::to_string(dof_); - return true; - } - else - { - for(index = 0; index < 7; index++) // should always send 7 joint commands, whatever current DOF is. - { - if(indexmove_servoj(pose[0], req.mvvelo, req.mvacc, req.mvtime); - res.message = "move servoj, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::MoveServoCartCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) - { - float pose[1][6]; - int index = 0; - if(req.pose.size() != 6) - { - res.ret = -1; - res.message = "MoveServoCartCB parameters incorrect!"; - return true; - } - else - { - for(index = 0; index < 6; index++) - { - pose[0][index] = req.pose[index]; - } - } - - res.ret = arm_cmd_->move_servo_cartesian(pose[0], req.mvvelo, req.mvacc, req.mvtime); - res.message = "move servo_cartesian, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::GripperConfigCB(xarm_msgs::GripperConfig::Request &req, xarm_msgs::GripperConfig::Response &res) - { - if(req.pulse_vel>5000) - req.pulse_vel = 5000; - else if(req.pulse_vel<0) - req.pulse_vel = 0; - - - int ret1 = arm_cmd_->gripper_modbus_set_mode(0); - int ret2 = arm_cmd_->gripper_modbus_set_en(1); - int ret3 = arm_cmd_->gripper_modbus_set_posspd(req.pulse_vel); - - if(ret1 || ret2 || ret3) - { - res.ret = ret3; - } - else - { - res.ret = 0; - } - res.message = "gripper_config, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::GripperMoveCB(xarm_msgs::GripperMove::Request &req, xarm_msgs::GripperMove::Response &res) - { - if(req.pulse_pos>850) - req.pulse_pos = 850; - else if(req.pulse_pos<-100) - req.pulse_pos = -100; - - res.ret = arm_cmd_->gripper_modbus_set_pos(req.pulse_pos); - res.message = "gripper_move, ret = " + std::to_string(res.ret); - return true; - } - - bool XARMDriver::GripperStateCB(xarm_msgs::GripperState::Request &req, xarm_msgs::GripperState::Response &res) - { - int err_code = 0; - float pos_now = 0; - if(arm_cmd_->gripper_modbus_get_errcode(&err_code)) - return false; - - if(arm_cmd_->gripper_modbus_get_pos(&pos_now)) - return false; - - res.err_code = err_code; - res.curr_pos = pos_now; - // fprintf(stderr, "gripper_pos: %f, gripper_err: %d\n", res.curr_pos, res.err_code); - return true; - } - - bool XARMDriver::VacuumGripperCB(xarm_msgs::SetInt16::Request &req, xarm_msgs::SetInt16::Response &res) - { - if(req.data) - { - res.ret = arm_cmd_->tgpio_set_digital(1, 1); - res.ret = arm_cmd_->tgpio_set_digital(2, 0); - } - else - { - res.ret = arm_cmd_->tgpio_set_digital(1, 0); - res.ret = arm_cmd_->tgpio_set_digital(2, 1); - } - res.message = "set vacuum gripper: " + std::to_string(req.data) + " ret = " + std::to_string(res.ret); - return true; - } - - void XARMDriver::pub_robot_msg(xarm_msgs::RobotMsg &rm_msg) - { - curr_err_ = rm_msg.err; - curr_state_ = rm_msg.state; - robot_rt_state_.publish(rm_msg); - } - - void XARMDriver::pub_joint_state(sensor_msgs::JointState &js_msg) - { - joint_state_.publish(js_msg); - } - - void XARMDriver::pub_io_state() - { - arm_cmd_->tgpio_get_digital(&io_msg.digital_1, &io_msg.digital_2); - arm_cmd_->tgpio_get_analog1(&io_msg.analog_1); - arm_cmd_->tgpio_get_analog2(&io_msg.analog_2); - - end_input_state_.publish(io_msg); - } - - int XARMDriver::get_frame(void) - { - int ret; - ret = arm_report_->read_frame(rx_data_); - return ret; - } - - int XARMDriver::get_rich_data(ReportDataNorm &norm_data) - { - int ret; - ret = norm_data_.flush_data(rx_data_); - norm_data = norm_data_; - return ret; - } - - int XARMDriver::wait_for_finish() - { - bool wait; - int ret = 0; - - nh_.getParam("wait_for_finish", wait); - - if(!wait) - return ret; - - ros::Duration(0.2).sleep(); // delay 0.2s, for 5Hz state update - ros::Rate sleep_rate(10); // 10Hz - - while(curr_state_== 1) // in MOVE state - { - if(curr_err_) - { - ret = UXBUS_STATE::ERR_CODE; - break; - } - sleep_rate.sleep(); - } - - if(!ret) - { - int err_warn[2] = {0}; - arm_cmd_->get_err_code(err_warn); - if(err_warn[0]) - { - ROS_ERROR("XARM ERROR CODE: %d ", err_warn[0]); - ret = UXBUS_STATE::ERR_CODE; - } - } - - return ret; - } - -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm_driver_node.cpp b/interbotix_ros_uxarms/xarm_api/src/xarm_driver_node.cpp deleted file mode 100755 index bd9f22d..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm_driver_node.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* Copyright 2018 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jason Peng - ============================================================================*/ -#include -#include -#include -#include "xarm/connect.h" -#include "xarm/report_data.h" - -void exit_sig_handler(int signum) -{ - fprintf(stderr, "[xarm_driver] Ctrl-C caught, exit process...\n"); - exit(-1); -} - -class XarmRTConnection -{ - public: - XarmRTConnection(ros::NodeHandle& root_nh, char *server_ip, xarm_api::XARMDriver &drv) - { - root_nh.getParam("DOF", joint_num_); - root_nh.getParam("joint_names", joint_name_); - ip = server_ip; - xarm_driver = drv; - xarm_driver.XARMDriverInit(root_nh, server_ip); - ros::Duration(0.5).sleep(); - thread_id = thread_init(thread_proc, (void *)this); - } - - void thread_run(void) - { - int ret; - int err_num; - int rxcnt; - int i; - int first_cycle = 1; - double d, prev_angle[joint_num_]; - - ros::Rate r(REPORT_RATE_HZ); // 10Hz - - while(xarm_driver.isConnectionOK()) - { - r.sleep(); - ret = xarm_driver.get_frame(); - if (ret != 0) continue; - - ret = xarm_driver.get_rich_data(norm_data); - if (ret == 0) - { - rxcnt++; - - now = ros::Time::now(); - js_msg.header.stamp = now; - js_msg.header.frame_id = "real-time data"; - js_msg.name.resize(joint_num_); - js_msg.position.resize(joint_num_); - js_msg.velocity.resize(joint_num_); - js_msg.effort.resize(joint_num_); - for(i = 0; i < joint_num_; i++) - { - d = (double)norm_data.angle_[i]; - js_msg.name[i] = joint_name_[i]; - js_msg.position[i] = d; - - if (first_cycle) - { - js_msg.velocity[i] = 0; - first_cycle = 0; - } - else - { - js_msg.velocity[i] = (js_msg.position[i] - prev_angle[i])*REPORT_RATE_HZ; - } - - js_msg.effort[i] = (double)norm_data.tau_[i]; - - prev_angle[i] = d; - } - - xarm_driver.pub_joint_state(js_msg); - - rm_msg.state = norm_data.runing_; - rm_msg.mode = norm_data.mode_; - rm_msg.cmdnum = norm_data.cmdnum_; - rm_msg.err = norm_data.err_; - rm_msg.warn = norm_data.war_; - rm_msg.mt_brake = norm_data.mt_brake_; - rm_msg.mt_able = norm_data.mt_able_; - rm_msg.angle.resize(joint_num_); - - for(i = 0; i < joint_num_; i++) - { - /* set the float precision*/ - double d = norm_data.angle_[i]; - double r; - char str[8]; - sprintf(str, "%0.3f", d); - sscanf(str, "%lf", &r); - rm_msg.angle[i] = r; - } - for(i = 0; i < 6; i++) - { - rm_msg.pose[i] = norm_data.pose_[i]; - rm_msg.offset[i] = norm_data.tcp_offset_[i]; - } - xarm_driver.pub_robot_msg(rm_msg); - - // publish io state: This line may interfere with servoj execution - // xarm_driver.pub_io_state(); - - } - else - { - printf("Error: real_data.flush_data failed, ret = %d\n", ret); - err_num++; - } - - } - ROS_ERROR("xArm Connection Failed! Please Shut Down (Ctrl-C) and Retry ..."); - } - - static void* thread_proc(void *arg) - { - XarmRTConnection* pThreadTest=(XarmRTConnection*)arg; - pThreadTest->thread_run(); - pthread_exit(0); - } - - public: - pthread_t thread_id; - char *ip; - ros::Time now; - SocketPort *arm_report; - ReportDataNorm norm_data; - sensor_msgs::JointState js_msg; - xarm_api::XARMDriver xarm_driver; - xarm_msgs::RobotMsg rm_msg; - - int joint_num_; - std::vector joint_name_; - constexpr static const double REPORT_RATE_HZ = 10; /* 10Hz, same with norm_report frequency */ -}; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "xarm_driver_node"); - - // with namespace (ns) specified in the calling launch file (xarm by default) - ros::NodeHandle n; - - xarm_api::XARMDriver driver; - ROS_INFO("start xarm driver"); - - std::string robot_ip = "192.168.1.121"; - if (!n.hasParam("xarm_robot_ip")) - { - ROS_ERROR("No param named 'xarm_robot_ip'"); - ROS_ERROR("Use default robot ip = 192.168.1.121"); - } - else - { - n.getParam("xarm_robot_ip", robot_ip); - } - - char server_ip[20]={0}; - strcpy(server_ip,robot_ip.c_str()); - XarmRTConnection rt_connect(n, server_ip, driver); - - signal(SIGINT, exit_sig_handler); - ros::waitForShutdown(); - - printf("end"); - - return 0; -} diff --git a/interbotix_ros_uxarms/xarm_api/src/xarm_ros_client.cpp b/interbotix_ros_uxarms/xarm_api/src/xarm_ros_client.cpp deleted file mode 100755 index cbd6def..0000000 --- a/interbotix_ros_uxarms/xarm_api/src/xarm_ros_client.cpp +++ /dev/null @@ -1,381 +0,0 @@ -/* Copyright 2018 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: Jason Peng - ============================================================================*/ -#include - -namespace xarm_api{ - -void XArmROSClient::init(ros::NodeHandle& nh) -{ - nh_ = nh; - std::string client_ns = nh_.getNamespace() + "/"; - ros::service::waitForService(client_ns+"motion_ctrl"); - ros::service::waitForService(client_ns+"set_state"); - ros::service::waitForService(client_ns+"set_mode"); - ros::service::waitForService(client_ns+"move_servoj"); - - motion_ctrl_client_ = nh_.serviceClient("motion_ctrl"); - set_mode_client_ = nh_.serviceClient("set_mode"); - set_state_client_ = nh_.serviceClient("set_state"); - set_tcp_offset_client_ = nh_.serviceClient("set_tcp_offset"); - set_load_client_ = nh_.serviceClient("set_load"); - clear_err_client_ = nh_.serviceClient("clear_err"); - get_err_client_ = nh_.serviceClient("get_err"); - go_home_client_ = nh_.serviceClient("go_home"); - move_lineb_client_ = nh_.serviceClient("move_lineb"); - move_line_client_ = nh_.serviceClient("move_line"); - move_joint_client_ = nh_.serviceClient("move_joint"); - move_servoj_client_ = nh_.serviceClient("move_servoj",true); // persistent connection for servoj - move_servo_cart_client_ = nh_.serviceClient("move_servo_cart",true); // persistent connection for servo_cartesian - - //xarm gripper: - gripper_move_client_ = nh_.serviceClient("gripper_move"); - gripper_config_client_ = nh_.serviceClient("gripper_config"); - gripper_state_client_ = nh_.serviceClient("gripper_state"); - - //tool modbus: - config_modbus_client_ = nh_.serviceClient("config_tool_modbus"); - send_modbus_client_ = nh_.serviceClient("set_tool_modbus"); -} - -int XArmROSClient::motionEnable(short en) -{ - set_axis_srv_.request.id = 8; - set_axis_srv_.request.data = en; - if(motion_ctrl_client_.call(set_axis_srv_)) - { - ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); - return set_axis_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service motion_ctrl"); - return 1; - } - -} - -int XArmROSClient::setState(short state) -{ - set_int16_srv_.request.data = state; - if(set_state_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - return set_int16_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service set_state"); - return 1; - } -} - -int XArmROSClient::setMode(short mode) -{ - set_int16_srv_.request.data = mode; - if(set_mode_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - return set_int16_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service set_mode"); - return 1; - } - -} - -int XArmROSClient::clearErr() -{ - if(clear_err_client_.call(clear_err_srv_)) - { - ROS_INFO("%s\n", clear_err_srv_.response.message.c_str()); - return clear_err_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service clear_err"); - return 1; - } - -} - -int XArmROSClient::getErr() -{ - if(get_err_client_.call(get_err_srv_)) - { - ROS_INFO("%s\n", get_err_srv_.response.message.c_str()); - return get_err_srv_.response.err; - } - else - { - ROS_ERROR("Failed to call service get_err"); - return 1; - } - -} - -int XArmROSClient::setServoJ(const std::vector& joint_cmd) -{ - servoj_msg_.request.mvvelo = 0; - servoj_msg_.request.mvacc = 0; - servoj_msg_.request.mvtime = 0; - servoj_msg_.request.pose = joint_cmd; - - if(move_servoj_client_.call(servoj_msg_)) - { - // ROS_INFO("%s\n", servoj_msg_.response.message.c_str()); - return servoj_msg_.response.ret; - } - else - { - ROS_ERROR("Failed to call service move_servoj"); - return 1; - } -} - -int XArmROSClient::setServoCartisian(const std::vector& cart_cmd) -{ - servo_cart_msg_.request.mvvelo = 0; - servo_cart_msg_.request.mvacc = 0; - servo_cart_msg_.request.mvtime = 0; - servo_cart_msg_.request.pose = cart_cmd; - - - if(move_servo_cart_client_.call(servo_cart_msg_)) - { - // ROS_INFO("%s\n", servo_cart_msg_.response.message.c_str()); - return servo_cart_msg_.response.ret; - } - else - { - ROS_ERROR("Failed to call service move_servo_cart"); - return 1; - } -} - -int XArmROSClient::setTCPOffset(const std::vector& tcp_offset) -{ - if(tcp_offset.size() != 6) - { - ROS_ERROR("Set tcp offset service parameter should be 6-element Cartesian offset!"); - return 1; - } - - offset_srv_.request.x = tcp_offset[0]; - offset_srv_.request.y = tcp_offset[1]; - offset_srv_.request.z = tcp_offset[2]; - offset_srv_.request.roll = tcp_offset[3]; - offset_srv_.request.pitch = tcp_offset[4]; - offset_srv_.request.yaw = tcp_offset[5]; - - if(set_tcp_offset_client_.call(offset_srv_)) - { - return offset_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service set_tcp_offset"); - return 1; - } - -} - -int XArmROSClient::setLoad(float mass, const std::vector& center_of_mass) -{ - set_load_srv_.request.mass = mass; - set_load_srv_.request.xc = center_of_mass[0]; - set_load_srv_.request.yc = center_of_mass[1]; - set_load_srv_.request.zc = center_of_mass[2]; - - if(set_load_client_.call(set_load_srv_)) - { - return set_load_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service set_load"); - return 1; - } -} - -int XArmROSClient::goHome(float jnt_vel_rad, float jnt_acc_rad) -{ - move_srv_.request.mvvelo = jnt_vel_rad; - move_srv_.request.mvacc = jnt_acc_rad; - move_srv_.request.mvtime = 0; - - if(go_home_client_.call(move_srv_)) - { - return move_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service go_home"); - return 1; - } -} - -int XArmROSClient::moveJoint(const std::vector& joint_cmd, float jnt_vel_rad, float jnt_acc_rad) -{ - move_srv_.request.mvvelo = jnt_vel_rad; - move_srv_.request.mvacc = jnt_acc_rad; - move_srv_.request.mvtime = 0; - move_srv_.request.pose = joint_cmd; - - if(move_joint_client_.call(move_srv_)) - { - return move_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service move_joint"); - return 1; - } -} - -int XArmROSClient::moveLine(const std::vector& cart_cmd, float cart_vel_mm, float cart_acc_mm) -{ - move_srv_.request.mvvelo = cart_vel_mm; - move_srv_.request.mvacc = cart_acc_mm; - move_srv_.request.mvtime = 0; - move_srv_.request.pose = cart_cmd; - - if(move_line_client_.call(move_srv_)) - { - return move_srv_.response.ret; - } - else - { - ROS_ERROR("Failed to call service move_line"); - return 1; - } -} - -int XArmROSClient::moveLineB(int num_of_pnts, const std::vector cart_cmds[], float cart_vel_mm, float cart_acc_mm, float radii) -{ - move_srv_.request.mvvelo = cart_vel_mm; - move_srv_.request.mvacc = cart_acc_mm; - move_srv_.request.mvtime = 0; - move_srv_.request.mvradii = radii; - - for(int i=0; i - ============================================================================*/ -#include - -#include "xarm/connect.h" -#include "xarm/report_data.h" - -int main(int argc, char **argv) { - if (argc < 2) { - printf("Please enter IP address\n"); - return 0; - } - char *server_ip = argv[1]; - SocketPort *arm_report = connext_tcp_report_norm(server_ip); - if (arm_report == NULL) return 0; - - int rxcnt = 0; - ReportDataNorm *norm_data = new ReportDataNorm(); - - unsigned char rx_data[1280]; - int ret; - int err_num = 0; - while (1) { - usleep(1000); - - ret = arm_report->read_frame(rx_data); - if (ret != 0) continue; - ret = norm_data->flush_data(rx_data); - - if (ret == 0) { - rxcnt++; - printf("\n【normal report】: len = %d, rxcnt = %d, err_num = %d\n", - bin8_to_32(rx_data), rxcnt, err_num); - norm_data->print_data(); - } else { - printf("Error: norm_data.flush_data failed, ret = %d\n", ret); - err_num++; - } - } -} diff --git a/interbotix_ros_uxarms/xarm_api/test/move_test.cpp b/interbotix_ros_uxarms/xarm_api/test/move_test.cpp deleted file mode 100755 index d2e136b..0000000 --- a/interbotix_ros_uxarms/xarm_api/test/move_test.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* Copyright 2018 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: waylon - Jason - ============================================================================*/ -#include "ros/ros.h" -#include - -int go_home_test(xarm_msgs::Move srv, ros::ServiceClient client) -{ - srv.request.mvvelo = 20.0 / 57.0; - srv.request.mvacc = 1000; - srv.request.mvtime = 0; - if(client.call(srv)) - { - ROS_INFO("%s\n", srv.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service go home"); - return 1; - } - return 0; -} - -int servoj_test(xarm_msgs::Move srv, ros::ServiceClient client) -{ - std::vector joint[2] = {{0, 0, 0, 0, 0, 0, 0}, - {0, -0.3, 0, 0, 0, 0, 0}}; - - srv.request.mvvelo = 0; - srv.request.mvacc = 0; - srv.request.mvtime = 0; - for (int i = 0; i < 2; i++) - { - srv.request.pose = joint[i]; - if(client.call(srv)) - { - ROS_INFO("%s\n", srv.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service move_servoj"); - return 1; - } - sleep(2); - } - return 0; -} - -int move_lineb_test(xarm_msgs::Move srv, ros::ServiceClient client) -{ - std::vector pose[5] = { {300, 0, 100, -3.14, 0, 0}, - {300, 100, 100, -3.14, 0, 0}, - {400, 100, 100, -3.14, 0, 0}, - {400, -100, 100, -3.14, 0, 0}, - {300, -100, 100, -3.14, 0, 0}}; - srv.request.mvvelo = 100; - srv.request.mvacc = 1000; - srv.request.mvtime = 0; - srv.request.mvradii = 20; - - for(int k=0; k<3; k++) - { - for(int i = 0; i < 5; i++) - { - srv.request.pose = pose[i]; - if(client.call(srv)) - { - ROS_INFO("%s\n", srv.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service move_lineb"); - return 1; - } - } - } - return 0; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "xarm_move_test"); - ros::NodeHandle nh; - - nh.setParam("/xarm/wait_for_finish", true); // return after motion service finish - ros::Publisher sleep_pub_ = nh.advertise("/xarm/sleep_sec", 1); - ros::ServiceClient motion_ctrl_client_ = nh.serviceClient("/xarm/motion_ctrl"); - ros::ServiceClient set_mode_client_ = nh.serviceClient("/xarm/set_mode"); - ros::ServiceClient set_state_client_ = nh.serviceClient("/xarm/set_state"); - ros::ServiceClient go_home_client_ = nh.serviceClient("/xarm/go_home"); - ros::ServiceClient move_lineb_client_ = nh.serviceClient("/xarm/move_lineb"); - ros::ServiceClient move_servoj_client_ = nh.serviceClient("/xarm/move_servoj"); - - xarm_msgs::SetAxis set_axis_srv_; - xarm_msgs::SetInt16 set_int16_srv_; - xarm_msgs::Move move_srv_; - - set_axis_srv_.request.id = 8; - set_axis_srv_.request.data = 1; - - if(motion_ctrl_client_.call(set_axis_srv_)) - { - ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service motion_ctrl"); - return 1; - } - - set_int16_srv_.request.data = 0; - if(set_mode_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_mode"); - return 1; - } - - set_int16_srv_.request.data = 0; - if(set_state_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_state"); - return 1; - } - - if(go_home_test(move_srv_, go_home_client_) == 1) - return 1; - - // MOVE_LINEB need some additional configurations: wait=False, sleep before sending commands - nh.setParam("/xarm/wait_for_finish", false); // This configuration is CRITICAL for move_lineb! - std_msgs::Float32 sleep_msg; - sleep_msg.data = 1.0; - sleep_pub_.publish(sleep_msg); - if(move_lineb_test(move_srv_, move_lineb_client_) == 1) - return 1; - nh.setParam("/xarm/wait_for_finish", true); - -} \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/test/servo_cartesian_test.cpp b/interbotix_ros_uxarms/xarm_api/test/servo_cartesian_test.cpp deleted file mode 100644 index 9f6dcf8..0000000 --- a/interbotix_ros_uxarms/xarm_api/test/servo_cartesian_test.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/* Copyright 2020 UFACTORY Inc. All Rights Reserved. - * - * Software License Agreement (BSD License) - * - * Author: jason - ============================================================================*/ -#include "ros/ros.h" -#include - -int go_home_test(xarm_msgs::Move &srv, ros::ServiceClient &client) -{ - srv.request.mvvelo = 20.0 / 57.0; - srv.request.mvacc = 1000; - srv.request.mvtime = 0; - if(client.call(srv)) - { - ROS_INFO("%s\n", srv.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service go home"); - return 1; - } - return 0; -} - -int servo_cart_test(xarm_msgs::Move &srv, ros::ServiceClient &client) -{ - std::vector inc = {0.3, 0, 0, 0, 0, 0}; - - srv.request.mvvelo = 0; - srv.request.mvacc = 0; - srv.request.mvtime = 1; - - srv.request.pose = inc; - - for (int i = 0; i < 500; i++) - { - if(client.call(srv)) - { - ROS_INFO("%s\n", srv.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service move_servoj"); - return 1; - } - usleep(1e4); // 0.01s - } - return 0; -} - - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "xarm_move_test"); - ros::NodeHandle nh; - nh.setParam("/xarm/wait_for_finish", true); // return after motion service finish - ros::ServiceClient motion_ctrl_client_ = nh.serviceClient("/xarm/motion_ctrl"); - ros::ServiceClient set_mode_client_ = nh.serviceClient("/xarm/set_mode"); - ros::ServiceClient set_state_client_ = nh.serviceClient("/xarm/set_state"); - ros::ServiceClient go_home_client_ = nh.serviceClient("/xarm/go_home"); - ros::ServiceClient servo_cart_client_ = nh.serviceClient("/xarm/move_servo_cart"); - - xarm_msgs::SetAxis set_axis_srv_; - xarm_msgs::SetInt16 set_int16_srv_; - xarm_msgs::Move move_srv_; - - - // STEP 1: Motion Enable - set_axis_srv_.request.id = 8; - set_axis_srv_.request.data = 1; - - if(motion_ctrl_client_.call(set_axis_srv_)) - { - ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service motion_ctrl"); - return 1; - } - - // STEP 2: Set Mode to 0 - set_int16_srv_.request.data = 0; - if(set_mode_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_mode"); - return 1; - } - - // STEP 3: Set State to 0 - set_int16_srv_.request.data = 0; - if(set_state_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_state"); - return 1; - } - - // STEP 4: Go to Home Position - if(go_home_test(move_srv_, go_home_client_) == 1) - return 1; - - - // STEP 5: Set Mode to 1 - set_int16_srv_.request.data = 1; - if(set_mode_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_mode"); - return 1; - } - - // STEP 6: Set state to 0 - set_int16_srv_.request.data = 0; - if(set_state_client_.call(set_int16_srv_)) - { - ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); - } - else - { - ROS_ERROR("Failed to call service set_state"); - return 1; - } - - // STEP 7: Call SERVO_CARTESIAN service in a loop - return servo_cart_test(move_srv_, servo_cart_client_); - -} \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_api/test/test_tool_modbus.cpp b/interbotix_ros_uxarms/xarm_api/test/test_tool_modbus.cpp deleted file mode 100644 index b023106..0000000 --- a/interbotix_ros_uxarms/xarm_api/test/test_tool_modbus.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "ros/ros.h" -#include "xarm_ros_client.h" -#include - -// Please run "export ROS_NAMESPACE=/xarm" first - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "xarm_modbus"); - ros::NodeHandle nh; - - xarm_api::XArmROSClient client; - client.init(nh); - - int recv_bytes = 7; - unsigned char send_data[6] = {0x01,0x06,0x00,0x0A,0x00,0x03}, recv_data[recv_bytes]={0}; - - int ret = 0; - ros::Rate rate(10); - - ret = client.send_tool_modbus(send_data, 6, recv_data, recv_bytes); - fprintf(stderr, "ret = %d, recv_data: ", ret); - - for(int i=0; i - - xarm_msgs - 0.0.0 - The xarm_msgs package - Jason Peng - BSD - catkin - std_msgs - message_generation - message_runtime - - message_generation - message_runtime - - std_msgs - std_msgs - - diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/ClearErr.srv b/interbotix_ros_uxarms/xarm_msgs/srv/ClearErr.srv deleted file mode 100644 index 8cf7c5e..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/ClearErr.srv +++ /dev/null @@ -1,6 +0,0 @@ - ---- - -int16 ret - -string message \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/ConfigToolModbus.srv b/interbotix_ros_uxarms/xarm_msgs/srv/ConfigToolModbus.srv deleted file mode 100644 index db2d856..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/ConfigToolModbus.srv +++ /dev/null @@ -1,10 +0,0 @@ -# configure the tool modbus communication baud rate, in bps: -int32 baud_rate - -# configure the timeout parameter in modbus communication, in milliseconds -int32 timeout_ms - ---- - -string message -int16 ret \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GetAnalogIO.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GetAnalogIO.srv deleted file mode 100644 index 18c8882..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GetAnalogIO.srv +++ /dev/null @@ -1,11 +0,0 @@ -# Getting one of the 2 analog Input port at robot end connector - -int16 port_num - ---- - -float32 analog_value - -int16 ret - -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GetControllerDigitalIO.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GetControllerDigitalIO.srv deleted file mode 100644 index 855046b..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GetControllerDigitalIO.srv +++ /dev/null @@ -1,11 +0,0 @@ -# Getting the controller DIGITAL input port status, io_num: from 1 to 8 - -int16 io_num - ---- - -int16 ret - -int16 value - -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GetDigitalIO.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GetDigitalIO.srv deleted file mode 100644 index 83e0a29..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GetDigitalIO.srv +++ /dev/null @@ -1,11 +0,0 @@ -# Getting the 2 digital Input port at robot end connector - ---- - -int32 digital_1 - -int32 digital_2 - -int16 ret - -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GetErr.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GetErr.srv deleted file mode 100644 index 4753c5c..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GetErr.srv +++ /dev/null @@ -1,6 +0,0 @@ - ---- - -int16 err - -string message \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GripperConfig.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GripperConfig.srv deleted file mode 100644 index f046513..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GripperConfig.srv +++ /dev/null @@ -1,7 +0,0 @@ -# Grip velocity configuration: range is from 1 to 5000 -float32 pulse_vel - ---- - -int16 ret -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GripperMove.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GripperMove.srv deleted file mode 100644 index ea61045..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GripperMove.srv +++ /dev/null @@ -1,7 +0,0 @@ -# position command of gripper: range from 0(close) to 850 (open) -float32 pulse_pos - ---- - -int16 ret -string message \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/GripperState.srv b/interbotix_ros_uxarms/xarm_msgs/srv/GripperState.srv deleted file mode 100644 index cb1f752..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/GripperState.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- - -float32 curr_pos -int16 err_code \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/Move.srv b/interbotix_ros_uxarms/xarm_msgs/srv/Move.srv deleted file mode 100755 index a8e9988..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/Move.srv +++ /dev/null @@ -1,38 +0,0 @@ -# request: command specification for motion executions. -# Units: -# joint space/angles: radian, radian/s and radian/s^2. -# Cartesian space: mm, mm/s, and mm/s^2. -# time: sec - -# pose: target coordinate. -# For Joint Space target,pose dimention is the number of joints. element as each target joint position. -# For Cartesian target: pose dimention is 6 for (x, y, z, roll, pitch, yaw) - -float32[] pose - -# mvvelo: specified maximum velocity during execution. linear or angular velocity - -float32 mvvelo - -# mvacc: specified maximum acceleration during execution. linear or angular acceleration. - -float32 mvacc - -# mvtime: currently do not have any special meaning, please just give it 0. -# 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) - -float32 mvtime - -# mvradii: this is special for move_ineb service, meaning the blending radius between 2 straight path trajectories, 0 for no blend. - -float32 mvradii - ---- - -# response: -# ret is 0 for successful execution and others for errors or warnings occured -# message is a string returned by function, indicating execution status. - -int16 ret - -string message \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/SetAxis.srv b/interbotix_ros_uxarms/xarm_msgs/srv/SetAxis.srv deleted file mode 100755 index 06732f2..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/SetAxis.srv +++ /dev/null @@ -1,15 +0,0 @@ -# request: for enabling / disabling motion control of one(or all) joint. -# id: 1-7 for target joint number, or 8 for all joints -# data: 0 for disabling servo control, 1 for enabling servo control. - -int16 id -int16 data - ---- - -# response: -# ret is 0 for successful execution and others for errors or warnings occured -# message is a string returned by function, indicating execution status. - -int16 ret -string message \ No newline at end of file diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/SetDigitalIO.srv b/interbotix_ros_uxarms/xarm_msgs/srv/SetDigitalIO.srv deleted file mode 100644 index 0be9580..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/SetDigitalIO.srv +++ /dev/null @@ -1,11 +0,0 @@ -# Setting the 2 digital Output port at robot end connector - -int16 io_num - -int16 value - ---- - -int16 ret - -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/SetInt16.srv b/interbotix_ros_uxarms/xarm_msgs/srv/SetInt16.srv deleted file mode 100755 index 8982ee7..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/SetInt16.srv +++ /dev/null @@ -1,13 +0,0 @@ -# request: set robot state or mode. -# data: int value indicating command robot state or execution mode. - -int16 data - ------ - -# response: -# ret is 0 for successful execution and others for errors or warnings occured -# message is a string returned by function, indicating execution status. - -int16 ret -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/SetLoad.srv b/interbotix_ros_uxarms/xarm_msgs/srv/SetLoad.srv deleted file mode 100644 index ee491eb..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/SetLoad.srv +++ /dev/null @@ -1,13 +0,0 @@ -# to set robot load parameters, based on initial Tool Frame located at flange center. -# Load mass (kg) -float32 mass - -# Load Center of Mass (mm), with respect to intial Tool Frame -float32 xc -float32 yc -float32 zc - ------ - -int16 ret -string message diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/SetToolModbus.srv b/interbotix_ros_uxarms/xarm_msgs/srv/SetToolModbus.srv deleted file mode 100644 index 3d63435..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/SetToolModbus.srv +++ /dev/null @@ -1,10 +0,0 @@ -# unsigned char data to be sent to tool device through modbus -uint8[] send_data - -# Specify the anticipated maximum respond data length in bytes -int16 respond_len - ---- - -int16 ret -uint8[] respond_data diff --git a/interbotix_ros_uxarms/xarm_msgs/srv/TCPOffset.srv b/interbotix_ros_uxarms/xarm_msgs/srv/TCPOffset.srv deleted file mode 100755 index e1a63df..0000000 --- a/interbotix_ros_uxarms/xarm_msgs/srv/TCPOffset.srv +++ /dev/null @@ -1,14 +0,0 @@ -# to give robot floating point number Cartesian TCP offset, based on initial Tool Frame located at flange center. - -float32 x -float32 y -float32 z - -float32 roll -float32 pitch -float32 yaw - ------ - -int16 ret -string message diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CATKIN_IGNORE b/interbotix_ros_xseries/COLCON_IGNORE similarity index 100% rename from interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CATKIN_IGNORE rename to interbotix_ros_xseries/COLCON_IGNORE diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox b/interbotix_ros_xseries/dynamixel_workbench_toolbox new file mode 160000 index 0000000..3ed8229 --- /dev/null +++ b/interbotix_ros_xseries/dynamixel_workbench_toolbox @@ -0,0 +1 @@ +Subproject commit 3ed8229d2382c4d0931b471fbe1c83a4888da6a8 diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/CHANGELOG.rst b/interbotix_ros_xseries/dynamixel_workbench_toolbox/CHANGELOG.rst deleted file mode 100644 index cb2e906..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/CHANGELOG.rst +++ /dev/null @@ -1,214 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package dynamixel_workbench_toolbox -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.2.0 (2020-09-25) ------------------- -* Added XW540 -* Bugs fixed, typos revised -* Contributors: Ryan Shim, Yutaka Kondo, T-Kitajima, Nico Zevallos, Madhur Deep Jain, Will Son, YongHo Na - -2.1.0 (2019-06-04) ------------------- -* Removed single_manager and single_manager_gui -* Added XH540, PRO (A Firmware), RH_P12_RN, RH_P12_RN (A Firmware) -* Contributors: Ryan Shim - -2.0.0 (2019-01-04) ------------------- -* added YAML configuration for Dynamixel -* supported position control mode, current based position control mode through Joint Trajectory Message -* supported velocity control mode through Twist Message -* reduced memory usage(Contribute by @KurtE) -* updated API for sync, bulk instruction -* updated control period (default is 10ms) -* supported Linux, macos -* supported Pro+ -* supported DynamixelSDK(after v.3.6.2) -* supported OpenManipulator -* Contributors: Darby Lim, KurtE, Pyo - -1.0.0 (2018-07-20) ------------------- -* upgraded read time #162 -* added Dynamixel PRO information #162 -* added readRegister function -* update dxl pro info -* update proInfo func -* modified max radian position -* Contributors: Darby Lim, Pyo, Taehun Lim - -0.3.1 (2018-06-04) ------------------- -* None - -0.3.0 (2018-06-01) ------------------- -* added getProtocolVersion() -* changed max_dxl_deries_num -* merged pull request `#152 `_ `#151 `_ `#149 `_ `#132 `_ -* Contributors: Darby Lim, Pyo - -0.2.4 (2018-03-20) ------------------- -* changed package.xml to format v2 -* Contributors: Pyo - -0.2.3 (2018-03-09) ------------------- -* None - -0.2.2 (2018-02-28) ------------------- -* modified the CI configurations (`#117 `_) -* modified the CMakeLists.txt to fix wrong path (`ros/rosdistro#17019 `_) -* Contributors: Pyo - -0.2.1 (2018-02-22) ------------------- -* modified the CMakeLists.txt to fix wrong path -* Contributors: Pyo - -0.2.0 (2018-02-19) ------------------- -* added conver function to PRO user -* added dxl_info_cnt init function -* added compatibility for different protocol -* added static -* added convert function -* added baudrate sorting -* added all dynamixel series -* added RX-10 -* added millis -* added init dynamixel example -* added setting for packet handler -* added monitor example -* added item -* added dynamixel_item -* added toolbox_ros and modified arduino path -* added linux build and example -* added begin and getprotocolversion function -* modified linux version -* modified description -* modified model_info -* modified variable range -* modified setTools function -* modified sync function -* modified merge conflict -* modified variable name -* modified reset function -* modified function name and return variable name -* modified name of return var -* modified item name -* modified reset function -* modified item name (added underscore) -* modified function name -* modified function for ROS depend -* modified function to make space -* modified begin function to reduce storage space -* modified MX (2.0) protocol setting bug -* modified example -* modified sync and bulk comm -* modified lib -* modified begin -* modified variable -* modified begin function -* modified joint and wheel mode -* modified variable name -* modified begin function -* modified set function -* modified dynamixel item -* modified scan function -* modified folder tree -* modified dynamixel_tool -* modified toolbox structure -* modified .device and modified funtion for opencm and opencr -* modified ifdef -* modified get file -* modified arduino version -* modified get device in arduino -* fixed reset bug -* deleted dead code -* deleted empty space -* deleted xl define -* deleted debug code and update ping func -* test OpenCM -* Contributors: Darby Lim, Yoonseok Pyo - -0.1.9 (2017-11-03) ------------------- -* modified dependency -* Contributors: Darby Lim - -0.1.8 (2017-11-01) ------------------- -* None - -0.1.7 (2017-10-30) ------------------- -* added rospy for the issue https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues/77 -* Contributors: Darby Lim - -0.1.6 (2017-08-09) ------------------- -* bug fixed -* updated error msg -* updated get model path -* updated Dynamixel PRO -* updated Dynamixel XL, XM and XH -* updated annotation -* updated multi driver -* updated address name -* updated msg name -* modified launch files -* modified variable -* modified file location -* added sync read -* added multi read function -* added multi_driver -* changed BSD license to Apache 2.0 license -* Contributors: Darby Lim - -0.1.5 (2017-05-23) ------------------- -* modified the cmake of toolbox -* Contributors: Darby Lim - -0.1.4 (2017-04-24) ------------------- -* toolbox bug fixed -* added dynamixel new model: XL430_W250 -* added dynamixel new model: XH -* renamed current controller -> torque controller -* Contributors: Darby Lim - -0.1.3 (2016-11-29) ------------------- -* modifiy folder path -* add drive_mode in XM series -* Contributors: Darby Lim - -0.1.2 (2016-10-31) ------------------- -* modify beta test feedback -* Contributors: Darby Lim - -0.1.1 (2016-10-21) ------------------- -* Revert "add baudrate combobox and modify velocity controller" - This reverts commit f4f83761d687c40660a2c864aa4fcbebe1df4ea4. -* add baudrate combobox and modify velocity controller -* Contributors: Darby Lim - -0.1.0 (2016-09-23) -------------------------- -* modified the package information for release -* edit cmake and xml files -* modify message -* add multiport controller and torque controller -* add position, velocity controller and pan-tilt, wheel tutorials -* add GUI package -* add pan tilt and wheel node in tutorial package -* add tutorial package -* add position, velocity, torque control package and change workbench_tool to workbench_toolbox -* Contributors: Darby Lim, Pyo diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/CMakeLists.txt b/interbotix_ros_xseries/dynamixel_workbench_toolbox/CMakeLists.txt deleted file mode 100644 index d204219..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_workbench_toolbox) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - dynamixel_sdk -) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_workbench_toolbox - CATKIN_DEPENDS roscpp dynamixel_sdk -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(dynamixel_workbench_toolbox - src/${PROJECT_NAME}/dynamixel_item.cpp - src/${PROJECT_NAME}/dynamixel_driver.cpp - src/${PROJECT_NAME}/dynamixel_tool.cpp - src/${PROJECT_NAME}/dynamixel_workbench.cpp -) -add_dependencies(dynamixel_workbench_toolbox ${catkin_EXPORTED_TARGETS}) -target_link_libraries(dynamixel_workbench_toolbox ${catkin_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS dynamixel_workbench_toolbox - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/README.md b/interbotix_ros_xseries/dynamixel_workbench_toolbox/README.md deleted file mode 100644 index 9d5405a..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/README.md +++ /dev/null @@ -1,29 +0,0 @@ -How to run dynamixel_workbench in linux without ROS - -1. Downloads DynamixelSDK - -``` -$ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK -``` - -2. Build DynamixelSDK - -http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/library_setup/cpp_linux/#build-the-library - - -3. Downloads Dynamixel-Workbench - -``` -$ git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench -``` - -4. Build Dynamixel-Workbench and Run examples - -``` -$ cd ${YOUR_DOWNLOAD_PATH}/dynamixel_workbench/dynamixel_workbench_toolbox/examples -$ mkdir build && cd build -$ cmake .. -$ make -$ sudo chmod a+rw ${USB_PORT} -$ ./monitor -``` \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CMakeLists.txt b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CMakeLists.txt deleted file mode 100644 index 2c0c72f..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_workbench) - -add_compile_options(-std=c++11) - -include_directories( - ../src -) - -add_library(dynamixel_workbench - ../src/dynamixel_workbench_toolbox/dynamixel_item.cpp - ../src/dynamixel_workbench_toolbox/dynamixel_driver.cpp - ../src/dynamixel_workbench_toolbox/dynamixel_tool.cpp - ../src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp -) - -if(APPLE) - target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_mac_cpp.dylib") -else() - target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_x64_cpp.so") -endif() - -add_executable(model_scan src/a_Model_Scan.cpp) -target_link_libraries(model_scan LINK_PUBLIC dynamixel_workbench) - -add_executable(ping src/b_Ping.cpp) -target_link_libraries(ping LINK_PUBLIC dynamixel_workbench) - -add_executable(id_change src/c_ID_Change.cpp) -target_link_libraries(id_change LINK_PUBLIC dynamixel_workbench) - -add_executable(bps_change src/d_BPS_Change.cpp) -target_link_libraries(bps_change LINK_PUBLIC dynamixel_workbench) - -add_executable(mode_change src/e_Mode_Change.cpp) -target_link_libraries(mode_change LINK_PUBLIC dynamixel_workbench) - -add_executable(reboot src/f_Reboot.cpp) -target_link_libraries(reboot LINK_PUBLIC dynamixel_workbench) - -add_executable(reset src/g_Reset.cpp) -target_link_libraries(reset LINK_PUBLIC dynamixel_workbench) - -add_executable(position src/h_Position.cpp) -target_link_libraries(position LINK_PUBLIC dynamixel_workbench) - -add_executable(velocity src/i_Velocity.cpp) -target_link_libraries(velocity LINK_PUBLIC dynamixel_workbench) - -add_executable(current_based_position src/j_Current_Based_Position.cpp) -target_link_libraries(current_based_position LINK_PUBLIC dynamixel_workbench) - -add_executable(read_write src/k_Read_Write.cpp) -target_link_libraries(read_write LINK_PUBLIC dynamixel_workbench) - -add_executable(sync_write src/l_Sync_Write.cpp) -target_link_libraries(sync_write LINK_PUBLIC dynamixel_workbench) - -add_executable(sync_read_write src/m_Sync_Read_Write.cpp) -target_link_libraries(sync_read_write LINK_PUBLIC dynamixel_workbench) - -add_executable(bulk_read_write src/n_Bulk_Read_Write.cpp) -target_link_libraries(bulk_read_write LINK_PUBLIC dynamixel_workbench) - -add_executable(find_dynamixel src/o_Find_Dynamixel.cpp) -target_link_libraries(find_dynamixel LINK_PUBLIC dynamixel_workbench) - -add_executable(monitor src/p_Monitor.cpp) -target_link_libraries(monitor LINK_PUBLIC dynamixel_workbench) \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/a_Model_Scan.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/a_Model_Scan.cpp deleted file mode 100644 index a0f299d..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/a_Model_Scan.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - - if (argc < 3) - { - printf("Please set '-port_name' and '-baud_rate' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint8_t scanned_id[16]; - uint8_t dxl_cnt = 0; - uint8_t range = 100; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeeded to init(%d)\n", baud_rate); - - printf("Wait for scan...\n"); - result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to scan\n"); - } - else - { - printf("Find %d Dynamixels\n", dxl_cnt); - - for (int cnt = 0; cnt < dxl_cnt; cnt++) - printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp deleted file mode 100644 index 3419ed7..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeeded to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp deleted file mode 100644 index 9bccf42..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/******************************************************************************* -* Copyright 2016 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - int new_dxl_id = 2; - - if (argc < 5) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id', '-new_dynamixel_id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - new_dxl_id = atoi(argv[4]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeeded to init(%d)\n", baud_rate); - - uint16_t model_number = 0; - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - - return 0; - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.changeID(dxl_id, new_dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - - uint8_t scanned_id[16]; - uint8_t dxl_cnt = 0; - uint8_t range = 100; - - printf("Wait for scan...\n"); - result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to scan\n"); - } - else - { - printf("Find %d Dynamixels\n", dxl_cnt); - - for (int cnt = 0; cnt < dxl_cnt; cnt++) - printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); - } -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp deleted file mode 100644 index 46a91eb..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int dxl_id = 1; - int baud_rate = 57600; - int new_baud_rate = 1000000; - - if (argc < 5) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel_id', '-new_baud_rate' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - new_baud_rate = atoi(argv[4]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeeded to init(%d)\n", baud_rate); - - uint16_t model_number = 0; - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - - return 0; - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.changeBaudrate(dxl_id, new_baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp deleted file mode 100644 index ea12c4b..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp +++ /dev/null @@ -1,177 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - int mode = 0; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeeded to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - printf("---> \n"); - printf("Please insert the right number from 0 to 5 to set control mode \n"); - printf("0 - current control mode\n"); - printf("1 - velocity control mode\n"); - printf("2 - position control mode\n"); - printf("3 - extended position control mode\n"); - printf("4 - current based position control mode\n"); - printf("5 - pwm control mode\n"); - printf("<--- \n\n"); - - scanf("%d", &mode); - - switch (mode) - { - case 0: - result = dxl_wb.setCurrentControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - case 1: - result = dxl_wb.setVelocityControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - case 2: - result = dxl_wb.setPositionControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - case 3: - result = dxl_wb.setExtendedPositionControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - case 4: - result = dxl_wb.setCurrentBasedPositionControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - case 5: - result = dxl_wb.setPWMControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - - default: - result = dxl_wb.setPositionControlMode(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to set mode\n"); - } - else - { - printf("Succeeded to set mode\n"); - } - break; - } - - return 0; -} diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp deleted file mode 100644 index 6521b17..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.reboot(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to reboot\n"); - } - else - { - printf("Succeed to reboot\n"); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp deleted file mode 100644 index 26c2258..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.reset(dxl_id, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to reset\n"); - } - else - { - printf("Succeed to reset\n"); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/h_Position.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/h_Position.cpp deleted file mode 100644 index b47a9f4..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/h_Position.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.jointMode(dxl_id, 0, 0, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change joint mode\n"); - } - else - { - printf("Succeed to change joint mode\n"); - printf("Dynamixel is moving...\n"); - - for (int count = 0; count < 3; count++) - { - dxl_wb.goalPosition(dxl_id, (int32_t)0); - sleep(3); - - dxl_wb.goalPosition(dxl_id, (int32_t)1023); - sleep(3); - } - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp deleted file mode 100644 index 25cb945..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.wheelMode(dxl_id, 0, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change wheel mode\n"); - } - else - { - printf("Succeed to change wheel mode\n"); - printf("Dynamixel is moving...\n"); - - for (int count = 0; count < 3; count++) - { - dxl_wb.goalVelocity(dxl_id, (int32_t)-100); - sleep(3); - - dxl_wb.goalVelocity(dxl_id, (int32_t)100); - sleep(3); - } - - dxl_wb.goalVelocity(dxl_id, (int32_t)0); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp deleted file mode 100644 index f078385..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.currentBasedPositionMode(dxl_id, 30, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change current based position mode\n"); - } - else - { - printf("Succeed to change current based position mode\n"); - printf("Dynamixel is moving...\n"); - - for (int count = 0; count < 3; count++) - { - dxl_wb.goalPosition(dxl_id, (int32_t)0); - sleep(3); - - dxl_wb.goalPosition(dxl_id, (int32_t)2048); - sleep(3); - } - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp deleted file mode 100644 index ad6045d..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - int dxl_id = 1; - - if (argc < 4) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id = atoi(argv[3]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint16_t model_number = 0; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - result = dxl_wb.ping(dxl_id, &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeed to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id, model_number); - } - - result = dxl_wb.itemWrite(dxl_id, "LED", 1, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to LED On\n"); - } - else - { - printf("Succeed to LED On\n"); - } - - int32_t get_data = 0; - result = dxl_wb.itemRead(dxl_id, "Present_Position", &get_data, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to get present position\n"); - } - else - { - printf("Succeed to get present position(value : %d)\n", get_data); - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp deleted file mode 100644 index fe5b381..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -void swap(int32_t *array); - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - - uint16_t model_number = 0; - uint8_t dxl_id[2] = {0, 0}; - - if (argc < 5) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id[0] = atoi(argv[3]); - dxl_id[1] = atoi(argv[4]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - for (int cnt = 0; cnt < 2; cnt++) - { - result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); - } - - result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change joint mode\n"); - } - else - { - printf("Succeed to change joint mode\n"); - } - } - - result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add sync write handler\n"); - } - - int32_t goal_position[2] = {0, 1023}; - - const uint8_t handler_index = 0; - - while(1) - { - result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to sync write position\n"); - } - - sleep(3); - - swap(goal_position); - } - - return 0; -} - -void swap(int32_t *array) -{ - int32_t tmp = array[0]; - array[0] = array[1]; - array[1] = tmp; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp deleted file mode 100644 index 7c24729..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -void swap(int32_t *array); - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - - uint16_t model_number = 0; - uint8_t dxl_id[2] = {0, 0}; - - if (argc < 5) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id[0] = atoi(argv[3]); - dxl_id[1] = atoi(argv[4]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - for (int cnt = 0; cnt < 2; cnt++) - { - result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); - } - - result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change joint mode\n"); - } - else - { - printf("Succeed to change joint mode\n"); - } - } - - result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add sync write handler\n"); - } - - result = dxl_wb.addSyncReadHandler(dxl_id[0], "Present_Position", &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add sync read handler\n"); - } - - int32_t goal_position[2] = {0, 1023}; - int32_t present_position[2] = {0, 0}; - - const uint8_t handler_index = 0; - - while(1) - { - result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to sync write position\n"); - } - - do - { - result = dxl_wb.syncRead(handler_index, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to sync read position\n"); - } - - result = dxl_wb.getSyncReadData(handler_index, &present_position[0], &log); - if (result == false) - { - printf("%s\n", log); - } - else - { - printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tGoal Position : %d\tPresent Position : %d\n" - ,dxl_id[0], goal_position[0], present_position[0], dxl_id[1], goal_position[1], present_position[1]); - } - - }while(abs(goal_position[0] - present_position[0]) > 15 && - abs(goal_position[1] - present_position[1]) > 15); - - swap(goal_position); - } - - return 0; -} - -void swap(int32_t *array) -{ - int32_t tmp = array[0]; - array[0] = array[1]; - array[1] = tmp; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp deleted file mode 100644 index 6fc2e07..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -void swap(int32_t *array); - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - int baud_rate = 57600; - - uint16_t model_number = 0; - uint8_t dxl_id[2] = {0, 0}; - - if (argc < 5) - { - printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - baud_rate = atoi(argv[2]); - dxl_id[0] = atoi(argv[3]); - dxl_id[1] = atoi(argv[4]); - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - result = dxl_wb.init(port_name, baud_rate, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - - return 0; - } - else - printf("Succeed to init(%d)\n", baud_rate); - - for (int cnt = 0; cnt < 2; cnt++) - { - result = dxl_wb.ping(dxl_id[cnt], &model_number, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("Succeeded to ping\n"); - printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); - } - - result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to change joint mode\n"); - } - else - { - printf("Succeed to change joint mode\n"); - } - } - - result = dxl_wb.initBulkWrite(&log); - if (result == false) - { - printf("%s\n", log); - } - else - { - printf("%s\n", log); - } - - result = dxl_wb.initBulkRead(&log); - if (result == false) - { - printf("%s\n", log); - } - else - { - printf("%s\n", log); - } - - result = dxl_wb.addBulkReadParam(dxl_id[0], "Present_Position", &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add bulk read position param\n"); - } - else - { - printf("%s\n", log); - } - - result = dxl_wb.addBulkReadParam(dxl_id[1], "LED", &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add bulk read led param\n"); - } - else - { - printf("%s\n", log); - } - - int32_t goal_position[2] = {0, 1023}; - int32_t led[2] = {0, 1}; - - int32_t get_data[2] = {0, 0}; - - const uint8_t handler_index = 0; - - while(1) - { - result = dxl_wb.addBulkWriteParam(dxl_id[0], "Goal_Position", goal_position[0], &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add bulk write position param\n"); - } - else - { - printf("%s\n", log); - } - - result = dxl_wb.addBulkWriteParam(dxl_id[1], "LED", led[0], &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to add bulk write led param\n"); - } - else - { - printf("%s\n", log); - } - - result = dxl_wb.bulkWrite(&log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to bulk write\n"); - } - - do - { - result = dxl_wb.bulkRead(&log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to bulk read\n"); - } - - result = dxl_wb.getBulkReadData(&get_data[0], &log); - if (result == false) - { - printf("%s\n", log); - } - else - { - printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tLED : %d\n" - ,dxl_id[0], goal_position[0], get_data[0], dxl_id[1], get_data[1]); - } - - }while(abs(goal_position[0] - get_data[0]) > 15); - - swap(goal_position); - swap(led); - } - - return 0; -} - -void swap(int32_t *array) -{ - int32_t tmp = array[0]; - array[0] = array[1]; - array[1] = tmp; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp deleted file mode 100644 index 5615952..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include - -#define BAUDRATE_NUM 7 - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - - if (argc < 2) - { - printf("Please set '-port_name' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - } - - DynamixelWorkbench dxl_wb; - - const char *log; - bool result = false; - - uint8_t scanned_id[100]; - uint8_t dxl_cnt = 0; - - uint32_t baudrate[BAUDRATE_NUM] = {9600, 57600, 115200, 1000000, 2000000, 3000000, 4000000}; - uint8_t range = 253; - - uint8_t index = 0; - - while (index < BAUDRATE_NUM) - { - result = dxl_wb.init(port_name, baudrate[index], &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - } - else - printf("Succeed to init(%d)\n", baudrate[index]); - - dxl_cnt = 0; - for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0; - - printf("Wait for scan...\n"); - result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); - if (result == false) - { - printf("%s\n", log); - printf("Failed to scan\n"); - } - else - { - printf("Find %d Dynamixels\n", dxl_cnt); - - for (int cnt = 0; cnt < dxl_cnt; cnt++) - printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); - } - - index++; - } - - return 0; -} \ No newline at end of file diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp deleted file mode 100644 index 05b2082..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp +++ /dev/null @@ -1,694 +0,0 @@ -/******************************************************************************* - Copyright 2016 ROBOTIS CO., LTD. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include -#include // FILE control -#include // Terminal IO -#include - -using namespace std; - -#define ESC_ASCII_VALUE 0x1b -#define SPACEBAR_ASCII_VALUE 0x20 -#define ENTER_ASCII_VALUE 0x0a - -uint8_t get_id[16]; -uint8_t scan_cnt = 0; -uint8_t ping_cnt = 0; - -int getch(void); -int kbhit(void); -bool isAvailableID(uint8_t id); -void printInst(); -bool monitoring(const char* port_name); - -DynamixelWorkbench dxl_wb; - -int main(int argc, char *argv[]) -{ - const char* port_name = "/dev/ttyUSB0"; - - if (argc < 2) - { - printf("Please set '-port_name' arguments for connected Dynamixels\n"); - return 0; - } - else - { - port_name = argv[1]; - } - - printInst(); - - while(true) - { - monitoring(port_name); - } - - return 0; -} - -bool monitoring(const char* port_name) -{ - char input[128]; - char cmd[80]; - char param[20][30]; - int num_param = 0; - char *token; - bool valid_cmd = false; - - bool wb_result = false; - const char *log; - - if (kbhit()) - { - if (getchar() == ENTER_ASCII_VALUE) - { - printf("[CMD]"); - fgets(input, sizeof(input), stdin); - - char *p; - if ((p = strchr(input, '\n'))!= NULL) *p = '\0'; - fflush(stdin); - - if (strlen(input) == 0) return false; - - token = strtok(input, " "); - - if (token == 0) return false; - - strcpy(cmd, token); - token = strtok(0, " "); - num_param = 0; - - while (token != 0) - { - strcpy(param[num_param++], token); - token = strtok(0, " "); - } - - if (strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0) - { - printInst(); - } - else if (strcmp(cmd, "begin") == 0) - { - uint32_t baud = 57600; - - baud = atoi(param[0]); - wb_result = dxl_wb.init(port_name, baud, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to init\n"); - } - else - printf("Succeed to init(%d)\n", baud); - } - else if (strcmp(cmd, "end") == 0) - { - exit(0); - } - else if (strcmp(cmd, "scan") == 0) - { - uint8_t range = 253; // default - - range = atoi(param[0]); - wb_result = dxl_wb.scan(get_id, &scan_cnt, range, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to scan\n"); - } - else - { - printf("Find %d Dynamixels\n", scan_cnt); - - for (int cnt = 0; cnt < scan_cnt; cnt++) - printf("id : %d\n", get_id[cnt]); - } - } - else if (strcmp(cmd, "ping") == 0) - { - get_id[ping_cnt] = 1; // default to 1 - - get_id[ping_cnt] = atoi(param[0]); - - uint16_t model_number = 0; - - wb_result = dxl_wb.ping(get_id[ping_cnt], &model_number, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to ping\n"); - } - else - { - printf("id : %d, model_number : %d\n", get_id[ping_cnt], model_number); - ping_cnt++; - } - } - else if (isAvailableID(atoi(param[0]))) - { - if (strcmp(cmd, "control_table") == 0) - { - uint8_t id = atoi(param[0]); - - const ControlItem *control_item = dxl_wb.getControlTable(id); - uint8_t the_number_of_control_item = dxl_wb.getTheNumberOfControlItem(id); - - uint16_t last_register_addr = control_item[the_number_of_control_item-1].address; - uint16_t last_register_addr_length = control_item[the_number_of_control_item-1].data_length; - - uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length]; - - if (control_item != NULL) - { - wb_result = dxl_wb.readRegister(id, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - for (int index = 0; index < the_number_of_control_item; index++) - { - uint32_t data = 0; - - if (dxl_wb.getProtocolVersion() == 2.0f) - { - data = getAllRegisteredData[control_item[index].address]; - printf("\t%s : %d\n", control_item[index].item_name, data); - } - else if (dxl_wb.getProtocolVersion() == 1.0f) - { - switch (control_item[index].data_length) - { - case BYTE: - data = getAllRegisteredData[control_item[index].address]; - printf("\t%s : %d\n", control_item[index].item_name, data); - break; - - case WORD: - data = DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]); - printf("\t%s : %d\n", control_item[index].item_name, data); - break; - - case DWORD: - data = DXL_MAKEDWORD(DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]), - DXL_MAKEWORD(getAllRegisteredData[control_item[index].address+2], getAllRegisteredData[control_item[index].address+3])); - printf("\t%s : %d\n", control_item[index].item_name, data); - break; - - default: - data = getAllRegisteredData[control_item[index].address]; - break; - } - } - } - } - } - } - else if (strcmp(cmd, "sync_write_handler") == 0) - { - static uint8_t sync_write_handler_index = 0; - uint8_t id = atoi(param[0]); - wb_result = dxl_wb.addSyncWriteHandler(id, param[1], &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to add sync write handler\n"); - return 0; - } - else - printf("%s, sync_write_handler_index = %d\n", log, sync_write_handler_index++); - } - else if (strcmp(cmd, "sync_read_handler") == 0) - { - static uint8_t sync_read_handler_index = 0; - uint8_t id = atoi(param[0]); - wb_result = dxl_wb.addSyncReadHandler(id, param[1], &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to add sync read handler\n"); - return 0; - } - else - printf("%s, sync_read_handler_index = %d\n", log, sync_read_handler_index++); - } - else if (strcmp(cmd, "bulk_write_handler") == 0) - { - wb_result = dxl_wb.initBulkWrite(&log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to init bulk write handler\n"); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "bulk_write_param") == 0) - { - uint8_t id = atoi(param[0]); - wb_result = dxl_wb.addBulkWriteParam(id, param[1], atoi(param[2]), &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to add param for bulk write\n"); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "bulk_write") == 0) - { - wb_result = dxl_wb.bulkWrite(&log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to bulk write\n"); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "bulk_read_handler") == 0) - { - wb_result = dxl_wb.initBulkRead(&log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to init bulk read handler\n"); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "bulk_read_param") == 0) - { - uint8_t id = atoi(param[0]); - wb_result = dxl_wb.addBulkReadParam(id, param[1], &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to add param for bulk read\n"); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "bulk_read") == 0) - { - wb_result = dxl_wb.bulkRead(&log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to bulk read\n"); - return 0; - } - else - printf("%s\n", log); - - int32_t get_data[dxl_wb.getTheNumberOfBulkReadParam()]; - wb_result = dxl_wb.getBulkReadData(&get_data[0], &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to get bulk read data\n"); - return 0; - } - else - { - printf("%s\n", log); - for (uint8_t index = 0; index < dxl_wb.getTheNumberOfBulkReadParam(); index++) - printf("data[%d] : %d ", index, get_data[index]); - - printf("\n"); - } - - dxl_wb.clearBulkReadParam(); - } - else if (isAvailableID(atoi(param[0])) && isAvailableID(atoi(param[1]))) - { - if (strcmp(cmd, "sync_write") == 0) - { - uint8_t id_1 = atoi(param[0]); - uint8_t id_2 = atoi(param[1]); - uint8_t id[2] = {id_1, id_2}; - uint8_t id_num = 2; - - int32_t data[2] = {0, 0}; - data[0] = atoi(param[3]); - data[1] = atoi(param[4]); - - uint8_t handler_index = atoi(param[2]); - - wb_result = dxl_wb.syncWrite(handler_index, id, id_num, (int32_t *)data, 1, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - printf("%s\n", log); - } - else if (strcmp(cmd, "sync_read") == 0) - { - uint8_t id_1 = atoi(param[0]); - uint8_t id_2 = atoi(param[1]); - uint8_t id[2] = {id_1, id_2}; - uint8_t id_num = 2; - - int32_t data[2] = {0, 0}; - uint8_t handler_index = atoi(param[2]); - - wb_result = dxl_wb.syncRead(handler_index, id, id_num, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - - wb_result = dxl_wb.getSyncReadData(handler_index, id, id_num, data, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - printf("id[%d] : %d, id[%d] : %d\n", id_1, data[0], id_2, data[1]); - } - } - } - else if (strcmp(cmd, "id") == 0) - { - uint8_t id = atoi(param[0]); - uint8_t new_id = atoi(param[1]); - - wb_result = dxl_wb.changeID(id, new_id, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "baud") == 0) - { - uint8_t id = atoi(param[0]); - uint32_t new_baud = atoi(param[1]); - - wb_result = dxl_wb.changeBaudrate(id, new_baud, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - wb_result = dxl_wb.setBaudrate(new_baud, &log); - printf("%s\n", log); - } - } - else if (strcmp(cmd, "torque_on") == 0) - { - uint8_t id = atoi(param[0]); - - wb_result = dxl_wb.torqueOn(id, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "torque_off") == 0) - { - uint8_t id = atoi(param[0]); - - wb_result = dxl_wb.torqueOff(id, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "joint") == 0) - { - uint8_t id = atoi(param[0]); - uint32_t goal = atoi(param[1]); - - wb_result = dxl_wb.jointMode(id, 0, 0, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - - wb_result = dxl_wb.goalPosition(id, (int32_t)goal, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "wheel") == 0) - { - uint8_t id = atoi(param[0]); - uint32_t goal = atoi(param[1]); - - wb_result = dxl_wb.wheelMode(id, 0, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - - wb_result = dxl_wb.goalVelocity(id, (int32_t)goal, &log); - if (wb_result == false) - { - printf("%s\n", log); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "write") == 0) - { - uint8_t id = atoi(param[0]); - int32_t value = atoi(param[2]); - - wb_result = dxl_wb.writeRegister(id, param[1], value, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to write\n"); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "read") == 0) - { - uint8_t id = atoi(param[0]); - - int32_t data = 0; - - wb_result = dxl_wb.readRegister(id, param[1], &data, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to read\n"); - return 0; - } - else - { - printf("%s\n", log); - printf("read data : %d\n", data); - } - } - else if (strcmp(cmd, "reboot") == 0) - { - uint8_t id = atoi(param[0]); - - wb_result = dxl_wb.reboot(id, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to reboot\n"); - return 0; - } - else - { - printf("%s\n", log); - } - } - else if (strcmp(cmd, "reset") == 0) - { - uint8_t id = atoi(param[0]); - - wb_result = dxl_wb.reset(id, &log); - if (wb_result == false) - { - printf("%s\n", log); - printf("Failed to reset\n"); - return 0; - } - else - { - printf("%s\n", log); - } - } - else - { - printf("Wrong command\n"); - } - } - else - { - printf("Please check ID\n"); - } - } - } - - return 0; -} - -bool isAvailableID(uint8_t id) -{ - for (int dxl_cnt = 0; dxl_cnt < (scan_cnt + ping_cnt); dxl_cnt++) - { - if (get_id[dxl_cnt] == id) - return true; - } - - return false; -} - -void printInst(void) -{ - printf("-------------------------------------\n"); - printf("Set begin before scan or ping\n"); - printf("-------------------------------------\n"); - printf("help\n"); - printf("begin (BAUD)\n"); - printf("scan (RANGE)\n"); - printf("ping (ID)\n"); - printf("control_table (ID)\n"); - printf("id (ID) (NEW_ID)\n"); - printf("baud (ID) (NEW_BAUD)\n"); - printf("torque_on (ID)\n"); - printf("torque_off (ID)\n"); - printf("joint (ID) (GOAL_POSITION)\n"); - printf("wheel (ID) (GOAL_VELOCITY)\n"); - printf("write (ID) (ADDRESS_NAME) (DATA)\n"); - printf("read (ID) (ADDRESS_NAME)\n"); - printf("sync_write_handler (Ref_ID) (ADDRESS_NAME)\n"); - printf("sync_write (ID_1) (ID_2) (HANDLER_INDEX) (PARAM_1) (PARAM_2)\n"); - printf("sync_read_handler (Ref_ID) (ADDRESS_NAME)\n"); - printf("sync_read (ID_1) (ID_2) (HANDLER_INDEX)\n"); - printf("bulk_write_handler\n"); - printf("bulk_write_param (ID) (ADDRESS_NAME) (PARAM)\n"); - printf("bulk_write\n"); - printf("bulk_read_handler\n"); - printf("bulk_read_param (ID) (ADDRESS_NAME)\n"); - printf("bulk_read\n"); - printf("reboot (ID) \n"); - printf("reset (ID) \n"); - printf("end\n"); - printf("-------------------------------------\n"); - printf("Press Enter Key\n"); -} - -int getch() -{ - struct termios oldt, newt; - int ch; - - tcgetattr( STDIN_FILENO, &oldt ); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - newt.c_cc[VMIN] = 0; - newt.c_cc[VTIME] = 1; - tcsetattr( STDIN_FILENO, TCSANOW, &newt ); - ch = getchar(); - tcsetattr( STDIN_FILENO, TCSANOW, &oldt ); - - return ch; -} - -int kbhit() -{ - struct termios oldt, newt; - int ch; - int oldf; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - oldf = fcntl(STDIN_FILENO, F_GETFL, 0); - fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); - - ch = getchar(); - - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - fcntl(STDIN_FILENO, F_SETFL, oldf); - - if (ch != EOF) - { - ungetc(ch, stdin); - return 1; - } - return 0; -} diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h b/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h deleted file mode 100644 index d5a2747..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h +++ /dev/null @@ -1,188 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#ifndef DYNAMIXEL_DRIVER_H -#define DYNAMIXEL_DRIVER_H - -#include "dynamixel_tool.h" - -#if defined(__OPENCR__) || defined(__OPENCM904__) - #include - #include -#elif defined(__linux__) || defined(__APPLE__) - #include "unistd.h" - #include "dynamixel_sdk/dynamixel_sdk.h" -#endif - -#define MAX_DXL_SERIES_NUM 5 -#define MAX_HANDLER_NUM 5 -#define MAX_BULK_PARAMETER 20 - -typedef struct -{ - const ControlItem *control_item; - dynamixel::GroupSyncWrite *groupSyncWrite; -} SyncWriteHandler; - -typedef struct -{ - const ControlItem *control_item; - dynamixel::GroupSyncRead *groupSyncRead; -} SyncReadHandler; - -typedef struct -{ - uint8_t id; - uint16_t address; - uint16_t data_length; -} BulkParameter; - -typedef struct -{ - int dxl_comm_result; - bool dxl_addparam_result; - bool dxl_getdata_result; - uint8_t dxl_error; -} ErrorFromSDK; - -class DynamixelDriver -{ - private: - dynamixel::PortHandler *portHandler_; - dynamixel::PacketHandler *packetHandler_; - - SyncWriteHandler syncWriteHandler_[MAX_HANDLER_NUM]; - SyncReadHandler syncReadHandler_[MAX_HANDLER_NUM]; - - dynamixel::GroupBulkRead *groupBulkRead_; - dynamixel::GroupBulkWrite *groupBulkWrite_; - BulkParameter bulk_read_param_[MAX_BULK_PARAMETER]; - - DynamixelTool tools_[MAX_DXL_SERIES_NUM]; - - uint8_t tools_cnt_; - uint8_t sync_write_handler_cnt_; - uint8_t sync_read_handler_cnt_; - uint8_t bulk_read_parameter_cnt_; - - public: - DynamixelDriver(); - ~DynamixelDriver(); - - bool init(const char* device_name = "/dev/ttyUSB0", - uint32_t baud_rate = 57600, - const char **log = NULL); - - bool begin(const char* device_name = "/dev/ttyUSB0", - uint32_t baud_rate = 57600, - const char **log = NULL); - - bool setPortHandler(const char *device_name, const char **log = NULL); - bool setBaudrate(uint32_t baud_rate, const char **log = NULL); - bool setPacketHandler(float protocol_version, const char **log = NULL); - - float getProtocolVersion(void); - uint32_t getBaudrate(void); - - const char * getModelName(uint8_t id, const char **log = NULL); - uint16_t getModelNumber(uint8_t id, const char **log = NULL); - const ControlItem *getControlTable(uint8_t id, const char **log = NULL); - const ControlItem *getItemInfo(uint8_t id, const char *item_name, const char **log = NULL); - uint8_t getTheNumberOfControlItem(uint8_t id, const char **log = NULL); - const ModelInfo* getModelInfo(uint8_t id, const char **log = NULL); - - uint8_t getTheNumberOfSyncWriteHandler(void); - uint8_t getTheNumberOfSyncReadHandler(void); - uint8_t getTheNumberOfBulkReadParam(void); - - bool scan(uint8_t *get_id, - uint8_t *get_the_number_of_id, - uint8_t range = 253, - const char **log = NULL); - - bool scan(uint8_t *get_id, - uint8_t *get_the_number_of_id, - uint8_t start_number, - uint8_t end_number, - const char **log = NULL); - - bool ping(uint8_t id, - uint16_t *get_model_number, - const char **log = NULL); - - bool ping(uint8_t id, - const char **log = NULL); - - bool clearMultiTurn(uint8_t id, const char **log = NULL); - - bool reboot(uint8_t id, const char **log = NULL); - bool reset(uint8_t id, const char **log = NULL); - - bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t* data, const char **log = NULL); - bool writeRegister(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); - - bool writeOnlyRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log = NULL); - bool writeOnlyRegister(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); - - bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log = NULL); - bool readRegister(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); - - void getParam(int32_t data, uint8_t *param); - - bool addSyncWriteHandler(uint16_t address, uint16_t length, const char **log = NULL); - bool addSyncWriteHandler(uint8_t id, const char *item_name, const char **log = NULL); - - bool syncWrite(uint8_t index, int32_t *data, const char **log = NULL); - bool syncWrite(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, uint8_t data_num_for_each_id, const char **log = NULL); - - bool addSyncReadHandler(uint16_t address, uint16_t length, const char **log = NULL); - bool addSyncReadHandler(uint8_t id, const char *item_name, const char **log = NULL); - - bool syncRead(uint8_t index, const char **log = NULL); - bool syncRead(uint8_t index, uint8_t *id, uint8_t id_num, const char **log = NULL); - - bool getSyncReadData(uint8_t index, int32_t *data, const char **log = NULL); - bool getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, const char **log = NULL); - bool getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, uint16_t address, uint16_t length, int32_t *data, const char **log = NULL); - - bool initBulkWrite(const char **log = NULL); - - bool addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log = NULL); - bool addBulkWriteParam(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); - - bool bulkWrite(const char **log = NULL); - - bool initBulkRead(const char **log = NULL); - - bool addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log = NULL); - bool addBulkReadParam(uint8_t id, const char *item_name, const char **log = NULL); - - bool bulkRead(const char **log = NULL); - - bool getBulkReadData(int32_t *data, const char **log = NULL); - bool getBulkReadData(uint8_t *id, uint8_t id_num, uint16_t *address, uint16_t *length, int32_t *data, const char **log = NULL); - - bool clearBulkReadParam(void); - - private: - void initTools(void); - bool setTool(uint16_t model_number, uint8_t id, const char **log = NULL); - uint8_t getTool(uint8_t id, const char **log = NULL); -}; - -#endif //DYNAMIXEL_DRIVER_H diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h b/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h deleted file mode 100644 index 743081b..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h +++ /dev/null @@ -1,137 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby), Ryan Shim */ - -#ifndef DYNAMIXEL_ITEM_H -#define DYNAMIXEL_ITEM_H - -#include -#include - -#define AX_12A 12 -#define AX_12W 300 -#define AX_18A 18 - -#define RX_10 10 -#define RX_24F 24 -#define RX_28 28 -#define RX_64 64 - -#define EX_106 107 - -#define MX_12W 360 -#define MX_28 29 -#define MX_28_2 30 -#define MX_64 310 -#define MX_64_2 311 -#define MX_106 320 -#define MX_106_2 321 - -#define XL_320 350 -#define XL430_W250 1060 - -#define XL430_W250_2 1090 // 2XL - -#define XC430_W150 1070 -#define XC430_W240 1080 - -#define XM430_W210 1030 -#define XM430_W350 1020 - -#define XM540_W150 1130 -#define XM540_W270 1120 - -#define XH430_W210 1010 -#define XH430_W350 1000 -#define XH430_V210 1050 -#define XH430_V350 1040 - -#define XH540_W150 1110 -#define XH540_W270 1100 -#define XH540_V150 1150 -#define XH540_V270 1140 - -#define XW540_T260 1170 -#define XW540_T140 1180 - -#define PRO_L42_10_S300_R 35072 -#define PRO_L54_30_S400_R 37928 -#define PRO_L54_30_S500_R 37896 -#define PRO_L54_50_S290_R 38176 -#define PRO_L54_50_S500_R 38152 - -#define PRO_M42_10_S260_R 43288 -#define PRO_M54_40_S250_R 46096 -#define PRO_M54_60_S250_R 46352 - -#define PRO_H42_20_S300_R 51200 -#define PRO_H54_100_S500_R 53768 -#define PRO_H54_200_S500_R 54024 - -#define PRO_M42_10_S260_R_A 43289 -#define PRO_M54_40_S250_R_A 46097 -#define PRO_M54_60_S250_R_A 46353 - -#define PRO_H42_20_S300_R_A 51201 -#define PRO_H54_100_S500_R_A 53769 -#define PRO_H54_200_S500_R_A 54025 - -#define PRO_PLUS_M42P_010_S260_R 2100 -#define PRO_PLUS_M54P_040_S250_R 2110 -#define PRO_PLUS_M54P_060_S250_R 2120 - -#define PRO_PLUS_H42P_020_S300_R 2000 -#define PRO_PLUS_H54P_100_S500_R 2010 -#define PRO_PLUS_H54P_200_S500_R 2020 - -#define RH_P12_RN 35073 -#define RH_P12_RN_A 35074 - -#define BYTE 1 -#define WORD 2 -#define DWORD 4 - -typedef struct -{ - const char *item_name; - uint16_t address; - uint8_t item_name_length; - uint16_t data_length; -} ControlItem; - -typedef struct -{ - float rpm; - - int64_t value_of_min_radian_position; - int64_t value_of_zero_radian_position; - int64_t value_of_max_radian_position; - - float min_radian; - float max_radian; -} ModelInfo; - -// Public Functions -namespace DynamixelItem -{ -const ControlItem *getControlTable(uint16_t model_number); -const ModelInfo *getModelInfo(uint16_t model_number); - -uint8_t getTheNumberOfControlItem(); -} - -#endif //DYNAMIXEL_ITEM_H diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_tool.h b/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_tool.h deleted file mode 100644 index 56f91de..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_tool.h +++ /dev/null @@ -1,82 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#ifndef DYNAMIXEL_TOOL_H -#define DYNAMIXEL_TOOL_H - -#include -#include - -#include "dynamixel_item.h" - -class DynamixelTool -{ - private: - enum {DYNAMIXEL_BUFFER = 36}; - uint8_t dxl_id_[DYNAMIXEL_BUFFER]; - uint8_t dxl_cnt_; - - const char *model_name_; - uint16_t model_number_; - - const ControlItem *control_table_; - const ModelInfo *model_info_; - - uint16_t the_number_of_control_item_; - - public: - DynamixelTool(); - ~DynamixelTool(); - - void initTool(void); - - bool addTool(const char *model_name, uint8_t id, const char **log = NULL); - bool addTool(uint16_t model_number, uint8_t id, const char **log = NULL); - - void addDXL(uint8_t id); - - const char *getModelName(void); - uint16_t getModelNumber(void); - - const uint8_t* getID(void); - uint8_t getDynamixelBuffer(void); - uint8_t getDynamixelCount(void); - - float getRPM(void); - - int64_t getValueOfMinRadianPosition(void); - int64_t getValueOfMaxRadianPosition(void); - int64_t getValueOfZeroRadianPosition(void); - - float getMinRadian(void); - float getMaxRadian(void); - - uint8_t getTheNumberOfControlItem(void); - - const ControlItem *getControlItem(const char *item_name, const char **log = NULL); - const ControlItem *getControlTable(void); - const ModelInfo *getModelInfo(void); - - private: - bool setControlTable(const char *model_name, const char **log = NULL); - bool setControlTable(uint16_t model_number, const char **log = NULL); - - bool setModelName(uint16_t model_number, const char **log = NULL); - bool setModelNumber(const char *model_name, const char **log = NULL); -}; -#endif //DYNAMIXEL_TOOL_H diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h b/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h deleted file mode 100644 index 5a32222..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h +++ /dev/null @@ -1,100 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#ifndef DYNAMIXEL_WORKBENCH_H_ -#define DYNAMIXEL_WORKBENCH_H_ - -#include "dynamixel_driver.h" - -class DynamixelWorkbench : public DynamixelDriver -{ - public: - DynamixelWorkbench(); - ~DynamixelWorkbench(); - - bool torque(uint8_t id, int32_t onoff, const char **log = NULL); - bool torqueOn(uint8_t id, const char **log = NULL); - bool torqueOff(uint8_t id, const char **log = NULL); - - bool changeID(uint8_t id, uint8_t new_id, const char **log = NULL); - bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log = NULL); - bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log = NULL); - - bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); - bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); - - bool led(uint8_t id, int32_t onoff, const char **log = NULL); - bool ledOn(uint8_t id, const char **log = NULL); - bool ledOff(uint8_t id, const char **log = NULL); - - bool setNormalDirection(uint8_t id, const char **log = NULL); - bool setReverseDirection(uint8_t id, const char **log = NULL); - - bool setVelocityBasedProfile(uint8_t id, const char **log = NULL); - bool setTimeBasedProfile(uint8_t id, const char **log = NULL); - - bool setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log = NULL); - - bool setCurrentControlMode(uint8_t id, const char **log = NULL); - bool setTorqueControlMode(uint8_t id, const char **log = NULL); - bool setVelocityControlMode(uint8_t id, const char **log = NULL); - bool setPositionControlMode(uint8_t id, const char **log = NULL); - bool setExtendedPositionControlMode(uint8_t id, const char **log = NULL); - bool setMultiTurnControlMode(uint8_t id, const char **log = NULL); - bool setCurrentBasedPositionControlMode(uint8_t id, const char **log = NULL); - bool setPWMControlMode(uint8_t id, const char **log = NULL); - - bool setOperatingMode(uint8_t id, uint8_t index, const char **log = NULL); - - bool jointMode(uint8_t id, int32_t velocity = 0, int32_t acceleration = 0, const char **log = NULL); - bool wheelMode(uint8_t id, int32_t acceleration = 0, const char **log = NULL); - bool currentBasedPositionMode(uint8_t id, int32_t current = 0, const char **log = NULL); - - bool goalPosition(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes - // bool goalPosition(uint8_t id, int32_t value, const char **log = NULL); - bool goalPosition(uint8_t id, float radian, const char **log = NULL); - - bool goalSpeed(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes - bool goalVelocity(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes - // bool goalVelocity(uint8_t id, int32_t value, const char **log = NULL); - bool goalVelocity(uint8_t id, float velocity, const char **log = NULL); - - bool getPresentPositionData(uint8_t id, int32_t* data, const char **log = NULL); - bool getRadian(uint8_t id, float* radian, const char **log = NULL); - - bool getPresentVelocityData(uint8_t id, int32_t* data, const char **log = NULL); - bool getVelocity(uint8_t id, float* velocity, const char **log = NULL); - - int32_t convertRadian2Value(uint8_t id, float radian); - float convertValue2Radian(uint8_t id, int32_t value); - - int32_t convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian); - float convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian); - - int32_t convertVelocity2Value(uint8_t id, float velocity); - float convertValue2Velocity(uint8_t id, int32_t value); - - int16_t convertCurrent2Value(uint8_t id, float current); - int16_t convertCurrent2Value(float current); - float convertValue2Current(uint8_t id, int16_t value); - float convertValue2Current(int16_t value); - - float convertValue2Load(int16_t value); -}; - -#endif /*DYNAMIXEL_WORKBENCH_H_*/ diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/library.properties b/interbotix_ros_xseries/dynamixel_workbench_toolbox/library.properties deleted file mode 100644 index cbdee9a..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/library.properties +++ /dev/null @@ -1,9 +0,0 @@ -name=DynamixelWorkbench -version=0.1.0 -author=Darby Lim (thlim@robotis.com) -maintainer=Pyo (pyo@robotis.com) -sentence=DynamixelWorkbench using DynamixelSDK -paragraph=Tools for Dynamixel -category=Other -url=https://github.com/ROBOTIS-GIT/dynamixel-workbench -architectures=OpenCM904 diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/package.xml b/interbotix_ros_xseries/dynamixel_workbench_toolbox/package.xml deleted file mode 100644 index 6e342ca..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - dynamixel_workbench_toolbox - 2.2.0 - - This package is composed of 'dynamixel_item', 'dynamixel_tool', 'dynamixel_driver' and 'dynamixel_workbench' class. - The 'dynamixel_item' is saved as control table item and information of Dynamixels. - The 'dynamixel_tool' class loads its by model number of Dynamixels. - The 'dynamixel_driver' class includes wraped function used in DYNAMIXEL SDK. - The 'dynamixel_workbench' class make simple to use Dynamixels - - Apache 2.0 - Darby Lim - Ryan Shim - Will Son - http://wiki.ros.org/dynamixel_workbench_toolbox - http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ - https://github.com/ROBOTIS-GIT/dynamixel-workbench - https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues - catkin - roscpp - dynamixel_sdk - diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/DynamixelWorkbench.h b/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/DynamixelWorkbench.h deleted file mode 100644 index bb90ec2..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/DynamixelWorkbench.h +++ /dev/null @@ -1,2 +0,0 @@ -#include "../include/dynamixel_workbench_toolbox/dynamixel_workbench.h" - diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp deleted file mode 100644 index 4e7ea19..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp +++ /dev/null @@ -1,1481 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby) */ - -#include "../../include/dynamixel_workbench_toolbox/dynamixel_driver.h" - -DynamixelDriver::DynamixelDriver() : tools_cnt_(0), - sync_write_handler_cnt_(0), - sync_read_handler_cnt_(0), - bulk_read_parameter_cnt_(0) -{ - -} - -DynamixelDriver::~DynamixelDriver() -{ - for (int i = 0; i < tools_cnt_; i++) - { - for (int j = 0; j < tools_[i].getDynamixelCount(); j++) - { - writeRegister(tools_[i].getID()[j], "Torque_Enable", (uint8_t)0); - } - } - - portHandler_->closePort(); -} - -void DynamixelDriver::initTools(void) -{ - for (uint8_t num = 0; num < MAX_DXL_SERIES_NUM; num++) - tools_[num].initTool(); - - tools_cnt_ = 0; -} - -bool DynamixelDriver::setTool(uint16_t model_number, uint8_t id, const char **log) -{ - bool result = false; - - // See if we have a matching tool? - for (uint8_t num = 0; num < tools_cnt_; num++) - { - if (tools_[num].getModelNumber() == model_number) - { - if (tools_[num].getDynamixelCount() < tools_[num].getDynamixelBuffer()) - { - // Found one with the right model number and it is not full - tools_[num].addDXL(id); - return true; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Too many Dynamixels are connected (default buffer size is 16, the same series of Dynamixels)"; - return false; - } - } - } - // We did not find one so lets allocate a new one - if (tools_cnt_ < MAX_DXL_SERIES_NUM) - { - // only do it if we still have some room... - result = tools_[tools_cnt_++].addTool(model_number, id, log); - return result; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Too many series are connected (MAX = 5 different series)"; - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Failed to set the Tool"; - return false; -} - -uint8_t DynamixelDriver::getTool(uint8_t id, const char **log) -{ - for (int i = 0; i < tools_cnt_; i++) - { - for (int j = 0; j < tools_[i].getDynamixelCount(); j++) - { - if (tools_[i].getID()[j] == id) - { - return i; - } - } - } - - if (log != NULL) *log = "[DynamixelDriver] Failed to get the Tool"; - return 0xff; -} - -bool DynamixelDriver::init(const char *device_name, uint32_t baud_rate, const char **log) -{ - bool result = false; - - result = setPortHandler(device_name, log); - if (result == false) return false; - - result = setBaudrate(baud_rate, log); - if (result == false) return false; - - result = setPacketHandler(2.0f, log); - if (result == false) return false; - - return result; -} - -bool DynamixelDriver::begin(const char *device_name, uint32_t baud_rate, const char **log) -{ - return init(device_name, baud_rate, log); -} - -bool DynamixelDriver::setPortHandler(const char *device_name, const char **log) -{ - portHandler_ = dynamixel::PortHandler::getPortHandler(device_name); - - if (portHandler_->openPort()) - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to open the port!"; - return true; - } - - if (log != NULL) *log = "[DynamixelDriver] Failed to open the port!"; - return false; -} - -bool DynamixelDriver::setBaudrate(uint32_t baud_rate, const char **log) -{ - if (portHandler_->setBaudRate((int)baud_rate)) - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to change the baudrate!"; - return true; - } - - if (log != NULL) *log = "[DynamixelDriver] Failed to change the baudrate!"; - return false; -} - -bool DynamixelDriver::setPacketHandler(float protocol_version, const char **log) -{ - packetHandler_ = dynamixel::PacketHandler::getPacketHandler(protocol_version); - - if (packetHandler_->getProtocolVersion() == protocol_version) - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to set the protocol!"; - return true; - } - - if (log != NULL) *log = "[DynamixelDriver] Failed to set the protocol!"; - return false; -} - -float DynamixelDriver::getProtocolVersion(void) -{ - return packetHandler_->getProtocolVersion(); -} - -uint32_t DynamixelDriver::getBaudrate(void) -{ - return portHandler_->getBaudRate(); -} - -const char* DynamixelDriver::getModelName(uint8_t id, const char **log) -{ - uint8_t factor = getTool(id, log); - if (factor == 0xff) - return NULL; - else - return tools_[factor].getModelName(); - - return NULL; -} - -uint16_t DynamixelDriver::getModelNumber(uint8_t id, const char **log) -{ - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - for (int i = 0; i < tools_[factor].getDynamixelCount(); i++) - { - if (tools_[factor].getID()[i] == id) - return tools_[factor].getModelNumber(); - } - - return false; -} - -const ControlItem* DynamixelDriver::getControlTable(uint8_t id, const char **log) -{ - uint8_t factor = getTool(id, log); - if (factor == 0xff) return NULL; - - return tools_[factor].getControlTable(); -} - -const ControlItem* DynamixelDriver::getItemInfo(uint8_t id, const char *item_name, const char **log) -{ - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return NULL; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return NULL; - else return control_item; - - return NULL; -} - -uint8_t DynamixelDriver::getTheNumberOfControlItem(uint8_t id, const char **log) -{ - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - return tools_[factor].getTheNumberOfControlItem(); -} - -const ModelInfo* DynamixelDriver::getModelInfo(uint8_t id, const char **log) -{ - uint8_t factor = getTool(id, log); - if (factor == 0xff) return NULL; - - return tools_[factor].getModelInfo(); -} - -uint8_t DynamixelDriver::getTheNumberOfSyncWriteHandler(void) -{ - return sync_write_handler_cnt_; -} - -uint8_t DynamixelDriver::getTheNumberOfSyncReadHandler(void) -{ - return sync_read_handler_cnt_; -} - -uint8_t DynamixelDriver::getTheNumberOfBulkReadParam(void) -{ - return bulk_read_parameter_cnt_; -} - -bool DynamixelDriver::scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8_t start_num, uint8_t end_num, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - bool result = false; - - uint8_t id = 0; - uint8_t id_cnt = 0; - - uint16_t model_number = 0; - - uint8_t get_end_num = end_num; - - if (get_end_num > 253) get_end_num = 253; - - initTools(); - - result = setPacketHandler(1.0f, log); - if (result == false) return false; - - for (id = start_num; id <= get_end_num; id++) - { - sdk_error.dxl_comm_result = packetHandler_->ping(portHandler_, id, &model_number, &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - } - else - { - if (sdk_error.dxl_error != 0) - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - - get_id[id_cnt++] = id; - setTool(model_number, id); - } - } - - if (id_cnt > 0) - { - *get_the_number_of_id = id_cnt; - return result; - } - - result = setPacketHandler(2.0f, log); - if (result == false) return false; - - for (id = start_num; id <= get_end_num; id++) - { - sdk_error.dxl_comm_result = packetHandler_->ping(portHandler_, id, &model_number, &sdk_error.dxl_error); - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - } - else - { - if (sdk_error.dxl_error != 0) - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - - get_id[id_cnt++] = id; - setTool(model_number, id); - } - } - - if (id_cnt > 0) - { - *get_the_number_of_id = id_cnt; - return result; - } - - return result; -} - -bool DynamixelDriver::scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8_t range, const char **log) -{ - return scan(get_id, get_the_number_of_id, 0, range, log); -} - -bool DynamixelDriver::ping(uint8_t id, uint16_t *get_model_number, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - bool result = false; - - uint16_t model_number = 0; - - result = setPacketHandler(1.0f, log); - if (result == false) return false; - - sdk_error.dxl_comm_result = packetHandler_->ping(portHandler_, id, &model_number, &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - } - else - { - if (sdk_error.dxl_error != 0) - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - - setTool(model_number, id); - if (get_model_number != NULL) *get_model_number = model_number; - return true; - } - - result = setPacketHandler(2.0f, log); - if (result == false) return false; - - sdk_error.dxl_comm_result = packetHandler_->ping(portHandler_, id, &model_number, &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - } - else - { - if (sdk_error.dxl_error != 0) - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - - setTool(model_number, id); - if (get_model_number != NULL) *get_model_number = model_number; - return true; - } - - return false; -} - -bool DynamixelDriver::ping(uint8_t id, const char **log) -{ - return ping(id, NULL, log); -} - -bool DynamixelDriver::clearMultiTurn(uint8_t id, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - sdk_error.dxl_comm_result = packetHandler_->clearMultiTurn(portHandler_, id, &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } - else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to clear!"; - } - return true; - } - - return false; -} - -bool DynamixelDriver::reboot(uint8_t id, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - if (getProtocolVersion() == 1.0) - { - if (log != NULL) *log = "[DynamixelDriver] reboot functions is not available with the Dynamixel Protocol 1.0."; - return false; - } - else - { - sdk_error.dxl_comm_result = packetHandler_->reboot(portHandler_, id, &sdk_error.dxl_error); -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(1000); -#else - usleep(1000*1000); -#endif - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } - else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to reboot!"; - } - return true; - } - } - - return false; -} - -bool DynamixelDriver::reset(uint8_t id, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - bool result = false; - - uint32_t new_baud_rate = 0; - uint8_t new_id = 1; - - const char* model_name = getModelName(id, log); - if (model_name == NULL) return false; - - uint16_t model_number = getModelNumber(id, log); - if (model_number == 0) return false; - - if (getProtocolVersion() == 1.0) - { - sdk_error.dxl_comm_result = packetHandler_->factoryReset(portHandler_, id, 0x00, &sdk_error.dxl_error); -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(2000); -#else - usleep(1000*2000); -#endif - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (!strncmp(model_name, "AX", strlen("AX")) || - !strncmp(model_name, "MX-12W", strlen("MX-12W"))) - new_baud_rate = 1000000; - else - new_baud_rate = 57600; - - result = setBaudrate(new_baud_rate, log); - if (result == false) - return false; - else - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XL", strlen("XL")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "PRO", strlen("PRO"))|| - !strncmp(model_name, "RH", strlen("RH"))) - { - result = setPacketHandler(2.0f, log); - if (result == false) return false; - } - else - { - result = setPacketHandler(1.0f, log); - if (result == false) return false; - } - } - } - - initTools(); - result = setTool(model_number, new_id, log); - if (result == false) return false; - - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to reset!"; - } - - return true; - } - else if (getProtocolVersion() == 2.0) - { - sdk_error.dxl_comm_result = packetHandler_->factoryReset(portHandler_, id, 0xff, &sdk_error.dxl_error); -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(2000); -#else - usleep(1000*2000); -#endif - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (!strncmp(model_name, "XL-320", strlen("XL-320"))) - new_baud_rate = 1000000; - else - new_baud_rate = 57600; - - result = setBaudrate(new_baud_rate, log); - if (result == false) - return false; - else - { - result = setPacketHandler(2.0f, log); - if (result == false) return false; - } - } - - initTools(); - result = setTool(model_number, new_id, log); - if (result == false) return false; - - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to reset!"; - } - - return true; - } - - return result; -} - -bool DynamixelDriver::writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t* data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(10); -#else - usleep(1000*10); -#endif - - sdk_error.dxl_comm_result = packetHandler_->writeTxRx(portHandler_, - id, - address, - length, - data, - &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else if (sdk_error.dxl_error != 0) - { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - return false; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to write!"; - return true; - } - - return false; -} - -bool DynamixelDriver::writeRegister(uint8_t id, const char *item_name, int32_t data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - uint8_t data_1_byte = (uint8_t)data; - uint16_t data_2_byte = (uint16_t)data; - uint32_t data_4_byte = (uint32_t)data; - -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(10); -#else - usleep(1000*10); -#endif - - switch (control_item->data_length) - { - case BYTE: - sdk_error.dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, - id, - control_item->address, - data_1_byte, - &sdk_error.dxl_error); - break; - - case WORD: - sdk_error.dxl_comm_result = packetHandler_->write2ByteTxRx(portHandler_, - id, - control_item->address, - data_2_byte, - &sdk_error.dxl_error); - break; - - case DWORD: - sdk_error.dxl_comm_result = packetHandler_->write4ByteTxRx(portHandler_, - id, - control_item->address, - data_4_byte, - &sdk_error.dxl_error); - break; - - default: - sdk_error.dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, - id, - control_item->address, - data_1_byte, - &sdk_error.dxl_error); - break; - } - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else if (sdk_error.dxl_error != 0) - { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - return false; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to write!"; - return true; - } - - return false; -} - -bool DynamixelDriver::writeOnlyRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(10); -#else - usleep(1000*10); -#endif - - sdk_error.dxl_comm_result = packetHandler_->writeTxOnly(portHandler_, - id, - address, - length, - data); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to write!"; - return true; - } - - return false; -} - -bool DynamixelDriver::writeOnlyRegister(uint8_t id, const char *item_name, int32_t data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(10); -#else - usleep(1000*10); -#endif - - switch (control_item->data_length) - { - case BYTE: - sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_, - id, - control_item->address, - (uint8_t)data); - break; - - case WORD: - sdk_error.dxl_comm_result = packetHandler_->write2ByteTxOnly(portHandler_, - id, - control_item->address, - (uint16_t)data); - break; - - case DWORD: - sdk_error.dxl_comm_result = packetHandler_->write4ByteTxOnly(portHandler_, - id, - control_item->address, - (uint32_t)data); - break; - - default: - sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_, - id, - control_item->address, - (uint8_t)data); - break; - } - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to write!"; - return true; - } - - return false; -} - -bool DynamixelDriver::readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - uint8_t data_read[length]; - - sdk_error.dxl_comm_result = packetHandler_->readTxRx(portHandler_, - id, - address, - length, - (uint8_t *)&data_read, - &sdk_error.dxl_error); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - switch (length) - { - case BYTE: - *data = data_read[0]; - break; - - case WORD: - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - break; - - case DWORD: - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - break; - - default: - for (uint16_t index = 0; index < length; index++) - { - data[index] = (uint32_t)data_read[index]; - } - break; - } - - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to read!"; - } - - return true; - } - - return false; -} - -bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - uint8_t data_1_byte = 0; - uint16_t data_2_byte = 0; - uint32_t data_4_byte = 0; - - switch (control_item->data_length) - { - case BYTE: - sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_, - id, - control_item->address, - &data_1_byte, - &sdk_error.dxl_error); - break; - - case WORD: - sdk_error.dxl_comm_result = packetHandler_->read2ByteTxRx(portHandler_, - id, - control_item->address, - &data_2_byte, - &sdk_error.dxl_error); - break; - - case DWORD: - sdk_error.dxl_comm_result = packetHandler_->read4ByteTxRx(portHandler_, - id, - control_item->address, - &data_4_byte, - &sdk_error.dxl_error); - break; - - default: - sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_, - id, - control_item->address, - &data_1_byte, - &sdk_error.dxl_error); - break; - } - - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - else - { - switch (control_item->data_length) - { - case BYTE: - *data = data_1_byte; - break; - - case WORD: - *data = data_2_byte; - break; - - case DWORD: - *data = data_4_byte; - break; - - default: - *data = data_1_byte; - break; - } - - if (sdk_error.dxl_error != 0) { - if (log != NULL) *log = packetHandler_->getRxPacketError(sdk_error.dxl_error); - } else { - if (log != NULL) *log = "[DynamixelDriver] Succeeded to read!"; - } - - return true; - } - - return false; -} - -void DynamixelDriver::getParam(int32_t data, uint8_t *param) -{ - param[0] = DXL_LOBYTE(DXL_LOWORD(data)); - param[1] = DXL_HIBYTE(DXL_LOWORD(data)); - param[2] = DXL_LOBYTE(DXL_HIWORD(data)); - param[3] = DXL_HIBYTE(DXL_HIWORD(data)); -} - -bool DynamixelDriver::addSyncWriteHandler(uint16_t address, uint16_t length, const char **log) -{ - if (sync_write_handler_cnt_ > (MAX_HANDLER_NUM-1)) - { - if (log != NULL) *log = "[DynamixelDriver] Too many sync write handler are added (MAX = 5)"; - return false; - } - - syncWriteHandler_[sync_write_handler_cnt_].control_item = NULL; - - syncWriteHandler_[sync_write_handler_cnt_].groupSyncWrite = new dynamixel::GroupSyncWrite(portHandler_, - packetHandler_, - address, - length); - - sync_write_handler_cnt_++; - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add sync write handler"; - return true; -} - -bool DynamixelDriver::addSyncWriteHandler(uint8_t id, const char *item_name, const char **log) -{ - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - if (sync_write_handler_cnt_ > (MAX_HANDLER_NUM-1)) - { - if (log != NULL) *log = "[DynamixelDriver] Too many sync write handler are added (MAX = 5)"; - return false; - } - - syncWriteHandler_[sync_write_handler_cnt_].control_item = control_item; - - syncWriteHandler_[sync_write_handler_cnt_].groupSyncWrite = new dynamixel::GroupSyncWrite(portHandler_, - packetHandler_, - control_item->address, - control_item->data_length); - - sync_write_handler_cnt_++; - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add sync write handler"; - return true; -} - -bool DynamixelDriver::syncWrite(uint8_t index, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - uint8_t dxl_cnt = 0; - uint8_t parameter[4] = {0, 0, 0, 0}; - - for (int i = 0; i < tools_cnt_; i++) - { - for (int j = 0; j < tools_[i].getDynamixelCount(); j++) - { - getParam(data[dxl_cnt], parameter); - sdk_error.dxl_addparam_result = syncWriteHandler_[index].groupSyncWrite->addParam(tools_[i].getID()[j], (uint8_t *)¶meter); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupSyncWrite addparam failed"; - return false; - } - else - dxl_cnt++; - } - } - - sdk_error.dxl_comm_result = syncWriteHandler_[index].groupSyncWrite->txPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - syncWriteHandler_[index].groupSyncWrite->clearParam(); - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to sync write!"; - return true; -} - -bool DynamixelDriver::syncWrite(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, uint8_t data_num_for_each_id, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - uint8_t parameter[4] = {0, 0, 0, 0}; - uint8_t multi_parameter[4*data_num_for_each_id]; - uint8_t cnt = 0; - - for (int i = 0; i < id_num; i++) - { - for (int j = 0; j < data_num_for_each_id; j++) - { - getParam(data[cnt++], parameter); - for (int k = 0; k < 4; k++) - { - multi_parameter[4*j+k] = parameter[k]; - } - } - - sdk_error.dxl_addparam_result = syncWriteHandler_[index].groupSyncWrite->addParam(id[i], (uint8_t *)&multi_parameter); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupSyncWrite addparam failed"; - return false; - } - } - - sdk_error.dxl_comm_result = syncWriteHandler_[index].groupSyncWrite->txPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - syncWriteHandler_[index].groupSyncWrite->clearParam(); - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to sync write!"; - return true; -} - -bool DynamixelDriver::addSyncReadHandler(uint16_t address, uint16_t length, const char **log) -{ - if (sync_read_handler_cnt_ > (MAX_HANDLER_NUM-1)) - { - if (log != NULL) *log = "[DynamixelDriver] Too many sync read handler are added (MAX = 5)"; - return false; - } - - syncReadHandler_[sync_read_handler_cnt_].control_item = NULL; - - syncReadHandler_[sync_read_handler_cnt_].groupSyncRead = new dynamixel::GroupSyncRead(portHandler_, - packetHandler_, - address, - length); - - sync_read_handler_cnt_++; - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add sync read handler"; - return true; -} - -bool DynamixelDriver::addSyncReadHandler(uint8_t id, const char *item_name, const char **log) -{ - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - if (sync_read_handler_cnt_ > (MAX_HANDLER_NUM-1)) - { - if (log != NULL) *log = "[DynamixelDriver] Too many sync read handler are added (MAX = 5)"; - return false; - } - - syncReadHandler_[sync_read_handler_cnt_].control_item = control_item; - - syncReadHandler_[sync_read_handler_cnt_].groupSyncRead = new dynamixel::GroupSyncRead(portHandler_, - packetHandler_, - control_item->address, - control_item->data_length); - - sync_read_handler_cnt_++; - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add sync read handler"; - return true; -} - -bool DynamixelDriver::syncRead(uint8_t index, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - syncReadHandler_[index].groupSyncRead->clearParam(); - for (int i = 0; i < tools_cnt_; i++) - { - for (int j = 0; j < tools_[i].getDynamixelCount(); j++) - { - sdk_error.dxl_addparam_result = syncReadHandler_[index].groupSyncRead->addParam(tools_[i].getID()[j]); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupSyncWrite addparam failed"; - return false; - } - } - } - - sdk_error.dxl_comm_result = syncReadHandler_[index].groupSyncRead->txRxPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to sync read!"; - return true; -} - -bool DynamixelDriver::syncRead(uint8_t index, uint8_t *id, uint8_t id_num, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - syncReadHandler_[index].groupSyncRead->clearParam(); - for (int i = 0; i < id_num; i++) - { - sdk_error.dxl_addparam_result = syncReadHandler_[index].groupSyncRead->addParam(id[i]); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupSyncWrite addparam failed"; - return false; - } - } - - sdk_error.dxl_comm_result = syncReadHandler_[index].groupSyncRead->txRxPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to sync read!"; - return true; -} - -bool DynamixelDriver::getSyncReadData(uint8_t index, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - for (int i = 0; i < tools_cnt_; i++) - { - for (int j = 0; j < tools_[i].getDynamixelCount(); j++) - { - sdk_error.dxl_getdata_result = syncReadHandler_[index].groupSyncRead->isAvailable(tools_[i].getID()[j], - syncReadHandler_[index].control_item->address, - syncReadHandler_[index].control_item->data_length); - if (sdk_error.dxl_getdata_result != true) - { - if (log != NULL) *log = "groupSyncRead getdata failed"; - return false; - } - else - { - data[i+j] = syncReadHandler_[index].groupSyncRead->getData(tools_[i].getID()[j], - syncReadHandler_[index].control_item->address, - syncReadHandler_[index].control_item->data_length); - } - } - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to get sync read data!"; - return true; -} - -bool DynamixelDriver::getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - for (int i = 0; i < id_num; i++) - { - sdk_error.dxl_getdata_result = syncReadHandler_[index].groupSyncRead->isAvailable(id[i], - syncReadHandler_[index].control_item->address, - syncReadHandler_[index].control_item->data_length); - if (sdk_error.dxl_getdata_result != true) - { - if (log != NULL) *log = "groupSyncRead getdata failed"; - return false; - } - else - { - data[i] = syncReadHandler_[index].groupSyncRead->getData(id[i], - syncReadHandler_[index].control_item->address, - syncReadHandler_[index].control_item->data_length); - } - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to get sync read data!"; - return true; -} - -bool DynamixelDriver::getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, uint16_t address, uint16_t length, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - for (int i = 0; i < id_num; i++) - { - sdk_error.dxl_getdata_result = syncReadHandler_[index].groupSyncRead->isAvailable(id[i], - address, - length); - if (sdk_error.dxl_getdata_result != true) - { - if (log != NULL) *log = "groupSyncRead getdata failed"; - return false; - } - else - { - data[i] = syncReadHandler_[index].groupSyncRead->getData(id[i], - address, - length); - } - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to get sync read data!"; - return true; -} - -bool DynamixelDriver::initBulkWrite(const char **log) -{ - if (portHandler_ == NULL) - { - if (log != NULL) *log = "[DynamixelDriver] Failed to load portHandler!"; - } - else if (packetHandler_ == NULL) - { - if (log != NULL) *log = "[DynamixelDriver] Failed to load packetHandler!"; - } - else - { - groupBulkWrite_ = new dynamixel::GroupBulkWrite(portHandler_, packetHandler_); - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to init groupBulkWrite!"; - return true; - } - - return false; -} - -bool DynamixelDriver::addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - uint8_t parameter[4] = {0, 0, 0, 0}; - - getParam(data, parameter); - sdk_error.dxl_addparam_result = groupBulkWrite_->addParam(id, - address, - length, - parameter); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupBulkWrite addparam failed"; - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add param for bulk write!"; - return true; -} - -bool DynamixelDriver::addBulkWriteParam(uint8_t id, const char *item_name, int32_t data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - uint8_t parameter[4] = {0, 0, 0, 0}; - - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - getParam(data, parameter); - sdk_error.dxl_addparam_result = groupBulkWrite_->addParam(id, - control_item->address, - control_item->data_length, - parameter); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "groupBulkWrite addparam failed"; - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add param for bulk write!"; - return true; -} - -bool DynamixelDriver::bulkWrite(const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - sdk_error.dxl_comm_result = groupBulkWrite_->txPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - groupBulkWrite_->clearParam(); - if (log != NULL) *log = "[DynamixelDriver] Succeeded to bulk write!"; - - return true; -} - -bool DynamixelDriver::initBulkRead(const char **log) -{ - if (portHandler_ == NULL) - { - if (log != NULL) *log = "[DynamixelDriver] Failed to load portHandler!"; - } - else if (packetHandler_ == NULL) - { - if (log != NULL) *log = "[DynamixelDriver] Failed to load packetHandler!"; - } - else - { - groupBulkRead_ = new dynamixel::GroupBulkRead(portHandler_, packetHandler_); - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to init groupBulkRead!"; - - return true; - } - - return false; -} - -bool DynamixelDriver::addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - sdk_error.dxl_addparam_result = groupBulkRead_->addParam(id, - address, - length); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "grouBulkRead addparam failed"; - return false; - } - - if (bulk_read_parameter_cnt_ < (MAX_BULK_PARAMETER-1)) - { - bulk_read_param_[bulk_read_parameter_cnt_].id = id; - bulk_read_param_[bulk_read_parameter_cnt_].address = address; - bulk_read_param_[bulk_read_parameter_cnt_].data_length = length; - bulk_read_parameter_cnt_++; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Too many bulk parameter are added (default buffer size is 10)"; - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add param for bulk read!"; - return true; -} - -bool DynamixelDriver::addBulkReadParam(uint8_t id, const char *item_name, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - const ControlItem *control_item; - - uint8_t factor = getTool(id, log); - if (factor == 0xff) return false; - - control_item = tools_[factor].getControlItem(item_name, log); - if (control_item == NULL) return false; - - sdk_error.dxl_addparam_result = groupBulkRead_->addParam(id, - control_item->address, - control_item->data_length); - if (sdk_error.dxl_addparam_result != true) - { - if (log != NULL) *log = "grouBulkRead addparam failed"; - return false; - } - - if (bulk_read_parameter_cnt_ < (MAX_BULK_PARAMETER-1)) - { - bulk_read_param_[bulk_read_parameter_cnt_].id = id; - bulk_read_param_[bulk_read_parameter_cnt_].address = control_item->address; - bulk_read_param_[bulk_read_parameter_cnt_].data_length = control_item->data_length; - bulk_read_parameter_cnt_++; - } - else - { - if (log != NULL) *log = "[DynamixelDriver] Too many bulk parameter are added (default buffer size is 10)"; - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to add param for bulk read!"; - return true; -} - -bool DynamixelDriver::bulkRead(const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - sdk_error.dxl_comm_result = groupBulkRead_->txRxPacket(); - if (sdk_error.dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(sdk_error.dxl_comm_result); - return false; - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to bulk read!"; - return true; -} - -bool DynamixelDriver::getBulkReadData(int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - for (int i = 0; i < bulk_read_parameter_cnt_; i++) - { - sdk_error.dxl_getdata_result = groupBulkRead_->isAvailable(bulk_read_param_[i].id, - bulk_read_param_[i].address, - bulk_read_param_[i].data_length); - if (sdk_error.dxl_getdata_result != true) - { - if (log != NULL) *log = "groupBulkRead getdata failed"; - return false; - } - else - { - data[i] = groupBulkRead_->getData(bulk_read_param_[i].id, - bulk_read_param_[i].address, - bulk_read_param_[i].data_length); - } - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to get bulk read data!"; - return true; -} - -bool DynamixelDriver::getBulkReadData(uint8_t *id, uint8_t id_num, uint16_t *address, uint16_t *length, int32_t *data, const char **log) -{ - ErrorFromSDK sdk_error = {0, false, false, 0}; - - for (int i = 0; i < id_num; i++) - { - sdk_error.dxl_getdata_result = groupBulkRead_->isAvailable(id[i], - address[i], - length[i]); - if (sdk_error.dxl_getdata_result != true) - { - if (log != NULL) *log = "groupBulkRead getdata failed"; - return false; - } - else - { - data[i] = groupBulkRead_->getData(id[i], - address[i], - length[i]); - } - } - - if (log != NULL) *log = "[DynamixelDriver] Succeeded to get bulk read data!"; - return true; -} - -bool DynamixelDriver::clearBulkReadParam(void) -{ - groupBulkRead_->clearParam(); - bulk_read_parameter_cnt_ = 0; - - return true; -} diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_item.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_item.cpp deleted file mode 100644 index 1307e70..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_item.cpp +++ /dev/null @@ -1,1602 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby), Ryan Shim */ - -#include "../../include/dynamixel_workbench_toolbox/dynamixel_item.h" - -//========================================================= -// Servo register definitions -//========================================================= - -//_________________________________________________________ - -static const char s_Acceleration_Limit[] = "Acceleration_Limit"; -static const char s_Alarm_LED[] = "Alarm_LED"; -static const char s_Baud_Rate[] = "Baud_Rate"; -static const char s_Bus_Watchdog[] = "Bus_Watchdog"; -static const char s_CCW_Angle_Limit[] = "CCW_Angle_Limit"; -static const char s_CCW_Compliance_Margin[] = "CCW_Compliance_Margin"; -static const char s_CCW_Compliance_Slope[] = "CCW_Compliance_Slope"; -static const char s_Control_Mode[] = "Control_Mode"; -static const char s_Current[] = "Current"; -static const char s_Current_Limit[] = "Current_Limit"; -static const char s_CW_Angle_Limit[] = "CW_Angle_Limit"; -static const char s_CW_Compliance_Margin[] = "CW_Compliance_Margin"; -static const char s_CW_Compliance_Slope[] = "CW_Compliance_Slope"; -static const char s_D_gain[] = "D_gain"; -static const char s_Drive_Mode[] = "Drive_Mode"; -static const char s_External_Port_Mode_1[] = "External_Port_Mode_1"; -static const char s_External_Port_Mode_2[] = "External_Port_Mode_2"; -static const char s_External_Port_Mode_3[] = "External_Port_Mode_3"; -static const char s_External_Port_Mode_4[] = "External_Port_Mode_4"; -static const char s_External_Port_Data_1[] = "External_Port_Data_1"; -static const char s_External_Port_Data_2[] = "External_Port_Data_2"; -static const char s_External_Port_Data_3[] = "External_Port_Data_3"; -static const char s_External_Port_Data_4[] = "External_Port_Data_4"; -static const char s_Feedforward_1st_Gain[] = "Feedforward_1st_Gain"; -static const char s_Feedforward_2nd_Gain[] = "Feedforward_2nd_Gain"; -static const char s_Firmware_Version[] = "Firmware_Version"; -static const char s_Goal_Acceleration[] = "Goal_Acceleration"; -static const char s_Goal_Current[] = "Goal_Current"; -static const char s_Goal_Position[] = "Goal_Position"; -static const char s_Goal_PWM[] = "Goal_PWM"; -static const char s_Goal_Torque[] = "Goal_Torque"; -static const char s_Goal_Velocity[] = "Goal_Velocity"; -static const char s_Hardware_Error_Status[] = "Hardware_Error_Status"; -static const char s_Homing_Offset[] = "Homing_Offset"; -static const char s_I_gain[] = "I_gain"; -static const char s_ID[] = "ID"; -static const char s_LED[] = "LED"; -static const char s_LED_BLUE[] = "LED_BLUE"; -static const char s_LED_GREEN[] = "LED_GREEN"; -static const char s_LED_RED[] = "LED_RED"; -static const char s_Lock[] = "Lock"; -static const char s_Max_Position_Limit[] = "Max_Position_Limit"; -static const char s_Max_Torque[] = "Max_Torque"; -static const char s_Max_Voltage_Limit[] = "Max_Voltage_Limit"; -static const char s_Min_Position_Limit[] = "Min_Position_Limit"; -static const char s_Min_Voltage_Limit[] = "Min_Voltage_Limit"; -static const char s_Model_Number[] = "Model_Number"; -static const char s_Moving[] = "Moving"; -static const char s_Moving_Speed[] = "Moving_Speed"; -static const char s_Moving_Status[] = "Moving_Status"; -static const char s_Moving_Threshold[] = "Moving_Threshold"; -static const char s_Multi_Turn_Offset[] = "Multi_Turn_Offset"; -static const char s_Operating_Mode[] = "Operating_Mode"; -static const char s_P_gain[] = "P_gain"; -static const char s_Position_D_Gain[] = "Position_D_Gain"; -static const char s_Position_I_Gain[] = "Position_I_Gain"; -static const char s_Position_P_Gain[] = "Position_P_Gain"; -static const char s_Position_Trajectory[] = "Position_Trajectory"; -static const char s_Present_Current[] = "Present_Current"; -static const char s_Present_Input[] = "Present_Input"; -static const char s_Present_Input_Voltage[] = "Present_Input_Voltage"; -static const char s_Present_Load[] = "Present_Load"; -static const char s_Present_Position[] = "Present_Position"; -static const char s_Present_PWM[] = "Present_PWM"; -static const char s_Present_Speed[] = "Present_Speed"; -static const char s_Present_Temperature[] = "Present_Temperature"; -static const char s_Present_Velocity[] = "Present_Velocity"; -static const char s_Present_Voltage[] = "Present_Voltage"; -static const char s_Profile_Acceleration[] = "Profile_Acceleration"; -static const char s_Profile_Velocity[] = "Profile_Velocity"; -static const char s_Protocol_Version[] = "Protocol_Version"; -static const char s_Punch[] = "Punch"; -static const char s_PWM_Limit[] = "PWM_Limit"; -static const char s_Realtime_Tick[] = "Realtime_Tick"; -static const char s_Registered[] = "Registered"; -static const char s_Registered_Instruction[] = "Registered_Instruction"; -static const char s_Resolution_Divider[] = "Resolution_Divider"; -static const char s_Return_Delay_Time[] = "Return_Delay_Time"; -static const char s_Secondary_ID[] = "Secondary_ID"; -static const char s_Sensored_Current[] = "Sensored_Current"; -static const char s_Shutdown[] = "Shutdown"; -static const char s_Status_Return_Level[] = "Status_Return_Level"; -static const char s_Temperature_Limit[] = "Temperature_Limit"; -static const char s_Torque_Control_Mode_Enable[] = "Torque_Control_Mode_Enable"; -static const char s_Torque_Enable[] = "Torque_Enable"; -static const char s_Torque_Limit[] = "Torque_Limit"; -static const char s_Velocity_I_Gain[] = "Velocity_I_Gain"; -static const char s_Velocity_Limit[] = "Velocity_Limit"; -static const char s_Velocity_P_Gain[] = "Velocity_P_Gain"; -static const char s_Velocity_Trajectory[] = "Velocity_Trajectory"; - -//_________________________________________________________ - -//--------------------------------------------------------- -// AX servos - (num == AX_12A || num == AX_12W || num == AX_18A) -//--------------------------------------------------------- -static const ControlItem items_AX[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1}, - {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1}, - {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1}, - {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 44, sizeof(s_Registered) - 1, 1}, - {s_Moving, 46, sizeof(s_Moving) - 1, 1}, - {s_Lock, 47, sizeof(s_Lock) - 1, 1}, - {s_Punch, 48, sizeof(s_Punch) - 1, 2}}; -#define COUNT_AX_ITEMS (sizeof(items_AX) / sizeof(items_AX[0])) - -static const ModelInfo info_AX = {0.11, - 0, - 512, - 1024, - -2.61799, - 2.61799}; - -//--------------------------------------------------------- -// RX servos - (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64) -//--------------------------------------------------------- -static const ControlItem items_RX[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1}, - {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1}, - {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1}, - {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 44, sizeof(s_Registered) - 1, 1}, - {s_Moving, 46, sizeof(s_Moving) - 1, 1}, - {s_Lock, 47, sizeof(s_Lock) - 1, 1}, - {s_Punch, 48, sizeof(s_Punch) - 1, 2}}; - -#define COUNT_RX_ITEMS (sizeof(items_RX) / sizeof(items_RX[0])) - -static const ModelInfo info_RX = {0.11, - 0, - 512, - 1024, - -2.61799, - 2.61799}; - -//--------------------------------------------------------- -// EX servos - (num == EX_106) -//--------------------------------------------------------- -static const ControlItem items_EX[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1}, - {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1}, - {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1}, - {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 34, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 35, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 44, sizeof(s_Registered) - 1, 1}, - {s_Moving, 46, sizeof(s_Moving) - 1, 1}, - {s_Lock, 47, sizeof(s_Lock) - 1, 1}, - {s_Punch, 48, sizeof(s_Punch) - 1, 2}, - {s_Sensored_Current, 56, sizeof(s_Sensored_Current) - 1, 2}}; - -#define COUNT_EX_ITEMS (sizeof(items_EX) / sizeof(items_EX[0])) - -static const ModelInfo info_EX = {0.11, - 0, - 2048, - 4096, - -2.18969008, - 2.18969008}; - -//--------------------------------------------------------- -// MX Protocol 1 servos - (num == MX_12W || num == MX_28) -//--------------------------------------------------------- -static const ControlItem items_MX[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - {s_Multi_Turn_Offset, 20, sizeof(s_Multi_Turn_Offset) - 1, 2}, - {s_Resolution_Divider, 22, sizeof(s_Resolution_Divider) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_D_gain, 26, sizeof(s_D_gain) - 1, 1}, - {s_I_gain, 27, sizeof(s_I_gain) - 1, 1}, - {s_P_gain, 28, sizeof(s_P_gain) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 44, sizeof(s_Registered) - 1, 1}, - {s_Moving, 46, sizeof(s_Moving) - 1, 1}, - {s_Lock, 47, sizeof(s_Lock) - 1, 1}, - {s_Punch, 48, sizeof(s_Punch) - 1, 2}, - {s_Goal_Acceleration, 73, sizeof(s_Goal_Acceleration) - 1, 1}}; - -#define COUNT_MX_ITEMS (sizeof(items_MX) / sizeof(items_MX[0])) - -static const ModelInfo info_MX = {0.11, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// MX Protocol 2 servos - (num == MX_28_2) -//--------------------------------------------------------- -static const ControlItem items_MX2[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Load, 126, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_MX2_ITEMS (sizeof(items_MX2) / sizeof(items_MX2[0])) - -static const ModelInfo info_MX2 = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// EXT MX Protocol 1 servos - (num == MX_64 || num == MX_106) -//--------------------------------------------------------- -static const ControlItem items_EXTMX[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - {s_Multi_Turn_Offset, 20, sizeof(s_Multi_Turn_Offset) - 1, 2}, - {s_Resolution_Divider, 22, sizeof(s_Resolution_Divider) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_D_gain, 26, sizeof(s_D_gain) - 1, 1}, - {s_I_gain, 27, sizeof(s_I_gain) - 1, 1}, - {s_P_gain, 28, sizeof(s_P_gain) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 44, sizeof(s_Registered) - 1, 1}, - {s_Moving, 46, sizeof(s_Moving) - 1, 1}, - {s_Lock, 47, sizeof(s_Lock) - 1, 1}, - {s_Punch, 48, sizeof(s_Punch) - 1, 2}, - {s_Current, 68, sizeof(s_Current) - 1, 2}, - {s_Torque_Control_Mode_Enable, 70, sizeof(s_Torque_Control_Mode_Enable) - 1, 1}, - {s_Goal_Torque, 71, sizeof(s_Goal_Torque) - 1, 2}, - {s_Goal_Acceleration, 73, sizeof(s_Goal_Acceleration) - 1, 1}}; - -#define COUNT_EXTMX_ITEMS (sizeof(items_EXTMX) / sizeof(items_EXTMX[0])) - -static const ModelInfo info_EXTMX = {0.11, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// EXT MX Protocol 2 Servos - (num == MX_64_2 || num == MX_106_2) -//--------------------------------------------------------- -static const ControlItem items_EXTMX2[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_EXTMX2_ITEMS (sizeof(items_EXTMX2) / sizeof(items_EXTMX2[0])) - -static const ModelInfo info_EXTMX2 = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// XL320 - (num == XL_320) -//--------------------------------------------------------- -static const ControlItem items_XL320[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 3, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2}, - {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2}, - {s_Control_Mode, 11, sizeof(s_Control_Mode) - 1, 1}, - {s_Temperature_Limit, 12, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Min_Voltage_Limit, 13, sizeof(s_Min_Voltage_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 14, sizeof(s_Max_Voltage_Limit) - 1, 1}, - {s_Max_Torque, 15, sizeof(s_Max_Torque) - 1, 2}, - {s_Status_Return_Level, 17, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 25, sizeof(s_LED) - 1, 1}, - {s_D_gain, 27, sizeof(s_D_gain) - 1, 1}, - {s_I_gain, 28, sizeof(s_I_gain) - 1, 1}, - {s_P_gain, 29, sizeof(s_P_gain) - 1, 1}, - {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2}, - {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2}, - {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2}, - {s_Present_Position, 37, sizeof(s_Present_Position) - 1, 2}, - {s_Present_Speed, 39, sizeof(s_Present_Speed) - 1, 2}, - {s_Present_Load, 41, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Voltage, 45, sizeof(s_Present_Voltage) - 1, 1}, - {s_Present_Temperature, 46, sizeof(s_Present_Temperature) - 1, 1}, - {s_Registered, 47, sizeof(s_Registered) - 1, 1}, - {s_Moving, 49, sizeof(s_Moving) - 1, 1}, - {s_Hardware_Error_Status, 50, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Punch, 51, sizeof(s_Punch) - 1, 2}}; - -#define COUNT_XL320_ITEMS (sizeof(items_XL320) / sizeof(items_XL320[0])) - -static const ModelInfo info_XL320 = {0.11, - 0, - 512, - 1024, - -2.61799, - 2.61799}; - -//--------------------------------------------------------- -// XL - (num == XL430_W250, XL430_W250_2, XC430_W150, XC430_W240) -//--------------------------------------------------------- -static const ControlItem items_XL[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Load, 126, sizeof(s_Present_Load) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_XL_ITEMS (sizeof(items_XL) / sizeof(items_XL[0])) - -static const ModelInfo info_XL = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// XM - (num == XM430_W210 || num == XM430_W350) -//--------------------------------------------------------- -static const ControlItem items_XM[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_XM_ITEMS (sizeof(items_XM) / sizeof(items_XM[0])) - -static const ModelInfo info_XM = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// EXTXM - (num == XM540_W150 || num == XM540_W270) -//--------------------------------------------------------- -static const ControlItem items_EXTXM[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 152, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 154, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 156, sizeof(s_External_Port_Data_3) - 1, 2}}; - -#define COUNT_EXTXM_ITEMS (sizeof(items_EXTXM) / sizeof(items_EXTXM[0])) - -static const ModelInfo info_EXTXM = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// XH - (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350) -//--------------------------------------------------------- -static const ControlItem items_XH[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_XH_ITEMS (sizeof(items_XH) / sizeof(items_XH[0])) - -static const ModelInfo info_XH = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// EXTXH - (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_W270) -//--------------------------------------------------------- -static const ControlItem items_EXTXH[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED, 65, sizeof(s_LED) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_EXTXH_ITEMS (sizeof(items_EXTXH) / sizeof(items_EXTXH[0])) - -static const ModelInfo info_EXTXH = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// XW - (num == XW540_T260 || XW540_T140) -//--------------------------------------------------------- -static const ControlItem items_XW[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1}, - {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 69, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 122, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1}, - {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}}; - -#define COUNT_XW_ITEMS (sizeof(items_XW) / sizeof(items_XW[0])) - -static const ModelInfo info_XW = {0.229, - 0, - 2048, - 4096, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// PRO - (num == PRO_L42_10_S300_R) -//--------------------------------------------------------- -static const ControlItem items_PRO[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2}, - {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1}, - {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4}, - {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2}, - {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4}, - {s_Moving, 610, sizeof(s_Moving) - 1, 1}, - {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4}, - {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2}, - {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}}; - -#define COUNT_PRO_ITEMS (sizeof(items_PRO) / sizeof(items_PRO[0])) - -static const ModelInfo info_PRO = {0.114, - -2048, - 0, - 2048, - -3.14159265, - 3.14159265}; - -//--------------------------------------------------------- -// EXT PRO - All Other Pros... -//--------------------------------------------------------- -static const ControlItem items_EXTPRO[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Homing_Offset, 13, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2}, - {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1}, - {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4}, - {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2}, - {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4}, - {s_Moving, 610, sizeof(s_Moving) - 1, 1}, - {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4}, - {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2}, - {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}}; - -#define COUNT_EXTPRO_ITEMS (sizeof(items_EXTPRO) / sizeof(items_EXTPRO[0])) - -static const ModelInfo info_EXTPRO[] = { - {0.00249657, -144197, 0, 144197, -3.14159265, 3.14159265}, // PRO_L54_30_S400_R - {0.00199234, -180692, 0, 180692, -3.14159265, 3.14159265}, // PRO_L54_30_S500_R, PRO_L54_50_S500_R - {0.00346667, -103846, 0, 103846, -3.14159265, 3.14159265}, // PRO_L54_50_S290_R - {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R - {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R, PRO_M54_60_S250_R - {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R - {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R, PRO_H54_200_S500_R - -//--------------------------------------------------------- -// EXT PRO (A Firmware_Version) -//--------------------------------------------------------- -static const ControlItem items_EXTPRO_A[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1}, - {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4}, - {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Goal_Current, 604, sizeof(s_Goal_Current) - 1, 2}, - {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Moving, 570, sizeof(s_Moving) - 1, 1}, - {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4}, - {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}}; - -#define COUNT_EXTPRO_A_ITEMS (sizeof(items_EXTPRO_A) / sizeof(items_EXTPRO_A[0])) - -static const ModelInfo info_EXTPRO_A[] = { - {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R_A - {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R_A, PRO_M54_60_S250_R_A - {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R_A - {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R_A, PRO_H54_200_S500_R_A - -//--------------------------------------------------------- -// PRO PLUS - (num == PRO_H42P_020_S300_R, PRO_H54P_100_S500_R, PRO_H54P_200_S500_R) -//--------------------------------------------------------- -static const ControlItem items_PRO_PLUS[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1}, - {s_Status_Return_Level, 516, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 517, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 518, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 536, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 538, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 546, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 548, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 550, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 568, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 570, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 571, sizeof(s_Moving) - 1, 1}, - {s_Present_PWM, 572, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 584, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 588, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}}; - -#define COUNT_EXTPRO_PLUS_ITEMS (sizeof(items_PRO_PLUS) / sizeof(items_PRO_PLUS[0])) - -static const ModelInfo info_PRO_PLUS[] = { - {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M42P_010_S260_R - {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_040_S250_R - {0.01, -262931, 0, 262931, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_060_S250_R - {0.01, -303454, 0, 303454, -3.14159265, 3.14159265}, // PRO_PLUS_H42P_020_S300_R - {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}, // PRO_PLUS_H54P_100_S500_R - {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}}; // PRO_PLUS_H54P_200_S500_R - -//--------------------------------------------------------- -// Gripper - (num == RH_P12_RN) -//--------------------------------------------------------- -static const ControlItem items_Gripper[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Homing_Offset, 13, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2}, - {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1}, - {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4}, - {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2}, - {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4}, - {s_Moving, 610, sizeof(s_Moving) - 1, 1}, - {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4}, - {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2}, - {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}}; -#define COUNT_Gripper_ITEMS (sizeof(items_Gripper) / sizeof(items_Gripper[0])) - -static const ModelInfo info_Gripper = {0.01, - 0, - 0, - 1150, - 0, - 1.7631835937}; - -//--------------------------------------------------------- -// Gripper A Firmware - (num == RH_P12_RN_A) -//--------------------------------------------------------- -static const ControlItem items_EXTGripper[]{ - {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2}, - {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1}, - {s_ID, 7, sizeof(s_ID) - 1, 1}, - {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1}, - {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1}, - {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1}, - {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1}, - {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1}, - {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4}, - {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4}, - {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1}, - {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2}, - {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2}, - {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2}, - {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2}, - {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4}, - {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4}, - {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4}, - {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4}, - {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1}, - {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1}, - {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1}, - {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1}, - {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1}, - - {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1}, - {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1}, - {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1}, - {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1}, - {s_Status_Return_Level, 516, sizeof(s_Status_Return_Level) - 1, 1}, - {s_Registered_Instruction, 517, sizeof(s_Registered_Instruction) - 1, 1}, - {s_Hardware_Error_Status, 518, sizeof(s_Hardware_Error_Status) - 1, 1}, - {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2}, - {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2}, - {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2}, - {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2}, - {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2}, - {s_Feedforward_2nd_Gain, 536, sizeof(s_Feedforward_2nd_Gain) - 1, 2}, - {s_Feedforward_1st_Gain, 538, sizeof(s_Feedforward_1st_Gain) - 1, 2}, - {s_Bus_Watchdog, 546, sizeof(s_Bus_Watchdog) - 1, 1}, - {s_Goal_PWM, 548, sizeof(s_Goal_PWM) - 1, 2}, - {s_Goal_Current, 550, sizeof(s_Goal_Current) - 1, 2}, - {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4}, - {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4}, - {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4}, - {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4}, - {s_Realtime_Tick, 568, sizeof(s_Realtime_Tick) - 1, 2}, - {s_Moving, 570, sizeof(s_Moving) - 1, 1}, - {s_Moving_Status, 571, sizeof(s_Moving) - 1, 1}, - {s_Present_PWM, 572, sizeof(s_Present_PWM) - 1, 2}, - {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2}, - {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4}, - {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4}, - {s_Velocity_Trajectory, 584, sizeof(s_Velocity_Trajectory) - 1, 4}, - {s_Position_Trajectory, 588, sizeof(s_Position_Trajectory) - 1, 4}, - {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2}, - {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1}, - {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2}, - {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2}, - {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2}, - {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}}; -#define COUNT_EXTGripper_ITEMS (sizeof(items_EXTGripper) / sizeof(items_EXTGripper[0])) - -static const ModelInfo info_EXTGripper = {0.01, - 0, - 0, - 1150, - 0, - 1.7631835937}; - -//========================================================= -// Get Servo control table for the specified servo type -//========================================================= -static uint8_t the_number_of_item = 0; -const ControlItem *DynamixelItem::getControlTable(uint16_t model_number) -{ - uint16_t num = model_number; - - const ControlItem *control_table; - if (num == AX_12A || num == AX_12W || num == AX_18A) - { - control_table = items_AX; - the_number_of_item = COUNT_AX_ITEMS; - } - else if (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64) - { - control_table = items_RX; - the_number_of_item = COUNT_RX_ITEMS; - } - else if (num == EX_106) - { - control_table = items_EX; - the_number_of_item = COUNT_EX_ITEMS; - } - else if (num == MX_12W || num == MX_28) - { - control_table = items_MX; - the_number_of_item = COUNT_MX_ITEMS; - } - else if (num == MX_64 || num == MX_106) - { - control_table = items_EXTMX; - the_number_of_item = COUNT_EXTMX_ITEMS; - } - else if (num == MX_28_2) - { - control_table = items_MX2; - the_number_of_item = COUNT_MX2_ITEMS; - } - else if (num == MX_64_2 || num == MX_106_2) - { - control_table = items_EXTMX2; - the_number_of_item = COUNT_EXTMX2_ITEMS; - } - else if (num == XL_320) - { - control_table = items_XL320; - the_number_of_item = COUNT_XL320_ITEMS; - } - else if (num == XL430_W250 || num == XL430_W250_2 || num == XC430_W150 || num == XC430_W240) - { - control_table = items_XL; - the_number_of_item = COUNT_XL_ITEMS; - } - else if (num == XM430_W210 || num == XM430_W350) - { - control_table = items_XM; - the_number_of_item = COUNT_XM_ITEMS; - } - else if (num == XM540_W150 || num == XM540_W270) - { - control_table = items_EXTXM; - the_number_of_item = COUNT_EXTXM_ITEMS; - } - else if (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350) - { - control_table = items_XH; - the_number_of_item = COUNT_XH_ITEMS; - } - else if (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_V270) - { - control_table = items_EXTXH; - the_number_of_item = COUNT_EXTXH_ITEMS; - } - else if (num == XW540_T260 || num == XW540_T140) - { - control_table = items_XW; - the_number_of_item = COUNT_XW_ITEMS; - } - else if (num == PRO_L42_10_S300_R) - { - control_table = items_PRO; - the_number_of_item = COUNT_PRO_ITEMS; - } - else if (num == PRO_L54_30_S400_R || num == PRO_L54_30_S500_R || num == PRO_L54_50_S290_R || num == PRO_L54_50_S500_R || - num == PRO_M42_10_S260_R || num == PRO_M54_40_S250_R || num == PRO_M54_60_S250_R || - num == PRO_H42_20_S300_R || num == PRO_H54_100_S500_R || num == PRO_H54_200_S500_R) - { - control_table = items_EXTPRO; - the_number_of_item = COUNT_EXTPRO_ITEMS; - } - else if (num == PRO_M42_10_S260_R_A || num == PRO_M54_40_S250_R_A || num == PRO_M54_60_S250_R_A || - num == PRO_H42_20_S300_R_A || num == PRO_H54_100_S500_R_A || num == PRO_H54_200_S500_R_A) - { - control_table = items_EXTPRO_A; - the_number_of_item = COUNT_EXTPRO_A_ITEMS; - } - else if (num == PRO_PLUS_M42P_010_S260_R || num == PRO_PLUS_M54P_040_S250_R || num == PRO_PLUS_M54P_060_S250_R || - num == PRO_PLUS_H42P_020_S300_R || num == PRO_PLUS_H54P_100_S500_R || num == PRO_PLUS_H54P_200_S500_R) - { - control_table = items_PRO_PLUS; - the_number_of_item = COUNT_EXTPRO_PLUS_ITEMS; - } - else if (num == RH_P12_RN) - { - control_table = items_Gripper; - the_number_of_item = COUNT_Gripper_ITEMS; - } - else if (num == RH_P12_RN_A) - { - control_table = items_EXTGripper; - the_number_of_item = COUNT_EXTGripper_ITEMS; - } - else - { - control_table = NULL; - the_number_of_item = 0; - } - - return control_table; -} - -const ModelInfo *DynamixelItem::getModelInfo(uint16_t model_number) -{ - uint16_t num = model_number; - - const ModelInfo *info; - - if (num == AX_12A || num == AX_12W || num == AX_18A) - { - info = &info_AX; - } - else if (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64) - { - info = &info_RX; - } - else if (num == EX_106) - { - info = &info_EX; - } - else if (num == MX_12W || num == MX_28) - { - info = &info_MX; - } - else if (num == MX_64 || num == MX_106) - { - info = &info_EXTMX; - } - else if (num == MX_28_2) - { - info = &info_MX2; - } - else if (num == MX_64_2 || num == MX_106_2) - { - info = &info_EXTMX2; - } - else if (num == XL_320) - { - info = &info_XL320; - } - else if (num == XL430_W250 || num == XL430_W250_2 || num == XC430_W150 || num == XC430_W240) - { - info = &info_XL; - } - else if (num == XM430_W210 || num == XM430_W350) - { - info = &info_XM; - } - else if (num == XM540_W150 || num == XM540_W270) - { - info = &info_EXTXM; - } - else if (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350) - { - info = &info_XH; - } - else if (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_V270) - { - info = &info_EXTXH; - } - else if (num == XW540_T260 || num == XW540_T140) - { - info = &info_XW; - } - else if (num == PRO_L42_10_S300_R) - { - info = &info_PRO; - } - else if (num == PRO_L54_30_S400_R) - { - info = &info_EXTPRO[0]; - } - else if (num == PRO_L54_30_S500_R || num == PRO_L54_50_S500_R) - { - info = &info_EXTPRO[1]; - } - else if (num == PRO_L54_50_S290_R) - { - info = &info_EXTPRO[2]; - } - else if (num == PRO_M42_10_S260_R) - { - info = &info_EXTPRO[3]; - } - else if (num == PRO_M54_40_S250_R || num == PRO_M54_60_S250_R) - { - info = &info_EXTPRO[4]; - } - else if (num == PRO_H42_20_S300_R) - { - info = &info_EXTPRO[5]; - } - else if (num == PRO_H54_100_S500_R || num == PRO_H54_200_S500_R) - { - info = &info_EXTPRO[6]; - } - else if (num == PRO_M42_10_S260_R_A) - { - info = &info_EXTPRO_A[0]; - } - else if (num == PRO_M54_40_S250_R_A || num == PRO_M54_60_S250_R_A) - { - info = &info_EXTPRO_A[1]; - } - else if (num == PRO_H42_20_S300_R_A) - { - info = &info_EXTPRO_A[2]; - } - else if (num == PRO_H54_100_S500_R_A || num == PRO_H54_200_S500_R_A) - { - info = &info_EXTPRO_A[3]; - } - else if (num == PRO_PLUS_M42P_010_S260_R) - { - info = &info_PRO_PLUS[0]; - } - else if (num == PRO_PLUS_M54P_040_S250_R) - { - info = &info_PRO_PLUS[1]; - } - else if (num == PRO_PLUS_M54P_060_S250_R) - { - info = &info_PRO_PLUS[2]; - } - else if (num == PRO_PLUS_H42P_020_S300_R) - { - info = &info_PRO_PLUS[3]; - } - else if (num == PRO_PLUS_H54P_100_S500_R) - { - info = &info_PRO_PLUS[4]; - } - else if (num == PRO_PLUS_H54P_200_S500_R) - { - info = &info_PRO_PLUS[5]; - } - else if (num == RH_P12_RN) - { - info = &info_Gripper; - } - else if (num == RH_P12_RN_A) - { - info = &info_EXTGripper; - } - else - { - info = NULL; - } - - return info; -} - -uint8_t DynamixelItem::getTheNumberOfControlItem() -{ - return the_number_of_item; -} diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_tool.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_tool.cpp deleted file mode 100644 index 6c656ac..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_tool.cpp +++ /dev/null @@ -1,324 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby), Ryan Shim */ - -#include "../../include/dynamixel_workbench_toolbox/dynamixel_tool.h" - -//=================================================================== -// Define Serial ID to Namd table -//=================================================================== -typedef struct -{ - uint16_t number; - const char* name; -} DynamixelModel; - -static const DynamixelModel dynamixel_model_table[] = { - {AX_12A, "AX-12A"}, - {AX_12W, "AX-12W"}, - {AX_18A, "AX-18A"}, - - {RX_10, "RX-10"}, - {RX_24F, "RX-24F"}, - {RX_28, "RX-28"}, - {RX_64, "RX-64"}, - - {EX_106, "EX-106"}, - - {MX_12W, "MX-12W"}, - {MX_28, "MX-28"}, - {MX_28_2, "MX-28-2"}, - {MX_64, "MX-64"}, - {MX_64_2, "MX-64-2"}, - {MX_106, "MX-106"}, - {MX_106_2, "MX-106-2"}, - - {XL_320, "XL-320"}, - {XL430_W250, "XL430-W250"}, - - {XL430_W250_2, "XL430-W250-2"}, // 2XL - - {XC430_W150, "XC430-W150"}, - {XC430_W240, "XC430-W240"}, - - {XM430_W210, "XM430-W210"}, - {XM430_W350, "XM430-W350"}, - - {XM540_W150, "XM540-W150"}, - {XM540_W270, "XM540-W270"}, - - {XH430_V210, "XH430-V210"}, - {XH430_V350, "XH430-V350"}, - {XH430_W210, "XH430-W210"}, - {XH430_W350, "XH430-W350"}, - - {XH540_W150, "XH540_W150"}, - {XH540_W270, "XH540_W270"}, - {XH540_V150, "XH540_V150"}, - {XH540_V270, "XH540_V270"}, - - {XW540_T260, "XW540_T260"}, - {XW540_T140, "XW540_T140"}, - - {PRO_L42_10_S300_R, "PRO-L42-10-S300-R"}, - {PRO_L54_30_S400_R, "PRO-L54-30-S400-R"}, - {PRO_L54_30_S500_R, "PRO-L54-30-S500-R"}, - {PRO_L54_50_S290_R, "PRO-L54-50-S290-R"}, - {PRO_L54_50_S500_R, "PRO-L54-50-S500-R"}, - - {PRO_M42_10_S260_R, "PRO-M42-10-S260-R"}, - {PRO_M54_40_S250_R, "PRO-M54-40-S250-R"}, - {PRO_M54_60_S250_R, "PRO-M54-60-S250-R"}, - - {PRO_H42_20_S300_R, "PRO-H42-20-S300-R"}, - {PRO_H54_100_S500_R, "PRO-H54-100-S500-R"}, - {PRO_H54_200_S500_R, "PRO-H54-200-S500-R"}, - - {PRO_M42_10_S260_R_A, "PRO-M42-10-S260-R-A"}, - {PRO_M54_40_S250_R_A, "PRO-M54-40-S250-R-A"}, - {PRO_M54_60_S250_R_A, "PRO-M54-60-S250-R-A"}, - - {PRO_H42_20_S300_R_A, "PRO-H42-20-S300-R-A"}, - {PRO_H54_100_S500_R_A, "PRO-H54-100-S500-R-A"}, - {PRO_H54_200_S500_R_A, "PRO-H54-200-S500-R-A"}, - - {PRO_PLUS_M42P_010_S260_R, "PRO-PLUS-M42P-010-S260-R"}, - {PRO_PLUS_M54P_040_S250_R, "PRO-PLUS-M54P-040-S250-R"}, - {PRO_PLUS_M54P_060_S250_R, "PRO-PLUS-M54P-060-S250-R"}, - - {PRO_PLUS_H42P_020_S300_R, "PRO-PLUS-H42P-020-S300-R"}, - {PRO_PLUS_H54P_100_S500_R, "PRO-PLUS-H54P-100-S500-R"}, - {PRO_PLUS_H54P_200_S500_R, "PRO-PLUS-H54P-200-S500-R"}, - - {RH_P12_RN, "RH-P12-RN"}, - - {RH_P12_RN_A, "RH-P12-RN-A"} -}; -#define COUNT_DYNAMIXEL_MODEL (sizeof(dynamixel_model_table)/sizeof(dynamixel_model_table[0])) - -DynamixelTool::DynamixelTool() : dxl_cnt_(0), the_number_of_control_item_(0){} - -DynamixelTool::~DynamixelTool(){} - -void DynamixelTool::initTool(void) -{ - for (uint8_t i = 0; i < DYNAMIXEL_BUFFER; i++) - dxl_id_[i] = 0; - - dxl_cnt_ = 0; -} - -bool DynamixelTool::addTool(const char *model_name, uint8_t id, const char **log) -{ - bool result = false; - initTool(); - - model_name_ = model_name; - result = setModelNumber(model_name, log); - if (result == false) return false; - dxl_id_[dxl_cnt_++] = id; - - result = setControlTable(model_name, log); - if (result == false) return false; - - return true; -} - -bool DynamixelTool::addTool(uint16_t model_number, uint8_t id, const char **log) -{ - bool result = false; - initTool(); - - result = setModelName(model_number, log); - if (result == false) return false; - model_number_ = model_number; - dxl_id_[dxl_cnt_++] = id; - - result = setControlTable(model_number, log); - if (result == false) return false; - - return result; -} - -void DynamixelTool::addDXL(uint8_t id) -{ - dxl_id_[dxl_cnt_++] = id; -} - -bool DynamixelTool::setControlTable(const char *model_name, const char **log) -{ - const char* name = model_name; - uint8_t name_length = strlen(name); - - for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) - { - if(strncmp(name, dynamixel_model_table[index].name, name_length) == 0) - { - return setControlTable(dynamixel_model_table[index].number, log); - } - } - - if (log != NULL) - *log = "[DynamixelTool] Failed to set control table due to mismatch model name and model number"; - return false; -} - -bool DynamixelTool::setControlTable(uint16_t model_number, const char **log) -{ - control_table_ = DynamixelItem::getControlTable(model_number); - the_number_of_control_item_ = DynamixelItem::getTheNumberOfControlItem(); - model_info_ = DynamixelItem::getModelInfo(model_number); - - if (control_table_ == NULL || model_info_ == NULL) - { - if (log != NULL) - *log = "[DynamixelTool] Failed to get control table or model info"; - return false; - } - - return true; -} - -bool DynamixelTool::setModelName(uint16_t model_number, const char **log) -{ - uint16_t num = model_number; - - for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) - { - if (num == dynamixel_model_table[index].number) - { - model_name_ = dynamixel_model_table[index].name; - return true; - } - } - - if (log != NULL) - *log = "[DynamixelTool] Failed to find model name"; - return false; -} - -bool DynamixelTool::setModelNumber(const char *model_name, const char **log) -{ - const char* name = model_name; - uint8_t name_length = strlen(name); - - for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) - { - if(strncmp(name, model_name_, name_length) == 0) - { - model_number_ = dynamixel_model_table[index].number; - return true; - } - } - - if (log != NULL) - *log = "[DynamixelTool] Failed to find model number"; - return false; -} - -const char* DynamixelTool::getModelName(void) -{ - return model_name_; -} - -uint16_t DynamixelTool::getModelNumber(void) -{ - return model_number_; -} - -const uint8_t* DynamixelTool::getID(void) -{ - const uint8_t* id_table_ = dxl_id_; - - return id_table_; -} - -uint8_t DynamixelTool::getDynamixelCount(void) -{ - return dxl_cnt_; -} - -uint8_t DynamixelTool::getDynamixelBuffer(void) -{ - return DYNAMIXEL_BUFFER; -} - -float DynamixelTool::getRPM(void) -{ - return model_info_->rpm; -} - -int64_t DynamixelTool::getValueOfMinRadianPosition(void) -{ - return model_info_->value_of_min_radian_position; -} - -int64_t DynamixelTool::getValueOfMaxRadianPosition(void) -{ - return model_info_->value_of_max_radian_position; -} - -int64_t DynamixelTool::getValueOfZeroRadianPosition(void) -{ - return model_info_->value_of_zero_radian_position; -} - -float DynamixelTool::getMinRadian(void) -{ - return model_info_->min_radian; -} - -float DynamixelTool::getMaxRadian(void) -{ - return model_info_->max_radian; -} - -uint8_t DynamixelTool::getTheNumberOfControlItem(void) -{ - return the_number_of_control_item_; -} - -const ControlItem *DynamixelTool::getControlItem(const char *item_name, const char **log) -{ - const ControlItem* control_item = control_table_; - uint8_t name_length = strlen(item_name); - - for (uint8_t num = 0; num < the_number_of_control_item_; num++) - { - if ((name_length == control_item->item_name_length) && - (memcmp(item_name, control_item->item_name, name_length) == 0)) - { - return control_item; - } - control_item++; - } - - if (log != NULL) - *log = "[DynamixelTool] Can't find Item"; - return NULL; -} - -const ControlItem *DynamixelTool::getControlTable(void) -{ - return control_table_; -} - -const ModelInfo *DynamixelTool::getModelInfo(void) -{ - return model_info_; -} - diff --git a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp b/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp deleted file mode 100644 index 91b0484..0000000 --- a/interbotix_ros_xseries/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp +++ /dev/null @@ -1,1432 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Taehun Lim (Darby), Ryan Shim */ - -#include "../../include/dynamixel_workbench_toolbox/dynamixel_workbench.h" - -static const uint8_t WHEEL_MODE = 1; -static const uint8_t JOINT_MODE = 2; - -static const uint8_t CURRENT_CONTROL_MODE = 0; -static const uint8_t VELOCITY_CONTROL_MODE = 1; -static const uint8_t POSITION_CONTROL_MODE = 3; -static const uint8_t EXTENDED_POSITION_CONTROL_MODE = 4; -static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE = 5; -static const uint8_t PWM_CONTROL_MODE = 16; -static const uint8_t TORQUE_CONTROL_MODE = 100; -static const uint8_t MULTI_TURN_MODE = 101; - -static const char* model_name = NULL; -static const ModelInfo* model_info = NULL; - -DynamixelWorkbench::DynamixelWorkbench(){} - -DynamixelWorkbench::~DynamixelWorkbench(){} - -bool DynamixelWorkbench::torque(uint8_t id, int32_t onoff, const char **log) -{ - bool result = false; - - result = itemWrite(id, "Torque_Enable", (int32_t)onoff, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to change torque status!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change torque status!"; - return result; -} - -bool DynamixelWorkbench::torqueOn(uint8_t id, const char **log) -{ - bool result = false; - - result = torque(id, 1, log); - - return result; -} - -bool DynamixelWorkbench::torqueOff(uint8_t id, const char **log) -{ - bool result = false; - - result = torque(id, 0, log); - - return result; -} - -bool DynamixelWorkbench::changeID(uint8_t id, uint8_t new_id, const char **log) -{ - bool result = false; - - result = torqueOff(id, log); - if (result == false) return false; - - result = writeRegister(id, "ID", new_id, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to change ID!"; - return false; - } - // millis(1000); - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change ID!"; - return result; -} - -bool DynamixelWorkbench::changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log) -{ - bool result = false; - - result = torqueOff(id, log); - if (result == false) return false; - - if (getProtocolVersion() == 1.0f) - { - switch (new_baudrate) - { - case 9600: - result = writeRegister(id, "Baud_Rate", 207, log); - break; - - case 19200: - result = writeRegister(id, "Baud_Rate", 103, log); - break; - - case 57600: - result = writeRegister(id, "Baud_Rate", 34, log); - break; - - case 115200: - result = writeRegister(id, "Baud_Rate", 16, log); - break; - - case 200000: - result = writeRegister(id, "Baud_Rate", 9, log); - break; - - case 250000: - result = writeRegister(id, "Baud_Rate", 7, log); - break; - - case 400000: - result = writeRegister(id, "Baud_Rate", 4, log); - break; - - case 500000: - result = writeRegister(id, "Baud_Rate", 3, log); - break; - - case 1000000: - result = writeRegister(id, "Baud_Rate", 1, log); - break; - - case 2250000: - result = writeRegister(id, "Baud_Rate", 250, log); - break; - - case 2500000: - result = writeRegister(id, "Baud_Rate", 251, log); - break; - - case 3000000: - result = writeRegister(id, "Baud_Rate", 252, log); - break; - - default: - result = writeRegister(id, "Baud_Rate", 34, log); - break; - } - } - else if (getProtocolVersion() == 2.0f) - { - switch (new_baudrate) - { - case 9600: - result = writeRegister(id, "Baud_Rate", 0, log); - break; - - case 57600: - result = writeRegister(id, "Baud_Rate", 1, log); - break; - - case 115200: - result = writeRegister(id, "Baud_Rate", 2, log); - break; - - case 1000000: - result = writeRegister(id, "Baud_Rate", 3, log); - break; - - case 2000000: - result = writeRegister(id, "Baud_Rate", 4, log); - break; - - case 3000000: - result = writeRegister(id, "Baud_Rate", 5, log); - break; - - case 4000000: - result = writeRegister(id, "Baud_Rate", 6, log); - break; - - case 4500000: - result = writeRegister(id, "Baud_Rate", 7, log); - break; - - case 10500000: - result = writeRegister(id, "Baud_Rate", 8, log); - break; - - default: - result = writeRegister(id, "Baud_Rate", 1, log); - break; - } - } -#if defined(__OPENCR__) || defined(__OPENCM904__) - delay(2000); -#else - usleep(1000*2000); -#endif - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to change Baud Rate!"; - return result; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change Baud Rate!"; - return result; -} - -bool DynamixelWorkbench::changeProtocolVersion(uint8_t id, uint8_t version, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = writeRegister(id, "Protocol_Version", version, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set protocol version!"; - return false; - } - } - - result = setPacketHandler((float)version, log); - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set protocol version!"; - return result; -} - -bool DynamixelWorkbench::itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log) -{ - return writeRegister(id, item_name, data, log); -} - -bool DynamixelWorkbench::itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log) -{ - return readRegister(id, item_name, data, log); -} - -bool DynamixelWorkbench::led(uint8_t id, int32_t onoff, const char **log) -{ - bool result = false; - - result = writeRegister(id, "LED", onoff, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to change led status!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change led status!"; - return result; -} - -bool DynamixelWorkbench::ledOn(uint8_t id, const char **log) -{ - bool result = false; - - result = led(id, 1, log); - - return result; -} - -bool DynamixelWorkbench::ledOff(uint8_t id, const char **log) -{ - bool result = false; - - result = led(id, 0, log); - - return result; -} - -bool DynamixelWorkbench::setNormalDirection(uint8_t id, const char **log) -{ - bool result = false; - int32_t data = 0; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = readRegister(id, "Drive_Mode", &data, log); - - data = data & 0b00000100; - result = writeRegister(id, "Drive_Mode", data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set normal direction!"; - return false; - } - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set normal direction!"; - return result; -} - -bool DynamixelWorkbench::setReverseDirection(uint8_t id, const char **log) -{ - bool result = false; - int32_t data = 0; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = readRegister(id, "Drive_Mode", &data, log); - - data = data | 0b00000001; - result = writeRegister(id, "Drive_Mode", data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set reverse direction!"; - return false; - } - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set reverse direction!"; - return result; -} - -bool DynamixelWorkbench::setVelocityBasedProfile(uint8_t id, const char **log) -{ - bool result = false; - int32_t data = 0; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = readRegister(id, "Drive_Mode", &data, log); - - data = data & 0b00000001; - result = writeRegister(id, "Drive_Mode", data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set velocity based profile!"; - return false; - } - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set velocity based profile!"; - return result; -} - -bool DynamixelWorkbench::setTimeBasedProfile(uint8_t id, const char **log) -{ - bool result = false; - int32_t data = 0; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = readRegister(id, "Drive_Mode", &data, log); - - data = data | 0b00000100; - result = writeRegister(id, "Drive_Mode", data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set time based profile!"; - return false; - } - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set time based profile!"; - return result; -} - -bool DynamixelWorkbench::setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = torqueOff(id, log); - if (result == false) return false; - - result = writeRegister(id, "Secondary_ID", secondary_id, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set secondary ID!"; - return false; - } - } - - // millis(1000); - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set secondary ID!"; - return result; -} - -bool DynamixelWorkbench::setPositionControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, POSITION_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Position Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Position Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setVelocityControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, VELOCITY_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Velocity Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Velocity Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setCurrentControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, CURRENT_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setTorqueControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, TORQUE_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Torque Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Torque Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setExtendedPositionControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, EXTENDED_POSITION_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Extended Position Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Extended Position Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setMultiTurnControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, MULTI_TURN_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Multi-Turn Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Multi-Turn Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setCurrentBasedPositionControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, CURRENT_BASED_POSITION_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setPWMControlMode(uint8_t id, const char **log) -{ - bool result = false; - - result = setOperatingMode(id, PWM_CONTROL_MODE, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set PWM Control Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set PWM Control Mode!"; - return result; -} - -bool DynamixelWorkbench::setOperatingMode(uint8_t id, uint8_t index, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - if (getProtocolVersion() == 1.0) - { - if (index == POSITION_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "PRO", strlen("PRO")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log); - } - else if (!strncmp(model_name, "AX", 2) || !strncmp(model_name, "RX", 2)) - { - result = writeRegister(id, "CW_Angle_Limit", 0, log); - result = writeRegister(id, "CCW_Angle_Limit", 1023, log); - } - else - { - result = writeRegister(id, "CW_Angle_Limit", 0, log); - result = writeRegister(id, "CCW_Angle_Limit", 4095, log); - } - } - else if (index == VELOCITY_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "PRO", strlen("PRO"))) - { - result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE); - } - else - { - result = writeRegister(id, "CW_Angle_Limit", 0, log); - result = writeRegister(id, "CCW_Angle_Limit", 0, log); - } - } - else if (index == CURRENT_CONTROL_MODE) - { - if (!strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log); - } - } - else if (index == TORQUE_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-64", strlen("MX-64")) || - !strncmp(model_name, "MX-106", strlen("MX-106")) ) - { - result = writeRegister(id, "Torque_Control_Mode_Enable", 1, log); - } - } - else if (index == MULTI_TURN_MODE) - { - if (!strncmp(model_name, "MX-64", strlen("MX-64")) || - !strncmp(model_name, "MX-106", strlen("MX-106")) ) - { - result = writeRegister(id, "CW_Angle_Limit", 4095, log); - result = writeRegister(id, "CCW_Angle_Limit", 4095, log); - } - } - else if (index == CURRENT_BASED_POSITION_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log); - } - } - else if (index == PWM_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log); - } - } - } - else if (getProtocolVersion() == 2.0) - { - if (index == POSITION_CONTROL_MODE) - { - if (!strncmp(model_name, "XL-320", strlen("XL-320"))) - result = writeRegister(id, "Control_Mode", JOINT_MODE, log); - else - result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log); - } - else if (index == VELOCITY_CONTROL_MODE) - { - if (!strncmp(model_name, "XL-320", strlen("XL-320"))) - result = writeRegister(id, "Control_Mode", WHEEL_MODE, log); - else - result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE, log); - } - else if (index == CURRENT_CONTROL_MODE) - { - if (!strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "RH", strlen("RH")) || - !strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS"))) - { - result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log); - } - } - else if (index == TORQUE_CONTROL_MODE) - { - if (!strncmp(model_name, "PRO", strlen("PRO")) || - strncmp(model_name, "PRO-L42", strlen("PRO-L42")) ) - { - result = writeRegister(id, "Operating_Mode", 0, log); - } - } - else if (index == EXTENDED_POSITION_CONTROL_MODE) - { - if (!strncmp(model_name, "PRO", strlen("PRO")) || - strncmp(model_name, "PRO-L42", strlen("PRO-L42")) ) - { - result = writeRegister(id, "Operating_Mode", EXTENDED_POSITION_CONTROL_MODE, log); - } - } - else if (index == CURRENT_BASED_POSITION_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log); - } - } - else if (index == PWM_CONTROL_MODE) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "PRO", strlen("PRO"))) - { - result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log); - } - } - } - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Operating Mode!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Operating Mode!"; - return result; -} - - -bool DynamixelWorkbench::jointMode(uint8_t id, int32_t velocity, int32_t acceleration, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - result = torqueOff(id, log); - if (result == false) return false; - - result = setPositionControlMode(id, log); - if (result == false) return false; - - if (getProtocolVersion() == 1.0) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - result = writeRegister(id, "Profile_Velocity", velocity, log); - } - else if (!strncmp(model_name, "MX-28", strlen("MX-28")) || - !strncmp(model_name, "MX-64", strlen("MX-64")) || - !strncmp(model_name, "MX-106", strlen("MX-106"))) - { - result = writeRegister(id, "Moving_Speed", velocity, log); - result = writeRegister(id, "Goal_Acceleration", acceleration, log); - } - else - { - result = writeRegister(id, "Moving_Speed", velocity, log); - } - } - else if (getProtocolVersion() == 2.0) - { - if (!strncmp(model_name, "XL-320", strlen("XL-320"))) - { - result = writeRegister(id, "Moving_Speed", velocity, log); - } - else if (!strncmp(model_name, "PRO-M42-10-S260-R-A", strlen("PRO-M42-10-S260-R-A")) || - !strncmp(model_name, "PRO-M54-40-S250-R-A", strlen("PRO-M54-40-S250-R-A")) || - !strncmp(model_name, "PRO-M54-60-S250-R-A", strlen("PRO-M54-60-S250-R-A")) || - !strncmp(model_name, "PRO-H42-20-S300-R-A", strlen("PRO-H42-20-S300-R-A")) || - !strncmp(model_name, "PRO-H54-100-S500-R-A", strlen("PRO-H54-100-S500-R-A")) || - !strncmp(model_name, "PRO-H54-200-S500-R-A", strlen("PRO-H54-200-S500-R-A")) || - !strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS"))) - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - result = writeRegister(id, "Profile_Velocity", velocity, log); - } - else if (!strncmp(model_name, "PRO-L", strlen("PRO-L")) || - !strncmp(model_name, "PRO-M", strlen("PRO-M")) || - !strncmp(model_name, "PRO-H", strlen("PRO-H"))) - { - result = writeRegister(id, "Goal_Velocity", velocity, log); - result = writeRegister(id, "Goal_Acceleration", acceleration, log); - } - else - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - result = writeRegister(id, "Profile_Velocity", velocity, log); - } - } - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Joint Mode!"; - return false; - } - - result = torqueOn(id, log); - if (result == false) return false; - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Joint Mode!"; - return result; -} - -bool DynamixelWorkbench::wheelMode(uint8_t id, int32_t acceleration, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - result = torqueOff(id, log); - if (result == false) return false; - - result = setVelocityControlMode(id, log); - if (result == false) return false; - - if (getProtocolVersion() == 1.0) - { - if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || - !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XL430", strlen("XL430")) || - !strncmp(model_name, "XC430", strlen("XC430")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW"))) - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - } - else if (!strncmp(model_name, "MX-28", strlen("MX-28")) || - !strncmp(model_name, "MX-64", strlen("MX-64")) || - !strncmp(model_name, "MX-106", strlen("MX-106"))) - { - result = writeRegister(id, "Goal_Acceleration", acceleration, log); - } - } - else if (getProtocolVersion() == 2.0) - { - if (!strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS"))) - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - } - else if (!strncmp(model_name, "PRO", strlen("PRO"))) - { - result = writeRegister(id, "Goal_Acceleration", acceleration, log); - } - else - { - result = writeRegister(id, "Profile_Acceleration", acceleration, log); - } - } - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Wheel Mode!"; - return false; - } - - result = torqueOn(id, log); - if (result == false) return false; - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Wheel Mode!"; - return result; -} - -bool DynamixelWorkbench::currentBasedPositionMode(uint8_t id, int32_t current, const char **log) -{ - bool result = false; - - model_name = getModelName(id, log); - if (model_name == NULL) return false; - - result = torqueOff(id, log); - if (result == false) return false; - - result = setCurrentBasedPositionControlMode(id, log); - if (result == false) return false; - - if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || - !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || - !strncmp(model_name, "XM", strlen("XM")) || - !strncmp(model_name, "XH", strlen("XH")) || - !strncmp(model_name, "XW", strlen("XW")) || - !strncmp(model_name, "RH", strlen("RH"))) - { - result = writeRegister(id, "Goal_Current", current, log); - } - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Mode!"; - return false; - } - - result = torqueOn(id, log); - if (result == false) return false; - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Wheel Mode!"; - return result; -} - -//keep compatibility with older codes -bool DynamixelWorkbench::goalPosition(uint8_t id, int value, const char **log) -{ -// goalPosition(id, (int32_t)value, log); -// } - -// bool DynamixelWorkbench::goalPosition(uint8_t id, int32_t value, const char **log) -// { - bool result = false; - - result = itemWrite(id, "Goal_Position", value, log); - - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!"; - return result; -} - -//keep compatibility with older codes -bool DynamixelWorkbench::goalSpeed(uint8_t id, int value, const char **log) -{ - bool result = false; - result = goalVelocity(id, (int32_t)value, log); - return result; -} - -//keep compatibility with older codes -bool DynamixelWorkbench::goalVelocity(uint8_t id, int value, const char **log) -{ -// goalVelocity(id, (int32_t)value, log); -// } - -// bool DynamixelWorkbench::goalVelocity(uint8_t id, int32_t value, const char **log) -// { - bool result[2] = {false, false}; - - if (getProtocolVersion() == 2.0f) - { - result[0] = writeRegister(id, "Goal_Velocity", value, log); - if (result[0] == false) - { - if (value < 0) - { - value = (-1) * value; - value |= 1024; - } - result[1] = writeRegister(id, "Moving_Speed", value, log); - if (result[1] == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; - return false; - } - else - { - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; - return true; - } - } - else - { - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; - return true; - } - } - else - { - result[0] = writeRegister(id, "Goal_Velocity", value, log); - if (result[0] == false) - { - if (value < 0) - { - value = (-1) * value; - value |= 1024; - } - result[1] = writeRegister(id, "Moving_Speed", value, log); - if (result[1] == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; - return false; - } - else - { - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; - return true; - } - } - else - { - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; - return true; - } - } - - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; - return false; -} - -bool DynamixelWorkbench::goalPosition(uint8_t id, float radian, const char **log) -{ - bool result = 0; - uint32_t value = 0; - - value = convertRadian2Value(id, radian); - - result = goalPosition(id, (int32_t)value, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!"; - return false; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!"; - return true; -} - -bool DynamixelWorkbench::goalVelocity(uint8_t id, float velocity, const char **log) -{ - bool result = 0; - int32_t value = 0; - - value = convertVelocity2Value(id, velocity); - - result = goalVelocity(id, value, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; - return result; - } - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; - return result; -} - -bool DynamixelWorkbench::getPresentPositionData(uint8_t id, int32_t* data, const char **log) -{ - bool result = 0; - int32_t get_data = 0; - - result = readRegister(id, "Present_Position", &get_data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to get present position data!"; - return result; - } - - *data = get_data; - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get present position data!"; - return result; -} - -bool DynamixelWorkbench::getRadian(uint8_t id, float* radian, const char **log) -{ - bool result = 0; - int32_t get_data = 0; - - result = getPresentPositionData(id, &get_data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to get radian!"; - return result; - } - - *radian = convertValue2Radian(id, get_data); - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get radian!"; - return result; -} - -bool DynamixelWorkbench::getVelocity(uint8_t id, float* velocity, const char **log) -{ - bool result = 0; - int32_t get_data = 0; - - result = getPresentVelocityData(id, &get_data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to get velocity!"; - return result; - } - - *velocity = convertValue2Velocity(id, get_data); - - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get velocity!"; - return result; -} - -bool DynamixelWorkbench::getPresentVelocityData(uint8_t id, int32_t* data, const char **log) -{ - bool result = 0; - int32_t get_data = 0; - - result = readRegister(id, "Present_Speed", &get_data, log); - if (result == false) - { - if (log != NULL) *log = "[DynamixelWorkbench] Failed to get present speed data!"; - return result; - } - - *data = get_data; - if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get present speed data!"; - return result; -} - -int32_t DynamixelWorkbench::convertRadian2Value(uint8_t id, float radian) -{ - int32_t position = 0; - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (radian > 0) - { - position = (radian * (model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position) / model_info->max_radian) + model_info->value_of_zero_radian_position; - } - else if (radian < 0) - { - position = (radian * (model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position) / model_info->min_radian) + model_info->value_of_zero_radian_position; - } - else - { - position = model_info->value_of_zero_radian_position; - } - - return position; -} - -float DynamixelWorkbench::convertValue2Radian(uint8_t id, int32_t value) -{ - float radian = 0.0; - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (value > model_info->value_of_zero_radian_position) - { - radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->max_radian / (float)(model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position); - } - else if (value < model_info->value_of_zero_radian_position) - { - radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->min_radian / (float)(model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position); - } - - return radian; -} - -int32_t DynamixelWorkbench::convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian) -{ - int32_t value = 0; - int32_t zero_position = (max_position + min_position)/2; - - if (radian > 0) - { - value = (radian * (max_position - zero_position) / max_radian) + zero_position; - } - else if (radian < 0) - { - value = (radian * (min_position - zero_position) / min_radian) + zero_position; - } - else - { - value = zero_position; - } - - return value; -} - -float DynamixelWorkbench::convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian) -{ - float radian = 0.0; - int32_t zero_position = (max_position + min_position)/2; - - if (value > zero_position) - { - radian = (float)(value - zero_position) * max_radian / (float)(max_position - zero_position); - } - else if (value < zero_position) - { - radian = (float)(value - zero_position) * min_radian / (float)(min_position - zero_position); - } - - return radian; -} - -int32_t DynamixelWorkbench::convertVelocity2Value(uint8_t id, float velocity) -{ - int32_t value = 0; - const float RPM2RADPERSEC = 0.104719755f; - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (getProtocolVersion() == 1.0f) - { - if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 || - strncmp(getModelName(id), "RX", strlen("RX")) == 0 || - strncmp(getModelName(id), "EX", strlen("EX")) == 0 || - strncmp(getModelName(id), "MX", strlen("MX")) == 0) - { - if (velocity == 0.0f) value = 0; - else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)); - else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023; - - return value; - } - } - else if (getProtocolVersion() == 2.0f) - { - if (strcmp(getModelName(id), "XL-320") == 0) - { - if (velocity == 0.0f) value = 0; - else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)); - else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023; - - return value; - } - else - { - value = velocity / (model_info->rpm * RPM2RADPERSEC); - } - - return value; - } - - return 0; -} - -float DynamixelWorkbench::convertValue2Velocity(uint8_t id, int32_t value) -{ - float velocity = 0; - const float RPM2RADPERSEC = 0.104719755f; - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (getProtocolVersion() == 1.0f) - { - if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 || - strncmp(getModelName(id), "RX", strlen("RX")) == 0 || - strncmp(getModelName(id), "EX", strlen("EX")) == 0 || - strncmp(getModelName(id), "MX", strlen("MX")) == 0) - { - if (value == 1023 || value == 0) velocity = 0.0f; - else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC; - else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f); - - return velocity; - } - } - else if (getProtocolVersion() == 2.0f) - { - if (strcmp(getModelName(id), "XL-320") == 0) - { - if (value == 1023 || value == 0) velocity = 0.0f; - else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC; - else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f); - } - else - { - velocity = value * (model_info->rpm * RPM2RADPERSEC); - } - - return velocity; - } - - return 0.0f; -} - -int16_t DynamixelWorkbench::convertCurrent2Value(uint8_t id, float current) -{ - float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (getProtocolVersion() == 1.0f) - { - return (current / CURRENT_UNIT); - } - else if (getProtocolVersion() == 2.0f) - { - if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 || - strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 || - strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0) - { - CURRENT_UNIT = 16.11328f; - return (current / CURRENT_UNIT); - } - else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0) - { - CURRENT_UNIT = 1.0f; - return (current / CURRENT_UNIT); - } - else - { - return (current / CURRENT_UNIT); - } - } - - return (current / CURRENT_UNIT); -} - -int16_t DynamixelWorkbench::convertCurrent2Value(float current) -{ - int16_t value = 0; - const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 - - value = current / CURRENT_UNIT; - - return value; -} - -float DynamixelWorkbench::convertValue2Current(uint8_t id, int16_t value) -{ - float current = 0; - float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 - - model_info = getModelInfo(id); - if (model_info == NULL) return false; - - if (getProtocolVersion() == 1.0f) - { - current = (int16_t)value * CURRENT_UNIT; - return current; - } - else if (getProtocolVersion() == 2.0f) - { - if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 || - strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 || - strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0) - { - CURRENT_UNIT = 16.11328f; - current = (int16_t)value * CURRENT_UNIT; - return current; - } - else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0) - { - CURRENT_UNIT = 1.0f; - current = (int16_t)value * CURRENT_UNIT; - return current; - } - else - { - current = (int16_t)value * CURRENT_UNIT; - return current; - } - } - - current = (int16_t)value * CURRENT_UNIT; - - return current; -} - -float DynamixelWorkbench::convertValue2Current(int16_t value) -{ - float current = 0; - const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 - - current = (int16_t)value * CURRENT_UNIT; - - return current; -} - -float DynamixelWorkbench::convertValue2Load(int16_t value) -{ - float load = 0; - const float LOAD_UNIT = 0.1f; //Unit : %, Ref : http://emanual.robotis.com/docs/en/dxl/mx/mx-28/#present-load - - if (value == 1023 || value == 0) load = 0.0f; - else if (value > 0 && value < 1023) load = value * LOAD_UNIT; - else if (value > 1023 && value < 2048) load = (value - 1023) * LOAD_UNIT * (-1.0f); - - return load; -} diff --git a/interbotix_ros_xseries/interbotix_ros_xseries/CMakeLists.txt b/interbotix_ros_xseries/interbotix_ros_xseries/CMakeLists.txt index 26aa2aa..0a57ebe 100644 --- a/interbotix_ros_xseries/interbotix_ros_xseries/CMakeLists.txt +++ b/interbotix_ros_xseries/interbotix_ros_xseries/CMakeLists.txt @@ -1,4 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(interbotix_ros_xseries) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_xseries/interbotix_ros_xseries/package.xml b/interbotix_ros_xseries/interbotix_ros_xseries/package.xml index 7a842f1..7c3c248 100644 --- a/interbotix_ros_xseries/interbotix_ros_xseries/package.xml +++ b/interbotix_ros_xseries/interbotix_ros_xseries/package.xml @@ -1,5 +1,6 @@ - + + interbotix_ros_xseries 0.0.0 The interbotix_ros_xseries metapackage @@ -8,12 +9,15 @@ Luke Schmitt - catkin + ament_cmake dynamixel_workbench_toolbox interbotix_xs_msgs interbotix_xs_sdk + ament_lint_auto + ament_lint_common + - + ament_cmake diff --git a/interbotix_ros_xseries/interbotix_xs_driver b/interbotix_ros_xseries/interbotix_xs_driver new file mode 160000 index 0000000..da27b8b --- /dev/null +++ b/interbotix_ros_xseries/interbotix_xs_driver @@ -0,0 +1 @@ +Subproject commit da27b8b2b6c7677844f74581b82c01829a834e1c diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt b/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt index ae9b266..575d0f3 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt +++ b/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt @@ -1,50 +1,48 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(interbotix_xs_msgs) -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs - trajectory_msgs - geometry_msgs -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_message_files( - FILES - JointGroupCommand.msg - JointSingleCommand.msg - JointTrajectoryCommand.msg - # xsarm_diagnostic_tool - JointTemps.msg - # xsarm_joy - ArmJoy.msg - #xshexapod_joy - HexJoy.msg - #xslocobot_joy - LocobotJoy.msg - #xsturret_joy - TurretJoy.msg -) +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) -add_service_files( - FILES - Reboot.srv - RobotInfo.srv - MotorGains.srv - TorqueEnable.srv - OperatingModes.srv - RegisterValues.srv +set(MSG_FILES + msg/ArmJoy.msg + msg/HexJoy.msg + msg/JointGroupCommand.msg + msg/JointSingleCommand.msg + msg/JointTemps.msg + msg/JointTrajectoryCommand.msg + msg/LocobotJoy.msg + msg/TurretJoy.msg ) -generate_messages( - DEPENDENCIES - std_msgs - trajectory_msgs - geometry_msgs +## Generate services in the 'srv' folder +set(SRV_FILES + srv/MotorGains.srv + srv/OperatingModes.srv + srv/Reboot.srv + srv/RegisterValues.srv + srv/RobotInfo.srv + srv/TorqueEnable.srv ) -catkin_package( - CATKIN_DEPENDS - message_runtime - std_msgs - trajectory_msgs +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_FILES} + ${SRV_FILES} + DEPENDENCIES std_msgs trajectory_msgs ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/msg/ArmJoy.msg b/interbotix_ros_xseries/interbotix_xs_msgs/msg/ArmJoy.msg index 87c5852..eb896c0 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/msg/ArmJoy.msg +++ b/interbotix_ros_xseries/interbotix_xs_msgs/msg/ArmJoy.msg @@ -29,8 +29,8 @@ int8 WAIST_CCW = 11 int8 WAIST_CW = 12 # Gripper Control -int8 GRIPPER_OPEN = 13 -int8 GRIPPER_CLOSE = 14 +int8 GRIPPER_RELEASE = 13 +int8 GRIPPER_GRASP = 14 # Pose Control int8 HOME_POSE = 15 @@ -44,7 +44,7 @@ int8 SPEED_INC = 17 int8 SPEED_DEC = 18 # Quickly toggle between a fast and slow speed setting -int8 SPEED_COURSE = 19 +int8 SPEED_COARSE = 19 int8 SPEED_FINE = 20 # Inc/Dec Gripper pressure diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/msg/HexJoy.msg b/interbotix_ros_xseries/interbotix_xs_msgs/msg/HexJoy.msg index 8097932..066322e 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/msg/HexJoy.msg +++ b/interbotix_ros_xseries/interbotix_xs_msgs/msg/HexJoy.msg @@ -62,7 +62,7 @@ int8 SPEED_INC = 29 int8 SPEED_DEC = 30 # Quickly toggle between a fast and slow speed setting -int8 SPEED_COURSE = 31 +int8 SPEED_COARSE = 31 int8 SPEED_FINE = 32 ######################################################################################################### diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/msg/LocobotJoy.msg b/interbotix_ros_xseries/interbotix_xs_msgs/msg/LocobotJoy.msg index 20e66da..472f745 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/msg/LocobotJoy.msg +++ b/interbotix_ros_xseries/interbotix_xs_msgs/msg/LocobotJoy.msg @@ -43,8 +43,8 @@ int8 WAIST_CCW = 17 int8 WAIST_CW = 18 # Gripper Control -int8 GRIPPER_OPEN = 19 -int8 GRIPPER_CLOSE = 20 +int8 GRIPPER_RELEASE = 19 +int8 GRIPPER_GRASP = 20 # Pose Control int8 HOME_POSE = 21 @@ -58,7 +58,7 @@ int8 SPEED_INC = 23 int8 SPEED_DEC = 24 # Quickly toggle between a fast and slow speed setting -int8 SPEED_COURSE = 25 +int8 SPEED_COARSE = 25 int8 SPEED_FINE = 26 # Inc/Dec Gripper pressure diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/msg/TurretJoy.msg b/interbotix_ros_xseries/interbotix_xs_msgs/msg/TurretJoy.msg index f04379d..70741d6 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/msg/TurretJoy.msg +++ b/interbotix_ros_xseries/interbotix_xs_msgs/msg/TurretJoy.msg @@ -21,7 +21,7 @@ int8 SPEED_INC = 6 int8 SPEED_DEC = 7 # Quickly toggle between a fast and slow speed setting -int8 SPEED_COURSE = 8 +int8 SPEED_COARSE = 8 int8 SPEED_FINE = 9 ######################################################################################################### diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/package.xml b/interbotix_ros_xseries/interbotix_xs_msgs/package.xml index 44312af..b0c52a8 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/package.xml +++ b/interbotix_ros_xseries/interbotix_xs_msgs/package.xml @@ -1,27 +1,30 @@ - + + + interbotix_xs_msgs - 0.0.1 + 0.0.0 - interbotix_xs_msgs provides messages for the Interbotix X-Series family - of products. + interbotix_xs_msgs provides messages for the Interbotix X-Series family of products. - Luke Schmitt Luke Schmitt BSD + Luke Schmitt - catkin + ament_cmake - message_generation - std_msgs - trajectory_msgs - geometry_msgs + std_msgs + trajectory_msgs - message_runtime - std_msgs - trajectory_msgs - geometry_msgs + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_cmake - + ament_cmake diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/srv/RobotInfo.srv b/interbotix_ros_xseries/interbotix_xs_msgs/srv/RobotInfo.srv index 33b4450..05f9bc9 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/srv/RobotInfo.srv +++ b/interbotix_ros_xseries/interbotix_xs_msgs/srv/RobotInfo.srv @@ -15,3 +15,4 @@ float32[] joint_velocity_limits # the velocity limit [rad/s] for each j float32[] joint_sleep_positions # the sleep position [rad] for each joint in a group or for the specified joint int16[] joint_state_indices # index for each joint in a group or for the specified joint in the published JointState message int16 num_joints # the number of joints in a group or 1 +string[] name # the name of the group or joint requested; if group was 'all', returns the names of all groups diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/99-interbotix-udev.rules b/interbotix_ros_xseries/interbotix_xs_sdk/99-interbotix-udev.rules index 72cbc8f..8528d03 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/99-interbotix-udev.rules +++ b/interbotix_ros_xseries/interbotix_xs_sdk/99-interbotix-udev.rules @@ -22,3 +22,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{seria # LifeCam Cinema SUBSYSTEM=="video4linux", ATTRS{idVendor}=="045e", ATTRS{idProduct}=="0812", ATTR{index}=="0", SYMLINK+="lifecam" + +# Footswitch +ACTION!="remove", KERNEL=="event[0-9]*", ENV{ID_VENDOR_ID}=="3553", ENV{ID_MODEL_ID}=="b001", ENV{LIBINPUT_IGNORE_DEVICE}="1" diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/CMakeLists.txt b/interbotix_ros_xseries/interbotix_xs_sdk/CMakeLists.txt index f51b355..d6136f8 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/CMakeLists.txt +++ b/interbotix_ros_xseries/interbotix_xs_sdk/CMakeLists.txt @@ -1,88 +1,67 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(interbotix_xs_sdk) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - interbotix_xs_msgs - actionlib - control_msgs - dynamixel_workbench_toolbox - message_generation - roscpp - sensor_msgs - urdf -) +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(interbotix_xs_msgs REQUIRED) +find_package(interbotix_xs_driver REQUIRED) +find_package(yaml-cpp REQUIRED) -# Resolve system dependency on yaml-cpp, which apparently does not -# provide a CMake find_package() module. -find_package(PkgConfig REQUIRED) -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -find_path(YAML_CPP_INCLUDE_DIR - NAMES yaml_cpp.h - PATHS ${YAML_CPP_INCLUDE_DIRS} -) -find_library(YAML_CPP_LIBRARY - NAMES YAML_CPP - PATHS ${YAML_CPP_LIBRARY_DIRS} -) -link_directories(${YAML_CPP_LIBRARY_DIRS}) +include_directories(include) -if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") -add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS interbotix_xs_msgs actionlib dynamixel_workbench_toolbox message_runtime roscpp sensor_msgs urdf +set(ROS_DEPENDENCIES + ament_index_cpp + rclcpp + sensor_msgs + urdf + interbotix_xs_msgs + interbotix_xs_driver ) ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/xs_sdk_obj.cpp -) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Declare a C++ executable ## Specify libraries to link a library or executable target against -## Add cmake target dependencies of the executable add_executable(xs_sdk src/xs_sdk.cpp src/xs_sdk_obj.cpp) -add_dependencies(xs_sdk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(xs_sdk ${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) +ament_target_dependencies(xs_sdk ${ROS_DEPENDENCIES}) +target_link_libraries(xs_sdk yaml-cpp) ############# ## Install ## ############# -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS - scripts/xs_sdk_sim - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS xs_sdk + DESTINATION lib/${PROJECT_NAME} +) + +install( + PROGRAMS scripts/xs_sdk_sim.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/README.md b/interbotix_ros_xseries/interbotix_xs_sdk/README.md index 6b33b15..07780cf 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/README.md +++ b/interbotix_ros_xseries/interbotix_xs_sdk/README.md @@ -2,11 +2,11 @@ ![xs_sdk_banner](images/xs_sdk_banner.png) ## Overview -This package contains two driver nodes responsible for controlling the many X-Series platforms sold by Interbotix. Descriptions for both of them are below. +This package contains two driver nodes responsible for controlling the many Interbotix X-Series platforms sold by Trossen Robotics. Descriptions for both of them are below. -The first one is loosely based on the [dynamixel_workbench_controllers.cpp](https://github.com/ROBOTIS-GIT/dynamixel-workbench/blob/master/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp) file created by [ROBOTIS](http://www.robotis.us/) to control a slew of their Dynamixel servo motors. Also written in C++, the node presents a ROS interface that is built on top of the [dynamixel_workbench_toolbox](https://github.com/ROBOTIS-GIT/dynamixel-workbench/tree/master/dynamixel_workbench_toolbox), a C++ library provided by ROBOTIS containing useful functions that utilize their communication protocol. Feel free to take a look at their [API](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/#api-references) to get a better understanding of how their functions work. Also, take a look at the *interbotix_xs_sdk*'s header file [here](include/interbotix_xs_sdk/xs_sdk_obj.h) which contains the various ROS Publishers, Subscribers, Timers, etc... all fully documented. In general, there are two ways this node can be used. One is to command the robot via the ROS topics and/or services. In this manner, the developer can code in any language that is capable of sending a ROS message. The other approach is to 'skip' the ROS topic layer and use the publicly available functions directly. All the user would need to do is create an instance of the 'InterbotixRobotXS' class as shown [here](src/xs_sdk.cpp) to take advantage of these functions. +The **xs_sdk** node is a ROS wrapper for the [Interbotix X-Series Driver](https://github.com/Interbotix/interbotix_xs_driver). This allows robots based on the X-Series servos to be controlled through higher-level ROS commands. The X-Series driver is loosely based on the [dynamixel_workbench_controllers.cpp](https://github.com/ROBOTIS-GIT/dynamixel-workbench/blob/master/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp) file created by [ROBOTIS](http://www.robotis.us/). Feel free to take a look at their [API](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/#api-references) to get a better understanding of how their functions work. Also, take a look at the *interbotix_xs_sdk*'s header file [here](include/interbotix_xs_sdk/xs_sdk_obj.hpp) which contains the fully documented ROS features like publishers, subscribers, timers, and services. -The second driver node is meant to simulate control of Dynamixel servos using RViz. It contains the same ROS interfaces as the actual driver and can be used to 'test-out' code before working with the physical motors. All higher level code or ROS packages can stay exactly the same (no code needs to be changed to make them compatible with the simulator). It is written in Python, and the documented code can be found [here](scripts/xs_sdk_sim). It does have some limitations which are described in the 'Structure' section below. +The **xs_sdk_sim** node is meant to simulate control of DYNAMIXEL servos which can then be visualized RViz. It contains the same ROS interfaces as the actual driver and can be used to 'test-out' code before working with the physical motors. All higher level code or ROS packages can stay exactly the same (no code needs to be changed to make them compatible with the simulator). It is written in Python, and the documented code can be found [here](scripts/xs_sdk_sim.py). It does have some limitations which are described in the 'Structure' section below. ## Structure @@ -55,8 +55,9 @@ The *interbotix_xs_sdk* package contains the actual driver node called **xs_sdk* - `motor_configs` - the file path to the 'motor config' YAML file; refer to the [Motor Config Template](config/motor_configs_template.yaml) file for details. - `mode_configs` - the file path to the 'mode config' YAML file; refer to the [Mode Config Template](config/mode_configs_template.yaml) file for details. - `load_configs` - a boolean that specifies whether or not the initial register values (under the 'motors' heading) in a motor config file should be written to the motors; as the values being written are stored in each motor's EEPROM (which means the values are retained even after a power cycle), this can be set to 'false' after the first time using the robot. Setting to 'false' also shortens the node startup time by a few seconds and preserves the life of the EEPROM. +- `xs_driver_logging_level` - a string value of `DEBUG`, `INFO`, `WARN`, `ERROR`, or `FATAL` that sets the logging level of the X-Series Driver. - **Simulation:** the `load_configs` parameter is unused. + **Simulation:** the `load_configs` and `xs_driver_logging_level` parameters are unused. ## Usage diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/TROUBLESHOOTING.md b/interbotix_ros_xseries/interbotix_xs_sdk/TROUBLESHOOTING.md index 57d2537..0d3b251 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/TROUBLESHOOTING.md +++ b/interbotix_ros_xseries/interbotix_xs_sdk/TROUBLESHOOTING.md @@ -1,77 +1,3 @@ # Troubleshooting a Dynamixel-based Robot -This guide walks a user through possible issues that can occur when using a Dynamixel-based robot and how to fix them. If, after following this guide, the issues persists, feel free to post an Issue on GitHub or send an email to trsupport@trossenrobotics.com. Also, please note that it is assumed that the robots are being directly used with a Linux machine (either Ubuntu 18.04 or 20.04), not on any other operating system (ex. Mac, Windows) or in a virtual machine. -## Dynamixel Wizard 2.0 Tool -For debugging purposes, we highly recommend to install the [Dynamixel Wizard 2.0](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#install-linux) tool created by ROBOTIS (our motor supplier). This tool makes it easy to find and view the registers of all the motors attached to the U2D2 from a graphical interface. It also allows you to set registers as well. After following the installation instructions, open up the Wizard, and click the *Options* button. A window similar to the one below should pop up. - -

- -

- -All of our X-Series robots use motors with Protocol 2.0 and a baud-rate set to 1000000 bps. Additionally, none of our robots currently use more than 36 motors. As such, to properly scan for the motors in your robot, you should at least check the -- Protocol 2.0 checkbox -- the desired USB port (typically ttyUSBO), -- Baudrate of 1000000 bps -- Scanning range from 0 - 36 - -After configuring the options, press 'OK' and hit the 'Scan' button. The Dynamixels should appear on the left-hand pane of the Wizard. If some motors are missing though, open up the 'Options' tab again and additionally check the -- Protocol 1.0 checkbox -- Baudrate of 57600 bps - -Then press 'OK' and hit the 'Scan' button again. The reasoning behind this last step is that ROBOTIS by default ships all new motors with baudrates of 57600 bps and IDs of 1. At Interbotix, we try to change the baudrates to 1000000 bps, but occasionally a motor could slip through the system. If that's the case with you, the baud rate can easily be changed by clicking the 'Baud Rate (Bus)' register, selecting the '1 Mbps' option in the pane on the lower right corner, and clicking 'Save'. - -## Common Issues - -##### Red LED Flashing on motor(s) -If a motor's LED is flashing roughly once per second, this indicates that it is in an error state. In this state, the motor will torque off and not respond to commands. Two common reasons this can happen is if it is 'overloaded' or 'overheated'. Overloading can occur if a motor is commanded to go to a position, but physically cannot due to an obstacle being in the way. This typically comes up if doing position control with a two-fingered gripper. If the gripper is commanded to *fully* close around a small rigid object, the motor will stall and after a few seconds, go into an error state. Thus, if doing position control with a gripper, make sure to close the gripper just enough to grasp the object but not more. Alternatively, use PWM or Current control. - -Similarly, overheating can occur if commanding high PWM or current values to a motor for a long period of time. To counteract this, either send lower commands or shorten the time that high commands are sent. - -In any event, to fix this error, either power cycle the motors (unplug/replug the power cable to the motor hub), or call the `reboot_motors` ROS service for those particular motors. The advantage of doing the second approach is that the ROS session does not have to be shut down beforehand. - -##### Can't find Dynamixel ID -If you see a red error message in the terminal after launching the ROS driver node that says `[xs_sdk] Can't find Dynamixel ID '1', Joint Name : 'waist'` or similar, don't panic! All the node is trying to say is that it tried to ping the motor with that ID and joint name, but never received a response. This could happen due to a few reasons: - -- *One or more motors are in an error state* - If any motor has a red flashing LED, the ROS node will not be able to find it during startup. In this case, unplug/replug power from from the motor hub and try again - -- *Motor has an incorrect baud rate* - The ROS driver node expects all motors to be set to communicate at 1 Mbps. However, it's possible that the motor specified in the error message has an incorrect baudrate (probably 57600 bps as that's the default ROBOTIS ships them with). To fix this, head up to the **Dynamixel Wizard 2.0 Tool** section above to learn how to set the correct baudrate. - -- *No power* - In order for the ROS node to find the motors, let alone move them, the motor hub must be plugged in to a 12V power supply (that can deliver at least 1A). For those connecting your 12V power supply to power strips, verify that the power strip is connected to an outlet and is turned on! If using a battery, make sure that it still has enough juice. - -- *Loose Cable* - Verify that all 3-pin cables chaining the Dynamixels together (including the U2D2 and power hub) are snugly fit into their housings. If not, use a finger to press them in, and try again. As an FYI, if using an X-Series arm, this issue is most prevalent with the cable connecting the 'wrist_rotate' motor to the 'gripper' motor, so make sure to check there first. - -##### Failed to open port at /dev/ttyDXL -If you see a red error message in the terminal after launching the ROS driver node that says `Failed to open port at /dev/ttyDXL`, that means the computer cannot find the U2D2 device. This could happen due to a couple of reasons. - -- *MicroUSB cable is not plugged in* - Verify that you have actually connected the U2D2 to your computer with a microUSB cable. Then try again. - -- *Port is busy* - If you have the Dynamixel Wizard 2.0 tool open and connected to the U2D2, make sure to either click the *Disconnect* button on the top left of the window or close out of the Wizard before running the ROS driver node. This way the port won't be busy when the driver node is run. - -##### Incorrect status packet -If you see 4 repeating red error messages in the terminal during node operation that say... -``` -[TxRxResult] Incorrect status packet! -groupSyncRead getdata failed -groupSyncRead getdata failed -groupSyncRead getdata failed -``` - -... that means the computer failed to read some of the registers on the motors (typically, it's the Present Position, Present_Velocity, and Present_Current/Present_Load registers as these are read constantly so that the joint state topic can be updated). This could happen if the microUSB cable connecting the computer to the U2D2 is not plugged in fully, or if one or more of the 3-pin cables connecting the Dynamixels together become loose during operation. To stop these errors, just push in all loose cables. - -## Less Common Issues - -##### U2D2 not recognized (no symlink created) -You may run into an issue where the computer is able to detect that it is connected to a device via `lsusb`, but no entry is created in the /dev directory. One possible reason may be that the FTDI drivers are out of date or not compatible with the U2D2 device. To fix this issue, download the latest recommended [VCP drivers](https://ftdichip.com/drivers/vcp-drivers/) for Linux. Then reload and install the drivers by navigating to the unpacked driver and running the following: -```bash -$ sudo modprobe ftdi_sio -``` - -## Helpful Tips - -##### Increase Motor Accuracy -One of the awesome things about Protocol 2.0 supporting Dynamixels is their internal tunable feedback-controllers (either PID for position control or PI for velocity control). Default values are assigned to the PID gains, but if they are not strong enough, one can simply increase their values. Specifically, it is recommended to increase the Position_P_Gain register as many of our products work in the position operating mode. However, this register should not be increased too much (more than a few hundred points above its default) or it could cause the motor to overload. - -##### Range Of Motion Check -For some of our robots, (like arms or turrets), the waist/pan motor has a limit of -180/180 degrees. To make this range feasible, the 3-pin cable connecting the waist/pan motor to the shoulder/tilt motor is extra long. As a result, it is quite possible for someone to manually turn this joint 360 degrees in an untorqued state, and then end up trying to operate the robot in that state. This could lead to quite a few issues - like the cable being pulled out during operation, or the cable physically preventing the robot from rotating. - -To fix this, we recommend cautiously flipping the robot upside down to look into the base cavity (as it's hollow). It should be readily apparent by looking at the 3-pin cable going from the waist/pan motor to the shoulder/tilt motor if it's wound up or straight. If it's wound up, carefully rotate the waist/pan motor in the direction that 'straightens' the cable. Alternatively, you can check the 'straightness' of the cable without flipping the robot by checking its tautness from the hole by the shoulder/tilt motors. If it feels too taught, rotate the waist/pan motor in the direction that loosens it. +See the [Troubleshooting Guide](https://docs.trossenrobotics.com/interbotix_xsarms_docs/troubleshooting.html) on the Interbotix X-Series Arms documentation site. diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/config/motor_configs_template.yaml b/interbotix_ros_xseries/interbotix_xs_sdk/config/motor_configs_template.yaml index ecebe27..c97ebe6 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/config/motor_configs_template.yaml +++ b/interbotix_ros_xseries/interbotix_xs_sdk/config/motor_configs_template.yaml @@ -39,16 +39,22 @@ joint_state_publisher: update_rate: 100 # Optional; rate at which the motor states are sampled (and potentially published) in Hz; defaults to '100' publish_states: true # Optional; boolean that either enables or disables the Publisher; defaults to 'true' topic_name: joint_states # Optional; desired JointState topic name; defaults to 'joint_states' + read_failure_behavior: 0 # Optional; the behavior on joint state read failures; defaults to 0 + # 0: Publishes whatever is given from the dxl_wb for all joints states (-pi) + # 1: Publishes NaN values for all joints groups: # Optional; defines an unlimited number of joint groups in the robot; custom 'group' names can be used. arm: [waist, shoulder, elbow, wrist_angle, wrist_rotate] # Optional; groups can contain overlapping joints but those joints will retain the operating mode of the latter group. # Note that 'all' is a special group name that contains all the joints in the 'joint_order' sequence. grippers: # Optional; defines an unlimited number of Interbotix grippers; custom 'gripper' names can be used. gripper: # Example gripper name + type: "swing_arm" # Optional; type of gripper mechanism used i.e('swing_arm' or 'rack_and_pinion'); defaults to 'swing_arm' horn_radius: 0.022 # Optional; length in meters from the middle of the gripper servo horn to its edge; defaults to '0.014' + pitch_radius: 0.0127 # Optional; length in meters radius of pinion; defaults to '0.0127' arm_length: 0.036 # Optional; length in meters from the end of the servo horn to a finger; defaults to '0.024' left_finger: left_finger # Optional; name of the 'left_finger' joint; this must match the name in the URDF (if present); defaults to 'left_finger' right_finger: right_finger # Optional; name of the 'right_finger' joint; this must match the name in the URDF (if present); defaults to 'right_finger' + calibrate: false # Optional; flag to calibrate gripper; defaults to 'false' shadows: # Optional; list of any 'master' motors and all of its shadows (usually 1, but could be unlimited) shoulder: diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.h b/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.h deleted file mode 100644 index 62e9b96..0000000 --- a/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.h +++ /dev/null @@ -1,322 +0,0 @@ -#ifndef XS_SDK_OBJ_H_ -#define XS_SDK_OBJ_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "interbotix_xs_msgs/Reboot.h" -#include "interbotix_xs_msgs/RobotInfo.h" -#include "interbotix_xs_msgs/MotorGains.h" -#include "interbotix_xs_msgs/TorqueEnable.h" -#include "interbotix_xs_msgs/OperatingModes.h" -#include "interbotix_xs_msgs/RegisterValues.h" -#include "interbotix_xs_msgs/JointGroupCommand.h" -#include "interbotix_xs_msgs/JointSingleCommand.h" -#include "interbotix_xs_msgs/JointTrajectoryCommand.h" - -#define BAUDRATE 1000000 // All motors are preset to 1M baud -#define PORT "/dev/ttyDXL" // Udev rule creates a symlink with this name -#define SYNC_WRITE_HANDLER_FOR_GOAL_POSITION 0 // Write goal positions [rad] to multiple motors at the same time -#define SYNC_WRITE_HANDLER_FOR_GOAL_VELOCITY 1 // Write goal velocities [rad/s] to multiple motors at the same time -#define SYNC_WRITE_HANDLER_FOR_GOAL_CURRENT 2 // Write goal currents [mA] to multiple motors at the same time -#define SYNC_WRITE_HANDLER_FOR_GOAL_PWM 3 // Write goal pwms to multiple motors at the same time -#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 // Read current joint states for multiple motors at the same time - -static const std::string OP_MODE = "position"; // Default motor operating mode is 'position' -static const std::string PROFILE_TYPE = "velocity"; // Default motor profile type is 'velocity' (as opposed to 'time') -static const int32_t PROFILE_VELOCITY = 0; // Allow joint velocity to be infinite when in position control mode - makes robot very reactive to joint commands -static const int32_t PROFILE_ACCELERATION = 0; // Allow joint acceleration to be infinite when in position control mode - makes robot very reactive to joint commands -static const bool TORQUE_ENABLE = true; // Torque motor on by default -static bool LOAD_CONFIGS = true; // Get motor configurations by default - -struct JointGroup // Struct to hold multiple joints that represent a group -{ - std::vector joint_names; // Names of all joints in the group - std::vector joint_ids; // Dynamixel ID of all joints in the group - uint8_t joint_num; // Number of joints in the group - std::string mode; // Operating Mode for all joints in the group - std::string profile_type; // Profile Type ('velocity' or 'time') for all joints in the group -}; - -struct MotorState // Struct to hold data on a single motor -{ - uint8_t motor_id; // Dynamixel ID of the motor - std::string mode; // Operating Mode of the motor - std::string profile_type; // Profile Type ('velocity' or 'time') for the motor -}; - -struct Gripper // Struct to hold data on an Interbotix Gripper -{ - size_t js_index; // Index in the published JointState message 'name' list belonging to the gripper motor - float horn_radius; // Distance [m] from the motor horn's center to its edge - float arm_length; // Distance [m] from the edge of the motor horn to a finger - std::string left_finger; // Name of the 'left_finger' joint as defined in the URDF (if present) - std::string right_finger; // Name of the 'right_finger' joint as defined in the URDF (if present) -}; - -struct MotorInfo // Struct to hold a desired register value for a given motor -{ - uint8_t motor_id; // Dynamixel ID of a motor - std::string reg; // Register name - int32_t value; // Value to write to the above register for the specified motor -}; - -// Interbotix Core Class to build any type of Dynamixel-based robot -class InterbotixRobotXS -{ -public: - - /// @brief Constructor for the InterbotixRobotXS - /// @param node_handle - ROS NodeHandle - /// @param success [out] - bool indicating if the node launched successfully - explicit InterbotixRobotXS(ros::NodeHandle *node_handle, bool &success); - - /// @brief Destructor for the InterbotixRobotXS - ~InterbotixRobotXS(); - - /// @brief Set the operating mode for a specific group of motors or a single motor - /// @param cmd_type - set to 'group' if changing the operating mode for a group of motors or 'single' if changing the operating mode for a single motor - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') - /// @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (modifies Bit 2 of the 'Drive_Mode' register) - /// @param profile_velocity - passthrough to the Profile_Velocity register on the motor - /// @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor - void robot_set_operating_modes(std::string const& cmd_type, std::string const& name, std::string const& mode, std::string const& profile_type=PROFILE_TYPE, int32_t profile_velocity=PROFILE_VELOCITY, int32_t profile_acceleration=PROFILE_ACCELERATION); - - /// @brief Helper function used to set the operating mode for a single motor - /// @param name - desired motor name - /// @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') - /// @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (modifies Bit 2 of the 'Drive_Mode' register) - /// @param profile_velocity - passthrough to the Profile_Velocity register on the motor - /// @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor - void robot_set_joint_operating_mode(std::string const& name, std::string const& mode, std::string const& profile_type=PROFILE_TYPE, int32_t profile_velocity=PROFILE_VELOCITY, int32_t profile_acceleration=PROFILE_ACCELERATION); - - /// @brief Torque On/Off a specific group of motors or a single motor - /// @param cmd_type - set to 'group' if torquing off a group of motors or 'single' if torquing off a single motor - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param enable - set to True to torque on or False to torque off - void robot_torque_enable(std::string const& cmd_type, std::string const& name, bool const& enable); - - /// @brief Reboot a specific group of motors or a single motor - /// @param cmd_type - set to 'group' if rebooting a group of motors or 'single' if rebooting a single motor - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param torque_enable - set to True to torque on or False to torque off after rebooting - /// @param smart_reboot - set to True to only reboot motor(s) in a specified group that have gone into an error state - void robot_reboot_motors(std::string const& cmd_type, std::string const& name, bool const& enable, bool const& smart_reboot); - - /// @brief Command a desired group of motors with the specified commands - /// @param name - desired motor group name - /// @param commands - vector of commands (order matches the order specified in the 'groups' section in the motor config file) - /// @details - commands are processed differently based on the operating mode specified for the motor group - void robot_write_commands(std::string const& name, std::vector commands); - - /// @brief Command a desired motor with the specified command - /// @param name - desired motor name - /// @param command - motor command - /// @details - the command is processed differently based on the operating mode specified for the motor - void robot_write_joint_command(std::string const& name, float command); - - /// @brief Set motor firmware PID gains - /// @param cmd_type - set to 'group' if changing the PID gains for a group of motors or 'single' if changing the PID gains for a single motor - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param gains - vector containing the desired PID gains - order is as shown in the function - void robot_set_motor_pid_gains(std::string const& cmd_type, std::string const& name, std::vector const& gains); - - /// @brief Set a register value to multiple motors - /// @param cmd_type - set to 'group' if setting register values for a group of motors or 'single' if setting a single register value - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param value - desired register value - void robot_set_motor_registers(std::string const& cmd_type, std::string const& name, std::string const& reg, int32_t const& value); - - /// @brief Get a register value from multiple motors - /// @param cmd_type - set to 'group' if getting register values from a group of motors or 'single' if getting a single register value - /// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - /// @param values [out] - vector of register values - void robot_get_motor_registers(std::string const& cmd_type, std::string const& name, std::string const& reg, std::vector &values); - - /// @brief Get states for a group of joints - /// @param name - desired joint group name - /// @param positions [out] - vector of current joint positions [rad] - /// @param velocities [out] - vector of current joint velocities [rad/s] - /// @param effort [out] - vector of current joint effort [mA] - void robot_get_joint_states(std::string const& name, std::vector *positions=NULL, std::vector *velocities=NULL, std::vector *effort=NULL); - - /// @brief Get states for a single joint - /// @param name - desired joint name - /// @param position [out] - current joint position [rad] - /// @param velocity [out] - current joint velocity [rad/s] - /// @param effort [out] - current joint effort [mA] - void robot_get_joint_state(std::string const& name, float *position=NULL, float *velocity=NULL, float *effort=NULL); - - /// @brief Converts a desired linear distance between two gripper fingers into an angular position - /// @param name - name of the gripper servo to command - /// @param linear_position - desired distance [m] between the two gripper fingers - /// @param [out] - angular position [rad] that achieves the desired linear distance - float robot_convert_linear_position_to_radian(std::string const& name, float const& linear_position); - - /// @brief Converts a specified angular position into the linear distance from one gripper finger to the center of the gripper servo horn - /// @param name - name of the gripper sevo to command - /// @param angular_position - desired gripper angular position [rad] - /// @param [out] - linear position [m] from a gripper finger to the center of the gripper servo horn - float robot_convert_angular_position_to_linear(std::string const& name, float const& angular_position); - -private: - - int timer_hz; // Frequency at which the ROS Timer publishing joint states should run - bool pub_states; // Boolean that determines if joint states should be published; - bool execute_joint_traj; // Boolean that changes value when a JointTrajectoryCommand begins and ends execution - JointGroup *all_ptr; // Pointer to the 'all' group (makes updating joint states more efficient) - DynamixelWorkbench dxl_wb; // DynamixelWorkbench object used to easily communicate with any Dynamixel servo - YAML::Node motor_configs; // Holds all the information in the motor_configs YAML file - YAML::Node mode_configs; // Holds all the information in the mode_configs YAML file - - ros::NodeHandle node; // ROS Node handle - ros::Publisher pub_joint_states; // ROS Publisher responsible for publishing joint states - ros::Subscriber sub_command_group; // ROS Subscriber responsible for subscribing to JointGroupCommand messages - ros::Subscriber sub_command_single; // ROS Subscriber responsible for subscribing to JointSingleCommand messages - ros::Subscriber sub_command_traj; // ROS Subscriber responsible for subscribing to JointTrajectoryCommand messages - ros::ServiceServer srv_motor_gains; // ROS Service Server used to set motor PID gains - ros::ServiceServer srv_operating_modes; // ROS Service Server used to set motor operating modes - ros::ServiceServer srv_set_registers; // ROS Service Server used to set any motor register - ros::ServiceServer srv_get_registers; // ROS Service Server used to get any motor register - ros::ServiceServer srv_get_robot_info; // ROS Service Server used to get general information about the robot (like limits) - ros::ServiceServer srv_torque_enable; // ROS Service Server used to torque on/off any motor - ros::ServiceServer srv_reboot_motors; // ROS Service Server used to reboot any motor - ros::Timer tmr_joint_states; // ROS Timer used to continuously publish joint states - ros::Timer tmr_joint_traj; // ROS One-Shot Timer used when commanding motor trajectories - sensor_msgs::JointState joint_states; // Holds the most recent JointState message - interbotix_xs_msgs::JointTrajectoryCommand joint_traj_cmd; // Holds the most recent JointTrajectoryCommand message - - std::string port; // Holds the USB port name that connects to the U2D2 - std::string js_topic; // Desired JointState topic name - std::vector motor_info_vec; // Vector containing all the desired EEPROM register values to command the motors at startup - std::vector gripper_order; // Vector containing the order in which multiple grippers (if present) are published in the JointState message - std::unordered_map control_items; // Dictionary mapping register names to information about them (like 'address' and expected 'data length') - std::unordered_map sleep_map; // Dictionary mapping a joint's name with its 'sleep' position - std::unordered_map group_map; // Dictionary mapping a joint group's name with information about it (as defined in the JointGroup struct) - std::unordered_map motor_map; // Dictionary mapping a motor's name with information about it (as defined in the MotorState struct) - std::unordered_map> shadow_map; // Dictionary mapping a motor's name with the names of its shadows - including itself - std::unordered_map> sister_map; // Dictionary mapping the name of either servo in a 2in1 motor with the other one (henceforth known as 'sister') - std::unordered_map gripper_map; // Dictionary mapping the name of a gripper motor with information about it (as defined in the Gripper struct) - std::unordered_map js_index_map; // Dictionary mapping the name of a joint with its position in the JointState 'name' list - - /// @brief Loads a robot-specific 'motor_configs' yaml file and populates class variables with its contents - /// @param [out] - True if the motor configs were successfully retrieved; False otherwise - bool robot_get_motor_configs(void); - - /// @brief Initializes the port to communicate with the Dynamixel servos - /// @param [out] - True if the port was successfully opened; False otherwise - bool robot_init_port(void); - - /// @brief Pings all motors to make sure they can be found - /// @param [out] - True if all motors were found; False otherwise - bool robot_ping_motors(void); - - /// @brief Writes some 'startup' EEPROM register values to the Dynamixel servos - /// @param [out] - True if all register values were written successfully; False otherwise - bool robot_load_motor_configs(void); - - /// @brief Retrieves information about 'Goal_XXX' and 'Present_XXX' registers - /// @details - Info includes a register's name, address, and data length - void robot_init_controlItems(void); - - /// @brief Creates SyncWrite and SyncRead Handlers to write/read data to multiple motors simultaneously - void robot_init_SDK_handlers(void); - - /// @brief Loads a 'mode_configs' yaml file containing desired operating modes and sets up the motors accordingly - void robot_init_operating_modes(void); - - /// @brief Initialize ROS Publishers - void robot_init_publishers(void); - - /// @brief Initialize ROS Subscribers - void robot_init_subscribers(void); - - /// @brief Initialize ROS Services - void robot_init_services(void); - - /// @brief Initialize ROS Timers - void robot_init_timers(void); - - /// @brief Waits until first JointState message is received - void robot_wait_for_joint_states(void); - - /// @brief ROS Subscriber callback function to command a group of joints - /// @param msg - JointGroupCommand message dictating the joint group to command along with the actual commands - /// @details - refer to the message definition for details - void robot_sub_command_group(const interbotix_xs_msgs::JointGroupCommand &msg); - - /// @brief ROS Subscriber callback function to command a single joint - /// @param msg - JointSingleCommand message dictating the joint to command along with the actual command - /// @details - refer to the message definition for details - void robot_sub_command_single(const interbotix_xs_msgs::JointSingleCommand &msg); - - /// @brief ROS Subscriber callback function to command a joint trajectory - /// @param msg - JointTrajectoryCommand message dictating the joint(s) to command along with the desired trajectory - /// @details - refer to the message definition for details - void robot_sub_command_traj(const interbotix_xs_msgs::JointTrajectoryCommand &msg); - - /// @brief ROS Service to torque the joints on the robot on/off - /// @param req - TorqueEnable service message request - /// @param res [out] - TorqueEnable service message response [unused] - /// @details - refer to the service definition for details - bool robot_srv_torque_enable(interbotix_xs_msgs::TorqueEnable::Request &req, interbotix_xs_msgs::TorqueEnable::Response &res); - - /// @brief ROS Service to reboot the motors on the robot - /// @param req - Reboot service message request - /// @param res [out] - Reboot service message response [unused] - /// @details - refer to the service definition for details - bool robot_srv_reboot_motors(interbotix_xs_msgs::Reboot::Request &req, interbotix_xs_msgs::Reboot::Response &res); - - /// @brief ROS Service that allows the user to get information about the robot - /// @param req - RobotInfo service message request - /// @param res [out] - RobotInfo service message response - /// @details - refer to the service definition for details - bool robot_srv_get_robot_info(interbotix_xs_msgs::RobotInfo::Request &req, interbotix_xs_msgs::RobotInfo::Response &res); - - /// @brief ROS Service that allows the user to change operating modes - /// @param req - OperatingModes service message request - /// @param res [out] - OperatingModes service message response [unused] - /// @details - refer to the service definition for details - bool robot_srv_set_operating_modes(interbotix_xs_msgs::OperatingModes::Request &req, interbotix_xs_msgs::OperatingModes::Response &res); - - /// @brief ROS Service that allows the user to set the motor firmware PID gains - /// @param req - MotorGains service message request - /// @param res [out] - MotorGains service message response [unused] - /// @details - refer to the service defintion for details - bool robot_srv_set_motor_pid_gains(interbotix_xs_msgs::MotorGains::Request &req, interbotix_xs_msgs::MotorGains::Response &res); - - /// @brief ROS Service that allows the user to change a specific register to a specific value for multiple motors - /// @param req - RegisterValues service message request - /// @param res [out] - RegisterValues service message response [unused] - /// @details - refer to the service definition for details - bool robot_srv_set_motor_registers(interbotix_xs_msgs::RegisterValues::Request &req, interbotix_xs_msgs::RegisterValues::Response &res); - - /// @brief ROS Service that allows the user to read a specific register on multiple motors - /// @param req - RegisterValues service message request - /// @param res [out] - RegisterValues service message response - /// @details - refer to the service definition for details - bool robot_srv_get_motor_registers(interbotix_xs_msgs::RegisterValues::Request &req, interbotix_xs_msgs::RegisterValues::Response &res); - - /// @brief Checks service call requests for validity - /// @param cmd_type request cmd_type field - /// @param name request name field - /// @returns true if the service call request is valid, false otherwise - /// @details cmd_type must be 'single' or 'group'; name must be in the group_map or motor_map - bool robot_srv_validate(const std::string &cmd_type, std::string &name); - - /// @brief ROS One-Shot Timer used to step through a commanded joint trajectory - /// @param e - TimerEvent message [unused] - void robot_execute_trajectory(const ros::TimerEvent &e); - - /// @brief ROS Timer that reads current states from all the motors and publishes them to the joint_states topic - /// @param e - TimerEvent message [unused] - void robot_update_joint_states(const ros::TimerEvent &e); -}; - -#endif diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.hpp b/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.hpp new file mode 100644 index 0000000..1451253 --- /dev/null +++ b/interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.hpp @@ -0,0 +1,287 @@ +// Copyright 2022 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERBOTIX_XS_SDK__XS_SDK_OBJ_HPP_ +#define INTERBOTIX_XS_SDK__XS_SDK_OBJ_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "urdf/model.h" +#include "yaml-cpp/yaml.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "interbotix_xs_driver/xs_common.hpp" +#include "interbotix_xs_driver/xs_driver.hpp" +#include "interbotix_xs_driver/xs_logging.hpp" + +#include "interbotix_xs_msgs/srv/reboot.hpp" +#include "interbotix_xs_msgs/srv/robot_info.hpp" +#include "interbotix_xs_msgs/srv/motor_gains.hpp" +#include "interbotix_xs_msgs/srv/torque_enable.hpp" +#include "interbotix_xs_msgs/srv/operating_modes.hpp" +#include "interbotix_xs_msgs/srv/register_values.hpp" +#include "interbotix_xs_msgs/msg/joint_group_command.hpp" +#include "interbotix_xs_msgs/msg/joint_single_command.hpp" +#include "interbotix_xs_msgs/msg/joint_trajectory_command.hpp" + +namespace interbotix_xs +{ + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("interbotix_xs_sdk.xs_sdk"); + +// simplify message and service usage +using MotorGains = interbotix_xs_msgs::srv::MotorGains; +using OperatingModes = interbotix_xs_msgs::srv::OperatingModes; +using Reboot = interbotix_xs_msgs::srv::Reboot; +using RegisterValues = interbotix_xs_msgs::srv::RegisterValues; +using RobotInfo = interbotix_xs_msgs::srv::RobotInfo; +using TorqueEnable = interbotix_xs_msgs::srv::TorqueEnable; +using JointGroupCommand = interbotix_xs_msgs::msg::JointGroupCommand; +using JointSingleCommand = interbotix_xs_msgs::msg::JointSingleCommand; +using JointTrajectoryCommand = interbotix_xs_msgs::msg::JointTrajectoryCommand; + +/// @brief The Interbotix X-Series ROS Node +class InterbotixRobotXS : public rclcpp::Node +{ +public: + /// @brief Constructor for the InterbotixRobotXS + /// @param succes Whether or not the Node initialized properly + /// @param options ROS NodeOptions + explicit InterbotixRobotXS( + bool & success, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// @brief Destructor for the InterbotixRobotXS + ~InterbotixRobotXS(); + +private: + // Frequency at which the ROS Timer publishing joint states should run + int timer_hz; + + // Boolean that determines if joint states should be published; + bool pub_states; + + // Boolean that changes value when a JointTrajectoryCommand begins and ends execution + bool execute_joint_traj; + + // InterbotixDriverXS object used to talk to the lower-level XS Interfaces + std::unique_ptr xs_driver; + + // ROS Publisher responsible for publishing joint states + rclcpp::Publisher::SharedPtr pub_joint_states; + + // ROS Subscriber responsible for subscribing to JointGroupCommand messages + rclcpp::Subscription::SharedPtr sub_command_group; + + // ROS Subscriber responsible for subscribing to JointSingleCommand messages + rclcpp::Subscription::SharedPtr sub_command_single; + + // ROS Subscriber responsible for subscribing to JointTrajectoryCommand messages + rclcpp::Subscription::SharedPtr sub_command_traj; + + // ROS Service Server used to set motor PID gains + rclcpp::Service::SharedPtr srv_motor_gains; + + // ROS Service Server used to set motor operating modes + rclcpp::Service::SharedPtr srv_operating_modes; + + // ROS Service Server used to set any motor register + rclcpp::Service::SharedPtr srv_set_registers; + + // ROS Service Server used to get any motor register + rclcpp::Service::SharedPtr srv_get_registers; + + // ROS Service Server used to get general information about the robot (like limits) + rclcpp::Service::SharedPtr srv_get_robot_info; + + // ROS Service Server used to torque on/off any motor + rclcpp::Service::SharedPtr srv_torque_enable; + + // ROS Service Server used to reboot any motor + rclcpp::Service::SharedPtr srv_reboot_motors; + + // ROS Timer used to continuously publish joint states + rclcpp::TimerBase::SharedPtr tmr_joint_states; + + // ROS One-Shot Timer used when commanding motor trajectories + rclcpp::TimerBase::SharedPtr tmr_joint_traj; + + // Holds the most recent JointState message + sensor_msgs::msg::JointState joint_states; + + // Holds the most recent JointTrajectoryCommand message + JointTrajectoryCommand::SharedPtr joint_traj_cmd; + + // Indicating the trajectory start time + rclcpp::Time traj_start_time; + + // Trajectory counter + size_t cntr; + + // Desired JointState topic name + std::string js_topic; + + // Absolute path to the motor configs file + std::string filepath_motor_configs; + + // Absolute path to the mode configs file + std::string filepath_mode_configs; + + // Whether or not to write to the EEPROM values on startup + bool write_eeprom_on_startup; + + // The xs_driver logging level as a string (to be set from ROS parameters) + std::string xs_driver_logging_level; + + /// @brief Loads the X-Series robot driver + /// @returns True if the driver was loaded successfully, False otherwise + bool robot_init_driver(); + + /// @brief Declare all parameters needed by the node + void robot_init_parameters(); + + /// @brief Initialize ROS Publishers + void robot_init_publishers(); + + /// @brief Initialize ROS Subscribers + void robot_init_subscribers(); + + /// @brief Initialize ROS Services + void robot_init_services(); + + /// @brief Initialize ROS Timers + void robot_init_timers(); + + /// @brief Waits until first JointState message is received + void robot_wait_for_joint_states(); + + /// @brief ROS Subscriber callback function to command a group of joints + /// @param msg JointGroupCommand message dictating the joint group to command along with the + /// actual commands + /// @details refer to the message definition for details + void robot_sub_command_group(const JointGroupCommand::SharedPtr msg); + + /// @brief ROS Subscriber callback function to command a single joint + /// @param msg JointSingleCommand message dictating the joint to command along with the actual + /// command + /// @details refer to the message definition for details + void robot_sub_command_single(const JointSingleCommand::SharedPtr msg); + + /// @brief ROS Subscriber callback function to command a joint trajectory + /// @param msg JointTrajectoryCommand message dictating the joint(s) to command along with the + /// desired trajectory + /// @details refer to the message definition for details + void robot_sub_command_traj(const JointTrajectoryCommand::SharedPtr msg); + + /// @brief ROS Service to torque the joints on the robot on/off + /// @param req TorqueEnable service message request + /// @param res [out] TorqueEnable service message response [unused] + /// @details refer to the service definition for details + bool robot_srv_torque_enable( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /// @brief ROS Service to reboot the motors on the robot + /// @param req Reboot service message request + /// @param res [out] Reboot service message response [unused] + /// @details refer to the service definition for details + bool robot_srv_reboot_motors( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /// @brief ROS Service that allows the user to get information about the robot + /// @param req RobotInfo service message request + /// @param res [out] RobotInfo service message response + /// @details refer to the service definition for details + bool robot_srv_get_robot_info( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + + /// @brief ROS Service that allows the user to change operating modes + /// @param req OperatingModes service message request + /// @param res [out] OperatingModes service message response [unused] + /// @details refer to the service definition for details + bool robot_srv_set_operating_modes( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /// @brief ROS Service that allows the user to set the motor firmware PID gains + /// @param req MotorGains service message request + /// @param res [out] MotorGains service message response [unused] + /// @details refer to the service defintion for details + bool robot_srv_set_motor_pid_gains( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /// @brief ROS Service that allows the user to change a specific register to a specific value for + /// multiple motors + /// @param req RegisterValues service message request + bool robot_srv_set_motor_registers( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res); + + /// @brief ROS Service that allows the user to read a specific register on multiple motors + /// @param req RegisterValues service message request + /// @param res [out] RegisterValues service message response + /// @details refer to the service definition for details + bool robot_srv_get_motor_registers( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + + /// @brief Checks service call requests for validity + /// @param cmd_type request cmd_type field + /// @param name request name field + /// @returns true if the service call request is valid, false otherwise + /// @details cmd_type must be 'CMD_TYPE_SINGLE' or 'CMD_TYPE_GROUP'; name must be in the + /// group_map or motor_map + bool robot_srv_validate(const std::string & cmd_type, const std::string & name); + + /// @brief ROS One-Shot Timer used to step through a commanded joint trajectory + void robot_execute_trajectory(); + + /// @brief ROS Timer that reads current states from all the motors and publishes them to the + /// joint_states topic + void robot_update_joint_states(); +}; + +} // namespace interbotix_xs + +#endif // INTERBOTIX_XS_SDK__XS_SDK_OBJ_HPP_ diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/package.xml b/interbotix_ros_xseries/interbotix_xs_sdk/package.xml index 2e2d806..fcd108a 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/package.xml +++ b/interbotix_ros_xseries/interbotix_xs_sdk/package.xml @@ -1,46 +1,27 @@ - + + interbotix_xs_sdk 0.0.0 The interbotix_xs_sdk package - Solomon Wiznitzer + Luke Schmitt BSD - - Solomon Wiznitzer + Solomon Wiznitzer - - - catkin - actionlib - control_msgs - dynamixel_workbench_toolbox - message_generation - roscpp - sensor_msgs - interbotix_xs_msgs - urdf - yaml-cpp - actionlib - control_msgs - dynamixel_workbench_toolbox - message_generation - roscpp - sensor_msgs - interbotix_xs_msgs - urdf - yaml-cpp - actionlib - control_msgs - dynamixel_workbench_toolbox - message_runtime - roscpp - sensor_msgs - interbotix_xs_msgs - urdf - yaml-cpp + ament_cmake + + rclcpp + rclpy + sensor_msgs + urdf + yaml-cpp + interbotix_xs_msgs + interbotix_xs_driver + + ament_lint_auto + ament_lint_common - - + ament_cmake diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim b/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim deleted file mode 100755 index 99a9065..0000000 --- a/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim +++ /dev/null @@ -1,468 +0,0 @@ -#!/usr/bin/env python - -import math -import yaml -import rospy -import threading -import numpy as np -from interbotix_xs_msgs.msg import * -from interbotix_xs_msgs.srv import * -from urdf_parser_py.urdf import URDF -from sensor_msgs.msg import JointState - -### @brief Class that simulates the actual InterbotixRobotXS class -### @details - allows to use the same higher level ROS packages or Python API code on a simulated robot with no change to the code -class InterbotixRobotXS(object): - def __init__(self): - self.rd = None # Holds the URDF loaded from the ROS parameter server - self.timer_hz = 20 # Rate [Hz] at which the ROS Timer (in charge of publishing joint states) should run - self.js_topic = None # Joint States topic - self.move_thresh = 300 # Threshold [in milliseconds] above which desired goal positions should be split up into waypoints and simulated - self.execute_joint_traj = False # Boolean that is True if a trajectory is currently being executed; False otherwise - self.joint_states = JointState() # Current state of all joints - self.cmd_mutex = threading.Lock() # Mutex to prevent reading/writing to self.commands from different threads - self.commands = {} # Holds the desired commands for each joint - self.sleep_map = {} # Maps a joint name with its sleep position [rad] - self.group_map = {} # Maps a group name with a JointGroup-type dictionary (refer to the top of the xs_sdk_obj.h file for reference) - self.motor_map = {} # Maps a joint name with a MotorState-type dictionary (refer to the top of the xs_sdk_obj.h file for reference) - self.gripper_map = {} # Maps a gripper name with a Gripper-type dictionary (refer to the top of the xs_sdk_obj.h file for reference) - self.js_index_map = {} # Maps a joint name with its index in self.joint_states.position - self.gripper_order = [] # List of grippers as they appear in the 'joint_order' parameter in the Motor Configs YAML file - self.get_urdf_info() - self.robot_get_motor_configs() - rospy.Service('torque_enable', TorqueEnable, self.robot_srv_torque_enable) - rospy.Service('reboot_motors', Reboot, self.robot_srv_reboot_motors) - rospy.Service('get_robot_info', RobotInfo, self.robot_srv_get_robot_info) - rospy.Service('set_operating_modes', OperatingModes, self.robot_srv_set_operating_modes) - rospy.Service('set_motor_pid_gains', MotorGains, self.robot_srv_set_motor_pid_gains) - rospy.Service('set_motor_registers', RegisterValues, self.robot_srv_set_motor_registers) - rospy.Service('get_motor_registers', RegisterValues, self.robot_srv_get_motor_registers) - rospy.Subscriber('commands/joint_group', JointGroupCommand, self.robot_sub_command_group) - rospy.Subscriber('commands/joint_single', JointSingleCommand, self.robot_sub_command_single) - rospy.Subscriber('commands/joint_trajectory', JointTrajectoryCommand, self.robot_sub_command_traj) - self.pub_joint_states = rospy.Publisher(self.js_topic, JointState, queue_size=1) - rospy.Timer(rospy.Duration(1.0/self.timer_hz), self.robot_update_joint_states) - - ### @brief Loads the URDF to get joint limit information - def get_urdf_info(self): - robot_name = rospy.get_namespace().strip("/") - full_rd_name = '/' + robot_name + '/robot_description' - if rospy.has_param(full_rd_name): - self.rd = URDF.from_parameter_server(key=full_rd_name) - - ### @brief Loads the 'motor_configs' and 'mode_configs' YAML files and populates various class dictionaries - def robot_get_motor_configs(self): - - # get filepaths to the YAML files from the ROS parameter server - motor_configs_filepath = rospy.get_param('~motor_configs') - mode_configs_filepath = rospy.get_param('~mode_configs') - - # try to open and load both files into local variables - try: - with open(motor_configs_filepath, "r") as yamlfile: - motor_configs = yaml.safe_load(yamlfile) - except IOError: - rospy.logerr("[xs_sdk] Motor Config File was not found.") - return False - - try: - with open(mode_configs_filepath, "r") as yamlfile: - mode_configs = yaml.safe_load(yamlfile) - except IOError: - mode_configs = {} - rospy.loginfo("[xs_sdk] Mode Config file was not found.") - - joint_order = motor_configs["joint_order"] - sleep_positions = motor_configs["sleep_positions"] - - self.js_topic = motor_configs["joint_state_publisher"]["topic_name"] - - motor_groups = motor_configs.get("groups", {}) - grippers = motor_configs.get("grippers", {}) - motors = motor_configs["motors"] - - mode_groups = mode_configs.get("groups", {}) - singles = mode_configs.get("singles", {}) - - # populate self.gripper_map - for gpr, items in grippers.items(): - self.gripper_map[gpr] = {"horn_radius" : items["horn_radius"], - "arm_length" : items["arm_length"], - "left_finger" : items["left_finger"], - "right_finger" : items["right_finger"]} - - # populate self.sleep_map, self.gripper_order, and self.js_index_map - # also, initialize self.joint_states - for indx in range(len(joint_order)): - self.sleep_map[joint_order[indx]] = sleep_positions[indx] - if joint_order[indx] in self.gripper_map: - self.gripper_order.append(joint_order[indx]) - self.gripper_map[joint_order[indx]]["js_index"] = indx - self.js_index_map[joint_order[indx]] = indx - self.joint_states.name.append(joint_order[indx]) - self.joint_states.position.append(sleep_positions[indx]) - - # continue to populate self.joint_states with gripper finger info - for gpr in self.gripper_order: - fingers = ["left_finger", "right_finger"] - lin_dist = self.robot_convert_angular_position_to_linear(gpr, self.sleep_map[gpr]) - for finger in fingers: - fngr = self.gripper_map[gpr][finger] - self.js_index_map[fngr] = len(self.js_index_map) - self.joint_states.name.append(fngr) - self.joint_states.position.append(lin_dist if finger == "left_finger" else -lin_dist) - - # populate self.motor_map - for name in joint_order: - self.motor_map[name] = {"motor_id" : motors[name]["ID"]} - - # populate self.group_map - groups = list(motor_groups) - groups.insert(0, "all") - motor_groups["all"] = joint_order - mode_groups["all"] = {} - for grp in groups: - joint_names = motor_groups[grp] - joint_group = {} - joint_group["joint_names"] = joint_names - joint_group["joint_num"] = len(joint_names) - joint_group["joint_ids"] = [self.motor_map[name]["motor_id"] for name in joint_names] - self.group_map[grp] = joint_group - mode = mode_groups[grp].get("operating_mode", "position") - profile_type = mode_groups[grp].get("profile_type", "velocity") - profile_velocity = mode_groups[grp].get("profile_velocity", 0) - profile_acceleration = mode_groups[grp].get("profile_acceleration", 0) - self.robot_set_operating_modes("group", grp, mode, profile_type, profile_velocity, profile_acceleration) - - # continue to populate self.motor_map - for sgl in singles: - info = singles[sgl] - mode = info.get("operating_mode", "position") - profile_type = info.get("profile_type", "velocity") - profile_velocity = info.get("profile_velocity", 0) - profile_acceleration = info.get("profile_acceleration", 0) - self.robot_set_joint_operating_mode(sgl, mode, profile_type, profile_velocity, profile_acceleration) - - ### @brief Set the operating mode for a specific group of motors or a single motor - ### @param cmd_type - set to 'group' if changing the operating mode for a group of motors or 'single' if changing the operating mode for a single motor - ### @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' - ### @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') - ### @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (only 'time' is supported in the simulator) - ### @param profile_velocity - passthrough to the Profile_Velocity register on the motor - ### @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor - def robot_set_operating_modes(self, cmd_type, name, mode, profile_type, profile_velocity, profile_acceleration): - if (cmd_type == "group" and name in self.group_map): - for joint_name in self.group_map[name]["joint_names"]: - self.robot_set_joint_operating_mode(joint_name, mode, profile_type, profile_velocity, profile_acceleration) - self.group_map[name]["mode"] = mode - self.group_map[name]["profile_type"] = profile_type - rospy.loginfo("[xs_sdk] The operating mode for the '%s' group was changed to %s." % (name, mode)) - elif (cmd_type == "single" and name in self.motor_map): - self.robot_set_joint_operating_mode(name, mode, profile_type, profile_velocity, profile_acceleration) - rospy.loginfo("[xs_sdk] The operating mode for the '%s' joint was changed to %s." % (name, mode)) - elif ((cmd_type == "group" and name not in self.group_map) or (cmd_type == "single" and name not in self.motor_map)): - rospy.loginfo("[xs_sdk] The '%s' joint/group does not exist. Was it added to the motor config file?" % name) - else: - rospy.logerr("[xs_sdk] Invalid command for argument 'cmd_type' while setting operating mode.") - - ### @brief Helper function used to set the operating mode for a single motor - ### @param name - desired motor name - ### @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') - ### @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (only 'time' is supported in the simulator) - ### @param profile_velocity - passthrough to the Profile_Velocity register on the motor - ### @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor - def robot_set_joint_operating_mode(self, name, mode, profile_type, profile_velocity, profile_acceleration): - self.motor_map[name]["mode"] = mode - self.motor_map[name]["profile_type"] = profile_type - self.motor_map[name]["profile_velocity"] = profile_velocity - self.motor_map[name]["profile_acceleration"] = profile_acceleration - - ### @brief Command a desired group of motors with the specified commands - ### @param name - desired motor group name - ### @param commands - vector of commands (order matches the order specified in the 'groups' section in the motor config file) - ### @details - commands are processed differently based on the operating mode specified for the motor group - def robot_write_commands(self, name, commands): - joints = self.group_map[name]["joint_names"] - for x in range(len(joints)): - self.robot_write_joint_command(joints[x], commands[x]) - - ### @brief Command a desired motor with the specified command - ### @param name - desired motor name - ### @param command - motor command - ### @details - the command is processed differently based on the operating mode specified for the motor - def robot_write_joint_command(self, name, command): - motor = self.motor_map[name] - mode = motor["mode"] - prof_vel = motor["profile_velocity"] - with self.cmd_mutex: - # create a trajectory of 'waypoints' to execute if motion should take longer than 'self.move_thresh' milliseconds - if "position" in mode and prof_vel > self.move_thresh: - num_itr = int(round(prof_vel / 1000.0 * self.timer_hz + 1)) - self.commands[name] = list(np.linspace(command, self.joint_states.position[self.js_index_map[name]], num_itr)) - else: - self.commands[name] = command - - ### @brief Converts a desired linear distance between two gripper fingers into an angular position - ### @param name - name of the gripper servo to command - ### @param linear_position - desired distance [m] between the two gripper fingers - ### @return result - angular position [rad] that achieves the desired linear distance - def robot_convert_linear_position_to_radian(self, name, linear_position): - half_dist = linear_position / 2.0 - arm_length = self.gripper_map[name]["arm_length"] - horn_radius = self.gripper_map[name]["horn_radius"] - result = math.pi/2.0 - math.acos((horn_radius**2 + half_dist**2 - arm_length**2) / (2 * horn_radius * half_dist)) - return result - - ### @brief Converts a specified angular position into the linear distance from one gripper finger to the center of the gripper servo horn - ### @param name - name of the gripper sevo to command - ### @param angular_position - desired gripper angular position [rad] - ### @return - linear position [m] from a gripper finger to the center of the gripper servo horn - def robot_convert_angular_position_to_linear(self, name, angular_position): - arm_length = self.gripper_map[name]["arm_length"] - horn_radius = self.gripper_map[name]["horn_radius"] - a1 = horn_radius * math.sin(angular_position) - c = math.sqrt(horn_radius**2 - a1**2) - a2 = math.sqrt(arm_length**2 - c**2) - return a1 + a2 - - ### @brief ROS Subscriber callback function to command a group of joints - ### @param msg - JointGroupCommand message dictating the joint group to command along with the actual commands - ### @details - refer to the message definition for details - def robot_sub_command_group(self, msg): - self.robot_write_commands(msg.name, msg.cmd) - - ### @brief ROS Subscriber callback function to command a single joint - ### @param msg - JointSingleCommand message dictating the joint to command along with the actual command - ### @details - refer to the message definition for details - def robot_sub_command_single(self, msg): - self.robot_write_joint_command(msg.name, msg.cmd) - - ### @brief ROS Subscriber callback function to command a joint trajectory - ### @param msg - JointTrajectoryCommand message dictating the joint(s) to command along with the desired trajectory - ### @details - refer to the message definition for details - def robot_sub_command_traj(self, msg): - - # check to make sure there is no trajectory currently running and that it is valid - if self.execute_joint_traj: - rospy.loginfo("[xs_sdk] Trajectory rejected since joints are still moving.") - return - if len(msg.traj.points) < 2: - rospy.loginfo("[xs_sdk] Trajectory has fewer than 2 points. Aborting...") - return - - # get the mode and joint_names - mode = None - joint_names = [] - if (msg.cmd_type == "group"): - mode = self.group_map[msg.name]["mode"] - joint_names = self.group_map[msg.name]["joint_names"] - elif (msg.cmd_type == "single"): - mode = self.motor_map[msg.name]["mode"] - joint_names.append(msg.name) - - # check to see if the initial positions match the current joint states - if len(msg.traj.points[0].positions) == len(joint_names): - for x in range(len(joint_names)): - expected_state = msg.traj.points[0].positions[x] - actual_state = self.joint_states.position[self.js_index_map[joint_names[x]]] - if not (abs(expected_state - actual_state) < 0.01): - rospy.loginfo("[xs_sdk] The %s joint is not at the correct initial state." % joint_names[x]) - rospy.loginfo("[xs_sdk] Expected state: %.2f, Actual State: %.2f." % (expected_state, actual_state)) - - # execute the trajectory - self.exectue_joint_traj = True - - points = msg.traj.points - time_start = rospy.Time.now() - for x in range(1, len(points)): - if msg.cmd_type == "group": - if "position" in mode: - self.robot_write_commands(msg.name, points[x].positions) - elif mode == "velocity": - self.robot_write_commands(msg.name, points[x].velocities) - elif mode == "pwm" or mode == "current": - self.robot_write_commands(msg.name, points[x].effort) - elif msg.cmd_type == "single": - if "position" in mode: - self.robot_write_joint_command(msg.name, points[x].positions[0]) - elif mode == "velocity": - self.robot_write_joint_command(msg.name, points[x].velocities[0]) - elif mode == "pwm" or mode == "current": - self.robot_write_joint_command(msg.name, points[x].effort[0]) - if x < len(points) - 1: - period = (points[x].time_from_start - (rospy.Time.now() - time_start)) - rospy.sleep(period) - - self.exectue_joint_traj = False - - ### @brief ROS Service to 'simulate' torquing the joints on the robot on/off (doesn't actually do anything) - ### @param req - TorqueEnable service message request - ### @return - TorqueEnable service message response [unused] - ### @details - refer to the service definition for details - def robot_srv_torque_enable(self, req): - if (req.cmd_type == "group"): - if (req.enable): rospy.loginfo("[xs_sdk] The '%s' group was torqued on." % req.name) - else: rospy.loginfo("[xs_sdk] The '%s' group was torqued off." % req.name) - else: - if (req.enable): rospy.loginfo("[xs_sdk] The '%s' joint was torqued on." % req.name) - else: rospy.loginfo("[xs_sdk] The '%s' joint was torqued off." % req.name) - return TorqueEnableResponse() - - ### @brief ROS Service to 'simulate' rebooting the motors on the robot (doesn't actually do anything) - ### @param req - Reboot service message request - ### @return - Reboot service message response [unused] - ### @details - refer to the service definition for details - def robot_srv_reboot_motors(self, req): - if (req.cmd_type == "group"): - rospy.loginfo("[xs_sdk] The '%s' group was rebooted." % req.name) - else: - rospy.loginfo("[xs_sdk] The '%s' joint was rebooted." % req.name) - return RebootResponse() - - ### @brief ROS Service that allows the user to get information about the robot - ### @param req - RobotInfo service message request - ### @return - RobotInfo service message response - ### @details - refer to the service definition for details - def robot_srv_get_robot_info(self, req): - res = RobotInfoResponse() - joint_names = [] - if (req.cmd_type == "group"): - joint_names = self.group_map[req.name]["joint_names"] - res.profile_type = self.group_map[req.name]["profile_type"] - res.mode = self.group_map[req.name]["mode"] - elif (req.cmd_type == "single"): - joint_names.append(req.name) - res.profile_type = self.motor_map[req.name]["profile_type"] - res.mode = self.motor_map[req.name]["mode"] - - res.num_joints = len(joint_names) - - for name in joint_names: - res.joint_ids.append(self.motor_map[name]["motor_id"]) - if name in self.gripper_map: - res.joint_sleep_positions.append(self.robot_convert_angular_position_to_linear(name, 0)) - name = self.gripper_map[name]["left_finger"] - res.joint_names.append(name) - else: - res.joint_sleep_positions.append(self.sleep_map[name]) - res.joint_names.append(name) - res.joint_state_indices.append(self.js_index_map[name]) - if self.rd is not None: - joint_object = next((joint for joint in self.rd.joints if joint.name == name), None) - res.joint_lower_limits.append(joint_object.limit.lower) - res.joint_upper_limits.append(joint_object.limit.upper) - res.joint_velocity_limits.append(joint_object.limit.velocity) - return res - - ### @brief ROS Service that allows the user to change operating modes - ### @param req - OperatingModes service message request - ### @return - OperatingModes service message response [unused] - ### @details - refer to the service definition for details - def robot_srv_set_operating_modes(self, req): - self.robot_set_operating_modes(req.cmd_type, req.name, req.mode, req.profile_type, req.profile_velocity, req.profile_acceleration) - return OperatingModesResponse() - - ### @brief ROS Service that allows the user to set the motor firmware PID gains (doesn't actually do anything) - ### @param req - MotorGains service message request - ### @return - MotorGains service message response [unused] - ### @details - refer to the service defintion for details - def robot_srv_set_motor_pid_gains(self, req): - return MotorGainsResponse() - - ### @brief ROS Service that allows the user to change a specific register to a specific value for multiple motors - ### @param req - RegisterValues service message request - ### @return - RegisterValues service message response [unused] - ### @details - refer to the service definition for details - only works with the 'Profile_Velocity' and - ### 'Profile_Acceleration' registers; otherwise, it doesn't do anything - def robot_srv_set_motor_registers(self, req): - if req.reg == "Profile_Velocity" or req.reg == "Profile_Acceleration": - reg = "profile_velocity" if req.reg == "Profile_Velocity" else "profile_acceleration" - if req.cmd_type == "group": - self.group_map[req.name][reg] = req.value - for joint in self.group_map[req.name]["joint_names"]: - self.motor_map[joint][reg] = req.value - elif req.cmd_type == "single": - self.motor_map[req.name][reg] = req.value - return RegisterValuesResponse() - - ### @brief ROS Service that allows the user to read a specific register on multiple motors - ### @param req - RegisterValues service message request - ### @return - RegisterValues service message response - ### @details - refer to the service definition for details - only works with the 'Profile_Velocity' and - ### 'Profile_Acceleration' registers; otherwise, it doesn't do anything - def robot_srv_get_motor_registers(self, req): - res = RegisterValuesResponse() - if req.reg == "Profile_Velocity" or req.reg == "Profile_Acceleration": - reg = "profile_velocity" if req.reg == "Profile_Velocity" else "profile_acceleration" - if req.cmd_type == "group": - for joint in self.group_map[req.name]["joint_names"]: - res.values.append(self.motor_map[joint][reg]) - elif req.cmd_type == "single": - res.values.append(self.motor_map[req.name][reg]) - return res - - ### @brief ROS Timer that updates the joint states based on the current 'self.commands' - ### @param event - TimerEvent message [unused] - ### @details - if a joint is in any form of 'position' mode and its profile_velocity is greater than - ### 'self.move_thresh' milliseconds, then 'self.commands[joint]' is a list of waypoints (in reverse); - ### that way, the latest position to execute can just be 'popped' off the end of the list - which is more efficient; - ### when the last element is taken out from the list, the dictionary key is thrown out. Otherwise, 'self.commands[joint]' - ### is a scalar. If the scalar is a position, the dictionary key is thrown out afterwards. Otherwise, the dictionary key is - ### not thrown out unless its value is 0 (which is what you want for 'pwm', 'current', or 'velocity' modes) - def robot_update_joint_states(self, event): - with self.cmd_mutex: - for joint in self.commands.copy(): - mode = self.motor_map[joint]["mode"] - - if "position" in mode: - if type(self.commands[joint]) == list: - value = self.commands[joint].pop() - if len(self.commands[joint]) == 0: - del self.commands[joint] - else: - value = self.commands[joint] - del self.commands[joint] - - if joint in self.gripper_map: - gpr = self.gripper_map[joint] - if mode == "linear_position": - angle = self.robot_convert_linear_position_to_radian(joint, value) - self.joint_states.position[self.js_index_map[joint]] = angle - self.joint_states.position[self.js_index_map[gpr["left_finger"]]] = value / 2.0 - self.joint_states.position[self.js_index_map[gpr["right_finger"]]] = -value / 2.0 - else: - lin_pos = self.robot_convert_angular_position_to_linear(joint, value) - self.joint_states.position[self.js_index_map[joint]] = value - self.joint_states.position[self.js_index_map[gpr["left_finger"]]] = lin_pos - self.joint_states.position[self.js_index_map[gpr["right_finger"]]] = -lin_pos - else: - self.joint_states.position[self.js_index_map[joint]] = value - - else: - value = self.commands[joint] - if value == 0: - del self.commands[joint] - else: - if mode == "velocity": - new_val = value / self.timer_hz - else: - # treat 'pwm' and 'current' equivalently - new_val = value / 2000.0 - self.joint_states.position[self.js_index_map[joint]] += new_val - if joint in self.gripper_map: - gpr = self.gripper_map[joint] - angle = self.joint_states.position[self.js_index_map[joint]] - lin_pos = self.robot_convert_angular_position_to_linear(joint, angle) - self.joint_states.position[self.js_index_map[gpr["left_finger"]]] = lin_pos - self.joint_states.position[self.js_index_map[gpr["right_finger"]]] = -lin_pos - - self.joint_states.header.stamp = rospy.Time.now() - self.pub_joint_states.publish(self.joint_states) - -def main(): - rospy.init_node('xs_sdk_sim') - InterbotixRobotXS() - rospy.spin() - -if __name__ == '__main__': - main() diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim.py b/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim.py new file mode 100755 index 0000000..bc06a91 --- /dev/null +++ b/interbotix_ros_xseries/interbotix_xs_sdk/scripts/xs_sdk_sim.py @@ -0,0 +1,855 @@ +#!/usr/bin/env python3 + +# Copyright 2022 Trossen Robotics +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from array import ArrayType +import math +import sys +import threading +from typing import Dict, List, Union + +from interbotix_xs_msgs.msg import ( + JointGroupCommand, + JointSingleCommand, + JointTrajectoryCommand, +) +from interbotix_xs_msgs.srv import ( + MotorGains, + OperatingModes, + Reboot, + RegisterValues, + RobotInfo, + TorqueEnable +) +import numpy as np +import rclpy +from rclpy.constants import S_TO_NS +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor +from rclpy.logging import get_logger +from rclpy.node import Node +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectoryPoint +from urdf_parser_py.urdf import URDF +import yaml + +LOGGER = get_logger('interbotix_xs_sdk.xs_sdk_sim') + + +class InterbotixRobotXS(Node): + """ + Class that simulates the actual InterbotixRobotXS class. + + :details: allows to use the same higher level ROS packages or Python API code on a + simulated robot with no change to the code + """ + + rd: URDF = None + """Holds the URDF loaded from the ROS parameter server""" + + timer_hz: float = 30.0 + """Rate [Hz] at which the ROS Timer (in charge of publishing joint states) should run""" + + js_topic: str = None + """Joint States topic""" + + move_thresh: int = 300 + """Threshold [ms] above which desired goal positions should be split up into waypoints and + simulated""" + + execute_joint_traj: bool = False + """`True` if a trajectory is currently being executed; `False` otherwise""" + + joint_states = JointState() + """Current state of all joints""" + + cmd_mutex = threading.Lock() + """Mutex to prevent reading/writing to self.commands from different threads""" + + commands: Dict[str, float] = {} + """Holds the desired commands for each joint""" + + sleep_map: Dict[str, float] = {} + """Maps a joint name with its sleep position [rad]""" + + group_map: Dict[str, Dict] = {} + """Maps a group name with a JointGroup-type dictionary (refer to the top of the xs_sdk_obj.hpp + file for reference)""" + + motor_map: Dict[str, Dict] = {} + """Maps a joint name with a MotorState-type dictionary (refer to the top of the # + xs_sdk_obj.hpp file for reference)""" + + gripper_map: Dict[str, Dict] = {} + """Maps a gripper name with a Gripper-type dictionary (refer to the top of the xs_sdk_obj.hpp + file for reference)""" + + js_index_map: Dict[str, int] = {} + """Maps a joint name with its index in self.joint_states.position""" + + gripper_order: List[str] = [] + """List of grippers as they appear in the 'joint_order' parameter in the Motor Configs YAML + file""" + + def __init__(self): + """Construct the InterbotixRobotXS simulaiton node.""" + super().__init__('xs_sdk_sim') + + self.declare_parameter('motor_configs', '') + self.declare_parameter('mode_configs', '') + self.declare_parameter('robot_description', '') + + self.get_urdf_info() + + if not self.robot_get_motor_configs(): + sys.exit(1) + + self.create_service( + TorqueEnable, + 'torque_enable', + self.robot_srv_torque_enable) + self.create_service( + Reboot, + 'reboot_motors', + self.robot_srv_reboot_motors) + self.create_service( + RobotInfo, + 'get_robot_info', + self.robot_srv_get_robot_info) + self.create_service( + OperatingModes, + 'set_operating_modes', + self.robot_srv_set_operating_modes) + self.create_service( + MotorGains, + 'set_motor_pid_gains', + self.robot_srv_set_motor_pid_gains) + self.create_service( + RegisterValues, + 'set_motor_registers', + self.robot_srv_set_motor_registers) + self.create_service( + RegisterValues, + 'get_motor_registers', + self.robot_srv_get_motor_registers) + + self.create_subscription( + JointGroupCommand, + 'commands/joint_group', + self.robot_sub_command_group, + 10) + self.create_subscription( + JointSingleCommand, + 'commands/joint_single', + self.robot_sub_command_single, + 10) + self.create_subscription( + JointTrajectoryCommand, + 'commands/joint_trajectory', + self.robot_sub_command_traj, + 10) + + self.pub_joint_states = self.create_publisher(JointState, self.js_topic, 10) + self.create_timer(1.0/self.timer_hz, self.robot_update_joint_states) + + LOGGER.info("Interbotix 'xs_sdk_sim' node is up!") + + def get_urdf_info(self) -> None: + """Load the URDF to get joint limit information.""" + if self.has_parameter('robot_description'): + self.rd = URDF.from_xml_string( + self.get_parameter('robot_description').get_parameter_value().string_value) + + def robot_get_motor_configs(self) -> bool: + """ + Load the 'motor_configs' and 'mode_configs' YAML files and populate class dictionaries. + + :return: `True` if configs were retrieved, `False` otherwise + """ + # get filepaths to the YAML files from the ROS parameter server + motor_configs_filepath = self.get_parameter( + 'motor_configs').get_parameter_value().string_value + mode_configs_filepath = self.get_parameter( + 'mode_configs').get_parameter_value().string_value + + # try to open and load both files into local variables + try: + with open(motor_configs_filepath, 'r') as yamlfile: + motor_configs = yaml.safe_load(yamlfile) + except IOError: + LOGGER.error('Motor Config File was not found.') + return False + LOGGER.info( + f'Loaded motor configs from `{motor_configs_filepath}`.' + ) + + try: + with open(mode_configs_filepath, 'r') as yamlfile: + mode_configs = yaml.safe_load(yamlfile) + except IOError: + mode_configs = {} + LOGGER.warning('Mode Config file was not found. Using defaults.') + LOGGER.info(f'Loaded mode configs from `{mode_configs_filepath}`.') + + joint_order = motor_configs['joint_order'] + sleep_positions = motor_configs['sleep_positions'] + + self.js_topic = motor_configs['joint_state_publisher']['topic_name'] + + motor_groups = motor_configs.get('groups', {}) + grippers = motor_configs.get('grippers', {}) + motors = motor_configs['motors'] + + mode_groups = mode_configs.get('groups', {}) + singles = mode_configs.get('singles', {}) + + # populate self.gripper_map + if grippers: + for gpr, items in grippers.items(): + self.gripper_map[gpr] = { + 'horn_radius': items['horn_radius'], + 'arm_length': items['arm_length'], + 'left_finger': items['left_finger'], + 'right_finger': items['right_finger'] + } + else: + self.gripper_map = {} + + # populate self.sleep_map, self.gripper_order, and self.js_index_map + # also, initialize self.joint_states + for indx in range(len(joint_order)): + self.sleep_map[joint_order[indx]] = sleep_positions[indx] + if joint_order[indx] in self.gripper_map: + self.gripper_order.append(joint_order[indx]) + self.gripper_map[joint_order[indx]]['js_index'] = indx + self.js_index_map[joint_order[indx]] = indx + self.joint_states.name.append(joint_order[indx]) + self.joint_states.position.append(sleep_positions[indx]) + + # continue to populate self.joint_states with gripper finger info + for gpr in self.gripper_order: + fingers = ['left_finger', 'right_finger'] + lin_dist = self.robot_convert_angular_position_to_linear(gpr, self.sleep_map[gpr]) + for finger in fingers: + fngr = self.gripper_map[gpr][finger] + self.js_index_map[fngr] = len(self.js_index_map) + self.joint_states.name.append(fngr) + self.joint_states.position.append( + lin_dist if finger == 'left_finger' else -lin_dist + ) + + # populate self.motor_map + for name in joint_order: + self.motor_map[name] = {'motor_id': motors[name]['ID']} + + # populate self.group_map + groups = list(motor_groups) + groups.insert(0, 'all') + motor_groups['all'] = joint_order + mode_groups['all'] = {} + for grp in groups: + joint_names = motor_groups[grp] + joint_group = {} + joint_group['joint_names'] = joint_names + joint_group['joint_num'] = len(joint_names) + joint_group['joint_ids'] = [self.motor_map[name]['motor_id'] for name in joint_names] + self.group_map[grp] = joint_group + mode = mode_groups[grp].get('operating_mode', 'position') + profile_type = mode_groups[grp].get('profile_type', 'velocity') + profile_velocity = mode_groups[grp].get('profile_velocity', 0) + profile_acceleration = mode_groups[grp].get('profile_acceleration', 0) + self.robot_set_operating_modes( + 'group', + grp, + mode, + profile_type, + profile_velocity, + profile_acceleration + ) + + # continue to populate self.motor_map + if singles: + for sgl in singles: + info = singles[sgl] + mode = info.get('operating_mode', 'position') + profile_type = info.get('profile_type', 'velocity') + profile_velocity = info.get('profile_velocity', 0) + profile_acceleration = info.get('profile_acceleration', 0) + self.robot_set_joint_operating_mode( + sgl, + mode, + profile_type, + profile_velocity, + profile_acceleration + ) + + return True + + def robot_set_operating_modes( + self, + cmd_type: str, + name: str, + mode: str, + profile_type: str, + profile_velocity: int, + profile_acceleration: int + ) -> None: + """ + Set the operating mode for a specific group of motors or a single motor. + + :param cmd_type: set to 'group' if changing the operating mode for a group of motors or + 'single' if changing the operating mode for a single motor + :param name: desired motor group name if cmd_type is set to 'group' or the desired motor + name if cmd_type is set to 'single' + :param mode: desired operating mode (either 'position', 'linear_position', 'ext_position', + 'velocity', 'pwm', 'current', or 'current_based_position') + :param profile_type: set to 'velocity' for a Velocity-based Profile or 'time' for a + Time-based Profile (only 'time' is supported in the simulator) + :param profile_velocity: passthrough to the Profile_Velocity register on the motor + :param profile_acceleration: passthrough to the Profile_Acceleration register on the motor + """ + if (cmd_type == 'group' and name in self.group_map): + for joint_name in self.group_map[name]['joint_names']: + self.robot_set_joint_operating_mode( + joint_name, + mode, + profile_type, + profile_velocity, + profile_acceleration + ) + self.group_map[name]['mode'] = mode + self.group_map[name]['profile_type'] = profile_type + LOGGER.info(( + f"The operating mode for the '{name}' group was changed to '{mode}'.")) + elif (cmd_type == 'single' and name in self.motor_map): + self.robot_set_joint_operating_mode( + name, + mode, + profile_type, + profile_velocity, + profile_acceleration + ) + LOGGER.info(( + f"The operating mode for the '{name}' joint was changed to '{mode}'." + )) + elif ( + (cmd_type == 'group' and name not in self.group_map) or + (cmd_type == 'single' and name not in self.motor_map) + ): + LOGGER.info(( + f"The '{name}' joint/group does not exist. " + 'Was it added to the motor config file?' + )) + else: + LOGGER.error( + "Invalid command for argument 'cmd_type' while setting operating mode." + ) + + def robot_set_joint_operating_mode( + self, + name: str, + mode: str, + profile_type: str, + profile_velocity: int, + profile_acceleration: int + ) -> None: + """ + Set the operating mode for a single motor. + + :param name: desired motor name + :param mode: desired operating mode (either 'position', 'linear_position', 'ext_position', + 'velocity', 'pwm', 'current', or 'current_based_position') + :param profile_type: set to 'velocity' for a Velocity-based Profile or 'time' for a + Time-based Profile (only 'time' is supported in the simulator) + :param profile_velocity: passthrough to the Profile_Velocity register on the motor + :param profile_acceleration: passthrough to the Profile_Acceleration register on the motor + """ + self.motor_map[name]['mode'] = mode + self.motor_map[name]['profile_type'] = profile_type + self.motor_map[name]['profile_velocity'] = profile_velocity + self.motor_map[name]['profile_acceleration'] = profile_acceleration + + def robot_write_commands( + self, + name: str, + commands: Union[List[float], ArrayType] + ) -> None: + """ + Command a desired group of motors with the specified commands. + + :param name: desired motor group name + :param commands: vector of commands (order matches the order specified in the 'groups' + section in the motor config file) + :details: commands are processed differently based on the operating mode specified for the + motor group + """ + joints = self.group_map[name]['joint_names'] + for x in range(len(joints)): + self.robot_write_joint_command(joints[x], commands[x]) + + def robot_write_joint_command( + self, + name: str, + command: float + ) -> None: + """ + Command a desired motor with the specified command. + + :param name: desired motor name + :param command: motor command + :details: the command is processed differently based on the operating mode specified for + the motor + """ + motor = self.motor_map[name] + mode = motor['mode'] + prof_vel = motor['profile_velocity'] + with self.cmd_mutex: + # create a trajectory of 'waypoints' to execute if motion should take longer than + # 'self.move_thresh' milliseconds + if 'position' in mode and prof_vel > self.move_thresh: + num_itr = int(round(prof_vel / 1000.0 * self.timer_hz + 1)) + self.commands[name] = list( + np.linspace( + command, + self.joint_states.position[self.js_index_map[name]], + num_itr + ) + ) + else: + self.commands[name] = command + + def robot_convert_linear_position_to_radian( + self, + name: str, + linear_position: float + ) -> float: + """ + Convert a linear distance between two gripper fingers into an angular position. + + :param name: name of the gripper servo to command + :param linear_position: desired distance [m] between the two gripper fingers + :return result: angular position [rad] that achieves the desired linear distance + """ + half_dist = linear_position / 2.0 + arm_length = self.gripper_map[name]['arm_length'] + horn_radius = self.gripper_map[name]['horn_radius'] + return math.pi/2.0 - math.acos( + (horn_radius**2 + half_dist**2 - arm_length**2) / (2 * horn_radius * half_dist) + ) + + def robot_convert_angular_position_to_linear( + self, + name: str, + angular_position: float + ) -> float: + """ + Convert an angular position into the linear distance from gripper horn to finger. + + :param name: name of the gripper sevo to command + :param angular_position: desired gripper angular position [rad] + :return: linear position [m] from a gripper finger to the center of the gripper servo horn + """ + arm_length = self.gripper_map[name]['arm_length'] + horn_radius = self.gripper_map[name]['horn_radius'] + a1 = horn_radius * math.sin(angular_position) + c = math.sqrt(horn_radius**2 - a1**2) + a2 = math.sqrt(arm_length**2 - c**2) + return a1 + a2 + + def robot_sub_command_group(self, msg: JointGroupCommand) -> None: + """ + Command a group of joints from a ROS Subscriber callback. + + :param msg: JointGroupCommand message dictating the joint group to command along with the + actual commands + :details: refer to the message definition for details + """ + self.robot_write_commands(msg.name, msg.cmd) + + def robot_sub_command_single(self, msg: JointSingleCommand) -> None: + """ + Command a single joint from a ROS subscriber callback. + + :param msg: JointSingleCommand message dictating the joint to command along with the actual + command + :details: refer to the message definition for details + """ + self.robot_write_joint_command(msg.name, msg.cmd) + + def robot_sub_command_traj(self, msg: JointTrajectoryCommand) -> None: + """ + Command a joint trajectory from a ROS Subscriber callback. + + :param msg: JointTrajectoryCommand message dictating the joint(s) to command along with the + desired trajectory + :details: refer to the message definition for details + """ + # check to make sure there is no trajectory currently running and that it is valid + if self.execute_joint_traj: + LOGGER.warning('Trajectory rejected since joints are still moving.') + return + if len(msg.traj.points) < 2: + LOGGER.warning('Trajectory has fewer than 2 points. Aborting...') + return + + # get the mode and joint_names + mode = None + joint_names = [] + if (msg.cmd_type == 'group'): + mode = self.group_map[msg.name]['mode'] + joint_names = self.group_map[msg.name]['joint_names'] + elif (msg.cmd_type == 'single'): + mode = self.motor_map[msg.name]['mode'] + joint_names.append(msg.name) + + # check to see if the initial positions match the current joint states + if len(msg.traj.points[0].positions) == len(joint_names): + for x in range(len(joint_names)): + expected_state = msg.traj.points[0].positions[x] + actual_state = self.joint_states.position[self.js_index_map[joint_names[x]]] + if not (abs(expected_state - actual_state) < 0.01): + LOGGER.warning( + f"The '{joint_names[x]}' joint is not at the correct initial state.") + LOGGER.warning(( + f"Expected state: '{expected_state:.2f}', " + f"Actual State: '{actual_state:.2f}'.")) + + # execute the trajectory + self.exectue_joint_traj = True + + points: List[JointTrajectoryPoint] = msg.traj.points + time_start = self.get_clock().now() + for x in range(1, len(points)): + if msg.cmd_type == 'group': + if 'position' in mode: + self.robot_write_commands(msg.name, points[x].positions) + elif mode == 'velocity': + self.robot_write_commands(msg.name, points[x].velocities) + elif mode == 'pwm' or mode == 'current': + self.robot_write_commands(msg.name, points[x].effort) + elif msg.cmd_type == 'single': + if 'position' in mode: + self.robot_write_joint_command(msg.name, points[x].positions[0]) + elif mode == 'velocity': + self.robot_write_joint_command(msg.name, points[x].velocities[0]) + elif mode == 'pwm' or mode == 'current': + self.robot_write_joint_command(msg.name, points[x].effort[0]) + if x < len(points) - 1: + # TODO: clean this up + period = ( + Duration.from_msg(points[x].time_from_start).nanoseconds + - (self.get_clock().now() - time_start).nanoseconds) / S_TO_NS + rate = self.create_rate(1.0 / period) + rate.sleep() + rate.destroy() + self.robot_update_joint_states() + + self.exectue_joint_traj = False + + def robot_srv_torque_enable( + self, + req: TorqueEnable.Request, + res: TorqueEnable.Response + ) -> TorqueEnable.Response: + """ + Simulate torquing the joints on the robot on/off from a ROS service. + + :param req: TorqueEnable service message request + :param res: TorqueEnable service message response + :return: TorqueEnable service message response + :details: refer to the service definition for details + """ + if (req.cmd_type == 'group'): + if (req.enable): + LOGGER.info(f"The '{req.name}' group was torqued on.") + else: + LOGGER.info(f"The '{req.name}' group was torqued off.") + else: + if (req.enable): + LOGGER.info(f"The '{req.name}' joint was torqued on.") + else: + LOGGER.info(f"The '{req.name}' joint was torqued off.") + return res + + def robot_srv_reboot_motors( + self, + req: Reboot.Request, + res: Reboot.Response + ) -> Reboot.Response: + """ + Simulate rebooting the motors on the robot from a ROS service. + + :param req: Reboot service message request + :param res: Reboot service message response + :return: Reboot service message response + :details: refer to the service definition for details + """ + if (req.cmd_type == 'group'): + LOGGER.info(f"The '{req.name}' group was rebooted.") + else: + LOGGER.info(f"The '{req.name}' joint was rebooted.") + return res + + def robot_srv_get_robot_info( + self, + req: RobotInfo.Request, + res: RobotInfo.Response + ) -> RobotInfo.Response: + """ + Get information about the robot from a ROS service. + + :param req: RobotInfo service message request + :param res: RobotInfo service message response + :return: RobotInfo service message response + :details: refer to the service definition for details + """ + joint_names = [] + if (req.cmd_type == 'group'): + joint_names = self.group_map[req.name]['joint_names'] + res.profile_type = self.group_map[req.name]['profile_type'] + res.mode = self.group_map[req.name]['mode'] + elif (req.cmd_type == 'single'): + joint_names.append(req.name) + res.profile_type = self.motor_map[req.name]['profile_type'] + res.mode = self.motor_map[req.name]['mode'] + + res.num_joints = len(joint_names) + + for name in joint_names: + res.joint_ids.append(self.motor_map[name]['motor_id']) + if name in self.gripper_map: + res.joint_sleep_positions.append( + self.robot_convert_angular_position_to_linear(name, 0) + ) + name = self.gripper_map[name]['left_finger'] + res.joint_names.append(name) + else: + res.joint_sleep_positions.append(self.sleep_map[name]) + res.joint_names.append(name) + res.joint_state_indices.append(self.js_index_map[name]) + if self.rd is not None: + joint_object = next( + (joint for joint in self.rd.joints if joint.name == name), + None + ) + res.joint_lower_limits.append(joint_object.limit.lower) + res.joint_upper_limits.append(joint_object.limit.upper) + res.joint_velocity_limits.append(joint_object.limit.velocity) + + if 'all' not in req.name: + res.name.append(req.name) + else: + for key, _ in self.group_map.items(): + res.name.append(key) + return res + + def robot_srv_set_operating_modes( + self, + req: OperatingModes.Request, + res: OperatingModes.Response + ) -> OperatingModes.Response: + """ + Change operating modes through a ROS service. + + :param req: OperatingModes service message request + :param res: OperatingModes service message response + :return: OperatingModes service message response + :details: refer to the service definition for details + """ + self.robot_set_operating_modes( + req.cmd_type, + req.name, + req.mode, + req.profile_type, + req.profile_velocity, + req.profile_acceleration + ) + return res + + def robot_srv_set_motor_pid_gains( + self, + req: MotorGains.Request, + res: MotorGains.Response + ) -> MotorGains.Response: + """ + Set the motor firmware PID gains from a ROS service. + + :param req: MotorGains service message request + :param res: MotorGains service message response + :return: MotorGains service message response + :details: refer to the service defintion for details + """ + return res + + def robot_srv_set_motor_registers( + self, + req: RegisterValues.Request, + res: RegisterValues.Response + ) -> RegisterValues.Response: + """ + Change a register to a value for multiple motors from a ROS service. + + :param req: RegisterValues service message request + :param res: RegisterValues service message response + :return: RegisterValues service message response + :details: refer to the service definition for details - only works with the + 'Profile_Velocity' and 'Profile_Acceleration' registers; otherwise, it doesn't do + anything + """ + if req.reg == 'Profile_Velocity' or req.reg == 'Profile_Acceleration': + reg = 'profile_velocity' if req.reg == 'Profile_Velocity' else 'profile_acceleration' + if req.cmd_type == 'group': + self.group_map[req.name][reg] = req.value + for joint in self.group_map[req.name]['joint_names']: + self.motor_map[joint][reg] = req.value + elif req.cmd_type == 'single': + self.motor_map[req.name][reg] = req.value + return res + + def robot_srv_get_motor_registers( + self, + req: RegisterValues.Request, + res: RegisterValues.Response + ) -> RegisterValues.Response: + """ + ROS Service that allows the user to read a specific register on multiple motors. + + :param req: RegisterValues service message request + :param res: RegisterValues service message response + :return: RegisterValues service message response + :details: refer to the service definition for details - only works with the + 'Profile_Velocity' and 'Profile_Acceleration' registers; otherwise, it doesn't do + anything + """ + if req.reg == 'Profile_Velocity' or req.reg == 'Profile_Acceleration': + reg = 'profile_velocity' if req.reg == 'Profile_Velocity' else 'profile_acceleration' + if req.cmd_type == 'group': + for joint in self.group_map[req.name]['joint_names']: + res.values.append(self.motor_map[joint][reg]) + elif req.cmd_type == 'single': + res.values.append(self.motor_map[req.name][reg]) + return res + + def robot_update_joint_states(self) -> None: + """ + ROS Timer that updates the joint states based on the current 'self.commands'. + + :details: if a joint is in any form of 'position' mode and its profile_velocity is greater + than 'self.move_thresh' milliseconds, then 'self.commands[joint]' is a list of + waypoints (in reverse); that way, the latest position to execute can just be 'popped' + off the end of the list - which is more efficient; when the last element is taken out + from the list, the dictionary key is thrown out. Otherwise, 'self.commands[joint]' is a + scalar. If the scalar is a position, the dictionary key is thrown out afterwards. + Otherwise, the dictionary key is not thrown out unless its value is 0 (which is what + you want for 'pwm', 'current', or 'velocity' modes) + """ + with self.cmd_mutex: + for joint in self.commands.copy(): + mode = self.motor_map[joint]['mode'] + + if 'position' in mode: + if isinstance(self.commands[joint], list): + value = self.commands[joint].pop() + if len(self.commands[joint]) == 0: + del self.commands[joint] + else: + value = self.commands[joint] + del self.commands[joint] + + if joint in self.gripper_map: + gpr = self.gripper_map[joint] + if mode == 'linear_position': + angle = self.robot_convert_linear_position_to_radian(joint, value) + self.joint_states.position[self.js_index_map[joint]] = angle + self.joint_states.position[ + self.js_index_map[gpr['left_finger']]] = value / 2.0 + self.joint_states.position[ + self.js_index_map[gpr['right_finger']]] = -value / 2.0 + else: + lin_pos = self.robot_convert_angular_position_to_linear(joint, value) + self.joint_states.position[self.js_index_map[joint]] = value + self.joint_states.position[ + self.js_index_map[gpr['left_finger']] + ] = lin_pos + self.joint_states.position[ + self.js_index_map[gpr['right_finger']] + ] = -lin_pos + else: + self.joint_states.position[self.js_index_map[joint]] = value + + else: + value = self.commands[joint] + if value == 0: + del self.commands[joint] + else: + if mode == 'velocity': + new_val = value / self.timer_hz + else: + # treat 'pwm' and 'current' equivalently + new_val = value / 2000.0 + self.joint_states.position[self.js_index_map[joint]] += new_val + if joint in self.gripper_map: + gpr = self.gripper_map[joint] + angle = self.joint_states.position[self.js_index_map[joint]] + lin_pos = self.robot_convert_angular_position_to_linear(joint, angle) + self.joint_states.position[ + self.js_index_map[gpr['left_finger']] + ] = lin_pos + self.joint_states.position[ + self.js_index_map[gpr['right_finger']] + ] = -lin_pos + + self.joint_states.header.stamp = self.get_clock().now().to_msg() + self.pub_joint_states.publish(self.joint_states) + + +def main(args=None): + rclpy.init(args=args) + executor = MultiThreadedExecutor() + try: + node = InterbotixRobotXS() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + print('Keyboard interrupt caught. Shutting down safely...') + finally: + executor.shutdown() + node.destroy_node() + except KeyboardInterrupt: + print('Keyboard interrupt caught. Shutting down safely...') + finally: + if executor is not None: + if not executor._is_shutdown: + executor.shutdown() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk.cpp b/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk.cpp index 2ad46eb..a4572a6 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk.cpp +++ b/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk.cpp @@ -1,20 +1,52 @@ -#include "interbotix_xs_sdk/xs_sdk_obj.h" +// Copyright 2022 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. -int main( int argc, char** argv ) +#include + +#include "interbotix_xs_sdk/xs_sdk_obj.hpp" + +int main(int argc, char ** argv) { - ros::init(argc, argv, "xs_sdk"); - ros::NodeHandle n; - bool success = true; - InterbotixRobotXS bot(&n, success); - if (success) - ros::spin(); - else - { - ROS_ERROR( - "[xs_sdk] For troubleshooting, please see " - "https://www.trossenrobotics.com/docs/interbotix_xsarms/troubleshooting/index.html"); - ros::shutdown(); - - } - return 0; + rclcpp::init(argc, argv); + bool success = true; + auto node = std::make_shared(success); + if (success) { + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node); + exec.spin(); + } else { + RCLCPP_FATAL( + interbotix_xs::LOGGER, + "For troubleshooting, please see " + "'https://docs.trossenrobotics.com/interbotix_xsarms_docs/troubleshooting.html'."); + rclcpp::shutdown(); + return 1; + } + rclcpp::shutdown(); + return 0; } diff --git a/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk_obj.cpp b/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk_obj.cpp index bdd8f02..44dc0e5 100644 --- a/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk_obj.cpp +++ b/interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk_obj.cpp @@ -1,1225 +1,513 @@ -#include "interbotix_xs_sdk/xs_sdk_obj.h" - -/// @brief Constructor for the InterbotixRobotXS -/// @param node_handle - ROS NodeHandle -InterbotixRobotXS::InterbotixRobotXS(ros::NodeHandle *node_handle, bool &success) - : node(*node_handle) -{ - if (!robot_get_motor_configs()) - { - success = false; - return; - } - - if (!robot_init_port()) - { - success = false; - return; - } - - if (!robot_ping_motors()) - { +// Copyright 2022 Trossen Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include "interbotix_xs_sdk/xs_sdk_obj.hpp" + +namespace interbotix_xs +{ + +InterbotixRobotXS::InterbotixRobotXS( + bool & success, + const rclcpp::NodeOptions & options) +: rclcpp::Node("xs_sdk", options) +{ + robot_init_parameters(); + if (!robot_init_driver()) { success = false; - ROS_ERROR("[xs_sdk] Failed to find all motors. Shutting down..."); return; } - - if (!robot_load_motor_configs()) - { - success = false; - ROS_ERROR("[xs_sdk] Failed to write configurations to all motors. Shutting down..."); - return; - } - - robot_init_controlItems(); - robot_init_SDK_handlers(); - robot_init_operating_modes(); robot_init_publishers(); robot_init_subscribers(); robot_init_services(); robot_init_timers(); robot_wait_for_joint_states(); - ROS_INFO("[xs_sdk] Interbotix 'xs_sdk' node is up!"); -} - -/// @brief Destructor for the InterbotixRobotXS -InterbotixRobotXS::~InterbotixRobotXS(){} - -/// @brief Set the operating mode for a specific group of motors or a single motor -/// @param cmd_type - set to 'group' if changing the operating mode for a group of motors or 'single' if changing the operating mode for a single motor -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') -/// @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (modifies Bit 2 of the 'Drive_Mode' register) -/// @param profile_velocity - passthrough to the Profile_Velocity register on the motor -/// @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor -void InterbotixRobotXS::robot_set_operating_modes(std::string const& cmd_type, std::string const& name, std::string const& mode, std::string const& profile_type, int32_t profile_velocity, int32_t profile_acceleration) -{ - if (cmd_type == "group" && group_map.count(name) > 0) - { - for (auto const& joint_name:group_map[name].joint_names) - robot_set_joint_operating_mode(joint_name, mode, profile_type, profile_velocity, profile_acceleration); - group_map[name].mode = mode; - group_map[name].profile_type = profile_type; - ROS_INFO("[xs_sdk] The operating mode for the '%s' group was changed to %s.", name.c_str(), mode.c_str()); - } - else if (cmd_type == "single" && motor_map.count(name) > 0) - { - robot_set_joint_operating_mode(name, mode, profile_type, profile_velocity, profile_acceleration); - ROS_INFO("[xs_sdk] The operating mode for the '%s' joint was changed to %s.", name.c_str(), mode.c_str()); - } - else if (cmd_type == "group" && group_map.count(name) == 0 || cmd_type == "single" && motor_map.count(name) == 0) - ROS_WARN("[xs_sdk] The '%s' joint/group does not exist. Was it added to the motor config file?", name.c_str()); - else - ROS_ERROR("[xs_sdk] Invalid command for argument 'cmd_type' while setting operating mode."); -} - -/// @brief Helper function used to set the operating mode for a single motor -/// @param name - desired motor name -/// @param mode - desired operating mode (either 'position', 'linear_position', 'ext_position', 'velocity', 'pwm', 'current', or 'current_based_position') -/// @param profile_type - set to 'velocity' for a Velocity-based Profile or 'time' for a Time-based Profile (modifies Bit 2 of the 'Drive_Mode' register) -/// @param profile_velocity - passthrough to the Profile_Velocity register on the motor -/// @param profile_acceleration - passthrough to the Profile_Acceleration register on the motor -void InterbotixRobotXS::robot_set_joint_operating_mode(std::string const& name, std::string const& mode, std::string const& profile_type, int32_t profile_velocity, int32_t profile_acceleration) -{ - for (auto const& joint_name:sister_map[name]) - { - dxl_wb.torque(motor_map[joint_name].motor_id, false); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, torqued off.", motor_map[joint_name].motor_id); - } - - for (auto const& motor_name:shadow_map[name]) - { - int32_t drive_mode; - dxl_wb.itemRead(motor_map[motor_name].motor_id, "Drive_Mode", &drive_mode); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, read Drive Mode %d.",motor_map[motor_name].motor_id, drive_mode); - - if (drive_mode <= 1 && profile_type == "time") - { - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Drive_Mode", drive_mode + 4); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, write Drive Mode %d.", motor_map[motor_name].motor_id, drive_mode + 4); - } - else if (drive_mode >= 4 && profile_type == "velocity") - { - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Drive_Mode", drive_mode - 4); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, write Drive Mode %d.", motor_map[motor_name].motor_id, drive_mode - 4); - } - - if (mode == "position" || mode == "linear_position") - { - dxl_wb.setPositionControlMode(motor_map[motor_name].motor_id); - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Profile_Velocity", profile_velocity); - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Profile_Acceleration", profile_acceleration); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set poscontrolmode, pv=%i, pa=%i.", motor_map[motor_name].motor_id, profile_velocity, profile_acceleration); - } - else if (mode == "ext_position") - { - dxl_wb.setExtendedPositionControlMode(motor_map[motor_name].motor_id); - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Profile_Velocity", profile_velocity); - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Profile_Acceleration", profile_acceleration); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set extposcontrolmode, pv=%i, pa=%i.", motor_map[motor_name].motor_id, profile_velocity, profile_acceleration); - - } - else if (mode == "velocity") - { - dxl_wb.setVelocityControlMode(motor_map[motor_name].motor_id); - dxl_wb.itemWrite(motor_map[motor_name].motor_id, "Profile_Acceleration", profile_acceleration); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set velcontrolmode, pa=%i.", motor_map[motor_name].motor_id, profile_acceleration); - } - else if (mode == "pwm") - { - dxl_wb.setPWMControlMode(motor_map[motor_name].motor_id); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set pwmcontrolmode.", motor_map[motor_name].motor_id); - } - - else if (mode == "current") - { - dxl_wb.setCurrentControlMode(motor_map[motor_name].motor_id); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set currentcontrolmode", motor_map[motor_name].motor_id); - } - else if (mode == "current_based_position") - { - dxl_wb.setCurrentBasedPositionControlMode(motor_map[motor_name].motor_id); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, set currentcontrolmode", motor_map[motor_name].motor_id); - } - else - { - ROS_ERROR("[xs_sdk] Invalid command for argument 'mode' while setting the operating mode for the %s motor.", motor_name.c_str()); - continue; - } - motor_map[motor_name].mode = mode; - motor_map[motor_name].profile_type = profile_type; - } - - for (auto const& joint_name:sister_map[name]) - { - dxl_wb.torque(motor_map[joint_name].motor_id, true); - ROS_DEBUG("[xs_sdk::robot_set_joint_operating_mode] ID: %d, torqued on.", motor_map[joint_name].motor_id); - } - + RCLCPP_INFO(LOGGER, "InterbotixRobotXS is up!"); } -/// @brief Torque On/Off a specific group of motors or a single motor -/// @param cmd_type - set to 'group' if torquing off a group of motors or 'single' if torquing off a single motor -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param enable - set to True to torque on or False to torque off -void InterbotixRobotXS::robot_torque_enable(std::string const& cmd_type, std::string const& name, bool const& enable) -{ - if (cmd_type == "group" && group_map.count(name) > 0) - { - for (auto const& joint_name:group_map[name].joint_names) - dxl_wb.torque(motor_map[joint_name].motor_id, enable); - if (enable) ROS_INFO("[xs_sdk] The '%s' group was torqued on.", name.c_str()); - else ROS_INFO("[xs_sdk] The '%s' group was torqued off.", name.c_str()); - } - else if (cmd_type == "single" && motor_map.count(name) > 0) - { - dxl_wb.torque(motor_map[name].motor_id, enable); - if (enable) ROS_INFO("[xs_sdk] The '%s' joint was torqued on.", name.c_str()); - else ROS_INFO("[xs_sdk] The '%s' joint was torqued off.", name.c_str()); - } - else if (cmd_type == "group" && group_map.count(name) == 0 || cmd_type == "single" && motor_map.count(name) == 0) - ROS_WARN("[xs_sdk] The '%s' joint/group does not exist. Was it added to the motor config file?", name.c_str()); - else - ROS_ERROR("[xs_sdk] Invalid command for argument 'cmd_type' while torquing joints."); -} +InterbotixRobotXS::~InterbotixRobotXS() {} -/// @brief Reboot a specific group of motors or a single motor -/// @param cmd_type - set to 'group' if rebooting a group of motors or 'single' if rebooting a single motor -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param torque_enable - set to True to torque on or False to torque off after rebooting -/// @param smart_reboot - set to True to only reboot motor(s) in a specified group that have gone into an error state -void InterbotixRobotXS::robot_reboot_motors(std::string const& cmd_type, std::string const& name, bool const& enable, bool const& smart_reboot) +bool InterbotixRobotXS::robot_init_driver() { - std::vector joints_to_torque; - if (cmd_type == "group" && group_map.count(name) > 0) - { - for (auto const& joint_name:group_map[name].joint_names) - { - if (smart_reboot) - { - int32_t value = 0; - const char *log; - bool success = dxl_wb.itemRead(motor_map[joint_name].motor_id, "Hardware_Error_Status", &value, &log); - if (success && value == 0) - continue; - } - dxl_wb.reboot(motor_map[joint_name].motor_id); - ROS_INFO("[xs_sdk] The '%s' joint was rebooted.", joint_name.c_str()); - if (enable) joints_to_torque.push_back(joint_name); - } - if (!smart_reboot) - ROS_INFO("[xs_sdk] The '%s' group was rebooted.", name.c_str()); - } - else if (cmd_type == "single" && motor_map.count(name) > 0) - { - dxl_wb.reboot(motor_map[name].motor_id); - ROS_INFO("[xs_sdk] The '%s' joint was rebooted.", name.c_str()); - if (enable) joints_to_torque.push_back(name); - } - else if (cmd_type == "group" && group_map.count(name) == 0 || cmd_type == "single" && motor_map.count(name) == 0) - ROS_WARN("[xs_sdk] The '%s' joint/group does not exist. Was it added to the motor config file?", name.c_str()); - else - ROS_ERROR("[xs_sdk] Invalid command for argument 'cmd_type' while rebooting motors."); - - for (auto const& joint_name:joints_to_torque) - { - for (auto const& name:sister_map[joint_name]) - robot_torque_enable("single", name, true); - } -} - -/// @brief Command a desired group of motors with the specified commands -/// @param name - desired motor group name -/// @param commands - vector of commands (order matches the order specified in the 'groups' section in the motor config file) -/// @details - commands are processed differently based on the operating mode specified for the motor group -void InterbotixRobotXS::robot_write_commands(std::string const& name, std::vector commands) -{ - std::string mode = group_map[name].mode; - int32_t dynamixel_commands[commands.size()]; - - if (mode == "position" || mode == "ext_position" || mode == "current_based_position" || mode == "linear_position") - { - for (size_t i{0}; i < commands.size(); i++) - { - if (mode == "linear_position") - commands.at(i) = robot_convert_linear_position_to_radian(group_map[name].joint_names.at(i), commands.at(i)); - dynamixel_commands[i] = dxl_wb.convertRadian2Value(group_map[name].joint_ids.at(i), commands.at(i)); - ROS_DEBUG("[xs_sdk::robot_write_commands] ID: %d, writing %s command %d.", group_map[name].joint_ids.at(i), mode.c_str(), dynamixel_commands[i]); - } - dxl_wb.syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_POSITION, group_map[name].joint_ids.data(), group_map[name].joint_num, dynamixel_commands, 1); - } - else if (mode == "velocity") - { - for (size_t i{0}; i < commands.size(); i++) - { - dynamixel_commands[i] = dxl_wb.convertVelocity2Value(group_map[name].joint_ids.at(i), commands.at(i)); - ROS_DEBUG("[xs_sdk::robot_write_commands] ID: %d, writing %s command %d.", group_map[name].joint_ids.at(i), mode.c_str(), dynamixel_commands[i]); - } - dxl_wb.syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_VELOCITY, group_map[name].joint_ids.data(), group_map[name].joint_num, dynamixel_commands, 1); - } - else if (mode == "current") - { - for (size_t i{0}; i < commands.size(); i++) - { - dynamixel_commands[i] = dxl_wb.convertCurrent2Value(commands.at(i)); - ROS_DEBUG("[xs_sdk::robot_write_commands] ID: %d, writing %s command %d.", group_map[name].joint_ids.at(i), mode.c_str(), dynamixel_commands[i]); - } - dxl_wb.syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_CURRENT, group_map[name].joint_ids.data(), group_map[name].joint_num, dynamixel_commands, 1); - } - else if (mode == "pwm") - { - for (size_t i{0}; i < commands.size(); i++) - { - dynamixel_commands[i] = int32_t(commands.at(i)); - ROS_DEBUG("[xs_sdk::robot_write_commands] ID: %d, writing %s command %d.", group_map[name].joint_ids.at(i), mode.c_str(), dynamixel_commands[i]); - } - dxl_wb.syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_PWM, group_map[name].joint_ids.data(), group_map[name].joint_num, dynamixel_commands, 1); - } - else - ROS_ERROR("[xs_sdk] Invalid command for argument 'mode' while commanding joint group."); -} - -/// @brief Command a desired motor with the specified command -/// @param name - desired motor name -/// @param command - motor command -/// @details - the command is processed differently based on the operating mode specified for the motor -void InterbotixRobotXS::robot_write_joint_command(std::string const& name, float command) -{ - std::string mode = motor_map[name].mode; - if (mode == "position" || mode == "ext_position" || mode == "current_based_position" || mode == "linear_position") - { - if (mode == "linear_position") - command = robot_convert_linear_position_to_radian(name, command); - ROS_DEBUG("[xs_sdk::robot_write_joint_command] ID: %d, writing %s command %f.", motor_map[name].motor_id, mode.c_str(), command); - dxl_wb.goalPosition(motor_map[name].motor_id, command); - } - else if (mode == "velocity") - { - ROS_DEBUG("[xs_sdk::robot_write_joint_command] ID: %d, writing %s command %f.", motor_map[name].motor_id, mode.c_str(), command); - dxl_wb.goalVelocity(motor_map[name].motor_id, command); - } - else if (mode == "current") - { - ROS_DEBUG("[xs_sdk::robot_write_joint_command] ID: %d, writing %s command %f.", motor_map[name].motor_id, mode.c_str(), command); - dxl_wb.itemWrite(motor_map[name].motor_id, "Goal_Current", dxl_wb.convertCurrent2Value(command)); - } - else if (mode == "pwm") - { - ROS_DEBUG("[xs_sdk::robot_write_joint_command] ID: %d, writing %s command %f.", motor_map[name].motor_id, mode.c_str(), command); - dxl_wb.itemWrite(motor_map[name].motor_id, "Goal_PWM", int32_t(command)); - } - else - ROS_ERROR("[xs_sdk] Invalid command for argument 'mode' while commanding joint."); -} - -/// @brief Set motor firmware PID gains -/// @param cmd_type - set to 'group' if changing the PID gains for a group of motors or 'single' if changing the PID gains for a single motor -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param gains - vector containing the desired PID gains - order is as shown in the function -void InterbotixRobotXS::robot_set_motor_pid_gains(std::string const& cmd_type, std::string const& name, std::vector const& gains) -{ - std::vector names; - if (cmd_type == "group") - names = group_map[name].joint_names; - else if (cmd_type == "single") - names.push_back(name); - - for (auto const& name : names) - { - uint8_t id = motor_map[name].motor_id; - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] ID: %d, writing gains:", motor_map[name].motor_id); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] Pos_P: %i", gains.at(0)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] Pos_I: %i", gains.at(1)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] Pos_D: %i", gains.at(2)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] FF_1: %i", gains.at(3)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] FF_2: %i", gains.at(4)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] Vel_P: %i", gains.at(5)); - ROS_DEBUG("[xs_sdk::robot_set_motor_pid_gains] Vel_I: %i", gains.at(6)); - dxl_wb.itemWrite(id, "Position_P_Gain", gains.at(0)); - dxl_wb.itemWrite(id, "Position_I_Gain", gains.at(1)); - dxl_wb.itemWrite(id, "Position_D_Gain", gains.at(2)); - dxl_wb.itemWrite(id, "Feedforward_1st_Gain", gains.at(3)); - dxl_wb.itemWrite(id, "Feedforward_2nd_Gain", gains.at(4)); - dxl_wb.itemWrite(id, "Velocity_P_Gain", gains.at(5)); - dxl_wb.itemWrite(id, "Velocity_I_Gain", gains.at(6)); - } -} - -/// @brief Set a register value to multiple motors -/// @param cmd_type - set to 'group' if setting register values for a group of motors or 'single' if setting a single register value -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param value - desired register value -void InterbotixRobotXS::robot_set_motor_registers(std::string const& cmd_type, std::string const& name, std::string const& reg, int32_t const& value) -{ - std::vector names; - if (cmd_type == "group") - names = group_map[name].joint_names; - else if (cmd_type == "single") - names.push_back(name); - - for (auto const& name : names) - { - ROS_DEBUG("[xs_sdk::robot_set_motor_registers] ID: %d, writing reg: %s, value: %d.", motor_map[name].motor_id, reg.c_str(), value); - dxl_wb.itemWrite(motor_map[name].motor_id, reg.c_str(), value); - } -} - -/// @brief Get a register value from multiple motors -/// @param cmd_type - set to 'group' if getting register values from a group of motors or 'single' if getting a single register value -/// @param name - desired motor group name if cmd_type is set to 'group' or the desired motor name if cmd_type is set to 'single' -/// @param values [out] - vector of register values -void InterbotixRobotXS::robot_get_motor_registers(std::string const& cmd_type, std::string const& name, std::string const& reg, std::vector &values) -{ - std::vector names; - if (cmd_type == "group") - names = group_map[name].joint_names; - else if (cmd_type == "single") - names.push_back(name); - - const ControlItem *goal_reg = dxl_wb.getItemInfo(motor_map[names.front()].motor_id, reg.c_str()); - if (goal_reg == NULL) - { - ROS_ERROR("[xs_sdk] Could not get '%s' Item Info. Did you spell the register name correctly?", reg.c_str()); - return; - } - - for (auto const& name : names) - { - int32_t value = 0; - const char *log; - if (!dxl_wb.itemRead(motor_map[name].motor_id, reg.c_str(), &value, &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - return; - } - else - { - ROS_DEBUG("[xs_sdk::robot_get_motor_registers] ID: %d, reading reg: %s, value: %d.", motor_map[name].motor_id, reg.c_str(), value); - } - if (goal_reg->data_length == 1) - values.push_back((uint8_t)value); - else if (goal_reg->data_length == 2) - values.push_back((int16_t)value); - else - values.push_back(value); - } -} - -/// @brief Get states for a group of joints -/// @param name - desired joint group name -/// @param positions [out] - vector of current joint positions [rad] -/// @param velocities [out] - vector of current joint velocities [rad/s] -/// @param effort [out] - vector of current joint effort [mA] -void InterbotixRobotXS::robot_get_joint_states(std::string const& name, std::vector *positions, std::vector *velocities, std::vector *effort) -{ - for (auto const& joint_name : group_map[name].joint_names) - { - if (positions) positions->push_back(joint_states.position.at(js_index_map[joint_name])); - if (velocities) velocities->push_back(joint_states.velocity.at(js_index_map[joint_name])); - if (effort) effort->push_back(joint_states.effort.at(js_index_map[joint_name])); - ROS_DEBUG("[xs_sdk::robot_get_joint_states] ID: %ld, got joint state.", js_index_map[joint_name]); + try { + xs_driver = std::make_unique( + filepath_motor_configs, + filepath_mode_configs, + write_eeprom_on_startup, + xs_driver_logging_level); + } catch (const std::runtime_error & e) { + RCLCPP_FATAL(LOGGER, "InterbotixDriverXS initialization failed: '%s'.", e.what()); + return false; } + return true; } -/// @brief Get states for a single joint -/// @param name - desired joint name -/// @param position [out] - current joint position [rad] -/// @param velocity [out] - current joint velocity [rad/s] -/// @param effort [out] - current joint effort [mA] -void InterbotixRobotXS::robot_get_joint_state(std::string const& name, float *position, float *velocity, float *effort) -{ - if (position) *position = joint_states.position.at(js_index_map[name]); - if (velocity) *velocity = joint_states.velocity.at(js_index_map[name]); - if (effort) *effort = joint_states.effort.at(js_index_map[name]); - ROS_DEBUG("[xs_sdk::robot_get_joint_state] ID: %ld, got joint state.", js_index_map[name]); -} - -/// @brief Converts a desired linear distance between two gripper fingers into an angular position -/// @param name - name of the gripper servo to command -/// @param linear_position - desired distance [m] between the two gripper fingers -/// @param [out] - angular position [rad] that achieves the desired linear distance -float InterbotixRobotXS::robot_convert_linear_position_to_radian(std::string const& name, float const& linear_position) -{ - float half_dist = linear_position / 2.0; - float arm_length = gripper_map[name].arm_length; - float horn_radius = gripper_map[name].horn_radius; - float result = 3.14159/2.0 - acos((pow(horn_radius, 2) + pow(half_dist,2) - pow(arm_length, 2)) / (2 * horn_radius * half_dist)); - return result; -} - -/// @brief Converts a specified angular position into the linear distance from one gripper finger to the center of the gripper servo horn -/// @param name - name of the gripper sevo to command -/// @param angular_position - desired gripper angular position [rad] -/// @param [out] - linear position [m] from a gripper finger to the center of the gripper servo horn -float InterbotixRobotXS::robot_convert_angular_position_to_linear(std::string const& name, float const& angular_position) -{ - float arm_length = gripper_map[name].arm_length; - float horn_radius = gripper_map[name].horn_radius; - float a1 = horn_radius * sin(angular_position); - float c = sqrt(pow(horn_radius,2) - pow(a1,2)); - float a2 = sqrt(pow(arm_length,2) - pow(c,2)); - return a1 + a2; -} - -/// @brief Loads a robot-specific 'motor_configs' yaml file and populates class variables with its contents -/// @param [out] - True if the motor configs were successfully retrieved; False otherwise -bool InterbotixRobotXS::robot_get_motor_configs(void) +void InterbotixRobotXS::robot_init_parameters() { - std::string motor_configs_file, mode_configs_file; - ros::param::get("~motor_configs", motor_configs_file); - try - { - motor_configs = YAML::LoadFile(motor_configs_file.c_str()); - } - catch (YAML::BadFile &error) - { - ROS_ERROR("[xs_sdk] Motor Config file was not found or has a bad format. Shutting down..."); - ROS_ERROR("[xs_sdk] YAML Error: '%s'", error.what()); - return false; - } - if (motor_configs.IsNull()) - { - ROS_ERROR("[xs_sdk] Motor Config file was not found. Shutting down..."); - return false; - } - - ros::param::get("~mode_configs", mode_configs_file); - try - { - mode_configs = YAML::LoadFile(mode_configs_file.c_str()); - } - catch (YAML::BadFile &error) - { - ROS_ERROR("[xs_sdk] Motor Config file was not found or has a bad format. Shutting down..."); - ROS_ERROR("[xs_sdk] YAML Error: '%s'", error.what()); - return false; - } - if (mode_configs.IsNull()) - ROS_INFO("[xs_sdk] Mode Config file is empty."); - - port = motor_configs["port"].as(PORT); - if (mode_configs["port"]) - port = mode_configs["port"].as(PORT); - - YAML::Node all_motors = motor_configs["motors"]; - for (YAML::const_iterator motor_itr = all_motors.begin(); motor_itr != all_motors.end(); motor_itr++) - { - std::string motor_name = motor_itr->first.as(); - YAML::Node single_motor = all_motors[motor_name]; - uint8_t id = (uint8_t)single_motor["ID"].as(); - motor_map.insert({motor_name, {id, "position", "velocity"}}); - for (YAML::const_iterator info_itr = single_motor.begin(); info_itr != single_motor.end(); info_itr++) - { - std::string reg = info_itr->first.as(); - if (reg != "ID" && reg != "Baud_Rate") - { - int32_t value = info_itr->second.as(); - MotorInfo motor_info = {id, reg, value}; - motor_info_vec.push_back(motor_info); - } - } - } - - YAML::Node all_grippers = motor_configs["grippers"]; - for (YAML::const_iterator gripper_itr = all_grippers.begin(); gripper_itr != all_grippers.end(); gripper_itr++) - { - std::string gripper_name = gripper_itr->first.as(); - YAML::Node single_gripper = all_grippers[gripper_name]; - Gripper gripper; - gripper.horn_radius = single_gripper["horn_radius"].as(0.014); - gripper.arm_length = single_gripper["arm_length"].as(0.024); - gripper.left_finger = single_gripper["left_finger"].as("left_finger"); - gripper.right_finger = single_gripper["right_finger"].as("right_finger"); - gripper_map.insert({gripper_name, gripper}); - } - - YAML::Node joint_order = motor_configs["joint_order"]; - YAML::Node sleep_positions = motor_configs["sleep_positions"]; - JointGroup all_joints; - all_joints.joint_num = (uint8_t) joint_order.size(); - all_joints.mode = "position"; - all_joints.profile_type = "velocity"; - for (size_t i{0}; i < joint_order.size(); i++) - { - std::string joint_name = joint_order[i].as(); - all_joints.joint_names.push_back(joint_name); - all_joints.joint_ids.push_back(motor_map[joint_name].motor_id); - js_index_map.insert({joint_name, i}); - shadow_map.insert({joint_name, {joint_name}}); - sister_map.insert({joint_name, {joint_name}}); - sleep_map.insert({joint_name, sleep_positions[i].as(0)}); - if (gripper_map.count(joint_name) > 0) - { - gripper_map[joint_name].js_index = i; - gripper_order.push_back(joint_name); - } - } - - for (auto const& name : gripper_order) - { - js_index_map.insert({gripper_map[name].left_finger, js_index_map.size()}); - js_index_map.insert({gripper_map[name].right_finger, js_index_map.size()}); - } - - group_map.insert({"all", all_joints}); - all_ptr = &group_map["all"]; - - YAML::Node all_shadows = motor_configs["shadows"]; - for (YAML::const_iterator master_itr = all_shadows.begin(); master_itr != all_shadows.end(); master_itr++) - { - std::string master_name = master_itr->first.as(); - YAML::Node master = all_shadows[master_name]; - YAML::Node shadow_list = master["shadow_list"]; - for (size_t i{0}; i < shadow_list.size(); i++) - shadow_map[master_name].push_back(shadow_list[i].as()); - } - - YAML::Node all_sisters = motor_configs["sisters"]; - for (YAML::const_iterator sister_itr = all_sisters.begin(); sister_itr != all_sisters.end(); sister_itr++) - { - std::string sister_one = sister_itr->first.as(); - std::string sister_two = sister_itr->second.as(); - sister_map[sister_one].push_back(sister_two); - sister_map[sister_two].push_back(sister_one); - } + this->declare_parameter("motor_configs", ""); + this->declare_parameter("mode_configs", ""); + this->declare_parameter("load_configs", false); + this->declare_parameter("robot_description", ""); + this->declare_parameter("xs_driver_logging_level", "INFO"); - YAML::Node all_groups = motor_configs["groups"]; - for (YAML::const_iterator group_itr = all_groups.begin(); group_itr != all_groups.end(); group_itr++) - { - std::string name = group_itr->first.as(); - YAML::Node joint_list = all_groups[name]; - JointGroup group; - group.joint_num = (uint8_t) joint_list.size(); - for (size_t i{0}; i < joint_list.size(); i++) - { - std::string joint_name = joint_list[i].as(); - group.joint_names.push_back(joint_name); - group.joint_ids.push_back(motor_map[joint_name].motor_id); - } - group_map.insert({name, group}); - } - - YAML::Node pub_configs = motor_configs["joint_state_publisher"]; + this->get_parameter("motor_configs", filepath_motor_configs); + this->get_parameter("mode_configs", filepath_mode_configs); + this->get_parameter("load_configs", write_eeprom_on_startup); + this->get_parameter("xs_driver_logging_level", xs_driver_logging_level); + YAML::Node pub_configs = YAML::LoadFile(filepath_motor_configs)["joint_state_publisher"]; timer_hz = pub_configs["update_rate"].as(100); pub_states = pub_configs["publish_states"].as(true); js_topic = pub_configs["topic_name"].as("joint_states"); - - ROS_INFO("[xs_sdk] Successfully retrieved motor configs from %s.", motor_configs_file.c_str()); - return true; } -/// @brief Initializes the port to communicate with the Dynamixel servos -/// @param [out] - True if the port was successfully opened; False otherwise -bool InterbotixRobotXS::robot_init_port(void) +void InterbotixRobotXS::robot_init_publishers() { - if (!dxl_wb.init(port.c_str(), BAUDRATE)) - { - ROS_ERROR("[xs_sdk] Failed to open port at %s. Shutting down...", port.c_str()); - return false; + if (pub_states) { + pub_joint_states = this->create_publisher(js_topic, 10); } - return true; } -/// @brief Pings all motors to make sure they can be found -/// @param [out] - True if all motors were found; False otherwise -bool InterbotixRobotXS::robot_ping_motors(void) +void InterbotixRobotXS::robot_init_subscribers() { - for (auto const& motor:motor_map) - { - uint16_t model_number = 0; - if(!dxl_wb.ping(motor.second.motor_id, &model_number)) - { - ROS_ERROR( - "[xs_sdk] Can't find Dynamixel ID '%d',\tJoint Name : '%s'", - motor.second.motor_id, motor.first.c_str()); - return false; - } - else - ROS_INFO( - "[xs_sdk] Found Dynamixel ID : %d,\tModel Number : %d,\tJoint Name : %s", - motor.second.motor_id, model_number, motor.first.c_str()); - dxl_wb.torque(motor.second.motor_id, false); - } - return true; + using namespace std::placeholders; + sub_command_group = this->create_subscription( + "commands/joint_group", + 10, + std::bind(&InterbotixRobotXS::robot_sub_command_group, this, _1)); + sub_command_single = this->create_subscription( + "commands/joint_single", + 10, + std::bind(&InterbotixRobotXS::robot_sub_command_single, this, _1)); + sub_command_traj = this->create_subscription( + "commands/joint_trajectory", + 10, + std::bind(&InterbotixRobotXS::robot_sub_command_traj, this, _1)); } -/// @brief Writes some 'startup' EEPROM register values to the Dynamixel servos -/// @param [out] - True if all register values were written successfully; False otherwise -bool InterbotixRobotXS::robot_load_motor_configs(void) +void InterbotixRobotXS::robot_init_services() { - if (ros::param::param("~load_configs", LOAD_CONFIGS, true)) - { - for (auto const& motor_info:motor_info_vec) - { - if (!dxl_wb.itemWrite(motor_info.motor_id, motor_info.reg.c_str(), motor_info.value)) - { - ROS_ERROR( - "[xs_sdk] Failed to write value[%d] on items[%s] to [ID : %d]", - motor_info.value, motor_info.reg.c_str(), motor_info.motor_id); - return false; - } - } - } - else - ROS_INFO("[xs_sdk] Skipping Load Configs..."); - return true; -} - -/// @brief Retrieves information about 'Goal_XXX' and 'Present_XXX' registers -/// @details - Info includes a register's name, address, and data length -void InterbotixRobotXS::robot_init_controlItems(void) -{ - uint8_t motor_id = motor_map.begin()->second.motor_id; - - const ControlItem *goal_position = dxl_wb.getItemInfo(motor_id, "Goal_Position"); - if (!goal_position) - ROS_ERROR("[xs_sdk] Could not get 'Goal_Position' Item Info"); - - const ControlItem *goal_velocity = dxl_wb.getItemInfo(motor_id, "Goal_Velocity"); - if (!goal_velocity) goal_velocity = dxl_wb.getItemInfo(motor_id, "Moving_Speed"); - if (!goal_velocity) - ROS_ERROR("[xs_sdk] Could not get 'Goal_Velocity' or 'Moving_Speed' Item Info"); - - const ControlItem *goal_current = NULL; - for (auto const& motor:motor_map) - { - goal_current = dxl_wb.getItemInfo(motor.second.motor_id, "Goal_Current"); - if (goal_current) - break; - } - if (!goal_current) - ROS_INFO( - "[xs_sdk] Could not get 'Goal_Current' Item Info. This message can be " - "ignored if none of the robot's motors support current control."); - - const ControlItem *goal_pwm = dxl_wb.getItemInfo(motor_id, "Goal_PWM"); - if (!goal_pwm) - ROS_ERROR("[xs_sdk] Could not get 'Goal_PWM' Item Info"); - - const ControlItem *present_position = dxl_wb.getItemInfo(motor_id, "Present_Position"); - if (!present_position) - ROS_ERROR("[xs_sdk] Could not get 'Present_Position' Item Info"); - - const ControlItem *present_velocity = dxl_wb.getItemInfo(motor_id, "Present_Velocity"); - if (!present_velocity) present_velocity = dxl_wb.getItemInfo(motor_id, "Present_Speed"); - if (!present_velocity) - ROS_ERROR("[xs_sdk] Could not get 'Present_Velocity' or 'Present_Speed' Item Info"); - - const ControlItem *present_current = dxl_wb.getItemInfo(motor_id, "Present_Current"); - if (!present_current) present_current = dxl_wb.getItemInfo(motor_id, "Present_Load"); - if (!present_current) - ROS_ERROR("[xs_sdk] Could not get 'Present_Current' or 'Present_Load' Item Info"); - - control_items["Goal_Position"] = goal_position; - control_items["Goal_Velocity"] = goal_velocity; - control_items["Goal_Current"] = goal_current; - control_items["Goal_PWM"] = goal_pwm; - - control_items["Present_Position"] = present_position; - control_items["Present_Velocity"] = present_velocity; - control_items["Present_Current"] = present_current; + using namespace std::placeholders; + srv_torque_enable = this->create_service( + "torque_enable", + std::bind(&InterbotixRobotXS::robot_srv_torque_enable, this, _1, _2, _3)); + srv_reboot_motors = this->create_service( + "reboot_motors", + std::bind(&InterbotixRobotXS::robot_srv_reboot_motors, this, _1, _2, _3)); + srv_get_robot_info = this->create_service( + "get_robot_info", + std::bind(&InterbotixRobotXS::robot_srv_get_robot_info, this, _1, _2, _3)); + srv_operating_modes = this->create_service( + "set_operating_modes", + std::bind(&InterbotixRobotXS::robot_srv_set_operating_modes, this, _1, _2, _3)); + srv_motor_gains = this->create_service( + "set_motor_pid_gains", + std::bind(&InterbotixRobotXS::robot_srv_set_motor_pid_gains, this, _1, _2, _3)); + srv_set_registers = this->create_service( + "set_motor_registers", + std::bind(&InterbotixRobotXS::robot_srv_set_motor_registers, this, _1, _2, _3)); + srv_get_registers = this->create_service( + "get_motor_registers", + std::bind(&InterbotixRobotXS::robot_srv_get_motor_registers, this, _1, _2, _3)); } - /// @brief Creates SyncWrite and SyncRead Handlers to write/read data to multiple motors simultaneously -void InterbotixRobotXS::robot_init_SDK_handlers(void) +void InterbotixRobotXS::robot_init_timers() { - if (!dxl_wb.addSyncWriteHandler(control_items["Goal_Position"]->address, control_items["Goal_Position"]->data_length)) - ROS_ERROR("[xs_sdk] Failed to add SyncWriteHandler for Goal_Position."); - - if (!dxl_wb.addSyncWriteHandler(control_items["Goal_Velocity"]->address, control_items["Goal_Velocity"]->data_length)) - ROS_ERROR("[xs_sdk] Failed to add SyncWriteHandler for Goal_Velocity."); - - // only add a SyncWriteHandler for 'Goal_Current' if the register actually exists! - if (control_items["Goal_Current"]) - { - if (!dxl_wb.addSyncWriteHandler(control_items["Goal_Current"]->address, control_items["Goal_Current"]->data_length)) - ROS_ERROR("[xs_sdk] Failed to add SyncWriteHandler for Goal_Current."); - } - else - ROS_INFO("[xs_sdk] SyncWriteHandler for Goal_Current not added as it's not supported."); - - if (!dxl_wb.addSyncWriteHandler(control_items["Goal_PWM"]->address, control_items["Goal_PWM"]->data_length)) - ROS_ERROR("[xs_sdk] Failed to add SyncWriteHandler for Goal_PWM."); - - if (dxl_wb.getProtocolVersion() == 2.0f) - { - uint16_t start_address = std::min(control_items["Present_Position"]->address, control_items["Present_Current"]->address); - /* - As some models have an empty space between Present_Velocity and Present Current, read_length is modified as below. - */ - // uint16_t read_length = control_items["Present_Position"]->data_length + control_items["Present_Velocity"]->data_length + control_items["Present_Current"]->data_length; - uint16_t read_length = control_items["Present_Position"]->data_length + control_items["Present_Velocity"]->data_length + control_items["Present_Current"]->data_length+2; - if (!dxl_wb.addSyncReadHandler(start_address, read_length)) - ROS_ERROR("[xs_sdk] Failed to add SyncReadHandler"); + execute_joint_traj = false; + using namespace std::chrono_literals; + if (timer_hz != 0) { + // timer that updates the joint states with a period of 1/timer_hz + tmr_joint_states = this->create_wall_timer( + std::chrono::nanoseconds(static_cast(1.0e9 / (timer_hz))), + std::bind( + &InterbotixRobotXS::robot_update_joint_states, + this)); } } -/// @brief Loads a 'mode_configs' yaml file containing desired operating modes and sets up the motors accordingly -void InterbotixRobotXS::robot_init_operating_modes(void) +void InterbotixRobotXS::robot_wait_for_joint_states() { - YAML::Node all_shadows = motor_configs["shadows"]; - for (YAML::const_iterator master_itr = all_shadows.begin(); master_itr != all_shadows.end(); master_itr++) - { - std::string master_name = master_itr->first.as(); - YAML::Node master = all_shadows[master_name]; - if (master["calibrate"].as(false)) - { - int32_t master_position; - dxl_wb.itemRead(motor_map[master_name].motor_id, "Present_Position", &master_position); - for (auto const& shadow_name : shadow_map[master_name]) - { - if (shadow_name == master_name) continue; - dxl_wb.itemWrite(motor_map[shadow_name].motor_id, "Homing_Offset", 0); - int32_t shadow_position, shadow_drive_mode; - dxl_wb.itemRead(motor_map[shadow_name].motor_id, "Present_Position", &shadow_position); - dxl_wb.itemRead(motor_map[shadow_name].motor_id, "Drive_Mode", &shadow_drive_mode); - bool shadow_forward = (shadow_drive_mode % 2 == 0); - int32_t homing_offset; - if (shadow_forward) - homing_offset = master_position - shadow_position; - else - homing_offset = shadow_position - master_position; - dxl_wb.itemWrite(motor_map[shadow_name].motor_id, "Homing_Offset", homing_offset); - } - } - } - - YAML::Node all_groups = mode_configs["groups"]; - for (YAML::const_iterator group_itr = all_groups.begin(); group_itr != all_groups.end(); group_itr++) - { - std::string name = group_itr->first.as(); - YAML::Node single_group = all_groups[name]; - std::string operating_mode = single_group["operating_mode"].as(OP_MODE); - std::string profile_type = single_group["profile_type"].as(PROFILE_TYPE); - int32_t profile_velocity = single_group["profile_velocity"].as(PROFILE_VELOCITY); - int32_t profile_acceleration = single_group["profile_acceleration"].as(PROFILE_ACCELERATION); - robot_set_operating_modes("group", name, operating_mode, profile_type, profile_velocity, profile_acceleration); - if (!single_group["torque_enable"].as(TORQUE_ENABLE)) - robot_torque_enable("group", name, false); - } - - YAML::Node all_singles = mode_configs["singles"]; - for (YAML::const_iterator single_itr = all_singles.begin(); single_itr != all_singles.end(); single_itr++) - { - std::string single_name = single_itr->first.as(); - YAML::Node single_joint = all_singles[single_name]; - std::string operating_mode = single_joint["operating_mode"].as(OP_MODE); - std::string profile_type = single_joint["profile_type"].as(PROFILE_TYPE); - int32_t profile_velocity = single_joint["profile_velocity"].as(PROFILE_VELOCITY); - int32_t profile_acceleration = single_joint["profile_acceleration"].as(PROFILE_ACCELERATION); - robot_set_operating_modes("single", single_name, operating_mode, profile_type, profile_velocity, profile_acceleration); - if (!single_joint["torque_enable"].as(TORQUE_ENABLE)) - robot_torque_enable("single", single_name, false); + if (timer_hz == 0) { + return; } -} - -/// @brief Initialize ROS Publishers -void InterbotixRobotXS::robot_init_publishers(void) -{ - if (pub_states) - pub_joint_states = node.advertise(js_topic, 1); -} - -/// @brief Initialize ROS Subscribers -void InterbotixRobotXS::robot_init_subscribers(void) -{ - sub_command_group = node.subscribe("commands/joint_group", 5, &InterbotixRobotXS::robot_sub_command_group, this); - sub_command_single = node.subscribe("commands/joint_single", 5, &InterbotixRobotXS::robot_sub_command_single, this); - sub_command_traj = node.subscribe("commands/joint_trajectory", 5, &InterbotixRobotXS::robot_sub_command_traj, this); -} - -/// @brief Initialize ROS Services -void InterbotixRobotXS::robot_init_services(void) -{ - srv_torque_enable = node.advertiseService("torque_enable", &InterbotixRobotXS::robot_srv_torque_enable, this); - srv_reboot_motors = node.advertiseService("reboot_motors", &InterbotixRobotXS::robot_srv_reboot_motors, this); - srv_get_robot_info = node.advertiseService("get_robot_info", &InterbotixRobotXS::robot_srv_get_robot_info, this); - srv_operating_modes = node.advertiseService("set_operating_modes", &InterbotixRobotXS::robot_srv_set_operating_modes, this); - srv_motor_gains = node.advertiseService("set_motor_pid_gains", &InterbotixRobotXS::robot_srv_set_motor_pid_gains, this); - srv_set_registers = node.advertiseService("set_motor_registers", &InterbotixRobotXS::robot_srv_set_motor_registers, this); - srv_get_registers = node.advertiseService("get_motor_registers", &InterbotixRobotXS::robot_srv_get_motor_registers, this); -} - -/// @brief Initialize ROS Timers -void InterbotixRobotXS::robot_init_timers(void) -{ - execute_joint_traj = false; - if (timer_hz != 0) - tmr_joint_states = node.createTimer(ros::Duration(1.0/timer_hz), &InterbotixRobotXS::robot_update_joint_states, this); - tmr_joint_traj = node.createTimer(ros::Duration(0), &InterbotixRobotXS::robot_execute_trajectory, this, true, false); -} - -/// @brief Waits until first JointState message is received -void InterbotixRobotXS::robot_wait_for_joint_states(void) -{ - if (timer_hz == 0) return; - ros::Rate r(10); - while (ros::ok() && joint_states.name.size() == 0) - { - ROS_DEBUG("[xs_sdk::robot_wait_for_joint_states] Waiting for joint states..."); - ros::spinOnce(); + rclcpp::Rate r(10); + while (rclcpp::ok() && !joint_states.name.size()) { + rclcpp::spin_some(this->get_node_base_interface()); r.sleep(); } - ROS_DEBUG("[xs_sdk::robot_wait_for_joint_states] Joint states found. Continuing."); } -/// @brief ROS Subscriber callback function to command a group of joints -/// @param msg - JointGroupCommand message dictating the joint group to command along with the actual commands -/// @details - refer to the message definition for details -void InterbotixRobotXS::robot_sub_command_group(const interbotix_xs_msgs::JointGroupCommand &msg) +void InterbotixRobotXS::robot_sub_command_group(const JointGroupCommand::SharedPtr msg) { - robot_write_commands(msg.name, msg.cmd); + xs_driver->write_commands(msg->name, msg->cmd); } -/// @brief ROS Subscriber callback function to command a single joint -/// @param msg - JointSingleCommand message dictating the joint to command along with the actual command -/// @details - refer to the message definition for details -void InterbotixRobotXS::robot_sub_command_single(const interbotix_xs_msgs::JointSingleCommand &msg) +void InterbotixRobotXS::robot_sub_command_single(const JointSingleCommand::SharedPtr msg) { - robot_write_joint_command(msg.name, msg.cmd); + xs_driver->write_joint_command(msg->name, msg->cmd); } -/// @brief ROS Subscriber callback function to command a joint trajectory -/// @param msg - JointTrajectoryCommand message dictating the joint(s) to command along with the desired trajectory -/// @details - refer to the message definition for details -void InterbotixRobotXS::robot_sub_command_traj(const interbotix_xs_msgs::JointTrajectoryCommand &msg) +void InterbotixRobotXS::robot_sub_command_traj(const JointTrajectoryCommand::SharedPtr msg) { - if (execute_joint_traj) - { - ROS_WARN("[xs_sdk] Trajectory rejected since joints are still moving."); + using namespace std::chrono_literals; + if (execute_joint_traj) { + RCLCPP_WARN(LOGGER, "Trajectory rejected since joints are still moving."); return; } - if (msg.traj.points.size() < 2) - { - ROS_WARN("[xs_sdk] Trajectory has fewer than 2 points. Aborting..."); + + if (msg->traj.points.size() < 2) { + RCLCPP_WARN(LOGGER, "Trajectory has fewer than 2 points. Aborting..."); return; } std::vector joint_names; - if (msg.cmd_type == "group") - joint_names = group_map[msg.name].joint_names; - else if (msg.cmd_type == "single") - joint_names.push_back(msg.name); - - if (timer_hz != 0 && msg.traj.points[0].positions.size() == joint_names.size()) - { - for (size_t i{0}; i < joint_names.size(); i++) - { - float expected_state = msg.traj.points[0].positions.at(i); - float actual_state = joint_states.position.at(js_index_map[joint_names.at(i)]); - if (!(fabs(expected_state - actual_state) < 0.01)) - { - ROS_WARN("[xs_sdk] The %s joint is not at the correct initial state.", joint_names.at(i).c_str()); - ROS_WARN("[xs_sdk] Expected state: %.2f, Actual State: %.2f.", expected_state, actual_state); + if (msg->cmd_type == cmd_type::GROUP) { + joint_names = xs_driver->get_group_info(msg->name)->joint_names; + } else if (msg->cmd_type == cmd_type::SINGLE) { + joint_names.push_back(msg->name); + } + + if (timer_hz != 0 && msg->traj.points[0].positions.size() == joint_names.size()) { + for (size_t i{0}; i < joint_names.size(); i++) { + float expected_state = msg->traj.points[0].positions.at(i); + float actual_state = joint_states.position.at(xs_driver->get_js_index(joint_names.at(i))); + if (!(fabs(expected_state - actual_state) < 0.01)) { + RCLCPP_WARN( + LOGGER, + "The %s joint is not at the correct initial state.", + joint_names.at(i).c_str()); + RCLCPP_WARN( + LOGGER, + "Expected state: %.2f, Actual State: %.2f.", + expected_state, actual_state); } } } joint_traj_cmd = msg; execute_joint_traj = true; - tmr_joint_traj.setPeriod(ros::Duration(0)); - tmr_joint_traj.start(); + + // create timer that immediately triggers callback + tmr_joint_traj = create_wall_timer( + 0s, + std::bind( + &InterbotixRobotXS::robot_execute_trajectory, + this)); } -/// @brief ROS Service to torque the joints on the robot on/off -/// @param req - TorqueEnable service message request -/// @param res [out] - TorqueEnable service message response [unused] -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_torque_enable(interbotix_xs_msgs::TorqueEnable::Request &req, interbotix_xs_msgs::TorqueEnable::Response &res) +bool InterbotixRobotXS::robot_srv_torque_enable( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - robot_torque_enable(req.cmd_type, req.name, req.enable); + xs_driver->torque_enable(req->cmd_type, req->name, req->enable); return true; } -/// @brief ROS Service to reboot the motors on the robot -/// @param req - Reboot service message request -/// @param res [out] - Reboot service message response [unused] -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_reboot_motors(interbotix_xs_msgs::Reboot::Request &req, interbotix_xs_msgs::Reboot::Response &res) +bool InterbotixRobotXS::robot_srv_reboot_motors( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - robot_reboot_motors(req.cmd_type, req.name, req.enable, req.smart_reboot); + xs_driver->reboot_motors( + req->cmd_type, + req->name, + req->enable, + req->smart_reboot); return true; } -/// @brief ROS Service that allows the user to get information about the robot -/// @param req - RobotInfo service message request -/// @param res [out] - RobotInfo service message response -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_get_robot_info(interbotix_xs_msgs::RobotInfo::Request &req, interbotix_xs_msgs::RobotInfo::Response &res) +bool InterbotixRobotXS::robot_srv_get_robot_info( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; - + } bool urdf_exists = false; urdf::Model model; urdf::JointConstSharedPtr ptr; + // Parse the urdf model to get joint limit info - std::string robot_name = node.getNamespace(); - if (ros::param::has("robot_description")) - { - model.initParam(robot_name + "/robot_description"); + std::string robot_description; + this->get_parameter("robot_description", robot_description); + if (!robot_description.empty()) { + model.initString(robot_description); urdf_exists = true; } - if (req.cmd_type == "group") - { - res.joint_names = group_map[req.name].joint_names; - res.profile_type = group_map[req.name].profile_type; - res.mode = group_map[req.name].mode; - } - else if (req.cmd_type == "single") - { - res.joint_names.push_back(req.name); - res.profile_type = motor_map[req.name].profile_type; - res.mode = motor_map[req.name].mode; - } - - res.num_joints = res.joint_names.size(); - for (auto &name : res.joint_names) - { - res.joint_ids.push_back(motor_map[name].motor_id); - if (gripper_map.count(name) > 0) - { - res.joint_sleep_positions.push_back(robot_convert_angular_position_to_linear(name, 0)); - name = gripper_map[name].left_finger; - } - else - res.joint_sleep_positions.push_back(sleep_map[name]); - res.joint_state_indices.push_back(js_index_map[name]); - if (urdf_exists) - { + // get names, profile type, and operating mode + if (req->cmd_type == cmd_type::GROUP) { + res->joint_names = xs_driver->get_group_info(req->name)->joint_names; + res->profile_type = xs_driver->get_group_info(req->name)->profile_type; + res->mode = xs_driver->get_group_info(req->name)->mode; + } else if (req->cmd_type == cmd_type::SINGLE) { + res->joint_names.push_back(req->name); + res->profile_type = xs_driver->get_motor_info(req->name)->profile_type; + res->mode = xs_driver->get_motor_info(req->name)->mode; + } + + // get the number of joints in the group or single + res->num_joints = res->joint_names.size(); + + for (auto & name : res->joint_names) { + // get this joint's ID + res->joint_ids.push_back(xs_driver->get_motor_info(name)->motor_id); + if (xs_driver->is_motor_gripper(name) > 0) { + // if the joint is a gripper, add left finger joint info + res->joint_sleep_positions.push_back(xs_driver->convert_angular_position_to_linear(name, 0)); + // replace gripper joint name with name of left finger joint + name = xs_driver->get_gripper_info(name)->left_finger; + } else { + res->joint_sleep_positions.push_back(xs_driver->get_joint_sleep_position(name)); + } + + // get this joint's joint state index + res->joint_state_indices.push_back(xs_driver->get_js_index(name)); + if (urdf_exists) { + // get joint limits if the URDF exists ptr = model.getJoint(name); - res.joint_lower_limits.push_back(ptr->limits->lower); - res.joint_upper_limits.push_back(ptr->limits->upper); - res.joint_velocity_limits.push_back(ptr->limits->velocity); + res->joint_lower_limits.push_back(ptr->limits->lower); + res->joint_upper_limits.push_back(ptr->limits->upper); + res->joint_velocity_limits.push_back(ptr->limits->velocity); + } + } + if (req->name != "all") { + // if the request name is not "all", return the name from the request + res->name.push_back(req->name); + } else { + // if the request name is "all", return all group names + for (auto const &[group_name, _] : *xs_driver->get_group_map()) { + res->name.push_back(group_name); } } return true; } -/// @brief ROS Service that allows the user to change operating modes -/// @param req - OperatingModes service message request -/// @param res [out] - OperatingModes service message response [unused] -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_set_operating_modes(interbotix_xs_msgs::OperatingModes::Request &req, interbotix_xs_msgs::OperatingModes::Response &res) +bool InterbotixRobotXS::robot_srv_set_operating_modes( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - robot_set_operating_modes(req.cmd_type, req.name, req.mode, req.profile_type, req.profile_velocity, req.profile_acceleration); + xs_driver->set_operating_modes( + req->cmd_type, + req->name, + req->mode, + req->profile_type, + req->profile_velocity, + req->profile_acceleration); return true; } -/// @brief ROS Service that allows the user to set the motor firmware PID gains -/// @param req - MotorGains service message request -/// @param res [out] - MotorGains service message response [unused] -/// @details - refer to the service defintion for details -bool InterbotixRobotXS::robot_srv_set_motor_pid_gains(interbotix_xs_msgs::MotorGains::Request &req, interbotix_xs_msgs::MotorGains::Response &res) +bool InterbotixRobotXS::robot_srv_set_motor_pid_gains( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - std::vector gains = {req.kp_pos, req.ki_pos, req.kd_pos, req.k1, req.k2, req.kp_vel, req.ki_vel}; - robot_set_motor_pid_gains(req.cmd_type, req.name, gains); + std::vector gains = { + req->kp_pos, + req->ki_pos, + req->kd_pos, + req->k1, + req->k2, + req->kp_vel, + req->ki_vel}; + xs_driver->set_motor_pid_gains(req->cmd_type, req->name, gains); return true; } -/// @brief ROS Service that allows the user to change a specific register to a specific value for multiple motors -/// @param req - RegisterValues service message request -/// @param res [out] - RegisterValues service message response [unused] -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_set_motor_registers(interbotix_xs_msgs::RegisterValues::Request &req, interbotix_xs_msgs::RegisterValues::Response &res) +bool InterbotixRobotXS::robot_srv_set_motor_registers( + const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - robot_set_motor_registers(req.cmd_type, req.name, req.reg, req.value); + xs_driver->set_motor_registers(req->cmd_type, req->name, req->reg, req->value); return true; } -/// @brief ROS Service that allows the user to read a specific register on multiple motors -/// @param req - RegisterValues service message request -/// @param res [out] - RegisterValues service message response -/// @details - refer to the service definition for details -bool InterbotixRobotXS::robot_srv_get_motor_registers(interbotix_xs_msgs::RegisterValues::Request &req, interbotix_xs_msgs::RegisterValues::Response &res) +bool InterbotixRobotXS::robot_srv_get_motor_registers( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res) { - if (!robot_srv_validate(req.cmd_type, req.name)) + (void)request_header; + if (!robot_srv_validate(req->cmd_type, req->name)) { return false; + } - robot_get_motor_registers(req.cmd_type, req.name, req.reg, res.values); + xs_driver->get_motor_registers(req->cmd_type, req->name, req->reg, res->values); return true; } -/// @brief ROS One-Shot Timer used to step through a commanded joint trajectory -/// @param e - TimerEvent message [unused] -void InterbotixRobotXS::robot_execute_trajectory(const ros::TimerEvent &e) +void InterbotixRobotXS::robot_execute_trajectory() { - static size_t cntr = 1; + // get the current real time for this callback execution + rclcpp::Time current_real = this->get_clock()->now(); - if (cntr == joint_traj_cmd.traj.points.size()) - { + static size_t cntr = 1; + RCLCPP_DEBUG( + LOGGER, + "Executing trajectory step %li/%li.", + cntr, joint_traj_cmd->traj.points.size()); + + // check if end of trajectory has been reached if done, reset counter, set execution status bool + // to false, and cancel the trajectory execution timer. if not done, cancel the timer (pseudo + // one-shot), and start another. + if (cntr == joint_traj_cmd->traj.points.size()) { + if (!tmr_joint_traj->is_canceled()) { + tmr_joint_traj->cancel(); + } execute_joint_traj = false; cntr = 1; + RCLCPP_DEBUG(LOGGER, "Reached end of trajectory."); return; - } - else - { - ros::Duration period = joint_traj_cmd.traj.points[cntr].time_from_start - joint_traj_cmd.traj.points[cntr-1].time_from_start; - tmr_joint_traj.stop(); - tmr_joint_traj.setPeriod(period - (ros::Time::now() - e.current_real)); - tmr_joint_traj.start(); - } - - if (joint_traj_cmd.cmd_type == "group") - { - if (group_map[joint_traj_cmd.name].mode.find("position") != std::string::npos) - { - std::vector commands(joint_traj_cmd.traj.points[cntr].positions.begin(), joint_traj_cmd.traj.points[cntr].positions.end()); - robot_write_commands(joint_traj_cmd.name, commands); - } - else if (group_map[joint_traj_cmd.name].mode == "velocity") - { - std::vector commands(joint_traj_cmd.traj.points[cntr].velocities.begin(), joint_traj_cmd.traj.points[cntr].velocities.end()); - robot_write_commands(joint_traj_cmd.name, commands); - } - else if (group_map[joint_traj_cmd.name].mode == "pwm" || group_map[joint_traj_cmd.name].mode == "current") - { - std::vector commands(joint_traj_cmd.traj.points[cntr].effort.begin(), joint_traj_cmd.traj.points[cntr].effort.end()); - robot_write_commands(joint_traj_cmd.name, commands); + } else { + // cancel trajectory timer + if (!tmr_joint_traj->is_canceled()) { + tmr_joint_traj->cancel(); + } + + // get the length of time the timer should be if perfect + rclcpp::Duration period = std::chrono::nanoseconds( + joint_traj_cmd->traj.points[cntr].time_from_start.nanosec - + joint_traj_cmd->traj.points[cntr - 1].time_from_start.nanosec); + + // create new timer with the actual length of time it should execute + // (period - (now - start_of_callback)) + tmr_joint_traj = this->create_wall_timer( + std::chrono::nanoseconds( + period.nanoseconds() - ( + this->get_clock()->now().nanoseconds() - current_real.nanoseconds())), + std::bind( + &InterbotixRobotXS::robot_execute_trajectory, + this)); + } + + + // write commands to the motors depending on cmd_type and mode + if (joint_traj_cmd->cmd_type == cmd_type::GROUP) { + // get the mode + const std::string mode = xs_driver->get_group_info(joint_traj_cmd->name)->mode; + if ( + (mode == mode::POSITION) || + (mode == mode::EXT_POSITION) || + (mode == mode::CURRENT_BASED_POSITION) || + (mode == mode::LINEAR_POSITION)) + { + // position commands + std::vector commands( + joint_traj_cmd->traj.points[cntr].positions.begin(), + joint_traj_cmd->traj.points[cntr].positions.end()); + xs_driver->write_commands(joint_traj_cmd->name, commands); + } else if (mode == mode::VELOCITY) { + // velocity commands + std::vector commands( + joint_traj_cmd->traj.points[cntr].velocities.begin(), + joint_traj_cmd->traj.points[cntr].velocities.end()); + xs_driver->write_commands(joint_traj_cmd->name, commands); + } else if ((mode == mode::PWM) || (mode == mode::CURRENT)) { + // effort commands + std::vector commands( + joint_traj_cmd->traj.points[cntr].effort.begin(), + joint_traj_cmd->traj.points[cntr].effort.end()); + xs_driver->write_commands(joint_traj_cmd->name, commands); + } + } else if (joint_traj_cmd->cmd_type == cmd_type::SINGLE) { + const std::string mode = xs_driver->get_motor_info(joint_traj_cmd->name)->mode; + if ( + (mode == mode::POSITION) || + (mode == mode::EXT_POSITION) || + (mode == mode::CURRENT_BASED_POSITION) || + (mode == mode::LINEAR_POSITION)) + { + // position commands + xs_driver->write_joint_command( + joint_traj_cmd->name, + joint_traj_cmd->traj.points[cntr].positions.at(0)); + } else if (mode == mode::VELOCITY) { + // velocity commands + xs_driver->write_joint_command( + joint_traj_cmd->name, + joint_traj_cmd->traj.points[cntr].velocities.at(0)); + } else if ((mode == mode::PWM) || (mode == mode::CURRENT)) { + // effort commands + xs_driver->write_joint_command( + joint_traj_cmd->name, + joint_traj_cmd->traj.points[cntr].effort.at(0)); } } - else if (joint_traj_cmd.cmd_type == "single") - { - if (motor_map[joint_traj_cmd.name].mode.find("position") != std::string::npos) - robot_write_joint_command(joint_traj_cmd.name, joint_traj_cmd.traj.points[cntr].positions.at(0)); - else if (motor_map[joint_traj_cmd.name].mode == "velocity") - robot_write_joint_command(joint_traj_cmd.name, joint_traj_cmd.traj.points[cntr].velocities.at(0)); - else if (motor_map[joint_traj_cmd.name].mode == "pwm" || motor_map[joint_traj_cmd.name].mode == "current") - robot_write_joint_command(joint_traj_cmd.name, joint_traj_cmd.traj.points[cntr].effort.at(0)); - } cntr++; } -/// @brief ROS Timer that reads current states from all the motors and publishes them to the joint_states topic -/// @param e - TimerEvent message [unused] -void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e) +void InterbotixRobotXS::robot_update_joint_states() { - bool result = false; - const char* log; + sensor_msgs::msg::JointState joint_state_msg; - sensor_msgs::JointState joint_state_msg; + joint_state_msg.name = xs_driver->get_all_joint_names(); - std::vector get_current(all_ptr->joint_num, 0); - std::vector get_velocity(all_ptr->joint_num, 0); - std::vector get_position(all_ptr->joint_num, 0); - joint_state_msg.name = all_ptr->joint_names; - - if (dxl_wb.getProtocolVersion() == 2.0f) - { - // Checks if data can be sent properly - if (!dxl_wb.syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - all_ptr->joint_ids.data(), - all_ptr->joint_num, - &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - } - - // Gets present current of all servos - if (!dxl_wb.getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - all_ptr->joint_ids.data(), - all_ptr->joint_num, - control_items["Present_Current"]->address, - control_items["Present_Current"]->data_length, - get_current.data(), - &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - } - - // Gets present velocity of all servos - if (!dxl_wb.getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - all_ptr->joint_ids.data(), - all_ptr->joint_num, - control_items["Present_Velocity"]->address, - control_items["Present_Velocity"]->data_length, - get_velocity.data(), - &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - } - - // Gets present position of all servos - if (!dxl_wb.getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - all_ptr->joint_ids.data(), - all_ptr->joint_num, - control_items["Present_Position"]->address, - control_items["Present_Position"]->data_length, - get_position.data(), - &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - } - - uint8_t index = 0; - for (auto const& id : all_ptr->joint_ids) - { - float position = 0; - float velocity = 0; - float effort = 0; - - if (strcmp(dxl_wb.getModelName(id), "XL-320") == 0) effort = dxl_wb.convertValue2Load(get_current.at(index)); - else effort = dxl_wb.convertValue2Current(get_current.at(index)); - velocity = dxl_wb.convertValue2Velocity(id, get_velocity.at(index)); - position = dxl_wb.convertValue2Radian(id, get_position.at(index)); - joint_state_msg.effort.push_back(effort); - joint_state_msg.velocity.push_back(velocity); - joint_state_msg.position.push_back(position); - index++; - } - } - else if(dxl_wb.getProtocolVersion() == 1.0f) - { - uint16_t length_of_data = control_items["Present_Position"]->data_length + - control_items["Present_Velocity"]->data_length + - control_items["Present_Current"]->data_length; - std::vector get_all_data(length_of_data, 0); - - for (auto const& id : all_ptr->joint_ids) - { - if (!dxl_wb.readRegister(id, - control_items["Present_Position"]->address, - length_of_data, - get_all_data.data(), - &log)) - { - ROS_ERROR("[xs_sdk] %s", log); - } - - int16_t effort_raw = DXL_MAKEWORD(get_all_data.at(4), get_all_data.at(5)); - int32_t velocity_raw = DXL_MAKEWORD(get_all_data.at(2), get_all_data.at(3)); - int32_t position_raw = DXL_MAKEWORD(get_all_data.at(0), get_all_data.at(1)); - - // Convert raw register values to the metric system - float effort = dxl_wb.convertValue2Load(effort_raw); - float velocity = dxl_wb.convertValue2Velocity(id, velocity_raw); - float position = dxl_wb.convertValue2Radian(id, position_raw); - - joint_state_msg.effort.push_back(effort); - joint_state_msg.velocity.push_back(velocity); - joint_state_msg.position.push_back(position); - } - } - for (auto const& name : gripper_order) - { - joint_state_msg.name.push_back(gripper_map[name].left_finger.c_str()); - joint_state_msg.name.push_back(gripper_map[name].right_finger.c_str()); - float pos = robot_convert_angular_position_to_linear(name, joint_state_msg.position.at(gripper_map[name].js_index)); + xs_driver->get_joint_states( + "all", + &joint_state_msg.position, + &joint_state_msg.velocity, + &joint_state_msg.effort); + for (auto const & name : xs_driver->get_gripper_order()) { + joint_state_msg.name.push_back(xs_driver->get_gripper_info(name)->left_finger.c_str()); + joint_state_msg.name.push_back(xs_driver->get_gripper_info(name)->right_finger.c_str()); + float pos = xs_driver->convert_angular_position_to_linear( + name, joint_state_msg.position.at(xs_driver->get_gripper_info(name)->js_index)); joint_state_msg.position.push_back(pos); joint_state_msg.position.push_back(-pos); joint_state_msg.velocity.push_back(0); @@ -1227,46 +515,44 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e) joint_state_msg.effort.push_back(0); joint_state_msg.effort.push_back(0); } + // Publish the message to the joint_states topic - joint_state_msg.header.stamp = ros::Time::now(); + joint_state_msg.header.stamp = this->get_clock()->now(); joint_states = joint_state_msg; - if (pub_states) pub_joint_states.publish(joint_state_msg); + if (pub_states) { + pub_joint_states->publish(joint_state_msg); + } } -/// @brief Checks service call requests for validity -/// @param cmd_type request cmd_type field -/// @param name request name field -/// @returns true if the service call request is valid, false otherwise -/// @details cmd_type must be 'single' or 'group'; name must be in the group_map or motor_map -bool InterbotixRobotXS::robot_srv_validate(const std::string &cmd_type, std::string &name) +bool InterbotixRobotXS::robot_srv_validate( + const std::string & cmd_type, + const std::string & name) { - if (cmd_type == "group") // if group command... - { - if (group_map.count(name) == 1) // if group name is valid, return true - { - return true; - } - else // otherwise error and return false - { - ROS_ERROR("[xs_sdk] Invalid service call. Group '%s' does not exist. Was it added to the motor config file?", name.c_str()); + if (cmd_type == cmd_type::GROUP) { + if (!xs_driver->get_group_map()->count(name)) { + RCLCPP_ERROR( + LOGGER, + "Group '%s' does not exist. Was it added to the motor config file?", + name.c_str()); return false; } - } - else if (cmd_type == "single") // if single joint command... - { - if (motor_map.count(name) == 1) // if joint name is valid, return true - { - return true; - } - else // otherwise error and return false - { - ROS_ERROR("[xs_sdk] Invalid service call. Joint '%s' does not exist. Was it added to the motor config file?", name.c_str()); + } else if (cmd_type == cmd_type::SINGLE) { + if (!xs_driver->get_motor_map()->count(name)) { + RCLCPP_ERROR( + LOGGER, + "Joint '%s' does not exist. Was it added to the motor config file?", + name.c_str()); return false; } - } - else // if command type is invalid, error and return false - { - ROS_ERROR("[xs_sdk] Invalid service call. cmd_type '%s'. Choices are 'group' or 'single'.", cmd_type.c_str()); + } else { + // if command type is invalid, error and return false + RCLCPP_ERROR( + LOGGER, + "cmd_type was '%s'. Choices are 'group' or 'single'.", + cmd_type.c_str()); return false; } + return true; } + +} // namespace interbotix_xs