diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml new file mode 100644 index 0000000..adf4943 --- /dev/null +++ b/.github/workflows/ros-ci.yml @@ -0,0 +1,45 @@ +name: ros-ci + +# Controls when the action will run. Triggers the workflow on push or pull request +on: + push: + branches: [ humble ] + pull_request: + branches: [ humble ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + humble: + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + ros_distribution: + - humble + include: + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 + # # Rolling + # - docker_image: ubuntu:jammy + # ros_distribution: rolling + # ros_version: 2 + container: + image: ${{ matrix.docker_image }} + steps: + - name: Setup directories + run: mkdir -p ros_ws/src + - name: checkout + uses: actions/checkout@v3 + with: + path: ros_ws/src + - name: Setup ROS environment + uses: ros-tooling/setup-ros@0.7.1 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Build and Test + uses: ros-tooling/action-ros-ci@0.3.5 + with: + package-name: dynamixel_hardware_interface + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/dynamixel_hardware_interface/main/dynamixel_hardware_interface_ci.repos" diff --git a/.gitignore b/.gitignore index 979cf4c..95e3931 100644 --- a/.gitignore +++ b/.gitignore @@ -1,61 +1,533 @@ -# Compiled Object files -*.slo -*.lo +############################## +# C +############################## + +## Prerequisites +*.d + +## Object files *.o +*.ko *.obj +*.elf + +## Linker output +*.ilk -# Precompiled Headers +### (reserved) +*.map +*.exp + +## Precompiled Headers *.gch *.pch -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod +## Libraries +*.lib -# Compiled Static libraries -*.lai -*.la +### (reserved) *.a -*.lib +*.la +*.lo -# Executables +## Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +## Executables *.exe *.out *.app +*.i*86 +*.x86_64 +*.hex -# user setting -*.*~ -*.bak -.svn -.tags -.tags_sorted_by_file -*.cfgc -*.txt.user* -*-build/ -cpp/ -bin/ +## Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +## Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +modules.order +Module.symvers +Mkfile.old +dkms.conf + + +############################## +# C++ +############################## + +## Prerequisites +*.dpp + +## Compiled Object files +*.slo + +## Fortran module files +*.smod + +## Compiled Static libraries +*.lai + + +############################## +# Python +############################## + +## Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +## Distribution / packaging +.Python build/ -msg_gen/ -srv_gen/ -docs/ -lib/ -cmake_install.cmake -Makefile -CMakeFiles/ -assets/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ + +### (reserved) lib/ +### (reserved) lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +## PyInstaller +## Usually these files are written by a python script from a template +## before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +## Installer logs +pip-log.txt +pip-delete-this-directory.txt + +## Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ +.pytest_cache/ + +## Translations +*.mo +*.pot + +## Django stuff: +*.log +local_settings.py +db.sqlite3 + +## Flask stuff: +instance/ +.webassets-cache + +## Scrapy stuff: +.scrapy + +## Sphinx documentation +docs/_build/ + +## PyBuilder +target/ + +## Jupyter Notebook +.ipynb_checkpoints + +## pyenv +.python-version + +## celery beat schedule file +celerybeat-schedule + +## SageMath parsed files +*.sage.py + +## Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +## Spyder project settings +.spyderproject +.spyproject + +## Rope project settings +.ropeproject + +## mkdocs documentation +/site + +## mypy +.mypy_cache/ + + +############################## +# Java +############################## + +## Compiled class file +*.class + +## Log file +*.log + +## BlueJ files +*.ctxt + +## Mobile Tools for Java (J2ME) +.mtj.tmp/ + +## Package Files +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +## virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + + +############################## +# Android +############################## + +## Built application files +*.apk +*.ap_ + +## Files for the ART/Dalvik VM +*.dex + +## Generated files +bin/ gen/ -build.xml -lint.xml +out/ + +## Gradle files +.gradle/ +build/ + +## Local configuration file (sdk path, etc) local.properties -proguard-project.txt -build-*/ + +## Proguard folder generated by Eclipse +proguard/ + +## Android Studio Navigation editor temp files +.navigation/ + +## Android Studio captures folder +captures/ + +## IntelliJ +*.iml +.idea/workspace.xml +.idea/tasks.xml +.idea/gradle.xml +.idea/assetWizardSettings.xml +.idea/dictionaries +.idea/libraries +.idea/caches + +## Keystore files +## Uncomment the following line if you do not want to check your keystore files in. +*.jks + +## External native build folder generated in Android Studio 2.2 and later +.externalNativeBuild + +## Google Services (e.g. APIs or Firebase) +google-services.json + +## Freeline +freeline.py +freeline/ +freeline_project_description.json + +## fastlane +fastlane/report.xml +fastlane/Preview.html +fastlane/screenshots +fastlane/test_output +fastlane/readme.md + + +############################## +# CUDA +############################## +*.i +*.ii +*.gpu +*.ptx +*.cubin +*.fatbin + + +############################## +# CMAKE +############################## +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake + + +############################## +# QT +############################## + +## Qt-es +object_script.*.Release +object_script.*.Debug +*_plugin_import.cpp +/.qmake.cache +/.qmake.stash +*.pro.user +*.pro.user.* +*.qbs.user +*.qbs.user.* +*.moc +moc_*.cpp +moc_*.h +qrc_*.cpp +ui_*.h +*.qmlc +*.jsc + +### (reserved) +Makefile* +*build-* + +## Qt unit tests +target_wrapper.* + +## QtCreator *.autosave -*.pyc -*.dat -*.txt.user -*.gch -/.project + +## QtCreator Qml +*.qmlproject.user +*.qmlproject.user.* + +## QtCreator CMake +CMakeLists.txt.user* + + +############################## +# Node.js +############################## + +## Logs +logs +*.log +npm-debug.log* +yarn-debug.log* +yarn-error.log* +lerna-debug.log* + +## Diagnostic reports (https://nodejs.org/api/report.html) +report.[0-9]*.[0-9]*.[0-9]*.[0-9]*.json + +## Runtime data +pids +*.pid +*.seed +*.pid.lock + +## Directory for instrumented libs generated by jscoverage/JSCover +lib-cov + +## Coverage directory used by tools like istanbul +coverage +*.lcov + +## nyc test coverage +.nyc_output + +## Grunt intermediate storage (https://gruntjs.com/creating-plugins#storing-task-files) +.grunt + +## Bower dependency directory (https://bower.io/) +bower_components + +## node-waf configuration +.lock-wscript + +## Compiled binary addons (https://nodejs.org/api/addons.html) +build/Release + +## Dependency directories +node_modules/ +jspm_packages/ + +## TypeScript v1 declaration files +typings/ + +## TypeScript cache +*.tsbuildinfo + +## Optional npm cache directory +.npm + +## Optional eslint cache +.eslintcache + +## Microbundle cache +.rpt2_cache/ +.rts2_cache_cjs/ +.rts2_cache_es/ +.rts2_cache_umd/ + +## Optional REPL history +.node_repl_history + +## Output of 'npm pack' +*.tgz + +## Yarn Integrity file +.yarn-integrity + +## dotenv environment variables file +.env +.env.test + +## parcel-bundler cache (https://parceljs.org/) +.cache + +## Next.js build output +.next + +## Nuxt.js build / generate output +.nuxt +dist + +## Gatsby files +.cache/ + +## vuepress build output +.vuepress/dist + +## Serverless directories +.serverless/ + +## FuseBox cache +.fusebox/ + +## DynamoDB Local files +.dynamodb/ + +## TernJS port file +.tern-port + +## Yarn +.yarn/* +!.yarn/releases +!.yarn/plugins +!.yarn/sdks +.pnp.* + + +############################## +# Quasar +############################## + +.DS_Store +.thumbs.db +node_modules + +# Quasar core related directories +.quasar +/dist + +# Cordova related directories and files +/src-cordova/node_modules +/src-cordova/platforms +/src-cordova/plugins +/src-cordova/www + +# Capacitor related directories and files +/src-capacitor/www +/src-capacitor/node_modules + +# BEX related directories and files +/src-bex/www +/src-bex/js/core + +# Log files +npm-debug.log* +yarn-debug.log* +yarn-error.log* + +# Editor directories and files +.idea +*.suo +*.ntvs* +*.njsproj +*.sln + + +############################## +# ROS +############################## + +## Colcon/Catkin ignore files +CATKIN_IGNORE +AMENT_IGNORE +COLCON_IGNORE + + +############################## +# ETC. +############################## + +## Ignore generated docs +*.dox +*.wikidoc + +## doxygen +doc/ + +## IDE: vscode +.vscode/ + +## IDE: Emacs +.#* + +## IDE: VIM +*.swp + +## Setting files +*.settings + +## GurumDDS +*.ar diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c17765e..40a877c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.0 (2025-01-17) +------------------ +* Extend Bulk/Sync Selection Logic to Include Indirect Operations +* Enhance DXL item initialization by prioritizing 'Limit' parameters +* Contributors: Woojin Wie + +1.1.0 (2024-12-27) +------------------ +* Added new control table entries for Dynamixel X +* Contributors: Woojin Wie, Hye-Jong Kim + 1.0.0 (2024-12-04) ------------------ * First release of dynamixel_hardware_interface package diff --git a/dynamixel_hardware_interface_ci.repos b/dynamixel_hardware_interface_ci.repos new file mode 100644 index 0000000..1aa12f4 --- /dev/null +++ b/dynamixel_hardware_interface_ci.repos @@ -0,0 +1,9 @@ +repositories: + utils/DynamixelSDK: + type: git + url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git + version: humble + dynamixel_hardware_interface/dynamixel_interfaces: + type: git + url: https://github.com/ROBOTIS-GIT/dynamixel_interfaces.git + version: humble diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index 095d4d7..11763ba 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -1,19 +1,18 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM, Sungho Woo */ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_ @@ -77,7 +76,7 @@ typedef struct uint16_t indirect_data_addr; ///< Base address for indirect data. uint16_t cnt; ///< Number of control items. uint8_t size; ///< Total size in bytes. - std::vector item_name; ///< Names of the control items. + std::vector item_name; ///< Names of the control items. std::vector item_size; ///< Sizes of each control item in bytes. } IndirectInfo; @@ -103,7 +102,7 @@ typedef struct std::vector item_name; ///< List of control item names. std::vector item_size; ///< Sizes of the control items. std::vector item_addr; ///< Addresses of the control items. - std::vector> item_data_ptr_vec; ///< Pointers to the data. + std::vector> item_data_ptr_vec; ///< Pointers to the data. } RWItemList; class Dynamixel diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index 78477e5..e099d3a 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -1,31 +1,32 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM */ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ -#include +#include +#include + #include #include -#include -#include -#include #include -#include +#include +#include + +#include namespace dynamixel_hardware_interface { diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index dbd2bfd..ea239ab 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -1,19 +1,18 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM, Sungho Woo */ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_ @@ -65,7 +64,7 @@ typedef struct HandlerVarType_ uint8_t id; /**< ID of the Dynamixel component. */ std::string name; /**< Name of the component. */ std::vector interface_name_vec; /**< Vector of interface names. */ - std::vector> value_ptr_vec; /**< Vector of pointers to interface values. */ + std::vector> value_ptr_vec; /**< Vector interface values. */ } HandlerVarType; /** @@ -117,7 +116,8 @@ class DynamixelHardware : public * @return Callback return indicating success or error. */ DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC - hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) + override; /** * @brief Exports state interfaces for ROS2. @@ -139,7 +139,8 @@ class DynamixelHardware : public * @return Callback return indicating success or error. */ DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC - hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) + override; /** * @brief Callback for deactivating the hardware interface. @@ -147,7 +148,8 @@ class DynamixelHardware : public * @return Callback return indicating success or error. */ DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC - hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) + override; /** * @brief Reads data from the hardware. diff --git a/package.xml b/package.xml index 4eb77ce..6e1432f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ dynamixel_hardware_interface - 1.0.0 + 1.2.0 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/2xc430_w250.model b/param/dxl_model/2xc430_w250.model index c55a0cb..59e7d1f 100644 --- a/param/dxl_model/2xc430_w250.model +++ b/param/dxl_model/2xc430_w250.model @@ -59,7 +59,7 @@ Address Size Data Name 146 1 Present Temperature 168 2 Indirect Address 1 224 1 Indirect Data 1 -168 2 Indirect Address Read -224 1 Indirect Data Read -578 2 Indirect Address Write -634 1 Indirect Data Write +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/2xl430_w250.model b/param/dxl_model/2xl430_w250.model new file mode 100644 index 0000000..59e7d1f --- /dev/null +++ b/param/dxl_model/2xl430_w250.model @@ -0,0 +1,65 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 6b5b920..3fd5e86 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -1,10 +1,38 @@ Number Name +350 xl320.model 1000 xh430_w350.model +1001 xd430_t350.model +1010 xh430_w210.model +1011 xd430_t210.model 1020 xm430_w350.model +1030 xm430_w210.model +1040 xh430_v350.model +1050 xh430_v210.model 1060 xl430_w250.model +1070 xc430_w150.model +1070 xc430_w150.model 1080 xc430_w240.model +1080 xc430_w240.model +1090 2xl430_w250.model 1100 xh540_w270.model +1101 xd540_t270.model +1110 xh540_w150.model +1111 xd540_t150.model +1120 xm540_w270.model +1130 xm540_w150.model +1140 xh540_v270.model +1150 xh540_v150.model 1160 2xc430_w250.model +1170 xw540_t260.model +1180 xw540_t140.model +1190 xl330_m077.model +1200 xl330_m288.model +1210 xc330_t181.model +1220 xc330_t288.model +1230 xc330_m181.model +1240 xc330_m288.model +1270 xw430_t333.model +1310 xw540_h260.model 4000 ym070_210_m001.model 4020 ym070_210_r051.model 4030 ym070_210_r099.model diff --git a/param/dxl_model/rh_p12_rn.model b/param/dxl_model/rh_p12_rn.model index f922f9d..428fbee 100644 --- a/param/dxl_model/rh_p12_rn.model +++ b/param/dxl_model/rh_p12_rn.model @@ -67,7 +67,7 @@ Address Size Data Name 592 2 Present Input Voltage 594 1 Present Temperature 634 1 Indirect Data 1 -168 2 Indirect Address Read -634 1 Indirect Data Read -296 2 Indirect Address Write -698 1 Indirect Data Write +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/xc330_m181.model b/param/dxl_model/xc330_m181.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xc330_m181.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_m288.model b/param/dxl_model/xc330_m288.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xc330_m288.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xc330_t181.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xc330_t288.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc430_w150.model b/param/dxl_model/xc430_w150.model new file mode 100644 index 0000000..59e7d1f --- /dev/null +++ b/param/dxl_model/xc430_w150.model @@ -0,0 +1,65 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xc430_w240.model b/param/dxl_model/xc430_w240.model index c55a0cb..59e7d1f 100644 --- a/param/dxl_model/xc430_w240.model +++ b/param/dxl_model/xc430_w240.model @@ -59,7 +59,7 @@ Address Size Data Name 146 1 Present Temperature 168 2 Indirect Address 1 224 1 Indirect Data 1 -168 2 Indirect Address Read -224 1 Indirect Data Read -578 2 Indirect Address Write -634 1 Indirect Data Write +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xd430_t210.model b/param/dxl_model/xd430_t210.model new file mode 100644 index 0000000..6ca9523 --- /dev/null +++ b/param/dxl_model/xd430_t210.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xd430_t350.model b/param/dxl_model/xd430_t350.model new file mode 100644 index 0000000..6ca9523 --- /dev/null +++ b/param/dxl_model/xd430_t350.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xd540_t150.model b/param/dxl_model/xd540_t150.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xd540_t150.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xd540_t270.model b/param/dxl_model/xd540_t270.model new file mode 100644 index 0000000..a020845 --- /dev/null +++ b/param/dxl_model/xd540_t270.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read \ No newline at end of file diff --git a/param/dxl_model/xh430_v210.model b/param/dxl_model/xh430_v210.model new file mode 100644 index 0000000..da137fd --- /dev/null +++ b/param/dxl_model/xh430_v210.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read \ No newline at end of file diff --git a/param/dxl_model/xh430_v350.model b/param/dxl_model/xh430_v350.model new file mode 100644 index 0000000..6ca9523 --- /dev/null +++ b/param/dxl_model/xh430_v350.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh430_w210.model b/param/dxl_model/xh430_w210.model new file mode 100644 index 0000000..6ca9523 --- /dev/null +++ b/param/dxl_model/xh430_w210.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh430_w350.model b/param/dxl_model/xh430_w350.model index c256b36..6ca9523 100644 --- a/param/dxl_model/xh430_w350.model +++ b/param/dxl_model/xh430_w350.model @@ -61,7 +61,7 @@ Address Size Data Name 146 1 Present Temperature 168 2 Indirect Address 1 224 1 Indirect Data 1 -168 2 Indirect Address Read -224 1 Indirect Data Read -578 2 Indirect Address Write -634 1 Indirect Data Write +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh540_v150.model b/param/dxl_model/xh540_v150.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xh540_v150.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh540_v270.model b/param/dxl_model/xh540_v270.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xh540_v270.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh540_w150.model b/param/dxl_model/xh540_w150.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xh540_w150.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xh540_w270.model b/param/dxl_model/xh540_w270.model index 264645d..f7d3ae5 100644 --- a/param/dxl_model/xh540_w270.model +++ b/param/dxl_model/xh540_w270.model @@ -66,4 +66,8 @@ Address Size Data Name 154 2 External Port Data 2 156 2 External Port Data 3 168 2 Indirect Address 1 -224 1 Indirect Data 1 \ No newline at end of file +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xl320.model b/param/dxl_model/xl320.model new file mode 100644 index 0000000..4a61382 --- /dev/null +++ b/param/dxl_model/xl320.model @@ -0,0 +1,41 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 1 Firmware Version +3 1 ID +4 1 Baud Rate +5 1 Return Delay Time +6 2 CW Angle Limit +8 2 CCW Angle Limit +11 1 Control Mode +12 1 Temperature Limit +13 1 Min Voltage Limit +14 1 Max Voltage Limit +15 2 Max Torque +17 1 Status Return Level +18 1 Shutdown +24 1 Torque Enable +25 1 LED +27 1 D Gain +28 1 I Gain +29 1 P Gain +30 2 Goal Position +32 2 Moving Speed +35 2 Torque Limit +37 2 Present Position +39 2 Present Speed +41 2 Present Load +45 1 Present Voltage +46 1 Present Temperature +47 1 Registered Instruction +49 1 Moving +50 1 Hardware Error Status +51 2 Punch diff --git a/param/dxl_model/xl330_m077.model b/param/dxl_model/xl330_m077.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xl330_m077.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xl330_m288.model b/param/dxl_model/xl330_m288.model new file mode 100644 index 0000000..cb4a784 --- /dev/null +++ b/param/dxl_model/xl330_m288.model @@ -0,0 +1,68 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xl430_w250.model b/param/dxl_model/xl430_w250.model index c55a0cb..59e7d1f 100644 --- a/param/dxl_model/xl430_w250.model +++ b/param/dxl_model/xl430_w250.model @@ -59,7 +59,7 @@ Address Size Data Name 146 1 Present Temperature 168 2 Indirect Address 1 224 1 Indirect Data 1 -168 2 Indirect Address Read -224 1 Indirect Data Read -578 2 Indirect Address Write -634 1 Indirect Data Write +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xm430_w210.model b/param/dxl_model/xm430_w210.model new file mode 100644 index 0000000..6ca9523 --- /dev/null +++ b/param/dxl_model/xm430_w210.model @@ -0,0 +1,67 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xm430_w350.model b/param/dxl_model/xm430_w350.model index c256b36..6ca9523 100644 --- a/param/dxl_model/xm430_w350.model +++ b/param/dxl_model/xm430_w350.model @@ -61,7 +61,7 @@ Address Size Data Name 146 1 Present Temperature 168 2 Indirect Address 1 224 1 Indirect Data 1 -168 2 Indirect Address Read -224 1 Indirect Data Read -578 2 Indirect Address Write -634 1 Indirect Data Write +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xm540_w150.model b/param/dxl_model/xm540_w150.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xm540_w150.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xm540_w270.model b/param/dxl_model/xm540_w270.model new file mode 100644 index 0000000..f7d3ae5 --- /dev/null +++ b/param/dxl_model/xm540_w270.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xw430_t200.model b/param/dxl_model/xw430_t200.model new file mode 100644 index 0000000..d6a2105 --- /dev/null +++ b/param/dxl_model/xw430_t200.model @@ -0,0 +1,66 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xw430_t333.model b/param/dxl_model/xw430_t333.model new file mode 100644 index 0000000..d6a2105 --- /dev/null +++ b/param/dxl_model/xw430_t333.model @@ -0,0 +1,66 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xw540_h260.model b/param/dxl_model/xw540_h260.model new file mode 100644 index 0000000..d6a2105 --- /dev/null +++ b/param/dxl_model/xw540_h260.model @@ -0,0 +1,66 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xw540_t140.model b/param/dxl_model/xw540_t140.model new file mode 100644 index 0000000..d6a2105 --- /dev/null +++ b/param/dxl_model/xw540_t140.model @@ -0,0 +1,66 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/xw540_t260.model b/param/dxl_model/xw540_t260.model new file mode 100644 index 0000000..d6a2105 --- /dev/null +++ b/param/dxl_model/xw540_t260.model @@ -0,0 +1,66 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/param/dxl_model/ym070_210_a099.model b/param/dxl_model/ym070_210_a099.model index 6aaa7d8..a530828 100644 --- a/param/dxl_model/ym070_210_a099.model +++ b/param/dxl_model/ym070_210_a099.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym070_210_m001.model b/param/dxl_model/ym070_210_m001.model index 8f977ee..4cc5c06 100644 --- a/param/dxl_model/ym070_210_m001.model +++ b/param/dxl_model/ym070_210_m001.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym070_210_r051.model b/param/dxl_model/ym070_210_r051.model index 125b20c..04d5d9c 100644 --- a/param/dxl_model/ym070_210_r051.model +++ b/param/dxl_model/ym070_210_r051.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym070_210_r099.model b/param/dxl_model/ym070_210_r099.model index 22072ef..900af9f 100644 --- a/param/dxl_model/ym070_210_r099.model +++ b/param/dxl_model/ym070_210_r099.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym080_220_m001.model b/param/dxl_model/ym080_220_m001.model index 8f977ee..4cc5c06 100644 --- a/param/dxl_model/ym080_220_m001.model +++ b/param/dxl_model/ym080_220_m001.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym080_230_a099.model b/param/dxl_model/ym080_230_a099.model index 6aaa7d8..a530828 100644 --- a/param/dxl_model/ym080_230_a099.model +++ b/param/dxl_model/ym080_230_a099.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym080_230_r051.model b/param/dxl_model/ym080_230_r051.model index 125b20c..04d5d9c 100644 --- a/param/dxl_model/ym080_230_r051.model +++ b/param/dxl_model/ym080_230_r051.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/param/dxl_model/ym080_230_r099.model b/param/dxl_model/ym080_230_r099.model index 22072ef..900af9f 100644 --- a/param/dxl_model/ym080_230_r099.model +++ b/param/dxl_model/ym080_230_r099.model @@ -102,7 +102,7 @@ Address Size Data Name 571 1 Present Motor Temperature 634 1 Indirect Data 1 919 1 Backup Ready -256 2 Indirect Address Read -634 1 Indirect Data Read -384 2 Indirect Address Write -698 1 Indirect Data Write +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 037d66b..4292dd0 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -1,19 +1,18 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM, Sungho Woo */ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp" @@ -658,6 +657,23 @@ DxlError Dynamixel::WriteMultiDxlData() bool Dynamixel::checkReadType() { for (size_t dxl_index = 1; dxl_index < read_data_list_.size(); dxl_index++) { + // Check if Indirect Data Read address and size are different + uint16_t indirect_addr[2]; // [i-1], [i] + uint8_t indirect_size[2]; // [i-1], [i] + + if (!dxl_info_.GetDxlControlItem( + read_data_list_.at(dxl_index).id, "Indirect Data Read", indirect_addr[1], + indirect_size[1]) || + !dxl_info_.GetDxlControlItem( + read_data_list_.at(dxl_index - 1).id, "Indirect Data Read", indirect_addr[0], + indirect_size[0])) + { + return BULK; + } + if (indirect_addr[1] != indirect_addr[0] || indirect_size[1] != indirect_size[0]) { + return BULK; + } + if (read_data_list_.at(dxl_index).item_name.size() != read_data_list_.at(dxl_index - 1).item_name.size()) { @@ -667,7 +683,11 @@ bool Dynamixel::checkReadType() item_index++) { if (read_data_list_.at(dxl_index).item_name.at(item_index) != - read_data_list_.at(dxl_index - 1).item_name.at(item_index)) + read_data_list_.at(dxl_index - 1).item_name.at(item_index) || + read_data_list_.at(dxl_index).item_addr.at(item_index) != + read_data_list_.at(dxl_index - 1).item_addr.at(item_index) || + read_data_list_.at(dxl_index).item_size.at(item_index) != + read_data_list_.at(dxl_index - 1).item_size.at(item_index)) { return BULK; } @@ -679,6 +699,22 @@ bool Dynamixel::checkReadType() bool Dynamixel::checkWriteType() { for (size_t dxl_index = 1; dxl_index < write_data_list_.size(); dxl_index++) { + // Check if Indirect Data Write address and size are different + uint16_t indirect_addr[2]; // [i-1], [i] + uint8_t indirect_size[2]; // [i-1], [i] + if (!dxl_info_.GetDxlControlItem( + write_data_list_.at(dxl_index).id, "Indirect Data Write", indirect_addr[1], + indirect_size[1]) || + !dxl_info_.GetDxlControlItem( + write_data_list_.at(dxl_index - 1).id, "Indirect Data Write", indirect_addr[0], + indirect_size[0])) + { + return BULK; + } + if (indirect_addr[1] != indirect_addr[0] || indirect_size[1] != indirect_size[0]) { + return BULK; + } + if (write_data_list_.at(dxl_index).item_name.size() != write_data_list_.at(dxl_index - 1).item_name.size()) { @@ -688,7 +724,11 @@ bool Dynamixel::checkWriteType() item_index++) { if (write_data_list_.at(dxl_index).item_name.at(item_index) != - write_data_list_.at(dxl_index - 1).item_name.at(item_index)) + write_data_list_.at(dxl_index - 1).item_name.at(item_index) || + write_data_list_.at(dxl_index).item_addr.at(item_index) != + write_data_list_.at(dxl_index - 1).item_addr.at(item_index) || + write_data_list_.at(dxl_index).item_size.at(item_index) != + write_data_list_.at(dxl_index - 1).item_size.at(item_index)) { return BULK; } @@ -870,11 +910,12 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector id_arr) } // Set indirect addr. indirect_info_read_[it_id].indirect_data_addr = IN_ADDR; + + fprintf( + stderr, + "set bulk read (indirect addr) : addr %d, size %d\n", + IN_ADDR, indirect_info_read_[id_arr.at(0)].size); } - fprintf( - stderr, - "set bulk read (indirect addr) : addr %d, size %d\n", - IN_ADDR, indirect_info_read_[id_arr.at(0)].size); group_bulk_read_ = new dynamixel::GroupBulkRead(port_handler_, packet_handler_); @@ -1130,15 +1171,12 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() DxlError Dynamixel::SetBulkWriteHandler(std::vector id_arr) { - uint16_t INDIRECT_ADDR = 0; - uint8_t INDIRECT_SIZE; + uint16_t IN_ADDR = 0; + uint8_t IN_SIZE = 0; for (auto it_id : id_arr) { // Get the indirect addr. - if (dxl_info_.GetDxlControlItem( - it_id, "Indirect Data Write", INDIRECT_ADDR, - INDIRECT_SIZE) == false) - { + if (dxl_info_.GetDxlControlItem(it_id, "Indirect Data Write", IN_ADDR, IN_SIZE) == false) { fprintf( stderr, "Fail to set indirect address bulk write. " @@ -1146,14 +1184,16 @@ DxlError Dynamixel::SetBulkWriteHandler(std::vector id_arr) return DxlError::SET_BULK_WRITE_FAIL; } // Set indirect addr. - indirect_info_write_[it_id].indirect_data_addr = INDIRECT_ADDR; + indirect_info_write_[it_id].indirect_data_addr = IN_ADDR; + + fprintf( + stderr, + "set bulk write (indirect addr) : addr %d, size %d\n", + IN_ADDR, indirect_info_write_[id_arr.at(0)].size); } - fprintf( - stderr, - "set bulk write (indirect addr) : addr %d, size %d\n", - INDIRECT_ADDR, indirect_info_write_[id_arr.at(0)].size); group_bulk_write_ = new dynamixel::GroupBulkWrite(port_handler_, packet_handler_); + return DxlError::OK; } diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index d242bf2..83565de 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -1,19 +1,18 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM*/ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #include "dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp" #include diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index c00667d..cfb6ae7 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -1,19 +1,18 @@ -/******************************************************************************* -* Copyright 2024 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: Hye-Jong KIM, Sungho Woo */ +// Copyright 2024 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: Hye-Jong KIM, Sungho Woo #include "dynamixel_hardware_interface/dynamixel_hardware_interface.hpp" @@ -51,7 +50,9 @@ DynamixelHardware::~DynamixelHardware() hardware_interface::CallbackReturn DynamixelHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { RCLCPP_ERROR_STREAM(logger_, "Failed to initialize DynamixelHardware"); return hardware_interface::CallbackReturn::ERROR; } @@ -78,9 +79,11 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( RCLCPP_INFO_STREAM(logger_, "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"); RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Comm Port"); - - if (info_.hardware_parameters.find("use_revolute_to_prismatic_gripper") != info_.hardware_parameters.end()) { - use_revolute_to_prismatic_ = std::stoi(info_.hardware_parameters.at("use_revolute_to_prismatic_gripper")) != 0; + if (info_.hardware_parameters.find("use_revolute_to_prismatic_gripper") != + info_.hardware_parameters.end()) + { + use_revolute_to_prismatic_ = + std::stoi(info_.hardware_parameters.at("use_revolute_to_prismatic_gripper")) != 0; } if (use_revolute_to_prismatic_) { @@ -93,7 +96,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( } else if (gpio.parameters.at("type") == "sensor") { sensor_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); } else { - RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensoe type"); + RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensor type"); exit(-1); } } @@ -383,7 +386,7 @@ hardware_interface::CallbackReturn DynamixelHardware::stop() } hardware_interface::return_type DynamixelHardware::read( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & time, const rclcpp::Duration & period) { if (dxl_status_ == REBOOTING) { RCLCPP_ERROR_STREAM(logger_, "Dynamixel Read Fail : REBOOTING"); @@ -426,14 +429,14 @@ hardware_interface::return_type DynamixelHardware::read( dxl_state_pub_uni_ptr_->unlockAndPublish(); } -if (rclcpp::ok()) { + if (rclcpp::ok()) { rclcpp::spin_some(this->get_node_base_interface()); -} + } return hardware_interface::return_type::OK; } hardware_interface::return_type DynamixelHardware::write( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & time, const rclcpp::Duration & period) { if (dxl_status_ == DXL_OK || dxl_status_ == HW_ERROR) { dxl_comm_->WriteItemBuf(); @@ -551,8 +554,26 @@ bool DynamixelHardware::InitDxlItems() RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Items"); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + // First write items containing "Limit" + for (auto it : gpio.parameters) { + if (it.first != "ID" && it.first != "type" && it.first.find("Limit") != std::string::npos) { + if (dxl_comm_->WriteItem( + id, it.first, + static_cast(stoi(it.second))) != DxlError::OK) + { + RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error"); + return false; + } + RCLCPP_INFO_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" << + stoi(it.second)); + } + } + + // Then write the remaining items for (auto it : gpio.parameters) { - if (it.first != "ID" && it.first != "type") { + if (it.first != "ID" && it.first != "type" && it.first.find("Limit") == std::string::npos) { if (dxl_comm_->WriteItem( id, it.first, static_cast(stoi(it.second))) != DxlError::OK) @@ -580,7 +601,6 @@ bool DynamixelHardware::InitDxlReadItems() hdl_gpio_sensor_states_.clear(); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "dxl") { - uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_read; @@ -963,11 +983,15 @@ void DynamixelHardware::set_dxl_torque_srv_callback( void DynamixelHardware::initRevoluteToPrismaticParam() { - if (info_.hardware_parameters.find("revolute_to_prismatic_dxl") != info_.hardware_parameters.end()) { + if (info_.hardware_parameters.find("revolute_to_prismatic_dxl") != + info_.hardware_parameters.end()) + { conversion_dxl_name_ = info_.hardware_parameters.at("revolute_to_prismatic_dxl"); } - if (info_.hardware_parameters.find("revolute_to_prismatic_joint") != info_.hardware_parameters.end()) { + if (info_.hardware_parameters.find("revolute_to_prismatic_joint") != + info_.hardware_parameters.end()) + { conversion_joint_name_ = info_.hardware_parameters.at("revolute_to_prismatic_joint"); }