From 78bf8d1e9ee2b50570e324e33d52b5ae44991773 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 11:28:37 -0800 Subject: [PATCH 01/15] Add hardware interface for Dynamixels --- ada_description/rviz/view_robot.rviz | 25 + ada_hardware/CMakeLists.txt | 8 +- ada_hardware/ada_hardware.xml | 8 + .../ada_hardware_forward_controllers.yaml | 6 + .../ada_hardware/dynamixel_hardware.hpp | 112 ++++ .../include/ada_hardware/visiblity_control.h | 56 ++ ada_hardware/launch/ada_hardware.launch.py | 7 + ada_hardware/package.xml | 4 +- ada_hardware/src/dynamixel_hardware.cpp | 525 ++++++++++++++++++ .../urdf/ada_hardware.ros2_control.xacro | 43 +- ada_hardware/urdf/ada_hardware.xacro | 6 + 11 files changed, 795 insertions(+), 5 deletions(-) create mode 100644 ada_hardware/include/ada_hardware/dynamixel_hardware.hpp create mode 100644 ada_hardware/include/ada_hardware/visiblity_control.h create mode 100644 ada_hardware/src/dynamixel_hardware.cpp diff --git a/ada_description/rviz/view_robot.rviz b/ada_description/rviz/view_robot.rviz index d51156d..7fd900c 100644 --- a/ada_description/rviz/view_robot.rviz +++ b/ada_description/rviz/view_robot.rviz @@ -67,6 +67,31 @@ Visualization Manager: Expand Joint Details: false Expand Link Details: false Expand Tree: false + AFBase: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + AFMotor1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + AFMotorFixture1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + AFMotor2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + AFFTAdapter: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true FT: Alpha: 1 Show Axes: false diff --git a/ada_hardware/CMakeLists.txt b/ada_hardware/CMakeLists.txt index 8b4534d..14171e5 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) @@ -74,5 +76,5 @@ if(BUILD_TESTING) ) 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 2604cb8..dc09be7 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -14,4 +14,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 5cb5a33..42f504b 100644 --- a/ada_hardware/config/ada_hardware_forward_controllers.yaml +++ b/ada_hardware/config/ada_hardware_forward_controllers.yaml @@ -25,6 +25,8 @@ forward_position_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - AFMotor1_to_AFBase + - AFFTAdapter_to_AFMotor2 forward_velocity_controller: ros__parameters: @@ -37,6 +39,8 @@ forward_velocity_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - AFMotor1_to_AFBase + - AFFTAdapter_to_AFMotor2 forward_effort_controller: ros__parameters: @@ -49,3 +53,5 @@ forward_effort_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - AFMotor1_to_AFBase + - AFFTAdapter_to_AFMotor2 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..846a7af --- /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, + Currrent, + 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 71c9601..bb30475 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -189,6 +189,12 @@ def generate_launch_description(): output="both", on_exit=Shutdown(), ) + # joint_state_pub_node = Node( + # package="joint_state_publisher_gui", + # executable="joint_state_publisher_gui", + # on_exit=Shutdown(), + # ) + robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -255,6 +261,7 @@ def generate_launch_description(): nodes = [ control_node, robot_state_pub_node, + # joint_state_pub_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, 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/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index f53c32e..0cbc2c3 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -107,4 +107,45 @@ - \ No newline at end of file + + + + + mock_components/GenericSystem + false + 0.0 + + + ada_hardware/Dynamixel + + + + + + + 1 + + + + + + + + + + + 2 + + + + + + + + + + + + + + diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 3f79482..5198a90 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -7,6 +7,7 @@ + @@ -17,4 +18,9 @@ sim="$(arg sim)" readonly="$(arg readonly)" /> + + From 04c11b273b2b40396cbaff0b4eb1f8760fdd8706 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 13:48:31 -0800 Subject: [PATCH 02/15] Remove lingering comments --- ada_hardware/launch/ada_hardware.launch.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index bb30475..d0c523a 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -189,11 +189,6 @@ def generate_launch_description(): output="both", on_exit=Shutdown(), ) - # joint_state_pub_node = Node( - # package="joint_state_publisher_gui", - # executable="joint_state_publisher_gui", - # on_exit=Shutdown(), - # ) robot_state_pub_node = Node( package="robot_state_publisher", @@ -261,7 +256,6 @@ def generate_launch_description(): nodes = [ control_node, robot_state_pub_node, - # joint_state_pub_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, From 1987de5bfe1cce971c306d28488669e89889bf01 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 15:30:23 -0800 Subject: [PATCH 03/15] Add WIP unit test for Dynamixel hardware interface --- ada_hardware/CMakeLists.txt | 10 +++ ada_hardware/test/test_dynamixel.cpp | 103 +++++++++++++++++++++++++++ 2 files changed, 113 insertions(+) create mode 100644 ada_hardware/test/test_dynamixel.cpp diff --git a/ada_hardware/CMakeLists.txt b/ada_hardware/CMakeLists.txt index 14171e5..0a43d51 100644 --- a/ada_hardware/CMakeLists.txt +++ b/ada_hardware/CMakeLists.txt @@ -74,6 +74,16 @@ 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 dynamixel_workbench_toolbox) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp new file mode 100644 index 0000000..5118b42 --- /dev/null +++ b/ada_hardware/test/test_dynamixel.cpp @@ -0,0 +1,103 @@ +// 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 TestDynamixel : public ::testing::Test { +protected: + void SetUp() override { + hardware_system_articulable_fork_ = + R"( + + + ada_hardware/Dynamixel + /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 TestDynamixel; + + 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(TestDynamixel, 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(); + } +} From 9b179914d6ba9a9b77b5daf5fa6875c6fd831e63 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 20:35:13 -0800 Subject: [PATCH 04/15] Rename hardware interface Dynamixel to DynamixelHardware to match implementation --- ada_hardware/ada_hardware.xml | 4 ++-- .../config/ada_hardware_forward_controllers.yaml | 12 ++++++------ ada_hardware/test/test_dynamixel.cpp | 13 ++++++------- ada_hardware/urdf/ada_hardware.ros2_control.xacro | 4 +--- ada_hardware/urdf/ada_hardware.xacro | 2 +- 5 files changed, 16 insertions(+), 19 deletions(-) diff --git a/ada_hardware/ada_hardware.xml b/ada_hardware/ada_hardware.xml index dc09be7..ccfb28e 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -15,8 +15,8 @@ - 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 42f504b..8c39258 100644 --- a/ada_hardware/config/ada_hardware_forward_controllers.yaml +++ b/ada_hardware/config/ada_hardware_forward_controllers.yaml @@ -25,8 +25,8 @@ forward_position_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 - - AFMotor1_to_AFBase - - AFFTAdapter_to_AFMotor2 + - af_joint_1 + - af_joint_2 forward_velocity_controller: ros__parameters: @@ -39,8 +39,8 @@ forward_velocity_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 - - AFMotor1_to_AFBase - - AFFTAdapter_to_AFMotor2 + - af_joint_1 + - af_joint_2 forward_effort_controller: ros__parameters: @@ -53,5 +53,5 @@ forward_effort_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 - - AFMotor1_to_AFBase - - AFFTAdapter_to_AFMotor2 + - af_joint_1 + - af_joint_2 diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp index 5118b42..b10edc2 100644 --- a/ada_hardware/test/test_dynamixel.cpp +++ b/ada_hardware/test/test_dynamixel.cpp @@ -37,18 +37,17 @@ const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); } // namespace -class TestDynamixel : public ::testing::Test { +class TestDynamixelHardware : public ::testing::Test { protected: void SetUp() override { hardware_system_articulable_fork_ = R"( - ada_hardware/Dynamixel - /dev/ttyUSB0 - 1000000 + ada_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 - 1 @@ -79,7 +78,7 @@ class ResourceStorage; class TestableResourceManager : public hardware_interface::ResourceManager { public: - friend TestDynamixel; + friend TestDynamixelHardware; TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -90,7 +89,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager { activate_all) {} }; -TEST_F(TestDynamixel, load_articulable_fork) { +TEST_F(TestDynamixelHardware, load_articulable_fork) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_articulable_fork_ + ros2_control_test_assets::urdf_tail; diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index 0cbc2c3..182c1de 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -116,7 +116,7 @@ 0.0 - ada_hardware/Dynamixel + ada_hardware/DynamixelHardware @@ -127,7 +127,6 @@ - @@ -138,7 +137,6 @@ - diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 5198a90..1ba3be4 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -19,7 +19,7 @@ readonly="$(arg readonly)" /> From f25ed9d6d288c2e458e4ce8e2e26879617ffa188 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 21:18:58 -0800 Subject: [PATCH 05/15] Rename port to usb_port --- ada_hardware/urdf/ada_hardware.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index 182c1de..f9f9d80 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -117,7 +117,7 @@ ada_hardware/DynamixelHardware - + From 97fc98a1543f23427840a8713904e5a48372f96e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 21:42:10 -0800 Subject: [PATCH 06/15] Fix parameter definitions for usb_port and baud_rate, rename joints --- ada_hardware/urdf/ada_hardware.ros2_control.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index f9f9d80..35a5ecc 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -117,12 +117,12 @@ ada_hardware/DynamixelHardware - - + /dev/ttyUSB0 + 1000000 - + 1 @@ -132,7 +132,7 @@ - + 2 From 51b0fd7320df0b1a2f41e5dd44ed943b33a137d8 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Nov 2024 22:07:52 -0800 Subject: [PATCH 07/15] Adjust line spacing --- ada_hardware/test/test_dynamixel.cpp | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp index b10edc2..c326a0f 100644 --- a/ada_hardware/test/test_dynamixel.cpp +++ b/ada_hardware/test/test_dynamixel.cpp @@ -41,7 +41,7 @@ class TestDynamixelHardware : public ::testing::Test { protected: void SetUp() override { hardware_system_articulable_fork_ = - R"( + R"( ada_hardware/DynamixelHardware @@ -50,19 +50,21 @@ class TestDynamixelHardware : public ::testing::Test { 1 - - - - - + + + + + + - + 2 - - - - - + + + + + + )"; From 68087b0ce0d4f2d31f54b10f7fc06e23f5f4b1b8 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Nov 2024 10:32:00 -0800 Subject: [PATCH 08/15] Fix mis-named joint to af_joint_2 --- ada_hardware/test/test_dynamixel.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp index c326a0f..750a3d9 100644 --- a/ada_hardware/test/test_dynamixel.cpp +++ b/ada_hardware/test/test_dynamixel.cpp @@ -41,7 +41,7 @@ class TestDynamixelHardware : public ::testing::Test { protected: void SetUp() override { hardware_system_articulable_fork_ = - R"( + R"( ada_hardware/DynamixelHardware @@ -52,16 +52,14 @@ class TestDynamixelHardware : public ::testing::Test { 1 - - + 2 - From c0072fce05d343f35c0fb77bd6e1355a678f3f82 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Nov 2024 08:47:19 -0800 Subject: [PATCH 09/15] Rename ada_hardware::DynamixelHardware to dynamixel_hardware::DynamixelHardware --- ada_hardware/ada_hardware.xml | 4 ++-- ada_hardware/urdf/ada_hardware.ros2_control.xacro | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ada_hardware/ada_hardware.xml b/ada_hardware/ada_hardware.xml index ccfb28e..5c301ff 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -15,8 +15,8 @@ - The ROBOTIS Dynamixel system interface. diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index 35a5ecc..09cc970 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -115,8 +115,8 @@ false 0.0 - - ada_hardware/DynamixelHardware + + dynamixel_hardware/DynamixelHardware /dev/ttyUSB0 1000000 From 440b1d2404054b5300640dc3c17894b689408c9a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Nov 2024 09:22:33 -0800 Subject: [PATCH 10/15] Fix mis-named namespace --- ada_hardware/test/test_dynamixel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp index 750a3d9..b3b8f6a 100644 --- a/ada_hardware/test/test_dynamixel.cpp +++ b/ada_hardware/test/test_dynamixel.cpp @@ -44,7 +44,7 @@ class TestDynamixelHardware : public ::testing::Test { R"( - ada_hardware/DynamixelHardware + dynamixel_hardware/DynamixelHardware /dev/ttyUSB0 1000000 From cc9041689d1d52c7664b78fac34baefd422faf05 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 12 Dec 2024 14:40:41 -0800 Subject: [PATCH 11/15] Formatting --- ada_description/rviz/view_robot.rviz | 1 - ada_hardware/launch/ada_hardware.launch.py | 1 - 2 files changed, 2 deletions(-) diff --git a/ada_description/rviz/view_robot.rviz b/ada_description/rviz/view_robot.rviz index 4595149..15546ea 100644 --- a/ada_description/rviz/view_robot.rviz +++ b/ada_description/rviz/view_robot.rviz @@ -91,7 +91,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true FT: Alpha: 1 Show Axes: false diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index d0c523a..71c9601 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -189,7 +189,6 @@ def generate_launch_description(): output="both", on_exit=Shutdown(), ) - robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", From 2c3f00c8006ba2b6b37f6e22ba15c75e6962e177 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 12 Dec 2024 14:53:14 -0800 Subject: [PATCH 12/15] Add end_effector_tool configuration for ada_hardware.launch.py --- ada_hardware/launch/ada_hardware.launch.py | 12 ++++++------ ada_hardware/urdf/ada_hardware.xacro | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index 71c9601..007c8b4 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -95,9 +95,9 @@ 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", ) ) declared_arguments.append( @@ -137,7 +137,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") @@ -159,8 +159,8 @@ def generate_launch_description(): "readonly:=", readonly, " ", - "use_forque:=", - use_forque, + "end_effector_tool:=", + end_effector_tool, ] ) robot_description = { diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 1ba3be4..0710346 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -7,7 +7,7 @@ - + From c7a815faf8cd0588737de15ffda9da700aadd667 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Dec 2024 22:47:02 -0800 Subject: [PATCH 13/15] Use updated ada macro --- ada_hardware/launch/ada_hardware.launch.py | 3 ++- ada_hardware/urdf/ada_hardware.xacro | 7 ++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index 007c8b4..78b9ada 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -97,7 +97,8 @@ def generate_launch_description(): DeclareLaunchArgument( "end_effector_tool", default_value="none", - description="The end-effector tool being used: none, fork, articulable_fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=['none', 'fork', 'articulable_fork'] ) ) declared_arguments.append( diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 0710346..62ac5d5 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -7,12 +7,17 @@ - + + + + + + Date: Wed, 18 Dec 2024 09:26:21 -0800 Subject: [PATCH 14/15] Run pre-commit --- ada_hardware/launch/ada_hardware.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index 20701a4..171be36 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -101,7 +101,7 @@ def generate_launch_description(): "end_effector_tool", default_value="none", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) ) declared_arguments.append( From aed616dd4a5752834863d38845bb3f3d45d89cd7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 18 Dec 2024 09:28:49 -0800 Subject: [PATCH 15/15] Fix typo in dynamixel_hardware.hpp --- ada_hardware/include/ada_hardware/dynamixel_hardware.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp index 846a7af..ef93f07 100644 --- a/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp +++ b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp @@ -48,7 +48,7 @@ enum class ControlMode { Position, Velocity, Torque, - Currrent, + Current, ExtendedPosition, MultiTurn, CurrentBasedPosition,