├── .clang-format
├── .gitignore
├── CHANGELOG.rst
├── CMakeLists.txt
├── LICENSE
├── config
    ├── controller_config.yaml
    └── motor_data.yaml
├── include
    └── dynamixel_interface
    │   ├── dynamixel_const.h
    │   ├── dynamixel_interface_controller.h
    │   └── dynamixel_interface_driver.h
├── launch
    └── dynamixel_interface_controller.launch
├── msg
    ├── DataPort.msg
    ├── DataPorts.msg
    ├── ServoDiag.msg
    └── ServoDiags.msg
├── package.xml
├── readme.md
├── scripts
    └── set_lowlatency_mode.sh
├── src
    ├── dynamixel_interface_controller.cpp
    ├── dynamixel_interface_driver.cpp
    └── dynamixel_interface_main.cpp
└── tutorials
    ├── tutorial_1_using_the_controller.md
    └── tutorial_2_multi_port_communication.md


/.clang-format:
--------------------------------------------------------------------------------
 1 | 
 2 | Language: Cpp
 3 | BasedOnStyle: Google
 4 | 
 5 | AccessModifierOffset: -2
 6 | AlignAfterOpenBracket: Align
 7 | AlignConsecutiveAssignments: false
 8 | AlignConsecutiveDeclarations: false
 9 | AlignEscapedNewlines: Left
