From 9332ce67269af9bccd867f1d637df88c9226f36d Mon Sep 17 00:00:00 2001 From: ligx Date: Tue, 13 Jan 2026 11:10:14 +0800 Subject: [PATCH 01/16] update .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 19b7394a..0ff06f1c 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ .DS_Store .vscode/ __pycache__/ +.github/ From 9e28f509cb0f3d3e9fddf5656f22933123516444 Mon Sep 17 00:00:00 2001 From: ligx Date: Wed, 14 Jan 2026 10:11:33 +0800 Subject: [PATCH 02/16] add oy motor hardware interface --- openarm_hardware/CMakeLists.txt | 3 ++- openarm_hardware/openarm_hardware.xml | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index 1ed46920..e063c9db 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -30,7 +30,8 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(OpenArmCAN REQUIRED) add_library(${PROJECT_NAME} SHARED - src/v10_simple_hardware.cpp + # src/v10_simple_hardware.cpp + src/oy_hardware.cpp ) target_include_directories( diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index fd4fee92..c209096b 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -19,7 +19,15 @@ type="openarm_hardware::OpenArm_v10HW" base_class_type="hardware_interface::SystemInterface"> - ros2_control hardware interface for OpenArm V10. + ros2_control hardware interface for OpenArm V10 (Damiao Motors). + + + + + ros2_control hardware interface for OpenArm with OY Motors. + Configurable arm DOF via hardware parameters. From ae335dac2b9bea64aa90222cb3cfd646a2fbf643 Mon Sep 17 00:00:00 2001 From: ligx Date: Wed, 14 Jan 2026 10:11:49 +0800 Subject: [PATCH 03/16] add oy motor hardware interface --- .../include/openarm_hardware/oy_hardware.hpp | 148 ++++++++ openarm_hardware/src/oy_hardware.cpp | 350 ++++++++++++++++++ 2 files changed, 498 insertions(+) create mode 100644 openarm_hardware/include/openarm_hardware/oy_hardware.hpp create mode 100644 openarm_hardware/src/oy_hardware.cpp diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp new file mode 100644 index 00000000..471016d9 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -0,0 +1,148 @@ +// Copyright 2025 Enactic, 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "openarm_hardware/visibility_control.h" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace openarm_hardware { + +/** + * @brief OpenArm Hardware Interface for OY Motors + * + * This hardware interface uses the OpenArm CAN API with OY motors, + * following the pattern from full_arm.cpp example. + * Configurable for different arm configurations via hardware parameters. + */ +class OpenArmOYHW : public hardware_interface::SystemInterface { + public: + OpenArmOYHW(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo& info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read(const rclcpp::Time& time, + const rclcpp::Duration& period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time& time, const rclcpp::Duration& period) override; + + private: + // default configuration + static constexpr size_t ARM_DOF = 7; + static constexpr bool ENABLE_GRIPPER = true; + + // Default OY motor configuration for V10 + // Motor types: GIM8115_9p for large joints, GIM4310_40/GIM4315_8 for small joints + const std::vector DEFAULT_MOTOR_TYPES = { + openarm::oy_motor::MotorType::GIM8115_9p, // Joint 1 + openarm::oy_motor::MotorType::GIM8115_9p, // Joint 2 + openarm::oy_motor::MotorType::GIM4310_40, // Joint 3 + openarm::oy_motor::MotorType::GIM4310_40, // Joint 4 + openarm::oy_motor::MotorType::GIM4315_8, // Joint 5 + openarm::oy_motor::MotorType::GIM4315_8, // Joint 6 + openarm::oy_motor::MotorType::GIM4315_8 // Joint 7 + }; + + const std::vector DEFAULT_SEND_CAN_IDS = {0x101, 0x102, 0x103, 0x104, + 0x105, 0x106, 0x107}; + const std::vector DEFAULT_RECV_CAN_IDS = {0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07}; + + const openarm::oy_motor::MotorType DEFAULT_GRIPPER_MOTOR_TYPE = + openarm::oy_motor::MotorType::GIM4315_8; + const uint32_t DEFAULT_GRIPPER_SEND_CAN_ID = 0x108; + const uint32_t DEFAULT_GRIPPER_RECV_CAN_ID = 0x08; + + // Default gains for OY motors (may need tuning) + // Order: Joint 1-7 + Gripper + const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, + 5.0, 5.0, 5.0, 0.5}; + const std::vector DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, + 0.7, 0.6, 0.5, 0.1}; + + const double GRIPPER_JOINT_0_POSITION = 0.044; + const double GRIPPER_JOINT_1_POSITION = 0.0; + const double GRIPPER_MOTOR_0_RADIANS = 0.0; + const double GRIPPER_MOTOR_1_RADIANS = -1.0472; + const double GRIPPER_DEFAULT_KP = 5.0; + const double GRIPPER_DEFAULT_KD = 0.1; + + // Configuration + std::string can_interface_; + std::string arm_prefix_; + bool hand_; + bool can_fd_; + + // OpenArm instance + std::unique_ptr openarm_; + + // Generated joint names for this arm instance + std::vector joint_names_; + + // ROS2 control state and command vectors + std::vector pos_commands_; + std::vector vel_commands_; + std::vector tau_commands_; + std::vector pos_states_; + std::vector vel_states_; + std::vector tau_states_; + + // Helper methods + void return_to_zero(); + bool parse_config(const hardware_interface::HardwareInfo& info); + void generate_joint_names(); + + // Gripper mapping functions + double joint_to_motor_radians(double joint_value); + double motor_radians_to_joint(double motor_radians); +}; + +} // namespace openarm_hardware diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp new file mode 100644 index 00000000..19369743 --- /dev/null +++ b/openarm_hardware/src/oy_hardware.cpp @@ -0,0 +1,350 @@ +// Copyright 2025 Enactic, 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 "openarm_hardware/oy_hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace openarm_hardware { + +OpenArmOYHW::OpenArmOYHW() = default; + +bool OpenArmOYHW::parse_config(const hardware_interface::HardwareInfo& info) { + // Parse CAN interface (default: can0) + auto it = info.hardware_parameters.find("can_interface"); + can_interface_ = (it != info.hardware_parameters.end()) ? it->second : "can0"; + + // Parse arm prefix (default: empty for single arm, "left_" or "right_" for + // bimanual) + it = info.hardware_parameters.find("arm_prefix"); + arm_prefix_ = (it != info.hardware_parameters.end()) ? it->second : ""; + + // Parse gripper enable (default: true) + it = info.hardware_parameters.find("hand"); + if (it == info.hardware_parameters.end()) { + hand_ = true; // Default to true + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + hand_ = (value == "true"); + } + + // Parse CAN-FD enable (default: true) + it = info.hardware_parameters.find("can_fd"); + if (it == info.hardware_parameters.end()) { + can_fd_ = true; // Default to true + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + can_fd_ = (value == "true"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Configuration: CAN=%s, arm_prefix=%s, arm_dof=%zu, hand=%s, can_fd=%s", + can_interface_.c_str(), arm_prefix_.c_str(), ARM_DOF, + hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); + return true; +} + +void OpenArmOYHW::generate_joint_names() { + joint_names_.clear(); + // TODO: read from urdf properly and sort in the future. + // Currently, the joint names are hardcoded for order consistency to align + // with hardware. Generate arm joint names: openarm_{arm_prefix}joint{N} + for (size_t i = 1; i <= ARM_DOF; ++i) { + std::string joint_name = + "openarm_" + arm_prefix_ + "joint" + std::to_string(i); + joint_names_.push_back(joint_name); + } + + // Generate gripper joint name if enabled + if (hand_) { + std::string gripper_joint_name = "openarm_" + arm_prefix_ + "finger_joint1"; + joint_names_.push_back(gripper_joint_name); + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Added gripper joint: %s", + gripper_joint_name.c_str()); + } else { + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Gripper joint NOT added because hand_=false"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Generated %zu joint names for arm prefix '%s'", + joint_names_.size(), arm_prefix_.c_str()); +} + +hardware_interface::CallbackReturn OpenArmOYHW::on_init( + const hardware_interface::HardwareInfo& info) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + // Parse configuration + if (!parse_config(info)) { + return CallbackReturn::ERROR; + } + + // Generate joint names based on arm prefix + generate_joint_names(); + + // Validate joint count (arm joints + optional gripper) + size_t expected_joints = ARM_DOF + (hand_ ? 1 : 0); + if (joint_names_.size() != expected_joints) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArmOYHW"), + "Generated %zu joint names, expected %zu", joint_names_.size(), + expected_joints); + return CallbackReturn::ERROR; + } + + // Initialize OpenArm with configurable CAN-FD setting + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Initializing OpenArm (OY Motor) on %s with CAN-FD %s...", + can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); + openarm_ = + std::make_unique(can_interface_, can_fd_); + + // Initialize arm motors with defaults (OY motors) + // Use only the required number of motors based on ARM_DOF + std::vector motor_types( + DEFAULT_MOTOR_TYPES.begin(), + DEFAULT_MOTOR_TYPES.begin() + std::min(ARM_DOF, DEFAULT_MOTOR_TYPES.size())); + std::vector send_ids( + DEFAULT_SEND_CAN_IDS.begin(), + DEFAULT_SEND_CAN_IDS.begin() + std::min(ARM_DOF, DEFAULT_SEND_CAN_IDS.size())); + std::vector recv_ids( + DEFAULT_RECV_CAN_IDS.begin(), + DEFAULT_RECV_CAN_IDS.begin() + std::min(ARM_DOF, DEFAULT_RECV_CAN_IDS.size())); + + openarm_->init_arm_motors(motor_types, send_ids, recv_ids); + + // Initialize gripper if enabled + if (hand_) { + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Initializing gripper..."); + openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, + DEFAULT_GRIPPER_SEND_CAN_ID, + DEFAULT_GRIPPER_RECV_CAN_ID); + } + + // Initialize state and command vectors based on generated joint count + const size_t total_joints = joint_names_.size(); + pos_commands_.resize(total_joints, 0.0); + vel_commands_.resize(total_joints, 0.0); + tau_commands_.resize(total_joints, 0.0); + pos_states_.resize(total_joints, 0.0); + vel_states_.resize(total_joints, 0.0); + tau_states_.resize(total_joints, 0.0); + + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "OpenArm OY Motor HW initialized successfully with %zu DOF", + ARM_DOF + (hand_ ? 1 : 0)); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArmOYHW::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + // Refresh motor states during configuration + openarm_->get_arm().refresh_all(); + if (hand_) { + openarm_->get_gripper().refresh_all(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + return CallbackReturn::SUCCESS; +} + +std::vector +OpenArmOYHW::export_state_interfaces() { + std::vector state_interfaces; + for (size_t i = 0; i < joint_names_.size(); ++i) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &pos_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &vel_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_states_[i])); + } + + return state_interfaces; +} + +std::vector +OpenArmOYHW::export_command_interfaces() { + std::vector command_interfaces; + // TODO: consider exposing only needed interfaces to avoid undefined behavior. + for (size_t i = 0; i < joint_names_.size(); ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + &pos_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + &vel_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn OpenArmOYHW::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Activating OpenArm OY Motor..."); + + // Enable all motors + openarm_->get_arm().enable_all(); + if (hand_) { + openarm_->get_gripper().enable_all(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + // Return to zero position + return_to_zero(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "OpenArm OY Motor activated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArmOYHW::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Deactivating OpenArm OY Motor..."); + + // Disable all motors + openarm_->get_arm().disable_all(); + if (hand_) { + openarm_->get_gripper().disable_all(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "OpenArm OY Motor deactivated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type OpenArmOYHW::read( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Refresh and receive all motor states + openarm_->get_arm().refresh_all(); + if (hand_) { + openarm_->get_gripper().refresh_all(); + } + openarm_->recv_all(); + + // Read arm joint states + const auto& arm_motors = openarm_->get_arm().get_motors(); + for (size_t i = 0; i < ARM_DOF && i < arm_motors.size(); ++i) { + pos_states_[i] = arm_motors[i].get_position(); + vel_states_[i] = arm_motors[i].get_velocity(); + tau_states_[i] = arm_motors[i].get_torque(); + } + + // Read gripper state if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + const auto& gripper_motors = openarm_->get_gripper().get_motors(); + if (!gripper_motors.empty()) { + // TODO the mappings are approximates + // Convert motor position (radians) to joint value (0-0.044m) + double motor_pos = gripper_motors[0].get_position(); + pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); + + // Unimplemented: Velocity and torque mapping + vel_states_[ARM_DOF] = 0; // gripper_motors[0].get_velocity(); + tau_states_[ARM_DOF] = 0; // gripper_motors[0].get_torque(); + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type OpenArmOYHW::write( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Control arm motors with MIT control + // OY motor MITParam order: {q, dq, kp, kd, tau} + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + double kp = (i < DEFAULT_KP.size()) ? DEFAULT_KP[i] : DEFAULT_KP.back(); + double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); + arm_params.push_back({pos_commands_[i], vel_commands_[i], + kp, kd, tau_commands_[i]}); + } + openarm_->get_arm().mit_control_all(arm_params); + + // Control gripper if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + // TODO the true mappings are unimplemented. + double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); + // OY motor MITParam order: {q, dq, kp, kd, tau} + openarm_->get_gripper().mit_control_all( + {{motor_command, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); + } + openarm_->recv_all(1000); + return hardware_interface::return_type::OK; +} + +void OpenArmOYHW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + "Returning to zero position..."); + + // Return arm to zero with MIT control + // OY motor MITParam order: {q, dq, kp, kd, tau} + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + double kp = (i < DEFAULT_KP.size()) ? DEFAULT_KP[i] : DEFAULT_KP.back(); + double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); + arm_params.push_back({0.0, 0.0, kp, kd, 0.0}); + } + openarm_->get_arm().mit_control_all(arm_params); + + // Return gripper to zero if enabled + if (hand_) { + // OY motor MITParam order: {q, dq, kp, kd, tau} + openarm_->get_gripper().mit_control_all( + {{GRIPPER_JOINT_0_POSITION, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); + } + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + openarm_->recv_all(); +} + +// Gripper mapping helper functions +double OpenArmOYHW::joint_to_motor_radians(double joint_value) { + // Joint 0=closed -> motor 0 rad, Joint 0.044=open -> motor -1.0472 rad + return (joint_value / GRIPPER_JOINT_0_POSITION) * + GRIPPER_MOTOR_1_RADIANS; // Scale from 0-0.044 to 0 to -1.0472 +} + +double OpenArmOYHW::motor_radians_to_joint(double motor_radians) { + // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 0.044 + return GRIPPER_JOINT_0_POSITION * + (motor_radians / + GRIPPER_MOTOR_1_RADIANS); // Scale from 0 to -1.0472 to 0-0.044 +} + +} // namespace openarm_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmOYHW, + hardware_interface::SystemInterface) From 89fe3fed92734d0fb42c7671a9e5caca1e7e8875 Mon Sep 17 00:00:00 2001 From: ligx Date: Wed, 14 Jan 2026 15:59:36 +0800 Subject: [PATCH 04/16] complete oy_hardware --- .gitignore | 1 + openarm_hardware/src/oy_hardware.cpp | 24 ++++++------------------ 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/.gitignore b/.gitignore index 0ff06f1c..2f6c655b 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ .vscode/ __pycache__/ .github/ +openarm_description/ diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index 19369743..2a979d1a 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -165,10 +165,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_init( hardware_interface::CallbackReturn OpenArmOYHW::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { // Refresh motor states during configuration - openarm_->get_arm().refresh_all(); - if (hand_) { - openarm_->get_gripper().refresh_all(); - } + openarm_->refresh_all(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); openarm_->recv_all(); @@ -213,10 +210,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_activate( RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Activating OpenArm OY Motor..."); // Enable all motors - openarm_->get_arm().enable_all(); - if (hand_) { - openarm_->get_gripper().enable_all(); - } + openarm_->enable_all(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); openarm_->recv_all(); @@ -233,10 +227,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_deactivate( "Deactivating OpenArm OY Motor..."); // Disable all motors - openarm_->get_arm().disable_all(); - if (hand_) { - openarm_->get_gripper().disable_all(); - } + openarm_->disable_all(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); openarm_->recv_all(); @@ -247,10 +238,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_deactivate( hardware_interface::return_type OpenArmOYHW::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Refresh and receive all motor states - openarm_->get_arm().refresh_all(); - if (hand_) { - openarm_->get_gripper().refresh_all(); - } + openarm_->refresh_all(); openarm_->recv_all(); // Read arm joint states @@ -271,8 +259,8 @@ hardware_interface::return_type OpenArmOYHW::read( pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); // Unimplemented: Velocity and torque mapping - vel_states_[ARM_DOF] = 0; // gripper_motors[0].get_velocity(); - tau_states_[ARM_DOF] = 0; // gripper_motors[0].get_torque(); + vel_states_[ARM_DOF] = gripper_motors[0].get_velocity(); + tau_states_[ARM_DOF] = gripper_motors[0].get_torque(); } } From 5a001e3e7bef6f6f05e168d830adca7477049b46 Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 15 Jan 2026 13:41:19 +0800 Subject: [PATCH 05/16] change class name --- .../include/openarm_hardware/oy_hardware.hpp | 4 +- openarm_hardware/src/oy_hardware.cpp | 56 +++++++++---------- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index 471016d9..74b18506 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -38,9 +38,9 @@ namespace openarm_hardware { * following the pattern from full_arm.cpp example. * Configurable for different arm configurations via hardware parameters. */ -class OpenArmOYHW : public hardware_interface::SystemInterface { +class OpenArm_OYHW : public hardware_interface::SystemInterface { public: - OpenArmOYHW(); + OpenArm_OYHW(); TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index 2a979d1a..9c813ff4 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -26,9 +26,9 @@ namespace openarm_hardware { -OpenArmOYHW::OpenArmOYHW() = default; +OpenArm_OYHW::OpenArm_OYHW() = default; -bool OpenArmOYHW::parse_config(const hardware_interface::HardwareInfo& info) { +bool OpenArm_OYHW::parse_config(const hardware_interface::HardwareInfo& info) { // Parse CAN interface (default: can0) auto it = info.hardware_parameters.find("can_interface"); can_interface_ = (it != info.hardware_parameters.end()) ? it->second : "can0"; @@ -67,7 +67,7 @@ bool OpenArmOYHW::parse_config(const hardware_interface::HardwareInfo& info) { return true; } -void OpenArmOYHW::generate_joint_names() { +void OpenArm_OYHW::generate_joint_names() { joint_names_.clear(); // TODO: read from urdf properly and sort in the future. // Currently, the joint names are hardcoded for order consistency to align @@ -82,19 +82,19 @@ void OpenArmOYHW::generate_joint_names() { if (hand_) { std::string gripper_joint_name = "openarm_" + arm_prefix_ + "finger_joint1"; joint_names_.push_back(gripper_joint_name); - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Added gripper joint: %s", + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Added gripper joint: %s", gripper_joint_name.c_str()); } else { - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Gripper joint NOT added because hand_=false"); } - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Generated %zu joint names for arm prefix '%s'", joint_names_.size(), arm_prefix_.c_str()); } -hardware_interface::CallbackReturn OpenArmOYHW::on_init( +hardware_interface::CallbackReturn OpenArm_OYHW::on_init( const hardware_interface::HardwareInfo& info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { @@ -111,14 +111,14 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_init( // Validate joint count (arm joints + optional gripper) size_t expected_joints = ARM_DOF + (hand_ ? 1 : 0); if (joint_names_.size() != expected_joints) { - RCLCPP_ERROR(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_OYHW"), "Generated %zu joint names, expected %zu", joint_names_.size(), expected_joints); return CallbackReturn::ERROR; } // Initialize OpenArm with configurable CAN-FD setting - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Initializing OpenArm (OY Motor) on %s with CAN-FD %s...", can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); openarm_ = @@ -140,7 +140,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_init( // Initialize gripper if enabled if (hand_) { - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Initializing gripper..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Initializing gripper..."); openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, DEFAULT_GRIPPER_SEND_CAN_ID, DEFAULT_GRIPPER_RECV_CAN_ID); @@ -155,14 +155,14 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_init( vel_states_.resize(total_joints, 0.0); tau_states_.resize(total_joints, 0.0); - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "OpenArm OY Motor HW initialized successfully with %zu DOF", ARM_DOF + (hand_ ? 1 : 0)); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArmOYHW::on_configure( +hardware_interface::CallbackReturn OpenArm_OYHW::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { // Refresh motor states during configuration openarm_->refresh_all(); @@ -173,7 +173,7 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_configure( } std::vector -OpenArmOYHW::export_state_interfaces() { +OpenArm_OYHW::export_state_interfaces() { std::vector state_interfaces; for (size_t i = 0; i < joint_names_.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -188,7 +188,7 @@ OpenArmOYHW::export_state_interfaces() { } std::vector -OpenArmOYHW::export_command_interfaces() { +OpenArm_OYHW::export_command_interfaces() { std::vector command_interfaces; // TODO: consider exposing only needed interfaces to avoid undefined behavior. for (size_t i = 0; i < joint_names_.size(); ++i) { @@ -205,9 +205,9 @@ OpenArmOYHW::export_command_interfaces() { return command_interfaces; } -hardware_interface::CallbackReturn OpenArmOYHW::on_activate( +hardware_interface::CallbackReturn OpenArm_OYHW::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "Activating OpenArm OY Motor..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Activating OpenArm OY Motor..."); // Enable all motors openarm_->enable_all(); @@ -217,13 +217,13 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_activate( // Return to zero position return_to_zero(); - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "OpenArm OY Motor activated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "OpenArm OY Motor activated"); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArmOYHW::on_deactivate( +hardware_interface::CallbackReturn OpenArm_OYHW::on_deactivate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Deactivating OpenArm OY Motor..."); // Disable all motors @@ -231,11 +231,11 @@ hardware_interface::CallbackReturn OpenArmOYHW::on_deactivate( std::this_thread::sleep_for(std::chrono::milliseconds(100)); openarm_->recv_all(); - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), "OpenArm OY Motor deactivated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "OpenArm OY Motor deactivated"); return CallbackReturn::SUCCESS; } -hardware_interface::return_type OpenArmOYHW::read( +hardware_interface::return_type OpenArm_OYHW::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Refresh and receive all motor states openarm_->refresh_all(); @@ -258,7 +258,7 @@ hardware_interface::return_type OpenArmOYHW::read( double motor_pos = gripper_motors[0].get_position(); pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); - // Unimplemented: Velocity and torque mapping + //Velocity and torque mapping vel_states_[ARM_DOF] = gripper_motors[0].get_velocity(); tau_states_[ARM_DOF] = gripper_motors[0].get_torque(); } @@ -267,7 +267,7 @@ hardware_interface::return_type OpenArmOYHW::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type OpenArmOYHW::write( +hardware_interface::return_type OpenArm_OYHW::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Control arm motors with MIT control // OY motor MITParam order: {q, dq, kp, kd, tau} @@ -292,8 +292,8 @@ hardware_interface::return_type OpenArmOYHW::write( return hardware_interface::return_type::OK; } -void OpenArmOYHW::return_to_zero() { - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), +void OpenArm_OYHW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Returning to zero position..."); // Return arm to zero with MIT control @@ -317,13 +317,13 @@ void OpenArmOYHW::return_to_zero() { } // Gripper mapping helper functions -double OpenArmOYHW::joint_to_motor_radians(double joint_value) { +double OpenArm_OYHW::joint_to_motor_radians(double joint_value) { // Joint 0=closed -> motor 0 rad, Joint 0.044=open -> motor -1.0472 rad return (joint_value / GRIPPER_JOINT_0_POSITION) * GRIPPER_MOTOR_1_RADIANS; // Scale from 0-0.044 to 0 to -1.0472 } -double OpenArmOYHW::motor_radians_to_joint(double motor_radians) { +double OpenArm_OYHW::motor_radians_to_joint(double motor_radians) { // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 0.044 return GRIPPER_JOINT_0_POSITION * (motor_radians / @@ -334,5 +334,5 @@ double OpenArmOYHW::motor_radians_to_joint(double motor_radians) { #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmOYHW, +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArm_OYHW, hardware_interface::SystemInterface) From 7d4eb5bce73ce8cc1b39e7b1c8c8fb2a809671d4 Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 15 Jan 2026 16:45:08 +0800 Subject: [PATCH 06/16] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20oy=E7=A1=AC=E4=BB=B6?= =?UTF-8?q?=E6=8E=A5=E5=8F=A3=EF=BC=8C=E4=B8=A6=E9=85=8D=E7=BD=AE=E6=8F=92?= =?UTF-8?q?=E4=BB=B6=E2=80=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/openarm_hardware/oy_hardware.hpp | 4 +- openarm_hardware/openarm_hardware.xml | 4 +- openarm_hardware/src/oy_hardware.cpp | 54 +++++++++---------- 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index 74b18506..c5247b15 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -38,9 +38,9 @@ namespace openarm_hardware { * following the pattern from full_arm.cpp example. * Configurable for different arm configurations via hardware parameters. */ -class OpenArm_OYHW : public hardware_interface::SystemInterface { +class OpenArm_oyHW : public hardware_interface::SystemInterface { public: - OpenArm_OYHW(); + OpenArm_oyHW(); TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index c209096b..3446dd94 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -22,8 +22,8 @@ ros2_control hardware interface for OpenArm V10 (Damiao Motors). - ros2_control hardware interface for OpenArm with OY Motors. diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index 9c813ff4..4c6f29cd 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -26,9 +26,9 @@ namespace openarm_hardware { -OpenArm_OYHW::OpenArm_OYHW() = default; +OpenArm_oyHW::OpenArm_oyHW() = default; -bool OpenArm_OYHW::parse_config(const hardware_interface::HardwareInfo& info) { +bool OpenArm_oyHW::parse_config(const hardware_interface::HardwareInfo& info) { // Parse CAN interface (default: can0) auto it = info.hardware_parameters.find("can_interface"); can_interface_ = (it != info.hardware_parameters.end()) ? it->second : "can0"; @@ -67,7 +67,7 @@ bool OpenArm_OYHW::parse_config(const hardware_interface::HardwareInfo& info) { return true; } -void OpenArm_OYHW::generate_joint_names() { +void OpenArm_oyHW::generate_joint_names() { joint_names_.clear(); // TODO: read from urdf properly and sort in the future. // Currently, the joint names are hardcoded for order consistency to align @@ -82,19 +82,19 @@ void OpenArm_OYHW::generate_joint_names() { if (hand_) { std::string gripper_joint_name = "openarm_" + arm_prefix_ + "finger_joint1"; joint_names_.push_back(gripper_joint_name); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Added gripper joint: %s", + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Added gripper joint: %s", gripper_joint_name.c_str()); } else { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Gripper joint NOT added because hand_=false"); } - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Generated %zu joint names for arm prefix '%s'", joint_names_.size(), arm_prefix_.c_str()); } -hardware_interface::CallbackReturn OpenArm_OYHW::on_init( +hardware_interface::CallbackReturn OpenArm_oyHW::on_init( const hardware_interface::HardwareInfo& info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { @@ -111,14 +111,14 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_init( // Validate joint count (arm joints + optional gripper) size_t expected_joints = ARM_DOF + (hand_ ? 1 : 0); if (joint_names_.size() != expected_joints) { - RCLCPP_ERROR(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_oyHW"), "Generated %zu joint names, expected %zu", joint_names_.size(), expected_joints); return CallbackReturn::ERROR; } // Initialize OpenArm with configurable CAN-FD setting - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Initializing OpenArm (OY Motor) on %s with CAN-FD %s...", can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); openarm_ = @@ -140,7 +140,7 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_init( // Initialize gripper if enabled if (hand_) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Initializing gripper..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Initializing gripper..."); openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, DEFAULT_GRIPPER_SEND_CAN_ID, DEFAULT_GRIPPER_RECV_CAN_ID); @@ -155,14 +155,14 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_init( vel_states_.resize(total_joints, 0.0); tau_states_.resize(total_joints, 0.0); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "OpenArm OY Motor HW initialized successfully with %zu DOF", ARM_DOF + (hand_ ? 1 : 0)); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArm_OYHW::on_configure( +hardware_interface::CallbackReturn OpenArm_oyHW::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { // Refresh motor states during configuration openarm_->refresh_all(); @@ -173,7 +173,7 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_configure( } std::vector -OpenArm_OYHW::export_state_interfaces() { +OpenArm_oyHW::export_state_interfaces() { std::vector state_interfaces; for (size_t i = 0; i < joint_names_.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -188,7 +188,7 @@ OpenArm_OYHW::export_state_interfaces() { } std::vector -OpenArm_OYHW::export_command_interfaces() { +OpenArm_oyHW::export_command_interfaces() { std::vector command_interfaces; // TODO: consider exposing only needed interfaces to avoid undefined behavior. for (size_t i = 0; i < joint_names_.size(); ++i) { @@ -205,9 +205,9 @@ OpenArm_OYHW::export_command_interfaces() { return command_interfaces; } -hardware_interface::CallbackReturn OpenArm_OYHW::on_activate( +hardware_interface::CallbackReturn OpenArm_oyHW::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "Activating OpenArm OY Motor..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Activating OpenArm OY Motor..."); // Enable all motors openarm_->enable_all(); @@ -217,13 +217,13 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_activate( // Return to zero position return_to_zero(); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "OpenArm OY Motor activated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "OpenArm OY Motor activated"); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArm_OYHW::on_deactivate( +hardware_interface::CallbackReturn OpenArm_oyHW::on_deactivate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Deactivating OpenArm OY Motor..."); // Disable all motors @@ -231,11 +231,11 @@ hardware_interface::CallbackReturn OpenArm_OYHW::on_deactivate( std::this_thread::sleep_for(std::chrono::milliseconds(100)); openarm_->recv_all(); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), "OpenArm OY Motor deactivated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "OpenArm OY Motor deactivated"); return CallbackReturn::SUCCESS; } -hardware_interface::return_type OpenArm_OYHW::read( +hardware_interface::return_type OpenArm_oyHW::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Refresh and receive all motor states openarm_->refresh_all(); @@ -267,7 +267,7 @@ hardware_interface::return_type OpenArm_OYHW::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type OpenArm_OYHW::write( +hardware_interface::return_type OpenArm_oyHW::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Control arm motors with MIT control // OY motor MITParam order: {q, dq, kp, kd, tau} @@ -292,8 +292,8 @@ hardware_interface::return_type OpenArm_OYHW::write( return hardware_interface::return_type::OK; } -void OpenArm_OYHW::return_to_zero() { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_OYHW"), +void OpenArm_oyHW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Returning to zero position..."); // Return arm to zero with MIT control @@ -317,13 +317,13 @@ void OpenArm_OYHW::return_to_zero() { } // Gripper mapping helper functions -double OpenArm_OYHW::joint_to_motor_radians(double joint_value) { +double OpenArm_oyHW::joint_to_motor_radians(double joint_value) { // Joint 0=closed -> motor 0 rad, Joint 0.044=open -> motor -1.0472 rad return (joint_value / GRIPPER_JOINT_0_POSITION) * GRIPPER_MOTOR_1_RADIANS; // Scale from 0-0.044 to 0 to -1.0472 } -double OpenArm_OYHW::motor_radians_to_joint(double motor_radians) { +double OpenArm_oyHW::motor_radians_to_joint(double motor_radians) { // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 0.044 return GRIPPER_JOINT_0_POSITION * (motor_radians / @@ -334,5 +334,5 @@ double OpenArm_OYHW::motor_radians_to_joint(double motor_radians) { #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArm_OYHW, +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArm_oyHW, hardware_interface::SystemInterface) From 2245d10e26babe2092fc65e5462d0709d876296b Mon Sep 17 00:00:00 2001 From: ligx Date: Fri, 16 Jan 2026 14:34:01 +0800 Subject: [PATCH 07/16] add oy related files including controllers urdf and so on --- ...56\346\224\271\350\250\230\351\214\204.md" | 59 ++++++ .../config/openarm_bimanual.urdf.xacro | 3 +- .../launch/demo.launch.py | 8 +- .../openarm_oy_bimanual_controllers.yaml | 175 ++++++++++++++++++ ...rm_oy_bimanual_controllers_namespaced.yaml | 138 ++++++++++++++ .../openarm_oy_controllers.yaml | 100 ++++++++++ .../launch/openarm.bimanual.launch.py | 12 +- openarm_bringup/launch/openarm.launch.py | 10 +- openarm_hardware/src/oy_hardware.cpp | 2 +- 9 files changed, 489 insertions(+), 18 deletions(-) create mode 100644 "dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" create mode 100644 openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml create mode 100644 openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml create mode 100644 openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml diff --git "a/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" "b/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" new file mode 100644 index 00000000..94801cde --- /dev/null +++ "b/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" @@ -0,0 +1,59 @@ +# 註冊新硬件接口 +openarm_hardware.xml +``` +class name="openarm_hardware/OpenArm_OYHW" +``` + + +## Markdown 內容樣式範例 + +### 標題 +```markdown +# 一級標題 +## 二級標題 +### 三級標題 +``` + +### 列表 +```markdown +- 無序列表項目 1 +- 無序列表項目 2 + +1. 有序列表項目 1 +2. 有序列表項目 2 +``` + +### 程式碼區塊 +```markdown +`行內程式碼` + +​```python +def hello(): + print("Hello World") +​``` +``` + +### 連結與圖片 +```markdown +[連結文字](https://example.com) +![圖片描述](image.png) +``` + +### 表格 +```markdown +| 欄位1 | 欄位2 | 欄位3 | +|-------|-------|-------| +| 內容1 | 內容2 | 內容3 | +``` + +### 引用 +```markdown +> 這是一段引用文字 +``` + +### 強調 +```markdown +**粗體文字** +*斜體文字* +~~刪除線~~ +``` \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro index 6f6f4951..7a621541 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro @@ -19,8 +19,7 @@ - - + diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py index 5ed6731c..62dff55d 100644 --- a/openarm_bimanual_moveit_config/launch/demo.launch.py +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -146,9 +146,9 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", ), - DeclareLaunchArgument("arm_type", default_value="v10"), + DeclareLaunchArgument("arm_type", default_value="oy"), DeclareLaunchArgument("use_fake_hardware", default_value="false"), DeclareLaunchArgument( "robot_controller", @@ -164,7 +164,7 @@ def generate_launch_description(): DeclareLaunchArgument("left_can_interface", default_value="can1"), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", + default_value="openarm_oy_bimanual_controllers.yaml", ), ] @@ -181,7 +181,7 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml new file mode 100644 index 00000000..8a386eec --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml @@ -0,0 +1,175 @@ +# Copyright 2025 Enactic, Inc. +# Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + left_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + left_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + left_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + left_gripper_controller: + type: position_controllers/GripperActionController + + right_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + right_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + right_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_gripper_controller: + type: position_controllers/GripperActionController + + +left_forward_position_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + +left_forward_velocity_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +left_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +left_gripper_controller: + ros__parameters: + joint: openarm_left_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + + +right_forward_position_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + +right_forward_velocity_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +right_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +right_gripper_controller: + ros__parameters: + joint: openarm_right_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position \ No newline at end of file diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml new file mode 100644 index 00000000..ca0b9721 --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml @@ -0,0 +1,138 @@ +# Copyright 2025 Enactic, 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. + +/**: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + left_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + left_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + right_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + left_gripper_controller: + type: forward_command_controller/ForwardCommandController + + right_gripper_controller: + type: forward_command_controller/ForwardCommandController + +/**/left_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: false + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.6 + openarm_left_joint1: {trajectory: 0.1, goal: 0.1} + openarm_left_joint2: {trajectory: 0.1, goal: 0.1} + openarm_left_joint3: {trajectory: 0.1, goal: 0.1} + openarm_left_joint4: {trajectory: 0.1, goal: 0.1} + openarm_left_joint5: {trajectory: 0.1, goal: 0.1} + openarm_left_joint6: {trajectory: 0.1, goal: 0.1} + openarm_left_joint7: {trajectory: 0.1, goal: 0.1} + +/**/right_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: false + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.6 + openarm_right_joint1: {trajectory: 0.1, goal: 0.1} + openarm_right_joint2: {trajectory: 0.1, goal: 0.1} + openarm_right_joint3: {trajectory: 0.1, goal: 0.1} + openarm_right_joint4: {trajectory: 0.1, goal: 0.1} + openarm_right_joint5: {trajectory: 0.1, goal: 0.1} + openarm_right_joint6: {trajectory: 0.1, goal: 0.1} + openarm_right_joint7: {trajectory: 0.1, goal: 0.1} + +/**/left_forward_position_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + +/**/right_forward_position_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + +/**/left_gripper_controller: + ros__parameters: + joints: + - openarm_left_finger_joint1 + interface_name: position + +/**/right_gripper_controller: + ros__parameters: + joints: + - openarm_right_finger_joint1 + interface_name: position diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml new file mode 100644 index 00000000..89a81171 --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml @@ -0,0 +1,100 @@ +# Copyright 2025 Enactic, 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. + +# Controller configuration for OpenArm with OY Motors + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: position_controllers/GripperActionController + +forward_position_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + + +forward_velocity_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +joint_trajectory_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + + command_interfaces: + - position + state_interfaces: + - position + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + + +gripper_controller: + ros__parameters: + joint: openarm_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index 99250a5c..73c0b4b1 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -82,7 +82,7 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description if namespace: controllers_file_str = controllers_file_str.replace( - "openarm_v10_bimanual_controllers.yaml", "openarm_v10_bimanual_controllers_namespaced.yaml" + "openarm_oy_bimanual_controllers.yaml", "openarm_oy_bimanual_controllers_namespaced.yaml" ) robot_state_pub_node = Node( package="robot_state_publisher", @@ -144,13 +144,13 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", description="URDF/XACRO description file with the robot.", ), DeclareLaunchArgument( "arm_type", - default_value="v10", - description="Type of arm (e.g., v10).", + default_value="oy", + description="Type of arm (e.g., oy, v10).", ), DeclareLaunchArgument( "use_fake_hardware", @@ -186,7 +186,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", + default_value="openarm_oy_bimanual_controllers.yaml", description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", ), ] @@ -205,7 +205,7 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py index 847bbd0d..b67446f3 100644 --- a/openarm_bringup/launch/openarm.launch.py +++ b/openarm_bringup/launch/openarm.launch.py @@ -108,13 +108,13 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", description="URDF/XACRO description file with the robot.", ), DeclareLaunchArgument( "arm_type", - default_value="v10", - description="Type of arm (e.g., v10).", + default_value="oy", + description="Type of arm (e.g., oy, v10).", ), DeclareLaunchArgument( "use_fake_hardware", @@ -145,7 +145,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_controllers.yaml", + default_value="openarm_oy_controllers.yaml", description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", ), ] @@ -163,7 +163,7 @@ def generate_launch_description(): # Configuration file paths controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) # Robot nodes spawner (both state publisher and control) diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index 4c6f29cd..5c648d92 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -60,7 +60,7 @@ bool OpenArm_oyHW::parse_config(const hardware_interface::HardwareInfo& info) { can_fd_ = (value == "true"); } - RCLCPP_INFO(rclcpp::get_logger("OpenArmOYHW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Configuration: CAN=%s, arm_prefix=%s, arm_dof=%zu, hand=%s, can_fd=%s", can_interface_.c_str(), arm_prefix_.c_str(), ARM_DOF, hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); From 61a674bf4a2a5adf6703c444909b51ec063765ea Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 22 Jan 2026 12:00:53 +0800 Subject: [PATCH 08/16] update srdf config and add some comments --- .../config/openarm_bimanual.srdf | 4 +-- .../launch/demo.launch.py | 35 +++++++++++++++++++ openarm_hardware/openarm_hardware.xml | 4 +-- 3 files changed, 39 insertions(+), 4 deletions(-) diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf index 8c2c6e1d..632c1f58 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf @@ -104,8 +104,8 @@ - - + + diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py index 62dff55d..a1ae036a 100644 --- a/openarm_bimanual_moveit_config/launch/demo.launch.py +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -139,6 +139,41 @@ def controller_spawner(context: LaunchContext, robot_controller): def generate_launch_description(): + """Generate launch description for OpenARM bimanual robot with MoveIt2 configuration. + + This function creates a complete launch description that sets up a dual-arm (bimanual) + OpenARM robot system with MoveIt2 motion planning capabilities. + + The launch description includes: + - Robot description and state publisher nodes via OpaqueFunction + - Joint state broadcaster for publishing joint states + - Arm trajectory controllers (forward position or joint trajectory) + - Gripper controllers for both left and right grippers + - MoveIt2 move_group node for motion planning + - RViz2 visualization with MoveIt configuration + + Launch Arguments: + description_package (str): Package containing robot description. Default: "openarm_description" + description_file (str): URDF/xacro file name. Default: "oy.urdf.xacro" + arm_type (str): Type of arm configuration. Default: "oy" + use_fake_hardware (str): Whether to use simulated hardware. Default: "false" + robot_controller (str): Controller type to use. Default: "joint_trajectory_controller" + Choices: "forward_position_controller", "joint_trajectory_controller" + runtime_config_package (str): Package with runtime configs. Default: "openarm_bringup" + arm_prefix (str): Prefix for arm links/joints. Default: "" + right_can_interface (str): CAN interface for right arm. Default: "can0" + left_can_interface (str): CAN interface for left arm. Default: "can1" + controllers_file (str): Controllers YAML config file. Default: "openarm_oy_bimanual_controllers.yaml" + + Note: + OpaqueFunction is used to defer node creation until launch time, allowing + dynamic configuration based on launch arguments that are only resolved + at runtime. This is necessary when node parameters depend on substitutions + that cannot be evaluated during launch file parsing. + + Returns: + LaunchDescription: Complete launch description for the bimanual robot system. + """ declared_arguments = [ DeclareLaunchArgument( "description_package", diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index 3446dd94..8e4e3f24 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -15,13 +15,13 @@ --> - ros2_control hardware interface for OpenArm V10 (Damiao Motors). - + --> From c0b7ac6af1218e2faeb39f8adff07dc82e77e75f Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 29 Jan 2026 10:58:48 +0800 Subject: [PATCH 09/16] update calling function --- openarm_bringup/launch/openarm.bimanual.launch.py | 2 ++ openarm_hardware/src/oy_hardware.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index 73c0b4b1..221f68a8 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -64,6 +64,8 @@ def generate_robot_description(context: LaunchContext, description_package, desc "left_can_interface": left_can_interface_str, } ).toprettyxml(indent=" ") + # 輸出robot_description + # print(robot_description) return robot_description diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index 5c648d92..b29318cd 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -278,14 +278,14 @@ hardware_interface::return_type OpenArm_oyHW::write( arm_params.push_back({pos_commands_[i], vel_commands_[i], kp, kd, tau_commands_[i]}); } - openarm_->get_arm().mit_control_all(arm_params); + openarm_->get_arm().oy_mit_control_all(arm_params); // Control gripper if enabled if (hand_ && joint_names_.size() > ARM_DOF) { // TODO the true mappings are unimplemented. double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); // OY motor MITParam order: {q, dq, kp, kd, tau} - openarm_->get_gripper().mit_control_all( + openarm_->get_gripper().oy_mit_control_all( {{motor_command, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); } openarm_->recv_all(1000); @@ -304,12 +304,12 @@ void OpenArm_oyHW::return_to_zero() { double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); arm_params.push_back({0.0, 0.0, kp, kd, 0.0}); } - openarm_->get_arm().mit_control_all(arm_params); + openarm_->get_arm().oy_mit_control_all(arm_params); // Return gripper to zero if enabled if (hand_) { // OY motor MITParam order: {q, dq, kp, kd, tau} - openarm_->get_gripper().mit_control_all( + openarm_->get_gripper().oy_mit_control_all( {{GRIPPER_JOINT_0_POSITION, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); } std::this_thread::sleep_for(std::chrono::microseconds(1000)); From 8d61c61620e5322c82d14e61360dbcb068547ead Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 5 Feb 2026 18:10:43 +0800 Subject: [PATCH 10/16] modify kp kd value --- openarm_hardware/include/openarm_hardware/oy_hardware.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index c5247b15..fb22bec5 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -105,8 +105,8 @@ class OpenArm_oyHW : public hardware_interface::SystemInterface { // Order: Joint 1-7 + Gripper const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, 5.0, 5.0, 5.0, 0.5}; - const std::vector DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, - 0.7, 0.6, 0.5, 0.1}; + const std::vector DEFAULT_KD = {2.75, 2.5, 0.2, 0.2, + 0.2, 0.2, 2.75, 0.1}; const double GRIPPER_JOINT_0_POSITION = 0.044; const double GRIPPER_JOINT_1_POSITION = 0.0; From 069941ede807f248465688f3c94b8a58dc546799 Mon Sep 17 00:00:00 2001 From: ligx Date: Thu, 26 Feb 2026 16:32:38 +0800 Subject: [PATCH 11/16] update kp kd value --- .../include/openarm_hardware/oy_hardware.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index fb22bec5..d5d82881 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -103,10 +103,11 @@ class OpenArm_oyHW : public hardware_interface::SystemInterface { // Default gains for OY motors (may need tuning) // Order: Joint 1-7 + Gripper - const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, - 5.0, 5.0, 5.0, 0.5}; - const std::vector DEFAULT_KD = {2.75, 2.5, 0.2, 0.2, - 0.2, 0.2, 2.75, 0.1}; + // 針對帶載機械臂優化（減小 KD 消除震動） +const std::vector DEFAULT_KP = {100.0, 50.0, 40.0, 30.0, + 20.0, 15.0, 10.0, 2.0}; +const std::vector DEFAULT_KD = {3.0, 3.0, 1.2, 1.0, + 0.6, 0.3, 0.4, 0.15}; const double GRIPPER_JOINT_0_POSITION = 0.044; const double GRIPPER_JOINT_1_POSITION = 0.0; From e104e924c8c8d7a47e90753afab8283c4157e7d1 Mon Sep 17 00:00:00 2001 From: ligx Date: Tue, 3 Mar 2026 11:11:58 +0800 Subject: [PATCH 12/16] update kp value --- .../include/openarm_hardware/oy_hardware.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index d5d82881..ef3fa9a3 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -104,11 +104,17 @@ class OpenArm_oyHW : public hardware_interface::SystemInterface { // Default gains for OY motors (may need tuning) // Order: Joint 1-7 + Gripper // 針對帶載機械臂優化（減小 KD 消除震動） -const std::vector DEFAULT_KP = {100.0, 50.0, 40.0, 30.0, + const std::vector DEFAULT_KP = {60.0, 80.0, 20.0, 20.0, 20.0, 15.0, 10.0, 2.0}; -const std::vector DEFAULT_KD = {3.0, 3.0, 1.2, 1.0, + const std::vector DEFAULT_KD = {3, 1.5, 1.2, 1.0, 0.6, 0.3, 0.4, 0.15}; + // Default gains +// const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, +// 5.0, 5.0, 5.0, 0.5}; +// const std::vector DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, +// 0.7, 0.6, 0.5, 0.1}; + const double GRIPPER_JOINT_0_POSITION = 0.044; const double GRIPPER_JOINT_1_POSITION = 0.0; const double GRIPPER_MOTOR_0_RADIANS = 0.0; From efbd450f8749883929e6a7dd12ff305a662e0c19 Mon Sep 17 00:00:00 2001 From: StartAgainAgain <96602888+StartAgainAgain@users.noreply.github.com> Date: Tue, 3 Mar 2026 12:00:39 +0800 Subject: [PATCH 13/16] Update repository URL and version for openarm_can --- openarm.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/openarm.repos b/openarm.repos index b3cce359..e90cbb20 100644 --- a/openarm.repos +++ b/openarm.repos @@ -15,5 +15,5 @@ repositories: openarm_can: type: git - url: https://github.com/enactic/openarm_can.git - version: main \ No newline at end of file + url: git@github.com:openvmi/openarm_can.git + version: oy_openarm_can From 3ad5a5b22eb9ccdf2734946a521519e65d68024e Mon Sep 17 00:00:00 2001 From: ligx Date: Fri, 6 Mar 2026 09:12:57 +0800 Subject: [PATCH 14/16] decrease kp to 2 kd to 0.2 for end motor --- .../include/openarm_hardware/oy_hardware.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index ef3fa9a3..5152cd4b 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -103,11 +103,11 @@ class OpenArm_oyHW : public hardware_interface::SystemInterface { // Default gains for OY motors (may need tuning) // Order: Joint 1-7 + Gripper - // 針對帶載機械臂優化（減小 KD 消除震動） - const std::vector DEFAULT_KP = {60.0, 80.0, 20.0, 20.0, - 20.0, 15.0, 10.0, 2.0}; - const std::vector DEFAULT_KD = {3, 1.5, 1.2, 1.0, - 0.6, 0.3, 0.4, 0.15}; + // 帶載機械臂優化：降低 KP 避免過衝，適當提高 KD 增加阻尼抑制震盪 + const std::vector DEFAULT_KP = {35.0, 35.0, 15.0, 15.0, + 2.0, 2.0, 2.0, 2.0}; + const std::vector DEFAULT_KD = {3.5, 3.5, 1.5, 1.2, + 0.2, 0.2, 0.2, 0.15}; // Default gains // const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, From 57e2629096ef66c22dba0ba54010774be6e2f256 Mon Sep 17 00:00:00 2001 From: ligx Date: Mon, 9 Mar 2026 10:19:19 +0800 Subject: [PATCH 15/16] =?UTF-8?q?=E7=B5=A6joint=5Ftrajectory=20controller?= =?UTF-8?q?=20=E6=B7=BB=E5=8A=A0=20=E6=84=9B=E9=80=9F=E5=BA=A6=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=8F=83=E6=95=B8=EF=BC=8C=E9=98=B2=E6=AD=A2=E6=8A=96?= =?UTF-8?q?=E5=8B=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ros2_controllers.yaml | 2 ++ .../openarm_oy_bimanual_controllers.yaml | 8 ++++++-- .../openarm_oy_bimanual_controllers_namespaced.yaml | 6 ++++-- .../oy_controllers/openarm_oy_controllers.yaml | 4 +++- .../openarm_v10_bimanual_controllers.yaml | 8 ++++++-- ...openarm_v10_bimanual_controllers_namespaced.yaml | 6 ++++-- .../v10_controllers/openarm_v10_controllers.yaml | 4 +++- .../include/openarm_hardware/oy_hardware.hpp | 4 ++-- openarm_hardware/src/oy_hardware.cpp | 13 +++++++++++++ 9 files changed, 43 insertions(+), 12 deletions(-) diff --git a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml index de64f007..f878ee15 100644 --- a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml +++ b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml @@ -44,6 +44,7 @@ left_arm_controller: - openarm_left_joint7 command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -61,6 +62,7 @@ right_arm_controller: - openarm_right_joint7 command_interfaces: - position + - velocity state_interfaces: - position - velocity diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml index 8a386eec..ab6c8eaa 100644 --- a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml @@ -90,15 +90,17 @@ left_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) left_gripper_controller: @@ -155,15 +157,17 @@ right_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) right_gripper_controller: diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml index ca0b9721..3e2ceee7 100644 --- a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml @@ -50,6 +50,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -59,7 +60,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_left_joint1: {trajectory: 0.1, goal: 0.1} openarm_left_joint2: {trajectory: 0.1, goal: 0.1} @@ -82,6 +83,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -91,7 +93,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_right_joint1: {trajectory: 0.1, goal: 0.1} openarm_right_joint2: {trajectory: 0.1, goal: 0.1} diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml index 89a81171..b696e647 100644 --- a/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml +++ b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml @@ -79,15 +79,17 @@ joint_trajectory_controller: command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml index 8a386eec..ab6c8eaa 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml @@ -90,15 +90,17 @@ left_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) left_gripper_controller: @@ -155,15 +157,17 @@ right_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) right_gripper_controller: diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml index ca0b9721..3e2ceee7 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml @@ -50,6 +50,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -59,7 +60,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_left_joint1: {trajectory: 0.1, goal: 0.1} openarm_left_joint2: {trajectory: 0.1, goal: 0.1} @@ -82,6 +83,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -91,7 +93,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_right_joint1: {trajectory: 0.1, goal: 0.1} openarm_right_joint2: {trajectory: 0.1, goal: 0.1} diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml index f2f3a03c..89646639 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml @@ -78,15 +78,17 @@ joint_trajectory_controller: command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp index 5152cd4b..342a35f1 100644 --- a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -104,9 +104,9 @@ class OpenArm_oyHW : public hardware_interface::SystemInterface { // Default gains for OY motors (may need tuning) // Order: Joint 1-7 + Gripper // 帶載機械臂優化：降低 KP 避免過衝，適當提高 KD 增加阻尼抑制震盪 - const std::vector DEFAULT_KP = {35.0, 35.0, 15.0, 15.0, + const std::vector DEFAULT_KP = {40.0, 40.0, 15.0, 15.0, 2.0, 2.0, 2.0, 2.0}; - const std::vector DEFAULT_KD = {3.5, 3.5, 1.5, 1.2, + const std::vector DEFAULT_KD = {2.0, 2.0, 1.5, 1.2, 0.2, 0.2, 0.2, 0.15}; // Default gains diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp index b29318cd..1efab5d9 100644 --- a/openarm_hardware/src/oy_hardware.cpp +++ b/openarm_hardware/src/oy_hardware.cpp @@ -247,6 +247,12 @@ hardware_interface::return_type OpenArm_oyHW::read( pos_states_[i] = arm_motors[i].get_position(); vel_states_[i] = arm_motors[i].get_velocity(); tau_states_[i] = arm_motors[i].get_torque(); + //print joint name and states for debugging + // RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Joint %s: pos=%.3f, vel=%.3f, tau=%.3f", + // joint_names_[i].c_str(), + // pos_states_[i], + // vel_states_[i], + // tau_states_[i]); } // Read gripper state if enabled @@ -275,9 +281,16 @@ hardware_interface::return_type OpenArm_oyHW::write( for (size_t i = 0; i < ARM_DOF; ++i) { double kp = (i < DEFAULT_KP.size()) ? DEFAULT_KP[i] : DEFAULT_KP.back(); double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); + //print kp kd pos vel tau for debugging + // if( i == 0 && pos_commands_[i] != 0.0) { // Only print for the first joint if there's a command + // RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + // "Commanding joint %zu: pos_cmd=%.4f, vel_cmd=%.4f, kp=%.2f, kd=%.2f, tau_cmd=%.4f", + // i, pos_commands_[i], vel_commands_[i], kp, kd, tau_commands_[i]); + // } arm_params.push_back({pos_commands_[i], vel_commands_[i], kp, kd, tau_commands_[i]}); } + openarm_->get_arm().oy_mit_control_all(arm_params); // Control gripper if enabled From bb7b88e8b3ed005bedbfc9c246016d9c3b8f70cb Mon Sep 17 00:00:00 2001 From: ligx Date: Mon, 9 Mar 2026 14:52:17 +0800 Subject: [PATCH 16/16] update launch config --- openarm_bringup/launch/openarm.launch.py | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py index b67446f3..c9df16eb 100644 --- a/openarm_bringup/launch/openarm.launch.py +++ b/openarm_bringup/launch/openarm.launch.py @@ -90,7 +90,8 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description package="controller_manager", executable="ros2_control_node", output="both", - parameters=[robot_description_param, controllers_file_str], + parameters=[controllers_file_str], + remappings=[("~/robot_description", "/robot_description")], ) return [robot_state_pub_node, control_node] @@ -198,13 +199,13 @@ def generate_launch_description(): robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[robot_controller, "-c", "/controller_manager"], + arguments=[robot_controller, "--controller-manager", "/controller_manager"], ) gripper_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["gripper_controller", "-c", "/controller_manager"], + arguments=["gripper_controller", "--controller-manager", "/controller_manager"], ) # Timing and sequencing @@ -213,13 +214,17 @@ def generate_launch_description(): actions=[joint_state_broadcaster_spawner], ) - delayed_robot_controller = TimerAction( - period=1.0, - actions=[robot_controller_spawner], + delayed_robot_controller = RegisterEventHandler( + OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) ) - delayed_gripper_controller = TimerAction( - period=1.0, - actions=[gripper_controller_spawner], + delayed_gripper_controller = RegisterEventHandler( + OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[gripper_controller_spawner], + ) ) return LaunchDescription(