From e66aaf6efc9dd83085220bba3cd1326915c5b45e Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sat, 29 Nov 2025 02:02:15 -0500 Subject: [PATCH 01/10] added update rate to rmd/smc, cleaned up rmd/smc, first draft servo hwi --- .../arm/arm.rmd.ros2_control.xacro | 18 +- .../arm/arm.servo.ros2_control.xacro | 6 + .../arm/arm.smc.ros2_control.xacro | 38 +- .../drive/drive.rmd.ros2_control.xacro | 34 +- .../rmd_hardware_interface.hpp | 16 +- .../src/rmd_hardware_interface.cpp | 252 ++++++-------- .../servo_hardware_interface.hpp | 67 ++-- .../src/servo_hardware_interface.cpp | 329 ++++++++++++++---- .../smc_hardware_interface.hpp | 6 + .../src/smc_hardware_interface.cpp | 182 +++++----- .../config/athena_arm_controllers.yaml | 6 +- .../manual_arm_joint_by_joint_controller.cpp | 2 +- 12 files changed, 562 insertions(+), 394 deletions(-) diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index 7d09e83b..b5b4c959 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -6,27 +6,25 @@ rmd_ros2_control/RMDHardwareInterface + 50 + can0 - 330 - 586 + 0x14A 100 1.0 - - -1.0 - + 150 + - 324 - 580 + 0x144 100 1.0 - - -2.0 - + 150 + diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index 9deb797e..ccf86f4d 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -7,9 +7,15 @@ servo_ros2_control/SERVOHardwareInterface + 10 + can0 + 0x80 + 0x1 + 1 + diff --git a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro index 30ce0a9e..1cd3a1fc 100644 --- a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro @@ -7,47 +7,37 @@ smc_ros2_control/SMCHardwareInterface + 10 + can0 - 325 + 0x145 40 - - ${-2*pi} - ${2*pi} - + 300 + - - 0.0 - + - 326 + 0x146 40 - - ${-2*pi} - ${2*pi} - + 1000 + - - 0.0 - + - 327 + 0x147 40 - - ${-pi} - ${pi} - + 1000 + - - 0.0 - + diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index d21547a8..2739e403 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -6,57 +6,51 @@ rmd_ros2_control/RMDHardwareInterface + 100 + can0 - 323 - 579 + 143 1 -1.0 + 150 - - 0.0 - + - 322 - 578 + 142 1 1.0 + 150 - - 0.0 - + - 321 - 577 + 0x141 1 -1.0 + 150 - - 0.0 - + - 329 - 585 + 0x149 1 1.0 + 150 - - 0.0 - + diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index 0e9fc460..f02d0a30 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -98,10 +98,9 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher private: int num_joints; - int current_joint; - - // Mutex - std::mutex state_mutex_; + int update_rate; + double elapsed_update_time; + std::string can_interface; // Store the state for the simulated robot std::vector joint_state_position_; @@ -111,16 +110,19 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher std::vector joint_command_position_; std::vector joint_command_velocity_; - std::vector encoder_position; + // Place holders for data from the canBus, will be accessed in read() std::vector motor_velocity; std::vector motor_position; + + // Velocity at which **joint** rotates to reach position in 1 dps + uint16_t operating_velocity; CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; - std::vector joint_node_write_ids; - std::vector joint_node_read_ids; + std::vector joint_node_write_ids; + std::vector joint_node_read_ids; std::vector joint_gear_ratios; std::vector joint_orientation; diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 0f287fd0..2c1efc73 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/lexical_casts.hpp" @@ -52,19 +53,28 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor + // Setting Parameters for (auto& joint : info_.joints) { - joint_node_write_ids.push_back(std::stoi(joint.parameters.at("node_write_id"))); - joint_node_read_ids.push_back(std::stoi(joint.parameters.at("node_read_id"))); - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation"))); + int write_id = std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160); + int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + joint_node_write_ids.push_back(write_id); + joint_node_read_ids.push_back(write_id+0x100); + + joint_gear_ratios.push_back(gear_ratio); + joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1); + operating_velocity = std::clamp( + std::stoi(joint.parameters.at("operating_velocity")), + 0, + static_cast(3.5 * gear_ratio) + ); + } num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + can_interface = info_.hardware_parameters.at("can_interface"); + + elapsed_update_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -73,14 +83,11 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); - encoder_position.assign(num_joints, 0.0); motor_position.assign(num_joints, 0.0); motor_velocity.assign(num_joints, 0.0); control_level_.resize(num_joints, integration_level_t::POSITION); - current_joint = 0; - for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); } @@ -134,7 +141,7 @@ RMDHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open("can0", std::bind(&RMDHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&RMDHardwareInterface::onCanMessage, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -150,10 +157,11 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == joint_node_read_ids[i] && can_rx_frame_.data[0] == 0x9C){ + if(can_rx_frame_.id == joint_node_read_ids[i] && + (can_rx_frame_.data[0] == 0xA2 || can_rx_frame_.data[0] == 0xA4)){ - // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; + // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) + // data[0] = 0x9C; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -162,43 +170,13 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { data[6] = can_rx_frame_.data[6]; // encoder position low byte data[7] = can_rx_frame_.data[7]; // encoder position high byte - // ENCODER POSITION + // POSITION // uint16 -> int16 -> double (for calcs) - encoder_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); + motor_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); // SPEED // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); - - // CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - - } - else if(can_rx_frame_.id == joint_node_read_ids[i] && can_rx_frame_.data[0] == 0x92){ - - // DECODING CAN MESSAGE FOR POSITION - data[0] = 0x92; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = can_rx_frame_.data[4]; // Multi-turn low byte - data[5] = can_rx_frame_.data[5]; - data[6] = can_rx_frame_.data[6]; - data[7] = can_rx_frame_.data[7]; // Multi-turn high byte - - // POSITION - // uint32 -> int32 -> double (for calcs) - motor_position[i] = static_cast(static_cast((data[7] << 24) | (data[6] << 16) | (data[5] << 8) | data[4])); - - if(DEBUG_MODE == 1) { - for(int j = 0; j < 8; j++){ - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Received at index %d: %d", j, can_rx_frame_.data[j]); - } - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Motor Position: %f", motor_position[i]); - } - - // CALCULATING JOINT STATE - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); } else{ if(DEBUG_MODE == 1) { @@ -308,130 +286,104 @@ int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_veloci hardware_interface::return_type RMDHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - std::lock_guard lock(state_mutex_); - - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - current_joint+=1; - current_joint = current_joint % num_joints; +{ for(int i = 0; i < num_joints; i++) { - if(current_joint == i){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Command to read multi-turn angle - can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + // CALCULATING JOINT STATE + joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); + joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - // Command to read motor status 2 - can_tx_frame_.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // // CALCULATING JOINT STATE - // joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - // joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); - } + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", + info_.joints[i].name.c_str(), + motor_position[i], + joint_state_position_[i], + joint_state_velocity_[i]); } } - - // // Values retrieved - // for(int i = 0; i < num_joints; i++){ - // if(current_joint == i){ - // // CALCULATING JOINT STATE - // joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - // joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - // if(DEBUG_MODE == 1) { - // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - // info_.joints[i].name.c_str(), - // motor_position[i], - // joint_state_position_[i], - // joint_state_velocity_[i]); - // } - // } - // } return hardware_interface::return_type::OK; } hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + elapsed_update_time+=period.seconds(); int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; int32_t joint_angle = 0; - int16_t operating_velocity = 200; int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + double update_period = 1.0/update_rate; - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - - } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(update_period > elapsed_update_time){ + return hardware_interface::return_type::OK; + } + else{ + elapsed_update_time = 0.0; + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = joint_node_write_ids[i]; + can_tx_frame_.dlc = 8; - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); + // CALCULATE DESIRED JOINT ANGLE + joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", + info_.joints[i].name.c_str(), + joint_angle, + joint_command_position_[i]); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA4; + data[1] = 0x00; + data[2] = operating_velocity & 0xFF; + data[3] = (operating_velocity >> 8) & 0xFF; + data[4] = joint_angle & 0xFF; + data[5] = (joint_angle >> 8) & 0xFF; + data[6] = (joint_angle >> 16) & 0xFF; + data[7] = (joint_angle >> 24) & 0xFF; + } + else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", + info_.joints[i].name.c_str(), + joint_velocity); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA2; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state"); - } + } + else{ + // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state"); + } - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + canBus.send(can_tx_frame_); } - - canBus.send(can_tx_frame_); } return hardware_interface::return_type::OK; diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index a44628d6..fb2163bd 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #ifndef SERVO_HARDWARE_INTERACE_HPP_ #define SERVO_HARDWARE_INTERACE_HPP_ @@ -38,7 +20,9 @@ #include #include #include -#include "msgs/msg/cana.hpp" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" namespace servo_ros2_control { @@ -57,11 +41,13 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector export_command_interfaces() override; - // Establish communications and set initial values for state and command interfaces - // hardware_interface::CallbackReturn on_configure( - // const rclcpp_lifecycle::State & previous_state) override; - // Lifecycle + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -82,9 +68,25 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + void onCanMessage(const CANLib::CanFrame& frame); + + // Helper Functions + double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); + double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); + + int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); + private: int num_joints; + int current_joint; + int update_rate; + double elapsed_update_time; + std::string can_interface; + + int can_command_id; + int can_response_id; // Store the state for the simulated robot std::vector joint_state_position_; @@ -94,19 +96,17 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector joint_command_position_; std::vector joint_command_velocity_; - double encoder_position; - double motor_speed; + std::vector encoder_position; + std::vector motor_velocity; + std::vector motor_position; - rclcpp::Publisher::SharedPtr actuator_can_publisher_; - rclcpp::Subscription::SharedPtr actuator_can_subscriber_; - rclcpp::Node::SharedPtr node_; - - msgs::msg::CANA received_joint_data_; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; std::vector joint_node_ids; std::vector joint_gear_ratios; - enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -114,12 +114,11 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh VELOCITY = 2, }; - // Active control mode for each servok + // Active control mode for each actuator std::vector control_level_; }; } // namespace servo_hardware_interface -#endif // SERVO_HARDWARE_INTERACE_HPP_ - +#endif // SERVO_HARDWARE_INTERACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index 1afbecd8..10f365c5 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #include "servo_ros2_control/servo_hardware_interface.hpp" #include @@ -36,6 +18,10 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#define DEBUG_MODE 0 // 0 for off 1 for on + +using std::placeholders::_1; + namespace servo_ros2_control { hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( @@ -48,51 +34,47 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - // This stores the can ids for each joint aka motor - /* for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - } */ + for (auto& joint : info_.joints) { + joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); + joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + } num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + can_interface = info_.hardware_parameters.at("can_interface"); + can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); + can_response_id = can_command_id+0x1; // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0); + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_state_velocity_.assign(num_joints, 0); - joint_command_position_.assign(num_joints, 0); + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); - encoder_position = 0; - motor_speed = 0; + encoder_position.assign(num_joints, 0.0); + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); - control_level_.resize(num_joints, integration_level_t::UNDEFINED); + control_level_.resize(num_joints, integration_level_t::POSITION); - node_ = rclcpp::Node::make_shared("servo_hardware_node"); - actuator_can_publisher_ = node_->create_publisher("can_tx", 10); + current_joint = 0; - // Lambda function that takes the message as a shared pointer, dereferences it, - // and stores it in received_joint_data_ to be used - actuator_can_subscriber_ = node_->create_subscription( - "can_rx", - 10, - [this](const msgs::msg::CANA::SharedPtr received_message) - { - received_joint_data_ = *received_message; - }); + for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); + } return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn SERVOHardwareInterface::on_shutdown( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & previous_state) { - // may not need this either - return hardware_interface::CallbackReturn::SUCCESS; + // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. + // Need this in case on_cleanup never gets called + return this->on_cleanup(previous_state); } @@ -129,12 +111,101 @@ SERVOHardwareInterface::export_command_interfaces() return command_interfaces; } +hardware_interface::CallbackReturn SERVOHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!canBus.open(can_interface, std::bind(&SERVOHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { + can_rx_frame_ = frame; // Store the received frame for processing in read() + + std::string result; + + int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int8_t command_nibble = ((can_rx_frame_.data[0]>>4) & 0x0F); + int8_t device_id_nibble = (can_rx_frame_.data[0] & 0x0F); + double raw_motor_position = 0.0; + double raw_motor_velocity = 0.0; + + for(int i = 0; i < num_joints; i++){ + if(can_rx_frame_.id == can_response_id && command_nibble == 0x07 && device_id_nibble == joint_node_ids[i]){ + + // DECODING CAN MESSAGE FOR VELOCITY + data[0] = 0x9C; + data[1] = can_rx_frame_.data[1]; // Motor Temperature + data[2] = can_rx_frame_.data[2]; // Position low byte + data[3] = can_rx_frame_.data[3]; // Position high byte + data[4] = can_rx_frame_.data[4]; // Velocity low byte + data[5] = can_rx_frame_.data[5]; // Velocity high byte + data[6] = can_rx_frame_.data[6]; // Torque position low byte + data[7] = can_rx_frame_.data[7]; // Torque position high byte + + // POSITION + // uint32 -> int16 -> double (for calcs) + raw_motor_position = static_cast(static_cast((data[3] << 8) | data[2])); + + // VELOCITY + // uint16 -> int16 -> double (for calcs) + raw_motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); + + // TORQUE + // TODO + + // CALCULATING JOINT STATE + motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = calculate_joint_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + } + else{ + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); + } + } + } + +} + +hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up ...please wait..."); + int8_t command_nibble = 0x5; + int8_t device_id_nibble; + // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here + for(int i = 0; i < num_joints; i++){ + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 1; + + // Motor Shutdown Command + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + canBus.send(can_tx_frame_); + } + + canBus.close(); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up successful!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} hardware_interface::CallbackReturn SERVOHardwareInterface::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Activating ...please wait..."); + // Sets initial command to joint state + joint_command_position_ = joint_state_position_; + + for (size_t i = 0; i < joint_command_position_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + } + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -144,18 +215,81 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_activate( hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - auto joint_tx = msgs::msg::CANA(); RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Deactivating ...please wait..."); + int8_t command_nibble = 0x6; + int8_t device_id_nibble; + + for(int i = 0; i < num_joints; i++){ + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 1; + + // Motor Stop Command + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + canBus.send(can_tx_frame_); + } RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully deactivated all SERVO motors!"); return hardware_interface::CallbackReturn::SUCCESS; } +double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from 0.1 deg to deg to radians + return (motor_position * 0.1 * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +int32_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg -> 0.01 deg + return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +} + +int32_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s -> 0.01 deg/s + return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +} + + hardware_interface::return_type SERVOHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + int8_t command_nibble = 0x7; + int8_t device_id_nibble; + + current_joint+=1; + current_joint = current_joint % num_joints; + for(int i = 0; i < num_joints; i++) { + if(current_joint == i){ + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 1; + + // Command to read motor status command + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + canBus.send(can_tx_frame_); + + // CALCULATING JOINT STATE + joint_state_velocity_[i] = motor_velocity[i]; + joint_state_position_[i] = motor_position[i]; + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", + info_.joints[i].name.c_str(), + motor_position[i], + joint_state_position_[i], + joint_state_velocity_[i]); + } + } + } + return hardware_interface::return_type::OK; } @@ -163,6 +297,74 @@ hardware_interface::return_type SERVOHardwareInterface::read( hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int32_t joint_angle = 0; + int16_t operating_velocity = 20; // dps + int32_t joint_velocity = 0; + + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 8; + + if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + + // CALCULATE DESIRED JOINT ANGLE + joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", + info_.joints[i].name.c_str(), + joint_angle, + joint_command_position_[i]); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA4; + data[1] = 0x00; + data[2] = operating_velocity & 0xFF; + data[3] = (operating_velocity >> 8) & 0xFF; + data[4] = joint_angle & 0xFF; + data[5] = (joint_angle >> 8) & 0xFF; + data[6] = (joint_angle >> 16) & 0xFF; + data[7] = (joint_angle >> 24) & 0xFF; + + } + else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", + info_.joints[i].name.c_str(), + joint_velocity); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA2; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; + + } + else{ + // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + } + + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + + canBus.send(can_tx_frame_); + } + return hardware_interface::return_type::OK; } @@ -177,29 +379,28 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi ss << "] stop_interfaces: ["; for (auto &s : stop_interfaces) ss << s << ","; ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "%s", ss.str().c_str()); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), ss.str().c_str()); // For each joint, decide its new control mode based on start/stop interfaces. + // We allow partial starts/stops: only affected joints are switched. std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - // -------- STOP INTERFACES -------- + // Process stop interfaces first: mark those joints as UNDEFINED for (const auto &ifname : stop_interfaces) { for (int i = 0; i < num_joints; ++i) { const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); - if (ifname == pos_if || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // -------- START INTERFACES -------- + // Process start interfaces: set POSITION or VELOCITY per joint for (const auto &ifname : start_interfaces) { for (int i = 0; i < num_joints; ++i) { const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); - if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -208,10 +409,12 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi } } - // -------- APPLY MODES -------- + // Now apply the requested_modes to control_level_. + // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. for (int i = 0; i < num_joints; ++i) { - if (requested_modes[i] == integration_level_t::UNDEFINED) { + // if stop requested, set to UNDEFINED; otherwise leave existing mode + // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; for (const auto &ifname : stop_interfaces) { if (ifname.find(info_.joints[i].name) != std::string::npos) { @@ -219,29 +422,29 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi break; } } - if (was_stopped) { control_level_[i] = integration_level_t::UNDEFINED; joint_command_velocity_[i] = 0; + // optional: reset position cmd to current state to be safe joint_command_position_[i] = joint_state_position_[i]; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: stopped -> UNDEFINED", - info_.joints[i].name.c_str()); + "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); } - + // else, leave control_level_ as-is } else { - + // Set the mode requested control_level_[i] = requested_modes[i]; - + // If switching to velocity, optionally set command velocity to current state to avoid jumps if (requested_modes[i] == integration_level_t::VELOCITY) { - joint_command_velocity_[i] = 0.0; + // joint_command_velocity_[i] = joint_state_velocity_[i]; + joint_command_velocity_[i] = 0; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to VELOCITY", - info_.joints[i].name.c_str()); + "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_velocity_[i]); } else if (requested_modes[i] == integration_level_t::POSITION) { joint_command_position_[i] = joint_state_position_[i]; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to POSITION (init pos=%f)", + "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", info_.joints[i].name.c_str(), joint_command_position_[i]); } } @@ -256,4 +459,4 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) + servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 70920c85..314ec901 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -97,6 +97,9 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher private: int num_joints; + int update_rate; + double elapsed_update_time; + std::string can_interface; // Store the state for the simulated robot std::vector joint_state_position_; @@ -109,6 +112,9 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher std::vector encoder_position; std::vector motor_velocity; std::vector motor_position; + + // Velocity at which **joint** rotates to reach position in 1 dps + uint16_t operating_velocity; std::vector joint_initialization_; diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 5e204761..304c8c71 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/lexical_casts.hpp" @@ -50,17 +51,31 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor + // Setting Parameters for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); + int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160)); + joint_gear_ratios.push_back(gear_ratio); + operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); } + // Build a single log message + std::stringstream ss; + for (size_t i = 0; i < info_.joints.size(); ++i) { + ss << "Joint " << info_.joints[i].name << " configured:\n" + << " node_id: 0x" << std::hex << joint_node_ids[i] << std::dec << "\n" + << " gear_ratio: " << joint_gear_ratios[i] << "\n"; + } + + // Log all joints at once + RCLCPP_INFO(rclcpp::get_logger("SMCHWI"), "%s", ss.str().c_str()); + + num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + can_interface = info_.hardware_parameters.at("can_interface"); + + elapsed_update_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -124,7 +139,7 @@ SMCHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open("can0", std::bind(&SMCHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&SMCHardwareInterface::onCanMessage, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -139,11 +154,13 @@ void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == joint_node_ids[i] && can_rx_frame_.data[0] == 0x9C){ + for(int i = 0; i < num_joints; i++){ + if(can_rx_frame_.id == joint_node_ids[i] && + (can_rx_frame_.data[0] == 0xA2 || can_rx_frame_.data[0] == 0xA4) && + can_rx_frame_.data[1] != 0x00){ // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; + // data[0] = 0x9C; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -308,24 +325,6 @@ int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_veloci hardware_interface::return_type SMCHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - - // Since you can't publish two messages in one sweep without a delay, every other iteration of read - // will call angle or velocity messages - for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - // Command to read multi-turn angle - can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // Command to read motor status 2 - can_tx_frame_.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - } - // Values retrieved for(int i = 0; i < num_joints; i++){ @@ -345,72 +344,91 @@ hardware_interface::return_type SMCHardwareInterface::read( hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + elapsed_update_time+=period.seconds(); int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; int32_t joint_angle = 0; int16_t operating_velocity = 200; int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d", - info_.joints[i].name.c_str(), - joint_angle); + double update_period = 1.0/update_rate; + + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(update_period > elapsed_update_time){ + return hardware_interface::return_type::OK; + } + else{ + elapsed_update_time = 0.0; + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = joint_node_ids[i]; + can_tx_frame_.dlc = 8; + + if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + + // CALCULATE DESIRED JOINT ANGLE + joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d", + info_.joints[i].name.c_str(), + joint_angle); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA4; + data[1] = 0x00; + data[2] = operating_velocity & 0xFF; + data[3] = (operating_velocity >> 8) & 0xFF; + data[4] = joint_angle & 0xFF; + data[5] = (joint_angle >> 8) & 0xFF; + data[6] = (joint_angle >> 16) & 0xFF; + data[7] = (joint_angle >> 24) & 0xFF; } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", + info_.joints[i].name.c_str(), + joint_velocity); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA2; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; + + } + else{ + // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state"); + } + + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; } - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state"); - } + // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Sent"); + + // Command for position/velocity control + canBus.send(can_tx_frame_); - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; + // Command to read multi-turn angle + can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + canBus.send(can_tx_frame_); } - - canBus.send(can_tx_frame_); } return hardware_interface::return_type::OK; diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 0708f2e7..22968485 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 10 # Hz + update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -106,7 +106,7 @@ manual_arm_joint_by_joint_controller: - elbow_pitch - wrist_pitch - wrist_roll - # - gripper_claw + - gripper_claw interface_name: velocity joint_max_velocities: - 62.83185307 # 10 rot/s, please change dawg @@ -114,4 +114,4 @@ manual_arm_joint_by_joint_controller: - 0.05148721 - 0.785398 # 45 dps - 0.785398 - # - 0.005 # TODO: Find this + - 0.005 # TODO: Find this diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index cdf10dca..9705f3ab 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -268,7 +268,7 @@ controller_interface::return_type ManualArmJointByJointController::update( // RCLCPP_INFO(get_node()->get_logger(), "Talon: %f", joint_velocities_[5]); } else{ - RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); + // RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); joint_velocities_.resize(6, 0.0); } From 396c99000631ea7ece9af854050c78e26265313a Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 1 Dec 2025 22:11:02 -0500 Subject: [PATCH 02/10] working drive setup setup --- .../ros2_control/drive/drive.rmd.ros2_control.xacro | 6 +++--- .../drive/drive_bringup/launch/athena_drive.launch.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index 2739e403..fdb6f5f6 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -6,12 +6,12 @@ rmd_ros2_control/RMDHardwareInterface - 100 + 50 can0 - 143 + 0x143 1 -1.0 150 @@ -22,7 +22,7 @@ - 142 + 0x142 1 1.0 150 diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 4594ea45..26f22e20 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -384,7 +384,7 @@ def generate_launch_description(): robot_state_pub_node, joystick_publisher, teleop_twist_joy, - joint_state_publisher, + # joint_state_publisher, # delay_can_node_after_control_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, delay_rviz_after_joint_state_broadcaster_spawner, From f282ad20f538e9b5e31d520420cd7ed8ac18ee1d Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Sun, 23 Nov 2025 03:33:42 -0500 Subject: [PATCH 03/10] populating servo hwi 1st draft --- .../rmd_hardware_interface.hpp | 18 --------------- .../src/rmd_hardware_interface.cpp | 23 ++++--------------- .../servo_hardware_interface.hpp | 2 +- 3 files changed, 5 insertions(+), 38 deletions(-) diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index f02d0a30..20ac8e15 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #ifndef RMD_HARDWARE_INTERACE_HPP_ #define RMD_HARDWARE_INTERACE_HPP_ diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 2c1efc73..59273600 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #include "rmd_ros2_control/rmd_hardware_interface.hpp" #include @@ -155,6 +137,8 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { std::string result; int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + double raw_motor_velocity = 0.0; + double raw_motor_position = 0.0; for(int i = 0; i < num_joints; i++){ if(can_rx_frame_.id == joint_node_read_ids[i] && @@ -174,7 +158,7 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { // uint16 -> int16 -> double (for calcs) motor_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); - // SPEED + // VELOCITY // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); } @@ -385,6 +369,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( canBus.send(can_tx_frame_); } } + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Period passed to write: %f seconds", period.seconds()); return hardware_interface::return_type::OK; } diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index fb2163bd..1fcddee8 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -121,4 +121,4 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh } // namespace servo_hardware_interface -#endif // SERVO_HARDWARE_INTERACE_HPP_ \ No newline at end of file +#endif // SERVO_HARDWARE_INTERACE_HPP_ From 86da53ee8effa1627cffcb9034d086d3d3d3b46a Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Tue, 30 Dec 2025 17:49:25 -0500 Subject: [PATCH 04/10] some progress on stepper hardware interface --- .../arm/arm.servo.ros2_control.xacro | 5 +- .../science/science.servo.ros2_control.xacro | 27 ++- .../src/rmd_hardware_interface.cpp | 6 +- .../servo_hardware_interface.hpp | 23 +- .../src/servo_hardware_interface.cpp | 227 ++++++++++-------- .../scripts/joystick_publisher.py | 2 +- .../teleop/teleop/joystick_publisher.py | 2 +- 7 files changed, 189 insertions(+), 103 deletions(-) diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index ccf86f4d..dc6d8310 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -4,7 +4,6 @@ - servo_ros2_control/SERVOHardwareInterface 10 @@ -15,7 +14,9 @@ 0x1 1 - + 90 + standard + prismatic diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index abc0f49e..4c6da7db 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -4,9 +4,11 @@ - servo_ros2_control/SERVOHardwareInterface + 10 + can0 + 0x80 @@ -24,28 +26,49 @@ + 0x2 + 1 + 178 + standard + revolute + + 0x3 + 1 + 178 + standard + revolute + 0x4 + 1 + 90 + standard + prismatic + + 0x5 + 1 + 270 + standard + prismatic - diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 59273600..919e5912 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -145,7 +145,6 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { (can_rx_frame_.data[0] == 0xA2 || can_rx_frame_.data[0] == 0xA4)){ // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) - // data[0] = 0x9C; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -369,7 +368,10 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( canBus.send(can_tx_frame_); } } - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Period passed to write: %f seconds", period.seconds()); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "RMD - Period passed to write: %f seconds", period.seconds()); + } return hardware_interface::return_type::OK; } diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index 1fcddee8..e146c0a9 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -99,6 +99,7 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector encoder_position; std::vector motor_velocity; std::vector motor_position; + std::vector rated_max; CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; @@ -107,7 +108,7 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector joint_node_ids; std::vector joint_gear_ratios; - enum integration_level_t : std::uint8_t + enum class integration_level_t : std::uint8_t { UNDEFINED = 0, POSITION = 1, @@ -117,6 +118,26 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Active control mode for each actuator std::vector control_level_; + enum class servo_type_t : std::uint8_t + { + UNDEFINED = 0, + STANDARD = 1, + CONTINUOUS = 2, + }; + + // Type of servo for each actuator + std::vector servo_type_; + + enum class joint_type_t : std::uint8_t + { + UNDEFINED = 0, + REVOLUTE = 1, + PRISMATIC = 2, + }; + + // Type of joint for each actuator + std::vector joint_type_; + }; } // namespace servo_hardware_interface diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index 10f365c5..d7c5eb48 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -18,7 +18,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on +#define DEBUG_MODE 1 // 0 for off 1 for on using std::placeholders::_1; @@ -34,17 +34,35 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - // This stores the can ids for each joint aka motor + // This stores the parameters for each joint for (auto& joint : info_.joints) { joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + rated_max.push_back(std::abs(std::stod(joint.parameters.at("rated_max")))*(M_PI/180.0)); // Convert to radians + + std::string servo_type = joint.parameters.at("servo_type"); + std::string joint_type = joint.parameters.at("joint_type"); + + if (servo_type != "standard" && servo_type != "continuous") { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' or 'continuous'.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } else { + servo_type_.push_back(joint.parameters.at("servo_type")); + } + + if (joint_type != "revolute" && joint_type != "prismatic") { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } else { + joint_type_.push_back(joint.parameters.at("joint_type")); + } } num_joints = static_cast(info_.joints.size()); update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); can_interface = info_.hardware_parameters.at("can_interface"); can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); - can_response_id = can_command_id+0x1; + can_response_id = can_command_id+0x01; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -134,32 +152,27 @@ void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { double raw_motor_velocity = 0.0; for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == can_response_id && command_nibble == 0x07 && device_id_nibble == joint_node_ids[i]){ + if(can_rx_frame_.id == can_response_id && command_nibble == 0x7 && device_id_nibble == joint_node_ids[i]){ // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; - data[1] = can_rx_frame_.data[1]; // Motor Temperature - data[2] = can_rx_frame_.data[2]; // Position low byte - data[3] = can_rx_frame_.data[3]; // Position high byte - data[4] = can_rx_frame_.data[4]; // Velocity low byte - data[5] = can_rx_frame_.data[5]; // Velocity high byte - data[6] = can_rx_frame_.data[6]; // Torque position low byte - data[7] = can_rx_frame_.data[7]; // Torque position high byte + data[1] = can_rx_frame_.data[1]; // Position low byte + data[2] = can_rx_frame_.data[2]; // Position high byte + data[3] = can_rx_frame_.data[3]; // Velocity low byte + data[4] = can_rx_frame_.data[4]; // Velocity high byte + data[6] = can_rx_frame_.data[6]; // Device Status + // POSITION // uint32 -> int16 -> double (for calcs) - raw_motor_position = static_cast(static_cast((data[3] << 8) | data[2])); + raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); // VELOCITY // uint16 -> int16 -> double (for calcs) - raw_motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); - - // TORQUE - // TODO + raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); // CALCULATING JOINT STATE - motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); - motor_velocity[i] = calculate_joint_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + motor_position[i] = calculate_joint_angle_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); } else{ if(DEBUG_MODE == 1) { @@ -174,20 +187,22 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up ...please wait..."); + + // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here + // Motor Shutdown Command int8_t command_nibble = 0x5; int8_t device_id_nibble; - // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here for(int i = 0; i < num_joints; i++){ can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; can_tx_frame_.dlc = 1; - // Motor Shutdown Command device_id_nibble = joint_node_ids[i] & 0x0F; can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; canBus.send(can_tx_frame_); } + // Close CAN bus canBus.close(); RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up successful!"); @@ -235,24 +250,24 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.1 deg to deg to radians +double SERVOHardwareInterface::calculate_joint_angle_from_motor_position(double motor_position, int gear_ratio){ + // Converts from deg to radians with gear ratio return (motor_position * 0.1 * (M_PI/180.0))/gear_ratio; } -double SERVOHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s +double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s with gear ratio return (motor_velocity * (M_PI/180.0))/gear_ratio; } -int32_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_angle(double joint_position, int gear_ratio){ + // radians -> deg with gear ratio + return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); } -int32_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); } @@ -281,11 +296,12 @@ hardware_interface::return_type SERVOHardwareInterface::read( joint_state_position_[i] = motor_position[i]; if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), + "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", + info_.joints[i].name.c_str(), + motor_position[i], + joint_state_position_[i], + joint_state_velocity_[i]); } } } @@ -295,76 +311,99 @@ hardware_interface::return_type SERVOHardwareInterface::read( hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + elapsed_update_time+=period.seconds(); int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - int32_t joint_angle = 0; - int16_t operating_velocity = 20; // dps - int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 8; - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + int16_t joint_angle = 0; + int16_t joint_velocity = 0; - // CALCULATE DESIRED JOINT ANGLE - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + double update_period = 1.0/update_rate; - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(update_period > elapsed_update_time){ + return hardware_interface::return_type::OK; + } + else{ + elapsed_update_time = 0.0; + + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = can_command_id; - } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + if(control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::STANDARD && std::isfinite(joint_command_position_[i])) { + can_tx_frame_.dlc = 8; + + // CALCULATE DESIRED JOINT ANGLE + joint_angle = std::clamp(joint_command_position_[i], 0, rated_max[i]); + if(joint_type_[i] == joint_type_t::REVOLUTE){ + joint_angle = calculate_motor_position_from_desired_joint_angle(joint_command_position_[i], joint_gear_ratios[i]); + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + joint_angle = calculate_motor_position_from_desired_joint_angle(joint_command_position_[i], joint_gear_ratios[i]); + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", + info_.joints[i].name.c_str(), + joint_angle, + joint_command_position_[i]); + } + + // ENCODING CAN MESSAGE + data[0] = 0x20 + joint_node_ids[i]; + data[1] = joint_angle & 0xFF; + data[2] = (joint_angle >> 8) & 0xFF; + + } + else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { + + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", + info_.joints[i].name.c_str(), + joint_velocity); + } + + // ENCODING CAN MESSAGE + data[0] = 0xA2; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + } + else if(servo_type_[i] == servo_type_t::UNDEFINED){ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The servo type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + else{ + // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; } - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + canBus.send(can_tx_frame_); } + } - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; - } - - canBus.send(can_tx_frame_); + if(DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Servo - Period passed to write: %f seconds", period.seconds()); } - + return hardware_interface::return_type::OK; } diff --git a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py index 3b9abba8..cf2e2e9e 100644 --- a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py +++ b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py @@ -48,7 +48,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + # self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 18f1ce7a..7b39c8a0 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -57,7 +57,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + # self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: From ec9f03472afdcdc0b34c5d0374064d0976534d42 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Tue, 20 Jan 2026 05:16:59 -0500 Subject: [PATCH 05/10] finalized initial servo hwi fixes, adjusted manual controller to implement servo functionality for the arm --- .../servo_hardware_interface.hpp | 14 ++- .../src/servo_hardware_interface.cpp | 52 +++++---- .../config/athena_arm_controllers.yaml | 7 +- .../athena_arm_controllers.xml | 20 ---- .../manual_arm_joint_by_joint_controller.yaml | 38 ++++--- .../manual_arm_joint_by_joint_controller.hpp | 25 +---- .../manual_arm_joint_by_joint_controller.cpp | 100 +++++++----------- 7 files changed, 109 insertions(+), 147 deletions(-) diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index e146c0a9..f45712c7 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -74,8 +74,8 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); - int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); private: @@ -120,9 +120,8 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh enum class servo_type_t : std::uint8_t { - UNDEFINED = 0, - STANDARD = 1, - CONTINUOUS = 2, + STANDARD = 0, + CONTINUOUS = 1, }; // Type of servo for each actuator @@ -130,9 +129,8 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh enum class joint_type_t : std::uint8_t { - UNDEFINED = 0, - REVOLUTE = 1, - PRISMATIC = 2, + REVOLUTE = 0, + PRISMATIC = 1, }; // Type of joint for each actuator diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index d7c5eb48..ac38403d 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -18,7 +18,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 1 // 0 for off 1 for on +#define DEBUG_MODE 0 // 0 for off 1 for on using std::placeholders::_1; @@ -42,19 +42,23 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( std::string servo_type = joint.parameters.at("servo_type"); std::string joint_type = joint.parameters.at("joint_type"); - - if (servo_type != "standard" && servo_type != "continuous") { + + if (servo_type == "standard") { + servo_type_.push_back(servo_type_t::STANDARD); + } else if (servo_type == "continuous") { + servo_type_.push_back(servo_type_t::CONTINUOUS); + } else { RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' or 'continuous'.", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; - } else { - servo_type_.push_back(joint.parameters.at("servo_type")); } - if (joint_type != "revolute" && joint_type != "prismatic") { + if (joint_type == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + } else if (joint_type == "prismatic") { + joint_type_.push_back(joint_type_t::PRISMATIC); + } else { RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'.", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; - } else { - joint_type_.push_back(joint.parameters.at("joint_type")); } } @@ -76,6 +80,15 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( motor_velocity.assign(num_joints, 0.0); control_level_.resize(num_joints, integration_level_t::POSITION); + for (int i = 0; i < num_joints; i++) { + if(servo_type_[i] == servo_type_t::STANDARD){ + control_level_[i] = integration_level_t::POSITION; + } else if(servo_type_[i] == servo_type_t::CONTINUOUS){ + control_level_[i] = integration_level_t::VELOCITY; + } else{ + control_level_[i] = integration_level_t::UNDEFINED; + } + } current_joint = 0; @@ -171,8 +184,8 @@ void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); // CALCULATING JOINT STATE - motor_position[i] = calculate_joint_angle_from_motor_position(raw_motor_position, joint_gear_ratios[i]); - motor_velocity[i] = calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = calculate_joint_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); } else{ if(DEBUG_MODE == 1) { @@ -250,22 +263,22 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double SERVOHardwareInterface::calculate_joint_angle_from_motor_position(double motor_position, int gear_ratio){ +double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ // Converts from deg to radians with gear ratio return (motor_position * 0.1 * (M_PI/180.0))/gear_ratio; } -double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ +double SERVOHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ // Converts from dps to radians/s with gear ratio return (motor_velocity * (M_PI/180.0))/gear_ratio; } -int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_angle(double joint_position, int gear_ratio){ +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ // radians -> deg with gear ratio return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); } -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ // radians/s -> deg/s with gear ratio return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); } @@ -337,12 +350,12 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ can_tx_frame_.dlc = 8; // CALCULATE DESIRED JOINT ANGLE - joint_angle = std::clamp(joint_command_position_[i], 0, rated_max[i]); + joint_angle = std::clamp(joint_command_position_[i], 0.0, rated_max[i]); if(joint_type_[i] == joint_type_t::REVOLUTE){ - joint_angle = calculate_motor_position_from_desired_joint_angle(joint_command_position_[i], joint_gear_ratios[i]); + joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); } else if(joint_type_[i] == joint_type_t::PRISMATIC){ - joint_angle = calculate_motor_position_from_desired_joint_angle(joint_command_position_[i], joint_gear_ratios[i]); + joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); } else{ RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); @@ -364,7 +377,7 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); if(DEBUG_MODE == 1) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", @@ -383,9 +396,6 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ data[7] = (joint_velocity >> 24) & 0xFF; } - else if(servo_type_[i] == servo_type_t::UNDEFINED){ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The servo type for joint %s is undefined.", info_.joints[i].name.c_str()); - } else{ // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); } diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 22968485..99c4cf7d 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -100,14 +100,16 @@ manual_arm_cylindrical_controller: manual_arm_joint_by_joint_controller: ros__parameters: - joints: + velocity_joints: - base_yaw - shoulder_pitch - elbow_pitch - wrist_pitch - wrist_roll - gripper_claw - interface_name: velocity + position_joints: + - actuator + # interface_name: velocity joint_max_velocities: - 62.83185307 # 10 rot/s, please change dawg - 0.05148721 # 2.95 dps @@ -115,3 +117,4 @@ manual_arm_joint_by_joint_controller: - 0.785398 # 45 dps - 0.785398 - 0.005 # TODO: Find this + - 0.03 # Actuator max displacement diff --git a/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml b/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml index 9b03cd76..c66dadc2 100644 --- a/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml +++ b/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml @@ -1,23 +1,3 @@ - - diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml index bd5d8f23..03f73d72 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml +++ b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml @@ -18,36 +18,46 @@ # manual_arm_joint_by_joint_controller: - joints: { + position_joints: { type: string_array, default_value: [], - description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + description: "Specifies joints which are controlled via position.", read_only: true, validation: { unique<>: null, not_empty<>: null, } } - state_joints: { + velocity_joints: { type: string_array, default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + description: "Specifies joints which are controlled via velocity.", read_only: true, validation: { unique<>: null, - } - } - interface_name: { - type: string, - default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", - read_only: true, - validation: { not_empty<>: null, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null } } + # state_joints: { + # type: string_array, + # default_value: [], + # description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + # read_only: true, + # validation: { + # unique<>: null, + # } + # } + # interface_name: { + # type: string, + # default_value: "", + # description: "Name of the interface used by the controller on joints and command_joints.", + # read_only: true, + # validation: { + # not_empty<>: null, + # one_of<>: [["position", "velocity", "acceleration", "effort",]], + # forbidden_interface_name_prefix: null + # } + # } joint_max_velocities: { type: double_array, default_value: [], diff --git a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp index 05ceed33..bbe8eb5b 100644 --- a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp +++ b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp @@ -1,22 +1,3 @@ -// Copyright (c) 2025, UMDLoop -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// #ifndef ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ #define ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ @@ -100,7 +81,9 @@ class ManualArmJointByJointController : public controller_interface::ControllerI std::shared_ptr param_listener_; manual_arm_joint_by_joint_controller::Params params_; - std::vector state_joints_; + int num_joints; + bool actuator_active_ = false; + double actuator_iterator = 0.0; // Represents velocities of each joint std::vector joint_velocities_; @@ -128,4 +111,4 @@ class ManualArmJointByJointController : public controller_interface::ControllerI } // namespace arm_controllers -#endif // ATHENA_ARM_CONTROLLERS__manual_arm_joint_by_joint_CONTROLLER_HPP_ +#endif // ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index 9705f3ab..b1fe5161 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -1,23 +1,3 @@ -// Copyright (c) 2025, UMDLoop -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - #include "athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp" #include @@ -81,25 +61,8 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu { params_ = param_listener_->get_params(); - if (!params_.state_joints.empty()) - { - state_joints_ = params_.state_joints; - } - else - { - state_joints_ = params_.joints; - } - - if (params_.joints.size() != state_joints_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", - params_.joints.size(), state_joints_.size()); - return CallbackReturn::FAILURE; - } - - joint_velocities_.resize(params_.joints.size(), 0.0); // Output + num_joints = static_cast(params_.position_joints.size()) + static_cast(params_.velocity_joints.size()); + joint_velocities_.resize(num_joints, 0.0); // Output max_velocities_ = params_.joint_max_velocities; // topics QoS @@ -154,7 +117,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu // TODO(anyone): Reserve memory in state publisher depending on the message type state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->msg_.header.frame_id = params_.velocity_joints[0]; state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -163,20 +126,6 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu void ManualArmJointByJointController::reference_callback(const std::shared_ptr msg) { - /* - if (msg->joint_names.size() == params_.joints.size()) - { - input_ref_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); - } - */ - input_ref_.writeFromNonRT(msg); } @@ -185,10 +134,15 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::co controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(params_.joints.size()); - for (const auto & joint : params_.joints) + command_interfaces_config.names.reserve(num_joints); + for (const auto & joint : params_.position_joints) + { + command_interfaces_config.names.push_back(joint + "/position"); + } + + for (const auto & joint : params_.velocity_joints) { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + command_interfaces_config.names.push_back(joint + "/velocity"); } return command_interfaces_config; @@ -199,10 +153,15 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_joints_.size()); - for (const auto & joint : state_joints_) + state_interfaces_config.names.reserve(num_joints); + for (const auto & joint : params_.position_joints) + { + state_interfaces_config.names.push_back(joint + "/position"); + } + + for (const auto & joint : params_.velocity_joints) { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + state_interfaces_config.names.push_back(joint + "/velocity"); } return state_interfaces_config; @@ -266,10 +225,29 @@ controller_interface::return_type ManualArmJointByJointController::update( joint_velocities_[5] = 0.0; } // RCLCPP_INFO(get_node()->get_logger(), "Talon: %f", joint_velocities_[5]); + + // Actuator (EXPERIMENTAL) + // Pressing square activates actuator movement + if((*current_ref)->buttons[3] == 1){ + actuator_active_ = true; + actuator_iterator = 0.001; + joint_velocities_[6] == 0.001; // to start off + } + + // Once actuator reaches original position, stop movement + if (actuator_active_ = true && joint_velocities_[6] == 0.0 && actuator_iterator < 0){ + actuator_active_ = false; + } + + // Move actuator up to max position, then begin moving it down + if (joint_velocities_[6] >= max_velocities_[6] && actuator_active_ == true && actuator_iterator > 0){ + actuator_iterator = -0.001; + } + joint_velocities_[6] = joint_velocities_[6] + actuator_iterator; } else{ // RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); - joint_velocities_.resize(6, 0.0); + joint_velocities_.resize(7, 0.0); } for (size_t i = 0; i < command_interfaces_.size(); ++i) From 46b769f689092b8448f9e63c10b0ac9628d41426 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Fri, 23 Jan 2026 03:42:29 -0500 Subject: [PATCH 06/10] completed first draft of servo hwi, ready to test --- .../arm/arm.servo.ros2_control.xacro | 9 +- .../arm/arm.talon.ros2_control.xacro | 5 +- .../science/science.servo.ros2_control.xacro | 10 +- src/description/urdf/athena_arm.urdf.xacro | 2 - .../servo_hardware_interface.hpp | 11 +- .../src/servo_hardware_interface.cpp | 222 ++++++++++++------ .../talon_hardware_interface.hpp | 18 -- .../src/talon_hardware_interface.cpp | 18 -- 8 files changed, 171 insertions(+), 124 deletions(-) diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index dc6d8310..0730608a 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -6,7 +6,7 @@ servo_ros2_control/SERVOHardwareInterface - 10 + 20 can0 0x80 @@ -14,9 +14,10 @@ 0x1 1 - 90 - standard - prismatic + 180 + 0.00016666666 + standard + prismatic diff --git a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro index 954ba7cf..5991910f 100644 --- a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro @@ -11,10 +11,7 @@ 2 - - 0 - 1 - + 0.0 diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index 4c6da7db..f7f512d6 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -28,7 +28,7 @@ 0x2 1 - 178 + 178 standard revolute @@ -39,7 +39,7 @@ 0x3 1 - 178 + 178 standard revolute @@ -50,7 +50,8 @@ 0x4 1 - 90 + 90 + 0.00016666666 standard prismatic @@ -62,7 +63,8 @@ 0x5 1 - 270 + 270 + 0.00016666666 standard prismatic diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/athena_arm.urdf.xacro index ddaa25b0..0a5a8f7d 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/athena_arm.urdf.xacro @@ -51,6 +51,4 @@ - - \ No newline at end of file diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index f45712c7..2f2a7fe1 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -72,15 +72,18 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Helper Functions double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); - double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); + double calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg); + double calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); + double calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg); int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); - int16_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg); + int16_t calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg); private: int num_joints; - int current_joint; int update_rate; double elapsed_update_time; std::string can_interface; @@ -100,6 +103,8 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector motor_velocity; std::vector motor_position; std::vector rated_max; + std::vector meters_per_deg; + std::vector device_status; CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index ac38403d..18ec2439 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -27,39 +27,55 @@ namespace servo_ros2_control hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS){ return hardware_interface::CallbackReturn::ERROR; } - // This stores the parameters for each joint + // -- Parameters -- for (auto& joint : info_.joints) { joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); - rated_max.push_back(std::abs(std::stod(joint.parameters.at("rated_max")))*(M_PI/180.0)); // Convert to radians std::string servo_type = joint.parameters.at("servo_type"); std::string joint_type = joint.parameters.at("joint_type"); - if (servo_type == "standard") { + // Standard or Continuous + if (servo_type == "standard" && joint.parameters.count("max_pos")) { servo_type_.push_back(servo_type_t::STANDARD); - } else if (servo_type == "continuous") { + } else if (servo_type == "continuous" && joint.parameters.count("max_vel")) { servo_type_.push_back(servo_type_t::CONTINUOUS); } else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' or 'continuous'.", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' with 'max_pos' or 'continuous' with 'max_vel'.", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } + // Revolute or Prismatic if (joint_type == "revolute") { joint_type_.push_back(joint_type_t::REVOLUTE); - } else if (joint_type == "prismatic") { + meters_per_deg.push_back(std::nan("")); // Not used for revolute joints + } else if (joint_type == "prismatic" && joint.parameters.count("meters_per_deg")) { joint_type_.push_back(joint_type_t::PRISMATIC); + meters_per_deg.push_back(std::abs(std::stod(joint.parameters.at("meters_per_deg")))); } else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'.", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'meters_per_deg' parameter.", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } + + // Determining rated max based on joint and servo type + if (joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::STANDARD) { + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")))*(M_PI/180.0)); // Convert from degrees to radians + } else if(joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::CONTINUOUS){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")))*(M_PI/180.0)); // Convert from dps to radians/s + } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::STANDARD){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from degrees to meters + } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::CONTINUOUS){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from dps to meters/s + } + else { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid combination of joint_type and servo_type for joint %s.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } num_joints = static_cast(info_.joints.size()); @@ -68,7 +84,7 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); can_response_id = can_command_id+0x01; - // Initializes command and state interface values + // -- Command and State Interface initialization -- joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_state_velocity_.assign(num_joints, 0); @@ -78,7 +94,9 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( encoder_position.assign(num_joints, 0.0); motor_position.assign(num_joints, 0.0); motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, -1); + // Set initial control type based on servo type control_level_.resize(num_joints, integration_level_t::POSITION); for (int i = 0; i < num_joints; i++) { if(servo_type_[i] == servo_type_t::STANDARD){ @@ -90,8 +108,6 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( } } - current_joint = 0; - for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); } @@ -158,7 +174,7 @@ void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { std::string result; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[7] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; int8_t command_nibble = ((can_rx_frame_.data[0]>>4) & 0x0F); int8_t device_id_nibble = (can_rx_frame_.data[0] & 0x0F); double raw_motor_position = 0.0; @@ -184,8 +200,20 @@ void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); // CALCULATING JOINT STATE - motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); - motor_velocity[i] = calculate_joint_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + if(joint_type_[i] == joint_type_t::REVOLUTE){ + motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); // radians + motor_velocity[i] = calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); // radians/s + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + motor_position[i] = calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], meters_per_deg[i]); // meters + motor_velocity[i] = calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], meters_per_deg[i]); // meters/s + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + + // Populate device status + device_status[i] = data[6]; } else{ if(DEBUG_MODE == 1) { @@ -265,57 +293,82 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ // Converts from deg to radians with gear ratio - return (motor_position * 0.1 * (M_PI/180.0))/gear_ratio; + return (motor_position * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg){ + // Converts from deg to meters with gear ratio + return (motor_position * meters_per_deg)/gear_ratio; } -double SERVOHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ +double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ // Converts from dps to radians/s with gear ratio return (motor_velocity * (M_PI/180.0))/gear_ratio; } +double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg){ + // Converts from dps to meters/s with gear ratio + return (motor_velocity * meters_per_deg)/gear_ratio; +} + int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ // radians -> deg with gear ratio return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); } -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg){ + // m -> deg with gear ratio + return static_cast(std::round((joint_position*meters_per_deg*gear_ratio))); +} + +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ // radians/s -> deg/s with gear ratio return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); } +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg){ + // m/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*meters_per_deg*gear_ratio))); +} + hardware_interface::return_type SERVOHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - int8_t command_nibble = 0x7; - int8_t device_id_nibble; - - current_joint+=1; - current_joint = current_joint % num_joints; for(int i = 0; i < num_joints; i++) { - if(current_joint == i){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 1; - - // Command to read motor status command - device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; - canBus.send(can_tx_frame_); + // CALCULATING JOINT STATE + joint_state_velocity_[i] = motor_velocity[i]; + joint_state_position_[i] = motor_position[i]; - // CALCULATING JOINT STATE - joint_state_velocity_[i] = motor_velocity[i]; - joint_state_position_[i] = motor_position[i]; + // ACCOUNTING FOR DEVICE STATUS + if(device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1){ + return hardware_interface::return_type::ERROR; + } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); + if(DEBUG_MODE == 1) { + if(device_status[i] == 0x00){ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is IDLE. Ready to receive commands.", info_.joints[i].name.c_str()); + } + else if(device_status[i] == 0x01){ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is ACTIVE. Currently not receiving commands.", info_.joints[i].name.c_str()); } + else if(device_status[i] == 0x02){ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s DOES NOT EXIST!", info_.joints[i].name.c_str()); + } + else if(device_status[i] == 0x03){ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is in ERROR state!", info_.joints[i].name.c_str()); + } + else{ // This includes -1 case. In order to avoid killing the HWI, it will operate as normal but print this warning. + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is in UNDEFINED state! Operation will continue.", info_.joints[i].name.c_str()); + } + + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), + "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", + info_.joints[i].name.c_str(), + motor_position[i], + joint_state_position_[i], + joint_state_velocity_[i]); } } @@ -327,7 +380,9 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { elapsed_update_time+=period.seconds(); - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[3] = {0x00, 0x00, 0x00}; + int8_t command_nibble; + int8_t device_id_nibble; int16_t joint_angle = 0; int16_t joint_velocity = 0; @@ -341,25 +396,40 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ } else{ elapsed_update_time = 0.0; - + + // Motor Command for each joint at given frequency for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + // Motor Status Command for each joint at given frequency + command_nibble = 0x7; + can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 1; + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + canBus.send(can_tx_frame_); + + // Motor Command for each joint at given frequency + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 3; if(control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::STANDARD && std::isfinite(joint_command_position_[i])) { - can_tx_frame_.dlc = 8; - // CALCULATE DESIRED JOINT ANGLE - joint_angle = std::clamp(joint_command_position_[i], 0.0, rated_max[i]); + joint_angle = std::clamp(joint_command_position_[i], 0.0, rated_max[i]); // Input must be within bounds of rated max (positive and either rad or m) if(joint_type_[i] == joint_type_t::REVOLUTE){ - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + joint_angle = calculate_motor_position_from_desired_joint_position(joint_angle, joint_gear_ratios[i]); } else if(joint_type_[i] == joint_type_t::PRISMATIC){ - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + joint_angle = calculate_motor_position_from_desired_joint_displacement(joint_angle, joint_gear_ratios[i], meters_per_deg[i]); } else{ RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); } + + // ENCODING CAN MESSAGE + data[0] = 0x20 + joint_node_ids[i]; + data[1] = joint_angle & 0xFF; + data[2] = (joint_angle >> 8) & 0xFF; if(DEBUG_MODE == 1) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", @@ -368,40 +438,50 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ joint_command_position_[i]); } - // ENCODING CAN MESSAGE - data[0] = 0x20 + joint_node_ids[i]; - data[1] = joint_angle & 0xFF; - data[2] = (joint_angle >> 8) & 0xFF; - } else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + joint_velocity = std::clamp(joint_command_velocity_[i], -rated_max[i], rated_max[i]); // Input must be within bounds of rated max (all real values and either rad or m) + if(joint_type_[i] == joint_type_t::REVOLUTE){ + joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity(joint_velocity, joint_gear_ratios[i]); + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + joint_velocity = calculate_motor_velocity_from_desired_joint_linear_velocity(joint_velocity, joint_gear_ratios[i], meters_per_deg[i]); + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + + // ENCODING CAN MESSAGE + data[0] = 0x30 + joint_node_ids[i]; + data[1] = joint_velocity & 0xFF; + data[2] = (joint_velocity >> 8) & 0xFF; if(DEBUG_MODE == 1) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", info_.joints[i].name.c_str(), joint_velocity); } - - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - + } + else if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { + if (DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); + } + } + else if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { + if (DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); + } } else{ - // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + if (DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + } } // Cast data to uint8_t - for(int j = 0; j < 8; j++){ + for(int j = 0; j < 3; j++){ data[j] = static_cast(data[j]); can_tx_frame_.data[j] = data[j]; } diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp index a11a9e44..253f2ff2 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #ifndef TALON_HARDWARE_INTERACE_HPP_ #define TALON_HARDWARE_INTERACE_HPP_ diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 694767a7..05498603 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #include "talon_ros2_control/talon_hardware_interface.hpp" #include From 179b273d42b82a00418b20d3d8bc562a7e437f3a Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Fri, 23 Jan 2026 04:52:28 -0500 Subject: [PATCH 07/10] talon hwi fixes that account for joint type (revolute or prismatic), needs testing --- .../arm/arm.talon.ros2_control.xacro | 8 +- .../science/science.talon.ros2_control.xacro | 22 ++--- .../talon_ros2_control/talon_control.cpp | 28 +++++-- .../talon_ros2_control/talon_control.h | 6 +- .../talon_hardware_interface.hpp | 16 +++- .../src/talon_hardware_interface.cpp | 84 +++++++++++-------- 6 files changed, 101 insertions(+), 63 deletions(-) diff --git a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro index 5991910f..6b5a5028 100644 --- a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro @@ -7,15 +7,17 @@ talon_ros2_control/TALONHardwareInterface + 10 + can0 2 + 0.06 + prismatic - - 0.0 - + diff --git a/src/description/ros2_control/science/science.talon.ros2_control.xacro b/src/description/ros2_control/science/science.talon.ros2_control.xacro index 12bcc36c..f3da4734 100644 --- a/src/description/ros2_control/science/science.talon.ros2_control.xacro +++ b/src/description/ros2_control/science/science.talon.ros2_control.xacro @@ -7,31 +7,25 @@ talon_ros2_control/TALONHardwareInterface + 10 + can0 1 - - 0 - 1 - + revolute + - - 0.0 - + 2 - - 0 - 1 - + revolute + - - 0.0 - + diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp index 13031734..9fea40aa 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp @@ -106,7 +106,7 @@ void setDutyCycle(TalonSRX *motor, double dutyCycle, int ms) { ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds } -float setVelocity(TalonSRX *motor, double claw_vel, int ms) { +float setVelocityFromLinearVelocity(TalonSRX *motor, double claw_vel, int ms) { // float desired_rpm = joy_input * MAX_RPM; // float desired_rev_per_100ms = desired_rpm / 600; // 600 100ms to 1 minute @@ -120,12 +120,30 @@ float setVelocity(TalonSRX *motor, double claw_vel, int ms) { return desired_talon_units_per_100ms; } -float setPosition(TalonSRX *motor, double claw_dist, int ms) { +float setVelocityFromAngularVelocity(TalonSRX *motor, double joint_velocity, int ms) { + float desired_talon_units_per_100ms = convertRevtoTalonUnits(joint_velocity / (2*M_PI*10)); // rad/s -> rad/100ms -> rev/100ms -> talon/100ms + // printf("Spin command received...\n\r"); + + motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms); + ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds + + return desired_talon_units_per_100ms; +} + +float setPositionFromDisplacement(TalonSRX *motor, double claw_displacement, int ms) { // float length_range = LENGTH_RANGE * 2.54; // in cm // float desired_pos = (joy_input * length_range) / DISTANCE_PER_REV; // in rev // float desired_talon_units = convertRevtoTalonUnits(desired_pos); - float desired_talon_units = convertRevtoTalonUnits(convertDistanceToRev(claw_dist)); // m -> rev -> talon units + float desired_talon_units = convertRevtoTalonUnits(convertDistanceToRev(claw_displacement)); // m -> rev -> talon units + motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units); + ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds + + return desired_talon_units; +} + +float setPositionFromJointCommand(TalonSRX *motor, double joint_position, int ms) { + float desired_talon_units = convertRevtoTalonUnits(joint_position / (2*M_PI)); // rad -> rev -> talon units motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds @@ -139,14 +157,14 @@ void stopMotor(TalonSRX *motor, int ms){ } // float runMotor(TalonSRX *motor, double speed, int ms) { -// float velocity = setVelocity(motor, speed); // motor receives talon units/100ms, function returns rev/s +// float velocity = setVelocityFromLinearVelocity(motor, speed); // motor receives talon units/100ms, function returns rev/s // // setDutyCycle(motor, speed); // set percent output and direction // ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds // return velocity; // } // float setMotorPosition(TalonSRX *motor, double joy_input, int ms) { -// float position = setPosition(motor, joy_input); // in Talon Units +// float position = setPositionFromDisplacement(motor, joy_input); // in Talon Units // ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds // return position; // } diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h index e6a48403..6bc28e8f 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h @@ -25,8 +25,10 @@ float getClawVelocity(TalonSRX *); // Retrieves Velocity of claw in m/s // Setting States void setDutyCycle(TalonSRX *, double, int); // Sets duty cycle, aka percent of max speed basically -float setVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (mm/s), returns this as well -float setPosition(TalonSRX *, double, int); // Sets position (Talon units) based on input position (cm), returns this as well +float setVelocityFromLinearVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (mm/s), returns this as well +float setVelocityFromAngularVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (radians/s), returns this as well +float setPositionFromDisplacement(TalonSRX *, double, int); // Sets position (Talon units) based on input displacement (cm), returns this as well +float setPositionFromJointCommand(TalonSRX *, double, int); // Sets position (Talon units) based on input joint command (radians), returns this as well void stopMotor(TalonSRX *, int); // Stops the motor #endif \ No newline at end of file diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp index 253f2ff2..a14fac4d 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp @@ -68,10 +68,11 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh private: int num_joints; - - // EXPERIMENTING - std::vector initial_position_; + int update_rate; + std::string can_interface; + // Maximum displacement for prismatic joints + std::vector max_disp; // Store the state for the simulated robot std::vector joint_state_position_; @@ -93,6 +94,15 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh // Active control mode std::vector control_level_; + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + // Type of joint for each actuator + std::vector joint_type_; + std::vector talon_motors; std::thread worker; std::atomic is_running = false; diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 05498603..6dd44aee 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -18,7 +18,6 @@ #include "rclcpp/rclcpp.hpp" #include "talon_ros2_control/talon_control.h" -// #include "talon_control.h" #define DEBUG_MODE 0 // 0 for off 1 for on @@ -30,21 +29,31 @@ TALONHardwareInterface::TALONHardwareInterface() { hardware_interface::CallbackReturn TALONHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! AS OF RIGHT NOW, NOT SURE - IF NEEDED. USE info - */ + // -- Parameters -- + can_interface = info_.hardware_parameters.at("can_interface"); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); for (auto& joint : info_.joints) { joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - initial_position_.push_back(std::stof(joint.state_interfaces[0].initial_value)); + + std::string joint_type = joint.parameters.at("joint_type"); + + // Revolute or Prismatic + if (joint_type == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + max_disp.push_back(std::nan("")); // Not used for revolute joints + } else if (joint_type == "prismatic" && joint.parameters.count("max_disp")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'max_disp' parameter.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } } num_joints = static_cast(info_.joints.size()); @@ -90,7 +99,6 @@ std::vector TALONHardwareInterface::export for(int i = 0; i < num_joints; i++){ command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); } @@ -106,7 +114,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_configure( RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Beginning configure."); for(int i = 0; i < num_joints; i++){ - TalonSRX *motor = new TalonSRX(joint_node_ids[i], "can0"); + TalonSRX *motor = new TalonSRX(joint_node_ids[i], can_interface); talon_motors.push_back(motor); RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "talon_motor initialized."); initMotor(motor); @@ -145,7 +153,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_activate( is_running.store(true); worker = std::thread(&TALONHardwareInterface::enable_system_thread, this); - // setPosition(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself + // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully activated!"); @@ -161,7 +169,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - setVelocity(talon_motors[i], 0.0, 50); + setVelocityFromLinearVelocity(talon_motors[i], 0.0, 50); } is_running.store(false); @@ -169,7 +177,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_deactivate( worker.join(); } - // setPosition(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself + // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully deactivated!"); @@ -195,36 +203,40 @@ hardware_interface::return_type TALONHardwareInterface::read( hardware_interface::return_type talon_ros2_control::TALONHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - float max_dist = 0.06; //in m for(int i = 0; i < num_joints; i++){ - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_position_[i])) { // TO DO: implement joint limits so i dont gotta do this - if(joint_command_position_[i] > max_dist) { - joint_command_position_[i] = max_dist; - } - else if(joint_command_position_[i] < 0.0) { - joint_command_position_[i] = 0.0; - } - - // COMMAND POSITION - setPosition(talon_motors[i], joint_command_position_[i], 50); + joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, max_disp[i]); + // COMMAND PRISMATIC POSITION + setPositionFromDisplacement(talon_motors[i], joint_command_position_[i], 50); - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing positions for Talon - Joint command: %f", - joint_command_position_[i]); - } - } else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_velocity_[i])) { + + // COMMAND PRISMATIC VELOCITY + setVelocityFromLinearVelocity(talon_motors[i], joint_command_velocity_[i], 50); - // COMMAND VELOCITY - setVelocity(talon_motors[i], joint_command_velocity_[i], 50); + } else if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_position_[i])) { - if(DEBUG_MODE == 1) { + // COMMAND REVOLUTE POSITION + setPositionFromJointCommand(talon_motors[i], joint_command_position_[i], 50); + + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_velocity_[i])) { + + // COMMAND REVOLUTE VELOCITY + setVelocityFromAngularVelocity(talon_motors[i], joint_command_velocity_[i], 50); + + } + else { + // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); + } + + if(control_level_[i] == integration_level_t::POSITION && DEBUG_MODE == 1) { + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing positions for Talon - Joint command: %f", + joint_command_position_[i]); + } else if(control_level_[i] == integration_level_t::VELOCITY && DEBUG_MODE == 1) { RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing velocities for Talon - Joint command: %f", joint_command_velocity_[i]); - } - } else { - // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); } } From 996503fd37ce08e463e45d8dfb656f17122bdc63 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Tue, 27 Jan 2026 22:05:32 -0500 Subject: [PATCH 08/10] add a logger function in all hwis --- .../arm/arm.rmd.ros2_control.xacro | 8 +- .../arm/arm.servo.ros2_control.xacro | 9 +- .../arm/arm.smc.ros2_control.xacro | 8 +- .../arm/arm.talon.ros2_control.xacro | 8 +- .../drive/drive.rmd.ros2_control.xacro | 9 +- .../science/science.servo.ros2_control.xacro | 6 +- .../science/science.talon.ros2_control.xacro | 8 +- .../rmd_hardware_interface.hpp | 35 +-- .../src/rmd_hardware_interface.cpp | 184 ++++++++------ .../servo_hardware_interface.hpp | 32 ++- .../src/servo_hardware_interface.cpp | 234 ++++++++++-------- .../smc_hardware_interface.hpp | 55 ++-- .../src/smc_hardware_interface.cpp | 232 ++++++++--------- .../talon_ros2_control/talon_control.cpp | 3 - .../talon_hardware_interface.hpp | 27 +- .../src/talon_hardware_interface.cpp | 119 ++++++--- 16 files changed, 521 insertions(+), 456 deletions(-) diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index b5b4c959..9754c9d1 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -1,12 +1,12 @@ - - rmd_ros2_control/RMDHardwareInterface - 50 + 10 + 5 + 0 can0 @@ -28,7 +28,5 @@ - - \ No newline at end of file diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index 0730608a..7f3638ec 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -1,12 +1,12 @@ - - servo_ros2_control/SERVOHardwareInterface - 20 + 10 + 5 + 0 can0 0x80 @@ -22,9 +22,6 @@ - - - diff --git a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro index 1cd3a1fc..1dc8b9b4 100644 --- a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro @@ -1,13 +1,12 @@ - - - smc_ros2_control/SMCHardwareInterface 10 + 5 + 0 can0 @@ -40,9 +39,6 @@ - - - diff --git a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro index 6b5a5028..620a27c8 100644 --- a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro @@ -1,13 +1,12 @@ - - - talon_ros2_control/TALONHardwareInterface 10 + 5 + 0 can0 @@ -20,9 +19,6 @@ - - - diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index fdb6f5f6..112d0805 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -1,12 +1,12 @@ - - rmd_ros2_control/RMDHardwareInterface - 50 + 10 + 5 + 0 can0 @@ -53,9 +53,6 @@ - - - \ No newline at end of file diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index f7f512d6..c8529a65 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -1,12 +1,12 @@ - - servo_ros2_control/SERVOHardwareInterface 10 + 5 + 0 can0 0x80 @@ -72,7 +72,5 @@ - - diff --git a/src/description/ros2_control/science/science.talon.ros2_control.xacro b/src/description/ros2_control/science/science.talon.ros2_control.xacro index f3da4734..00cbc8fe 100644 --- a/src/description/ros2_control/science/science.talon.ros2_control.xacro +++ b/src/description/ros2_control/science/science.talon.ros2_control.xacro @@ -1,13 +1,12 @@ - - - talon_ros2_control/TALONHardwareInterface 10 + 5 + 0 can0 @@ -28,9 +27,6 @@ - - - \ No newline at end of file diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index 20ac8e15..e4ffc1d9 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -26,22 +26,21 @@ namespace rmd_ros2_control { -class RMDHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface +class RMDHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RMDHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct + // Initialization: Reading parameters, initializing variables, checking if all the joint state and command interfaces are correct hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle + // -- Lifecycle Functions -- hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -68,21 +67,26 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void onCanMessage(const CANLib::CanFrame& frame); - // Helper Functions + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); private: - - int num_joints; + // Hardware Interface Parameters int update_rate; - double elapsed_update_time; - std::string can_interface; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + + // Keeps track of amount of joints + int num_joints; // Store the state for the simulated robot std::vector joint_state_position_; @@ -98,16 +102,20 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher // Velocity at which **joint** rotates to reach position in 1 dps uint16_t operating_velocity; - + + // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; - + std::string can_interface; + + // Joint specific parameters std::vector joint_node_write_ids; std::vector joint_node_read_ids; std::vector joint_gear_ratios; std::vector joint_orientation; + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -117,7 +125,6 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher // Active control mode for each actuator std::vector control_level_; - }; } // namespace rmd_hardware_interface diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 919e5912..fd3be944 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -19,19 +19,77 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on using std::placeholders::_1; namespace rmd_ros2_control { + +double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from 0.01 deg to deg to radians/s + return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; +} + +double RMDHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +int32_t RMDHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg -> 0.01 deg + return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +} + +int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s -> 0.01 deg/s + return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +} + +void RMDHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nRMD Logger"; + + // HWI Specific + std::ostringstream oss; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Write CAN ID: 0x" << std::hex << std::uppercase << joint_node_write_ids[i] + << " | Read CAN ID: 0x" << std::hex << std::uppercase << joint_node_read_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] + << " | Orientation: " << joint_orientation[i] << "\n" + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn RMDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } @@ -54,9 +112,13 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( num_joints = static_cast(info_.joints.size()); update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); can_interface = info_.hardware_parameters.at("can_interface"); elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -70,10 +132,6 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( control_level_.resize(num_joints, integration_level_t::POSITION); - for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); - } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -123,7 +181,7 @@ RMDHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open(can_interface, std::bind(&RMDHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&RMDHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -131,7 +189,7 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { +void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; // Store the received frame for processing in read() std::string result; @@ -142,7 +200,10 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { for(int i = 0; i < num_joints; i++){ if(can_rx_frame_.id == joint_node_read_ids[i] && - (can_rx_frame_.data[0] == 0xA2 || can_rx_frame_.data[0] == 0xA4)){ + (can_rx_frame_.data[0] == 0xA2 || + can_rx_frame_.data[0] == 0xA4 || + can_rx_frame_.data[0] == 0x9C) && + can_rx_frame_.data[1] != 0x00) { // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) data[1] = can_rx_frame_.data[1]; // Motor Temperature @@ -161,13 +222,12 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); } - else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); + else { + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); } } } - } hardware_interface::CallbackReturn RMDHardwareInterface::on_cleanup( @@ -210,10 +270,6 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_activate( // Sets initial command to joint state joint_command_position_ = joint_state_position_; - - for (size_t i = 0; i < joint_command_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); - } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully activated!"); @@ -245,66 +301,46 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; -} - -double RMDHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} - -int32_t RMDHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); -} - -int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); -} - - hardware_interface::return_type RMDHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + // CALCULATING JOINT STATE for(int i = 0; i < num_joints; i++) { - // CALCULATING JOINT STATE joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); - } } - + return hardware_interface::return_type::OK; } hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) + const rclcpp::Time & time, const rclcpp::Duration & period) { elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; int32_t joint_angle = 0; int32_t joint_velocity = 0; - double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, // keep track of time passed over iterations of this function and if it exceeds the // desired frequency of the HWI, skip message - if(update_period > elapsed_update_time){ - return hardware_interface::return_type::OK; - } - else{ + if(elapsed_update_time > update_period){ elapsed_update_time = 0.0; for(int i = 0; i < num_joints; i++) { can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated @@ -315,13 +351,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( // CALCULATE DESIRED JOINT ANGLE joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } // ENCODING CAN MESSAGE data[0] = 0xA4; @@ -331,20 +360,13 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( data[4] = joint_angle & 0xFF; data[5] = (joint_angle >> 8) & 0xFF; data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - + data[7] = (joint_angle >> 24) & 0xFF; } else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { // CALCULATE DESIRED JOINT VELOCITY joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); - } - + // ENCODING CAN MESSAGE data[0] = 0xA2; data[1] = 0x00; @@ -354,10 +376,18 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( data[5] = (joint_velocity >> 8) & 0xFF; data[6] = (joint_velocity >> 16) & 0xFF; data[7] = (joint_velocity >> 24) & 0xFF; - } else{ - // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state"); + // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); + // ENCODING CAN MESSAGE + data[0] = 0x9C; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = 0x00; + data[5] = 0x00; + data[6] = 0x00; + data[7] = 0x00; } // Cast data to uint8_t @@ -369,10 +399,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( } } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "RMD - Period passed to write: %f seconds", period.seconds()); - } - return hardware_interface::return_type::OK; } diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index 2f2a7fe1..cf4ee76b 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -38,7 +38,6 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; // Lifecycle @@ -68,9 +67,11 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void onCanMessage(const CANLib::CanFrame& frame); - // Helper Functions + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); + double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg); double calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); @@ -82,14 +83,20 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh int16_t calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg); private: - - int num_joints; + // Hardware Interface Parameters int update_rate; - double elapsed_update_time; - std::string can_interface; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + // Keeps track of amount of joints + int num_joints; + + // Stores Arbitration IDs int can_command_id; - int can_response_id; + uint32_t can_response_id; // Store the state for the simulated robot std::vector joint_state_position_; @@ -98,21 +105,25 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Store the command for the simulated robot std::vector joint_command_position_; std::vector joint_command_velocity_; - - std::vector encoder_position; + + // Place holders for data from the canBus, will be accessed in read() std::vector motor_velocity; std::vector motor_position; std::vector rated_max; std::vector meters_per_deg; std::vector device_status; + // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; + std::string can_interface; + // Joint specific parameters std::vector joint_node_ids; std::vector joint_gear_ratios; + // Modes for control mode enum class integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -132,6 +143,7 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Type of servo for each actuator std::vector servo_type_; + // Types of joints enum class joint_type_t : std::uint8_t { REVOLUTE = 0, diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index 18ec2439..3345aa55 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -18,12 +18,111 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on - using std::placeholders::_1; namespace servo_ros2_control { + +double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from deg to radians with gear ratio + return (motor_position * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg){ + // Converts from deg to meters with gear ratio + return (motor_position * meters_per_deg)/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s with gear ratio + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg){ + // Converts from dps to meters/s with gear ratio + return (motor_velocity * meters_per_deg)/gear_ratio; +} + +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg with gear ratio + return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); +} + +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg){ + // m -> deg with gear ratio + return static_cast(std::round((joint_position*meters_per_deg*gear_ratio))); +} + +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); +} + +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg){ + // m/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*meters_per_deg*gear_ratio))); +} + +void SERVOHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSERVO Logger"; + std::ostringstream oss; + std::string status; + + // HWI Specific + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | Command CAN ID: 0x" << std::hex << std::uppercase << can_command_id + << " | Response CAN ID: 0x" << std::hex << std::uppercase << can_response_id + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + switch (device_status[i]) { + case 0x00: + status = "IDLE (ready)"; + break; + case 0x01: + status = "ACTIVE (busy)"; + break; + case 0x02: + status = "DOES NOT EXIST"; + break; + case 0x03: + status = "ERROR"; + break; + default: + status = "UNDEFINED"; + break; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] + << " | Device Status: " << std::hex << std::uppercase << device_status[i] + << "- " << status << "\n" + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { @@ -80,9 +179,15 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( num_joints = static_cast(info_.joints.size()); update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); can_interface = info_.hardware_parameters.at("can_interface"); can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); can_response_id = can_command_id+0x01; + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; // -- Command and State Interface initialization -- joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -91,7 +196,6 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); - encoder_position.assign(num_joints, 0.0); motor_position.assign(num_joints, 0.0); motor_velocity.assign(num_joints, 0.0); device_status.assign(num_joints, -1); @@ -108,10 +212,6 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( } } - for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); - } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -161,7 +261,7 @@ SERVOHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn SERVOHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open(can_interface, std::bind(&SERVOHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&SERVOHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -169,7 +269,7 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { +void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; // Store the received frame for processing in read() std::string result; @@ -216,8 +316,8 @@ void SERVOHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { device_status[i] = data[6]; } else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); } } } @@ -291,47 +391,6 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from deg to radians with gear ratio - return (motor_position * (M_PI/180.0))/gear_ratio; -} - -double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg){ - // Converts from deg to meters with gear ratio - return (motor_position * meters_per_deg)/gear_ratio; -} - -double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s with gear ratio - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} - -double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg){ - // Converts from dps to meters/s with gear ratio - return (motor_velocity * meters_per_deg)/gear_ratio; -} - -int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg with gear ratio - return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); -} - -int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg){ - // m -> deg with gear ratio - return static_cast(std::round((joint_position*meters_per_deg*gear_ratio))); -} - -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s with gear ratio - return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); -} - -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg){ - // m/s -> deg/s with gear ratio - return static_cast(std::round((joint_velocity*meters_per_deg*gear_ratio))); -} - - hardware_interface::return_type SERVOHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) @@ -345,31 +404,6 @@ hardware_interface::return_type SERVOHardwareInterface::read( if(device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1){ return hardware_interface::return_type::ERROR; } - - if(DEBUG_MODE == 1) { - if(device_status[i] == 0x00){ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is IDLE. Ready to receive commands.", info_.joints[i].name.c_str()); - } - else if(device_status[i] == 0x01){ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is ACTIVE. Currently not receiving commands.", info_.joints[i].name.c_str()); - } - else if(device_status[i] == 0x02){ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s DOES NOT EXIST!", info_.joints[i].name.c_str()); - } - else if(device_status[i] == 0x03){ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is in ERROR state!", info_.joints[i].name.c_str()); - } - else{ // This includes -1 case. In order to avoid killing the HWI, it will operate as normal but print this warning. - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is in UNDEFINED state! Operation will continue.", info_.joints[i].name.c_str()); - } - - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); - } } return hardware_interface::return_type::OK; @@ -380,21 +414,29 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; int data[3] = {0x00, 0x00, 0x00}; int8_t command_nibble; int8_t device_id_nibble; int16_t joint_angle = 0; int16_t joint_velocity = 0; - double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } // HWI can only go as fast as the controller manager. To limit frequency of bus messages, // keep track of time passed over iterations of this function and if it exceeds the // desired frequency of the HWI, skip message - if(update_period > elapsed_update_time){ - return hardware_interface::return_type::OK; - } - else{ + if(elapsed_update_time > update_period){ elapsed_update_time = 0.0; // Motor Command for each joint at given frequency @@ -430,13 +472,6 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ data[0] = 0x20 + joint_node_ids[i]; data[1] = joint_angle & 0xFF; data[2] = (joint_angle >> 8) & 0xFF; - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } } else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { @@ -458,25 +493,20 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ data[1] = joint_velocity & 0xFF; data[2] = (joint_velocity >> 8) & 0xFF; - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); - } } else if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { - if (DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); } } else if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { - if (DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); } } else{ - if (DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); } } @@ -490,10 +520,6 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ } } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Servo - Period passed to write: %f seconds", period.seconds()); - } - return hardware_interface::return_type::OK; } diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 314ec901..73b301d4 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #ifndef SMC_HARDWARE_INTERACE_HPP_ #define SMC_HARDWARE_INTERACE_HPP_ @@ -38,6 +20,7 @@ #include #include #include + #include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" @@ -55,10 +38,9 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle + // -- Lifecycle Functions -- hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -85,21 +67,26 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void onCanMessage(const CANLib::CanFrame& frame); - - // Helper Functions + + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + // Keeps track of amount of joints int num_joints; - int update_rate; - double elapsed_update_time; - std::string can_interface; // Store the state for the simulated robot std::vector joint_state_position_; @@ -108,7 +95,8 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Store the command for the simulated robot std::vector joint_command_position_; std::vector joint_command_velocity_; - + + // Place holders for data from the canBus, will be accessed in read() std::vector encoder_position; std::vector motor_velocity; std::vector motor_position; @@ -116,16 +104,18 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Velocity at which **joint** rotates to reach position in 1 dps uint16_t operating_velocity; - std::vector joint_initialization_; - + // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; + std::string can_interface; + // Joint specific parameters std::vector joint_node_ids; std::vector joint_gear_ratios; - - + std::vector joint_initialization_; + + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -135,7 +125,6 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Active control mode for each actuator std::vector control_level_; - }; } // namespace smc_hardware_interface diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 304c8c71..19e6440c 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, 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. - -// -// Authors: Denis Stogl -// - #include "smc_ros2_control/smc_hardware_interface.hpp" #include @@ -37,17 +19,72 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on - namespace smc_ros2_control { + +double SMCHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from 0.01 deg to deg to radians/s + return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; +} + +double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg -> 0.01 deg + return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +} + +int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s -> 0.01 deg/s + return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +} + +void SMCHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSMC Logger"; + + // HWI Specific + std::ostringstream oss; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] << "\n" + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn SMCHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } @@ -59,23 +96,15 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_init( operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); } - // Build a single log message - std::stringstream ss; - for (size_t i = 0; i < info_.joints.size(); ++i) { - ss << "Joint " << info_.joints[i].name << " configured:\n" - << " node_id: 0x" << std::hex << joint_node_ids[i] << std::dec << "\n" - << " gear_ratio: " << joint_gear_ratios[i] << "\n"; - } - - // Log all joints at once - RCLCPP_INFO(rclcpp::get_logger("SMCHWI"), "%s", ss.str().c_str()); - - num_joints = static_cast(info_.joints.size()); update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); can_interface = info_.hardware_parameters.at("can_interface"); elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -139,7 +168,7 @@ SMCHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open(can_interface, std::bind(&SMCHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&SMCHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -147,7 +176,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { +void SMCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; // Store the received frame for processing in read() std::string result; @@ -156,11 +185,12 @@ void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { for(int i = 0; i < num_joints; i++){ if(can_rx_frame_.id == joint_node_ids[i] && - (can_rx_frame_.data[0] == 0xA2 || can_rx_frame_.data[0] == 0xA4) && - can_rx_frame_.data[1] != 0x00){ + (can_rx_frame_.data[0] == 0xA2 || + can_rx_frame_.data[0] == 0xA4 || + can_rx_frame_.data[0] == 0x9C) && + can_rx_frame_.data[1] != 0x00){ // DECODING CAN MESSAGE FOR VELOCITY - // data[0] = 0x9C; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -176,10 +206,6 @@ void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { // SPEED // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); - - //CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - } else if(can_rx_frame_.id == joint_node_ids[i] && can_rx_frame_.data[0] == 0x92){ data[0] = 0x92; @@ -192,7 +218,6 @@ void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { data[7] = can_rx_frame_.data[7]; // Multi-turn high byte // POSITION - // TODO: This sign extension may only work when the value is negative, look into it // In this case, we have 56 bits to store in 64, so we must have a sign extension // uint64 -> int64 (to do sign extension) -> sign extension (left shift 8, then arithmetic right shift 8) --> double (for calcs) uint64_t motor_position_raw = (static_cast(data[7]) << 48) | @@ -203,32 +228,13 @@ void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { (static_cast(data[2]) << 8) | static_cast(data[1]); - motor_position[i] = static_cast((static_cast(motor_position_raw) << 8) >> 8); - - if(DEBUG_MODE == 1) { - for(int j = 0; j < 8; j++){ - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Received at index %d: %d", j, can_rx_frame_.data[j]); - } - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Motor Position: %f", motor_position[i]); - } - - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - + motor_position[i] = static_cast((static_cast(motor_position_raw) << 8) >> 8); } else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reply not heard."); + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reply not heard."); } } - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reading for joint: %s Joint position: %f Joint velocity: %f \nJoint Initialization State: %d \n", - info_.joints[i].name.c_str(), - joint_state_position_[i], - joint_state_velocity_[i], - static_cast(joint_initialization_[i])); - } - } } @@ -270,11 +276,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_activate( } // Sets initial command to joint state - // joint_command_position_ = joint_state_position_; - for (size_t i = 0; i < joint_state_velocity_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_state_velocity_[i]); - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint %zu joint command in on_activate: %f", i, joint_command_position_[i]); - } + joint_command_position_ = joint_state_position_; RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully activated!"); @@ -302,43 +304,16 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double SMCHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; -} - -double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} - -int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); -} - -int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); -} hardware_interface::return_type SMCHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // Values retrieved + //CALCULATING JOINT STATE for(int i = 0; i < num_joints; i++){ - - //CALCULATING JOINT STATE joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reading for joint: %s Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - joint_state_position_[i], - joint_state_velocity_[i]); - } } + return hardware_interface::return_type::OK; } @@ -347,20 +322,29 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; int32_t joint_angle = 0; int16_t operating_velocity = 200; int32_t joint_velocity = 0; - double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, // keep track of time passed over iterations of this function and if it exceeds the // desired frequency of the HWI, skip message - if(update_period > elapsed_update_time){ - return hardware_interface::return_type::OK; - } - else{ + if(elapsed_update_time > update_period){ elapsed_update_time = 0.0; for(int i = 0; i < num_joints; i++) { can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated @@ -372,12 +356,6 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( // CALCULATE DESIRED JOINT ANGLE joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d", - info_.joints[i].name.c_str(), - joint_angle); - } - // ENCODING CAN MESSAGE data[0] = 0xA4; data[1] = 0x00; @@ -392,13 +370,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( // CALCULATE DESIRED JOINT VELOCITY joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); - } - + // ENCODING CAN MESSAGE data[0] = 0xA2; data[1] = 0x00; @@ -408,10 +380,18 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( data[5] = (joint_velocity >> 8) & 0xFF; data[6] = (joint_velocity >> 16) & 0xFF; data[7] = (joint_velocity >> 24) & 0xFF; - } else{ - // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state"); + // RCLCPP_WARN(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); + // ENCODING CAN MESSAGE + data[0] = 0x9C; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = 0x00; + data[5] = 0x00; + data[6] = 0x00; + data[7] = 0x00; } // Cast data to uint8_t @@ -419,14 +399,6 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( data[j] = static_cast(data[j]); can_tx_frame_.data[j] = data[j]; } - - // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Sent"); - - // Command for position/velocity control - canBus.send(can_tx_frame_); - - // Command to read multi-turn angle - can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; canBus.send(can_tx_frame_); } } diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp index 9fea40aa..188bfd52 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp @@ -101,7 +101,6 @@ float getClawVelocity(TalonSRX *motor) { } void setDutyCycle(TalonSRX *motor, double dutyCycle, int ms) { - // printf("Spin command received...\n\r"); motor->Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, dutyCycle); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds } @@ -112,7 +111,6 @@ float setVelocityFromLinearVelocity(TalonSRX *motor, double claw_vel, int ms) { //TO DO: why does the 5 fix this. I dont have a clue rn float desired_talon_units_per_100ms = convertRevtoTalonUnits(convertDistanceToRev((claw_vel*5)/10)); // m/s -> m/100ms -> rev/100ms -> talon/100ms - // printf("Spin command received...\n\r"); motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds @@ -122,7 +120,6 @@ float setVelocityFromLinearVelocity(TalonSRX *motor, double claw_vel, int ms) { float setVelocityFromAngularVelocity(TalonSRX *motor, double joint_velocity, int ms) { float desired_talon_units_per_100ms = convertRevtoTalonUnits(joint_velocity / (2*M_PI*10)); // rad/s -> rad/100ms -> rev/100ms -> talon/100ms - // printf("Spin command received...\n\r"); motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp index a14fac4d..a68327b3 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp @@ -36,8 +36,6 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector export_command_interfaces() override; - void enable_system_thread(); - // Lifecycle hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -65,11 +63,21 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + // Helper Functions + void logger_function(); + void enable_system_thread(); + private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + // Keeps track of amount of joints int num_joints; - int update_rate; - std::string can_interface; // Maximum displacement for prismatic joints std::vector max_disp; @@ -82,8 +90,14 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector joint_command_position_; std::vector joint_command_velocity_; + // Talon specific information std::vector joint_node_ids; + std::string can_interface; + std::vector talon_motors; + std::thread worker; + std::atomic is_running = false; + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -94,6 +108,7 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh // Active control mode std::vector control_level_; + // Types of joints enum class joint_type_t : std::uint8_t { REVOLUTE = 0, @@ -102,10 +117,6 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh // Type of joint for each actuator std::vector joint_type_; - - std::vector talon_motors; - std::thread worker; - std::atomic is_running = false; }; } // namespace talon_hardware_interface diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 6dd44aee..3ca31325 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -19,13 +19,47 @@ #include "talon_ros2_control/talon_control.h" -#define DEBUG_MODE 0 // 0 for off 1 for on - namespace talon_ros2_control { TALONHardwareInterface::TALONHardwareInterface() { } +void TALONHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nTALON Logger"; + std::ostringstream oss; + std::string status; + + // HWI Specific + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << "\n" + // << " | Gear Ratio: " << joint_gear_ratios[i] + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Joint Command Position: " << joint_command_position_[i] << "\n" + << "Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] << "\n" + << "Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), log_msg.c_str()); +} + hardware_interface::CallbackReturn TALONHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { @@ -37,6 +71,8 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_init( // -- Parameters -- can_interface = info_.hardware_parameters.at("can_interface"); update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); for (auto& joint : info_.joints) { joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); @@ -51,7 +87,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_init( joint_type_.push_back(joint_type_t::PRISMATIC); max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); } else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'max_disp' parameter.", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("TALONHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'max_disp' parameter.", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } } @@ -190,53 +226,64 @@ hardware_interface::return_type TALONHardwareInterface::read( for(int i = 0; i < num_joints; i++){ joint_state_position_[i] = getPositionDistance(talon_motors[i]); // meters joint_state_velocity_[i] = getClawVelocity(talon_motors[i]); // m/s - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Reading for Talon - Joint position: %f Joint velocity: %f \n", - joint_state_position_[i], - joint_state_velocity_[i]); - } } return hardware_interface::return_type::OK; } hardware_interface::return_type talon_ros2_control::TALONHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - for(int i = 0; i < num_joints; i++){ - if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_position_[i])) { + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + int32_t joint_angle = 0; + int32_t joint_velocity = 0; - // TO DO: implement joint limits so i dont gotta do this - joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, max_disp[i]); - // COMMAND PRISMATIC POSITION - setPositionFromDisplacement(talon_motors[i], joint_command_position_[i], 50); + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } - } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_velocity_[i])) { + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; - // COMMAND PRISMATIC VELOCITY - setVelocityFromLinearVelocity(talon_motors[i], joint_command_velocity_[i], 50); + // Motor Command for each joint at given frequency + for(int i = 0; i < num_joints; i++){ + if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_position_[i])) { - } else if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_position_[i])) { + // TO DO: implement joint limits so i dont gotta do this + joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, max_disp[i]); + // COMMAND PRISMATIC POSITION + setPositionFromDisplacement(talon_motors[i], joint_command_position_[i], 50); - // COMMAND REVOLUTE POSITION - setPositionFromJointCommand(talon_motors[i], joint_command_position_[i], 50); + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_velocity_[i])) { - } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_velocity_[i])) { + // COMMAND PRISMATIC VELOCITY + setVelocityFromLinearVelocity(talon_motors[i], joint_command_velocity_[i], 50); - // COMMAND REVOLUTE VELOCITY - setVelocityFromAngularVelocity(talon_motors[i], joint_command_velocity_[i], 50); + } else if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_position_[i])) { - } - else { - // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); - } - - if(control_level_[i] == integration_level_t::POSITION && DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing positions for Talon - Joint command: %f", - joint_command_position_[i]); - } else if(control_level_[i] == integration_level_t::VELOCITY && DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing velocities for Talon - Joint command: %f", - joint_command_velocity_[i]); + // COMMAND REVOLUTE POSITION + setPositionFromJointCommand(talon_motors[i], joint_command_position_[i], 50); + + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_velocity_[i])) { + + // COMMAND REVOLUTE VELOCITY + setVelocityFromAngularVelocity(talon_motors[i], joint_command_velocity_[i], 50); + + } + else { + // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); + } } } From 075aba4115fccbc4299a2b00bd63df302fc1dc36 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Tue, 3 Feb 2026 05:22:58 -0500 Subject: [PATCH 09/10] fixed bug in controller and further improved hwis --- .../rmd_hardware_interface.hpp | 11 +++ .../src/rmd_hardware_interface.cpp | 79 +++++++-------- .../servo_hardware_interface.hpp | 9 ++ .../src/servo_hardware_interface.cpp | 95 ++++++++++--------- .../smc_hardware_interface.hpp | 9 ++ .../src/smc_hardware_interface.cpp | 68 ++++++------- .../src/talon_hardware_interface.cpp | 13 ++- src/subsystems/arm/README.md | 4 +- .../manual_arm_joint_by_joint_controller.cpp | 46 +++++---- 9 files changed, 181 insertions(+), 153 deletions(-) diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index e4ffc1d9..e97c7b06 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -69,6 +69,7 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // -- Helper Functions -- + void send_command(int can_id, int cmd_id); void on_can_message(const CANLib::CanFrame& frame); void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); @@ -125,6 +126,16 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Active control mode for each actuator std::vector control_level_; + + // CAN Commands + static constexpr uint8_t BRAKE_RELEASE_CMD = 0X77; + static constexpr uint8_t BRAKE_LOCK_CMD = 0x78; + static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; + static constexpr uint8_t MOTOR_STOP_CMD = 0x81; + static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; + static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; + }; } // namespace rmd_hardware_interface diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index fd3be944..fcb765db 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -45,6 +45,15 @@ int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_veloci return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); } +void RMDHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 8; + frame.data.fill(0); + frame.data[0] = cmd_id; + canBus.send(frame); +} + void RMDHardwareInterface::logger_function(){ // Prevents breaking the logger @@ -52,6 +61,7 @@ void RMDHardwareInterface::logger_function(){ // Building Message std::string log_msg = "\033[2J\033[H \nRMD Logger"; + std::string control_mode = ""; // HWI Specific std::ostringstream oss; @@ -64,13 +74,22 @@ void RMDHardwareInterface::logger_function(){ << "\n--- Joint Specific ---"; for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } oss << "\nJOINT: " << info_.joints[i].name << "\n" << "Parameters: Write CAN ID: 0x" << std::hex << std::uppercase << joint_node_write_ids[i] << " | Read CAN ID: 0x" << std::hex << std::uppercase << joint_node_read_ids[i] << " | Gear Ratio: " << joint_gear_ratios[i] << " | Orientation: " << joint_orientation[i] << "\n" << "-- Commands --\n" - << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Control Mode: " << control_mode << "\n" << "Motor Position: " << motor_position[i] << " | Joint Command Position: " << joint_command_position_[i] << "\n" << "Motor Velocity: " << motor_velocity[i] @@ -194,15 +213,15 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::string result; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + std::vector data(8, 0x00); double raw_motor_velocity = 0.0; double raw_motor_position = 0.0; for(int i = 0; i < num_joints; i++){ if(can_rx_frame_.id == joint_node_read_ids[i] && - (can_rx_frame_.data[0] == 0xA2 || - can_rx_frame_.data[0] == 0xA4 || - can_rx_frame_.data[0] == 0x9C) && + (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || + can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || + can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && can_rx_frame_.data[1] != 0x00) { // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) @@ -234,18 +253,11 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Shutdown Command - can_tx_frame_.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + std::vector data(8, 0x00); - // Brake Lock Command (don't think this works) - can_tx_frame_.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + for(int i = 0; i < num_joints; i++){ + send_command(joint_node_write_ids[i], MOTOR_SHUTDOWN_CMD); // Motor Shutdown Command + send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) } canBus.close(); @@ -259,13 +271,8 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Activating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - // Brake Release command (pretty sure brakes don't work) - can_tx_frame_.data = {0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_write_ids[i], BRAKE_RELEASE_CMD); } // Sets initial command to joint state @@ -283,17 +290,8 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Stop Command - can_tx_frame_.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // Brake Lock Command (don't think this works) - can_tx_frame_.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_write_ids[i], MOTOR_STOP_CMD); // Motor Stop Command + send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully deactivated all RMD motors!"); @@ -320,7 +318,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( { elapsed_update_time+=period.seconds(); double update_period = 1.0/update_rate; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[8] = {0x00}; int32_t joint_angle = 0; int32_t joint_velocity = 0; @@ -353,7 +351,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE - data[0] = 0xA4; + data[0] = ABSOLUTE_POS_CONTROL_CMD; data[1] = 0x00; data[2] = operating_velocity & 0xFF; data[3] = (operating_velocity >> 8) & 0xFF; @@ -368,7 +366,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE - data[0] = 0xA2; + data[0] = SPEED_CONTROL_CMD; data[1] = 0x00; data[2] = 0x00; data[3] = 0x00; @@ -380,14 +378,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( else{ // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); // ENCODING CAN MESSAGE - data[0] = 0x9C; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = 0x00; - data[5] = 0x00; - data[6] = 0x00; - data[7] = 0x00; + data[0] = MOTOR_STATUS_2_CMD; } // Cast data to uint8_t diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index cf4ee76b..d6e10772 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -153,6 +153,15 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Type of joint for each actuator std::vector joint_type_; + // CAN Commands + static constexpr uint8_t PCB_HEARTBEAT_CMD = 0X10; + static constexpr uint8_t LED_STATUS_CMD = 0x11; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; + static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; + static constexpr uint8_t MOTOR_STATE_CMD = 0x4; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x5; + static constexpr uint8_t MAINTENANCE_CMD = 0x6; + static constexpr uint8_t SERVO_SPECS_CMD = 0x7; }; } // namespace servo_hardware_interface diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index 3345aa55..4a69414e 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -106,7 +106,7 @@ void SERVOHardwareInterface::logger_function(){ << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << " | Gear Ratio: " << joint_gear_ratios[i] << " | Device Status: " << std::hex << std::uppercase << device_status[i] - << "- " << status << "\n" + << " - " << status << "\n" << "-- Commands --\n" << "Control Mode: " << static_cast(control_level_[i]) << "\n" << "Motor Position: " << motor_position[i] @@ -198,7 +198,7 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( motor_position.assign(num_joints, 0.0); motor_velocity.assign(num_joints, 0.0); - device_status.assign(num_joints, -1); + device_status.assign(num_joints, 0); // Set initial control type based on servo type control_level_.resize(num_joints, integration_level_t::POSITION); @@ -274,22 +274,20 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::string result; - int data[7] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[7] = {0x00}; int8_t command_nibble = ((can_rx_frame_.data[0]>>4) & 0x0F); int8_t device_id_nibble = (can_rx_frame_.data[0] & 0x0F); double raw_motor_position = 0.0; double raw_motor_velocity = 0.0; for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == can_response_id && command_nibble == 0x7 && device_id_nibble == joint_node_ids[i]){ + if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATE_CMD && device_id_nibble == joint_node_ids[i]){ // DECODING CAN MESSAGE FOR VELOCITY data[1] = can_rx_frame_.data[1]; // Position low byte data[2] = can_rx_frame_.data[2]; // Position high byte data[3] = can_rx_frame_.data[3]; // Velocity low byte data[4] = can_rx_frame_.data[4]; // Velocity high byte - data[6] = can_rx_frame_.data[6]; // Device Status - // POSITION // uint32 -> int16 -> double (for calcs) @@ -311,9 +309,11 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { else{ RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); } - + } + else if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATUS_CMD && device_id_nibble == joint_node_ids[i]){ + // Populate device status - device_status[i] = data[6]; + device_status[i] = can_rx_frame_.data[1]; } else{ if(logger_state == 1) { @@ -330,16 +330,17 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up ...please wait..."); // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - // Motor Shutdown Command - int8_t command_nibble = 0x5; + int8_t command_nibble = MAINTENANCE_CMD; + int8_t maintenance_command = 2; // Motor Shutdown Command int8_t device_id_nibble; for(int i = 0; i < num_joints; i++){ can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 1; + can_tx_frame_.dlc = 2; device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); + can_tx_frame_.data[1] = maintenance_command; canBus.send(can_tx_frame_); } @@ -372,17 +373,19 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Deactivating ...please wait..."); - int8_t command_nibble = 0x6; + int8_t command_nibble = MAINTENANCE_CMD; + int8_t maintenance_command = 1; // Motor Stop Command int8_t device_id_nibble; for(int i = 0; i < num_joints; i++){ can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 1; + can_tx_frame_.dlc = 2; // Motor Stop Command device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); + can_tx_frame_.data[1] = maintenance_command; canBus.send(can_tx_frame_); } @@ -415,7 +418,7 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ { elapsed_update_time+=period.seconds(); double update_period = 1.0/update_rate; - int data[3] = {0x00, 0x00, 0x00}; + int data[3] = {0x00}; int8_t command_nibble; int8_t device_id_nibble; int16_t joint_angle = 0; @@ -441,15 +444,6 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ // Motor Command for each joint at given frequency for(int i = 0; i < num_joints; i++) { - // Motor Status Command for each joint at given frequency - command_nibble = 0x7; - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 1; - device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; - canBus.send(can_tx_frame_); - // Motor Command for each joint at given frequency can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; @@ -469,9 +463,15 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ } // ENCODING CAN MESSAGE - data[0] = 0x20 + joint_node_ids[i]; + data[0] = ABSOLUTE_POS_CONTROL_CMD + joint_node_ids[i]; data[1] = joint_angle & 0xFF; data[2] = (joint_angle >> 8) & 0xFF; + + // Cast data to uint8_t + for(int j = 0; j < 3; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } } else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { @@ -489,31 +489,38 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ } // ENCODING CAN MESSAGE - data[0] = 0x30 + joint_node_ids[i]; + data[0] = VELOCITY_CONTROL_CMD + joint_node_ids[i]; data[1] = joint_velocity & 0xFF; data[2] = (joint_velocity >> 8) & 0xFF; - } - else if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { - if (logger_state == 1) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); + // Cast data to uint8_t + for(int j = 0; j < 3; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; } + } - else if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { - if (logger_state == 1) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); + else { + command_nibble = MOTOR_STATE_CMD; // Motor State Command for each joint at given frequency + can_tx_frame_.dlc = 1; + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + + if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); + } } - } - else{ - if (logger_state == 1) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + else if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); + } + } + else{ + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + } } - } - - // Cast data to uint8_t - for(int j = 0; j < 3; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; } canBus.send(can_tx_frame_); diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 73b301d4..57247583 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -69,6 +69,7 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // -- Helper Functions -- + void send_command(int can_id, int cmd_id); void on_can_message(const CANLib::CanFrame& frame); void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); @@ -125,6 +126,14 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Active control mode for each actuator std::vector control_level_; + + // CAN Commands + static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; + static constexpr uint8_t MOTOR_STOP_CMD = 0x81; + static constexpr uint8_t MOTOR_RUNNING_CMD = 0x88; + static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; + static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; }; } // namespace smc_hardware_interface diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 19e6440c..1021bd37 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -42,6 +42,15 @@ int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_veloci return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); } +void SMCHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 8; + frame.data.fill(0); + frame.data[0] = cmd_id; + canBus.send(frame); +} + void SMCHardwareInterface::logger_function(){ // Prevents breaking the logger @@ -49,6 +58,7 @@ void SMCHardwareInterface::logger_function(){ // Building Message std::string log_msg = "\033[2J\033[H \nSMC Logger"; + std::string control_mode = ""; // HWI Specific std::ostringstream oss; @@ -61,11 +71,20 @@ void SMCHardwareInterface::logger_function(){ << "\n--- Joint Specific ---"; for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } oss << "\nJOINT: " << info_.joints[i].name << "\n" << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << " | Gear Ratio: " << joint_gear_ratios[i] << "\n" << "-- Commands --\n" - << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Control Mode: " << control_mode << "\n" << "Motor Position: " << motor_position[i] << " | Joint Command Position: " << joint_command_position_[i] << "\n" << "Motor Velocity: " << motor_velocity[i] @@ -181,13 +200,13 @@ void SMCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::string result; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[8] = {0x00}; for(int i = 0; i < num_joints; i++){ if(can_rx_frame_.id == joint_node_ids[i] && - (can_rx_frame_.data[0] == 0xA2 || - can_rx_frame_.data[0] == 0xA4 || - can_rx_frame_.data[0] == 0x9C) && + (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || + can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || + can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && can_rx_frame_.data[1] != 0x00){ // DECODING CAN MESSAGE FOR VELOCITY @@ -244,13 +263,8 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_cleanup( { // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - // Motor Off (Shutdown) Command - can_tx_frame_.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_ids[i], MOTOR_SHUTDOWN_CMD); } canBus.close(); @@ -265,14 +279,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Activating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - - can_tx_frame_.data = {0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - control_level_[i] = integration_level_t::UNDEFINED; + send_command(joint_node_ids[i], MOTOR_RUNNING_CMD); } // Sets initial command to joint state @@ -290,13 +297,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Stop Command - can_tx_frame_.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_ids[i], MOTOR_STOP_CMD); } RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully deactivated all SMC motors!"); @@ -323,7 +324,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( { elapsed_update_time+=period.seconds(); double update_period = 1.0/update_rate; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[8] = {0x00}; int32_t joint_angle = 0; int16_t operating_velocity = 200; int32_t joint_velocity = 0; @@ -357,7 +358,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE - data[0] = 0xA4; + data[0] = ABSOLUTE_POS_CONTROL_CMD; data[1] = 0x00; data[2] = operating_velocity & 0xFF; data[3] = (operating_velocity >> 8) & 0xFF; @@ -372,7 +373,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE - data[0] = 0xA2; + data[0] = SPEED_CONTROL_CMD; data[1] = 0x00; data[2] = 0x00; data[3] = 0x00; @@ -384,14 +385,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( else{ // RCLCPP_WARN(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); // ENCODING CAN MESSAGE - data[0] = 0x9C; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = 0x00; - data[5] = 0x00; - data[6] = 0x00; - data[7] = 0x00; + data[0] = MOTOR_STATUS_2_CMD; } // Cast data to uint8_t diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 3ca31325..a376c1fa 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -31,6 +31,7 @@ void TALONHardwareInterface::logger_function(){ // Building Message std::string log_msg = "\033[2J\033[H \nTALON Logger"; + std::string control_mode = ""; std::ostringstream oss; std::string status; @@ -43,12 +44,20 @@ void TALONHardwareInterface::logger_function(){ << "\n--- Joint Specific ---"; for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } oss << "\nJOINT: " << info_.joints[i].name << "\n" << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << "\n" - // << " | Gear Ratio: " << joint_gear_ratios[i] << "-- Commands --\n" - << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Control Mode: " << control_mode << "\n" << "Joint Command Position: " << joint_command_position_[i] << "\n" << "Joint Command Velocity: " << joint_command_velocity_[i] << "\n" << "-- State --\n" diff --git a/src/subsystems/arm/README.md b/src/subsystems/arm/README.md index 82bd24bd..fa3f71e5 100644 --- a/src/subsystems/arm/README.md +++ b/src/subsystems/arm/README.md @@ -65,9 +65,9 @@ Use *ip link* to check that there is a can0/vcan0. **Open another terminal, source the workspace, and call the service to set controllers:** ```bash source install/setup.bash -ros2 service call /set_controller athena_arm_msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" ``` -Example: `ros2 service call /set_controller athena_arm_msgs/srv/SetController "{controller_names: [joint_trajectory_controller, arm_velocity_controller]}"` +Example: `ros2 service call /set_controller msgs/srv/SetController "{controller_names: [joint_trajectory_controller, arm_velocity_controller]}"` --- diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index b1fe5161..cbd74874 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -135,16 +135,17 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::co command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(num_joints); - for (const auto & joint : params_.position_joints) - { - command_interfaces_config.names.push_back(joint + "/position"); - } for (const auto & joint : params_.velocity_joints) { command_interfaces_config.names.push_back(joint + "/velocity"); } + for (const auto & joint : params_.position_joints) + { + command_interfaces_config.names.push_back(joint + "/position"); + } + return command_interfaces_config; } @@ -154,14 +155,12 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; state_interfaces_config.names.reserve(num_joints); - for (const auto & joint : params_.position_joints) - { - state_interfaces_config.names.push_back(joint + "/position"); + for (const auto & joint : params_.velocity_joints) { + state_interfaces_config.names.push_back(joint + "/velocity"); } - for (const auto & joint : params_.velocity_joints) - { - state_interfaces_config.names.push_back(joint + "/velocity"); + for (const auto & joint : params_.position_joints) { + state_interfaces_config.names.push_back(joint + "/position"); } return state_interfaces_config; @@ -170,10 +169,6 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st controller_interface::CallbackReturn ManualArmJointByJointController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): if you have to manage multiple interfaces that need to be sorted check - // `on_activate` method in `JointTrajectoryController` for exemplary use of - // `controller_interface::get_ordered_interfaces` helper function - // Set default value in command reset_controller_reference_msg(*(input_ref_.readFromRT)(), joystick_axes, joystick_buttons); @@ -183,8 +178,6 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_activat controller_interface::CallbackReturn ManualArmJointByJointController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop for (size_t i = 0; i < command_interfaces_.size(); ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); @@ -199,18 +192,22 @@ controller_interface::return_type ManualArmJointByJointController::update( if (!std::isnan((*current_ref)->axes[0])) { - // ODrive + // Base Yaw: L/R Left Stick joint_velocities_[0] = ((*current_ref)->buttons[1] == 1) ? 0.0 : (*current_ref)->axes[0] * max_velocities_[0]; - // RMD - joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; // Shoulder Pitch - joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; // Elbow Pitch + // Shoulder Pitch: U/D Left Stick + joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; + + // Elbow Pitch: U/D Right Stick + joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; - // SMC + // Wrist Pitch: U/D on Left Joystick AND O button joint_velocities_[3] = -(*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]) * max_velocities_[3]; + + // Wrist Roll: L/R on Left Joystick AND O button joint_velocities_[4] = -(*current_ref)->axes[0] * static_cast((*current_ref)->buttons[1]) * max_velocities_[4]; - // Talons + // Gripper Claw: Bumpers and Triggers if ((*current_ref)->buttons[4] && (*current_ref)->buttons[5]) { // closeClaw(motor); } else if ((*current_ref)->buttons[4]) { // Left bumper @@ -224,14 +221,13 @@ controller_interface::return_type ManualArmJointByJointController::update( } else{ joint_velocities_[5] = 0.0; } - // RCLCPP_INFO(get_node()->get_logger(), "Talon: %f", joint_velocities_[5]); // Actuator (EXPERIMENTAL) // Pressing square activates actuator movement if((*current_ref)->buttons[3] == 1){ actuator_active_ = true; actuator_iterator = 0.001; - joint_velocities_[6] == 0.001; // to start off + joint_velocities_[6] = 0.001; // to start off } // Once actuator reaches original position, stop movement @@ -250,6 +246,8 @@ controller_interface::return_type ManualArmJointByJointController::update( joint_velocities_.resize(7, 0.0); } + // RCLCPP_INFO(get_node()->get_logger(), "Size of Command Interface: %d", command_interfaces_.size()); + for (size_t i = 0; i < command_interfaces_.size(); ++i) { if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) From c9811e22945c557295671822445e8947263926fb Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Wed, 4 Feb 2026 02:38:58 -0500 Subject: [PATCH 10/10] talon fixes --- .../src/talon_hardware_interface.cpp | 102 ++++++++++++------ 1 file changed, 68 insertions(+), 34 deletions(-) diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index a376c1fa..b06ea01b 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -303,48 +303,82 @@ hardware_interface::return_type TALONHardwareInterface::perform_command_mode_swi const std::vector& start_interfaces, const std::vector& stop_interfaces) { - std::vector new_modes = {}; - for (std::string key : start_interfaces) - { - for (int i = 0; i < num_joints; i++){ - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ - new_modes.push_back(integration_level_t::POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ - new_modes.push_back(integration_level_t::VELOCITY); + // Debug: print incoming requests + std::ostringstream ss; + ss << "perform_command_mode_switch called. start_interfaces: ["; + for (auto &s : start_interfaces) ss << s << ","; + ss << "] stop_interfaces: ["; + for (auto &s : stop_interfaces) ss << s << ","; + ss << "]"; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), ss.str().c_str()); + + // For each joint, decide its new control mode based on start/stop interfaces. + // We allow partial starts/stops: only affected joints are switched. + std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + + // Process stop interfaces first: mark those joints as UNDEFINED + for (const auto &ifname : stop_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + requested_modes[i] = integration_level_t::UNDEFINED; } } } - - // All joints must be given new command mode at the same time - if (new_modes.size() != static_cast(num_joints)){ - return hardware_interface::return_type::ERROR; - } - // All joints must have the same command mode - if (!std::all_of( - new_modes.begin() + 1, new_modes.end(), - [&](integration_level_t mode) { return mode == new_modes[0]; })) - { - return hardware_interface::return_type::ERROR; - } - // Stop motion on all relevant joints that are stopping - for (std::string key : stop_interfaces) { - for (int i = 0; i < num_joints; i++){ - if (key.find(info_.joints[i].name) != std::string::npos) { - joint_command_position_[i] = 0; - joint_command_velocity_[i] = 0; - control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined + // Process start interfaces: set POSITION or VELOCITY per joint + for (const auto &ifname : start_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if) { + requested_modes[i] = integration_level_t::POSITION; + } else if (ifname == vel_if) { + requested_modes[i] = integration_level_t::VELOCITY; } } } - // Set the new command modes. By this point everything should be undefined after the "stop motion" loop - for (int i = 0; i < num_joints; i++){ - if (control_level_[i] != integration_level_t::UNDEFINED) { - // Something else is using the joint! Abort! - return hardware_interface::return_type::ERROR; + + // Now apply the requested_modes to control_level_. + // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. + for (int i = 0; i < num_joints; ++i) { + if (requested_modes[i] == integration_level_t::UNDEFINED) { + // if stop requested, set to UNDEFINED; otherwise leave existing mode + // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) + bool was_stopped = false; + for (const auto &ifname : stop_interfaces) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + was_stopped = true; + break; + } + } + if (was_stopped) { + control_level_[i] = integration_level_t::UNDEFINED; + joint_command_velocity_[i] = 0; + // optional: reset position cmd to current state to be safe + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + } + // else, leave control_level_ as-is + } else { + // Set the mode requested + control_level_[i] = requested_modes[i]; + // If switching to velocity, optionally set command velocity to current state to avoid jumps + if (requested_modes[i] == integration_level_t::VELOCITY) { + // joint_command_velocity_[i] = joint_state_velocity_[i]; + joint_command_velocity_[i] = 0; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_velocity_[i]); + } else if (requested_modes[i] == integration_level_t::POSITION) { + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_position_[i]); + } } - control_level_[i] = new_modes[i]; } return hardware_interface::return_type::OK;