diff --git a/ada_hardware/CMakeLists.txt b/ada_hardware/CMakeLists.txt index 8b4534d..0a43d51 100644 --- a/ada_hardware/CMakeLists.txt +++ b/ada_hardware/CMakeLists.txt @@ -14,16 +14,18 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(pluginlib REQUIRED) find_package(hardware_interface REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(dynamixel_workbench_toolbox REQUIRED) add_library(ada_hardware SHARED src/jaco2.cpp - src/isaacsim.cpp) + src/isaacsim.cpp + src/dynamixel_hardware.cpp) target_include_directories(ada_hardware PUBLIC include) target_link_libraries(ada_hardware -l:Kinova.API.CommLayerUbuntu.so -l:Kinova.API.USBCommandLayerUbuntu.so) -ament_target_dependencies(ada_hardware rclcpp rclcpp_lifecycle pluginlib hardware_interface sensor_msgs) +ament_target_dependencies(ada_hardware rclcpp rclcpp_lifecycle lifecycle_msgs pluginlib hardware_interface sensor_msgs dynamixel_workbench_toolbox) pluginlib_export_plugin_description_file(hardware_interface ada_hardware.xml) @@ -72,7 +74,17 @@ if(BUILD_TESTING) hardware_interface sensor_msgs ) + + ament_add_gmock(test_dynamixel test/test_dynamixel.cpp) + ament_target_dependencies(test_dynamixel + pluginlib + ros2_control_test_assets + rclcpp + rclcpp_lifecycle + hardware_interface + sensor_msgs + ) endif() -ament_export_dependencies(rclcpp rclcpp_lifecycle pluginlib hardware_interface) +ament_export_dependencies(rclcpp rclcpp_lifecycle pluginlib hardware_interface dynamixel_workbench_toolbox) ament_package() diff --git a/ada_hardware/ada_hardware.xml b/ada_hardware/ada_hardware.xml index 5550a63..3fd99d1 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -19,4 +19,12 @@ Interface for controlling a 6DOF arm (+ 2 fingers) in IsaacSim. + + + + The ROBOTIS Dynamixel system interface. + + diff --git a/ada_hardware/config/ada_hardware_forward_controllers.yaml b/ada_hardware/config/ada_hardware_forward_controllers.yaml index 1371d0b..2b1a6b9 100644 --- a/ada_hardware/config/ada_hardware_forward_controllers.yaml +++ b/ada_hardware/config/ada_hardware_forward_controllers.yaml @@ -28,6 +28,8 @@ forward_position_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 forward_velocity_controller: ros__parameters: @@ -40,6 +42,8 @@ forward_velocity_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 forward_effort_controller: ros__parameters: @@ -52,3 +56,5 @@ forward_effort_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 diff --git a/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp new file mode 100644 index 0000000..ef93f07 --- /dev/null +++ b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp @@ -0,0 +1,112 @@ +// 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. + +#ifndef DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ +#define DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include + +#include "ada_hardware/visiblity_control.h" +#include "rclcpp/macros.hpp" + +using hardware_interface::CallbackReturn; +using hardware_interface::return_type; + +namespace dynamixel_hardware { +struct JointValue { + double position{0.0}; + double velocity{0.0}; + double effort{0.0}; +}; + +struct Joint { + JointValue state{}; + JointValue command{}; + JointValue prev_command{}; +}; + +enum class ControlMode { + Position, + Velocity, + Torque, + Current, + ExtendedPosition, + MultiTurn, + CurrentBasedPosition, + PWM, +}; + +class DynamixelHardware : public hardware_interface::SystemInterface { +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardware) + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + + DYNAMIXEL_HARDWARE_PUBLIC + std::vector + export_state_interfaces() override; + + DYNAMIXEL_HARDWARE_PUBLIC + std::vector + export_command_interfaces() override; + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_PUBLIC + return_type read(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + DYNAMIXEL_HARDWARE_PUBLIC + return_type write(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + +private: + return_type enable_torque(const bool enabled); + + return_type set_control_mode(const ControlMode &mode, + const bool force_set = false); + + return_type reset_command(); + + CallbackReturn set_joint_positions(); + CallbackReturn set_joint_velocities(); + CallbackReturn set_joint_params(); + + DynamixelWorkbench dynamixel_workbench_; + std::map control_items_; + std::vector joints_; + std::vector joint_ids_; + bool torque_enabled_{false}; + ControlMode control_mode_{ControlMode::Position}; + bool mode_changed_{false}; + bool use_dummy_{false}; +}; +} // namespace dynamixel_hardware + +#endif // DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ diff --git a/ada_hardware/include/ada_hardware/visiblity_control.h b/ada_hardware/include/ada_hardware/visiblity_control.h new file mode 100644 index 0000000..3b842bb --- /dev/null +++ b/ada_hardware/include/ada_hardware/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__VISIBLITY_CONTROL_H_ +#define DYNAMIXEL_HARDWARE__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_EXPORT __attribute__((dllexport)) +#define DYNAMIXEL_HARDWARE_IMPORT __attribute__((dllimport)) +#else +#define DYNAMIXEL_HARDWARE_EXPORT __declspec(dllexport) +#define DYNAMIXEL_HARDWARE_IMPORT __declspec(dllimport) +#endif +#ifdef DYNAMIXEL_HARDWARE_BUILDING_DLL +#define DYNAMIXEL_HARDWARE_PUBLIC DYNAMIXEL_HARDWARE_EXPORT +#else +#define DYNAMIXEL_HARDWARE_PUBLIC DYNAMIXEL_HARDWARE_IMPORT +#endif +#define DYNAMIXEL_HARDWARE_PUBLIC_TYPE DYNAMIXEL_HARDWARE_PUBLIC +#define DYNAMIXEL_HARDWARE_LOCAL +#else +#define DYNAMIXEL_HARDWARE_EXPORT __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_IMPORT +#if __GNUC__ >= 4 +#define DYNAMIXEL_HARDWARE_PUBLIC __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_LOCAL __attribute__((visibility("hidden"))) +#else +#define DYNAMIXEL_HARDWARE_PUBLIC +#define DYNAMIXEL_HARDWARE_LOCAL +#endif +#define DYNAMIXEL_HARDWARE_PUBLIC_TYPE +#endif + +#endif // DYNAMIXEL_HARDWARE__VISIBLITY_CONTROL_H_ diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index 400a4ce..64bec98 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -98,9 +98,10 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "use_forque", - default_value="true", - description="Whether to include F/T sensor.", + "end_effector_tool", + default_value="none", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], ) ) declared_arguments.append( @@ -140,7 +141,7 @@ def generate_launch_description(): description_file = LaunchConfiguration("description_file") sim = LaunchConfiguration("sim") readonly = LaunchConfiguration("readonly") - use_forque = LaunchConfiguration("use_forque") + end_effector_tool = LaunchConfiguration("end_effector_tool") robot_controller = LaunchConfiguration("robot_controller") start_controller = LaunchConfiguration("start_controller") start_rviz = LaunchConfiguration("start_rviz") @@ -162,8 +163,8 @@ def generate_launch_description(): "readonly:=", readonly, " ", - "use_forque:=", - use_forque, + "end_effector_tool:=", + end_effector_tool, ] ) robot_description = { diff --git a/ada_hardware/package.xml b/ada_hardware/package.xml index 2a31563..3a272fd 100644 --- a/ada_hardware/package.xml +++ b/ada_hardware/package.xml @@ -2,7 +2,7 @@ ada_hardware - 0.0.1 + 0.0.2 ROS Hardware Interface for ADA Ethan K. Gordon BSD-3-Clause @@ -12,8 +12,10 @@ rclcpp rclcpp_lifecycle pluginlib + lifecycle_msgs hardware_interface sensor_msgs + dynamixel_workbench_toolbox ament_lint_auto ament_cmake_copyright diff --git a/ada_hardware/src/dynamixel_hardware.cpp b/ada_hardware/src/dynamixel_hardware.cpp new file mode 100644 index 0000000..edf2d4a --- /dev/null +++ b/ada_hardware/src/dynamixel_hardware.cpp @@ -0,0 +1,525 @@ +// 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. + +#include "ada_hardware/dynamixel_hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace dynamixel_hardware { +constexpr const char *kDynamixelHardware = "DynamixelHardware"; +constexpr uint8_t kGoalPositionIndex = 0; +constexpr uint8_t kGoalVelocityIndex = 1; +constexpr uint8_t kPresentPositionVelocityCurrentIndex = 0; +constexpr const char *kGoalPositionItem = "Goal_Position"; +constexpr const char *kGoalVelocityItem = "Goal_Velocity"; +constexpr const char *kMovingSpeedItem = "Moving_Speed"; +constexpr const char *kPresentPositionItem = "Present_Position"; +constexpr const char *kPresentVelocityItem = "Present_Velocity"; +constexpr const char *kPresentSpeedItem = "Present_Speed"; +constexpr const char *kPresentCurrentItem = "Present_Current"; +constexpr const char *kPresentLoadItem = "Present_Load"; +constexpr const char *const kExtraJointParameters[] = { + "Profile_Velocity", "Profile_Acceleration", "Position_P_Gain", + "Position_I_Gain", "Position_D_Gain", "Velocity_P_Gain", + "Velocity_I_Gain", +}; + +CallbackReturn +DynamixelHardware::on_init(const hardware_interface::HardwareInfo &info) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure"); + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + joints_.resize(info_.joints.size(), Joint()); + joint_ids_.resize(info_.joints.size(), 0); + + for (uint i = 0; i < info_.joints.size(); i++) { + joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id")); + joints_[i].state.position = std::numeric_limits::quiet_NaN(); + joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); + joints_[i].state.effort = std::numeric_limits::quiet_NaN(); + joints_[i].command.position = std::numeric_limits::quiet_NaN(); + joints_[i].command.velocity = std::numeric_limits::quiet_NaN(); + joints_[i].command.effort = std::numeric_limits::quiet_NaN(); + joints_[i].prev_command.position = joints_[i].command.position; + joints_[i].prev_command.velocity = joints_[i].command.velocity; + joints_[i].prev_command.effort = joints_[i].command.effort; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, + joint_ids_[i]); + } + + if (info_.hardware_parameters.find("use_dummy") != + info_.hardware_parameters.end() && + info_.hardware_parameters.at("use_dummy") == "true") { + use_dummy_ = true; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode"); + return CallbackReturn::SUCCESS; + } + + auto usb_port = info_.hardware_parameters.at("usb_port"); + auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate")); + const char *log = nullptr; + + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", + usb_port.c_str()); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", + baud_rate); + + if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + for (uint i = 0; i < info_.joints.size(); ++i) { + uint16_t model_number = 0; + if (!dynamixel_workbench_.ping(joint_ids_[i], &model_number, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + } + + enable_torque(false); + set_control_mode(ControlMode::Position, true); + set_joint_params(); + enable_torque(true); + + const ControlItem *goal_position = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem); + if (goal_position == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *goal_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalVelocityItem); + if (goal_velocity == nullptr) { + goal_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kMovingSpeedItem); + } + if (goal_velocity == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_position = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentPositionItem); + if (present_position == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentVelocityItem); + if (present_velocity == nullptr) { + present_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentSpeedItem); + } + if (present_velocity == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentCurrentItem); + if (present_current == nullptr) { + present_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentLoadItem); + } + if (present_current == nullptr) { + return CallbackReturn::ERROR; + } + + control_items_[kGoalPositionItem] = goal_position; + control_items_[kGoalVelocityItem] = goal_velocity; + control_items_[kPresentPositionItem] = present_position; + control_items_[kPresentVelocityItem] = present_velocity; + control_items_[kPresentCurrentItem] = present_current; + + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalPositionItem]->address, + control_items_[kGoalPositionItem]->data_length, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalVelocityItem]->address, + control_items_[kGoalVelocityItem]->data_length, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + uint16_t start_address = + std::min(control_items_[kPresentPositionItem]->address, + control_items_[kPresentCurrentItem]->address); + uint16_t read_length = control_items_[kPresentPositionItem]->data_length + + control_items_[kPresentVelocityItem]->data_length + + control_items_[kPresentCurrentItem]->data_length + 2; + if (!dynamixel_workbench_.addSyncReadHandler(start_address, read_length, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +std::vector +DynamixelHardware::export_state_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), + "export_state_interfaces"); + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &joints_[i].state.position)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &joints_[i].state.velocity)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, + &joints_[i].state.effort)); + } + + return state_interfaces; +} + +std::vector +DynamixelHardware::export_command_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), + "export_command_interfaces"); + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &joints_[i].command.position)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &joints_[i].command.velocity)); + } + + return command_interfaces; +} + +CallbackReturn DynamixelHardware::on_activate( + const rclcpp_lifecycle::State & /* previous_state */) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); + for (uint i = 0; i < joints_.size(); i++) { + if (use_dummy_ && std::isnan(joints_[i].state.position)) { + joints_[i].state.position = 0.0; + joints_[i].state.velocity = 0.0; + joints_[i].state.effort = 0.0; + } + } + read(rclcpp::Time{}, rclcpp::Duration(0, 0)); + reset_command(); + write(rclcpp::Time{}, rclcpp::Duration(0, 0)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop"); + return CallbackReturn::SUCCESS; +} + +return_type DynamixelHardware::read(const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { + if (use_dummy_) { + return return_type::OK; + } + + std::vector ids(info_.joints.size(), 0); + std::vector positions(info_.joints.size(), 0); + std::vector velocities(info_.joints.size(), 0); + std::vector currents(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + const char *log = nullptr; + + if (!dynamixel_workbench_.syncRead(kPresentPositionVelocityCurrentIndex, + ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + for (uint i = 0; i < ids.size(); i++) { + joints_[i].state.position = + dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); + joints_[i].state.velocity = + dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); + joints_[i].state.effort = + dynamixel_workbench_.convertValue2Current(currents[i]); + } + + return return_type::OK; +} + +return_type DynamixelHardware::write(const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { + if (use_dummy_) { + for (auto &joint : joints_) { + joint.prev_command.position = joint.command.position; + joint.state.position = joint.command.position; + } + return return_type::OK; + } + + // Velocity control + if (std::any_of(joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.velocity != j.prev_command.velocity; + })) { + set_control_mode(ControlMode::Velocity); + if (mode_changed_) { + set_joint_params(); + } + set_joint_velocities(); + return return_type::OK; + } + + // Position control + if (std::any_of(joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.position != j.prev_command.position; + })) { + set_control_mode(ControlMode::Position); + if (mode_changed_) { + set_joint_params(); + } + set_joint_positions(); + return return_type::OK; + } + + // Effort control + if (std::any_of(joints_.cbegin(), joints_.cend(), + [](auto j) { return j.command.effort != 0.0; })) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), + "Effort control is not implemented"); + return return_type::ERROR; + } + + // If all command values are unchanged, then remain in existing control mode + // and set corresponding command values + switch (control_mode_) { + case ControlMode::Velocity: + set_joint_velocities(); + return return_type::OK; + break; + case ControlMode::Position: + set_joint_positions(); + return return_type::OK; + break; + default: // effort, etc + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), + "Control mode not implemented"); + return return_type::ERROR; + break; + } +} + +return_type DynamixelHardware::enable_torque(const bool enabled) { + const char *log = nullptr; + + if (enabled && !torque_enabled_) { + for (uint i = 0; i < info_.joints.size(); ++i) { + if (!dynamixel_workbench_.torqueOn(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + reset_command(); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque enabled"); + } else if (!enabled && torque_enabled_) { + for (uint i = 0; i < info_.joints.size(); ++i) { + if (!dynamixel_workbench_.torqueOff(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque disabled"); + } + + torque_enabled_ = enabled; + return return_type::OK; +} + +return_type DynamixelHardware::set_control_mode(const ControlMode &mode, + const bool force_set) { + const char *log = nullptr; + mode_changed_ = false; + + if (mode == ControlMode::Velocity && + (force_set || control_mode_ != ControlMode::Velocity)) { + bool torque_enabled = torque_enabled_; + if (torque_enabled) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); + if (control_mode_ != ControlMode::Velocity) { + mode_changed_ = true; + control_mode_ = ControlMode::Velocity; + } + + if (torque_enabled) { + enable_torque(true); + } + return return_type::OK; + } + + if (mode == ControlMode::Position && + (force_set || control_mode_ != ControlMode::Position)) { + bool torque_enabled = torque_enabled_; + if (torque_enabled) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); + if (control_mode_ != ControlMode::Position) { + mode_changed_ = true; + control_mode_ = ControlMode::Position; + } + + if (torque_enabled) { + enable_torque(true); + } + return return_type::OK; + } + + if (control_mode_ != ControlMode::Velocity && + control_mode_ != ControlMode::Position) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), + "Only position/velocity control are implemented"); + return return_type::ERROR; + } + + return return_type::OK; +} + +return_type DynamixelHardware::reset_command() { + for (uint i = 0; i < joints_.size(); i++) { + joints_[i].command.position = joints_[i].state.position; + joints_[i].command.velocity = 0.0; + joints_[i].command.effort = 0.0; + joints_[i].prev_command.position = joints_[i].command.position; + joints_[i].prev_command.velocity = joints_[i].command.velocity; + joints_[i].prev_command.effort = joints_[i].command.effort; + } + + return return_type::OK; +} + +CallbackReturn DynamixelHardware::set_joint_positions() { + const char *log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + for (uint i = 0; i < ids.size(); i++) { + joints_[i].prev_command.position = joints_[i].command.position; + commands[i] = dynamixel_workbench_.convertRadian2Value( + ids[i], static_cast(joints_[i].command.position)); + } + if (!dynamixel_workbench_.syncWrite(kGoalPositionIndex, ids.data(), + ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_velocities() { + const char *log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + for (uint i = 0; i < ids.size(); i++) { + joints_[i].prev_command.velocity = joints_[i].command.velocity; + commands[i] = dynamixel_workbench_.convertVelocity2Value( + ids[i], static_cast(joints_[i].command.velocity)); + } + if (!dynamixel_workbench_.syncWrite(kGoalVelocityIndex, ids.data(), + ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_params() { + const char *log = nullptr; + for (uint i = 0; i < info_.joints.size(); ++i) { + for (auto paramName : kExtraJointParameters) { + if (info_.joints[i].parameters.find(paramName) != + info_.joints[i].parameters.end()) { + auto value = std::stoi(info_.joints[i].parameters.at(paramName)); + if (!dynamixel_workbench_.itemWrite(joint_ids_[i], paramName, value, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), + "%s set to %d for joint %d", paramName, value, i); + } + } + } + return CallbackReturn::SUCCESS; +} + +} // namespace dynamixel_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(dynamixel_hardware::DynamixelHardware, + hardware_interface::SystemInterface) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp new file mode 100644 index 0000000..b3b8f6a --- /dev/null +++ b/ada_hardware/test/test_dynamixel.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 Personal Robotics Lab, University of Washington +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// Author: Jose Jaime + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include +#include + +namespace { +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +} // namespace + +class TestDynamixelHardware : public ::testing::Test { +protected: + void SetUp() override { + hardware_system_articulable_fork_ = + R"( + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + 1 + + + + + + + + 2 + + + + + + + +)"; + } + + std::string hardware_system_articulable_fork_; +}; + +// Forward declaration +namespace hardware_interface { +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager { +public: + friend TestDynamixelHardware; + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager(const std::string &urdf, + bool validate_interfaces = true, + bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, + activate_all) {} +}; + +TEST_F(TestDynamixelHardware, load_articulable_fork) { + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_articulable_fork_ + + ros2_control_test_assets::urdf_tail; + try { + TestableResourceManager rm(urdf); + SUCCEED(); + } catch (std::exception const &err) { + FAIL() << err.what(); + } +} diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index 9b08d1a..09cc970 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -107,4 +107,43 @@ + + + + + mock_components/GenericSystem + false + 0.0 + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + + + 1 + + + + + + + + + + 2 + + + + + + + + + + + + diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 3f79482..62ac5d5 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -7,14 +7,25 @@ + + + + + + + +