10 | AlignOperands: true
11 | AlignTrailingComments: true
12 | AllowAllParametersOfDeclarationOnNextLine: true
13 | AllowShortBlocksOnASingleLine: false
14 | AllowShortCaseLabelsOnASingleLine: false
15 | AllowShortFunctionsOnASingleLine: InlineOnly
16 | AllowShortIfStatementsOnASingleLine: false
17 | AlwaysBreakAfterDefinitionReturnType: None
18 | AlwaysBreakBeforeMultilineStrings: false
19 | BinPackArguments: true
20 | BinPackParameters: true
21 | BreakBeforeBraces: Custom
22 | BraceWrapping:
23 |   AfterClass: true
24 |   AfterControlStatement: true
25 |   AfterEnum: true
26 |   AfterFunction: true
27 |   AfterNamespace: true
28 |   AfterStruct: true
29 |   AfterUnion: true
30 |   # AfterExternBlock: false
31 |   BeforeCatch: true
32 |   BeforeElse: true
33 |   IndentBraces: false
34 |   SplitEmptyFunction: false
35 |   SplitEmptyRecord: true
36 | BreakBeforeBinaryOperators: None
37 | BreakBeforeInheritanceComma: false
38 | BreakBeforeTernaryOperators: false
39 | BreakConstructorInitializers: BeforeComma
40 | BreakStringLiterals: true
41 | ColumnLimit: 120
42 | CompactNamespaces: false
43 | ConstructorInitializerAllOnOneLineOrOnePerLine: false
44 | ConstructorInitializerIndentWidth: 2
45 | ContinuationIndentWidth: 2
46 | Cpp11BracedListStyle: false
47 | DerivePointerAlignment: false
48 | FixNamespaceComments: true
49 | # IncludeCategories:
50 | #   - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
51 | #     Priority:        2
52 | #   - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
53 | #     Priority:        3
54 | #   - Regex:           '.*'
55 | #     Priority:        1
56 | # IncludeCategories: TODO
57 | # IncludeMainReges: TODO
58 | IncludeBlocks: Preserve
59 | IndentCaseLabels: true
60 | # IndentPPDirectives: None
61 | IndentWidth: 2
62 | IndentWrappedFunctionNames: true
63 | KeepEmptyLinesAtTheStartOfBlocks: false
64 | # MacroBlockBegin: <strings>
65 | # MacroBlockEnd: <strings>
66 | MaxEmptyLinesToKeep: 2
67 | NamespaceIndentation: None
68 | # PenaltyBreakAssignment: <int>
69 | # PenaltyBreakBeforeFirstCallParameter: <int>
70 | # PenaltyBreakComment: <int>
71 | # PenaltyBreakFirstLessLess: <int>
72 | # PenaltyBreakString: <int>
73 | # PenaltyExcessCharacter: <int>
74 | # PenaltyReturnTypeOnItsOwnLine: <int>
75 | PointerAlignment: Right
76 | ReflowComments: true
77 | SortIncludes: true
78 | SortUsingDeclarations: true
79 | SpaceAfterCStyleCast: false
80 | SpaceAfterTemplateKeyword: true
81 | SpaceBeforeAssignmentOperators: true
82 | # SpaceBeforeCtorInitializerColon: true
83 | # SpaceBeforeInheritanceColon: true
84 | SpaceBeforeParens: ControlStatements
85 | # SpaceBeforeRangeBasedForLoopColon: true
86 | SpaceInEmptyParentheses: false
87 | SpacesBeforeTrailingComments: 2
88 | SpacesInAngles: false
89 | SpacesInCStyleCastParentheses: false
90 | SpacesInContainerLiterals: true
91 | SpacesInParentheses: false
92 | SpacesInSquareBrackets: false
93 | Standard: Cpp11
94 | TabWidth: 2
95 | UseTab: Never
96 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | #ignore vscode folder
2 | .vscode/
3 | doc/
4 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package dynamixel_interface
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 | 
5 | 1.0.0 (2021-09-06)
6 | -----------
7 | * Initial Release


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2020
  2 | # Commonwealth Scientific and Industrial Research Organisation (CSIRO)
  3 | # ABN 41 687 119 230
  4 | #
  5 | # Author: Tom Molnar
  6 | cmake_minimum_required(VERSION 3.5)
  7 | project(dynamixel_interface)
  8 | 
  9 | # C++ standards setup.
 10 | set(CMAKE_CXX_STANDARD 17)
 11 | set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
 12 | 
 13 | ################################################################################
 14 | # Packages
 15 | ################################################################################
 16 | 
 17 | ## Find catkin macros and libraries
 18 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 19 | ## is used, also find other catkin packages
 20 | 
 21 | find_package(catkin REQUIRED COMPONENTS
 22 |   dynamixel_sdk
 23 |   roscpp
 24 |   xmlrpcpp
 25 |   roslib
 26 |   message_generation
 27 |   sensor_msgs
 28 |   std_msgs
 29 | )
 30 | 
 31 | find_package(PkgConfig REQUIRED)
 32 | pkg_check_modules(yaml-cpp REQUIRED yaml-cpp)
 33 | if(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5")
 34 | add_definitions(-DHAVE_NEW_YAMLCPP)
 35 | endif(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5")
 36 | 
 37 | ################################################
 38 | ## Declare ROS messages, services and actions ##
 39 | ################################################
 40 | 
 41 | add_message_files(
 42 |   FILES
 43 |   DataPort.msg
 44 |   DataPorts.msg
 45 |   ServoDiag.msg
 46 |   ServoDiags.msg
 47 | )
 48 | 
 49 | generate_messages(
 50 |    DEPENDENCIES
 51 |    std_msgs
 52 |    sensor_msgs
 53 | )
 54 | 
 55 | ###################################
 56 | ## catkin specific configuration ##
 57 | ###################################
 58 | 
 59 | ## The catkin_package macro generates cmake config files for your package
 60 | ## Declare things to be passed to dependent projects
 61 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 62 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 63 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 64 | ## DEPENDS: system dependencies of this project that dependent projects also need
 65 | 
 66 | catkin_package(
 67 |    INCLUDE_DIRS include
 68 | #  LIBRARIES dynamixel_interface_controller
 69 |    CATKIN_DEPENDS roscpp roslib xmlrpcpp sensor_msgs std_msgs dynamixel_sdk message_runtime
 70 | #  DEPENDS system_lib
 71 | )
 72 | 
 73 | ###########
 74 | ## Build ##
 75 | ###########
 76 | 
 77 | ## Library
 78 | add_library(dynamixel_interface src/dynamixel_interface_controller.cpp src/dynamixel_interface_driver.cpp)
 79 | add_dependencies(dynamixel_interface ${dynamixel_interface_EXPORTED_TARGETS})
 80 | target_include_directories(dynamixel_interface
 81 |   PRIVATE
 82 |     include
 83 |   )
 84 | target_include_directories(dynamixel_interface SYSTEM
 85 |   PRIVATE
 86 |     "${catkin_INCLUDE_DIRS}"
 87 |     "${yaml-cpp_INCLUDE_DIRS}"
 88 |   )
 89 | target_link_libraries(dynamixel_interface
 90 |   PRIVATE
 91 |     yaml-cpp
 92 |     ${catkin_LIBRARIES}
 93 |     ${yaml-cpp_LIBRARIES}
 94 |   )
 95 | 
 96 | add_executable(dynamixel_interface_controller_node src/dynamixel_interface_main.cpp)
 97 | add_dependencies(dynamixel_interface_controller_node ${dynamixel_interface_EXPORTED_TARGETS} dynamixel_interface_generate_messages_cpp)
 98 | target_include_directories(dynamixel_interface_controller_node
 99 |   PRIVATE
100 |     include
101 | )
102 | target_include_directories(dynamixel_interface_controller_node SYSTEM
103 |   PRIVATE
104 |     "${catkin_INCLUDE_DIRS}"
105 |     "${yaml-cpp_INCLUDE_DIRS}"
106 | )
107 | target_link_libraries(dynamixel_interface_controller_node
108 |   PRIVATE
109 |     dynamixel_interface
110 |     yaml-cpp
111 |     ${catkin_LIBRARIES}
112 |     ${yaml-cpp_LIBRARIES}
113 | )
114 | 
115 | 
116 | #############
117 | ## Install ##
118 | #############
119 | 
120 | ## Mark executables and/or libraries for installation
121 | install(TARGETS dynamixel_interface dynamixel_interface_controller_node
122 |    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
123 |    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
124 |    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
125 | )
126 | 
127 | # install launch and config info for running
128 | install(DIRECTORY launch config scripts
129 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
130 | )
131 | 
132 | ## Mark cpp header files for installation
133 | install(DIRECTORY include/${PROJECT_NAME}/
134 |   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
135 |   FILES_MATCHING PATTERN "*.h"
136 |   PATTERN ".svn" EXCLUDE
137 | )


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
 2 | Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
 3 | All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following terms, except where otherwise indicated for third party material.
 4 | Redistribution and use of this software in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 5 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 6 | * 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.
 7 | * Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission of CSIRO.
 8 | 
 9 | EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE, OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
10 | TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL, INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH CLAIM, LOSS, DAMAGES OR OTHER LIABILITY.
11 | APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED, RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".  TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
12 | (a)               THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
13 | (b)               THE REPAIR OF THE SOFTWARE;
14 | (c)               THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
15 | IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
16 | 
17 | Third Party Components:
18 | 
19 | The following third party components are distributed with the Software.  You agree to comply with the license terms for these components as part of accessing the Software.  Other third party software may also be identified in separate files distributed with the Software.
20 | ___________________________________________________________________
21 | 
22 | dynamixel_interface is forked from projects authored by Brian Axelrod (on behalf of Willow Garage):
23 | https://github.com/baxelrod/dynamixel_pro_controller
24 | https://github.com/baxelrod/dynamixel_pro_driver
25 | 
26 | Thus they retain the following notice:
27 | 
28 | Software License Agreement (BSD License)
29 | Copyright (c) 2013, Willow Garage
30 | All rights reserved.
31 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
32 |  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
33 |  * 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.
34 |  * Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
35 | 
36 | 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.
37 | ___________________________________________________________________
38 | 
39 | 


--------------------------------------------------------------------------------
/config/controller_config.yaml:
--------------------------------------------------------------------------------
 1 | #  This file is used to configure the paramters used to control the servos.
 2 | #  This is an example only. To use the controller define a new controller_config.yaml
 3 | #  in your ros package along with a new launch file and use those. see the readme for
 4 | #  more info.
 5 | 
 6 | # # --------------------------------------------------------------------------- # #
 7 | # GLOBAL OPERATION PARAMETERS
 8 | loop_rate: 100                    # desired rate for joint state updates. actual rate may be less depending on number
 9 |                                   # of dynamixels connected and port baud rate.
10 | control_mode: position            # control mode, either 'position', 'velocity', or 'effort'
11 | disable_torque_on_shutdown: true  # with this enabled the motors will switch off when the controller closes
12 | ignore_input_velocity: false      # ignore input velocity commands in position mode (no profile velocity)
13 | diagnostics_rate: 1               # rate to publish diagnostic information
14 | dataport_rate: 1                  # rate to read from dynamixel external dataports
15 | recv_queue_size: 1                # receive queue size for desired_joint_states topic
16 | # The below values are used as global defaults and are applied for each servo unless overridden in the entry for
17 | # the servo below
18 | global_max_vel: 5.0               # maximum joint speed (rad/s) (in position or velocity control)
19 | global_torque_limit: 1.0          # maximum motor torque for all modes, given as a fraction of rated max (0-1)
20 | 
21 | # # --------------------------------------------------------------------------- # #
22 | # PORT AND SERVO CONFIGURATIONS
23 | ports:
24 | 
25 |   # PORT LIST
26 |   - name: Port_1               # name for this port in config
27 |     device: /dev/ttyUSB0       # serial device this port will communicate on
28 |     baudrate: 3000000          # baudrate in use
29 |     use_legacy_protocol: false # wether to use new 2.0 protocol (false) or legacy 1.0 protocol (true)
30 |     group_read_enabled: true   # specify whether to use group comms for reading
31 |     group_write_enabled: true  # specify whether to use group comms for writing
32 |     servos:
33 |       # SERVO LIST FOR THIS PORT
34 |       - id: 1                  # (ID set in servo eeprom, must be unique on this port)
35 |         joint_name: joint_1    # (MUST BE UNIQUE ACROSS ALL PORTS)
36 |         #
37 |         # The three values below are mandatory, they define the orientation and zeroing of the dynamixel:
38 |         #
39 |         zero_pos: 2048         # 0 rad servo position (in raw encoder count)
40 |         min_pos: 0             # minimum servo position (in raw encoder count)
41 |         max_pos: 4095          # maximum servo position, Note when MIN > MAX ROTATION IS REVERSED
42 |         #
43 |         # The below arguments are all optional and override the global values:
44 |         #
45 |         max_vel: 5.0           # maximum joint speed (rad/s) (in position or velocity control)
46 |         torque_limit: 1.0      # maximum motor torque for all modes, given as a fraction of rated max (0-1)
47 | 
48 |       - id: 2
49 |         joint_name: joint_2
50 |         zero_pos: 2048
51 |         min_pos: 0
52 |         max_pos: 4095
53 |         #
54 |         # This servo doesn't have any optional values defined, the global defaults will be used
55 |         #
56 | 
57 |   # more ports can be defined in the same manner as above, each port can even have a different baudrate and protcol
58 |   # - name: Port_2
59 |   #   device: /dev/ttyUSB1
60 |   #   baudrate: 115200
61 |   #   use_legacy_protocol: true
62 |   #   group_read_enabled: true
63 |   #   group_write_enabled: true
64 |   #   servos:
65 |   #     - id: 1               # id only needs to be unique for each port and so id 1 can be reused here
66 |   #       joint_name: joint_3 # name DOES have to be unique though, so we continue the namin_posg scheme
67 |   #       zero_pos: 512
68 |   #       min_pos: 0
69 |   #       max_pos: 1023
70 | 
71 |   #     - id: 2
72 |   #       joint_name: joint_4
73 |   #       zero_pos: 512
74 |   #       min_pos: 0
75 |   #       max_pos: 1023
76 | 
77 | # --------------------------------------------------------------------------- # #
78 | 


--------------------------------------------------------------------------------
/config/motor_data.yaml:
--------------------------------------------------------------------------------
  1 | # This file contains servo parameters specific to each model, used to convert input positions, velocities and torques
  2 | # into servo register values. These values should NOT need to be edited once they are added.
  3 | # To add a new model of dynamixel to the driver, insert an entry in the list below with the following information:
  4 | # - name: The model name
  5 | # - model_number: The unique model number for this dynamixel
  6 | # - series: The series of dynamixel this model belongs to, the possible values are:
  7 | #    - AX: The AX series (discontinued)
  8 | #    - RX: The RX series (discontinued)
  9 | #    - LM: The old, 1.0 protocol MX series, prior to firmware version 47
 10 | #    - M: The current, 2.0 protocol compliant MX series
 11 | #    - X: The X series dynamixels, not including the xl-320 (not supported)
 12 | #    - LP: The older, legacy pro series of dynamixel (everything before the pro+)
 13 | #    - P: The newer, dynamixel pro+ (also now just called dynamixel P)
 14 | # - encoder_cpr: The position encoder counts per revolution
 15 | # - encoder_range: (optional) This parameter is only for dynamixels whose output range of motion is less than a full
 16 | #                  revolution, e.g on the AX and RX series, the encoder range of 0-1023 is mapped to a 300 degree range
 17 | # - velocity_radps_to_reg: The conversion factor for converting velocity in radians/sec to register values
 18 | # - effort_reg_max: The max value possible in the effort/current register, this is usually the maximum possible value of
 19 | #                   the max_torque/current_limit/effort_limit register
 20 | # - effort_reg_to_mA: The conversion factor for converting from register values to current in mA
 21 | # - external_ports: (optional) Specify if the model has external dataports
 22 | # All the above can be obtained from the datasheet for the motor, found at https://emanual.robotis.com/
 23 | # --------------------------- MX SERIES (1.0) ---------------------------- #
 24 | - name: MX12-W
 25 |   model_number: 360
 26 |   series: LM
 27 |   encoder_cpr: 4096
 28 |   velocity_radps_to_reg: 10.425
 29 |   effort_reg_max: 1000
 30 |   effort_reg_to_mA: 0.001
 31 | 
 32 | - name: MX28
 33 |   model_number: 29
 34 |   series: LM
 35 |   encoder_cpr: 4096
 36 |   velocity_radps_to_reg: 86.812
 37 |   effort_reg_max: 1000
 38 |   effort_reg_to_mA: 0.001
 39 | 
 40 | - name: MX64
 41 |   model_number: 310
 42 |   series: LM
 43 |   encoder_cpr: 4096
 44 |   velocity_radps_to_reg: 86.812
 45 |   effort_reg_max: 1000
 46 |   effort_reg_to_mA: 0.001
 47 | 
 48 | - name: MX106
 49 |   model_number: 320
 50 |   series: LM
 51 |   encoder_cpr: 4096
 52 |   velocity_radps_to_reg: 86.812
 53 |   effort_reg_max: 1000
 54 |   effort_reg_to_mA: 0.001
 55 | 
 56 | # ------------------------ MX SERIES (2.0) ------------------------- #
 57 | 
 58 | 
 59 | 
 60 | - name: MX28-(2.0)
 61 |   model_number: 30
 62 |   series: M
 63 |   encoder_cpr: 4096
 64 |   velocity_radps_to_reg: 41.700
 65 |   effort_reg_max: 1000
 66 |   effort_reg_to_mA: 0.001
 67 | 
 68 | - name: MX64-(2.0)
 69 |   model_number: 311
 70 |   series: M
 71 |   encoder_cpr: 4096
 72 |   velocity_radps_to_reg: 41.700
 73 |   effort_reg_max: 2047
 74 |   effort_reg_to_mA: 3.36
 75 | 
 76 | - name: MX106-(2.0)
 77 |   model_number: 321
 78 |   series: M
 79 |   encoder_cpr: 4096
 80 |   velocity_radps_to_reg: 41.700
 81 |   effort_reg_max: 2047
 82 |   effort_reg_to_mA: 3.36
 83 | 
 84 | # ---------------------------- X SERIES ---------------------------- #
 85 | 
 86 | # ---- XL ---- #
 87 | 
 88 | - name: XL430-W250
 89 |   model_number: 1060
 90 |   series: X
 91 |   encoder_cpr: 4096
 92 |   velocity_radps_to_reg: 41.700
 93 |   effort_reg_max: 1000
 94 |   effort_reg_to_mA: 0.001
 95 | 
 96 | - name: 2XL430-W250
 97 |   model_number: 1090
 98 |   series: X
 99 |   encoder_cpr: 4096
100 |   velocity_radps_to_reg: 41.700
101 |   effort_reg_max: 1000
102 |   effort_reg_to_mA: 0.001
103 | 
104 | # ---- XC ---- #
105 | 
106 | - name: XC430-W150
107 |   model_number: 1070
108 |   series: X
109 |   encoder_cpr: 4096
110 |   velocity_radps_to_reg: 41.700
111 |   effort_reg_max: 1000
112 |   effort_reg_to_mA: 0.001
113 | 
114 | - name: XC430-W240
115 |   model_number: 1080
116 |   series: X
117 |   encoder_cpr: 4096
118 |   velocity_radps_to_reg: 41.700
119 |   effort_reg_max: 1000
120 |   effort_reg_to_mA: 0.001
121 | 
122 | - name: 2XC430-W250
123 |   model_number: 1160
124 |   series: X
125 |   encoder_cpr: 4096
126 |   velocity_radps_to_reg: 41.700
127 |   effort_reg_max: 1000
128 |   effort_reg_to_mA: 0.001
129 | 
130 | # ---- XM ---- #
131 | 
132 | - name: XM430-W210
133 |   model_number: 1030
134 |   series: X
135 |   encoder_cpr: 4096
136 |   velocity_radps_to_reg: 41.700
137 |   effort_reg_max: 1193
138 |   effort_reg_to_mA: 2.69
139 | 
140 | - name: XM430-W350
141 |   model_number: 1020
142 |   series: X
143 |   encoder_cpr: 4096
144 |   velocity_radps_to_reg: 41.700
145 |   effort_reg_max: 1193
146 |   effort_reg_to_mA: 2.69
147 | 
148 | - name: XM540-W270
149 |   model_number: 1120
150 |   series: X
151 |   external_ports: true
152 |   encoder_cpr: 4096
153 |   velocity_radps_to_reg: 41.700
154 |   effort_reg_max: 1193
155 |   effort_reg_to_mA: 2.69
156 | 
157 | - name: XM540-W150
158 |   model_number: 1130
159 |   series: X
160 |   external_ports: true
161 |   encoder_cpr: 4096
162 |   velocity_radps_to_reg: 41.700
163 |   effort_reg_max: 2047
164 |   effort_reg_to_mA: 2.69
165 | 
166 | # ---- XH ---- #
167 | 
168 | - name: XH430-V210
169 |   model_number: 1050
170 |   series: X
171 |   encoder_cpr: 4096
172 |   velocity_radps_to_reg: 41.700
173 |   effort_reg_max: 689
174 |   effort_reg_to_mA: 1.34
175 | 
176 | - name: XH430-V350
177 |   model_number: 1040
178 |   series: X
179 |   encoder_cpr: 4096
180 |   velocity_radps_to_reg: 41.700
181 |   effort_reg_max: 689
182 |   effort_reg_to_mA: 1.34
183 | 
184 | - name: XH430-W210
185 |   model_number: 1010
186 |   series: X
187 |   encoder_cpr: 4096
188 |   velocity_radps_to_reg: 41.700
189 |   effort_reg_max: 648
190 |   effort_reg_to_mA: 2.69
191 | 
192 | - name: XH430-W350
193 |   model_number: 1000
194 |   series: X
195 |   encoder_cpr: 4096
196 |   velocity_radps_to_reg: 41.700
197 |   effort_reg_max: 648
198 |   effort_reg_to_mA: 2.69
199 | 
200 | - name: XH540-W150
201 |   model_number: 1110
202 |   series: X
203 |   external_ports: true
204 |   encoder_cpr: 4096
205 |   velocity_radps_to_reg: 41.700
206 |   effort_reg_max: 2047
207 |   effort_reg_to_mA: 2.69
208 | 
209 | - name: XH540-W270
210 |   model_number: 1100
211 |   series: X
212 |   external_ports: true
213 |   encoder_cpr: 4096
214 |   velocity_radps_to_reg: 41.700
215 |   effort_reg_max: 2047
216 |   effort_reg_to_mA: 2.69
217 | 
218 | - name: XH540-V150
219 |   model_number: 1150
220 |   series: X
221 |   external_ports: true
222 |   encoder_cpr: 4096
223 |   velocity_radps_to_reg: 41.700
224 |   effort_reg_max: 2047
225 |   effort_reg_to_mA: 2.69
226 | 
227 | - name: XH540-V270
228 |   model_number: 1140
229 |   series: X
230 |   external_ports: true
231 |   encoder_cpr: 4096
232 |   velocity_radps_to_reg: 41.700
233 |   effort_reg_max: 2047
234 |   effort_reg_to_mA: 2.69
235 | 
236 | # ---- XW ---- #
237 | 
238 | - name: XW540-T140
239 |   model_number: 1180
240 |   series: X
241 |   encoder_cpr: 4096
242 |   velocity_radps_to_reg: 41.700
243 |   effort_reg_max: 2047
244 |   effort_reg_to_mA: 2.69
245 | 
246 | - name: XW540-T260
247 |   model_number: 1170
248 |   series: X
249 |   encoder_cpr: 4096
250 |   velocity_radps_to_reg: 41.700
251 |   effort_reg_max: 2047
252 |   effort_reg_to_mA: 2.69
253 | 
254 | # -------------------------- P SERIES ---------------------------- #
255 | 
256 | # ---- M ---- #
257 | 
258 | - name: PM42-010-S260
259 |   model_number: 2100
260 |   series: P
261 |   encoder_cpr: 526374
262 |   velocity_radps_to_reg: 954.930
263 |   effort_reg_max: 1461
264 |   effort_reg_to_mA: 1
265 | 
266 | 
267 | - name: PM54-040-S250
268 |   model_number: 2110
269 |   series: P
270 |   external_ports: true
271 |   encoder_cpr: 502834
272 |   velocity_radps_to_reg: 954.930
273 |   effort_reg_max: 4470
274 |   effort_reg_to_mA: 1
275 | 
276 | - name: PM54-060-S250
277 |   model_number: 2110
278 |   series: P
279 |   external_ports: true
280 |   encoder_cpr: 502834
281 |   velocity_radps_to_reg: 954.930
282 |   effort_reg_max: 7980
283 |   effort_reg_to_mA: 1
284 | 
285 | # ---- H ---- #
286 | 
287 | - name: PH54-100-S500
288 |   model_number: 2010
289 |   series: P
290 |   external_ports: true
291 |   encoder_cpr: 1003846
292 |   velocity_radps_to_reg: 954.930
293 |   effort_reg_max: 15900
294 |   effort_reg_to_mA: 1
295 | 
296 | - name: PH54-200-S500
297 |   model_number: 2020
298 |   series: P
299 |   external_ports: true
300 |   encoder_cpr: 1003846
301 |   velocity_radps_to_reg: 954.930
302 |   effort_reg_max: 22740
303 |   effort_reg_to_mA: 1
304 | 
305 | 
306 | # -------------------------- Legacy (discontinued) PRO+ (PRO-A) SERIES ---------------------------- #
307 | 
308 | # ---- M ---- #
309 | 
310 | - name: M42-010-S260A
311 |   model_number: 43289
312 |   series: P
313 |   external_ports: true
314 |   encoder_cpr: 526374
315 |   velocity_radps_to_reg: 954.930
316 |   effort_reg_max: 1461
317 |   effort_reg_to_mA: 1
318 | 
319 | - name: M54-040-S250A
320 |   model_number: 46097
321 |   series: P
322 |   external_ports: true
323 |   encoder_cpr: 502834
324 |   velocity_radps_to_reg: 954.930
325 |   effort_reg_max: 4470
326 |   effort_reg_to_mA: 1
327 | 
328 | - name: M54-060-S250A
329 |   model_number: 46353
330 |   series: P
331 |   external_ports: true
332 |   encoder_cpr: 502834
333 |   velocity_radps_to_reg: 954.930
334 |   effort_reg_max: 7980
335 |   effort_reg_to_mA: 1
336 | 
337 | # ---- H ---- #
338 | 
339 | - name: H42-20-S300A
340 |   model_number: 51201
341 |   series: P
342 |   external_ports: true
343 |   encoder_cpr: 607500
344 |   velocity_radps_to_reg: 954.930
345 |   effort_reg_max: 4500
346 |   effort_reg_to_mA: 1
347 | 
348 | - name: H54-100-S500A
349 |   model_number: 53769
350 |   series: P
351 |   external_ports: true
352 |   encoder_cpr: 1003846
353 |   velocity_radps_to_reg: 954.930
354 |   effort_reg_max: 15900
355 |   effort_reg_to_mA: 1
356 | 
357 | - name: H54-200-S500A
358 |   model_number: 54025
359 |   series: P
360 |   external_ports: true
361 |   encoder_cpr: 1003846
362 |   velocity_radps_to_reg: 954.930
363 |   effort_reg_max: 22740
364 |   effort_reg_to_mA: 1
365 | 
366 | # -------------------------- Legacy (discontinued) PRO SERIES ---------------------------- #
367 | 
368 |  # ---- L ---- #
369 | 
370 | - name: L42-10-S300
371 |   model_number: 35072
372 |   series: LP
373 |   external_ports: true
374 |   encoder_cpr: 4096
375 |   velocity_radps_to_reg: 300
376 |   effort_reg_max: 1023
377 |   effort_reg_to_mA: 4.028
378 | 
379 | - name: L54-30-S400
380 |   model_number: 37928
381 |   series: LP
382 |   encoder_cpr: 288393
383 |   velocity_radps_to_reg: 3824.966
384 |   effort_reg_max: 32767
385 |   effort_reg_to_mA: 16.113
386 | 
387 | - name: L54-50-S290
388 |   model_number: 38176
389 |   series: LP
390 |   external_ports: true
391 |   encoder_cpr: 207692
392 |   velocity_radps_to_reg: 2754.602
393 |   effort_reg_max: 32767
394 |   effort_reg_to_mA: 16.113
395 | 
396 | - name: L54-30-S500
397 |   model_number: 37896
398 |   series: LP
399 |   external_ports: true
400 |   encoder_cpr: 361385
401 |   velocity_radps_to_reg: 4793.006
402 |   effort_reg_max: 32767
403 |   effort_reg_to_mA: 16.113
404 | 
405 | - name: L54-50-S500
406 |   model_number: 38152
407 |   series: LP
408 |   external_ports: true
409 |   encoder_cpr: 361385
410 |   velocity_radps_to_reg: 4793.006
411 |   effort_reg_max: 32767
412 |   effort_reg_to_mA: 16.113
413 | 
414 | # ---- M ---- #
415 | 
416 | - name: M42-10-S260
417 |   model_number: 43288
418 |   series: LP
419 |   external_ports: true
420 |   encoder_cpr: 263187
421 |   velocity_radps_to_reg: 2454.353
422 |   effort_reg_max: 32767
423 |   effort_reg_to_mA: 4.028
424 | 
425 | - name: M54-40-S250
426 |   model_number: 46096
427 |   series: LP
428 |   external_ports: true
429 |   encoder_cpr: 251417
430 |   velocity_radps_to_reg: 2400.853
431 |   effort_reg_max: 32767
432 |   effort_reg_to_mA: 16.113
433 | 
434 | - name: M54-60-S250
435 |   model_number: 46352
436 |   series: LP
437 |   external_ports: true
438 |   encoder_cpr: 251417
439 |   velocity_radps_to_reg: 2400.853
440 |   effort_reg_max: 32767
441 |   effort_reg_to_mA: 16.113
442 | 
443 | # ---- H ---- #
444 | 
445 | - name: H42-20-S300
446 |   model_number: 51200
447 |   series: LP
448 |   external_ports: true
449 |   encoder_cpr: 303751
450 |   velocity_radps_to_reg: 300.0
451 |   effort_reg_max: 1395
452 |   effort_reg_to_mA: 4.028
453 | 
454 | - name: H54-100-S500
455 |   model_number: 53768
456 |   series: LP
457 |   external_ports: true
458 |   encoder_cpr: 501923
459 |   velocity_radps_to_reg: 4793.006
460 |   effort_reg_max: 32767
461 |   effort_reg_to_mA: 16.113
462 | 
463 | - name: H54-200-S500
464 |   model_number: 54024
465 |   series: LP
466 |   external_ports: true
467 |   encoder_cpr: 501923
468 |   velocity_radps_to_reg: 4793.006
469 |   effort_reg_max: 32767
470 |   effort_reg_to_mA: 16.113
471 | 
472 | # ---------------------------- AX SERIES ---------------------------- #
473 | 
474 | - name: AX-12A
475 |   model_number: 12
476 |   series: A
477 |   encoder_cpr: 1023
478 |   encoder_range_deg: 300
479 |   velocity_radps_to_reg: 86.023
480 |   effort_reg_max: 1000
481 |   effort_reg_to_mA: 0.001
482 | 
483 | - name: AX-12W
484 |   model_number: 300
485 |   series: A
486 |   encoder_cpr: 1023
487 |   encoder_range_deg: 300
488 |   velocity_radps_to_reg: 86.023
489 |   effort_reg_max: 1000
490 |   effort_reg_to_mA: 0.001
491 | 
492 | - name: AX-18A
493 |   model_number: 18
494 |   series: A
495 |   encoder_cpr: 1023
496 |   encoder_range_deg: 300
497 |   velocity_radps_to_reg: 86.023
498 |   effort_reg_max: 1000
499 |   effort_reg_to_mA: 0.001
500 | 
501 | # ---------------------------- RX SERIES ---------------------------- #
502 | 
503 | - name: RX-10
504 |   model_number: 10
505 |   series: R
506 |   encoder_cpr: 1023
507 |   encoder_range_deg: 300
508 |   velocity_radps_to_reg: 86.023
509 |   effort_reg_max: 1000
510 |   effort_reg_to_mA: 0.001
511 | 
512 | - name: RX-24F
513 |   model_number: 24
514 |   series: R
515 |   encoder_cpr: 1023
516 |   encoder_range_deg: 300
517 |   velocity_radps_to_reg: 86.023
518 |   effort_reg_max: 1000
519 |   effort_reg_to_mA: 0.001
520 | 
521 | - name: RX-28
522 |   model_number: 28
523 |   series: R
524 |   encoder_cpr: 1023
525 |   encoder_range_deg: 300
526 |   velocity_radps_to_reg: 86.023
527 |   effort_reg_max: 1000
528 |   effort_reg_to_mA: 0.001
529 | 
530 | - name: RX-64
531 |   model_number: 28
532 |   series: 64
533 |   encoder_cpr: 1023
534 |   encoder_range_deg: 300
535 |   velocity_radps_to_reg: 86.023
536 |   effort_reg_max: 1000
537 |   effort_reg_to_mA: 0.001
538 | 
539 | # ---------------------------- DX SERIES ---------------------------- #
540 | 
541 | - name: DX-113
542 |   model_number: 113
543 |   series: D
544 |   encoder_cpr: 1023
545 |   encoder_range_deg: 300
546 |   velocity_radps_to_reg: 86.023
547 |   effort_reg_max: 1000
548 |   effort_reg_to_mA: 0.001
549 | 
550 | - name: DX-116
551 |   model_number: 116
552 |   series: D
553 |   encoder_cpr: 1023
554 |   encoder_range_deg: 300
555 |   velocity_radps_to_reg: 86.023
556 |   effort_reg_max: 1000
557 |   effort_reg_to_mA: 0.001
558 | 
559 | - name: DX-117
560 |   model_number: 117
561 |   series: D
562 |   encoder_cpr: 1023
563 |   encoder_range_deg: 300
564 |   velocity_radps_to_reg: 86.023
565 |   effort_reg_max: 1000
566 |   effort_reg_to_mA: 0.001
567 | 
568 | # ---------------------------- EX SERIES ---------------------------- #
569 | 
570 | - name: EX-106+
571 |   model_number: 107
572 |   series: E
573 |   encoder_cpr: 1023
574 |   encoder_range_deg: 300
575 |   velocity_radps_to_reg: 86.023
576 |   effort_reg_max: 1000
577 |   effort_reg_to_mA: 0.001
578 | 


--------------------------------------------------------------------------------
/include/dynamixel_interface/dynamixel_const.h:
--------------------------------------------------------------------------------
  1 | /* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
  2 |  * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
  3 |  * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
  4 |  * terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
  5 |  * and binary forms, with or without modification, are permitted provided that the following conditions are met:
  6 |  * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
  7 |  *   disclaimer.
  8 |  * - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
  9 |  *   disclaimer in the documentation and/or other materials provided with the distribution.
 10 |  * - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
 11 |  *   this software without specific prior written permission of CSIRO.
 12 |  *
 13 |  * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
 14 |  * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 15 |  * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
 16 |  * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
 17 |  * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
 18 |  * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
 19 |  * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
 20 |  * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
 21 |  * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
 22 |  * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
 23 |  * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
 24 |  * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
 25 |  * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
 26 |  * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
 27 |  * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
 28 |  * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
 29 |  * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
 30 |  * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
 31 |  * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
 32 |  * (b)  THE REPAIR OF THE SOFTWARE;
 33 |  * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
 34 |  *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
 35 |  * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
 36 |  * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
 37 |  *
 38 |  * Third Party Components:
 39 |  *
 40 |  * The following third party components are distributed with the Software.  You agree to comply with the license terms
 41 |  * for these components as part of accessing the Software.  Other third party software may also be identified in
 42 |  * separate files distributed with the Software.
 43 |  * ___________________________________________________________________
 44 |  *
 45 |  * dynamixel_interface is forked from projects authored by Brian
 46 |  * Axelrod (on behalf of Willow Garage):
 47 |  *
 48 |  * https://github.com/baxelrod/dynamixel_pro_controller
 49 |  * https://github.com/baxelrod/dynamixel_pro_driver
 50 |  *
 51 |  * Thus they retain the following notice:
 52 |  *
 53 |  * Software License Agreement (BSD License)
 54 |  * Copyright (c) 2013, Willow Garage
 55 |  * All rights reserved.
 56 |  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 57 |  * following conditions are met:
 58 |  *  - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 59 |  *    disclaimer.
 60 |  *  - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 61 |  *    following disclaimer in the documentation and/or other materials provided with the distribution.
 62 |  *  - Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products
 63 |  *    derived from this software without specific prior written permission.
 64 |  *
 65 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 66 |  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 67 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 68 |  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 69 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 70 |  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 71 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 72 |  * ___________________________________________________________________
 73 |  */
 74 | 
 75 | 
 76 | /**
 77 |  * @file   dynamixel_const.h
 78 |  * @author Tom Molnar (Tom.Molnar@data61.csiro.au), Brian Axelrod
 79 |  * @date   December, 2016
 80 |  * @brief  Defines the register address tables for each series of dynamixel,
 81 |  * as well as the control and status codes for communication.
 82 |  */
 83 | 
 84 | 
 85 | #ifndef DYNAMIXEL_CONST_H__
 86 | #define DYNAMIXEL_CONST_H__
 87 | 
 88 | #include <stdint.h>
 89 | #include <string>
 90 | 
 91 | 
 92 | namespace dynamixel_interface
 93 | {
 94 | /// Dynamixel types
 95 | enum DynamixelSeriesType
 96 | {
 97 |   kSeriesAX = 0,
 98 |   kSeriesRX = 1,
 99 |   kSeriesDX = 2,
100 |   kSeriesEX = 3,
101 |   kSeriesLegacyMX = 4,
102 |   kSeriesMX = 5,
103 |   kSeriesX = 6,
104 |   kSeriesLegacyPro = 7,
105 |   kSeriesP = 8,
106 |   kSeriesUnknown = 9
107 | };
108 | 
109 | /// Legacy control table, for older dynamixels:
110 | /// - MX (1.0)
111 | /// - AX
112 | /// - RX
113 | enum DynamixelLegacyRegisterTable
114 | {
115 |   // EEPROM
116 |   kRegLegacy_ModelNumber = 0,
117 |   kRegLegacy_FirmwareVersion = 2,
118 |   kRegLegacy_ID = 3,
119 |   kRegLegacy_BaudRate = 4,
120 |   kRegLegacy_ReturnDelayTime = 5,
121 |   kRegLegacy_CWAngleLimit = 6,
122 |   kRegLegacy_CCWAngleLimit = 8,
123 |   kRegLegacy_DriveMode = 10,
124 |   kRegLegacy_TemperatureLimit = 11,
125 |   kRegLegacy_MinVoltageLimit = 12,
126 |   kRegLegacy_MaxVoltageLimit = 13,
127 |   kRegLegacy_MaxTorque = 14,
128 |   kRegLegacy_ReturnLevel = 16,
129 |   kRegLegacy_AlarmLED = 17,
130 |   kRegLegacy_AlarmShutdown = 18,
131 |   kRegLegacy_MultiTurnOffset = 20,
132 |   kRegLegacy_ResolutionDivider = 22,
133 | 
134 |   // RAM
135 |   kRegLegacy_TorqueEnable = 24,
136 |   kRegLegacy_LED = 25,
137 | 
138 |   // - MX (1.0) specific
139 |   kRegLegacy_DGain = 26,
140 |   kRegLegacy_IGain = 27,
141 |   kRegLegacy_PGain = 28,
142 | 
143 |   // - AX/RX specific
144 |   kRegLegacy_CWComplianceMargin = 26,
145 |   kRegLegacy_CCWComplianceMargin = 27,
146 |   kRegLegacy_CWComplianceSlope = 28,
147 |   kRegLegacy_CCWComplianceSlope = 29,
148 | 
149 |   kRegLegacy_GoalPosition = 30,
150 |   kRegLegacy_MovingSpeed = 32,
151 |   kRegLegacy_TorqueLimit = 34,
152 |   kRegLegacy_PresentPosition = 36,
153 |   kRegLegacy_PresentSpeed = 38,
154 |   kRegLegacy_PresentLoad = 40,
155 |   kRegLegacy_PresentVoltage = 42,
156 |   kRegLegacy_PresentTemperature = 43,
157 |   kRegLegacy_RegisteredInstruction = 44,
158 |   kRegLegacy_Moving = 46,
159 |   kRegLegacy_Lock = 47,
160 |   kRegLegacy_Punch = 48,
161 |   kRegLegacy_RealtimeTick = 50,
162 |   kRegLegacy_SensedCurrent = 56,
163 |   kRegLegacy_PresentCurrent = 68,
164 |   kRegLegacy_TorqueControlEnable = 70,
165 |   kRegLegacy_GoalTorque = 71,
166 |   kRegLegacy_GoalAcceleration = 73,
167 | };
168 | 
169 | 
170 | /// Standard control table, for newer models of dynamixels supporting protocol 2.0:
171 | /// - MX (2.0)
172 | /// - X (2.0) (excepting the XL-320, which has a unique table)
173 | enum DynamixelStandardRegisterTable
174 | {
175 |   // EEPROM
176 |   kRegStandard_ModelNumber = 0,
177 |   kRegStandard_ModelInfo = 2,
178 |   kRegStandard_FirmwareVersion = 6,
179 |   kRegStandard_ID = 7,
180 |   kRegStandard_BaudRate = 8,
181 |   kRegStandard_ReturnDelayTime = 9,
182 |   kRegStandard_DriveMode = 10,
183 |   kRegStandard_OperatingMode = 11,
184 |   kRegStandard_ShadowID = 12,
185 |   kRegStandard_ProtocolVersion = 13,
186 |   kRegStandard_HomingOffset = 20,
187 |   kRegStandard_MovingThreshold = 24,
188 |   kRegStandard_TemperatureLimit = 31,
189 |   kRegStandard_MaxVoltageLimit = 32,
190 |   kRegStandard_MinVoltageLimit = 34,
191 |   kRegStandard_PWMLimit = 36,
192 |   kRegStandard_CurrentLimit = 38,
193 |   kRegStandard_AccelerationLimit = 40,
194 |   kRegStandard_VelocityLimit = 44,
195 |   kRegStandard_MaxPositionLimit = 48,
196 |   kRegStandard_MinPositionLimit = 52,
197 |   kRegStandard_DataPort1Mode = 56,
198 |   kRegStandard_DataPort2Mode = 57,
199 |   kRegStandard_DataPort3Mode = 58,
200 |   kRegStandard_Shutdown = 63,
201 | 
202 |   // RAM
203 |   kRegStandard_TorqueEnable = 64,
204 |   kRegStandard_LED = 65,
205 |   kRegStandard_StatusReturnLevel = 68,
206 |   kRegStandard_RegisteredInstruction = 69,
207 |   kRegStandard_HardwareErrorStatus = 70,
208 |   kRegStandard_VelocityIGain = 76,
209 |   kRegStandard_VelocityPGain = 78,
210 |   kRegStandard_PositionDGain = 80,
211 |   kRegStandard_PositionIGain = 82,
212 |   kRegStandard_PositionPGain = 84,
213 |   kRegStandard_Feedforward2ndGain = 88,
214 |   kRegStandard_Feedforward1stGain = 90,
215 |   kRegStandard_BusWatchdog = 98,
216 |   kRegStandard_GoalPWM = 100,
217 |   kRegStandard_GoalCurrent = 102,
218 |   kRegStandard_GoalVelocity = 104,
219 |   kRegStandard_ProfileAcceleration = 108,
220 |   kRegStandard_ProfileVelocity = 112,
221 |   kRegStandard_GoalPosition = 116,
222 |   kRegStandard_RealtimeTick = 120,
223 |   kRegStandard_Moving = 122,
224 |   kRegStandard_MovingStatus = 123,
225 |   kRegStandard_PresentPWM = 124,
226 |   kRegStandard_PresentCurrent = 126,
227 |   kRegStandard_PresentVelocity = 128,
228 |   kRegStandard_PresentPosition = 132,
229 |   kRegStandard_VelocityTrajectory = 136,
230 |   kRegStandard_PositionTrajectory = 140,
231 |   kRegStandard_PresentInputVoltage = 144,
232 |   kRegStandard_PresentTemperature = 146,
233 |   kRegStandard_DataPort1 = 152,
234 |   kRegStandard_DataPort2 = 154,
235 |   kRegStandard_DataPort3 = 156,
236 |   kRegStandard_IndirectAddress1 = 168,
237 |   kRegStandard_IndirectData1 = 224,
238 | };
239 | 
240 | /// Control table for Dynamixel P series (new pro)
241 | /// - Dynamixel-P
242 | /// - Dynamixel PRO+, sometimes listed as PRO(A)
243 | enum DynamixelProRegisterTable
244 | {
245 |   // EEPROM
246 |   kRegP_ModelNumber = 0,
247 |   kRegP_ModelInfo = 2,
248 |   kRegP_FirmwareVersion = 6,
249 |   kRegP_ID = 7,
250 |   kRegP_BaudRate = 8,
251 |   kRegP_ReturnDelayTime = 9,
252 |   kRegP_DriveMode = 10,
253 |   kRegP_OperatingMode = 11,
254 |   kRegP_ShadowID = 12,
255 |   kRegP_ProtocolType = 13,
256 |   kRegP_HomingOffset = 20,
257 |   kRegP_MovingThreshold = 24,
258 |   kRegP_TemperatureLimit = 31,
259 |   kRegP_MaxVoltageLimit = 32,
260 |   kRegP_MinVoltageLimit = 34,
261 |   kRegP_PWMLimit = 36,
262 |   kRegP_CurrentLimit = 38,
263 |   kRegP_AccelerationLimit = 40,
264 |   kRegP_VelocityLimit = 44,
265 |   kRegP_MaxPositionLimit = 48,
266 |   kRegP_MinPositionLimit = 52,
267 |   kRegP_DataPort1Mode = 56,
268 |   kRegP_DataPort2Mode = 57,
269 |   kRegP_DataPort3Mode = 58,
270 |   kRegP_DataPort4Mode = 59,
271 |   kRegP_Shutdown = 63,
272 |   kRegP_IndirectAddress1 = 168,
273 | 
274 |   // RAM
275 |   kRegP_TorqueEnable = 512,
276 |   kRegP_RedLED = 513,
277 |   kRegP_GreenLED = 514,
278 |   kRegP_BlueLED = 515,
279 |   kRegP_StatusReturnLevel = 516,
280 |   kRegP_RegisteredInstruction = 517,
281 |   kRegP_HardwareErrorStatus = 518,
282 |   kRegP_VelocityIGain = 524,
283 |   kRegP_VelocityPGain = 526,
284 |   kRegP_PositionDGain = 528,
285 |   kRegP_PositionIGain = 530,
286 |   kRegP_PositionPGain = 532,
287 |   kRegP_Feedforward2ndGain = 536,
288 |   kRegP_Feedforward1stGain = 538,
289 |   kRegP_BusWatchdog = 546,
290 |   kRegP_GoalPWM = 548,
291 |   kRegP_GoalCurrent = 550,
292 |   kRegP_GoalVelocity = 552,
293 |   kRegP_ProfileAcceleration = 556,
294 |   kRegP_ProfileVelocity = 560,
295 |   kRegP_GoalPosition = 564,
296 |   kRegP_RealtimeTick = 568,
297 |   kRegP_Moving = 570,
298 |   kRegP_MovingStatus = 571,
299 |   kRegP_PresentPWM = 572,
300 |   kRegP_PresentCurrent = 574,
301 |   kRegP_PresentVelocity = 576,
302 |   kRegP_PresentPosition = 580,
303 |   kRegP_VelocityTrajectory = 584,
304 |   kRegP_PositionTrajectory = 588,
305 |   kRegP_PresentInputVoltage = 592,
306 |   kRegP_PresentTemperature = 594,
307 |   kRegP_DataPort1 = 600,
308 |   kRegP_DataPort2 = 602,
309 |   kRegP_DataPort3 = 604,
310 |   kRegP_DataPort4 = 606,
311 |   kRegP_IndirectData1 = 634
312 | };
313 | 
314 | 
315 | /// Control table/register addresses for each series of dynamixel
316 | enum DynamixelLegacyProRegisterTable
317 | {
318 |   // EEPROM
319 |   kRegLegacyPro_ModelNumber = 0,
320 |   kRegLegacyPro_ModelInfo = 2,
321 |   kRegLegacyPro_FirmwareVersion = 6,
322 |   kRegLegacyPro_ID = 7,
323 |   kRegLegacyPro_BaudRate = 8,
324 |   kRegLegacyPro_ReturnDelayTime = 9,
325 |   kRegLegacyPro_OperatingMode = 11,
326 |   kRegLegacyPro_LimitTemperature = 21,
327 |   kRegLegacyPro_DownLimitVoltage = 24,
328 |   kRegLegacyPro_UpLimitVoltage = 22,
329 |   kRegLegacyPro_LED = 25,
330 |   kRegLegacyPro_AccelLimit = 26,
331 |   kRegLegacyPro_VelocityLimit = 32,
332 |   kRegLegacyPro_MaxTorque = 30,
333 |   kRegLegacyPro_MaxAngleLimit = 36,
334 |   kRegLegacyPro_MinAngleLimit = 40,
335 |   kRegLegacyPro_DataPort1Mode = 44,
336 |   kRegLegacyPro_DataPort2Mode = 45,
337 |   kRegLegacyPro_DataPort3Mode = 46,
338 |   kRegLegacyPro_DataPort4Mode = 47,
339 | 
340 |   // RAM
341 |   kRegLegacyPro_VelocityIGain = 586,
342 |   kRegLegacyPro_VelocityPGain = 588,
343 |   kRegLegacyPro_PositionPGain = 594,
344 |   kRegLegacyPro_TorqueEnable = 562,
345 |   kRegLegacyPro_GoalPosition = 596,
346 |   kRegLegacyPro_GoalVelocity = 600,
347 |   kRegLegacyPro_GoalTorque = 604,
348 |   kRegLegacyPro_PresentPosition = 611,
349 |   kRegLegacyPro_PresentVelocity = 615,
350 |   kRegLegacyPro_PresentCurrent = 621,
351 |   kRegLegacyPro_PresentVoltage = 623,
352 |   kRegLegacyPro_PresentTemperature = 625,
353 | 
354 |   kRegLegacyPro_DataPort1 = 626,
355 |   kRegLegacyPro_DataPort2 = 628,
356 |   kRegLegacyPro_DataPort3 = 630,
357 |   kRegLegacyPro_DataPort4 = 632,
358 | 
359 |   kRegLegacyPro_RegisteredInstruction = 890,
360 |   kRegLegacyPro_Moving = 610,
361 |   kRegLegacyPro_ReturnLevel = 891,
362 |   kRegLegacyPro_HardwareErrorStatus = 892,
363 | };
364 | 
365 | 
366 | /// The different control modes available on the dynamixel servos. The values chosen for each type reflect those used
367 | /// on the motors themselves.
368 | enum DynamixelControlMode
369 | {
370 |   UNKNOWN = -1,
371 | 
372 |   kModeTorqueControl = 0,
373 |   kModeVelocityControl = 1,
374 |   kModePositionControl = 3,
375 | 
376 |   kModeExtendedPositionControl = 4,
377 |   kModeCurrentBasedPositionControl = 5,
378 | 
379 |   kModePWMControl = 16
380 | 
381 | };
382 | 
383 | 
384 | /// Instruction codes for various commands
385 | enum DynamixelInstruction
386 | {
387 |   kInstPing = 1,
388 |   kInstReadData = 2,
389 |   kInstWriteData = 3,
390 |   kInstRegWrite = 4,
391 |   kInstAction = 5,
392 |   kInstReset = 6,
393 | 
394 |   kInstSyncWrite = 0x83,
395 |   kInstBulkRead = 0x92,
396 | 
397 |   kInstBroadcast = 254,
398 | 
399 | };
400 | 
401 | 
402 | /// Error return codes
403 | enum DynamixelErrorCode
404 | {
405 |   kErrorNoError = 0,
406 |   // Common error codes
407 |   kErrorOverload = 32,
408 |   kErrorElectricShock = 16,
409 |   kErrorMotorEncoder = 8,
410 |   kErrorOverheating = 4,
411 |   kErrorMotorHall = 2,
412 |   kErrorInputVoltage = 1,
413 |   // Legacy specific error codes
414 |   kErrorLegacyInstruction = 64,
415 |   kErrorLegacyChecksum = 16,
416 |   kErrorLegacyRange = 8,
417 |   kErrorLegacyAngleLimit = 2,
418 | 
419 | };
420 | 
421 | }  // namespace dynamixel_interface
422 | 
423 | #endif  // DYNAMIXEL_CONST_H__
424 | 


--------------------------------------------------------------------------------
/include/dynamixel_interface/dynamixel_interface_controller.h:
--------------------------------------------------------------------------------
  1 | /* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
  2 |  * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
  3 |  * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
  4 |  * terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
  5 |  * and binary forms, with or without modification, are permitted provided that the following conditions are met:
  6 |  * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
  7 |  *   disclaimer.
  8 |  * - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
  9 |  *   disclaimer in the documentation and/or other materials provided with the distribution.
 10 |  * - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
 11 |  *   this software without specific prior written permission of CSIRO.
 12 |  *
 13 |  * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
 14 |  * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 15 |  * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
 16 |  * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
 17 |  * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
 18 |  * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
 19 |  * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
 20 |  * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
 21 |  * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
 22 |  * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
 23 |  * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
 24 |  * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
 25 |  * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
 26 |  * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
 27 |  * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
 28 |  * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
 29 |  * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
 30 |  * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
 31 |  * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
 32 |  * (b)  THE REPAIR OF THE SOFTWARE;
 33 |  * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
 34 |  *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
 35 |  * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
 36 |  * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
 37 |  *
 38 |  * Third Party Components:
 39 |  *
 40 |  * The following third party components are distributed with the Software.  You agree to comply with the license terms
 41 |  * for these components as part of accessing the Software.  Other third party software may also be identified in
 42 |  * separate files distributed with the Software.
 43 |  * ___________________________________________________________________
 44 |  *
 45 |  * dynamixel_interface is forked from projects authored by Brian
 46 |  * Axelrod (on behalf of Willow Garage):
 47 |  *
 48 |  * https://github.com/baxelrod/dynamixel_pro_controller
 49 |  * https://github.com/baxelrod/dynamixel_pro_driver
 50 |  *
 51 |  * Thus they retain the following notice:
 52 |  *
 53 |  * Software License Agreement (BSD License)
 54 |  * Copyright (c) 2013, Willow Garage
 55 |  * All rights reserved.
 56 |  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 57 |  * following conditions are met:
 58 |  *  - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 59 |  *    disclaimer.
 60 |  *  - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 61 |  *    following disclaimer in the documentation and/or other materials provided with the distribution.
 62 |  *  - Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products
 63 |  *    derived from this software without specific prior written permission.
 64 |  *
 65 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 66 |  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 67 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 68 |  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 69 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 70 |  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 71 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 72 |  * ___________________________________________________________________
 73 |  */
 74 | 
 75 | /**
 76 |  * @file   dynamixel_interface_controller.h
 77 |  * @author Tom Molnar (Tom.Molnar@data61.csiro.au), Brian Axelrod
 78 |  * @date   January, 2017
 79 |  * @brief  Defines the dynamixel controller class and the types used therein
 80 |  */
 81 | 
 82 | #ifndef DYNAMIXEL_INTERFACE_CONTROLLER_H_
 83 | #define DYNAMIXEL_INTERFACE_CONTROLLER_H_
 84 | 
 85 | #include <algorithm>
 86 | #include <map>
 87 | #include <memory>
 88 | #include <mutex>
 89 | #include <string>
 90 | 
 91 | #include <XmlRpcValue.h>
 92 | 
 93 | #include <ros/callback_queue.h>
 94 | #include <ros/ros.h>
 95 | #include <ros/spinner.h>
 96 | #include <sensor_msgs/JointState.h>
 97 | 
 98 | #include <dynamixel_interface/dynamixel_interface_driver.h>
 99 | 
100 | #include <dynamixel_interface/DataPort.h>
101 | #include <dynamixel_interface/DataPorts.h>
102 | #include <dynamixel_interface/ServoDiag.h>
103 | #include <dynamixel_interface/ServoDiags.h>
104 | 
105 | namespace dynamixel_interface
106 | {
107 | /// Struct that describes each servo's place in the system including which joint it corresponds to.
108 | struct DynamixelInfo
109 | {
110 |   int id;                  ///< The unique (per port) ID of the motor
111 |   std::string joint_name;  ///< The unique (globally) name of the joint
112 | 
113 |   double max_vel;       ///< Motor maximum joint velocity (rad/s)
114 |   double torque_limit;  ///< Motor maximum torque limit (%rated max)
115 | 
116 |   int zero_pos;  ///< Motor initial position (in raw encoder values). This value defines the 0 radian position
117 |   int min_pos;   ///< Motor minimum encoder value. Note that if min > max, the motor direction is reversed
118 |   int max_pos;   ///< Motor maximum encoder value. Note that if min > max, the motor direction is reversed
119 | 
120 |   const DynamixelSpec *model_spec;  ///< Motor model specification including encoder counts and unit conversion factors
121 | 
122 |   bool torque_enabled;                ///< Motor enable flag
123 |   DynamixelControlMode current_mode;  ///< control mode (position, velocity, torque)
124 |   uint8_t hardware_status;            ///< current motor status, used for hardware error reporting
125 | };
126 | 
127 | /// Struct which stores information about each port in use and which joints use that port
128 | struct PortInfo
129 | {
130 |   std::string port_name;  ///< User defined port name
131 | 
132 |   std::string device;  ///< Serial device name
133 |   int baudrate;        ///< Serial baud rate
134 | 
135 |   bool use_legacy_protocol;  ///< boolean indicating if legacy protocol (for older series dynamixels) is in use
136 | 
137 |   std::unique_ptr<DynamixelInterfaceDriver> driver;  ///< The driver object
138 | 
139 |   std::unordered_map<std::string, DynamixelInfo> joints;  ///< map of joint information by name
140 | };
141 | 
142 | /// This class forms a ROS Node that provides an interface with the dynamixel series of servos.
143 | /// The controller functions on a timer based callback for updating the motor states. Commands are
144 | /// Continuously sent to the motor and updated via callback once new messages are published to the command
145 | /// Topic. This class also provides a threaded interface for controlling multiple sets of dynamixels simultaneously
146 | /// and synchronously through different serial ports. This allows robots with many motors to reduce the overall IO
147 | /// time required for control.
148 | class DynamixelInterfaceController
149 | {
150 | public:
151 |   /// Constructor, loads the motor configuration information from the specified yaml file and intialises
152 |   /// The motor states.
153 |   DynamixelInterfaceController();
154 | 
155 |   /// Destructor, deletes the objects holding the serial ports and disables the motors if required
156 |   ~DynamixelInterfaceController();
157 | 
158 |   /// Parses param information from the rosparam server
159 |   /// @returns true if all params parsed successfully, false otherwise
160 |   bool parseParameters(void);
161 | 
162 |   /// Initialises the controller, performing the following steps:
163 |   /// - for each port:
164 |   ///   - Initialise driver
165 |   ///   - for each dynamixel
166 |   //      - check response on bus
167 |   ///     - load model information
168 |   ///     - validate compatibility with port
169 |   /// @returns true on successful initialisation, false otherwise
170 |   bool initialise();
171 | 
172 |   /// Gets the target loop rate for the controller
173 |   /// @returns the target loop rate for the controller (Hz)
174 |   inline double getLoopRate(void) { return loop_rate_; };
175 | 
176 |   /// main loop for performing IO, handles the creation and joining of IO threads for each port, so that IO for multiple
177 |   /// usb devices can be threaded.
178 |   void loop(void);
179 | 
180 | private:
181 |   /// Parses the information in the yaml file for each port
182 |   /// @param[in] ports the xml structure to be parsed
183 |   /// @returns true if all params parsed false otherwise
184 |   bool parsePortInformation(XmlRpc::XmlRpcValue ports);
185 | 
186 |   /// Parses the information in the yaml file for each servo
187 |   /// @param[out] port the port object to parse the servo info into
188 |   /// @param[out] servos the xml structure to be parsed
189 |   /// @returns true if all params parsed, false otherwise
190 |   bool parseServoInformation(PortInfo &port, XmlRpc::XmlRpcValue servos);
191 | 
192 |   /// Initialises the port, opening the driver, and validating all dynamixels
193 |   /// @param[in] port the port object to initialise
194 |   /// @returns true on successful initialisation, false otherwise
195 |   bool initialisePort(PortInfo &port);
196 | 
197 |   /// Initialises the dynamixel. Pings the given id to make sure it exists, then loads it's model information and sets
198 |   /// up the relevant registers
199 |   /// @param[in] port the port for this dynamixel
200 |   /// @param[in] dynamixel the dynamixel to intialise
201 |   /// @returns true on successful initialisation, false otherwise
202 |   bool initialiseDynamixel(PortInfo &port, DynamixelInfo &dynamixel);
203 | 
204 |   /// Callback for recieving a command from the /desired_joint_state topic. The function atomically updates the class
205 |   /// member variable containing the latest message and sets a flag indicating a new message has been received
206 |   /// @param[in] joint_commands the command received from the topic
207 |   void jointStateCallback(const sensor_msgs::JointState::ConstPtr &joint_commands);
208 | 
209 |   /// Top level control function for each port's IO thread.
210 |   /// @param[in] port the port for this thread to perform IO on
211 |   /// @param[out] read_msg the JointState this threads joint data is read into
212 |   /// @param[out] dataport_msg the DataPorts msg this thread reads dataport data into
213 |   /// @param[out] status_msgs the ServoDiags msg this thread reads diagnostic data into
214 |   /// @param[in] perform_write whether we are writing data this iteration
215 |   void multiThreadedIO(PortInfo &port, sensor_msgs::JointState &read_msg, dynamixel_interface::DataPorts &dataport_msg,
216 |                        dynamixel_interface::ServoDiags &status_msg, bool perform_write) const;
217 | 
218 |   /// Function called in each thread to perform a write on a port
219 |   /// @param[in] port_num index used to retrieve port information from port list
220 |   /// @param[in] joint_commands message cointaining the commands for each joint
221 |   void multiThreadedWrite(PortInfo &port, sensor_msgs::JointState joint_commands) const;
222 | 
223 |   /// Function in thread to perform read on a port.
224 |   /// @param[in] port the port for this thread to perform IO on
225 |   /// @param[out] read_msg the JointState this threads joint data is read into
226 |   /// @param[out] dataport_msg the DataPorts msg this thread reads dataport data into
227 |   /// @param[out] status_msgs the ServoDiags msg this thread reads diagnostic data into
228 |   void multiThreadedRead(PortInfo &port, sensor_msgs::JointState &read_msg,
229 |                          dynamixel_interface::DataPorts &dataports_msg,
230 |                          dynamixel_interface::ServoDiags &diags_msg) const;
231 | 
232 |   std::unique_ptr<ros::NodeHandle> nh_;  ///< Handler for the ROS Node
233 | 
234 |   std::vector<PortInfo> dynamixel_ports_;  ///< method of control (position/velocity/torque)
235 | 
236 |   sensor_msgs::JointState write_msg_;  ///< Stores the last message received from the write command topic
237 | 
238 |   std::mutex write_mutex_;    ///< Mutex for write_msg, as there are potentially multiple threads
239 |   bool write_ready_ = false;  ///< Booleans indicating if we have received commands
240 | 
241 |   ros::Subscriber joint_state_subscriber_;  ///< Gets joint states for writes
242 | 
243 |   ros::Publisher joint_state_publisher_;  ///< Publishes joint states from reads
244 |   ros::Publisher diagnostics_publisher_;  ///< Publishes joint states from reads
245 |   ros::Publisher dataport_publisher_;     ///< Publishes the data from the external ports on dynamixel_pros
246 |   ros::Publisher debug_publisher_;        ///< Debug message publisher
247 | 
248 |   ros::Timer broadcast_timer_;  ///< Timer that controls the rate of the IO callback
249 | 
250 |   // variables for tracking diagnostics reading rate
251 |   bool read_diagnostics_;         ///< Bool for telling threads to read diagnostics data
252 |   uint diagnostics_counter_ = 0;  ///< Counter for tracking diagnostics rate
253 |   uint diagnostics_iters_ = 0;    ///< publish when diagnostic_counter_ == this
254 | 
255 |   // variables for tracking dataport reading rate
256 |   bool read_dataport_;         ///< Bool for telling threads to read dataport data
257 |   uint dataport_counter_ = 0;  ///< counter for tracking dataport rate
258 |   uint dataport_iters_ = 0;    ///< publish when dataport_counter_ == this
259 | 
260 |   DynamixelControlMode control_type_;  /// method of control (position/velocity/torque)
261 | 
262 |   bool parameters_parsed_ = false;  ///< Bool indicating if we have parsed parameters
263 |   bool initialised_ = false;        ///< Bool indicating if we are ready for operation
264 | 
265 |   bool stop_motors_on_shutdown_;  ///< Indicates if the motors should be turned off when the controller stops
266 |   bool ignore_input_velocity_;    ///< can set driver to ignore profile velocity commands in position mode
267 | 
268 |   double global_max_vel_;       ///< global joint speed limit
269 |   double global_torque_limit_;  ///< global joint torque limit
270 | 
271 |   double loop_rate_;         ///< Desired loop rate (joint states are published at this rate)
272 |   double diagnostics_rate_;  ///< Desired rate at which servo diagnostic information is published
273 |   double dataport_rate_;     ///< Rate at which the pro external dataport is read
274 |   int recv_queue_size_ = 1;  ///< Receive queue size for desired_joint_states topic
275 | };
276 | 
277 | }  // namespace dynamixel_interface
278 | 
279 | #endif  // DYNAMIXEL_INTERFACE_CONTROLLER_H_
280 | 


--------------------------------------------------------------------------------
/include/dynamixel_interface/dynamixel_interface_driver.h:
--------------------------------------------------------------------------------
  1 | /* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
  2 |  * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
  3 |  * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
  4 |  * terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
  5 |  * and binary forms, with or without modification, are permitted provided that the following conditions are met:
  6 |  * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
  7 |  *   disclaimer.
  8 |  * - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
  9 |  *   disclaimer in the documentation and/or other materials provided with the distribution.
 10 |  * - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
 11 |  *   this software without specific prior written permission of CSIRO.
 12 |  *
 13 |  * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
 14 |  * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 15 |  * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
 16 |  * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
 17 |  * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
 18 |  * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
 19 |  * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
 20 |  * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
 21 |  * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
 22 |  * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
 23 |  * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
 24 |  * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
 25 |  * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
 26 |  * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
 27 |  * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
 28 |  * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
 29 |  * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
 30 |  * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
 31 |  * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
 32 |  * (b)  THE REPAIR OF THE SOFTWARE;
 33 |  * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
 34 |  *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
 35 |  * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
 36 |  * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
 37 |  *
 38 |  * Third Party Components:
 39 |  *
 40 |  * The following third party components are distributed with the Software.  You agree to comply with the license terms
 41 |  * for these components as part of accessing the Software.  Other third party software may also be identified in
 42 |  * separate files distributed with the Software.
 43 |  * ___________________________________________________________________
 44 |  *
 45 |  * dynamixel_interface is forked from projects authored by Brian
 46 |  * Axelrod (on behalf of Willow Garage):
 47 |  *
 48 |  * https://github.com/baxelrod/dynamixel_pro_controller
 49 |  * https://github.com/baxelrod/dynamixel_pro_driver
 50 |  *
 51 |  * Thus they retain the following notice:
 52 |  *
 53 |  * Software License Agreement (BSD License)
 54 |  * Copyright (c) 2013, Willow Garage
 55 |  * All rights reserved.
 56 |  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 57 |  * following conditions are met:
 58 |  *  - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 59 |  *    disclaimer.
 60 |  *  - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 61 |  *    following disclaimer in the documentation and/or other materials provided with the distribution.
 62 |  *  - Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products
 63 |  *    derived from this software without specific prior written permission.
 64 |  *
 65 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 66 |  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 67 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 68 |  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 69 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 70 |  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 71 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 72 |  * ___________________________________________________________________
 73 |  */
 74 | 
 75 | /**
 76 |  * @file   dynamixel_interface_driver.h
 77 |  * @author Tom Molnar (Tom.Molnar@data61.csiro.au), Brian Axelrod
 78 |  * @date   January, 2017
 79 |  * @brief  Defines the hardware abstraction methods for communicating with dynamixels
 80 |  */
 81 | 
 82 | 
 83 | #ifndef DYNAMIXEL_INTERFACE_DRIVER_H_
 84 | #define DYNAMIXEL_INTERFACE_DRIVER_H_
 85 | 
 86 | #include <stdint.h>
 87 | #include <map>
 88 | #include <string>
 89 | #include <unordered_map>
 90 | #include <vector>
 91 | 
 92 | #include <dynamixel_interface/dynamixel_const.h>
 93 | #include <dynamixel_sdk/dynamixel_sdk.h>
 94 | #include <ros/ros.h>
 95 | 
 96 | namespace dynamixel_interface
 97 | {
 98 | /// Struct that describes the dynamixel motor's static and physical properties
 99 | struct DynamixelSpec
100 | {
101 |   std::string name;              ///< The Model Name
102 |   uint16_t model_number;         ///< Model number (e.g 29 = MX-28)
103 |   DynamixelSeriesType type;      ///< Model type (e.g MX, AX, Pro)
104 |   bool external_ports;           ///< If this model has data ports
105 |   int encoder_cpr;               ///< Motor encoder counts per revolution
106 |   double encoder_range_deg;      ///< Motor encoder range in degrees
107 |   double velocity_radps_to_reg;  ///< Conversion factor from velocity in radians/sec to register counts
108 |   double effort_reg_max;         ///< Max possible value for effort register
109 |   double effort_reg_to_mA;       ///< Conversion factor from register values to current in mA
110 | };
111 | 
112 | 
113 | /// Basic struct for representing dynamixel data exchange
114 | struct SyncData
115 | {
116 |   int id;                     ///< id of dynamixel
117 |   DynamixelSeriesType type;   ///< type of dynamixel
118 |   bool success;               ///< bool indicating comms success
119 |   uint8_t error;              ///< Error code
120 |   std::vector<uint8_t> data;  ///< IO data array
121 | };
122 | 
123 | /// data struct used with getBulkState() to retrieve physical state
124 | struct DynamixelState : SyncData
125 | {
126 |   int32_t position;  ///< position register value, units differ by series
127 |   int32_t velocity;  ///< velocity register value, units differ by series
128 |   int32_t effort;    ///< effort register value, units differ by series
129 | };
130 | 
131 | /// data struct used with getBulkDiagnosticInfo() to retrieve diagnostics
132 | struct DynamixelDiagnostic : SyncData
133 | {
134 |   int32_t voltage;      ///< voltage register value, usually units of 0.1V
135 |   int32_t temperature;  ///< temperature register value, usually units of 0.1C
136 | };
137 | 
138 | /// data struct used with getBulkDataportInfo() to retrieve dataport values
139 | struct DynamixelDataport : SyncData
140 | {
141 |   std::array<uint16_t, 4> port_values;  ///< values from dataports, units differ by series and dataport setting
142 | };
143 | 
144 | /// Provides the handling of the low level communications between the
145 | /// dynamixels and the controller
146 | class DynamixelInterfaceDriver
147 | {
148 | public:
149 |   /// Constructor.
150 |   /// @param[in] device The serial port to connect to
151 |   /// @param[in] baud The baud rate to use
152 |   /// @param[optional] use_legacy_protocol Whether to use legacy 1.0 protocol (default false)
153 |   /// @param[optional] use_group_read Whether to use bulk protocol methods with bulk getters (default true)
154 |   /// @param[optional] use_group_read Whether to use bulk protocol methods with bulk setters (default true)
155 |   DynamixelInterfaceDriver(const std::string &device = "/dev/ttyUSB0", int baud = 1000000,
156 |                            bool use_legacy_protocol = false, bool use_group_read = true, bool use_group_write = true);
157 | 
158 |   /// Destructor. Closes and releases serial port.
159 |   ~DynamixelInterfaceDriver();
160 | 
161 |   /// Parses Motor Data, Opens port and sets up driver
162 |   /// @returns true if initialisation successful, false otherwise
163 |   bool initialise(void);
164 | 
165 |   /// Ping the specified id, used to check if a dynamixel with that ID is on the bus
166 |   /// @param[in] servo_id The ID to ping on the bus.
167 |   /// @return True if a dynamixel responds, false otherwise.
168 |   bool ping(int servo_id) const;
169 | 
170 |   /// Get pointer to model spec data for given model number
171 |   inline const DynamixelSpec *getModelSpec(uint model_number) const
172 |   {
173 |     auto it = model_specs_.find(model_number);
174 |     if (it != model_specs_.end())
175 |     {
176 |       return &(it->second);
177 |     }
178 |     else
179 |     {
180 |       return nullptr;
181 |     }
182 |   };
183 | 
184 |   // ************************************ GETTERS ***************************************** //
185 | 
186 |   /// Retrieves the model number from the dynamixel's eeprom
187 |   /// @param[in] servo_id The ID of the servo to retrieve from
188 |   /// @param[out] model_number Stores the model_number returned
189 |   /// @return True on comm success, false otherwise.
190 |   bool getModelNumber(int servo_id, uint16_t *model_number) const;
191 | 
192 |   /// Retrieves the hardware status error value from the dynamixel's eeprom
193 |   /// @param[in] servo_id The ID of the servo to retrieve from
194 |   /// @param[in] type the type of the servo to read from
195 |   /// @param[out] error Stores the returned error code
196 |   /// @return True on comm success, false otherwise.
197 |   bool getErrorStatus(int servo_id, DynamixelSeriesType type, uint8_t *error) const;
198 | 
199 |   /// Retrieves the maximum torque limit from the dynamixel's eeprom.
200 |   /// @param[in] servo_id The ID of the servo to retrieve from
201 |   /// @param[in] type the type of the servo to read from
202 |   /// @param[out] max_torque Stores the value returned
203 |   /// @return True on comm success, false otherwise.
204 |   bool getMaxTorque(int servo_id, DynamixelSeriesType type, uint16_t *max_torque) const;
205 | 
206 |   /// Retrieves the torque enabled value from the dynamixel's ram.
207 |   /// @param[in] servo_id The ID of the servo to retrieve from
208 |   /// @param[in] type the type of the servo to read from
209 |   /// @param[out] torque_enabled Stores the status of torque enable
210 |   /// @return True on comm success, false otherwise.
211 |   bool getTorqueEnabled(int servo_id, DynamixelSeriesType type, bool *torque_enabled) const;
212 | 
213 |   /// Retrieves the current target_velocity from the dynamixel's ram.
214 |   /// @param[in] servo_id The ID of the servo to retrieve from
215 |   /// @param[in] type the type of the servo to read from
216 |   /// @param[out] target_velocity Stores the value returned
217 |   /// @return True on comm success, false otherwise.
218 |   bool getTargetTorque(int servo_id, DynamixelSeriesType type, int16_t *target_torque) const;
219 | 
220 |   /// Retrieves arbitrary register readings from the Dynamixel, can be used in cases where provided getters are
221 |   /// insufficient or to read multiple contiguous values at once.
222 |   /// @param[in] servo_id The ID of the servo to retrieve from
223 |   /// @param[in] address The address value in the control table the dynamixel will start reading from
224 |   /// @param[in] length The number of bytes to read consecutively from the control table
225 |   /// @param[out] response Array to store the raw dynamixel response.
226 |   bool readRegisters(int servo_id, uint16_t address, uint16_t length, std::vector<uint8_t> *response) const;
227 | 
228 |   // *********************** BULK_READ METHODS *************************** //
229 | 
230 |   /// Bulk Reads the Present Position, Present Velocity and Present Current in one instruction. If the group read fails
231 |   /// the function will fall back on reading each motor individually. Optionally, the group comms can be disabled on
232 |   /// initialisation of the driver (by setting use_group_read to false) in which case the function will always read
233 |   /// from each dynamixel individually.
234 |   /// @param[in] state_map map of servo ids to state data to read into
235 |   /// @return True on comm success, false otherwise
236 |   bool getBulkState(std::unordered_map<int, DynamixelState> &state_map) const;
237 | 
238 |   /// Bulk Reads the voltage and temperature in one instruction, behaves the same as getBulkState()
239 |   /// @param[in] data_map map of servo ids to dataport objects to read into
240 |   /// @return True on comm success, false otherwise
241 |   bool getBulkDataportInfo(std::unordered_map<int, DynamixelDataport> &data_map) const;
242 | 
243 |   /// Bulk Reads the voltage and temperature in one instruction, behaves the same as getBulkState()
244 |   /// @param[in] diag_map map of servo ids to diagnostics objects to read into
245 |   /// @return True on comm success, false otherwise
246 |   bool getBulkDiagnosticInfo(std::unordered_map<int, DynamixelDiagnostic> &diag_map) const;
247 | 
248 |   // **************************** SETTERS ******************************** //
249 | 
250 |   /// Sets the Operating mode of the Dynamixel. The three possible control modes are: Position, Velocity or Torque.
251 |   /// The way these modes are enabled differs for each series of dynamixel. XM and Pro have an operating mode register
252 |   /// that can be used to changed the control type. For AX, RX and MX series, the method of control is defined by the
253 |   /// values in the angle limit registers and the torque_control_enable register. Note torque control is not available
254 |   /// on the MX-28 models.
255 |   /// @param[in] servo_id The ID of the servo to write to
256 |   /// @param[in] type the type of the servo to read from
257 |   /// @param[in] operating_mode The method of control
258 |   /// @return True on comm success and valid operating mode, false otherwise.
259 |   bool setOperatingMode(int servo_id, DynamixelSeriesType type, DynamixelControlMode operating_mode) const;
260 | 
261 |   /// Sets the minimum and maximum angle limits for the dynamixel
262 |   /// @param[in] servo_id The ID of the servo to write to
263 |   /// @param[in] type the type of the servo to read from
264 |   /// @param[in] min_angle the minimum angle limit (in encoder values)
265 |   /// @param[in] max_angle the maximum angle limit (in encoder values)
266 |   /// @return True on comm success, false otherwise.
267 |   bool setAngleLimits(int servo_id, DynamixelSeriesType type, int32_t min_angle, int32_t max_angle) const;
268 | 
269 |   /// Sets the minimum angle limit for the dynamixel
270 |   /// @param[in] servo_id The ID of the servo to write to
271 |   /// @param[in] type the type of the servo to read from
272 |   /// @param[in] angle The minimum angle limit (in encoder values)
273 |   /// @return True on comm success, false otherwise.
274 |   bool setMinAngleLimit(int servo_id, DynamixelSeriesType type, int32_t angle) const;
275 | 
276 |   /// Sets the maximum angle limit for the dynamixel
277 |   /// @param[in] servo_id The ID of the servo to write to
278 |   /// @param[in] type the type of the servo to read from
279 |   /// @param[in] angle The maximum angle limit (in encoder values)
280 |   /// @return True on comm success, false otherwise.
281 |   bool setMaxAngleLimit(int servo_id, DynamixelSeriesType type, int32_t angle) const;
282 | 
283 |   /// Sets the maximum torque limit for the dynamixel
284 |   /// @param[in] servo_id The ID of the servo to write to
285 |   /// @param[in] type the type of the servo to read from
286 |   /// @param[in] max_torque the maximum torque limit
287 |   /// @return True on comm success, false otherwise.
288 |   bool setMaxTorque(int servo_id, DynamixelSeriesType type, uint16_t max_torque) const;
289 | 
290 |   /// Sets the maximum velocity limit for the dynamixel
291 |   /// @param[in] servo_id The ID of the servo to write to
292 |   /// @param[in] type the type of the servo to read from
293 |   /// @param[in] max_vel the maximum velocity limit
294 |   /// @return True on comm success, false otherwise.
295 |   bool setMaxVelocity(int servo_id, DynamixelSeriesType type, uint32_t max_vel) const;
296 | 
297 |   /// Sets the torque enable register of the dynamixel. This value defines the on/off state of the servo.
298 |   /// @param[in] servo_id The ID of the servo to write to
299 |   /// @param[in] type the type of the servo to read from
300 |   /// @param[in] on The state of the servo (true = on, false = off).
301 |   /// @return True on comm success, false otherwise.
302 |   bool setTorqueEnabled(int servo_id, DynamixelSeriesType type, bool on) const;
303 | 
304 |   /// Sets the torque control enable register of the dynamixel mx series. can be used to dynamically
305 |   /// switch between position and torque control modes.
306 |   /// @param[in] servo_id The ID of the servo to write to
307 |   /// @param[in] type the type of the servo to read from
308 |   /// @param[in] on The torque control state of the servo (true = on, false = off).
309 |   /// @return True on comm success, false otherwise.
310 |   bool setTorqueControlEnabled(int servo_id, DynamixelSeriesType type, bool on) const;
311 | 
312 |   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
313 |   /// The following functions set the PID gains for the dynamixel. note that the PID gains are available based on
314 |   /// series and control type:
315 |   ///  - AX Series:
316 |   ///      - None
317 |   ///  - RX Series:
318 |   ///      - None
319 |   ///  - MX (1.0) Series:
320 |   ///      - Position: P,I,D
321 |   ///      - Velocity: None
322 |   ///      - Torque:   None
323 |   ///  - XM Series:
324 |   ///      - Position: P,I,D
325 |   ///      - Velocity: P,I
326 |   ///      - Torque:   None
327 |   ///  - Pro Series:
328 |   ///      - Position: P
329 |   ///      - Velocity: P,I
330 |   ///      - Torque:   None
331 |   ///  -
332 |   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333 | 
334 |   /// Sets the position PID values for the dynamixels
335 |   /// @param[in] servo_id The ID of the servo to write to
336 |   /// @param[in] type the type of the servo to read from
337 |   /// @param[in] p_gain The proportional gain value to write
338 |   /// @param[in] i_gain The integral gain value to write
339 |   /// @param[in] d_gain The derivative gain value to write
340 |   /// @return True on comm success, false otherwise.
341 |   bool setPositionPIDGains(int servo_id, DynamixelSeriesType type, double p_gain, double i_gain, double d_gain) const;
342 | 
343 |   /// Sets the proportional gain value for the position control mode if available. @see setPIDGains
344 |   /// @param[in] servo_id The ID of the servo to write to
345 |   /// @param[in] type the type of the servo to read from
346 |   /// @param[in] gain The proportional gain value to write
347 |   /// @return True on comm success, false otherwise.
348 |   bool setPositionProportionalGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;
349 | 
350 |   /// Sets the integral gain value for the position control mode if available. @see setPIDGains
351 |   /// @param[in] servo_id The ID of the servo to write to
352 |   /// @param[in] type the type of the servo to read from
353 |   /// @param[in] gain The integral gain value to write
354 |   /// @return True on comm success, false otherwise.
355 |   bool setPositionIntegralGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;
356 | 
357 |   /// Sets the derivative gain value for the position control mode if available. @see setPIDGains
358 |   /// @param[in] servo_id The ID of the servo to write to
359 |   /// @param[in] type the type of the servo to read from
360 |   /// @param[in] gain The derivative gain value to write
361 |   /// @return True on comm success, false otherwise.
362 |   bool setPositionDerivativeGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;
363 | 
364 |   /// Sets the velocity PID values for the dynamixels
365 |   /// @param[in] servo_id The ID of the servo to write to
366 |   /// @param[in] type the type of the servo to read from
367 |   /// @param[in] p_gain The proportional gain value to write
368 |   /// @param[in] i_gain The integral gain value to write
369 |   /// @param[in] d_gain The derivative gain value to write
370 |   /// @return True on comm success, false otherwise.
371 |   bool setVelocityPIDGains(int servo_id, DynamixelSeriesType type, double p_gain, double i_gain) const;
372 | 
373 |   /// Sets the proportional gain value for the velocity control mode if available. @see setPIDGains
374 |   /// @param[in] servo_id The ID of the servo to write to
375 |   /// @param[in] type the type of the servo to read from
376 |   /// @param[in] gain The proportional gain value to write
377 |   /// @return True on comm success, false otherwise.
378 |   bool setVelocityProportionalGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;
379 | 
380 |   /// Sets the integral gain value for the velocity control mode if available. @see setPIDGains
381 |   /// @param[in] servo_id The ID of the servo to write to
382 |   /// @param[in] type the type of the servo to read from
383 |   /// @param[in] gain The integral gain value to write
384 |   /// @return True on comm success, false otherwise.
385 |   bool setVelocityIntegralGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;
386 | 
387 |   /// Sets the profile velocity of the dynamixel. Profile velocity is how fast the servo should move between positions.
388 |   /// @param[in] servo_id The ID of the servo to write to
389 |   /// @param[in] type the type of the servo to read from
390 |   /// @param[in] velocity The profile velocity value to write
391 |   /// @return True on comm success, false otherwise.
392 |   bool setProfileVelocity(int servo_id, DynamixelSeriesType type, int32_t velocity) const;
393 | 
394 |   /// Sets the torque value of the dynamixel.
395 |   /// @param[in] servo_id The ID of the servo to write to
396 |   /// @param[in] type the type of the servo to read from
397 |   /// @param[in] torque The torque value to write
398 |   /// @return True on comm success, false otherwise.
399 |   bool setTorque(int servo_id, DynamixelSeriesType type, int16_t torque) const;
400 | 
401 |   /// Writes arbitrary register values to the Dynamixel, can be used in cases where provided setters are insufficient
402 |   /// or to write multiple contiguous values at once.
403 |   /// @param[in] servo_id The ID of the servo to write to
404 |   /// @param[in] address The address value in the control table the dynamixel will start writing to
405 |   /// @param[in] length The number of bytes to write consecutively to the control table
406 |   /// @param[in] data Array containing the value to be written.
407 |   /// @return True on comm success, false otherwise.
408 |   bool writeRegisters(int servo_id, uint16_t address, uint16_t length, uint8_t *data) const;
409 | 
410 |   // *********************** SYNC_WRITE METHODS *************************** //
411 | 
412 |   /// Set many dynamixels with new position values in one instruction. @see syncWrite.
413 |   /// @param[in] position_data map of ids to syncdata objects containing position data
414 |   /// @return True on comm success, false otherwise.
415 |   bool setMultiPosition(std::unordered_map<int, SyncData> &position_data) const;
416 | 
417 |   /// Set many dynamixels with new position values in one instruction. @see syncWrite.
418 |   /// @param[in] velocity_data map of ids to syncdata objects containing velocity data
419 |   /// @return True on comm success, false otherwise.
420 |   bool setMultiVelocity(std::unordered_map<int, SyncData> &velocity_data) const;
421 | 
422 |   /// Set many dynamixels with new profile velocity values in one instruction. @see syncWrite.
423 |   /// @param[in] velocity_data map of ids to syncdata objects containing profile velocity data
424 |   /// @return True on comm success, false otherwise.
425 |   bool setMultiProfileVelocity(std::unordered_map<int, SyncData> &velocity_data) const;
426 | 
427 |   /// Set many dynamixels with new torque values in one instruction. @see syncWrite.
428 |   /// @param[in] torque_data map of ids to syncdata objects containing torque data
429 |   /// @return True on comm success, false otherwise.
430 |   bool setMultiTorque(std::unordered_map<int, SyncData> &torque_data) const;
431 | 
432 |   /// Returns a string version of the dynamixel series based on the input type
433 |   /// @param[in] type the type of the servo to read from
434 |   /// @returns The string name of the Dynamixel series
435 |   inline std::string getSeriesName(DynamixelSeriesType type) const
436 |   {
437 |     std::string series_name = "undefined";
438 |     switch (type)
439 |     {
440 |       case kSeriesAX:
441 |         series_name = "AX";
442 |         break;
443 |       case kSeriesRX:
444 |         series_name = "RX";
445 |         break;
446 |       case kSeriesDX:
447 |         series_name = "DX";
448 |         break;
449 |       case kSeriesEX:
450 |         series_name = "EX";
451 |         break;
452 |       case kSeriesLegacyMX:
453 |         series_name = "Legacy MX";
454 |         break;
455 |       case kSeriesMX:
456 |         series_name = "MX";
457 |         break;
458 |       case kSeriesX:
459 |         series_name = "X";
460 |         break;
461 |       case kSeriesP:
462 |         series_name = "P";
463 |         break;
464 |       case kSeriesLegacyPro:
465 |         series_name = "Legacy Pro";
466 |         break;
467 |     }
468 |     return series_name;
469 |   };
470 | 
471 | private:
472 |   /// Loads in the motor specifications from the motor_data.yaml file
473 |   /// @returns true if load successful, false otherwise
474 |   bool loadMotorData(void);
475 | 
476 |   /// Performs the bulk read for each protocol. A bulk read is a broadcast instruction on a bus that commands a list
477 |   /// of dynamixels to respond in order with a read of a specified address and length (which can be different for each
478 |   /// dynamixel). This protocol can be used to read many parameters from many dynamixels on a bus in just one
479 |   /// instruction.
480 |   /// @param[in] read_data Pointer to a map of SyncData objects, containing ids and vectors to read into
481 |   /// @param[in] address The address value in the control table the dynamixels will start reading from
482 |   /// @param[in] length The number of bytes to read consecutively from the control table
483 |   /// @returns true if at least one dynamixel was successfully read
484 |   bool bulkRead(std::unordered_map<int, SyncData*> &read_data, uint16_t address, uint16_t length) const;
485 | 
486 |   /// Performs the sync read for each protocol. A sync read is a broadcast instruction on a bus that commands a list
487 |   /// of dynamixels to respond in order with a read of a specified address and length. This protocol can be used to read
488 |   /// many parameters from many dynamixels on a bus in just one instruction.
489 |   /// @param[in] read_data Pointer to a map of SyncData objects, containing ids and vectors to read into
490 |   /// @param[in] address The address value in the control table the dynamixels will start reading from
491 |   /// @param[in] length The number of bytes to read consecutively from the control table
492 |   /// @returns true if at least one dynamixel was successfully read
493 |   bool syncRead(std::unordered_map<int, SyncData*> &read_data, uint16_t address, uint16_t length) const;
494 | 
495 |   /// Performs the sync write for each protocol. A sync write is a broadcast instruction on a bus that commands a list
496 |   /// of dynamixels to write a value into a specified address (the value written can be different for each dynamixel
497 |   /// but the address is universal). This can be used to update a parameter (say goal position) for many dynamixels,
498 |   /// each with a unique value, all in one instruction.
499 |   /// @param[in] write_data Pointer to a map of SyncData objects, containing ids and vectors to write from.
500 |   /// @param[in] address The address value in the control table the dynamixels will write to
501 |   /// @param[in] length The number of bytes to write
502 |   /// @returns true on successful write, false otherwise
503 |   bool syncWrite(std::unordered_map<int, SyncData> &write_data, uint16_t address, uint16_t length) const;
504 | 
505 | private:
506 |   std::unique_ptr<dynamixel::PortHandler> portHandler_;  ///< The port handler object. The dynamixel sdk serial object.
507 |   std::unique_ptr<dynamixel::PacketHandler> packetHandler_;  ///< packet handler. Provides raw response deconstruction.
508 | 
509 |   std::string device_;  ///< name of device to open
510 |   int baud_rate_;       ///< baud rate
511 | 
512 |   bool use_legacy_protocol_;  ///< if we are using legacy 1.0 protocol or newer 2.0 protocol (depends on model support)
513 |   bool use_group_read_;       ///< using monolothic bulkRead or syncRead for bulk data exchange
514 |   bool use_group_write_;      ///< using monolothic syncWrite for bulk data exchange
515 | 
516 |   bool initialised_ = false;              ///< Variable to indicate if we're ready for comms
517 |   uint8_t single_read_fallback_counter_;  ///< indicates group comm failure fallback interval
518 | 
519 |   std::unordered_map<uint16_t, const DynamixelSpec> model_specs_;  ///< map of model numbers to motor specifications
520 |   std::unordered_map<int, std::vector<uint8_t>> raw_read_map_;     ///< map to store raw reads into
521 | };
522 | 
523 | }  // namespace dynamixel_interface
524 | 
525 | #endif  // DYNAMIXEL_INTERFACE_DRIVER_H_
526 | 


--------------------------------------------------------------------------------
/launch/dynamixel_interface_controller.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 | 	<node name="dynamixel_interface_controller_node" pkg="dynamixel_interface" type="dynamixel_interface_controller_node" output="screen">
3 | 		<rosparam command="load" file="$(find dynamixel_interface)/config/controller_config.yaml" />
4 | 	</node>
5 | </launch>
6 | 


--------------------------------------------------------------------------------
/msg/DataPort.msg:
--------------------------------------------------------------------------------
1 | string name #name of joint
2 | uint16[] port_values #array of port GPIO values [0,1] for digital or [0,4095] for analog


--------------------------------------------------------------------------------
/msg/DataPorts.msg:
--------------------------------------------------------------------------------
1 | Header header
2 | DataPort[] states #array of Dataport messages for all dynamixels on bus


--------------------------------------------------------------------------------
/msg/ServoDiag.msg:
--------------------------------------------------------------------------------
1 | string name #name of joint
2 | uint8 id  #id on bus
3 | string model_name #model name
4 | uint8 error_code # error code
5 | float64 voltage #current voltage
6 | float64 temperature #current temperature


--------------------------------------------------------------------------------
/msg/ServoDiags.msg:
--------------------------------------------------------------------------------
1 | Header header
2 | ServoDiag[] diagnostics #array of diagnostics messages for all dynamixels on bus


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>dynamixel_interface</name>
 4 |   <version>0.1.0</version>
 5 |   <description>A controller for commanding the dynamixel series of servos. Provides abstraction of the synchronous reading and writing of many dynamixels across multiple serial ports to two ros topics. The controller can read and publish synchronously from multiple serial ports through different instances of dynamixel_interface_driver. The controller can run in either position, velocity or torque control modes and is easily configured with an external yaml file.</description>
 6 | 
 7 |   <maintainer email="Thomas.Molnar@data61.csiro.au">Tom Molnar</maintainer>
 8 | 
 9 |   <license>CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)</license>
10 | 
11 |   <!-- This package is based on the dynamixel_pro_controller v0.8.4 package written by Brian Axelrod -->
12 |   <author>Tom Molnar</author>
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 |   <depend>message_generation</depend>
16 |   <depend>message_runtime</depend>
17 |   <depend>std_msgs</depend>
18 |   <depend>sensor_msgs</depend>
19 |   <depend>dynamixel_sdk</depend>
20 |   <depend>roscpp</depend>
21 |   <depend>roslib</depend>
22 |   <depend>yaml-cpp</depend>
23 |   <depend>xmlrpcpp</depend>
24 |   <depend>setserial</depend>
25 | 
26 |   <export>
27 |   </export>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/readme.md:
--------------------------------------------------------------------------------
  1 | # Dynamixel Interface: A Fast, Scalable Dynamixel Driver
  2 | 
  3 | 
  4 | [![Version](https://img.shields.io/badge/Current%20version-1.0.0-orange "Version")](https://github.com/csiro-robotics/dynamixel_interface) [![License](
  5 | https://img.shields.io/badge/License-BSD%2FMIT-blue "License")](https://github.com/csiro-robotics/dynamixel_interface/blob/master/LICENSE)
  6 | 
  7 | <p align="center">
  8 | <a href="https://research.csiro.au/robotics/our-work/research-areas/legged-robots/"><img alt="Gizmo Wizmo Zero" align="center" width="500" src="https://i.imgur.com/HCrmRDS.gif"/></a>
  9 | </p>
 10 | 
 11 | This package aims to provide a scalable and easily configurable interface for controlling many dynamixel's across multiple serial devices.
 12 | 
 13 | Features:
 14 | 
 15 | - Fast C++ implementation capable of controlling 10's of dynamixels at very high (>100Hz) rates.
 16 | - Easily configure all dynamixels in a single yaml file
 17 | - Synchronous across multiple serial ports (using threaded IO)
 18 | - All dynamixel states/commands are exchanged via a single sensor_msgs/JointState message
 19 | - Supports position, velocity and current* control modes.
 20 |   - In position control mode, can supply profile velocities as well
 21 | 
 22 | *current control not available on all models.
 23 | 
 24 | Currently the following series of motors are supported:
 25 | 
 26 | - AX
 27 | - RX
 28 | - DX
 29 | - EX
 30 | - MX
 31 | - XM
 32 | - PRO
 33 | - PRO+
 34 | - P
 35 | 
 36 | ## INSTALLATION
 37 | 
 38 | ### Requirements
 39 | 
 40 | * Ubuntu 18.04 LTS and ROS Melodic; or
 41 | * Ubuntu 20.04 LTS and ROS Noetic
 42 | 
 43 | ### Dependencies
 44 | 
 45 | * [dynamixel_sdk](http://wiki.ros.org/dynamixel_sdk)
 46 | 
 47 | ### Building from source
 48 | 
 49 | ```bash
 50 | mkdir -p catkin_ws/src
 51 | cd catkin_ws/src
 52 | git clone https://github.com/csiro-robotics/dynamixel_interface.git
 53 | cd ..
 54 | rosdep install --from-paths src --ignore-src -r -y
 55 | catkin build
 56 | ```
 57 | 
 58 | ## USAGE NOTES
 59 | 
 60 | For detailed instructions, consult the tutorials in the subdirectory of this package:
 61 | 
 62 | - [Tutorial 1: Using the controller](tutorials/tutorial_1_using_the_controller.md)
 63 | - [Tutorial 2: Multi Port Communications](tutorials/tutorial_2_multi_port_communication.md)
 64 | 
 65 | ### Serial Access
 66 | 
 67 | For serial communications, give the user access to the dialout group
 68 | 
 69 | ```bash
 70 | 
 71 | sudo usermod -a -G dialout $USER
 72 | 
 73 | ```
 74 | 
 75 | ### Operation
 76 | 
 77 | To use the dynamixel_interface_controller to operate servos:
 78 | 
 79 | - Create a controller_config.yaml with the necessary parameters for each servo (see the tutorials as well as the example in the config/ folder for an example and explanation of the various configuration options)
 80 | - Launch the dynamixel_interface_controller_node, loading in the custom config file (see the example in the launch/ folder)
 81 | 
 82 | During operation:
 83 | 
 84 | - The controller communicates using the sensor_msgs::JointState message.
 85 | - Current joint states are published to /joint_states
 86 | - Commands should be sent to /desired_joint_states
 87 | - For each control mode
 88 |   - Position Control: only position and velocity are considered in the command message, velocity controls the moving speed to the goal position of the dynamixel. If no velocity is specified the default is used
 89 |   - Velocity Control: only the velocity value is considered
 90 |   - Torque Control: only the effort value is considered
 91 | 
 92 |   - Note that all non-empty arrays must have the same length as the array of joint_names, otherwise the command is invalid
 93 | 
 94 | A note on units:
 95 | 
 96 |   - Positions are in radians
 97 |   - Velocities are in radians/second
 98 |   - Effort is in mA of current
 99 | 
100 | ## IMPROVING DRIVER PERFORMANCE
101 | 
102 | This driver is designed with goal of achieveing the maximum possible loop rate for a given configuration of dynamixels. To that end, there are three key optimisations that we recommend :
103 | 
104 | ### Baud rate
105 | 
106 | The baud rate used to communicate on the bus directly affects the maximum achieveable loop rate on the bus. A higher loop rate will result in faster update rates but can reduce the noise the bus can tolerate, for most applications the recommended baud rate is the maximum of 3Mbps.
107 | 
108 | ### Return delay time
109 | 
110 | The return delay time is a parameter on the dynamixels that sets the amount of delay in microseconds before they respond to a packet, by default this value is quite large (250, which equates to 500 microseconds). Lowering this value will reduce the total round-trip time for any comms on the bus. It is recommended to set this value very low (tested at 5, or 10 microseconds).
111 | 
112 | Both the above values will need to be configured PRIOR TO OPERATION using the dynamixel wizard available from robotis.
113 | 
114 | ### FTDI DRIVER LATENCY FOR SERIAL COMMUINICATIONS
115 | 
116 | Since 16.04, the default latency_timer value for the ftdi usb driver was changed from 1ms to 16ms. This value represents how long the kernel driver waits before passing data up to the user application. It significantly affects the speed the driver will be able to communicate with the dynamixels, resulting in very low achievable loop rates. To correct this, a script is included that can change a serial port into 'low latency' mode (a latency_timer value of 1), which requires the installation of the setserial command:
117 | 
118 | ```bash
119 | 
120 | sudo apt install setserial
121 | 
122 | ```
123 | 
124 | then, run the script (no roscore required) with:
125 | 
126 | ```bash
127 | 
128 | rosrun dynamixel_interface set_lowlatency_mode.sh <port>
129 | 
130 | ```
131 | 
132 | If setserial is not available as a command on your system, the value for the device can be set directly in the kernel:
133 | 
134 | ``` bash
135 | 
136 | echo "1" | sudo tee /sys/bus/usb-serial/devices/<device>/latency_timer
137 | 
138 | ```
139 | 
140 | ## Authors
141 | - Thomas Molnar
142 | - Ryan Steindl
143 | - Marisa Bucolo
144 | - Benjamin Tam
145 | - Jacob Oestreich
146 | 
147 | ## License
148 | This project is licensed under the CSIRO Open Source Software Licence Agreement (variation of the BSD / MIT License) - see the LICENSE file for details.
149 | 
150 | ## Contributing
151 | Thank you for being interested in contributing. We encourage the community to create new features and to fix bugs. If it is a new feature, please create a new issue via the Issue Tracker and describe the intended feature so that we can discuss the implementation. Else, search for the existing issue and comment on your proposed work. Please fork the repo, work on your changes and then send a pull request.
152 | 
153 | ## Issues
154 | Please report bugs using Issue Tracker or contact us via email shc-support@csiro.au.
155 | 


--------------------------------------------------------------------------------
/scripts/set_lowlatency_mode.sh:
--------------------------------------------------------------------------------
 1 | #!/bin/bash
 2 | #Since 16.04, the default latency_timer value for the ftdi usb driver was changed from 1ms to 16ms. This value
 3 | #represents how long the kernel driver waits before passing data up to the user application. It significantly affects
 4 | #the speed the driver will be able to communicate with the dynamixels, resulting in very low achievable loop rates.
 5 | #To correct this, this script will change a serial port into 'low latency' mode (a latency_timer value of 1).
 6 | if [ $# -lt 1 ]; then
 7 | 	echo "Usage ./set_lowlatency_mod <port>"
 8 | 	exit
 9 | fi
10 | 
11 | port=$1
12 | 
13 | setserial $port low_latency


--------------------------------------------------------------------------------
/src/dynamixel_interface_controller.cpp:
--------------------------------------------------------------------------------
   1 | /* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
   2 |  * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
   3 |  * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
   4 |  * terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
   5 |  * and binary forms, with or without modification, are permitted provided that the following conditions are met:
   6 |  * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
   7 |  *   disclaimer.
   8 |  * - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
   9 |  *   disclaimer in the documentation and/or other materials provided with the distribution.
  10 |  * - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
  11 |  *   this software without specific prior written permission of CSIRO.
  12 |  *
  13 |  * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
  14 |  * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
  15 |  * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
  16 |  * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
  17 |  * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
  18 |  * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
  19 |  * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
  20 |  * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
  21 |  * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
  22 |  * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
  23 |  * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
  24 |  * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
  25 |  * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
  26 |  * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
  27 |  * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
  28 |  * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
  29 |  * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
  30 |  * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
  31 |  * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
  32 |  * (b)  THE REPAIR OF THE SOFTWARE;
  33 |  * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
  34 |  *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
  35 |  * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
  36 |  * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
  37 |  *
  38 |  * Third Party Components:
  39 |  *
  40 |  * The following third party components are distributed with the Software.  You agree to comply with the license terms
  41 |  * for these components as part of accessing the Software.  Other third party software may also be identified in
  42 |  * separate files distributed with the Software.
  43 |  * ___________________________________________________________________
  44 |  *
  45 |  * dynamixel_interface is forked from projects authored by Brian
  46 |  * Axelrod (on behalf of Willow Garage):
  47 |  *
  48 |  * https://github.com/baxelrod/dynamixel_pro_controller
  49 |  * https://github.com/baxelrod/dynamixel_pro_driver
  50 |  *
  51 |  * Thus they retain the following notice:
  52 |  *
  53 |  * Software License Agreement (BSD License)
  54 |  * Copyright (c) 2013, Willow Garage
  55 |  * All rights reserved.
  56 |  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
  57 |  * following conditions are met:
  58 |  *  - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
  59 |  *    disclaimer.
  60 |  *  - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
  61 |  *    following disclaimer in the documentation and/or other materials provided with the distribution.
  62 |  *  - Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products
  63 |  *    derived from this software without specific prior written permission.
  64 |  *
  65 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
  66 |  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  67 |  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  68 |  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  69 |  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  70 |  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  71 |  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  72 |  * ___________________________________________________________________
  73 |  */
  74 | 
  75 | 
  76 | /**
  77 |  * @file   dynamixel_interface_controller.cpp
  78 |  * @author Tom Molnar (Tom.Molnar@data61.csiro.au), Brian Axelrod
  79 |  * @date   January, 2017
  80 |  * @brief  Implements the dynamixel controller class and defines the main loop of operation
  81 |  */
  82 | 
  83 | #include <stdlib.h>
  84 | #include <chrono>
  85 | #include <fstream>
  86 | #include <mutex>
  87 | #include <string>
  88 | #include <thread>
  89 | 
  90 | #define _USE_MATH_DEFINES
  91 | #include <math.h>
  92 | 
  93 | #include <dynamixel_interface/dynamixel_interface_controller.h>
  94 | #include <ros/callback_queue.h>
  95 | #include <ros/package.h>
  96 | #include <ros/spinner.h>
  97 | #include <ros/transport_hints.h>
  98 | 
  99 | 
 100 | namespace dynamixel_interface
 101 | {
 102 | /// Constructor, loads the motor configuration information from the specified yaml file and intialises
 103 | /// The motor states.
 104 | DynamixelInterfaceController::DynamixelInterfaceController()
 105 | {
 106 |   nh_ = std::make_unique<ros::NodeHandle>();
 107 | }
 108 | 
 109 | 
 110 | /// Destructor, deletes the objects holding the serial ports and disables the motors if required
 111 | DynamixelInterfaceController::~DynamixelInterfaceController()
 112 | {
 113 |   ROS_INFO("shutting_down dynamixel_interface_controller");
 114 | 
 115 |   if (stop_motors_on_shutdown_)
 116 |   {
 117 |     for (int i = 0; i < dynamixel_ports_.size(); i++)
 118 |     {
 119 |       // Turn off all the motors
 120 |       for (auto &it : dynamixel_ports_[i].joints)
 121 |       {
 122 |         dynamixel_ports_[i].driver->setTorqueEnabled(it.second.id, it.second.model_spec->type, 0);
 123 |         ROS_INFO("Torque disabled on %s joint\n", it.first.c_str());
 124 |       }
 125 |     }
 126 |   }
 127 | }
 128 | 
 129 | /// Parses param information from the rosparam server
 130 | /// @returns true if all params parsed successfully, false otherwise
 131 | bool DynamixelInterfaceController::parseParameters(void)
 132 | {
 133 |   /// don't parse parameters during operation
 134 |   if (parameters_parsed_)
 135 |   {
 136 |     ROS_ERROR("Parameters already parsed");
 137 |     return false;
 138 |   }
 139 | 
 140 |   // Stores config variables only used in init function
 141 |   std::string mode;
 142 |   std::string node_name = ros::this_node::getName();
 143 | 
 144 |   // load all the info from the param server, with defaults
 145 |   ros::param::param<double>("~loop_rate", loop_rate_, 50.0);
 146 |   ros::param::param<bool>("~disable_torque_on_shutdown", stop_motors_on_shutdown_, false);
 147 | 
 148 |   ros::param::param<bool>("~ignore_input_velocity", ignore_input_velocity_, false);
 149 | 
 150 |   ros::param::param<double>("~dataport_rate", dataport_rate_, 0.0);
 151 |   ros::param::param<double>("~diagnostics_rate", diagnostics_rate_, 0.0);
 152 |   ros::param::param<int>("~recv_queue_size", recv_queue_size_, 1);
 153 | 
 154 |   ros::param::param<std::string>("~control_mode", mode, "Position");
 155 | 
 156 |   ros::param::param<double>("~global_max_vel", global_max_vel_, 5.0);
 157 |   ros::param::param<double>("~global_torque_limit", global_torque_limit_, 1.0);
 158 | 
 159 | 
 160 |   // clamp read rates to sensible values
 161 |   if (loop_rate_ <= 0)
 162 |   {
 163 |     ROS_ERROR("Loop rate must be > 0!");
 164 |     return false;
 165 |   }
 166 | 
 167 |   if (recv_queue_size_ <= 0)
 168 |   {
 169 |     ROS_ERROR("recv queue size must be >= 1");
 170 |     return false;
 171 |   }
 172 | 
 173 |   if (diagnostics_rate_ < 0)
 174 |   {
 175 |     diagnostics_rate_ = 0;
 176 |   }
 177 |   else if (diagnostics_rate_ > loop_rate_)
 178 |   {
 179 |     ROS_WARN("Clamping diagnostics rate to loop rate.");
 180 |     diagnostics_rate_ = loop_rate_;
 181 |   }
 182 |   else
 183 |   {
 184 |     diagnostics_iters_ = std::round(loop_rate_ / diagnostics_rate_);
 185 |     diagnostics_counter_ = 0;
 186 |   }
 187 | 
 188 |   // num iters to read from
 189 |   if (dataport_rate_ < 0)
 190 |   {
 191 |     dataport_rate_ = 0;
 192 |   }
 193 |   else if (dataport_rate_ > loop_rate_)
 194 |   {
 195 |     ROS_WARN("Clamping dataport rate to loop rate.");
 196 |     dataport_rate_ = loop_rate_;
 197 |   }
 198 |   else
 199 |   {
 200 |     dataport_iters_ = std::round(loop_rate_ / dataport_rate_);
 201 |     dataport_counter_ = 0;
 202 |   }
 203 | 
 204 | 
 205 |   // Set control mode for this run
 206 |   if (mode == "position")
 207 |   {
 208 |     control_type_ = kModePositionControl;
 209 |   }
 210 |   else if (mode == "velocity")
 211 |   {
 212 |     control_type_ = kModeVelocityControl;
 213 |   }
 214 |   else if (mode == "effort")
 215 |   {
 216 |     control_type_ = kModeTorqueControl;
 217 |   }
 218 |   else
 219 |   {
 220 |     ROS_ERROR("Control Mode Not Supported!");
 221 |     return false;
 222 |   }
 223 | 
 224 |   // Attempt to parse information for each device (port)
 225 |   if (ros::param::has("~ports"))
 226 |   {
 227 |     // PARSE ARRAY OF PORT INFORMATION
 228 |     XmlRpc::XmlRpcValue ports;
 229 |     ros::param::get("~ports", ports);
 230 | 
 231 |     if (!parsePortInformation(ports))
 232 |     {
 233 |       ROS_ERROR("Unable to parse ports from config!");
 234 |       return false;
 235 |     }
 236 | 
 237 |     // shutdown if no valid ports
 238 |     if (dynamixel_ports_.size() == 0)
 239 |     {
 240 |       ROS_ERROR("No valid ports found, shutting_down...");
 241 |       return false;
 242 |     }
 243 |   }
 244 |   else
 245 |   {
 246 |     ROS_ERROR("No port details loaded to param server");
 247 |     return false;
 248 |   }
 249 | 
 250 |   if (diagnostics_rate_ > 0)
 251 |   {
 252 |     diagnostics_publisher_ = nh_->advertise<dynamixel_interface::ServoDiags>("servo_diagnostics", 1);
 253 |   }
 254 | 
 255 |   if (dataport_rate_ > 0)
 256 |   {
 257 |     dataport_publisher_ = nh_->advertise<dynamixel_interface::DataPorts>("external_dataports", 1);
 258 |   }
 259 | 
 260 |   parameters_parsed_ = true;
 261 |   return true;
 262 | }
 263 | 
 264 | 
 265 | /// Parses the information in the yaml file for each port
 266 | /// @param[in] ports the xml structure to be parsed
 267 | bool DynamixelInterfaceController::parsePortInformation(XmlRpc::XmlRpcValue ports)
 268 | {
 269 |   /// don't parse parameters during operation
 270 |   if (parameters_parsed_)
 271 |   {
 272 |     ROS_ERROR("Parameters already parsed");
 273 |     return false;
 274 |   }
 275 | 
 276 |   // If there is no servos array in the param server, return
 277 |   if (ports.getType() != XmlRpc::XmlRpcValue::TypeArray)
 278 |   {
 279 |     ROS_ERROR("Invalid/missing device information on the param server");
 280 |     return false;
 281 |   }
 282 | 
 283 |   // number of ports defined
 284 |   int num_ports = ports.size();
 285 | 
 286 |   // For every port, load and verify its information
 287 |   for (int i = 0; i < ports.size(); i++)
 288 |   {
 289 |     PortInfo port;
 290 | 
 291 |     bool use_group_read = false;
 292 |     bool use_group_write = false;
 293 | 
 294 |     /************************* PORT ARGUMENTS *********************/
 295 | 
 296 | 
 297 |     // check if info exists for specified port
 298 |     if (ports[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
 299 |     {
 300 |       ROS_ERROR("Invalid/Missing info-struct for servo index %d", i);
 301 |       return false;
 302 |     }
 303 | 
 304 | 
 305 |     // get port name
 306 |     if (ports[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString)
 307 |     {
 308 |       ROS_ERROR("Invalid/Missing port name for port %d", i);
 309 |       return false;
 310 |     }
 311 |     else
 312 |     {
 313 |       port.port_name = static_cast<std::string>(ports[i]["name"]);
 314 |     }
 315 | 
 316 | 
 317 |     // get device name
 318 |     if (ports[i]["device"].getType() != XmlRpc::XmlRpcValue::TypeString)
 319 |     {
 320 |       ROS_ERROR("Invalid/Missing device name for port %d", i);
 321 |       return false;
 322 |     }
 323 |     else
 324 |     {
 325 |       port.device = static_cast<std::string>(ports[i]["device"]);
 326 |     }
 327 | 
 328 |     // get baud rate
 329 |     if (ports[i]["baudrate"].getType() != XmlRpc::XmlRpcValue::TypeInt)
 330 |     {
 331 |       ROS_ERROR("Invalid/Missing baudrate for port %d", i);
 332 |       return false;
 333 |     }
 334 |     else
 335 |     {
 336 |       port.baudrate = static_cast<int>(ports[i]["baudrate"]);
 337 |     }
 338 | 
 339 |     // get protocol
 340 |     if (ports[i]["use_legacy_protocol"].getType() != XmlRpc::XmlRpcValue::TypeBoolean)
 341 |     {
 342 |       ROS_ERROR("Invalid/Missing use_legacy_protocol option for port %d", i);
 343 |       return false;
 344 |     }
 345 |     else
 346 |     {
 347 |       port.use_legacy_protocol = static_cast<bool>(ports[i]["use_legacy_protocol"]);
 348 |     }
 349 | 
 350 |     // get group read enabled
 351 |     if (ports[i]["group_read_enabled"].getType() != XmlRpc::XmlRpcValue::TypeBoolean)
 352 |     {
 353 |       ROS_ERROR("Invalid/Missing group_read_enabled option for port %d", i);
 354 |       return false;
 355 |     }
 356 |     else
 357 |     {
 358 |       use_group_read = static_cast<bool>(ports[i]["group_read_enabled"]);
 359 |     }
 360 | 
 361 |     // get group write enabled
 362 |     if (ports[i]["group_write_enabled"].getType() != XmlRpc::XmlRpcValue::TypeBoolean)
 363 |     {
 364 |       ROS_ERROR("Invalid/Missing group_write_enabled option for port %d", i);
 365 |       return false;
 366 |     }
 367 |     else
 368 |     {
 369 |       use_group_write = static_cast<bool>(ports[i]["group_write_enabled"]);
 370 |     }
 371 | 
 372 | 
 373 |     /************************* Driver initialisation *********************/
 374 | 
 375 |     // Create driver
 376 |     port.driver = std::unique_ptr<DynamixelInterfaceDriver>(new DynamixelInterfaceDriver(
 377 |       port.device, port.baudrate, port.use_legacy_protocol, use_group_read, use_group_write));
 378 | 
 379 |     /************************* Dynamixel initialisation *********************/
 380 | 
 381 |     XmlRpc::XmlRpcValue servos;
 382 | 
 383 |     // If there is no servos array in the param server, return
 384 |     if (ports[i]["servos"].getType() != XmlRpc::XmlRpcValue::TypeArray)
 385 |     {
 386 |       ROS_ERROR("Invalid/missing servo information on the param server");
 387 |       return false;
 388 |     }
 389 |     else
 390 |     {
 391 |       servos = ports[i]["servos"];
 392 |     }
 393 | 
 394 |     if (!parseServoInformation(port, servos))
 395 |     {
 396 |       ROS_ERROR("Unable to parse servo information for port %s", port.port_name.c_str());
 397 |       return false;
 398 |     }
 399 | 
 400 |     // add port only if dynamixels were found
 401 |     if (port.joints.size() > 0)
 402 |     {
 403 |       ROS_INFO("Adding port %s to loop", port.port_name.c_str());
 404 |       // add port information to server
 405 |       dynamixel_ports_.emplace_back(std::move(port));
 406 |     }
 407 |     else
 408 |     {
 409 |       ROS_ERROR("No dynamixels found on %s (%s)!", port.device.c_str(), port.port_name.c_str());
 410 |       return false;
 411 |     }
 412 |   }
 413 | 
 414 |   return true;
 415 | }
 416 | 
 417 | /// Parses the information in the yaml file for each servo
 418 | /// @param[in] port the port object to parse the servo info into
 419 | /// @param[in] servos the xml structure to be parsed
 420 | bool DynamixelInterfaceController::parseServoInformation(PortInfo &port, XmlRpc::XmlRpcValue servos)
 421 | {
 422 |   /// don't parse parameters during operation
 423 |   if (parameters_parsed_)
 424 |   {
 425 |     ROS_ERROR("Parameters already parsed");
 426 |     return false;
 427 |   }
 428 | 
 429 |   // number of servos defined
 430 |   int num_servos = servos.size();
 431 | 
 432 |   // For every servo, load and verify its information
 433 |   for (int i = 0; i < servos.size(); i++)
 434 |   {
 435 |     // store the loaded information in this struct
 436 |     DynamixelInfo info;
 437 | 
 438 |     // check if info exists for specified joint
 439 |     if (servos[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
 440 |     {
 441 |       ROS_ERROR("Invalid/Missing info-struct for servo index %d", i);
 442 |       return false;
 443 |     }
 444 | 
 445 |     // get joint id
 446 |     if (servos[i]["id"].getType() != XmlRpc::XmlRpcValue::TypeInt)
 447 |     {
 448 |       ROS_ERROR("Invalid/Missing id for servo index %d", i);
 449 |       return false;
 450 |     }
 451 |     else
 452 |     {
 453 |       // store the servo's ID
 454 |       info.id = static_cast<int>(servos[i]["id"]);
 455 |     }
 456 | 
 457 |     // get joint name
 458 |     if (servos[i]["joint_name"].getType() != XmlRpc::XmlRpcValue::TypeString)
 459 |     {
 460 |       ROS_ERROR("Invalid/Missing joint name for servo index %d, id: %d", i, info.id);
 461 |       return false;
 462 |     }
 463 |     else
 464 |     {
 465 |       // store the servo's corresponding joint name
 466 |       info.joint_name = static_cast<std::string>(servos[i]["joint_name"]);
 467 | 
 468 |       // check this port and all previous ports for duplicate joint names (not allowed as joints are
 469 |       // referred to by name)
 470 |       if (port.joints.find(info.joint_name) != port.joints.end())
 471 |       {
 472 |         ROS_ERROR("Cannot have multiple joints with the same name [%s]", info.joint_name.c_str());
 473 |         return false;
 474 |       }
 475 |       else
 476 |       {
 477 |         for (int j = 0; j < dynamixel_ports_.size(); j++)
 478 |         {
 479 |           if (dynamixel_ports_[j].joints.find(info.joint_name) != dynamixel_ports_[j].joints.end())
 480 |           {
 481 |             ROS_ERROR("Cannot have multiple joints with the same name [%s]", info.joint_name.c_str());
 482 |             return false;
 483 |           }
 484 |         }
 485 |       }
 486 |     }
 487 | 
 488 |     // get joint zero position
 489 |     if (servos[i]["zero_pos"].getType() != XmlRpc::XmlRpcValue::TypeInt)
 490 |     {
 491 |       ROS_ERROR("Invalid/Missing zero position for servo index %d, id: %d", i, info.id);
 492 |       return false;
 493 |     }
 494 |     else
 495 |     {
 496 |       // store the servo's corresponding joint
 497 |       info.zero_pos = static_cast<int>(servos[i]["zero_pos"]);
 498 |     }
 499 | 
 500 |     // get joint default min position
 501 |     if (servos[i]["min_pos"].getType() != XmlRpc::XmlRpcValue::TypeInt)
 502 |     {
 503 |       ROS_ERROR("Invalid/Missing min position for servo index %d, id: %d", i, info.id);
 504 |       return false;
 505 |     }
 506 |     else
 507 |     {
 508 |       info.min_pos = static_cast<int>(servos[i]["min_pos"]);
 509 |     }
 510 | 
 511 |     // get joint default max position
 512 |     if (servos[i]["max_pos"].getType() != XmlRpc::XmlRpcValue::TypeInt)
 513 |     {
 514 |       ROS_ERROR("Invalid/Missing max position for servo index %d, id: %d", i, info.id);
 515 |       return false;
 516 |     }
 517 |     else
 518 |     {
 519 |       info.max_pos = static_cast<int>(servos[i]["max_pos"]);
 520 |     }
 521 | 
 522 |     // get joint default joint speed (or set to global if none specified)
 523 |     if (servos[i]["max_vel"].getType() != XmlRpc::XmlRpcValue::TypeDouble)
 524 |     {
 525 |       info.max_vel = global_max_vel_;
 526 |     }
 527 |     else
 528 |     {
 529 |       info.max_vel = static_cast<double>(servos[i]["max_vel"]);
 530 |       if (info.max_vel < 0.0)
 531 |       {
 532 |         info.max_vel = global_max_vel_;
 533 |       }
 534 |     }
 535 | 
 536 | 
 537 |     // get joint torque limit (or set to global if none specified)
 538 |     if (servos[i]["torque_limit"].getType() != XmlRpc::XmlRpcValue::TypeDouble)
 539 |     {
 540 |       info.torque_limit = global_torque_limit_;
 541 |     }
 542 |     else
 543 |     {
 544 |       info.torque_limit = static_cast<double>(servos[i]["torque_limit"]);
 545 |       if ((info.torque_limit > 1.0) || (info.torque_limit < 0.0))
 546 |       {
 547 |         info.torque_limit = global_torque_limit_;
 548 |       }
 549 |     }
 550 | 
 551 |     // store current control mode
 552 |     info.current_mode = control_type_;
 553 | 
 554 |     // add joint to port
 555 |     port.joints[info.joint_name] = info;
 556 |     ROS_INFO("Added joint %s (%d) to port %s", info.joint_name.c_str(), info.id, port.port_name.c_str());
 557 |   }
 558 | 
 559 |   return true;
 560 | }
 561 | 
 562 | 
 563 | /// Initialises the controller, performing the following steps:
 564 | /// - for each port:
 565 | ///   - Initialise driver
 566 | ///   - for each dynamixel
 567 | //      - check response on bus
 568 | ///     - load model information
 569 | ///     - validate compatibility with port
 570 | /// @returns true on successful initialisation, false otherwise
 571 | bool DynamixelInterfaceController::initialise()
 572 | {
 573 |   // don't initialise if we have no parameters or are already running
 574 |   if (!parameters_parsed_)
 575 |   {
 576 |     ROS_ERROR("Parameters not parsed yet, must be parsed before init");
 577 |     return false;
 578 |   }
 579 |   else if (initialised_)
 580 |   {
 581 |     ROS_ERROR("Already initialised");
 582 |     return false;
 583 |   }
 584 | 
 585 |   for (auto &port : dynamixel_ports_)
 586 |   {
 587 |     if (!initialisePort(port))
 588 |     {
 589 |       ROS_ERROR("Unable to initialised port %s", port.port_name.c_str());
 590 |       return false;
 591 |     }
 592 |   }
 593 | 
 594 |   // advertise the joint state input and output topics
 595 |   joint_state_publisher_ = nh_->advertise<sensor_msgs::JointState>("joint_states", 1);
 596 |   joint_state_subscriber_ = nh_->subscribe<sensor_msgs::JointState>("desired_joint_states", recv_queue_size_,
 597 |                                                                     &DynamixelInterfaceController::jointStateCallback,
 598 |                                                                     this, ros::TransportHints().tcpNoDelay());
 599 | 
 600 |   initialised_ = true;
 601 |   return true;
 602 | }
 603 | 
 604 | 
 605 | /// Initialises the port, opening the driver, and validating all dynamixels
 606 | /// @returns true on successful initialisation, false otherwise
 607 | bool DynamixelInterfaceController::initialisePort(PortInfo &port)
 608 | {
 609 |   // don't initialise if we have no parameters or are already running
 610 |   if (!parameters_parsed_)
 611 |   {
 612 |     ROS_ERROR("Parameters not parsed yet, must be parsed before init");
 613 |     return false;
 614 |   }
 615 |   else if (initialised_)
 616 |   {
 617 |     ROS_ERROR("Already initialised");
 618 |     return false;
 619 |   }
 620 | 
 621 |   DynamixelSeriesType type_check;
 622 |   bool first_dynamixel = true;
 623 | 
 624 |   // Attempt driver initialisation
 625 |   if (!port.driver->initialise())
 626 |   {
 627 |     ROS_ERROR("Unable to initialise driver");
 628 |     return false;
 629 |   }
 630 | 
 631 |   // first, initialise dynamixels
 632 |   for (auto &it : port.joints)
 633 |   {
 634 |     if (!initialiseDynamixel(port, it.second))
 635 |     {
 636 |       ROS_ERROR("Unable to initialised dynamixel %s", it.second.joint_name.c_str());
 637 |       return false;
 638 |     }
 639 |   }
 640 | 
 641 |   // then, check type safety of bus
 642 |   for (auto &it : port.joints)
 643 |   {
 644 |     if (first_dynamixel)
 645 |     {
 646 |       type_check = it.second.model_spec->type;
 647 |       first_dynamixel = false;
 648 |     }
 649 |     else
 650 |     {
 651 |       switch (type_check)
 652 |       {
 653 |         case kSeriesAX:
 654 |         case kSeriesRX:
 655 |         case kSeriesDX:
 656 |         case kSeriesEX:
 657 |         case kSeriesLegacyMX:
 658 |           if (it.second.model_spec->type > kSeriesLegacyMX)
 659 |           {
 660 |             ROS_ERROR("Type mismatch on bus, only dynamixel who share a common register table may share a bus! "
 661 |                       "Have both %s and %s.",
 662 |                       port.driver->getSeriesName(type_check).c_str(),
 663 |                       port.driver->getSeriesName(it.second.model_spec->type).c_str());
 664 |             return false;
 665 |           }
 666 |           break;
 667 | 
 668 |         case kSeriesX:
 669 |         case kSeriesMX:
 670 |           if ((it.second.model_spec->type != kSeriesX) && (it.second.model_spec->type != kSeriesMX))
 671 |           {
 672 |             ROS_ERROR("Type mismatch on bus, only dynamixel who share a common register table may share a bus! "
 673 |                       "Have both %s and %s.",
 674 |                       port.driver->getSeriesName(type_check).c_str(),
 675 |                       port.driver->getSeriesName(it.second.model_spec->type).c_str());
 676 |             return false;
 677 |           }
 678 |           break;
 679 | 
 680 |         case kSeriesP:
 681 |           if (it.second.model_spec->type != kSeriesP)
 682 |           {
 683 |             ROS_ERROR("Type mismatch on bus, only dynamixel who share a common register table may share a bus! "
 684 |                       "Have both %s and %s.",
 685 |                       port.driver->getSeriesName(type_check).c_str(),
 686 |                       port.driver->getSeriesName(it.second.model_spec->type).c_str());
 687 |             return false;
 688 |           }
 689 |           break;
 690 | 
 691 |         case kSeriesLegacyPro:
 692 |           if (it.second.model_spec->type != kSeriesLegacyPro)
 693 |           {
 694 |             ROS_ERROR("Type mismatch on bus, only dynamixel who share a common register table may share a bus! "
 695 |                       "Have both %s and %s.",
 696 |                       port.driver->getSeriesName(type_check).c_str(),
 697 |                       port.driver->getSeriesName(it.second.model_spec->type).c_str());
 698 |             return false;
 699 |           }
 700 |           break;
 701 |       }
 702 |     }
 703 |   }
 704 | 
 705 |   return true;
 706 | }
 707 | 
 708 | /// Initialises the dynamixel. Pings the given id to make sure it exists, then loads it's model information and sets
 709 | /// up the relevant registers
 710 | /// @returns true on successful initialisation, false otherwise
 711 | bool DynamixelInterfaceController::initialiseDynamixel(PortInfo &port, DynamixelInfo &dynamixel)
 712 | {
 713 |   // don't initialise if we have no parameters or are already running
 714 |   if (!parameters_parsed_)
 715 |   {
 716 |     ROS_ERROR("Parameters not parsed yet, must be parsed before init");
 717 |     return false;
 718 |   }
 719 |   else if (initialised_)
 720 |   {
 721 |     ROS_ERROR("Already initialised");
 722 |     return false;
 723 |   }
 724 | 
 725 |   // sleep to make sure the bus is clear of comms
 726 |   ros::Duration(0.2).sleep();
 727 | 
 728 |   // Ping the servo to make sure that we can actually connect to it
 729 |   // and that it is alive and well on our bus, if not, sleep and try again
 730 |   // if all retry's fail, throw an error
 731 |   bool ping_success = true;
 732 |   int ping_count = 0;
 733 |   while (!port.driver->ping(dynamixel.id))
 734 |   {
 735 |     // increment ping count
 736 |     ping_count++;
 737 | 
 738 |     ROS_WARN("Failed to ping id: %d, attempt %d, retrying...", dynamixel.id, ping_count);
 739 |     // max number of retry's
 740 |     if (ping_count > 5)
 741 |     {
 742 |       // unable to detect motor
 743 |       ROS_ERROR("Cannot ping dynamixel id: %d", dynamixel.id);
 744 |       return false;
 745 |     }
 746 | 
 747 |     // sleep and try again
 748 |     ros::Duration(0.5).sleep();
 749 |   }
 750 | 
 751 |   // only add if ping was successful
 752 |   bool success = true;
 753 |   uint16_t model_number = 0;
 754 | 
 755 |   if (!port.driver->getModelNumber(dynamixel.id, &model_number))
 756 |   {
 757 |     ROS_ERROR("Unable to retrieve model number for dynamixel id %d", dynamixel.id);
 758 |     return false;
 759 |   }
 760 |   else if (model_number == 0)
 761 |   {
 762 |     ROS_ERROR("Invalid model number retrieved for dynamixel id %d", dynamixel.id);
 763 |     return false;
 764 |   }
 765 | 
 766 |   // If valid motor, setup in operating mode
 767 |   dynamixel.model_spec = port.driver->getModelSpec(model_number);
 768 |   if (dynamixel.model_spec == nullptr)
 769 |   {
 770 |     ROS_ERROR("Failed to load model information for dynamixel id %d", dynamixel.id);
 771 |     ROS_ERROR("Model Number: %d", model_number);
 772 |     ROS_ERROR("Info is not in database");
 773 |     return false;
 774 |     ;
 775 |   }
 776 | 
 777 |   // check error status of dynamixel
 778 |   uint8_t error_status = 0;
 779 |   if (!port.driver->getErrorStatus(dynamixel.id, dynamixel.model_spec->type, &error_status))
 780 |   {
 781 |     ROS_WARN("Failed to check error status of dynamixel id %d", dynamixel.id);
 782 |   }
 783 |   else if (error_status)
 784 |   {
 785 |     ROS_WARN("Dynamixel %d returned error code %d", dynamixel.id, error_status);
 786 |   }
 787 | 
 788 |   dynamixel.torque_enabled = false;
 789 | 
 790 |   // Display joint info
 791 |   ROS_INFO("Joint Name: %s, ID: %d, Series: %s, Model: %s", dynamixel.joint_name.c_str(), dynamixel.id,
 792 |            port.driver->getSeriesName(dynamixel.model_spec->type).c_str(), dynamixel.model_spec->name.c_str());
 793 | 
 794 |   // Only support effort control on newer spec dynamixels
 795 |   if (control_type_ == kModeTorqueControl)
 796 |   {
 797 |     switch (dynamixel.model_spec->type)
 798 |     {
 799 |       case kSeriesAX:
 800 |       case kSeriesRX:
 801 |       case kSeriesDX:
 802 |       case kSeriesEX:
 803 |       case kSeriesLegacyMX:
 804 |       case kSeriesLegacyPro:
 805 |         ROS_ERROR("Effort Control not supported for legacy series dynamixels!");
 806 |         return false;
 807 |     }
 808 |   }
 809 | 
 810 |   // maintain torque state in motor
 811 |   if (!port.driver->getTorqueEnabled(dynamixel.id, dynamixel.model_spec->type, &dynamixel.torque_enabled))
 812 |   {
 813 |     ROS_ERROR("Unable to get torque_enable status for dynamixel id %d", dynamixel.id);
 814 |     return false;
 815 |   }
 816 |   if (!port.driver->setTorqueEnabled(dynamixel.id, dynamixel.model_spec->type, 0))
 817 |   {
 818 |     ROS_ERROR("Unable to set torque_enable status for dynamixel id %d", dynamixel.id);
 819 |     return false;
 820 |   }
 821 | 
 822 |   // set operating mode for the motor
 823 |   if (!port.driver->setOperatingMode(dynamixel.id, dynamixel.model_spec->type, control_type_))
 824 |   {
 825 |     ROS_ERROR("Failed to set operating mode for dynamixel id %d", dynamixel.id);
 826 |     return false;
 827 |   }
 828 | 
 829 |   // set torque limit for the motor
 830 |   if (!port.driver->setMaxTorque(dynamixel.id, dynamixel.model_spec->type,
 831 |                                  (int)(dynamixel.torque_limit * dynamixel.model_spec->effort_reg_max)))
 832 |   {
 833 |     ROS_ERROR("Failed to set torque limit for dynamixel id %d", dynamixel.id);
 834 |     return false;
 835 |   }
 836 | 
 837 |   // set velocity limits for the motor
 838 |   if (dynamixel.model_spec->type > kSeriesLegacyMX)
 839 |   {
 840 |     if (!port.driver->setMaxVelocity(dynamixel.id, dynamixel.model_spec->type,
 841 |                                      (int)(dynamixel.max_vel * dynamixel.model_spec->velocity_radps_to_reg)))
 842 |     {
 843 |       ROS_ERROR("Failed to set velocity limit for dynamixel id %d", dynamixel.id);
 844 |       return false;
 845 |     }
 846 |   }
 847 | 
 848 |   // angle limits are only relevant in position control mode
 849 |   if (control_type_ == kModePositionControl)
 850 |   {
 851 |     // set angle limits & direction
 852 |     if (dynamixel.min_pos > dynamixel.max_pos)
 853 |     {
 854 |       if (!port.driver->setAngleLimits(dynamixel.id, dynamixel.model_spec->type, dynamixel.max_pos, dynamixel.min_pos))
 855 |       {
 856 |         ROS_ERROR("Failed to set angle limits for dynamixel id %d", dynamixel.id);
 857 |         return false;
 858 |       }
 859 |     }
 860 |     else
 861 |     {
 862 |       if (!port.driver->setAngleLimits(dynamixel.id, dynamixel.model_spec->type, dynamixel.min_pos, dynamixel.max_pos))
 863 |       {
 864 |         ROS_ERROR("Failed to set angle limits for dynamixel id %d", dynamixel.id);
 865 |         return false;
 866 |       }
 867 |     }
 868 |   }
 869 | 
 870 |   // preserve torque enable state
 871 |   if (!port.driver->setTorqueEnabled(dynamixel.id, dynamixel.model_spec->type, dynamixel.torque_enabled))
 872 |   {
 873 |     ROS_ERROR("Unable to reset torque_enable status for dynamixel id %d", dynamixel.id);
 874 |     return false;
 875 |   }
 876 | 
 877 |   return true;
 878 | }
 879 | 
 880 | /// Callback for recieving a command from the /desired_joint_state topic. The function atomically updates the class
 881 | /// member variable containing the latest message and sets a flag indicating a new message has been received
 882 | /// @param[in] joint_commands the command received from the topic
 883 | void DynamixelInterfaceController::jointStateCallback(const sensor_msgs::JointState::ConstPtr &joint_commands)
 884 | {
 885 |   // thread safe modification
 886 |   std::unique_lock<std::mutex> lock(write_mutex_);
 887 | 
 888 |   // In order to prevent information being lost if multiple nodes are publishing commands, the joint command is checked
 889 |   // against the existing message, either adding new dynamixel to the message or updating the commands for the joints
 890 |   // already inside. This way joint command messages with subsets of dynamixels are combined rather than overwritten.
 891 |   if (write_msg_.name.empty())
 892 |   {
 893 |     write_msg_ = *joint_commands;
 894 |   }
 895 |   // avoid array length issues
 896 |   else if ((write_msg_.position.empty() == joint_commands->position.empty()) &&
 897 |            (write_msg_.velocity.empty() == joint_commands->velocity.empty()) &&
 898 |            (write_msg_.effort.empty() == joint_commands->effort.empty()))
 899 |   {
 900 |     // only add joint if not already in list (THIS TAKES N*M TIME, NEED TO OPTIMISE)
 901 |     for (int i = 0; i < joint_commands->name.size(); i++)
 902 |     {
 903 |       bool in_msg = false;
 904 | 
 905 |       // if joint already in message, update values
 906 |       for (int j = 0; j < write_msg_.name.size(); j++)
 907 |       {
 908 |         if (joint_commands->name[i] == write_msg_.name[j])
 909 |         {
 910 |           if (!write_msg_.position.empty() && !joint_commands->position.empty())
 911 |           {
 912 |             write_msg_.position[j] = joint_commands->position[i];
 913 |           }
 914 | 
 915 |           if (!write_msg_.velocity.empty() && !joint_commands->velocity.empty())
 916 |           {
 917 |             write_msg_.velocity[j] = joint_commands->velocity[i];
 918 |           }
 919 | 
 920 |           if (!write_msg_.effort.empty() && !joint_commands->effort.empty())
 921 |           {
 922 |             write_msg_.effort[j] = joint_commands->effort[i];
 923 |           }
 924 | 
 925 |           in_msg = true;
 926 |           break;
 927 |         }
 928 |       }
 929 | 
 930 |       // not already in message, push back
 931 |       if (!in_msg)
 932 |       {
 933 |         write_msg_.name.push_back(joint_commands->name[i]);
 934 | 
 935 |         if (!write_msg_.position.empty() && !joint_commands->position.empty())
 936 |         {
 937 |           write_msg_.position.push_back(joint_commands->position[i]);
 938 |         }
 939 | 
 940 |         if (!write_msg_.velocity.empty() && !joint_commands->velocity.empty())
 941 |         {
 942 |           write_msg_.velocity.push_back(joint_commands->velocity[i]);
 943 |         }
 944 | 
 945 |         if (!write_msg_.effort.empty() && !joint_commands->effort.empty())
 946 |         {
 947 |           write_msg_.effort.push_back(joint_commands->effort[i]);
 948 |         }
 949 |       }
 950 |     }
 951 |   }
 952 | 
 953 |   write_ready_ = true;
 954 | }
 955 | 
 956 | /// main loop for performing IO, handles the creation and joining of IO threads for each port, so that IO for multiple
 957 | /// usb devices can be threaded.
 958 | void DynamixelInterfaceController::loop(void)
 959 | {
 960 |   // wait until init
 961 |   if (!initialised_)
 962 |   {
 963 |     return;
 964 |   }
 965 | 
 966 |   // don't access the driver after its been cleaned up
 967 |   int num_servos = 0;
 968 |   std::vector<std::thread> threads;
 969 | 
 970 |   sensor_msgs::JointState read_msg;
 971 |   sensor_msgs::JointState reads[dynamixel_ports_.size()];
 972 | 
 973 |   dynamixel_interface::DataPorts dataports_msg;
 974 |   dynamixel_interface::DataPorts dataports_reads[dynamixel_ports_.size()];
 975 | 
 976 |   dynamixel_interface::ServoDiags diags_msg;
 977 |   dynamixel_interface::ServoDiags diags_reads[dynamixel_ports_.size()];
 978 | 
 979 |   std::unique_lock<std::mutex> lock(write_mutex_);
 980 | 
 981 |   // Control loop rate of dataport reads
 982 |   if (dataport_rate_ > 0)
 983 |   {
 984 |     dataport_counter_++;
 985 |     if (dataport_counter_ >= dataport_iters_)
 986 |     {
 987 |       read_dataport_ = true;
 988 |       dataport_counter_ = 0;
 989 |     }
 990 |   }
 991 | 
 992 |   // Control loop rate of diagnostic reads
 993 |   if (diagnostics_rate_ > 0)
 994 |   {
 995 |     diagnostics_counter_++;
 996 |     if (diagnostics_counter_ >= diagnostics_iters_)
 997 |     {
 998 |       read_diagnostics_ = true;
 999 |       diagnostics_counter_ = 0;
1000 |     }
1001 |   }
1002 | 
1003 |   // spawn an IO thread for each additional port
1004 |   for (int i = 1; i < dynamixel_ports_.size(); i++)
1005 |   {
1006 |     num_servos = num_servos + dynamixel_ports_[i].joints.size();
1007 |     reads[i] = sensor_msgs::JointState();
1008 | 
1009 |     threads.emplace_back(&DynamixelInterfaceController::multiThreadedIO, this, std::ref(dynamixel_ports_[i]),
1010 |                          std::ref(reads[i]), std::ref(dataports_reads[i]), std::ref(diags_reads[i]), write_ready_);
1011 |   }
1012 | 
1013 |   // get messages for the first port
1014 |   reads[0] = sensor_msgs::JointState();
1015 | 
1016 |   num_servos = num_servos + dynamixel_ports_[0].joints.size();
1017 | 
1018 |   // keep the write message thread safe
1019 |   sensor_msgs::JointState temp_msg = write_msg_;
1020 | 
1021 |   // Perform the IO for the first port here (don't bother spinning out another thread)
1022 |   multiThreadedIO(dynamixel_ports_[0], reads[0], dataports_reads[0], diags_reads[0], write_ready_);
1023 | 
1024 |   // loop and get port information (wait for threads in order if any were created)
1025 |   for (int i = 0; i < dynamixel_ports_.size(); i++)
1026 |   {
1027 |     if (i > 0)
1028 |     {
1029 |       threads[i - 1].join();
1030 |     }
1031 | 
1032 |     if (reads[i].name.size() == 0)
1033 |     {
1034 |       // ROS_ERROR("No values passed back from %s thread", dynamixel_ports_[i].device.c_str());
1035 |       continue;
1036 |     }
1037 | 
1038 |     // append read values to published message
1039 |     read_msg.name.insert(read_msg.name.end(), reads[i].name.begin(), reads[i].name.end());
1040 |     read_msg.position.insert(read_msg.position.end(), reads[i].position.begin(), reads[i].position.end());
1041 |     read_msg.velocity.insert(read_msg.velocity.end(), reads[i].velocity.begin(), reads[i].velocity.end());
1042 |     read_msg.effort.insert(read_msg.effort.end(), reads[i].effort.begin(), reads[i].effort.end());
1043 | 
1044 |     // get dataport read info if available
1045 |     if (read_dataport_)
1046 |     {
1047 |       if (dataports_reads[i].states.size() > 0)
1048 |       {
1049 |         // append read values to published message
1050 |         dataports_msg.states.insert(dataports_msg.states.end(), dataports_reads[i].states.begin(),
1051 |                                     dataports_reads[i].states.end());
1052 |       }
1053 |     }
1054 | 
1055 |     // get diagnostics info if available
1056 |     if (read_diagnostics_)
1057 |     {
1058 |       if (diags_reads[i].diagnostics.size() > 0)
1059 |       {
1060 |         diags_msg.diagnostics.insert(diags_msg.diagnostics.end(), diags_reads[i].diagnostics.begin(),
1061 |                                      diags_reads[i].diagnostics.end());
1062 |       }
1063 |     }
1064 |   }
1065 | 
1066 |   // reset write flag
1067 |   if (write_ready_)
1068 |   {
1069 |     write_ready_ = false;
1070 |     write_msg_.name.clear();
1071 |     write_msg_.position.clear();
1072 |     write_msg_.velocity.clear();
1073 |     write_msg_.effort.clear();
1074 |   }
1075 | 
1076 |   lock.unlock();
1077 | 
1078 |   // publish joint states
1079 |   if (read_msg.name.size() > 0)
1080 |   {
1081 |     read_msg.header.stamp = ros::Time::now();
1082 |     joint_state_publisher_.publish(read_msg);
1083 |   }
1084 | 
1085 |   // publish external dataport message
1086 |   if ((read_dataport_) && (dataports_msg.states.size() > 0) && (dataport_rate_ > 0))
1087 |   {
1088 |     dataport_publisher_.publish(dataports_msg);
1089 |     read_dataport_ = false;
1090 |   }
1091 | 
1092 |   if (read_diagnostics_ && (diags_msg.diagnostics.size() > 0) && (diagnostics_rate_ > 0))
1093 |   {
1094 |     diagnostics_publisher_.publish(diags_msg);
1095 |     read_diagnostics_ = false;
1096 |   }
1097 | }
1098 | 
1099 | /// Top level control function for each port's IO thread.
1100 | /// @param[in] port the port for this thread to perform IO on
1101 | /// @param[out] read_msg the JointState this threads joint data is read into
1102 | /// @param[out] dataport_msg the DataPorts msg this thread reads dataport data into
1103 | /// @param[out] status_msgs the ServoDiags msg this thread reads diagnostic data into
1104 | /// @param[in] perform_write whether we are writing data this iteration
1105 | void DynamixelInterfaceController::multiThreadedIO(PortInfo &port, sensor_msgs::JointState &read_msg,
1106 |                                                    dynamixel_interface::DataPorts &dataport_msg,
1107 |                                                    dynamixel_interface::ServoDiags &diags_msg, bool perform_write) const
1108 | {
1109 |   // perform write
1110 |   if (perform_write)
1111 |   {
1112 |     multiThreadedWrite(port, write_msg_);
1113 |   }
1114 | 
1115 |   // perform read
1116 |   multiThreadedRead(port, read_msg, dataport_msg, diags_msg);
1117 | }
1118 | 
1119 | 
1120 | /// Function called in each thread to perform a write on a port
1121 | /// @param[in] port_num index used to retrieve port information from port list
1122 | /// @param[in] joint_commands message cointaining the commands for each joint
1123 | void DynamixelInterfaceController::multiThreadedWrite(PortInfo &port, sensor_msgs::JointState joint_commands) const
1124 | {
1125 |   // ignore empty messages
1126 |   if (joint_commands.name.size() < 1)
1127 |   {
1128 |     return;
1129 |   }
1130 | 
1131 |   // booleans used to setup which parameters to write
1132 |   bool has_pos = false;
1133 |   bool has_vel = false;
1134 |   bool has_eff = false;
1135 | 
1136 |   // figure out which values have been specified
1137 |   if ((joint_commands.position.size() == joint_commands.name.size()) && (control_type_ == kModePositionControl))
1138 |   {
1139 |     has_pos = true;
1140 |   }
1141 |   if ((joint_commands.velocity.size() == joint_commands.name.size()) &&
1142 |       ((control_type_ == kModeVelocityControl) || (control_type_ == kModePositionControl && !ignore_input_velocity_)))
1143 |   {
1144 |     has_vel = true;
1145 |   }
1146 |   if ((joint_commands.effort.size() == joint_commands.name.size()) && (control_type_ == kModeTorqueControl))
1147 |   {
1148 |     has_eff = true;
1149 |   }
1150 | 
1151 |   if ((!has_pos) && (!has_vel) && (!has_eff))
1152 |   {
1153 |     // no valid array sizes for current control mode, ignore
1154 |     return;
1155 |   }
1156 | 
1157 |   // write objects
1158 |   std::unordered_map<int, SyncData> velocities, positions, efforts;
1159 | 
1160 |   // push motor encoder value onto list
1161 |   SyncData write_data;
1162 | 
1163 |   // loop and calculate the values for each specified joint
1164 |   for (int i = 0; i < joint_commands.name.size(); i++)
1165 |   {
1166 |     // lookup the information for that particular joint to be able to control it
1167 |     if (port.joints.find(joint_commands.name[i]) == port.joints.end())
1168 |     {
1169 |       // Joint not on this port, ignore
1170 |       continue;
1171 |     }
1172 | 
1173 |     // Retrieve dynamixel information
1174 |     DynamixelInfo *info = &port.joints[joint_commands.name[i]];
1175 | 
1176 |     // enable torque if not already
1177 |     if (!info->torque_enabled)
1178 |     {
1179 |       if (port.driver->setTorqueEnabled(info->id, info->model_spec->type, true))
1180 |       {
1181 |         // if in position control mode we enable the default join movement speed (profile velocity)
1182 |         if (control_type_ == kModePositionControl)
1183 |         {
1184 |           int regVal = static_cast<int>((static_cast<double>(info->max_vel) * info->model_spec->velocity_radps_to_reg));
1185 | 
1186 |           if (!port.driver->setProfileVelocity(info->id, info->model_spec->type, regVal))
1187 |           {
1188 |             ROS_WARN("Unable to set profile velocity on %s joint", info->joint_name.c_str());
1189 |           }
1190 |         }
1191 |         ROS_INFO("Torque enabled on %s joint", info->joint_name.c_str());
1192 |         info->torque_enabled = true;
1193 |       }
1194 |       else
1195 |       {
1196 |         ROS_WARN("Unable to enable torque on %s joint", info->joint_name.c_str());
1197 |       }
1198 |     }
1199 | 
1200 |     // calculate the position register value for the motor
1201 |     if ((has_pos) && (control_type_ == kModePositionControl))
1202 |     {
1203 |       // used to bound positions to limits
1204 |       int up_lim, dn_lim;
1205 | 
1206 |       // get radian position value
1207 |       double rad_pos = joint_commands.position[i];
1208 | 
1209 |       // define our clamping limits to avoid exceeding safe joint angles
1210 |       if (info->min_pos > info->max_pos)
1211 |       {
1212 |         rad_pos = -rad_pos;
1213 |         up_lim = info->min_pos;
1214 |         dn_lim = info->max_pos;
1215 |       }
1216 |       else
1217 |       {
1218 |         up_lim = info->max_pos;
1219 |         dn_lim = info->min_pos;
1220 |       }
1221 | 
1222 |       // convert from radians to motor encoder value
1223 |       double pos =
1224 |         rad_pos * ((info->model_spec->encoder_cpr * 360.0) / (2 * M_PI * info->model_spec->encoder_range_deg)) +
1225 |         info->zero_pos;
1226 | 
1227 |       // clamp joint angle to be within safe limit
1228 |       if ((pos <= up_lim) && (pos >= dn_lim))
1229 |       {
1230 |         // push motor encoder value onto list
1231 |         write_data.id = info->id;
1232 |         write_data.type = info->model_spec->type;
1233 |         if (write_data.type <= kSeriesLegacyMX)
1234 |         {
1235 |           write_data.data.resize(2);
1236 |           *(reinterpret_cast<uint16_t*>(write_data.data.data())) = static_cast<uint16_t>(pos);
1237 |         }
1238 |         else
1239 |         {
1240 |           write_data.data.resize(4);
1241 |           *(reinterpret_cast<uint32_t*>(write_data.data.data())) = static_cast<uint32_t>(pos);
1242 |         }
1243 | 
1244 |         positions[info->id] = write_data;
1245 |       }
1246 |     }
1247 | 
1248 |     // calculate the velocity register value for the motor
1249 |     if (has_vel && (control_type_ != kModeTorqueControl))
1250 |     {
1251 |       // get rad/s value from message
1252 |       double rad_s_vel = joint_commands.velocity[i];
1253 | 
1254 |       // clamp to joint speed limit
1255 |       rad_s_vel = std::clamp(rad_s_vel, -info->max_vel, info->max_vel);
1256 | 
1257 |       // convert to motor encoder value
1258 |       int vel = (int)((rad_s_vel * info->model_spec->velocity_radps_to_reg));
1259 | 
1260 |       // Velocity values serve 2 different functions, in velocity control mode their sign
1261 |       // defines direction, however in position control mode their absolute value is used
1262 |       // to set the profile velocity (how fast the motor moves to the specified position)
1263 |       // we also need to take an absolute value as each motor series handles negative inputs
1264 |       // differently
1265 |       if ((control_type_ == kModeVelocityControl) && (info->min_pos > info->max_pos))
1266 |       {
1267 |         // The old dynamixel series use a different method of representing velocity values. They define the value in the
1268 |         // register as a signed 10-bit value, with the first 9 bits representing magnitude and the 10th bit representing
1269 |         // the sign.
1270 |         if (info->model_spec->type <= kSeriesLegacyMX)
1271 |         {
1272 |           vel = std::abs(vel) + 1024;
1273 |         }
1274 |         else
1275 |         {
1276 |           vel = -vel;
1277 |         }
1278 |       }
1279 |       else if (control_type_ == kModePositionControl)
1280 |       {
1281 |         vel = std::abs(vel);
1282 |       }
1283 | 
1284 |       // push motor encoder value onto list
1285 |       write_data.id = info->id;
1286 |       write_data.type = info->model_spec->type;
1287 |       if (write_data.type <= kSeriesLegacyMX)
1288 |       {
1289 |         write_data.data.resize(2);
1290 |         *(reinterpret_cast<uint16_t*>(write_data.data.data())) = static_cast<uint16_t>(vel);
1291 |       }
1292 |       else
1293 |       {
1294 |         write_data.data.resize(4);
1295 |         *(reinterpret_cast<uint32_t*>(write_data.data.data())) = static_cast<uint32_t>(vel);
1296 |       }
1297 | 
1298 |       velocities[info->id] = write_data;
1299 |     }
1300 | 
1301 |     if (has_eff && (control_type_ == kModeTorqueControl))
1302 |     {
1303 |       double input_effort = joint_commands.effort[i];
1304 | 
1305 |       int16_t effort = 0;
1306 | 
1307 |       // input and outputs are current values (in A)
1308 |       if (info->model_spec->effort_reg_to_mA != 0)
1309 |       {
1310 |         effort = (input_effort * 1000 / info->model_spec->effort_reg_to_mA);
1311 |         effort = abs(effort);
1312 | 
1313 |         if ((input_effort < 0) != (info->min_pos > info->max_pos))
1314 |         {
1315 |           effort = -effort;
1316 |         }
1317 |       }
1318 | 
1319 |       // push motor encoder value onto list
1320 |       write_data.id = info->id;
1321 |       write_data.type = info->model_spec->type;
1322 |       write_data.data.resize(2);
1323 |       *(reinterpret_cast<int16_t*>(write_data.data.data())) = effort;
1324 | 
1325 |       efforts[info->id] = write_data;
1326 |     }
1327 |   }
1328 | 
1329 |   // use the multi-motor write functions to reduce the bandwidth required to command
1330 |   // all the motors
1331 |   if (control_type_ == kModePositionControl)
1332 |   {
1333 |     // set the profile velocities if they have been defined
1334 |     if ((has_vel) && (!ignore_input_velocity_))
1335 |     {
1336 |       if (!port.driver->setMultiProfileVelocity(velocities))
1337 |       {
1338 |         ROS_ERROR("Failed to set profile velocities on port %s!", port.port_name.c_str());
1339 |       }
1340 |     }
1341 |     // send the positions to the motors
1342 |     if (has_pos)
1343 |     {
1344 |       if (!port.driver->setMultiPosition(positions))
1345 |       {
1346 |         ROS_ERROR("Failed to set positions on port %s!", port.port_name.c_str());
1347 |       }
1348 |     }
1349 |   }
1350 |   else if ((control_type_ == kModeVelocityControl) && has_vel)
1351 |   {
1352 |     // set the velocity values for each motor
1353 |     if (!port.driver->setMultiVelocity(velocities))
1354 |     {
1355 |       ROS_ERROR("Failed to set velocities on port %s!", port.port_name.c_str());
1356 |     }
1357 |   }
1358 |   else if ((control_type_ == kModeTorqueControl) && has_eff)
1359 |   {
1360 |     // set the effort values for each motor
1361 |     if (!port.driver->setMultiTorque(efforts))
1362 |     {
1363 |       ROS_ERROR("Failed to set efforts on port %s!", port.port_name.c_str());
1364 |     }
1365 |   }
1366 | }
1367 | 
1368 | 
1369 | /// Function in thread to perform read on a port.
1370 | /// @param[in] port the port for this thread to perform IO on
1371 | /// @param[out] read_msg the JointState this threads joint data is read into
1372 | /// @param[out] dataport_msg the DataPorts msg this thread reads dataport data into
1373 | /// @param[out] status_msgs the ServoDiags msg this thread reads diagnostic data into
1374 | void DynamixelInterfaceController::multiThreadedRead(PortInfo &port, sensor_msgs::JointState &read_msg,
1375 |                                                      dynamixel_interface::DataPorts &dataport_msg,
1376 |                                                      dynamixel_interface::ServoDiags &diags_msg) const
1377 | {
1378 |   bool comm_success;
1379 |   std::unordered_map<int, DynamixelState> state_map;
1380 |   std::unordered_map<int, DynamixelDiagnostic> diag_map;
1381 |   std::unordered_map<int, DynamixelDataport> data_map;
1382 | 
1383 |   // Iterate over all connected servos and add to list
1384 |   for (auto &it : port.joints)  //(map<string, DynamixelInfo>::iterator iter = port.joints.begin(); iter !=
1385 |                                 // port.joints.end(); iter++)
1386 |   {
1387 |     state_map[it.second.id] = DynamixelState();
1388 |     state_map[it.second.id].id = it.second.id;
1389 |     state_map[it.second.id].type = it.second.model_spec->type;
1390 |     diag_map[it.second.id] = DynamixelDiagnostic();
1391 |     diag_map[it.second.id].id = it.second.id;
1392 |     diag_map[it.second.id].type = it.second.model_spec->type;
1393 |     if (it.second.model_spec->external_ports)
1394 |     {
1395 |       data_map[it.second.id] = DynamixelDataport();
1396 |       data_map[it.second.id].id = it.second.id;
1397 |       data_map[it.second.id].type = it.second.model_spec->type;
1398 |     }
1399 |   }
1400 | 
1401 |   // get state info back from all dynamixels
1402 |   if (port.driver->getBulkState(state_map))
1403 |   {
1404 |     // Iterate over all connected servos and add to list
1405 |     for (auto &it : port.joints)
1406 |     {
1407 |       // //joint name
1408 |       std::string joint_name = it.first;
1409 | 
1410 |       // ignore joints that failed to read
1411 |       if (!state_map[it.second.id].success)
1412 |       {
1413 |         ROS_WARN("FAILED TO READ DYNAMIXEL %s (id %d)!", joint_name.c_str(), it.second.id);
1414 |         continue;
1415 |       }
1416 | 
1417 |       // put joint name in message
1418 |       read_msg.name.push_back(joint_name);
1419 | 
1420 |       // POSITION VALUE
1421 |       // get position and convert to radians
1422 |       // due to the fact that some series of dynamixel do not have a full 360 deg output range, we must scale the,
1423 |       // encoder counts by this fractional value to get the output in radians, thus:
1424 |       // rad_pos = (count-count_initial_position) * (range/360) * (2*PI/encoder_cpr)
1425 |       double rad_pos = (state_map[it.second.id].position - it.second.zero_pos) *
1426 |                        (2.0 * M_PI * it.second.model_spec->encoder_range_deg) /
1427 |                        (360.0 * it.second.model_spec->encoder_cpr);
1428 |       if (it.second.min_pos > it.second.max_pos)
1429 |       {
1430 |         rad_pos = -rad_pos;
1431 |       }
1432 | 
1433 |       // Put position in message
1434 |       read_msg.position.push_back(rad_pos);
1435 | 
1436 |       // VELOCITY VALUE
1437 |       int raw_vel = state_map[it.second.id].velocity;
1438 | 
1439 |       // handle the sign of the value based on the motor series
1440 |       if (it.second.model_spec->type <= kSeriesLegacyMX)
1441 |       {
1442 |         // The old dynamixel series use a different method of representing effort values. They define the value in the
1443 |         // register as a signed 10-bit value, with the first 9 bits representing magnitude and the 10th bit representing
1444 |         // the sign.
1445 |         raw_vel = (raw_vel & 0x3FF);
1446 | 
1447 |         if ((raw_vel > 1023) && (it.second.max_pos > it.second.min_pos))
1448 |         {
1449 |           raw_vel = -raw_vel;
1450 |         }
1451 |         else if ((raw_vel < 1023) && (it.second.max_pos < it.second.min_pos))
1452 |         {
1453 |           raw_vel = -raw_vel;
1454 |         }
1455 |       }
1456 |       else if (it.second.min_pos > it.second.max_pos)
1457 |       {
1458 |         raw_vel = -raw_vel;
1459 |       }
1460 | 
1461 |       // convert to rad/s
1462 |       double rad_s_vel = (static_cast<double>(raw_vel)) / it.second.model_spec->velocity_radps_to_reg;
1463 | 
1464 |       // put velocity in message
1465 |       read_msg.velocity.push_back(rad_s_vel);
1466 | 
1467 |       // TORQUE EFFORT VALUE
1468 |       double effort = 0;
1469 | 
1470 |       if (it.second.model_spec->type <= kSeriesLegacyMX)
1471 |       {
1472 |         // The old dynamixel series use a different method of representing effort values. They define the value in the
1473 |         // register as a signed 10-bit value, with the first 9 bits representing magnitude and the 10th bit representing
1474 |         // the sign.
1475 |         effort = static_cast<double>(state_map[it.second.id].effort & 0x3FF) * it.second.model_spec->effort_reg_to_mA;
1476 |         // check sign
1477 |         if (state_map[it.second.id].effort < 1023)
1478 |         {
1479 |           effort = 0.0 - effort;
1480 |         }
1481 |       }
1482 |       else
1483 |       {
1484 |         effort = static_cast<double>(state_map[it.second.id].effort) * it.second.model_spec->effort_reg_to_mA;
1485 |       }
1486 | 
1487 |       if (it.second.min_pos > it.second.max_pos)
1488 |       {
1489 |         effort = -effort;
1490 |       }
1491 | 
1492 |       // put effort in message
1493 |       read_msg.effort.push_back(effort);
1494 |     }
1495 |   }
1496 |   else
1497 |   {
1498 |     ROS_WARN("Failed to get bulk state on port %s", port.port_name.c_str());
1499 |   }
1500 | 
1501 | 
1502 |   if (read_dataport_ && (data_map.size() > 0))
1503 |   {
1504 |     if (port.driver->getBulkDataportInfo(data_map))
1505 |     {
1506 |       // dynamixel_interface::ServoState diag_msg;
1507 |       dataport_msg.header.frame_id = port.port_name.c_str();
1508 | 
1509 |       // Iterate over all connected servos and add to list
1510 |       for (auto &it : port.joints)
1511 |       {
1512 |         dynamixel_interface::DataPort msg;
1513 | 
1514 |         // ignore joints that failed to read
1515 |         if (!data_map[it.second.id].success)
1516 |         {
1517 |           continue;
1518 |         }
1519 | 
1520 |         // get data
1521 |         msg.name = it.first;
1522 |         msg.port_values.push_back(data_map[it.second.id].port_values[0]);
1523 |         msg.port_values.push_back(data_map[it.second.id].port_values[1]);
1524 |         msg.port_values.push_back(data_map[it.second.id].port_values[2]);
1525 |         msg.port_values.push_back(data_map[it.second.id].port_values[3]);
1526 | 
1527 |         // push msg into array
1528 |         dataport_msg.states.push_back(msg);
1529 |       }
1530 |     }
1531 |     else
1532 |     {
1533 |       ROS_WARN("Failed to get dataports on port %s", port.port_name.c_str());
1534 |     }
1535 |   }
1536 | 
1537 |   if (read_diagnostics_)
1538 |   {
1539 |     if (port.driver->getBulkDiagnosticInfo(diag_map))
1540 |     {
1541 |       // dynamixel_interface::ServoState diag_msg;
1542 |       diags_msg.header.frame_id = port.port_name.c_str();
1543 | 
1544 |       // Iterate over all connected servos and add to list
1545 |       for (auto &it : port.joints)
1546 |       {
1547 |         dynamixel_interface::ServoDiag msg;
1548 | 
1549 |         // ignore joints that failed to read
1550 |         if (!diag_map[it.second.id].success)
1551 |         {
1552 |           continue;
1553 |         }
1554 | 
1555 |         // get name
1556 |         msg.name = it.first;
1557 |         msg.id = it.second.id;
1558 |         msg.model_name = it.second.model_spec->name;
1559 | 
1560 |         // get error code
1561 |         msg.error_code = diag_map[it.second.id].error;
1562 | 
1563 |         // get voltage (div 10 as units are 0.1V)
1564 |         msg.voltage = diag_map[it.second.id].voltage / 10.0;
1565 | 
1566 |         // get temperatures
1567 |         msg.temperature = static_cast<double>(diag_map[it.second.id].temperature);
1568 | 
1569 |         // push msg into array
1570 |         diags_msg.diagnostics.push_back(msg);
1571 |       }
1572 |     }
1573 |     else
1574 |     {
1575 |       ROS_WARN("Failed to get diagnostics on port %s", port.port_name.c_str());
1576 |     }
1577 |   }
1578 | }
1579 | 
1580 | }  // namespace dynamixel_interface


--------------------------------------------------------------------------------
/src/dynamixel_interface_main.cpp:
--------------------------------------------------------------------------------
 1 | /// @file    dynamixel_interface_main.cpp
 2 | /// @brief   entry point for the ros dynamixel interface
 3 | ///
 4 | /// @author  Tom Molnar (tom.molnar@data61.csiro.au)
 5 | /// @date    November 2020
 6 | /// @version 0.0.1
 7 | ///
 8 | /// CSIRO Autonomous Systems Laboratory
 9 | /// Queensland Centre for Advanced Technologies
10 | /// PO Box 883, Kenmore, QLD 4069, Australia
11 | ///
12 | /// (c) Copyright CSIRO 2020
13 | ///
14 | /// CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
15 | /// Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
16 | /// All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
17 | /// terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
18 | /// and binary forms, with or without modification, are permitted provided that the following conditions are met:
19 | /// - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
20 | ///   disclaimer.
21 | /// - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
22 | ///   disclaimer in the documentation and/or other materials provided with the distribution.
23 | /// - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
24 | ///   this software without specific prior written permission of CSIRO.
25 | ///
26 | /// EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
27 | /// PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
28 | /// BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
29 | /// OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
30 | /// DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
31 | /// TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
32 | /// WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
33 | /// OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
34 | /// SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
35 | /// ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
36 | /// INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
37 | /// OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
38 | /// CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
39 | /// REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
40 | /// RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
41 | /// TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
42 | /// LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
43 | /// OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
44 | /// (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
45 | /// (b)  THE REPAIR OF THE SOFTWARE;
46 | /// (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
47 | ///      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
48 | /// IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
49 | /// WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
50 | 
51 | #include <dynamixel_interface/dynamixel_interface_controller.h>
52 | #include <ros/ros.h>
53 | 
54 | /// main
55 | int main(int argc, char **argv)
56 | {
57 |   // ros initialise
58 |   ros::init(argc, argv, "dynamixel_interface_node");
59 | 
60 |   // create controller
61 |   dynamixel_interface::DynamixelInterfaceController controller;
62 | 
63 |   // parse params
64 |   if (!controller.parseParameters())
65 |   {
66 |     return 1;
67 |   }
68 | 
69 |   // intialise
70 |   if (!controller.initialise())
71 |   {
72 |     return 2;
73 |   }
74 | 
75 |   // initialise rate controller
76 |   ros::Rate rate(controller.getLoopRate());
77 | 
78 |   // Loop and process
79 |   while (ros::ok())
80 |   {
81 |     controller.loop();
82 | 
83 |     ros::spinOnce();
84 |     rate.sleep();
85 |   }
86 | 
87 |   return 0;
88 | }
89 | 


--------------------------------------------------------------------------------
/tutorials/tutorial_1_using_the_controller.md:
--------------------------------------------------------------------------------
  1 | # Tutorial 1: Using the controller
  2 | 
  3 | This tutorial describes the basics of how to setup the dynamixel_interface_controller 
  4 | Tutorial Level: Beginner
  5 | 
  6 | ## Step 1: Create a package
  7 | 
  8 | To begin, create a package that will allow us to create and manage our own configuration of the controller. This tutorial assumes you have an initialised catkin workspace as per the tutorial: http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment. Create the package with:
  9 | 
 10 | ```bash
 11 | 
 12 | cd ~/catkin_ws/src
 13 | catkin_create_pkg my_dynamixel_project dynamixel_interface roscpp std_msgs sensor_msgs
 14 | 
 15 | ```
 16 | 
 17 | ## Step 2: Create a controller configuration file
 18 | 
 19 | Now that we have a package setup, it is time to create the controller config file that will define how our controller runs. First, create a config folder within your package
 20 | 
 21 | ```bash
 22 | 
 23 | roscd my_dynamixel_project
 24 | mkdir config
 25 | cd config
 26 | 
 27 | ```
 28 | 
 29 | Next, create the config file. In the config folder, create a file named controller_config.yaml and copy in the following text:
 30 | 
 31 | ```yaml
 32 | 
 33 | # GLOBAL OPERATION PARAMETERS
 34 | loop_rate: 100                    # desired rate for joint state updates. actual rate may be less depending on number
 35 |                                   # of dynamixels on each port
 36 | control_mode: position            # control mode, either 'position', 'velocity', or 'effort'
 37 | disable_torque_on_shutdown: true  # with this enabled the motors will switch off when the controller closes
 38 | ignore_input_velocity: false      # ignore input velocity commands in position mode (no profile velocity)
 39 | diagnostics_rate: 1               # rate to publish diagnostic information
 40 | dataport_rate: 1                  # rate to read from dynamixel external dataports
 41 | 
 42 | # The below values are used as global defaults and are applied for each servo unless overridden in the entry for the servo below
 43 | global_max_vel: 5.0               # maximum joint speed (rad/s) (in position or velocity control)
 44 | global_torque_limit: 1.0          # maximum motor torque for all modes, given as a fraction of rated max (0-1)
 45 | 
 46 | ```
 47 | 
 48 | These values define the overall operation of the controller. In particular control_mode defines what method of control and what commands our servos will respond to. Publish rates are in hz and can be fractional as well. note that the controller may max out it's publish rate depending on how many dynamixels are being controlled, so don't always expect the actual rate to be as high as you input. The global parameters are designed to make it easy to configure multiple dynamixels that will all have the same parameters and thus let you cut down on the size of your config file. We want the servos to stop moving when the controller shuts down so we enable the disable_torque_on_shutdown flag.
 49 | 
 50 | Now we will define out serial ports and servos, paste the following at the end of the file:
 51 | 
 52 | ```yaml
 53 | 
 54 | # PORT AND SERVO CONFIGURATIONS
 55 | ports:
 56 | 
 57 |   # PORT LIST
 58 |   - name: Port_1               # name for this port in config
 59 |     device: /dev/ttyUSB0       # serial device this port will communicate on
 60 |     baudrate: 1000000          # baudrate in use
 61 |     use_legacy_protocol: false # wether to use new 2.0 protocol (false) or legacy 1.0 protocol (true)
 62 |     group_read_enabled: true   # specify whether to use group comms for reading
 63 |     group_write_enabled: true  # specify whether to use group comms for writing
 64 |     servos:
 65 |         # SERVO LIST FOR THIS PORT
 66 |         - id: 1                # (ID set in servo eeprom, must be unique on this port)
 67 |           joint_name: joint_1  # (MUST BE UNIQUE ACROSS ALL PORTS)
 68 |           #
 69 |           # The three values below are mandatory, they define the orientation and zeroing of the dynamixel:
 70 |           #
 71 |           zero_pos: 2048       # initial (0 rad) servo position (in raw encoder count)
 72 |           min_pos: 0           # minimum servo position (in raw encoder count)
 73 |           max_pos: 4095        # maximum servo position, Note when MIN > MAX ROTATION IS REVERSED
 74 |           #
 75 |           # The below arguments are all optional and override the global values:
 76 |           #
 77 |           max_vel: 5.0         # maximum joint speed (rad/s) (in position or velocity control)
 78 |           torque_limit: 1.0    # maximum motor torque for all modes, given as a fraction of rated max (0-1)
 79 | 
 80 |         - id: 2
 81 |           joint_name: joint_2
 82 |           zero_pos: 2048
 83 |           min_pos: 0
 84 |           max_pos: 4095
 85 |           #
 86 |           # This servo doesn't have any optional values defined, the global defaults will be used
 87 |           #
 88 | 
 89 | ```
 90 | 
 91 | Here we can see that we have a list within a list. The outer parameter, ports, defines a list of different port objects. This controller can handle multiple serial objects at once. note that the name of the port is just for identification. make sure you set the device, baudrate and series to match the dynamixels you are currently using. Then for each port object there is a parameter, servos, which defines a list of servos that are on that port. For this example we will control two servos on the one port. so make sure you set the ID's of the servos to match. The three compulsory values, init, min and max, define the zero position, orientation and limit of the dynamixel. these values are set in the actual raw position counts used by the servos themselves, rather than in radians. The values below them are override values and are optional, if any one of those parameters is not specified for a motor, the global is used instead.
 92 | 
 93 | Once you have configured the servos accordingly, save and close the File.
 94 | 
 95 | ## Step 3: Creating the launch file
 96 | 
 97 | This step is easy. first, create a launch folder in your project:
 98 | 
 99 | ```bash
100 | 
101 | roscd my_dynamixel_project
102 | mkdir launch
103 | cd launch
104 | 
105 | ```
106 | 
107 | Then, in the launch folder, create a file named dynamixel_interface_controller.launch. paste in the follwing text:
108 | 
109 | ```xml
110 | 
111 | <launch>
112 |   <node name="dynamixel_interface_node" pkg="dynamixel_interface" type="dynamixel_interface_controller_node" output="screen">
113 |     <rosparam command="load" file="$(find dynamixel_interface)/config/controller_config.yaml" />
114 |   </node>
115 | </launch>
116 | 
117 | ```
118 | 
119 | That's it, all the launch file does is load the controller_config.yaml file from our package and launch the controller. Save and close the file.
120 | 
121 | ## Step 4: Launching the controller
122 | 
123 | First, make sure your dynamixel's are connected and the config file matches their id's, baudrate and port. Now it's time to launch the controller, run the following command:
124 | 
125 | ```bash
126 | 
127 | roslaunch my_dynamixel_project dynamixel_interface_controller.launch
128 | 
129 | ```
130 | 
131 | If all goes well and your controller starts up you should see output similar to this:
132 | 
133 | ```bash
134 | 
135 | [INFO] [1485821510.959546110]: Device is '/dev/ttyUSB0' with baud rate '1000000', using protocol 2
136 | [INFO] [1485821510.960439630]: Succeeded to open the port(/dev/ttyUSB0)!
137 | [INFO] [1485821510.962058345]: Succeeded to change the baudrate(1000000)!
138 | [INFO] [1485821510.963828396]: Joint Name: joint_1, ID: 1, Model: MX106
139 | [INFO] [1485821510.979844529]: Joint Name: joint_2, ID: 2, Model: MX64
140 | 
141 | ```
142 | 
143 | ## Step 5: Controlling the dynamixels
144 | 
145 | Now that the controller is up and running, run
146 | 
147 | ```bash
148 | 
149 | rostopic list
150 | 
151 | ```
152 | 
153 | You will see the following topics:
154 | 
155 | ```bash
156 | 
157 | /desired_joint_state
158 | /joint_states
159 | /rosout
160 | /rosout_agg
161 | 
162 | ```
163 | 
164 | The current states of the servos are published on /joint_states. go ahead and run
165 | 
166 | ```bash
167 | 
168 | rostopic echo -w 5 -c /joint_states
169 | 
170 | ```
171 | 
172 | This will diplay the current joint states, formatted nicely for us to view. the output should look something like:
173 | 
174 | ```bash
175 | 
176 | header:
177 |  seq: 45
178 |  stamp:
179 |  secs: 1485821873
180 |  nsecs: 625054383
181 |  frame_id: ''
182 |  name: ['joint_1', 'joint_2']
183 |  position: [0.0000, 0.0000]
184 |  velocity: [0.0000, 0.0000]
185 |  effort: [0.0000, 0.0000]
186 | 
187 | ```
188 | 
189 | This is the sensor_msgs::JointState message that the controller uses to send and receive joint state information. we are interested in the four arrays: name, position, velocity and effort. These form an in-order matrix of joint state information. we see that joint_1 has position 0 in the name array, thus all the values in position 0 of the other arrays correspond to joint_1. the same goes for joint_2 and position 1.
190 | 
191 | Now we want to send commands to the servos. We can do this on the command line by publishing to /desired_joint_state, go ahead and run
192 | 
193 | ```bash
194 | 
195 | rostopic pub /desired_joint_state sensor_msgs/JointState '{name: ['joint_1', 'joint_2'], position: [3, -3]}' --once
196 | 
197 | ```
198 | 
199 | The servos should move to the specified positions very quickly, this is because the default_joint_speed limit is quite high. To move the servos more slowly in position mode, we can also specify a velocity value, run the following:
200 | 
201 | ```bash
202 | 
203 | rostopic pub /desired_joint_state sensor_msgs/JointState '{name: ['joint_1', 'joint_2'], position: [0, 0], velocity: [0.5, 0.5]}' --once
204 | 
205 | ```
206 | 
207 | The servos should move the their desired positions much slower this time. Note that if you were to run the original command again, the motors would still move slowly, This is because the motors store their last defined velocity and continue to use that until it is overwritten.
208 | 
209 | That is it! The controller is always used in this manner, the only thing that changes are the configuaration in the controller_config.yaml file, and the values you write to control the dynamixels. From here the controller can be easily extended.
210 | 
211 | Next: Multi Port communication
212 | 


--------------------------------------------------------------------------------
/tutorials/tutorial_2_multi_port_communication.md:
--------------------------------------------------------------------------------
  1 | # Tutorial 2: Multi-Port Communications
  2 | 
  3 | This tutorial will show you how to configure the controller for controlling multiple dynamixels simulataneously through different serial ports. Note that this tutorial requires additional hardware in the form of a second USB2Dynamixel or equivalent TTL or RS-485 device.
  4 | Tutorial Level: Beginner
  5 | Previous Tutorial: Using the controller
  6 | 
  7 | ## Step 1: edit the controller_config.yaml file
  8 | 
  9 | This tutorial assumes that you have a package setup according to the previous tutorial: Using the controller. Open the controller_config.yaml file in my_dynamixel_project, it should look similar to this:
 10 | 
 11 | ```yaml
 12 | 
 13 | # GLOBAL OPERATION PARAMETERS
 14 | loop_rate: 100                    # desired rate for joint state updates. actual rate may be less depending on number
 15 |                                   # of dynamixels on each port
 16 | control_mode: position            # control mode, either 'position', 'velocity', or 'effort'
 17 | disable_torque_on_shutdown: true  # with this enabled the motors will switch off when the controller closes
 18 | ignore_input_velocity: false      # ignore input velocity commands in position mode (no profile velocity)
 19 | diagnostics_rate: 1               # rate to publish diagnostic information
 20 | dataport_rate: 1                  # rate to read from dynamixel external dataports
 21 | 
 22 | # The below values are used as global defaults and are applied for each servo unless overridden in the entry for the servo below
 23 | global_max_vel: 5.0               # maximum joint speed (rad/s) (in position or velocity control)
 24 | global_torque_limit: 1.0          # maximum motor torque for all modes, given as a fraction of rated max (0-1)
 25 | 
 26 | # PORT AND SERVO CONFIGURATIONS
 27 | ports:
 28 | 
 29 |   # PORT LIST
 30 |   - name: Port_1               # name for this port in config
 31 |     device: /dev/ttyUSB0       # serial device this port will communicate on
 32 |     baudrate: 1000000          # baudrate in use
 33 |     use_legacy_protocol: false # wether to use new 2.0 protocol (false) or legacy 1.0 protocol (true)
 34 |     group_read_enabled: true   # specify whether to use group comms for reading
 35 |     group_write_enabled: true  # specify whether to use group comms for writing
 36 |     servos:
 37 |         # SERVO LIST FOR THIS PORT
 38 |         - id: 1                # (ID set in servo eeprom, must be unique on this port)
 39 |           joint_name: joint_1  # (MUST BE UNIQUE ACROSS ALL PORTS)
 40 |           #
 41 |           # The three values below are mandatory, they define the orientation and zeroing of the dynamixel:
 42 |           #
 43 |           zero_pos: 2048       # initial (0 rad) servo position (in raw encoder count)
 44 |           min_pos: 0           # minimum servo position (in raw encoder count)
 45 |           max_pos: 4095        # maximum servo position, Note when MIN > MAX ROTATION IS REVERSED
 46 |           #
 47 |           # The below arguments are all optional and override the global values:
 48 |           #
 49 |           max_vel: 5.0         # maximum joint speed (rad/s) (in position or velocity control)
 50 |           torque_limit: 1.0    # maximum motor torque for all modes, given as a fraction of rated max (0-1)
 51 | 
 52 |         - id: 2
 53 |           joint_name: joint_2
 54 |           zero_pos: 2048
 55 |           min_pos: 0
 56 |           max_pos: 4095
 57 |           #
 58 |           # This servo doesn't have any optional values defined, the global defaults will be used
 59 |           #
 60 | 
 61 | ```
 62 | 
 63 | Adding a second serial port is very very easy, simply paste in the following at the end of the file:
 64 | 
 65 | ```yaml
 66 | 
 67 |   - name: Port_2
 68 |     device: /dev/ttyUSB1
 69 |     baudrate: 3000000
 70 |     use_legacy_protocol: false
 71 |     group_read_enabled: true
 72 |     group_write_enabled: true
 73 |     servos:
 74 |         - id: 1               # id only needs to be unique for each port and so id 1 can be reused here
 75 |           joint_name: joint_3 # name DOES have to be unique though, so we continue the naming scheme
 76 |           zero_pos: 1000
 77 |           min_pos: 1000
 78 |           max: 3000
 79 | 
 80 |         - id: 2
 81 |           joint_name: joint_4
 82 |           zero_pos: 2048
 83 |           min_pos: 0
 84 |           max_pos: 4095
 85 | 
 86 | ```
 87 | 
 88 | Here we can see we have defined a second serial port. this serial port can be configured differently to the first one, with different baudrate and even motor series. It is also important to note that as this is an entirely separate serial bus, the ID's on it can be configured independently from the first port. The only thing that must remain globally unique is the joint name, as this is used to identify the servo when sending and receiving commands. For something different this time, go ahead and change the control_mode to Velocity mode by editing the control_mode parameter at the top of the file. You may also have to change the configuration of motors to suit the hardware you have available. Once that is done, save and close the file.
 89 | 
 90 | That is all the changes we needed to make to define new motors, ports and switch the control mode. The controller is designed so that, through this file, it can be quickly configured to whatever application you need. We also do not need to edit the launch file.
 91 | 
 92 | ## Step 2: Launching and controlling the servos
 93 | 
 94 | Launch the controller the same way we did in the first tutorial:
 95 | 
 96 | ```bash
 97 | 
 98 | roslaunch my_dynamixel_project dynamixel_interface_controller.launch
 99 | 
100 | ```
101 | 
102 | Now, as before, there will be the same topics available to us. Go ahead and run
103 | 
104 | ```bash
105 | 
106 | rostopic echo -w 5 -c /joint_states
107 | 
108 | ```
109 | 
110 | You should see, if your configuration matches the one above, the following output:
111 | 
112 | ```bash
113 | 
114 | header:
115 |  seq: 84
116 |  stamp:
117 |  secs: 1485821873
118 |  nsecs: 625054383
119 |  frame_id: ''
120 |  name: ['joint_1', 'joint_2', 'joint_3', 'joint_4']
121 |  position: [0.0000, 0.0000, 0.0000, 0.0000]
122 |  velocity: [0.0000, 0.0000, 0.0000, 0.0000]
123 |  effort: [0.0000, 0.0000, 0.0000, 0.0000]
124 | 
125 | ```
126 | 
127 | And to publish a command, we can do the following:
128 | 
129 | ```bash
130 | 
131 | rostopic pub /desired_joint_state sensor_msgs/JointState '{name: ['joint_1', 'joint_2', 'joint_3', 'joint_4'], velocity: [1.0, 1.0, 1.0, 1.0]}' --once
132 | 
133 | ```
134 | 
135 | This will command all the motors to move at a velocity of 1 rad/s. As you can see, all joint state information from all servos across all ports is published into the one message simultaneously, this makes it very easy to control any configuration of motors as you never need to worry about which motors are on which port, as long as you send the right value to the right joint_name, the controller takes care of the rest!
136 | 
137 | That's it! From here it should be straightforward to configure this controller for whatever application you need.
138 | 


--------------------------------------------------------------------------------