commit ea69324ba4c8dd5b36c767a41877e7e52c3265d5 Author: Nils Schulte Date: Thu Feb 3 09:03:16 2022 +0100 init diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bfd9081 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +.ccls-cache +build/ +log/ +install/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..0d5e5bd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.5) +project(dynamixel_ros2_control) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -O3) +endif() + +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(dynamixel_sdk REQUIRED COMPONENTS) + +add_library( + ${PROJECT_NAME} + SHARED + src/dynamixel_ros2_control.cpp +) + +target_include_directories( + ${PROJECT_NAME} + PUBLIC + $ + $) + +ament_target_dependencies( + ${PROJECT_NAME} + dynamixel_sdk + rclcpp + hardware_interface + pluginlib +) + +pluginlib_export_plugin_description_file(hardware_interface dynamixel_ros2_control_hardware_interface.xml) + +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + dynamixel_sdk + rclcpp + hardware_interface + pluginlib +) + +ament_package() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..0ae86cb --- /dev/null +++ b/LICENSE @@ -0,0 +1,9 @@ + The MIT License (MIT) + +Copyright © 2022 + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..44032a3 --- /dev/null +++ b/README.md @@ -0,0 +1,85 @@ +# dynamixel-controller +This is a HardwareInterface for [`ros2_control`](https://github.com/ros-controls/ros2_control) for dynamixel actuators from [ROBOTIS](http://en.robotis.com/). Its mainly developed for and with the [XL](http://en.robotis.com/shop_en/list.php?ca_id=202030) series of actuators, +so other dynamixels may or may not work. Open up a issue when you encounter problems. Pull requests +are welcome too. + +## Build +Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed. +Clone it in your workspace and checkout the branch ros2: +``` +git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git +cd DynamixelSDK/ +git checkout ros2 +``` +finally build with colcon +`colcon build` + +## Configure Dynamixel motor parameters via URDF + +Add `serial_port`, `baud_rate`, and `joint_ids` parameters to the ros2_control part of your `.urdf` / `.xarco` files. + +```xml + + + dynamixel_ros2_control/DynamixelHardwareInterface + /dev/ttyUSB0 + /dev/ttyUSB0 + 4000000 + + + 10 + + + + + + + ... +``` +## Run a test +You need the urdf of the robot with the ros2_control xml added. +After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators: +``` +ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: '"$(date '+%s + 6' | bc)"'}}, joint_names: ["joint1"], points: [{positions: [0.0, ], velocities: [0, ], time_from_start: {sec: 0}}]}' --once +``` +The example launch file expects the urdf in its robot description package and a robot bringup package with the `controller.yaml` (example for that is also in this package). + +## USB parameter tuning +To increase the data rate you can decrease the usb latency of the serial contoller. +On linux you can do that with the help of sysfs: +``` +sudo chmod o+w /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +``` + +# USB Latency tuning + +To increase the frequency the actuators can be read and be written to via usb you can decrease the usb latency. On Linux you can do that with the following command (if the dynamixel serial device is on `ttyUSB0`): +``` +sudo chmod o+w /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +``` + +This change however gets lost on reboot. You can create a systemd-service to automatically do this on reboot: +``` +[Unit] +Description=lower usb latency timer + +[Service] +Type=oneshot +User=root +ExecStart=/usr/bin/bash -c '/usr/bin/echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer' + +[Install] +WantedBy=multi-user.target +``` +(Filepath `/etc/systemd/system/usb-latency.service`) +and enable with: +``` +sudo systemctl enable usb-latency.service --now +``` + +## Credits +This is partly based on the work of Youtalk ( +[GitHub link](https://github.com/youtalk/dynamixel_control)), who made a dynamixel ros2_control driver. +I swapt out the dynamixel_workbench parts with my own implementation, that only uses the dynamixelSDK. \ No newline at end of file diff --git a/controllers/controllers.yaml b/controllers/controllers.yaml new file mode 100644 index 0000000..8904978 --- /dev/null +++ b/controllers/controllers.yaml @@ -0,0 +1,64 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + #velocity_controller: + # type: velocity_controllers/JointGroupVelocityController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +#velocity_controller: +# ros__parameters: +# joints: +# - joint_motor1_arm1 +# - joint_motor2_arm1 +# - joint_motor3_arm1 +# - joint_motor1_arm2 +# - joint_motor2_arm2 +# - joint_motor3_arm2 +# - joint_motor1_arm3 +# - joint_motor2_arm3 +# - joint_motor3_arm3 +# - joint_motor1_arm4 +# - joint_motor2_arm4 +# - joint_motor3_arm4 +# - joint_motor1_arm5 +# - joint_motor2_arm5 +# - joint_motor3_arm5 +# - joint_motor1_arm6 +# - joint_motor2_arm6 +# - joint_motor3_arm6 + +joint_trajectory_controller: + ros__parameters: + joints: + - joint_motor1_arm1 + - joint_motor2_arm1 + - joint_motor3_arm1 + - joint_motor1_arm2 + - joint_motor2_arm2 + - joint_motor3_arm2 + - joint_motor1_arm3 + - joint_motor2_arm3 + - joint_motor3_arm3 + - joint_motor1_arm4 + - joint_motor2_arm4 + - joint_motor3_arm4 + - joint_motor1_arm5 + - joint_motor2_arm5 + - joint_motor3_arm5 + - joint_motor1_arm6 + - joint_motor2_arm6 + - joint_motor3_arm6 + + command_interfaces: + - position + + state_interfaces: + - position + + allow_partial_joints_goal: true diff --git a/dynamixel_ros2_control_hardware_interface.xml b/dynamixel_ros2_control_hardware_interface.xml new file mode 100644 index 0000000..a057b17 --- /dev/null +++ b/dynamixel_ros2_control_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + The ROBOTIS Dynamixel system interface. + + + diff --git a/include/dynamixel_ros2_control/dynamixel_driver.hpp b/include/dynamixel_ros2_control/dynamixel_driver.hpp new file mode 100644 index 0000000..1472434 --- /dev/null +++ b/include/dynamixel_ros2_control/dynamixel_driver.hpp @@ -0,0 +1,612 @@ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_sdk.h" +#include "iostream" +#include "servo_register_definitions.h" + +namespace dynamixel { + +class Driver { + public: + typedef std::function hw_error_callback; + private: + struct GroupSyncReader { + GroupSyncRead syncRead; + std::vector> read_functions; + GroupSyncReader( + std::unique_ptr& port, + std::unique_ptr& ph, + uint16_t& start_address, + uint16_t& data_length) : syncRead(port.get(), ph.get(), start_address, data_length){}; + GroupSyncReader( + std::unique_ptr& port, + std::unique_ptr& ph, + field& f) : GroupSyncReader(port, ph, f.address, f.data_length){}; + }; + + class Motor { + public: + const motor_id id; + const std::string name; + const model_info* model; + + bool led; + + double goal_position; + + double position; + + //double goal_velocity; + double velocity; + + double effort; + + bool torque; + + uint8_t hw_error; + + typedef std::function hw_error_callback; + //hw_error_callback error_callback; + + void get_command(const dynamixel::field_name& command, field& f) const { + try { + f = model->controll_table->at(command); + } catch (std::exception& e) { + throw command_type_not_implemented(command); + } + }; + + double velocity_from_raw(uint32_t raw_position) const { + const double RPM2RADPERSEC = 0.104719755f; + //FIXME this calculation is not always correct + //In the official Dynamixel-Toolbox there is a function which handles all the edge cases + //in which negative directions are marked by the 10th bit. + return double(static_cast(raw_position)) * model->rpm_factor * RPM2RADPERSEC; + } + + double position_from_raw(uint32_t raw_position) const { + return double(double(double(raw_position) - double(model->min_value)) / double(model->max_value - model->min_value)) * double(model->max_radian - model->min_radian) + double(model->min_radian); + }; + + uint32_t raw_from_position(double position) const { + return uint64_t((position - model->min_radian) / double(model->max_radian - model->min_radian) * double(model->max_value - model->min_value)) + double(model->min_value); + }; + + void set_position_from_raw(uint32_t raw) { position = position_from_raw(raw); } + void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); } + void set_effort_from_raw(uint32_t raw) { effort = double(raw)*0.1; } + + Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), goal_position(std::nan("")), position(std::nan("")){ + try { + model = &model_infos.at(type); + } catch (const std::exception &e) { + throw motor_type_not_implemented(type); + } + }; + + uint32_t get_position_raw() const { + return raw_from_position(position); + } + + void setup_sync_readers( + std::map>& syncReaders, + std::unique_ptr& portHandler, + std::unique_ptr& packetHandler, + hw_error_callback error_callback = [](auto){}) { + //Torque enabled + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Torque_Enable, + [this](uint32_t data) { + this->torque = bool(data); + if (this->torque == false) { + goal_position = std::nan(""); + } + }); + + //Present Position + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Present_Position, + [this](uint32_t data) { + this->set_position_from_raw(data); + }); + + //Present Velocity + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Present_Velocity, + [this](uint32_t data) { + this->set_velocity_from_raw(data); + }); + + //Present Effort (Load if available else Current) + try { + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Present_Load, + [this](uint32_t data) { + this->set_effort_from_raw(data); + }); + } catch (command_type_not_implemented& e) { + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Present_Current, + [this](uint32_t data) { + this->set_effort_from_raw(data); + }); + } + + //Hardware Error Status + if(error_callback) { + setup_sync_reader( + syncReaders, portHandler, packetHandler, + command::Hardware_Error_Status, + [this, error_callback](uint32_t data) { + uint8_t error = uint8_t(data); + this->hw_error = error; + if (error != 0) { + //std::bitset<8> error(data); + if (error & hardware_status::Input_Voltage_Error) { + error_callback(hardware_status::Input_Voltage_Error); + } + if (error & hardware_status::OverHeating_Error) { + error_callback(hardware_status::OverHeating_Error); + } + if (error & hardware_status::Motor_Encoder_Error) { + error_callback(hardware_status::Motor_Encoder_Error); + } + if (error & hardware_status::Electrical_Shock_Error) { + error_callback(hardware_status::Electrical_Shock_Error); + } + if (error & hardware_status::Overload_Error) { + error_callback(hardware_status::Overload_Error); + } + //https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status + } + }); + } + + } + + void setup_sync_reader( + std::map>& syncReaders, + std::unique_ptr& portHandler, + std::unique_ptr& packetHandler, + const dynamixel::field_name& field_name, + std::function callback) { + field sr_field; + this->get_command(field_name, sr_field); + auto sync_reader_it = syncReaders.find(sr_field); + //if there is no reader for the command field create it + if (sync_reader_it == syncReaders.end()) { + sync_reader_it = syncReaders.insert( + sync_reader_it, + {sr_field, + std::make_unique(portHandler, packetHandler, sr_field)}); + } + sync_reader_it->second->syncRead.addParam(this->id); + sync_reader_it->second->read_functions.push_back( + [this, sr_field, callback](GroupSyncRead& read) { + if (read.isAvailable(this->id, sr_field.address, sr_field.data_length)) { + uint32_t data = read.getData(this->id, sr_field.address, sr_field.data_length); + callback(data); + } else { + // No data available + } + }); + } + typedef std::shared_ptr Ptr; + }; + + std::map motors; + std::unique_ptr portHandler; + std::unique_ptr packetHandler; + + std::map> syncReaders; + std::map> syncWrites; + + model_t get_motor_type(motor_id id) const { + uint16_t model_number = 0; + uint8_t dxl_error = 0; + int dxl_comm_result = packetHandler->ping(portHandler.get(), id, &model_number, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + //error<<"Error pinging "<getRxPacketError(dxl_error); + //std::cerr << "Error pinging motor (id: " << std::to_string(id) << ") :\n" + // << packetHandler->getRxPacketError(dxl_error) << std::endl; + throw dynamixel_bus_error(id, std::string(packetHandler->getRxPacketError(dxl_error))); + } + return model_t(model_number); + } + + /** + * Adds the data to a syncwrite (and creates one if there is none for the specific command field) + * @param command_field the field (i.e. address and data length of the controll table) + * @param data the data to write to the Dynamixel + * @return true if successfull, false otherwise + */ + bool add_syncwrite_data(motor_id id, field& command_field, uint8_t* data) { + auto find = syncWrites.find(command_field); + if (find == syncWrites.end()) { + find = syncWrites.insert( + find, + {command_field, + std::make_unique( + portHandler.get(), + packetHandler.get(), + command_field.address, + command_field.data_length)}); + } + find->second->addParam(id, data); + return true; + } + + /** + * Searches the motor name given the id + * @param id the id to get the name for + * @return the name of the motor + */ + std::string get_motor_name_by_id(const motor_id& id) const { + for (const auto& [joint_name, motor_check] : motors) { + if (motor_check->id == id) + return joint_name; + } + throw std::invalid_argument( + "Joint with id \"" + std::to_string(id) + + "\" not added to driver!"); + } + + /** + * adds the specified motors torque state into the specific GroupSyncWriter + * to prepare it for the writeing over the bus. + */ + void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) { + if(torque && motor->hw_error) { + throw hardware_error(motor, "Can't enable torque!"); + } + field command_field; + motor->get_command(command::Torque_Enable, command_field); + uint32_t p = torque; + add_syncwrite_data(motor->id, command_field, reinterpret_cast(&p)); + } + + void reboot(const dynamixel::Driver::Motor::Ptr& motor) { + packetHandler->reboot(portHandler.get(),motor->id); + } + + hw_error_callback overload_error_callback; + hw_error_callback input_voltage_error_callback; + hw_error_callback overheating_error_callback; + hw_error_callback electrical_shock_error_callback; + hw_error_callback motor_encoder_error_callback; + + public: + struct command_type_not_implemented : public std::logic_error + { + command_type_not_implemented (const dynamixel::field_name type) : std::logic_error{"Command type \""+type+"\" not implemented."} {} + }; + + struct motor_type_not_implemented : public std::logic_error + { + motor_type_not_implemented (const model_t type) : std::logic_error{"Motor type \""+std::to_string(type)+"\" not implemented."} {} + }; + + struct torque_not_enabled : public std::runtime_error { + torque_not_enabled(const dynamixel::Driver::Motor::Ptr& motor) : std::runtime_error("torque of motor " + motor->name + "(id: " + std::to_string(motor->id) + ") is not enabled!") {} + }; + + struct dynamixel_bus_error : public std::runtime_error { + dynamixel_bus_error(const motor_id& id, std::string dynamixel_error) : std::runtime_error("dynamixel bus error while communicating with motor (id: " + std::to_string(id) + ") " + dynamixel_error) {} + }; + + struct hardware_error : public std::runtime_error { + hardware_error(const dynamixel::Driver::Motor::Ptr& motor, std::string consequence) : std::runtime_error("motor " + motor->name + "(id: " + std::to_string(motor->id) + ") has hardware error! " + consequence) {} + }; + + /** + * Dynamixel Driver class. + * Only works with dynmaixel-protocol version 2.0 as syncwrites and reads are used. + * @param port the name of the serial port the dynamixel actuators are connected to + * @param baudrate the baudrate of the dynamixels + */ + Driver(std::string port, uint baudrate = 57600) : portHandler(PortHandler::getPortHandler(port.c_str())), + packetHandler(PacketHandler::getPacketHandler(2.0f)) { + portHandler->setBaudRate(baudrate); + //syncRead = std::make_shared(portHandler_.get(),packetHandler_.get(),0,); + } + + void set_hardware_error_callbacks( + hw_error_callback overload_error_callback = [](auto){}, + hw_error_callback input_voltage_error_callback = [](auto){}, + hw_error_callback overheating_error_callback = [](auto){}, + hw_error_callback electrical_shock_error_callback = [](auto){}, + hw_error_callback motor_encoder_error_callback = [](auto){}) { + this->overload_error_callback = overload_error_callback; + this->input_voltage_error_callback = input_voltage_error_callback; + this->overheating_error_callback = overheating_error_callback; + this->electrical_shock_error_callback = electrical_shock_error_callback; + this->motor_encoder_error_callback = motor_encoder_error_callback; + } + + /** + * Tryes to add a motor to this driver by pinging the id over the bus. + * @param joint_name the name of the actuator + * @param id the id the dynamixel-actuator has + */ + void add_motor(const std::string& joint_name, motor_id id) { + for (const auto& [joint_name_check, motor_check] : motors) { + if (motor_check->id == id) + throw std::invalid_argument( + "Dynamixel-ID \"" + std::to_string(id) + + "\" already in use for joint \"" + joint_name_check + "\"!"); + if (joint_name_check == joint_name) + if (id != motor_check->id) + throw std::invalid_argument( + "Can not add Dynamixel \"" + joint_name + "\" with id " + std::to_string(id) + + ".\nIt is already added under different id (id:\"" + joint_name_check + "\")!"); + } + auto type = get_motor_type(id); + if (type == model_t::None) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + + auto motor = std::make_shared(id, joint_name, type); + motors[joint_name] = motor; + std::weak_ptr weak_motor(motors[joint_name]); + motor->setup_sync_readers(syncReaders, portHandler, packetHandler, + [this, weak_motor](dynamixel::hardware_status s) + { + switch (s) { + case dynamixel::hardware_status::Overload_Error: + overload_error_callback(weak_motor.lock()->name); + break; + case dynamixel::hardware_status::Input_Voltage_Error: + input_voltage_error_callback(weak_motor.lock()->name); + break; + case dynamixel::hardware_status::OverHeating_Error: + overheating_error_callback(weak_motor.lock()->name); + break; + case dynamixel::hardware_status::Motor_Encoder_Error: + motor_encoder_error_callback(weak_motor.lock()->name); + break; + case dynamixel::hardware_status::Electrical_Shock_Error: + electrical_shock_error_callback(weak_motor.lock()->name); + break; + default: + break; + } + }); + } + + /** + * pushes the commands over the bus and reads the states back. + */ + void write() { + for (const auto& [joint_name, motor]: motors) { + if(motor->torque) { + if(std::isnan(motor->goal_position)) + motor->goal_position = motor->position; + set_position(joint_name, motor->goal_position); + } + } + + for (const auto& [field, syncWrite] : syncWrites) { + //(void)field; + /*auto result = */ syncWrite->txPacket(); + //syncWrite->clearParam(); + //std::cout<<"SyncWrite"<getTxRxResult(result)<syncRead.txRxPacket(); + //std::cout<getTxRxResult(result)<read_functions) { + read_f(syncReader->syncRead); + } + } + } + + /** + * Tries to ping every Dynamixel to check it they are reachable. + */ + void ping_all() const { + for (const auto& [joint_name, motor] : motors) { + get_motor_type(motor->id); + } + } + + void for_each_joint(std::function f) { + for (const auto& [joint_name, motor]: motors) { + f(joint_name); + } + } + + /** + * Garanteed to have the same order as + * get_positions, get_velocities and get_efforts. + * + * Use to construct the JointStates message. + */ + void get_joint_names(std::vector& names) const { + names.resize(motors.size()); + int i = 0; + for (const auto& [joint_name, motor] : motors) { + (void)joint_name; + names[i++] = joint_name; + } + } + + double* get_goal_position_ptr(const std::string& joint_name) const { + auto find = motors.find(joint_name); + if (find == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + return &find->second->goal_position; + } + + double* get_position_ptr(const std::string& joint_name) const { + auto find = motors.find(joint_name); + if (find == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + return &find->second->position; + } + + double* get_velocity_ptr(const std::string& joint_name) const { + auto find = motors.find(joint_name); + if (find == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + return &find->second->velocity; + } + + double* get_effort_ptr(const std::string& joint_name) const { + auto find = motors.find(joint_name); + if (find == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + return &find->second->effort; + } + + /** + * Garanteed to have the same order as + * get_joint_names, get_velocities and get_efforts. + * + * Use to construct the JointStates message. + * @param positions the array that gets filled with the data + */ + void get_positions(std::vector& positions) const { + positions.resize(motors.size()); + int i = 0; + for (const auto& [joint_name, motor] : motors) { + (void)joint_name; + positions[i++] = motor->position; + } + } + + /** + * Garanteed to have the same order as + * get_joint_names, get_positions and get_efforts. + * + * Use to construct the JointStates message. + * @param velocities the array that gets filled with the data + */ + void get_velocities(std::vector& velocities) const { + velocities.resize(motors.size()); + int i = 0; + for (const auto& [joint_name, motor] : motors) { + (void)joint_name; + velocities[i++] = motor->velocity; + } + } + + /** + * Garanteed to have the same order as + * get_joint_names, get_positions and get_velocities. + * + * Use to construct the JointStates message. + * @param efforts the array that gets filled with the data + */ + void get_efforts(std::vector& efforts) const { + efforts.resize(motors.size()); + int i = 0; + for (const auto& [joint_name, motor] : motors) { + (void)joint_name; + efforts[i++] = 0; + } + } + + /** + * returns the number of actuators this controller handles. + * add_motor(...) to add more. + * @return returns the number of actuators + */ + std::size_t get_motor_count() const { + return motors.size(); + } + + /** + * adds the specified motors torque state into the specific GroupSyncWriter + * to prepare it for the writeing over the bus. + * @param joint_name the name of the joint + * @param torque true -> enable torque, false -> no torque + */ + void set_torque(const std::string& joint_name, bool torque) { + auto it_motor = motors.find(joint_name); + if (it_motor == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + set_torque(it_motor->second, torque); + } + + /** + * adds all motors torque state into the GroupSyncWriter + * to prepare it for the writeing over the bus. + * @param torque true -> enable torque, false -> no torque + */ + void set_torque_all(bool torque) { + std::map old_torques; + try{ + for (const auto& [joint_name, motor]: motors) { + old_torques[motor] = motor->torque; + set_torque(motor, torque); + } + } catch (hardware_error& e) { + for (const auto& [motor, old_torque]: old_torques) { + set_torque(motor, old_torque); + } + throw e; + } + } + + /** + * adds the specified motors position-data into the specific GroupSyncWriter + * to prepare it for the writeing over the bus. + * @param joint_name the name of the joint + * @param position the position in radians the actuator should move to + * @param enable_torque enable the torque if not enabled already + * @return true if successfull, false otherwise (e.g. joint_name is not available) + */ + void set_position(const std::string& joint_name, double position, bool enable_torque = false) { + if (std::isnan(position)) + throw std::invalid_argument("Can't set position to NaN for Joint \"" + joint_name + "\"!"); + auto it_motor = motors.find(joint_name); + if (it_motor == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + auto motor = it_motor->second; //motors[joint_name]; + + if (!motor->torque) { + if (enable_torque) + set_torque(motor, true); + else + throw torque_not_enabled(motor); + } + + field command_field; + motor->get_command(command::Goal_Position, command_field); + uint32_t p = motor->raw_from_position(position); + motor->goal_position = position; + add_syncwrite_data(motor->id, command_field, reinterpret_cast(&p)); + } + + /** + * reboots a joint. + * @param joint_name the name of the joint + */ + void reboot(const std::string& joint_name) { + auto it_motor = motors.find(joint_name); + if (it_motor == motors.end()) + throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + reboot(it_motor->second); + } +}; +} // namespace dynamixel diff --git a/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp b/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp new file mode 100644 index 0000000..d408148 --- /dev/null +++ b/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp @@ -0,0 +1,59 @@ +#pragma once +#include +#include +#include +#include + + +#include "dynamixel_ros2_control/visiblity_control.h" +#include "dynamixel_ros2_control/dynamixel_driver.hpp" +#include "rclcpp/macros.hpp" +#include +#include + +namespace dynamixel_ros2_control +{ + +class DynamixelHardwareInterface : public hardware_interface::SystemInterface { +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface) + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces() override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + std::vector + export_command_interfaces() override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type read() override; + + DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type write() override; + +private: + hardware_interface::return_type enable_torque(const bool enabled); + + void input_voltage_error_callback(const std::string &joint_name) const; + void overheating_error_callback(const std::string &joint_name) const; + void motor_encoder_error_callback(const std::string &joint_name) const; + void electrical_shock_error_callback(const std::string &joint_name) const; + void overload_error_callback(const std::string &joint_name) const; + + std::unique_ptr dynamixel_driver_; + std::map auto_clear_overload_error; +}; + +} // namespace dynamixel_ros2_control diff --git a/include/dynamixel_ros2_control/servo_register_definitions.h b/include/dynamixel_ros2_control/servo_register_definitions.h new file mode 100644 index 0000000..ad8cad4 --- /dev/null +++ b/include/dynamixel_ros2_control/servo_register_definitions.h @@ -0,0 +1,1694 @@ +#pragma once +//========================================================= +// Servo register definitions +//========================================================= + +#include +#include +#include +//_________________________________________________________ + +namespace dynamixel { + + typedef int motor_id; + + typedef enum { + None = 0, + + AX_12A = 12, + AX_12W = 300, + AX_18A = 18, + + RX_10 = 10, + RX_24F = 24, + RX_28 = 28, + RX_64 = 64, + + EX_106 = 107, + + MX_12W = 360, + MX_28 = 29, + MX_28_2 = 30, + MX_64 = 310, + MX_64_2 = 311, + MX_106 = 320, + MX_106_2 = 321, + + XL_320 = 350, + XL430_W250 = 1060, + + XL430_W250_2 = 1090, // 2XL + + XC430_W150 = 1070, + XC430_W240 = 1080, + + XM430_W210 = 1030, + XM430_W350 = 1020, + + XM540_W150 = 1130, + XM540_W270 = 1120, + + XH430_W210 = 1010, + XH430_W350 = 1000, + XH430_V210 = 1050, + XH430_V350 = 1040, + + XH540_W150 = 1110, + XH540_W270 = 1100, + XH540_V150 = 1150, + XH540_V270 = 1140, + + XW540_T260 = 1170, + XW540_T140 = 1180, + + PRO_L42_10_S300_R = 35072, + PRO_L54_30_S400_R = 37928, + PRO_L54_30_S500_R = 37896, + PRO_L54_50_S290_R = 38176, + PRO_L54_50_S500_R = 38152, + + PRO_M42_10_S260_R = 43288, + PRO_M54_40_S250_R = 46096, + PRO_M54_60_S250_R = 46352, + + PRO_H42_20_S300_R = 51200, + PRO_H54_100_S500_R = 53768, + PRO_H54_200_S500_R = 54024, + + PRO_M42_10_S260_R_A = 43289, + PRO_M54_40_S250_R_A = 46097, + PRO_M54_60_S250_R_A = 46353, + + PRO_H42_20_S300_R_A = 51201, + PRO_H54_100_S500_R_A = 53769, + PRO_H54_200_S500_R_A = 54025, + + PRO_PLUS_M42P_010_S260_R = 2100, + PRO_PLUS_M54P_040_S250_R = 2110, + PRO_PLUS_M54P_060_S250_R = 2120, + + PRO_PLUS_H42P_020_S300_R = 2000, + PRO_PLUS_H54P_100_S500_R = 2010, + PRO_PLUS_H54P_200_S500_R = 2020, + + RH_P12_RN = 35073, + RH_P12_RN_A = 35074 + } model_t; + + typedef struct + { + float rpm; + + int64_t value_of_min_radian_position; + int64_t value_of_zero_radian_position; + int64_t value_of_max_radian_position; + + float min_radian; + float max_radian; + } ModelInfo; + + + struct field { + uint16_t address; + uint16_t data_length; + bool operator <(const field &b) const { + return address < b.address || (address == b.address && data_length * const controll_table; + }; + + + namespace command { + static const std::string Acceleration_Limit = "Acceleration_Limit"; + static const std::string Alarm_LED = "Alarm_LED"; + static const std::string Baud_Rate = "Baud_Rate"; + static const std::string Bus_Watchdog = "Bus_Watchdog"; + static const std::string CCW_Angle_Limit = "CCW_Angle_Limit"; + static const std::string CCW_Compliance_Margin = "CCW_Compliance_Margin"; + static const std::string CCW_Compliance_Slope = "CCW_Compliance_Slope"; + static const std::string Control_Mode = "Control_Mode"; + static const std::string Current = "Current"; + static const std::string Current_Limit = "Current_Limit"; + static const std::string CW_Angle_Limit = "CW_Angle_Limit"; + static const std::string CW_Compliance_Margin = "CW_Compliance_Margin"; + static const std::string CW_Compliance_Slope = "CW_Compliance_Slope"; + static const std::string D_gain = "D_gain"; + static const std::string Drive_Mode = "Drive_Mode"; + static const std::string External_Port_Mode_1 = "External_Port_Mode_1"; + static const std::string External_Port_Mode_2 = "External_Port_Mode_2"; + static const std::string External_Port_Mode_3 = "External_Port_Mode_3"; + static const std::string External_Port_Mode_4 = "External_Port_Mode_4"; + static const std::string Feedforward_1st_Gain = "Feedforward_1st_Gain"; + static const std::string Feedforward_2nd_Gain = "Feedforward_2nd_Gain"; + static const std::string Firmware_Version = "Firmware_Version"; + static const std::string Goal_Acceleration = "Goal_Acceleration"; + static const std::string Goal_Current = "Goal_Current"; + static const std::string Goal_Position = "Goal_Position"; + static const std::string Goal_PWM = "Goal_PWM"; + static const std::string Goal_Torque = "Goal_Torque"; + static const std::string Goal_Velocity = "Goal_Velocity"; + static const std::string Hardware_Error_Status = "Hardware_Error_Status"; + static const std::string Homing_Offset = "Homing_Offset"; + static const std::string I_gain = "I_gain"; + static const std::string ID = "ID"; + static const std::string LED = "LED"; + static const std::string LED_BLUE = "LED_BLUE"; + static const std::string LED_GREEN = "LED_GREEN"; + static const std::string LED_RED = "LED_RED"; + static const std::string Lock = "Lock"; + static const std::string Max_Position_Limit = "Max_Position_Limit"; + static const std::string Max_Torque = "Max_Torque"; + static const std::string Max_Voltage_Limit = "Max_Voltage_Limit"; + static const std::string Min_Position_Limit = "Min_Position_Limit"; + static const std::string Min_Voltage_Limit = "Min_Voltage_Limit"; + static const std::string Model_Number = "Model_Number"; + static const std::string Moving = "Moving"; + static const std::string Moving_Speed = "Moving_Speed"; + static const std::string Moving_Status = "Moving_Status"; + static const std::string Moving_Threshold = "Moving_Threshold"; + static const std::string Multi_Turn_Offset = "Multi_Turn_Offset"; + static const std::string Operating_Mode = "Operating_Mode"; + static const std::string P_gain = "P_gain"; + static const std::string Position_D_Gain = "Position_D_Gain"; + static const std::string Position_I_Gain = "Position_I_Gain"; + static const std::string Position_P_Gain = "Position_P_Gain"; + static const std::string Position_Trajectory = "Position_Trajectory"; + static const std::string Present_Current = "Present_Current"; + static const std::string Present_Input = "Present_Input"; + static const std::string Present_Input_Voltage = "Present_Input_Voltage"; + static const std::string Present_Load = "Present_Load"; + static const std::string Present_Position = "Present_Position"; + static const std::string Present_PWM = "Present_PWM"; + static const std::string Present_Speed = "Present_Speed"; + static const std::string Present_Temperature = "Present_Temperature"; + static const std::string Present_Velocity = "Present_Velocity"; + static const std::string Present_Voltage = "Present_Voltage"; + static const std::string Profile_Acceleration = "Profile_Acceleration"; + static const std::string Profile_Velocity = "Profile_Velocity"; + static const std::string Protocol_Version = "Protocol_Version"; + static const std::string Punch = "Punch"; + static const std::string PWM_Limit = "PWM_Limit"; + static const std::string Realtime_Tick = "Realtime_Tick"; + static const std::string Registered = "Registered"; + static const std::string Registered_Instruction = "Registered_Instruction"; + static const std::string Resolution_Divider = "Resolution_Divider"; + static const std::string Return_Delay_Time = "Return_Delay_Time"; + static const std::string Secondary_ID = "Secondary_ID"; + static const std::string Sensored_Current = "Sensored_Current"; + static const std::string Shutdown = "Shutdown"; + static const std::string Status_Return_Level = "Status_Return_Level"; + static const std::string Temperature_Limit = "Temperature_Limit"; + static const std::string Torque_Control_Mode_Enable = "Torque_Control_Mode_Enable"; + static const std::string Torque_Enable = "Torque_Enable"; + static const std::string Torque_Limit = "Torque_Limit"; + static const std::string Velocity_I_Gain = "Velocity_I_Gain"; + static const std::string Velocity_Limit = "Velocity_Limit"; + static const std::string Velocity_P_Gain = "Velocity_P_Gain"; + static const std::string Velocity_Trajectory = "Velocity_Trajectory"; + } + //_________________________________________________________ + using namespace command; + + //https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status + enum hardware_status { + Input_Voltage_Error = 1<<0, //0b00000001; + OverHeating_Error = 1<<2, //0b00000100; + Motor_Encoder_Error = 1<<3, //0b00001000; + Electrical_Shock_Error = 1<<4, //0b00010000; + Overload_Error = 1<<5, //0b00100000; + }; + + //--------------------------------------------------------- + // AX servos - (num == AX_12A || num == AX_12W || num == AX_18A) + //--------------------------------------------------------- + + static std::map items_AX{ + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Temperature_Limit, {11, 1}}, + {Min_Voltage_Limit, {12, 1}}, + {Max_Voltage_Limit, {13, 1}}, + {Max_Torque, {14, 2}}, + {Status_Return_Level, {16, 1}}, + {Alarm_LED, {17, 1}}, + {Shutdown, {18, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {CW_Compliance_Margin, {26, 1}}, + {CCW_Compliance_Margin, {27, 1}}, + {CW_Compliance_Slope, {28, 1}}, + {CCW_Compliance_Slope, {29, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {32, 2}}, + {Torque_Limit, {34, 2}}, + {Present_Position, {36, 2}}, + {Present_Speed, {38, 2}}, + {Present_Load, {40, 2}}, + {Present_Voltage, {42, 1}}, + {Present_Temperature, {43, 1}}, + {Registered, {44, 1}}, + {Moving, {46, 1}}, + {Lock, {47, 1}}, + {Punch, {48, 2}}}; + #define COUNT_AX_ITEMS (sizeof(items_AX) / sizeof(items_AX[0])) + + static const ModelInfo info_AX = {0.11, + 0, + 512, + 1024, + -2.61799, + 2.61799}; + //--------------------------------------------------------- + // RX servos - (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64) + //--------------------------------------------------------- + + + static std::map items_RX = { + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Temperature_Limit, {11, 1}}, + {Min_Voltage_Limit, {12, 1}}, + {Max_Voltage_Limit, {13, 1}}, + {Max_Torque, {14, 2}}, + {Status_Return_Level, {16, 1}}, + {Alarm_LED, {17, 1}}, + {Shutdown, {18, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {CW_Compliance_Margin, {26, 1}}, + {CCW_Compliance_Margin, {27, 1}}, + {CW_Compliance_Slope, {28, 1}}, + {CCW_Compliance_Slope, {29, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {32, 2}}, + {Torque_Limit, {34, 2}}, + {Present_Position, {36, 2}}, + {Present_Speed, {38, 2}}, + {Present_Load, {40, 2}}, + {Present_Voltage, {42, 1}}, + {Present_Temperature, {43, 1}}, + {Registered, {44, 1}}, + {Moving, {46, 1}}, + {Lock, {47, 1}}, + {Punch, {48, 2}}}; + + #define COUNT_RX_ITEMS (sizeof(items_RX) / sizeof(items_RX[0])) + + static const ModelInfo info_RX = {0.11, + 0, + 512, + 1024, + -2.61799, + 2.61799}; + + + //--------------------------------------------------------- + // EX servos - (num == EX_106) + //--------------------------------------------------------- + static std::map items_EX = { + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Drive_Mode, {10, 1}}, + {Temperature_Limit, {11, 1}}, + {Min_Voltage_Limit, {12, 1}}, + {Max_Voltage_Limit, {13, 1}}, + {Max_Torque, {14, 2}}, + {Status_Return_Level, {16, 1}}, + {Alarm_LED, {17, 1}}, + {Shutdown, {18, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {CW_Compliance_Margin, {26, 1}}, + {CCW_Compliance_Margin, {27, 1}}, + {CW_Compliance_Slope, {28, 1}}, + {CCW_Compliance_Slope, {29, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {34, 2}}, + {Torque_Limit, {35, 2}}, + {Present_Position, {36, 2}}, + {Present_Speed, {38, 2}}, + {Present_Load, {40, 2}}, + {Present_Voltage, {42, 1}}, + {Present_Temperature, {43, 1}}, + {Registered, {44, 1}}, + {Moving, {46, 1}}, + {Lock, {47, 1}}, + {Punch, {48, 2}}, + {Sensored_Current, {56, 2}}}; + + #define COUNT_EX_ITEMS (sizeof(items_EX) / sizeof(items_EX[0])) + + static const ModelInfo info_EX = {0.11, + 0, + 2048, + 4096, + -2.18969008, + 2.18969008}; + + + //--------------------------------------------------------- + // MX Protocol 1 servos - (num == MX_12W || num == MX_28) + //--------------------------------------------------------- + static std::map items_MX = { + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Temperature_Limit, {11, 1}}, + {Min_Voltage_Limit, {12, 1}}, + {Max_Voltage_Limit, {13, 1}}, + {Max_Torque, {14, 2}}, + {Status_Return_Level, {16, 1}}, + {Alarm_LED, {17, 1}}, + {Shutdown, {18, 1}}, + {Multi_Turn_Offset, {20, 2}}, + {Resolution_Divider, {22, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {D_gain, {26, 1}}, + {I_gain, {27, 1}}, + {P_gain, {28, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {32, 2}}, + {Torque_Limit, {34, 2}}, + {Present_Position, {36, 2}}, + {Present_Speed, {38, 2}}, + {Present_Load, {40, 2}}, + {Present_Voltage, {42, 1}}, + {Present_Temperature, {43, 1}}, + {Registered, {44, 1}}, + {Moving, {46, 1}}, + {Lock, {47, 1}}, + {Punch, {48, 2}}, + {Goal_Acceleration, {73, 1}}}; + + #define COUNT_MX_ITEMS (sizeof(items_MX) / sizeof(items_MX[0])) + + static const ModelInfo info_MX = {0.11, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // MX Protocol 2 servos - (num == MX_28_2) + //--------------------------------------------------------- + static std::map items_MX2 = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Load, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + #define COUNT_MX2_ITEMS (sizeof(items_MX2) / sizeof(items_MX2[0])) + + static const ModelInfo info_MX2 = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // EXT MX Protocol 1 servos - (num == MX_64 || num == MX_106) + //--------------------------------------------------------- + static std::map items_EXTMX = { + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Temperature_Limit, {11, 1}}, + {Min_Voltage_Limit, {12, 1}}, + {Max_Voltage_Limit, {13, 1}}, + {Max_Torque, {14, 2}}, + {Status_Return_Level, {16, 1}}, + {Alarm_LED, {17, 1}}, + {Shutdown, {18, 1}}, + {Multi_Turn_Offset, {20, 2}}, + {Resolution_Divider, {22, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {D_gain, {26, 1}}, + {I_gain, {27, 1}}, + {P_gain, {28, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {32, 2}}, + {Torque_Limit, {34, 2}}, + {Present_Position, {36, 2}}, + {Present_Speed, {38, 2}}, + {Present_Load, {40, 2}}, + {Present_Voltage, {42, 1}}, + {Present_Temperature, {43, 1}}, + {Registered, {44, 1}}, + {Moving, {46, 1}}, + {Lock, {47, 1}}, + {Punch, {48, 2}}, + {Current, {68, 2}}, + {Torque_Control_Mode_Enable, {70, 1}}, + {Goal_Torque, {71, 2}}, + {Goal_Acceleration, {73, 1}}}; + + #define COUNT_EXTMX_ITEMS (sizeof(items_EXTMX) / sizeof(items_EXTMX[0])) + + static const ModelInfo info_EXTMX = {0.11, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // EXT MX Protocol 2 Servos - (num == MX_64_2 || num == MX_106_2) + //--------------------------------------------------------- + static std::map items_EXTMX2 = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Current, {102, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Current, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + #define COUNT_EXTMX2_ITEMS (sizeof(items_EXTMX2) / sizeof(items_EXTMX2[0])) + + static const ModelInfo info_EXTMX2 = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // XL320 - (num == XL_320) + //--------------------------------------------------------- + static std::map items_XL320 = { + {Model_Number, {0, 2}}, + {Firmware_Version, {2, 1}}, + {ID, {3, 1}}, + {Baud_Rate, {4, 1}}, + {Return_Delay_Time, {5, 1}}, + {CW_Angle_Limit, {6, 2}}, + {CCW_Angle_Limit, {8, 2}}, + {Control_Mode, {11, 1}}, + {Temperature_Limit, {12, 1}}, + {Min_Voltage_Limit, {13, 1}}, + {Max_Voltage_Limit, {14, 1}}, + {Max_Torque, {15, 2}}, + {Status_Return_Level, {17, 1}}, + {Shutdown, {18, 1}}, + + {Torque_Enable, {24, 1}}, + {LED, {25, 1}}, + {D_gain, {27, 1}}, + {I_gain, {28, 1}}, + {P_gain, {29, 1}}, + {Goal_Position, {30, 2}}, + {Moving_Speed, {32, 2}}, + {Torque_Limit, {34, 2}}, + {Present_Position, {37, 2}}, + {Present_Speed, {39, 2}}, + {Present_Load, {41, 2}}, + {Present_Voltage, {45, 1}}, + {Present_Temperature, {46, 1}}, + {Registered, {47, 1}}, + {Moving, {49, 1}}, + {Hardware_Error_Status, {50, 1}}, + {Punch, {51, 2}}}; + + #define COUNT_XL320_ITEMS (sizeof(items_XL320) / sizeof(items_XL320[0])) + + static const ModelInfo info_XL320 = {0.11, + 0, + 512, + 1024, + -2.61799, + 2.61799}; + + + //--------------------------------------------------------- + // XL - (num == XL430_W250, XL430_W250_2, XC430_W150, XC430_W240) + //--------------------------------------------------------- + static std::map items_XL = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Load, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + static const ModelInfo info_XL = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + + //--------------------------------------------------------- + // XM - (num == XM430_W210 || num == XM430_W350) + //--------------------------------------------------------- + static std::map items_XM = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Current, {102, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Current, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + #define COUNT_XM_ITEMS (sizeof(items_XM) / sizeof(items_XM[0])) + + static const ModelInfo info_XM = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // EXTXM - (num == XM540_W150 || num == XM540_W270) + //--------------------------------------------------------- + static std::map items_EXTXM = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {External_Port_Mode_1, {56, 1}}, + {External_Port_Mode_2, {57, 1}}, + {External_Port_Mode_3, {58, 1}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Current, {102, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Current, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + #define COUNT_EXTXM_ITEMS (sizeof(items_EXTXM) / sizeof(items_EXTXM[0])) + + static const ModelInfo info_EXTXM = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // XH - (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350) + //--------------------------------------------------------- + static std::map items_XH = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Protocol_Version, {13, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {64, 1}}, + {LED, {65, 1}}, + {Status_Return_Level, {68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, {70, 1}}, + {Velocity_I_Gain, {76, 2}}, + {Velocity_P_Gain, {78, 2}}, + {Position_D_Gain, {80, 2}}, + {Position_I_Gain, {82, 2}}, + {Position_P_Gain, {84, 2}}, + {Feedforward_2nd_Gain, {88, 2}}, + {Feedforward_1st_Gain, {90, 2}}, + {Bus_Watchdog, {98, 1}}, + {Goal_PWM, {100, 2}}, + {Goal_Current, {102, 2}}, + {Goal_Velocity, {104, 4}}, + {Profile_Acceleration, {108, 4}}, + {Profile_Velocity, {112, 4}}, + {Goal_Position, {116, 4}}, + {Realtime_Tick, {120, 2}}, + {Moving, {122, 1}}, + {Moving_Status, {123, 1}}, + {Present_PWM, {124, 2}}, + {Present_Current, {126, 2}}, + {Present_Velocity, {128, 4}}, + {Present_Position, {132, 4}}, + {Velocity_Trajectory, {136, 4}}, + {Position_Trajectory, {140, 4}}, + {Present_Input_Voltage, {144, 2}}, + {Present_Temperature, {146, 1}}}; + + #define COUNT_XH_ITEMS (sizeof(items_XH) / sizeof(items_XH[0])) + + static const ModelInfo info_XH = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // EXTXH - (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_V270) + //--------------------------------------------------------- + static std::map items_EXTXH = { + {Model_Number, { 0, 2}}, + {Firmware_Version, { 6, 1}}, + {ID, { 7, 1}}, + {Baud_Rate, { 8, 1}}, + {Return_Delay_Time, { 9, 1}}, + {Drive_Mode, { 10, 1}}, + {Operating_Mode, { 11, 1}}, + {Secondary_ID, { 12, 1}}, + {Protocol_Version, { 13, 1}}, + {Homing_Offset, { 20, 4}}, + {Moving_Threshold, { 24, 4}}, + {Temperature_Limit, { 31, 1}}, + {Max_Voltage_Limit, { 32, 2}}, + {Min_Voltage_Limit, { 34, 2}}, + {PWM_Limit, { 36, 2}}, + {Current_Limit, { 38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, { 44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, { 63, 1}}, + + {Torque_Enable, { 64, 1}}, + {LED, { 65, 1}}, + {Status_Return_Level, { 68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, { 70, 1}}, + {Velocity_I_Gain, { 76, 2}}, + {Velocity_P_Gain, { 78, 2}}, + {Position_D_Gain, { 80, 2}}, + {Position_I_Gain, { 82, 2}}, + {Position_P_Gain, { 84, 2}}, + {Feedforward_2nd_Gain, { 88, 2}}, + {Feedforward_1st_Gain, { 90, 2}}, + {Bus_Watchdog, { 98, 1}}, + {Goal_PWM, { 100, 2}}, + {Goal_Current, { 102, 2}}, + {Goal_Velocity, { 104, 4}}, + {Profile_Acceleration, { 108, 4}}, + {Profile_Velocity, { 112, 4}}, + {Goal_Position, { 116, 4}}, + {Realtime_Tick, { 120, 2}}, + {Moving, { 122, 1}}, + {Moving_Status, { 123, 1}}, + {Present_PWM, { 124, 2}}, + {Present_Current, { 126, 2}}, + {Present_Velocity, { 128, 4}}, + {Present_Position, { 132, 4}}, + {Velocity_Trajectory, { 136, 4}}, + {Position_Trajectory, { 140, 4}}, + {Present_Input_Voltage, { 144, 2}}, + {Present_Temperature, { 146, 1}}}; + + #define COUNT_EXTXH_ITEMS (sizeof(items_EXTXH) / sizeof(items_EXTXH[0])) + + static const ModelInfo info_EXTXH = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // XW - (num == XW540_T260 || XW540_T140) + //--------------------------------------------------------- + static std::map items_XW = { + {Model_Number, { 0, 2}}, + {Firmware_Version, { 6, 1}}, + {ID, { 7, 1}}, + {Baud_Rate, { 8, 1}}, + {Return_Delay_Time, { 9, 1}}, + {Drive_Mode, { 10, 1}}, + {Operating_Mode, { 11, 1}}, + {Secondary_ID, { 12, 1}}, + {Protocol_Version, { 13, 1}}, + {Homing_Offset, { 20, 4}}, + {Moving_Threshold, { 24, 4}}, + {Temperature_Limit, { 31, 1}}, + {Max_Voltage_Limit, { 32, 2}}, + {Min_Voltage_Limit, { 34, 2}}, + {PWM_Limit, { 36, 2}}, + {Current_Limit, { 38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, { 44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {Shutdown, { 63, 1}}, + + {Torque_Enable, { 64, 1}}, + {Status_Return_Level, { 68, 1}}, + {Registered_Instruction, {69, 1}}, + {Hardware_Error_Status, { 70, 1}}, + {Velocity_I_Gain, { 76, 2}}, + {Velocity_P_Gain, { 78, 2}}, + {Position_D_Gain, { 80, 2}}, + {Position_I_Gain, { 82, 2}}, + {Position_P_Gain, { 84, 2}}, + {Feedforward_2nd_Gain, { 88, 2}}, + {Feedforward_1st_Gain, { 90, 2}}, + {Bus_Watchdog, { 98, 1}}, + {Goal_PWM, { 100, 2}}, + {Goal_Current, { 102, 2}}, + {Goal_Velocity, { 104, 4}}, + {Profile_Acceleration, { 108, 4}}, + {Profile_Velocity, { 112, 4}}, + {Goal_Position, { 116, 4}}, + {Realtime_Tick, { 120, 2}}, + {Moving, { 122, 1}}, + {Moving_Status, { 123, 1}}, + {Present_PWM, { 124, 2}}, + {Present_Current, { 126, 2}}, + {Present_Velocity, { 128, 4}}, + {Present_Position, { 132, 4}}, + {Velocity_Trajectory, { 136, 4}}, + {Position_Trajectory, { 140, 4}}, + {Present_Input_Voltage, { 144, 2}}, + {Present_Temperature, { 146, 1}}}; + + #define COUNT_XW_ITEMS (sizeof(items_XW) / sizeof(items_XW[0])) + + static const ModelInfo info_XW = {0.229, + 0, + 2048, + 4096, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // PRO - (num == PRO_L42_10_S300_R) + //--------------------------------------------------------- + static std::map items_PRO = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Operating_Mode, {11, 1}}, + {Moving_Threshold, {17, 4}}, + {Temperature_Limit, {21, 1}}, + {Max_Voltage_Limit, {22, 2}}, + {Min_Voltage_Limit, {24, 2}}, + {Acceleration_Limit, {26, 4}}, + {Torque_Limit, {30, 2}}, + {Velocity_Limit, {32, 4}}, + {Max_Position_Limit, {36, 4}}, + {Min_Position_Limit, {40, 4}}, + {External_Port_Mode_1, {44, 1}}, + {External_Port_Mode_2, {45, 1}}, + {External_Port_Mode_3, {46, 1}}, + {External_Port_Mode_4, {47, 1}}, + {Shutdown, {48, 1}}, + + {Torque_Enable, {562, 1}}, + {LED_RED, {563, 1}}, + {LED_GREEN, {564, 1}}, + {LED_BLUE, {565, 1}}, + {Velocity_I_Gain, {586, 2}}, + {Velocity_P_Gain, {588, 2}}, + {Position_P_Gain, {594, 2}}, + {Goal_Position, {596, 4}}, + {Goal_Velocity, {600, 4}}, + {Goal_Torque, {604, 2}}, + {Goal_Acceleration, {606, 4}}, + {Moving, {610, 1}}, + {Present_Position, {611, 4}}, + {Present_Velocity, {615, 4}}, + {Present_Current, {621, 2}}, + {Present_Input_Voltage, {623, 2}}, + {Present_Temperature, {625, 1}}, + {External_Port_Mode_1, {626, 2}}, + {External_Port_Mode_2, {628, 2}}, + {External_Port_Mode_3, {630, 2}}, + {External_Port_Mode_4, {632, 2}}, + {Registered_Instruction, {890, 1}}, + {Status_Return_Level, {891, 1}}, + {Hardware_Error_Status, {892, 1}}}; + + #define COUNT_PRO_ITEMS (sizeof(items_PRO) / sizeof(items_PRO[0])) + + static const ModelInfo info_PRO = {0.114, + -2048, + 0, + 2048, + -3.14159265, + 3.14159265}; + + //--------------------------------------------------------- + // EXT PRO - All Other Pros... + //--------------------------------------------------------- + static std::map items_EXTPRO = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Operating_Mode, {11, 1}}, + {Homing_Offset, {13, 4}}, + {Moving_Threshold, {17, 4}}, + {Temperature_Limit, {21, 1}}, + {Max_Voltage_Limit, {22, 2}}, + {Min_Voltage_Limit, {24, 2}}, + {Acceleration_Limit, {26, 4}}, + {Torque_Limit, {30, 2}}, + {Velocity_Limit, {32, 4}}, + {Max_Position_Limit, {36, 4}}, + {Min_Position_Limit, {40, 4}}, + {External_Port_Mode_1, {44, 1}}, + {External_Port_Mode_2, {45, 1}}, + {External_Port_Mode_3, {46, 1}}, + {External_Port_Mode_4, {47, 1}}, + {Shutdown, {48, 1}}, + + {Torque_Enable, {562, 1}}, + {LED_RED, {563, 1}}, + {LED_GREEN, {564, 1}}, + {LED_BLUE, {565, 1}}, + {Velocity_I_Gain, {586, 2}}, + {Velocity_P_Gain, {588, 2}}, + {Position_P_Gain, {594, 2}}, + {Goal_Position, {596, 4}}, + {Goal_Velocity, {600, 4}}, + {Goal_Torque, {604, 2}}, + {Goal_Acceleration, {606, 4}}, + {Moving, {610, 1}}, + {Present_Position, {611, 4}}, + {Present_Velocity, {615, 4}}, + {Present_Current, {621, 2}}, + {Present_Input_Voltage, {623, 2}}, + {Present_Temperature, {625, 1}}, + {External_Port_Mode_1, {626, 2}}, + {External_Port_Mode_2, {628, 2}}, + {External_Port_Mode_3, {630, 2}}, + {External_Port_Mode_4, {632, 2}}, + {Registered_Instruction, {890, 1}}, + {Status_Return_Level, {891, 1}}, + {Hardware_Error_Status, {892, 1}}}; + + #define COUNT_EXTPRO_ITEMS (sizeof(items_EXTPRO) / sizeof(items_EXTPRO[0])) + + static const ModelInfo info_EXTPRO[] = { + {0.00249657, -144197, 0, 144197, -3.14159265, 3.14159265}, // PRO_L54_30_S400_R + {0.00199234, -180692, 0, 180692, -3.14159265, 3.14159265}, // PRO_L54_30_S500_R, PRO_L54_50_S500_R + {0.00346667, -103846, 0, 103846, -3.14159265, 3.14159265}, // PRO_L54_50_S290_R + {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R + {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R, PRO_M54_60_S250_R + {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R + {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R, PRO_H54_200_S500_R + + //--------------------------------------------------------- + // EXT PRO (A Firmware_Version) + //--------------------------------------------------------- + static std::map items_EXTPRO_A = { + {Model_Number, { 0, 2}}, + {Firmware_Version, { 6, 1}}, + {ID, { 7, 1}}, + {Baud_Rate, { 8, 1}}, + {Return_Delay_Time, { 9, 1}}, + {Operating_Mode, { 11, 1}}, + {Homing_Offset, { 20, 4}}, + {Moving_Threshold, { 24, 4}}, + {Temperature_Limit, { 31, 1}}, + {Max_Voltage_Limit, { 32, 2}}, + {Min_Voltage_Limit, { 34, 2}}, + {Current_Limit, { 38, 2}}, + {Acceleration_Limit, { 40, 4}}, + {Velocity_Limit, { 44, 4}}, + {Max_Position_Limit, { 48, 4}}, + {Min_Position_Limit, { 52, 4}}, + {External_Port_Mode_1, {56, 1}}, + {External_Port_Mode_2, {57, 1}}, + {External_Port_Mode_3, {58, 1}}, + {External_Port_Mode_4, {59, 1}}, + {Shutdown, { 63, 1}}, + + {Torque_Enable, { 512, 1}}, + {LED_RED, { 513, 1}}, + {LED_GREEN, { 514, 1}}, + {LED_BLUE, { 515, 1}}, + {Velocity_I_Gain, { 524, 2}}, + {Velocity_P_Gain, { 526, 2}}, + {Position_D_Gain, { 528, 2}}, + {Position_P_Gain, { 532, 2}}, + {Position_I_Gain, { 530, 2}}, + {Goal_Position, { 564, 4}}, + {Goal_Velocity, { 552, 4}}, + {Goal_Current, { 604, 2}}, + {Profile_Acceleration, { 556, 4}}, + {Profile_Velocity, { 560, 4}}, + {Moving, { 570, 1}}, + {Present_Position, { 580, 4}}, + {Present_Velocity, { 576, 4}}, + {Present_Current, { 574, 2}}, + {Present_Input_Voltage, { 592, 2}}, + {Present_Temperature, { 594, 1}}, + {External_Port_Mode_1, { 600, 2}}, + {External_Port_Mode_2, { 602, 2}}, + {External_Port_Mode_3, { 604, 2}}, + {External_Port_Mode_4, { 606, 2}}}; + + #define COUNT_EXTPRO_A_ITEMS (sizeof(items_EXTPRO_A) / sizeof(items_EXTPRO_A[0])) + + static const ModelInfo info_EXTPRO_A[] = { + {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R_A + {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R_A, PRO_M54_60_S250_R_A + {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R_A + {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R_A, PRO_H54_200_S500_R_A + + + + //--------------------------------------------------------- + // PRO PLUS - (num == PRO_H42P_020_S300_R, PRO_H54P_100_S500_R, PRO_H54P_200_S500_R) + //--------------------------------------------------------- + static std::map items_PRO_PLUS = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {External_Port_Mode_1, {56, 1}}, + {External_Port_Mode_2, {57, 1}}, + {External_Port_Mode_3, {58, 1}}, + {External_Port_Mode_4, {59, 1}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {512, 1}}, + {LED_RED, {513, 1}}, + {LED_GREEN, {514, 1}}, + {LED_BLUE, {515, 1}}, + {Status_Return_Level, {516, 1}}, + {Registered_Instruction, {517, 1}}, + {Hardware_Error_Status, {518, 1}}, + {Velocity_I_Gain, {524, 2}}, + {Velocity_P_Gain, {526, 2}}, + {Position_D_Gain, {528, 2}}, + {Position_I_Gain, {530, 2}}, + {Position_P_Gain, {532, 2}}, + {Feedforward_2nd_Gain, {536, 2}}, + {Feedforward_1st_Gain, {538, 2}}, + {Bus_Watchdog, {546, 1}}, + {Goal_PWM, {548, 2}}, + {Goal_Current, {550, 2}}, + {Goal_Velocity, {552, 4}}, + {Profile_Acceleration, {556, 4}}, + {Profile_Velocity, {560, 4}}, + {Goal_Position, {564, 4}}, + {Realtime_Tick, {568, 2}}, + {Moving, {570, 1}}, + {Moving_Status, {571, 1}}, + {Present_PWM, {572, 2}}, + {Present_Current, {574, 2}}, + {Present_Velocity, {576, 4}}, + {Present_Position, {580, 4}}, + {Velocity_Trajectory, {584, 4}}, + {Position_Trajectory, {588, 4}}, + {Present_Input_Voltage, {592, 2}}, + {Present_Temperature, {594, 1}}, + {External_Port_Mode_1, {600, 2}}, + {External_Port_Mode_2, {602, 2}}, + {External_Port_Mode_3, {604, 2}}, + {External_Port_Mode_4, {606, 2}}}; + + #define COUNT_EXTPRO_PLUS_ITEMS (sizeof(items_PRO_PLUS) / sizeof(items_PRO_PLUS[0])) + + static const ModelInfo info_PRO_PLUS[] = { + {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M42P_010_S260_R + {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_040_S250_R + {0.01, -262931, 0, 262931, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_060_S250_R + {0.01, -303454, 0, 303454, -3.14159265, 3.14159265}, // PRO_PLUS_H42P_020_S300_R + {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}, // PRO_PLUS_H54P_100_S500_R + {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}}; // PRO_PLUS_H54P_200_S500_R + + + + //--------------------------------------------------------- + // Gripper - (num == RH_P12_RN) + //--------------------------------------------------------- + static std::map items_Gripper = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Operating_Mode, {11, 1}}, + {Homing_Offset, {13, 4}}, + {Moving_Threshold, {17, 4}}, + {Temperature_Limit, {21, 1}}, + {Max_Voltage_Limit, {22, 2}}, + {Min_Voltage_Limit, {24, 2}}, + {Acceleration_Limit, {26, 4}}, + {Torque_Limit, {30, 2}}, + {Velocity_Limit, {32, 4}}, + {Max_Position_Limit, {36, 4}}, + {Min_Position_Limit, {40, 4}}, + {External_Port_Mode_1, {44, 1}}, + {External_Port_Mode_2, {45, 1}}, + {External_Port_Mode_3, {46, 1}}, + {External_Port_Mode_4, {47, 1}}, + {Shutdown, {48, 1}}, + + {Torque_Enable, {562, 1}}, + {LED_RED, {563, 1}}, + {LED_GREEN, {564, 1}}, + {LED_BLUE, {565, 1}}, + {Velocity_I_Gain, {586, 2}}, + {Velocity_P_Gain, {588, 2}}, + {Position_P_Gain, {594, 2}}, + {Goal_Position, {596, 4}}, + {Goal_Velocity, {600, 4}}, + {Goal_Torque, {604, 2}}, + {Goal_Acceleration, {606, 4}}, + {Moving, {610, 1}}, + {Present_Position, {611, 4}}, + {Present_Velocity, {615, 4}}, + {Present_Current, {621, 2}}, + {Present_Input_Voltage, {623, 2}}, + {Present_Temperature, {625, 1}}, + {External_Port_Mode_1, {626, 2}}, + {External_Port_Mode_2, {628, 2}}, + {External_Port_Mode_3, {630, 2}}, + {External_Port_Mode_4, {632, 2}}, + {Registered_Instruction, {890, 1}}, + {Status_Return_Level, {891, 1}}, + {Hardware_Error_Status, {892, 1}}}; + #define COUNT_Gripper_ITEMS (sizeof(items_Gripper) / sizeof(items_Gripper[0])) + + static const ModelInfo info_Gripper = {0.01, + 0, + 0, + 1150, + 0, + 1.7631835937}; + + //--------------------------------------------------------- + // Gripper A Firmware - (num == RH_P12_RN_A) + //--------------------------------------------------------- + static std::map items_EXTGripper = { + {Model_Number, {0, 2}}, + {Firmware_Version, {6, 1}}, + {ID, {7, 1}}, + {Baud_Rate, {8, 1}}, + {Return_Delay_Time, {9, 1}}, + {Drive_Mode, {10, 1}}, + {Operating_Mode, {11, 1}}, + {Secondary_ID, {12, 1}}, + {Homing_Offset, {20, 4}}, + {Moving_Threshold, {24, 4}}, + {Temperature_Limit, {31, 1}}, + {Max_Voltage_Limit, {32, 2}}, + {Min_Voltage_Limit, {34, 2}}, + {PWM_Limit, {36, 2}}, + {Current_Limit, {38, 2}}, + {Acceleration_Limit, {40, 4}}, + {Velocity_Limit, {44, 4}}, + {Max_Position_Limit, {48, 4}}, + {Min_Position_Limit, {52, 4}}, + {External_Port_Mode_1, {56, 1}}, + {External_Port_Mode_2, {57, 1}}, + {External_Port_Mode_3, {58, 1}}, + {External_Port_Mode_4, {59, 1}}, + {Shutdown, {63, 1}}, + + {Torque_Enable, {512, 1}}, + {LED_RED, {513, 1}}, + {LED_GREEN, {514, 1}}, + {LED_BLUE, {515, 1}}, + {Status_Return_Level, {516, 1}}, + {Registered_Instruction, {517, 1}}, + {Hardware_Error_Status, {518, 1}}, + {Velocity_I_Gain, {524, 2}}, + {Velocity_P_Gain, {526, 2}}, + {Position_D_Gain, {528, 2}}, + {Position_I_Gain, {530, 2}}, + {Position_P_Gain, {532, 2}}, + {Feedforward_2nd_Gain, {536, 2}}, + {Feedforward_1st_Gain, {538, 2}}, + {Bus_Watchdog, {546, 1}}, + {Goal_PWM, {548, 2}}, + {Goal_Current, {550, 2}}, + {Goal_Velocity, {552, 4}}, + {Profile_Acceleration, {556, 4}}, + {Profile_Velocity, {560, 4}}, + {Goal_Position, {564, 4}}, + {Realtime_Tick, {568, 2}}, + {Moving, {570, 1}}, + {Moving_Status, {571, 1}}, + {Present_PWM, {572, 2}}, + {Present_Current, {574, 2}}, + {Present_Velocity, {576, 4}}, + {Present_Position, {580, 4}}, + {Velocity_Trajectory, {584, 4}}, + {Position_Trajectory, {588, 4}}, + {Present_Input_Voltage, {592, 2}}, + {Present_Temperature, {594, 1}}, + {External_Port_Mode_1, {600, 2}}, + {External_Port_Mode_2, {602, 2}}, + {External_Port_Mode_3, {604, 2}}, + {External_Port_Mode_4, {606, 2}} + }; + + + + + + static const std::map model_infos = { + {AX_12A, + {AX_12A,0.11,0,1024,-2.61799,2.61799,&items_AX}}, + {AX_12W, + {AX_12W,0.11,0,1024,-2.61799,2.61799,&items_AX}}, + {AX_18A, + {AX_18A,0.11,0,1024,-2.61799,2.61799,&items_AX}}, + {XL430_W250, + {XL430_W250,0.229,0,4096,-3.14159265,3.14159265,&items_XL}}, + {XL430_W250_2, + {XL430_W250_2, 0.229,0,4096,-3.14159265,3.14159265,&items_XL}}, + {XC430_W150, + {XC430_W150,0.229,0,4096,-3.14159265,3.14159265,&items_XL}}, + {XC430_W240, + {XC430_W240,0.229,0,4096,-3.14159265,3.14159265,&items_XL}}, + }; + + /*static const std::map model_infos = { + {AX_12A,&infos_AX_12A}, + {AX_12W,&infos_AX_12W}, + {AX_18A,&infos_AX_18A}, + + {XL430_W250,&infos_XL430_W250}, + {XL430_W250_2,&infos_XL430_W250_2}, + {XC430_W150,&infos_XC430_W150}, + {XC430_W240,&infos_XC430_W240} + };*/ + + /* std::map* get_command_table() { + switch(type) { + case AX_12A: + case AX_12W: + case AX_18A: + return &items_AX; + case RX_10: + case RX_24F: + case RX_28: + case RX_64: + return &items_RX; + case EX_106: + return &items_EX; + case MX_12W: + case MX_28: + return &items_MX; + case MX_28_2: + return &items_MX2; + case XL_320: + return &items_XL320; + case XL430_W250: + case XL430_W250_2: + case XC430_W150: + case XC430_W240: + return &items_XL; + case XM430_W210: + case XM430_W350: + return &items_XM; + case XM540_W150: + case XM540_W270: + return &items_EXTXM; + case XH430_V210: + case XH430_V350: + return &items_XH; + case XH540_W150: + case XH540_W270: + case XH540_V150: + case XH540_V270: + return &items_EXTXH; + case XW540_T260: + case XW540_T140: + return &items_XW; + case PRO_L42_10_S300_R: + return &items_PRO; + //TODO add missing dynamixels types + default: + return 0; + } + }*/ + + /* + std::map info_tables; + dynamixel::info_tables.insert(std::pair(AX_12A,&dynamixel::info_AX))); + dynamixel::info_tables.insert(std::pair(AX_12W,&dynamixel::info_AX)); + dynamixel::info_tables.insert(std::pair(AX_18A,&dynamixel::info_AX)); + + dynamixel::info_tables.insert(std::pair(RX_10,&dynamixel::info_RX)); + dynamixel::info_tables.insert(std::pair(RX_24F,&dynamixel::info_RX)); + dynamixel::info_tables.insert(std::pair(RX_28,&dynamixel::info_RX)); + dynamixel::info_tables.insert(std::pair(RX_64,&dynamixel::info_RX)); + + dynamixel::info_tables.insert(std::pair(EX_106,&dynamixel::info_EX)); + + dynamixel::info_tables.insert(std::pair(MX_12W,&dynamixel::info_MX)); + dynamixel::info_tables.insert(std::pair(MX_28,&dynamixel::info_MX)); + + dynamixel::info_tables.insert(std::pair(MX_28_2,&dynamixel::info_MX2)); + + dynamixel::info_tables.insert(std::pair(MX_64,&dynamixel::info_EXTMX)); + dynamixel::info_tables.insert(std::pair(MX_106,&dynamixel::info_EXTMX)); + + dynamixel::info_tables.insert(std::pair(MX_64_2,&dynamixel::info_EXTMX2)); + dynamixel::info_tables.insert(std::pair(MX_106_2,&dynamixel::info_EXTMX2)); + + dynamixel::info_tables.insert(std::pair(XL_320,&dynamixel::info_XL320)); + + dynamixel::info_tables.insert(std::pair(XL430_W250,&dynamixel::info_XL)); + dynamixel::info_tables.insert(std::pair(XL430_W250_2,&dynamixel::info_XL)); + dynamixel::info_tables.insert(std::pair(XC430_W150,&dynamixel::info_XL)); + dynamixel::info_tables.insert(std::pair(XC430_W240,&dynamixel::info_XL)); + + dynamixel::info_tables.insert(std::pair(XM430_W210,&dynamixel::info_XM)); + dynamixel::info_tables.insert(std::pair(XM430_W350,&dynamixel::info_XM)); + + dynamixel::info_tables.insert(std::pair(XM540_W150,&dynamixel::info_EXTXM)); + dynamixel::info_tables.insert(std::pair(XM540_W270,&dynamixel::info_EXTXM)); + + dynamixel::info_tables.insert(std::pair(XH430_V210,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH430_V350,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH430_W210,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH430_W350,&dynamixel::info_XH)); + + dynamixel::info_tables.insert(std::pair(XH540_W150,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH540_W270,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH540_V150,&dynamixel::info_XH)); + dynamixel::info_tables.insert(std::pair(XH540_W270,&dynamixel::info_XH)); + + dynamixel::info_tables.insert(std::pair(XW540_T260,&dynamixel::info_XW)); + dynamixel::info_tables.insert(std::pair(XW540_T140,&dynamixel::info_XW)); + + dynamixel::info_tables.insert(std::pair(PRO_L42_10_S300_R,&dynamixel::info_PRO)); + + dynamixel::info_tables.insert(std::pair(PRO_L54_30_S400_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_L54_30_S500_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_L54_50_S290_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_L54_50_S500_R,&dynamixel::info_EXTPRO)); + + dynamixel::info_tables.insert(std::pair(PRO_M42_10_S260_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_M54_40_S250_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_M54_60_S250_R,&dynamixel::info_EXTPRO)); + + dynamixel::info_tables.insert(std::pair(PRO_H42_20_S300_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_H54_100_S500_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_H54_200_S500_R,&dynamixel::info_EXTPRO)); + + dynamixel::info_tables.insert(std::pair(PRO_M42_10_S260_R_A,&dynamixel::info_EXTPRO_A)); + dynamixel::info_tables.insert(std::pair(PRO_M54_40_S250_R_A,&dynamixel::info_EXTPRO_A)); + dynamixel::info_tables.insert(std::pair(PRO_M54_60_S250_R_A,&dynamixel::info_EXTPRO_A)); + + dynamixel::info_tables.insert(std::pair(PRO_H42_20_S300_R_A,&dynamixel::info_EXTPRO_A)); + dynamixel::info_tables.insert(std::pair(PRO_H54_100_S500_R_A,&dynamixel::info_EXTPRO_A)); + dynamixel::info_tables.insert(std::pair(PRO_H54_200_S500_R_A,&dynamixel::info_EXTPRO_A)); + + dynamixel::info_tables.insert(std::pair(PRO_PLUS_M42P_010_S260_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_PLUS_M54P_040_S250_R,&dynamixel::info_EXTPRO)); + dynamixel::info_tables.insert(std::pair(PRO_PLUS_M54P_060_S250_R,&dynamixel::info_EXTPRO)); + + dynamixel::info_tables.insert(std::pair(PRO_PLUS_H42P_020_S300_R,&dynamixel::info_PRO_PLUS)); + dynamixel::info_tables.insert(std::pair(PRO_PLUS_H54P_100_S500_R,&dynamixel::info_PRO_PLUS)); + dynamixel::info_tables.insert(std::pair(PRO_PLUS_H54P_200_S500_R,&dynamixel::info_PRO_PLUS)); + + dynamixel::info_tables.insert(std::pair(RH_P12_RN,&dynamixel::info_Gripper)); + + dynamixel::info_tables.insert(std::pair(RH_P12_RN_A,&dynamixel::info_Gripper)); + + std::map*> control_tables; + control_tables.insert(std::pair*>(AX_12A,&dynamixel::items_AX)); + control_tables.insert(std::pair*>(AX_12W,&dynamixel::items_AX)); + control_tables.insert(std::pair*>(AX_18A,&dynamixel::items_AX)); + + control_tables.insert(std::pair*>(RX_10,&dynamixel::items_RX)); + control_tables.insert(std::pair*>(RX_24F,&dynamixel::items_RX)); + control_tables.insert(std::pair*>(RX_28,&dynamixel::items_RX)); + control_tables.insert(std::pair*>(RX_64,&dynamixel::items_RX)); + + control_tables.insert(std::pair*>(EX_106,&dynamixel::items_EX)); + + control_tables.insert(std::pair*>(MX_12W,&dynamixel::items_MX)); + control_tables.insert(std::pair*>(MX_28,&dynamixel::items_MX)); + + control_tables.insert(std::pair*>(MX_28_2,&dynamixel::items_MX2)); + + control_tables.insert(std::pair*>(MX_64,&dynamixel::items_EXTMX)); + control_tables.insert(std::pair*>(MX_106,&dynamixel::items_EXTMX)); + + control_tables.insert(std::pair*>(MX_64_2,&dynamixel::items_EXTMX2)); + control_tables.insert(std::pair*>(MX_106_2,&dynamixel::items_EXTMX2)); + + control_tables.insert(std::pair*>(XL_320,&dynamixel::items_XL320)); + + control_tables.insert(std::pair*>(XL430_W250,&dynamixel::items_XL)); + control_tables.insert(std::pair*>(XL430_W250_2,&dynamixel::items_XL)); + control_tables.insert(std::pair*>(XC430_W150,&dynamixel::items_XL)); + control_tables.insert(std::pair*>(XC430_W240,&dynamixel::items_XL)); + + control_tables.insert(std::pair*>(XM430_W210,&dynamixel::items_XM)); + control_tables.insert(std::pair*>(XM430_W350,&dynamixel::items_XM)); + + control_tables.insert(std::pair*>(XM540_W150,&dynamixel::items_EXTXM)); + control_tables.insert(std::pair*>(XM540_W270,&dynamixel::items_EXTXM)); + + control_tables.insert(std::pair*>(XH430_V210,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH430_V350,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH430_W210,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH430_W350,&dynamixel::items_XH)); + + control_tables.insert(std::pair*>(XH540_W150,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH540_W270,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH540_V150,&dynamixel::items_XH)); + control_tables.insert(std::pair*>(XH540_W270,&dynamixel::items_XH)); + + control_tables.insert(std::pair*>(XW540_T260,&dynamixel::items_XW)); + control_tables.insert(std::pair*>(XW540_T140,&dynamixel::items_XW)); + + control_tables.insert(std::pair*>(PRO_L42_10_S300_R,&dynamixel::items_PRO)); + + control_tables.insert(std::pair*>(PRO_L54_30_S400_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_L54_30_S500_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_L54_50_S290_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_L54_50_S500_R,&dynamixel::items_EXTPRO)); + + control_tables.insert(std::pair*>(PRO_M42_10_S260_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_M54_40_S250_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_M54_60_S250_R,&dynamixel::items_EXTPRO)); + + control_tables.insert(std::pair*>(PRO_H42_20_S300_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_H54_100_S500_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_H54_200_S500_R,&dynamixel::items_EXTPRO)); + + control_tables.insert(std::pair*>(PRO_M42_10_S260_R_A,&dynamixel::items_EXTPRO_A)); + control_tables.insert(std::pair*>(PRO_M54_40_S250_R_A,&dynamixel::items_EXTPRO_A)); + control_tables.insert(std::pair*>(PRO_M54_60_S250_R_A,&dynamixel::items_EXTPRO_A)); + + control_tables.insert(std::pair*>(PRO_H42_20_S300_R_A,&dynamixel::items_EXTPRO_A)); + control_tables.insert(std::pair*>(PRO_H54_100_S500_R_A,&dynamixel::items_EXTPRO_A)); + control_tables.insert(std::pair*>(PRO_H54_200_S500_R_A,&dynamixel::items_EXTPRO_A)); + + control_tables.insert(std::pair*>(PRO_PLUS_M42P_010_S260_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_PLUS_M54P_040_S250_R,&dynamixel::items_EXTPRO)); + control_tables.insert(std::pair*>(PRO_PLUS_M54P_060_S250_R,&dynamixel::items_EXTPRO)); + + control_tables.insert(std::pair*>(PRO_PLUS_H42P_020_S300_R,&dynamixel::items_PRO_PLUS)); + control_tables.insert(std::pair*>(PRO_PLUS_H54P_100_S500_R,&dynamixel::items_PRO_PLUS)); + control_tables.insert(std::pair*>(PRO_PLUS_H54P_200_S500_R,&dynamixel::items_PRO_PLUS)); + + control_tables.insert(std::pair*>(RH_P12_RN,&dynamixel::items_Gripper)); + + control_tables.insert(std::pair*>(RH_P12_RN_A,&dynamixel::items_EXTGripper)); +*/ +} \ No newline at end of file diff --git a/include/dynamixel_ros2_control/visiblity_control.h b/include/dynamixel_ros2_control/visiblity_control.h new file mode 100644 index 0000000..3126b11 --- /dev/null +++ b/include/dynamixel_ros2_control/visiblity_control.h @@ -0,0 +1,56 @@ +// Copyright 2020 Yutaka Kondo +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef DYNAMIXEL_HARDWARE_INTERFACE__VISIBLITY_CONTROL_H_ +#define DYNAMIXEL_HARDWARE_INTERFACE__VISIBLITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define DYNAMIXEL_HARDWARE_INTERFACE_EXPORT __attribute__((dllexport)) +#define DYNAMIXEL_HARDWARE_INTERFACE_IMPORT __attribute__((dllimport)) +#else +#define DYNAMIXEL_HARDWARE_INTERFACE_EXPORT __declspec(dllexport) +#define DYNAMIXEL_HARDWARE_INTERFACE_IMPORT __declspec(dllimport) +#endif +#ifdef DYNAMIXEL_HARDWARE_INTERFACE_BUILDING_DLL +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC DYNAMIXEL_HARDWARE_INTERFACE_EXPORT +#else +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC DYNAMIXEL_HARDWARE_INTERFACE_IMPORT +#endif +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC_TYPE DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC +#define DYNAMIXEL_HARDWARE_INTERFACE_LOCAL +#else +#define DYNAMIXEL_HARDWARE_INTERFACE_EXPORT __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_INTERFACE_IMPORT +#if __GNUC__ >= 4 +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_INTERFACE_LOCAL __attribute__((visibility("hidden"))) +#else +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC +#define DYNAMIXEL_HARDWARE_INTERFACE_LOCAL +#endif +#define DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC_TYPE +#endif + +#endif // DYNAMIXEL_HARDWARE_INTERFACE__VISIBLITY_CONTROL_H_ diff --git a/launch/dynamixel-example.launch.py b/launch/dynamixel-example.launch.py new file mode 100755 index 0000000..1244a61 --- /dev/null +++ b/launch/dynamixel-example.launch.py @@ -0,0 +1,59 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + robot_name = "my_robot" + package_name = robot_name + "_description" + robot_description = os.path.join(get_package_share_directory( + package_name), "urdf", robot_name + ".urdf") + robot_description_config = xacro.process_file(robot_description) + + controller_config = os.path.join( + get_package_share_directory( + package_name), "controllers", "controllers.yaml" + ) + + share_dir = get_package_share_directory(robot_name+'_bringup') + + + ld = LaunchDescription([ + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"robot_description": robot_description_config.toxml()}, controller_config], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), + + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ), + + #Node( + # package="controller_manager", + # executable="spawner", + # arguments=["velocity_controller", "-c", "/controller_manager"], + #), + + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], + ),]) + return ld \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..9378b35 --- /dev/null +++ b/package.xml @@ -0,0 +1,26 @@ + + + + dynamixel_ros2_control + 0.0.1 + Package containing nodes for controlling dynamixel motors + Nils Schulte + MIT + + Yutaka Kondo + Nils Schulte + + ament_cmake + + dynamixel_sdk + hardware_interface + pluginlib + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/dynamixel_ros2_control.cpp b/src/dynamixel_ros2_control.cpp new file mode 100644 index 0000000..ad54388 --- /dev/null +++ b/src/dynamixel_ros2_control.cpp @@ -0,0 +1,249 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "dynamixel_ros2_control/dynamixel_ros2_controll.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dynamixel_ros2_control/dynamixel_driver.hpp" + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#define DYN_LOGGER_NAME_STR "Dynamixel_Hardware_Interface" + +#define DYN_BAUD_PARAM_STR "baud_rate" +#define DYN_PORTNAME_PARAM_STR "serial_port" +#define DYN_ENABLE_TORQUE_ON_START_PARAM_STR "enable_torque_on_startup" +#define DYN_AUTO_REBOOT_PARAM_STR "reboot_on_error" +#define DYN_ID_PARAM_STR "id" + +#define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0" + +namespace dynamixel_ros2_control { + +CallbackReturn DynamixelHardwareInterface::on_init( + const hardware_interface::HardwareInfo &info) { + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "configure"); + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + auto serial_port_it = info_.hardware_parameters.find(DYN_PORTNAME_PARAM_STR); + std::string serial_port = (serial_port_it != info_.hardware_parameters.end() + ? serial_port_it->second + : std::string(DYN_DEFAULT_PORT_STR)); + auto baud_rate_it = info_.hardware_parameters.find(DYN_BAUD_PARAM_STR); + int baud_rate = std::stoi(baud_rate_it != info_.hardware_parameters.end() + ? baud_rate_it->second + : "57600"); + dynamixel_driver_ = + std::make_unique(serial_port, baud_rate); + RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Serial Port \"" + serial_port + + "\" (baud rate: " + std::to_string(baud_rate) + ")") + .c_str()); + + dynamixel_driver_->set_hardware_error_callbacks( + std::bind(&DynamixelHardwareInterface::overload_error_callback, this, + std::placeholders::_1), + std::bind(&DynamixelHardwareInterface::input_voltage_error_callback, this, + std::placeholders::_1), + std::bind(&DynamixelHardwareInterface::overheating_error_callback, this, + std::placeholders::_1), + std::bind(&DynamixelHardwareInterface::electrical_shock_error_callback, + this, std::placeholders::_1), + std::bind(&DynamixelHardwareInterface::motor_encoder_error_callback, this, + std::placeholders::_1)); + + std::map motorIds; + for (const auto &joint : info_.joints) { + motorIds[joint.name] = std::stoi(joint.parameters.at(DYN_ID_PARAM_STR)); + auto auto_reboot = joint.parameters.find(DYN_AUTO_REBOOT_PARAM_STR); + auto_clear_overload_error[joint.name] = + (auto_reboot != joint.parameters.end()) + ? (auto_reboot->second == "true") + : false; + } + + bool failure = false; + for (auto const &x : motorIds) { + try { + dynamixel_driver_->add_motor(x.first, x.second); + RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Joint-Actuator \"" + std::string(x.first) + + "\" (ID: " + std::to_string(x.second) + ") added") + .c_str()); + } catch (const std::exception &e) { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Joint-Actuator \"" + std::string(x.first) + + "\"(ID: " + std::to_string(x.second) + + ") could not be added! \n" + e.what()) + .c_str()); + failure = true; + } + } + + RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + (std::string("Motors count: ") + + std::to_string(dynamixel_driver_->get_motor_count())) + .c_str()); + if (failure) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +void DynamixelHardwareInterface::input_voltage_error_callback( + const std::string &joint_name) const { + RCLCPP_ERROR( + rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("input voltage of joint \"" + joint_name + "\" not correct!").c_str()); +} +void DynamixelHardwareInterface::overheating_error_callback( + const std::string &joint_name) const { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Joint \"" + joint_name + "\" is overheating!").c_str()); +} +void DynamixelHardwareInterface::motor_encoder_error_callback( + const std::string &joint_name) const { + RCLCPP_ERROR( + rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Motor encoder error on joint \"" + joint_name + "\"!").c_str()); +} +void DynamixelHardwareInterface::electrical_shock_error_callback( + const std::string &joint_name) const { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Electrical shock on joint \"" + joint_name + "\": !").c_str()); +} +void DynamixelHardwareInterface::overload_error_callback( + const std::string &joint_name) const { + RCLCPP_ERROR( + rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("Mechanical overload error on joint \"" + joint_name + "\": !").c_str()); + if (auto_clear_overload_error.at(joint_name)) { + try { + dynamixel_driver_->reboot(joint_name); + RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + ("\"" + joint_name + " rebootet").c_str()); + // succeeded = true; + } catch (std::invalid_argument &e) { + RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what()); + } + } +} + +std::vector +DynamixelHardwareInterface::export_state_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + "export_state_interfaces"); + std::vector state_interfaces; + dynamixel_driver_->for_each_joint([&](const std::string &joint_name) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_name, hardware_interface::HW_IF_POSITION, + dynamixel_driver_->get_position_ptr(joint_name))); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_name, hardware_interface::HW_IF_VELOCITY, + dynamixel_driver_->get_velocity_ptr(joint_name))); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_name, hardware_interface::HW_IF_EFFORT, + dynamixel_driver_->get_effort_ptr(joint_name))); + }); + return state_interfaces; +} + +std::vector +DynamixelHardwareInterface::export_command_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + "export_command_interfaces"); + std::vector command_interfaces; + dynamixel_driver_->for_each_joint([&](const std::string &joint_name) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_name, hardware_interface::HW_IF_POSITION, + dynamixel_driver_->get_goal_position_ptr(joint_name))); + }); + return command_interfaces; +} + +CallbackReturn DynamixelHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "on_activate"); + try { + dynamixel_driver_->ping_all(); + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), + "Pinging dynamixels was successfull"); + } catch (const dynamixel::Driver::dynamixel_bus_error &e) { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what()); + return CallbackReturn::ERROR; + } + + try { + dynamixel_driver_->read(); + dynamixel_driver_->set_torque_all(true); + dynamixel_driver_->write(); + } catch (std::invalid_argument &e) { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what()); + return CallbackReturn::ERROR; + } catch (const dynamixel::Driver::hardware_error &e) { + RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "on_deactivate"); + try { + dynamixel_driver_->read(); + dynamixel_driver_->set_torque_all(false); + dynamixel_driver_->write(); + } catch (std::invalid_argument &e) { + RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what()); + return CallbackReturn::ERROR; + } catch (const dynamixel::Driver::hardware_error &e) { + RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type DynamixelHardwareInterface::read() { + dynamixel_driver_->read(); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type DynamixelHardwareInterface::write() { + dynamixel_driver_->write(); + return hardware_interface::return_type::OK; +} + +} // namespace dynamixel_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(dynamixel_ros2_control::DynamixelHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file