From 110fd278ffc278cabefb1531a781fa4a45624288 Mon Sep 17 00:00:00 2001 From: Yue Yin Date: Wed, 23 Jul 2025 16:08:34 +0900 Subject: [PATCH] Update hardware interface (#43) - Minimal working hardware plugin with v1.0 openarmcan. - gripper logic unimplemented --- openarm_hardware/CMakeLists.txt | 38 +- .../include/openarm_hardware/canbus.hpp | 48 -- .../include/openarm_hardware/motor.hpp | 156 ------ .../openarm_hardware/motor_control.hpp | 87 ---- .../openarm_hardware/openarm_hardware.hpp | 110 ---- .../openarm_hardware/v10_simple_hardware.hpp | 138 +++++ openarm_hardware/openarm_hardware.xml | 6 +- openarm_hardware/package.xml | 4 +- openarm_hardware/setup.bash | 34 -- openarm_hardware/src/canbus.cpp | 131 ----- openarm_hardware/src/motor.cpp | 152 ------ openarm_hardware/src/motor_control.cpp | 472 ------------------ openarm_hardware/src/openarm_hardware.cpp | 257 ---------- openarm_hardware/src/v10_simple_hardware.cpp | 322 ++++++++++++ .../test/test_openarm_hardware.cpp | 128 ----- 15 files changed, 474 insertions(+), 1609 deletions(-) delete mode 100644 openarm_hardware/include/openarm_hardware/canbus.hpp delete mode 100644 openarm_hardware/include/openarm_hardware/motor.hpp delete mode 100644 openarm_hardware/include/openarm_hardware/motor_control.hpp delete mode 100644 openarm_hardware/include/openarm_hardware/openarm_hardware.hpp create mode 100644 openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp delete mode 100755 openarm_hardware/setup.bash delete mode 100644 openarm_hardware/src/canbus.cpp delete mode 100644 openarm_hardware/src/motor.cpp delete mode 100644 openarm_hardware/src/motor_control.cpp delete mode 100644 openarm_hardware/src/openarm_hardware.cpp create mode 100644 openarm_hardware/src/v10_simple_hardware.cpp delete mode 100644 openarm_hardware/test/test_openarm_hardware.cpp diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index af0ab4d..a76fb3e 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -27,11 +27,11 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +# Find openarm_can library +find_package(OpenArmCAN REQUIRED) + add_library(${PROJECT_NAME} SHARED - src/openarm_hardware.cpp - src/canbus.cpp - src/motor.cpp - src/motor_control.cpp + src/v10_simple_hardware.cpp ) target_include_directories( @@ -40,6 +40,10 @@ target_include_directories( include ) +target_link_libraries(${PROJECT_NAME} + OpenArmCAN::openarm_can +) + ament_target_dependencies(${PROJECT_NAME} hardware_interface pluginlib @@ -53,35 +57,11 @@ install( TARGETS ${PROJECT_NAME} DESTINATION lib ) - + install(DIRECTORY include/ DESTINATION include ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gmock REQUIRED) - find_package(hardware_interface REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_openarm_hardware - test/test_openarm_hardware.cpp - ) - target_link_libraries(test_openarm_hardware - ${PROJECT_NAME} - ) - ament_target_dependencies(test_openarm_hardware - hardware_interface - ros2_control_test_assets - ) - - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - - ament_export_include_directories( include ) diff --git a/openarm_hardware/include/openarm_hardware/canbus.hpp b/openarm_hardware/include/openarm_hardware/canbus.hpp deleted file mode 100644 index dfa42a7..0000000 --- a/openarm_hardware/include/openarm_hardware/canbus.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -enum CANMode { CAN_MODE_CLASSIC = 0, CAN_MODE_FD = 1 }; -class CANBus { - public: - explicit CANBus(const std::string& interface, int mode); - ~CANBus(); - int whichCAN(); - bool send(uint16_t motor_id, const std::array& data); - std::array recv(uint16_t& out_id, uint8_t& out_len); - - private: - bool sendClassic(uint16_t motor_id, const std::array& data); - bool sendFD(uint16_t motor_id, const std::array& data); - - struct can_frame recvClassic(); - struct canfd_frame recvFD(); - - int sock_; - int mode_; -}; diff --git a/openarm_hardware/include/openarm_hardware/motor.hpp b/openarm_hardware/include/openarm_hardware/motor.hpp deleted file mode 100644 index e006ee2..0000000 --- a/openarm_hardware/include/openarm_hardware/motor.hpp +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include - -enum class DM_Motor_Type : uint8_t { - DM4310 = 0, - DM4310_48V, - DM4340, - DM4340_48V, - DM6006, - DM8006, - DM8009, - DM10010L, - DM10010, - DMH3510, - DMH6215, - DMG6220, - COUNT -}; - -enum class DM_variable : uint8_t { - UV_Value = 0, - KT_Value, - OT_Value, - OC_Value, - ACC, - DEC, - MAX_SPD, - MST_ID, - ESC_ID, - TIMEOUT, - CTRL_MODE, - Damp, - Inertia, - hw_ver, - sw_ver, - SN, - NPP, - Rs, - LS, - Flux, - Gr, - PMAX, - VMAX, - TMAX, - I_BW, - KP_ASR, - KI_ASR, - KP_APR, - KI_APR, - OV_Value, - GREF, - Deta, - V_BW, - IQ_c1, - VL_c1, - can_br, - sub_ver, - u_off = 50, - v_off, - k1, - k2, - m_off, - dir, - p_m = 80, - xout, - COUNT -}; - -enum class Control_Type : uint8_t { MIT = 1, POS_VEL, VEL, Torque_Pos, COUNT }; - -class Motor { - public: - Motor() = default; - Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID); - - void recv_data(double q, double dq, double tau, int tmos, int trotor); - double getPosition() const; - double getVelocity() const; - double getTorque() const; - int getParam(int RID) const; - void setTempParam(int RID, int val); - uint16_t SlaveID; - uint16_t MasterID; - bool isEnable; - Control_Type NowControlMode; - DM_Motor_Type MotorType; - - int getStateTmos() const; - int getStateTrotor() const; - double getGoalPosition() const; - double getGoalVelocity() const; - double getGoalTau() const; - - void setGoalPosition(double pos); - void setGoalVelocity(double vel); - void setGoalTau(double tau); - void setStateTmos(int tmos); - void setStateTrotor(int trotor); - - private: - double Pd, Vd; - double goal_position, goal_velocity, goal_tau; - double state_q, state_dq, state_tau; - int state_tmos, state_trotor; - std::map temp_param_dict; -}; - -double LIMIT_MIN_MAX(double x, double min, double max); - -uint16_t double_to_uint(double x, double x_min, double x_max, int bits); - -double uint_to_double(uint16_t x, double min, double max, int bits); - -std::array double_to_uint8s(double value); - -std::array float_to_uint8s(float value); - -float uint8s_to_float(const std::array& bytes); - -std::array data_to_uint8s(uint32_t value); - -uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, - uint8_t byte4); - -double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, - uint8_t byte4); - -bool is_in_ranges(int number); - -void print_hex(const std::vector& data); - -template -T get_enum_by_index(int index); diff --git a/openarm_hardware/include/openarm_hardware/motor_control.hpp b/openarm_hardware/include/openarm_hardware/motor_control.hpp deleted file mode 100644 index 57660a8..0000000 --- a/openarm_hardware/include/openarm_hardware/motor_control.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "canbus.hpp" -#include "motor.hpp" - -class MotorControl { - public: - explicit MotorControl(CANBus& canbus); - bool addMotor(Motor& motor); - void enable(Motor& motor); - void disable(Motor& motor); - void set_zero_position(Motor& motor); - void controlMIT(Motor& motor, double kp, double kd, double q, double dq, - double tau); - void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, - double tau); - void sendData(uint16_t motor_id, const std::array& data); - void recv(); - - void control_delay(Motor& motor, double kp, double kd, double q, double dq, - double tau, double delay); - void controlPosVel(Motor& motor, double q, double dq); - void controlPosVel2(Motor& motor, double q, double dq); - void controlVel(Motor& motor, double dq); - void controlVel2(Motor& motor, double dq); - void controlPosForce(Motor& motor, double q, double vel, double tau); - void controlPosForce2(Motor& motor, double q, double vel, double tau); - - bool switchControlMode(Motor& motor, Control_Type controlMode); - void save_motor_param(Motor& motor); - void change_limit_param(); - // void change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, - // double TMAX); - - void recv_set_param_data(); - - private: - CANBus& canbus_; - - std::map motors_map_; - static constexpr double Limit_Param[12][3] = { - {12.5, 30, 10}, // DM4310 - {12.5, 50, 10}, // DM4310_48V - {12.5, 8, 28}, // DM4340 - {12.5, 10, 28}, // DM4340_48V - {12.5, 45, 20}, // DM6006 - {12.5, 45, 40}, // DM8006 - {12.5, 45, 54}, // DM8009 - {12.5, 25, 200}, // DM10010L - {12.5, 20, 200}, // DM10010 - {12.5, 280, 1}, // DMH3510 - {12.5, 45, 10}, // DMH6215 - {12.5, 45, 10}, // DMG6220 - }; - - void processPacket(const can_frame& frame); - void processPacketFD(const canfd_frame& frame); - void controlCmd(Motor& motor, uint8_t cmd); - void readRIDParam(Motor& motor, DM_variable RID); - void writeMotorParam(Motor& motor, DM_variable RID, double value); -}; diff --git a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp deleted file mode 100644 index 6ae13aa..0000000 --- a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// Copyright 2025 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. - -#pragma once - -#include -#include -#include - -#include "canbus.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "motor.hpp" -#include "motor_control.hpp" -#include "openarm_hardware/visibility_control.h" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace openarm_hardware { - -std::vector motor_types{ - DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, - DM_Motor_Type::DM4340, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310, - DM_Motor_Type::DM4310}; -std::vector can_device_ids{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; -std::vector can_master_ids{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; -static const Control_Type CONTROL_MODE = Control_Type::MIT; -static const std::size_t ARM_DOF = 7; -static const std::size_t GRIPPER_DOF = 1; -static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF; -static const std::array KP = {80.0, 80.0, 20.0, 55.0, - 5.0, 5.0, 5.0, 0.5}; -static const std::array KD = {2.75, 2.5, 0.7, 0.4, - 0.7, 0.6, 0.5, 0.1}; -static const double START_POS_TOLERANCE_RAD = 0.1; -static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 16.0; - -static const bool USING_GRIPPER = true; -static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853; -static const double GRIPPER_GEAR_DIRECTION_MULTIPLIER = -1.0; -static const int GRIPPER_INDEX = TOTAL_DOF - 1; - -class OpenArmHW : public hardware_interface::SystemInterface { - public: - OpenArmHW(); - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo& info) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State& previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_state_interfaces() - override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_command_interfaces() - override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State& previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State& previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type read(const rclcpp::Time& time, - const rclcpp::Duration& period) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type write( - const rclcpp::Time& time, const rclcpp::Duration& period) override; - - std::size_t curr_dof = ARM_DOF; // minus gripper - private: - std::string prefix_; - std::unique_ptr canbus_; - std::unique_ptr motor_control_; - std::vector pos_commands_; - std::vector pos_states_; - std::vector vel_commands_; - std::vector vel_states_; - std::vector tau_ff_commands_; - std::vector tau_states_; - std::vector> motors_; - - void refresh_motors(); - bool disable_torque_; -}; - -} // namespace openarm_hardware diff --git a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp new file mode 100644 index 0000000..758944b --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp @@ -0,0 +1,138 @@ +// Copyright 2025 Reazon Holdings, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "openarm_hardware/visibility_control.h" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace openarm_hardware { + +/** + * @brief Simplified OpenArm V10 Hardware Interface + * + * This is a simplified version that uses the OpenArm CAN API directly, + * following the pattern from full_arm.cpp example. Much simpler than + * the original implementation. + */ +class OpenArm_v10HW : public hardware_interface::SystemInterface { + public: + OpenArm_v10HW(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo& info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read(const rclcpp::Time& time, + const rclcpp::Duration& period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time& time, const rclcpp::Duration& period) override; + + private: + // V10 default configuration + static constexpr size_t ARM_DOF = 7; + static constexpr bool ENABLE_GRIPPER = true; + + // Default motor configuration for V10 + const std::vector DEFAULT_MOTOR_TYPES = { + openarm::damiao_motor::MotorType::DM8009, // Joint 1 + openarm::damiao_motor::MotorType::DM8009, // Joint 2 + openarm::damiao_motor::MotorType::DM4340, // Joint 3 + openarm::damiao_motor::MotorType::DM4340, // Joint 4 + openarm::damiao_motor::MotorType::DM4310, // Joint 5 + openarm::damiao_motor::MotorType::DM4310, // Joint 6 + openarm::damiao_motor::MotorType::DM4310 // Joint 7 + }; + + const std::vector DEFAULT_SEND_CAN_IDS = {0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07}; + const std::vector DEFAULT_RECV_CAN_IDS = {0x11, 0x12, 0x13, 0x14, + 0x15, 0x16, 0x17}; + + const openarm::damiao_motor::MotorType DEFAULT_GRIPPER_MOTOR_TYPE = + openarm::damiao_motor::MotorType::DM4310; + const uint32_t DEFAULT_GRIPPER_SEND_CAN_ID = 0x08; + const uint32_t DEFAULT_GRIPPER_RECV_CAN_ID = 0x18; + + // Default gains + const std::vector DEFAULT_KP = {80.0, 80.0, 20.0, 55.0, + 5.0, 5.0, 5.0, 0.5}; + const std::vector DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, + 0.7, 0.6, 0.5, 0.1}; + + // Configuration + std::string can_interface_; + std::string arm_prefix_; + bool hand_; + bool can_fd_; + + // OpenArm instance + std::unique_ptr openarm_; + + // Generated joint names for this arm instance + std::vector joint_names_; + + // ROS2 control state and command vectors + std::vector pos_commands_; + std::vector vel_commands_; + std::vector tau_commands_; + std::vector pos_states_; + std::vector vel_states_; + std::vector tau_states_; + + // Helper methods + void return_to_zero(); + bool parse_config(const hardware_interface::HardwareInfo& info); + void generate_joint_names(); + + // Gripper mapping functions + double joint_to_motor_radians(double joint_value); + double motor_radians_to_joint(double motor_radians); +}; + +} // namespace openarm_hardware \ No newline at end of file diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index 72a8735..07d564b 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -15,11 +15,11 @@ --> - - ros2_control hardware interface. + ros2_control hardware interface for OpenArm V10. diff --git a/openarm_hardware/package.xml b/openarm_hardware/package.xml index 0d0f360..d174f95 100644 --- a/openarm_hardware/package.xml +++ b/openarm_hardware/package.xml @@ -17,9 +17,9 @@ --> openarm_hardware - 1.0.0 + 0.3.0 Hardware interface for OpenArm - Thomason Zhou + Enactic, Inc. Apache-2.0 ament_cmake diff --git a/openarm_hardware/setup.bash b/openarm_hardware/setup.bash deleted file mode 100755 index f6c4d0f..0000000 --- a/openarm_hardware/setup.bash +++ /dev/null @@ -1,34 +0,0 @@ -#!/bin/bash -# -# Copyright 2025 Reazon Holdings, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -CAN_INTERFACE=can0 - -for DEVICE in /dev/ttyACM*; do - if [ -e "$DEVICE" ]; then - echo "Using device: $DEVICE" - break - fi -done - -if [ -z "$DEVICE" ]; then - echo "No /dev/ttyACM* device found." - exit 1 -fi - -sudo pkill -f slcand -sudo slcand -o -c -s8 "$DEVICE" "$CAN_INTERFACE" -sudo ip link set "$CAN_INTERFACE" up type can bitrate 1000000 -ip link show "$CAN_INTERFACE" diff --git a/openarm_hardware/src/canbus.cpp b/openarm_hardware/src/canbus.cpp deleted file mode 100644 index a49d2b5..0000000 --- a/openarm_hardware/src/canbus.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "openarm_hardware/canbus.hpp" - -CANBus::CANBus(const std::string& interface, int mode) : mode_(mode) { - struct ifreq ifr; - struct sockaddr_can addr; - - sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (sock_ < 0) { - perror("Error while opening CAN socket"); - exit(EXIT_FAILURE); - } - - std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ); - if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) { - perror("Error getting interface index"); - exit(EXIT_FAILURE); - } - - std::memset(&addr, 0, sizeof(addr)); - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - if (mode_ == CAN_MODE_FD) { - int enable_canfd = 1; - if (setsockopt(sock_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, - sizeof(enable_canfd)) < 0) { - perror("CAN FD setsockopt failed"); - exit(EXIT_FAILURE); - } - } - - if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { - perror("Error in CAN socket bind"); - exit(EXIT_FAILURE); - } -} - -CANBus::~CANBus() { close(sock_); } - -int CANBus::whichCAN() { return mode_; } - -bool CANBus::send(uint16_t motor_id, const std::array& data) { - if (mode_ == CAN_MODE_FD) - return sendFD(motor_id, data); - else - return sendClassic(motor_id, data); -} - -std::array CANBus::recv(uint16_t& out_id, uint8_t& out_len) { - std::array buffer = {}; - if (mode_ == CAN_MODE_FD) { - auto frame = recvFD(); - out_id = frame.can_id; - out_len = frame.len; - std::copy(frame.data, frame.data + frame.len, buffer.begin()); - } else { - auto frame = recvClassic(); - out_id = frame.can_id; - out_len = frame.can_dlc; - std::copy(frame.data, frame.data + frame.can_dlc, buffer.begin()); - } - return buffer; -} - -bool CANBus::sendClassic(uint16_t motor_id, - const std::array& data) { - struct can_frame frame; - std::memset(&frame, 0, sizeof(frame)); - - frame.can_id = motor_id; - frame.can_dlc = data.size(); - std::copy(data.begin(), data.end(), frame.data); - - if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { - perror("Error sending CAN frame"); - return false; - } - return true; -} - -bool CANBus::sendFD(uint16_t motor_id, const std::array& data) { - struct canfd_frame frame; - std::memset(&frame, 0, sizeof(frame)); - - frame.can_id = motor_id; - frame.len = data.size(); - frame.flags = CANFD_BRS; - std::copy(data.begin(), data.end(), frame.data); - - if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { - perror("Error sending CAN FD frame"); - return false; - } - return true; -} - -struct can_frame CANBus::recvClassic() { - struct can_frame frame; - std::memset(&frame, 0, sizeof(frame)); - - int nbytes = read(sock_, &frame, sizeof(struct can_frame)); - if (nbytes < 0) { - perror("CAN read error"); - } - return frame; -} - -struct canfd_frame CANBus::recvFD() { - struct canfd_frame frame; - std::memset(&frame, 0, sizeof(frame)); - - int nbytes = read(sock_, &frame, sizeof(struct canfd_frame)); - if (nbytes < 0) { - perror("CAN FD read error"); - } - return frame; -} diff --git a/openarm_hardware/src/motor.cpp b/openarm_hardware/src/motor.cpp deleted file mode 100644 index 06069ba..0000000 --- a/openarm_hardware/src/motor.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OPENARM_MOTOR_H_ -#define OPENARM_MOTOR_H_ - -#include "openarm_hardware/motor.hpp" - -Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID) - : MotorType(motorType), - SlaveID(slaveID), - MasterID(masterID), - Pd(0.0), - Vd(0.0), - goal_position(0.0), - goal_tau(0.0), - state_q(0.0), - state_dq(0.0), - state_tau(0.0), - state_tmos(0), - state_trotor(0), - isEnable(false), - NowControlMode(Control_Type::MIT) {} - -void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) { - state_q = q; - state_dq = dq; - state_tau = tau; - state_tmos = tmos; - state_trotor = trotor; -} - -double Motor::getPosition() const { return state_q; } -double Motor::getVelocity() const { return state_dq; } -double Motor::getTorque() const { return state_tau; } - -double Motor::getGoalPosition() const { return goal_position; } - -void Motor::setGoalPosition(double pos) { goal_position = pos; } - -double Motor::getGoalVelocity() const { return goal_velocity; } - -void Motor::setGoalVelocity(double velocity) { goal_velocity = velocity; } - -double Motor::getGoalTau() const { return goal_tau; } - -void Motor::setGoalTau(double tau) { goal_tau = tau; } - -int Motor::getStateTmos() const { return state_tmos; } - -void Motor::setStateTmos(int tmos) { state_tmos = tmos; } - -int Motor::getStateTrotor() const { return state_trotor; } - -void Motor::setStateTrotor(int trotor) { state_trotor = trotor; } - -int Motor::getParam(int RID) const { - auto it = temp_param_dict.find(RID); - return (it != temp_param_dict.end()) ? it->second : -1; -} - -void Motor::setTempParam(int RID, int value) { temp_param_dict[RID] = value; } - -double LIMIT_MIN_MAX(double x, double min, double max) { - return std::max(min, std::min(x, max)); -} - -uint16_t double_to_uint(double x, double x_min, double x_max, int bits) { - x = LIMIT_MIN_MAX(x, x_min, x_max); - double span = x_max - x_min; - double data_norm = (x - x_min) / span; - return static_cast(data_norm * ((1 << bits) - 1)); -} - -double uint_to_double(uint16_t x, double min, double max, int bits) { - double span = max - min; - double data_norm = static_cast(x) / ((1 << bits) - 1); - return data_norm * span + min; -} - -std::array double_to_uint8s(double value) { - std::array bytes; - std::memcpy(bytes.data(), &value, sizeof(double)); - return bytes; -} - -std::array float_to_uint8s(float value) { - std::array bytes{}; - std::memcpy(bytes.data(), &value, sizeof(float)); - return bytes; -} - -float uint8s_to_float(const std::array& bytes) { - float value; - std::memcpy(&value, bytes.data(), sizeof(float)); - return value; -} - -std::array data_to_uint8s(uint32_t value) { - std::array bytes; - std::memcpy(bytes.data(), &value, sizeof(uint32_t)); - return bytes; -} - -uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, - uint8_t byte4) { - uint32_t value; - uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; - std::memcpy(&value, bytes, sizeof(uint32_t)); - return value; -} - -double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, - uint8_t byte4) { - double value; - uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; - std::memcpy(&value, bytes, sizeof(double)); - return value; -} - -bool is_in_ranges(int number) { - return (7 <= number && number <= 10) || (13 <= number && number <= 16) || - (35 <= number && number <= 36); -} - -void print_hex(const std::vector& data) { - for (auto byte : data) { - std::cout << std::hex << std::uppercase << (int)byte << " "; - } - std::cout << std::dec << std::endl; -} - -template -T get_enum_by_index(int index) { - if (index >= 0 && index < static_cast(T::COUNT)) { - return static_cast(index); - } - return static_cast(-1); -} - -#endif // OPENARM_MOTOR_H_ diff --git a/openarm_hardware/src/motor_control.cpp b/openarm_hardware/src/motor_control.cpp deleted file mode 100644 index c5adfde..0000000 --- a/openarm_hardware/src/motor_control.cpp +++ /dev/null @@ -1,472 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "openarm_hardware/motor_control.hpp" - -#include "openarm_hardware/motor.hpp" - -MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {} - -bool MotorControl::addMotor(Motor& motor) { - motors_map_[motor.SlaveID] = &motor; - if (motor.MasterID != 0) { - motors_map_[motor.MasterID] = &motor; - } - return true; -} - -void MotorControl::enable(Motor& motor) { - controlCmd(motor, 0xFC); - sleep(0.3); -} - -void MotorControl::disable(Motor& motor) { - controlCmd(motor, 0xFD); - sleep(0.3); -} - -void MotorControl::set_zero_position(Motor& motor) { - controlCmd(motor, 0xFE); - sleep(0.3); - recv(); -} - -void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, - double dq, double tau) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); - uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); - - int motor_index = static_cast(motor.MotorType); - double Q_MAX = Limit_Param[motor_index][0]; - double DQ_MAX = Limit_Param[motor_index][1]; - double TAU_MAX = Limit_Param[motor_index][2]; - - uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); - uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); - uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); - - std::array data = { - static_cast((q_uint >> 8) & 0xFF), - static_cast(q_uint & 0xFF), - static_cast(dq_uint >> 4), - static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), - static_cast(kp_uint & 0xFF), - static_cast(kd_uint >> 4), - static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), - static_cast(tau_uint & 0xFF)}; - - sendData(motor.SlaveID, data); - recv(); -} - -void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, - double dq, double tau) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); - uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); - - int motor_index = static_cast(motor.MotorType); - double Q_MAX = Limit_Param[motor_index][0]; - double DQ_MAX = Limit_Param[motor_index][1]; - double TAU_MAX = Limit_Param[motor_index][2]; - - uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); - uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); - uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); - - std::array data = { - static_cast((q_uint >> 8) & 0xFF), - static_cast(q_uint & 0xFF), - static_cast(dq_uint >> 4), - static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), - static_cast(kp_uint & 0xFF), - static_cast(kd_uint >> 4), - static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), - static_cast(tau_uint & 0xFF)}; - - sendData(motor.SlaveID, data); -} - -void MotorControl::sendData(uint16_t motor_id, - const std::array& data) { - canbus_.send(motor_id, data); -} - -void MotorControl::recv() { - uint16_t id; - uint8_t len; - std::array data = canbus_.recv(id, len); - - if (canbus_.whichCAN() == CAN_MODE_CLASSIC) { - can_frame frame; - std::memset(&frame, 0, sizeof(frame)); - frame.can_id = id; - frame.can_dlc = len; - std::memcpy(frame.data, data.data(), len); - - processPacket(frame); - } else if (canbus_.whichCAN() == CAN_MODE_FD) { - canfd_frame fd_frame; - std::memset(&fd_frame, 0, sizeof(fd_frame)); - fd_frame.can_id = id; - fd_frame.len = len; - std::memcpy(fd_frame.data, data.data(), len); - - processPacketFD(fd_frame); - } -} - -void MotorControl::control_delay(Motor& motor, double kp, double kd, double q, - double dq, double tau, double delay) { - controlMIT(motor, kp, kd, q, dq, tau); - std::this_thread::sleep_for( - std::chrono::milliseconds(static_cast(delay))); -} - -void MotorControl::controlPosVel(Motor& motor, double pos, double vel) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlPosVel ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x100 + motor.SlaveID; - std::array data_buf = {0}; - - auto vel_buf = float_to_uint8s(static_cast(vel)); - auto pos_buf = float_to_uint8s(static_cast(pos)); - - for (int i = 0; i < 4; ++i) { - data_buf[i] = pos_buf[i]; - data_buf[i + 4] = vel_buf[i]; - } - - sendData(motor_id, data_buf); - - recv(); -} - -void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x100 + motor.SlaveID; - std::array data_buf = {0}; - - auto pos_buf = float_to_uint8s(static_cast(pos)); - auto vel_buf = float_to_uint8s(static_cast(vel)); - - for (int i = 0; i < 4; ++i) { - data_buf[i] = pos_buf[i]; - data_buf[i + 4] = vel_buf[i]; - } - - sendData(motor_id, data_buf); -} - -void MotorControl::controlVel(Motor& motor, double vel) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlVel ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x200 + motor.SlaveID; - std::array data_buf = {0}; - - auto vel_buf = float_to_uint8s(static_cast(vel)); - for (int i = 0; i < 4; ++i) { - data_buf[i] = vel_buf[i]; - } - - sendData(motor_id, data_buf); - recv(); -} - -void MotorControl::controlVel2(Motor& motor, double vel) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlVel2 ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x200 + motor.SlaveID; - std::array data_buf = {0}; - - auto vel_buf = float_to_uint8s(static_cast(vel)); - for (int i = 0; i < 4; ++i) { - data_buf[i] = vel_buf[i]; - } - - sendData(motor_id, data_buf); -} - -void MotorControl::controlPosForce(Motor& motor, double pos, double vel, - double tau) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x300 + motor.SlaveID; - std::array data_buf = {0}; - - auto pos_buf = float_to_uint8s(static_cast(pos)); - auto vel_buf = float_to_uint8s(static_cast(vel)); - auto tau_buf = float_to_uint8s(static_cast(tau)); - - for (int i = 0; i < 4; ++i) { - data_buf[i] = pos_buf[i]; - } - - data_buf[4] = vel_buf[0]; - data_buf[5] = vel_buf[1]; - - data_buf[6] = tau_buf[0]; - data_buf[7] = tau_buf[1]; - - sendData(motor_id, data_buf); - recv(); -} - -void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, - double tau) { - if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { - std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; - return; - } - - uint16_t motor_id = 0x300 + motor.SlaveID; - std::array data_buf = {0}; - - auto pos_buf = float_to_uint8s(static_cast(pos)); - auto vel_buf = float_to_uint8s(static_cast(vel)); - auto tau_buf = float_to_uint8s(static_cast(tau)); - - for (int i = 0; i < 4; ++i) { - data_buf[i] = pos_buf[i]; - } - - data_buf[4] = vel_buf[0]; - data_buf[5] = vel_buf[1]; - - data_buf[6] = tau_buf[0]; - data_buf[7] = tau_buf[1]; - - sendData(motor_id, data_buf); -} - -bool MotorControl::switchControlMode(Motor& motor, Control_Type control_mode) { - const int max_retries = 20; - const double retry_interval = 0.1; - DM_variable RID = DM_variable::CTRL_MODE; - - writeMotorParam(motor, RID, static_cast(control_mode)); - - for (int i = 0; i < max_retries; ++i) { - usleep(static_cast(retry_interval * 1e6)); - recv_set_param_data(); - if (motor.getParam(static_cast(RID)) == - static_cast(control_mode)) { - return true; - } - } - return false; -} -void MotorControl::save_motor_param(Motor& motor) { - std::array data = { - static_cast(motor.SlaveID & 0xFF), - static_cast((motor.SlaveID >> 8) & 0xFF), - 0xAA, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00}; - disable(motor); - canbus_.send(0x7FF, data); - usleep(1000); -} -// void MotorControl::change_limit_param(DM_Motor_Type motor_type, double PMAX, -// double VMAX, double TMAX) { int index = static_cast(motor_type); -// Limit_Param[index][0] = PMAX; -// Limit_Param[index][1] = VMAX; -// Limit_Param[index][2] = TMAX; -// } - -void MotorControl::recv_set_param_data() { - uint16_t id; - uint8_t len; - std::array data = canbus_.recv(id, len); - - uint8_t cmd = 0x11; - - if (len >= 8) { - std::cout << "CANID: 0x" << std::hex << id << ", CMD: 0x" - << static_cast(cmd) << std::dec << std::endl; - for (int i = 0; i < 8; ++i) { - std::cout << "0x" << std::hex << static_cast(data[i]) << " "; - } - std::cout << std::dec << std::endl; - } -} - -void MotorControl::processPacket(const can_frame& frame) { - uint16_t motorID = frame.data[0]; - uint8_t cmd = 0x11; // someday fix - - if (cmd == 0x11) { - if (motorID != 0x00) { - auto it = motors_map_.find(motorID); - if (it != motors_map_.end() && it->second) { - Motor* motor = it->second; - - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; - - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } - } else { - uint16_t MasterID = frame.data[0] & 0x0F; - auto it = motors_map_.find(MasterID); - if (it != motors_map_.end() && it->second) { - Motor* motor = it->second; - - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; - - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } - } - } -} - -void MotorControl::processPacketFD(const canfd_frame& frame) { - uint16_t motorID = frame.data[0]; - uint8_t cmd = 0x11; // someday fix - - if (cmd == 0x11) { - if (motorID != 0x00) { - auto it = motors_map_.find(motorID); - if (it != motors_map_.end() && it->second) { - Motor* motor = it->second; - - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; - - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } - } else { - uint16_t MasterID = frame.data[0] & 0x0F; - auto it = motors_map_.find(MasterID); - if (it != motors_map_.end() && it->second) { - Motor* motor = it->second; - - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; - - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } - } - } -} - -void MotorControl::controlCmd(Motor& motor, uint8_t cmd) { - std::array data_buf = {0xFF, 0xFF, 0xFF, 0xFF, - 0xFF, 0xFF, 0xFF, cmd}; - sendData(motor.SlaveID, data_buf); -} - -void MotorControl::readRIDParam(Motor& motor, DM_variable RID) { - std::array data = { - static_cast(motor.SlaveID & 0xFF), - static_cast((motor.SlaveID >> 8) & 0xFF), - 0x33, - static_cast(RID), - 0x00, - 0x00, - 0x00, - 0x00}; - canbus_.send(0x7FF, data); -} - -void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, - double value) { - std::array data = { - static_cast(motor.SlaveID & 0xFF), - static_cast((motor.SlaveID >> 8) & 0xFF), 0x55, - static_cast(RID)}; - - if (is_in_ranges(static_cast(RID))) { - auto intData = data_to_uint8s(static_cast(value)); - std::copy(intData.begin(), intData.end(), data.begin() + 4); - } else { - auto doubleData = double_to_uint8s(value); - std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4); - } - - canbus_.send(0x7FF, data); -} diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp deleted file mode 100644 index 00d9ed5..0000000 --- a/openarm_hardware/src/openarm_hardware.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// Copyright 2025 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. - -#include "openarm_hardware/openarm_hardware.hpp" - -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace openarm_hardware { - -static const std::string& can_device_name = "can0"; - -OpenArmHW::OpenArmHW() = default; - -hardware_interface::CallbackReturn OpenArmHW::on_init( - const hardware_interface::HardwareInfo& info) { - if (hardware_interface::SystemInterface::on_init(info) != - CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } - - // read hardware parameters - if (info.hardware_parameters.find("can_device") == - info.hardware_parameters.end()) { - RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), - "No can_device parameter found"); - return CallbackReturn::ERROR; - } - - auto it = info.hardware_parameters.find("prefix"); - if (it == info.hardware_parameters.end()) { - prefix_ = ""; - } else { - prefix_ = it->second; - } - it = info.hardware_parameters.find("disable_torque"); - if (it == info.hardware_parameters.end()) { - disable_torque_ = false; - } else { - disable_torque_ = it->second == "true"; - } - - // temp CANFD - canbus_ = std::make_unique(info.hardware_parameters.at("can_device"), - CAN_MODE_FD); - motor_control_ = std::make_unique(*canbus_); - - if (USING_GRIPPER) { - motor_types.emplace_back(DM_Motor_Type::DM4310); - can_device_ids.emplace_back(0x08); - can_master_ids.emplace_back(0x18); - ++curr_dof; - } - - motors_.resize(curr_dof); - for (size_t i = 0; i < curr_dof; ++i) { - motors_[i] = std::make_unique(motor_types[i], can_device_ids[i], - can_master_ids[i]); - motor_control_->addMotor(*motors_[i]); - } - - pos_states_.resize(curr_dof, 0.0); - pos_commands_.resize(curr_dof, 0.0); - vel_states_.resize(curr_dof, 0.0); - vel_commands_.resize(curr_dof, 0.0); - tau_states_.resize(curr_dof, 0.0); - tau_ff_commands_.resize(curr_dof, 0.0); - refresh_motors(); - read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn OpenArmHW::on_configure( - const rclcpp_lifecycle::State& /*previous_state*/) { - read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - // zero position or calibrate to pose - // for (std::size_t i = 0; i < curr_dof; ++i) - // { - // motor_control_->set_zero_position(*motors_[i]); - // } - - return CallbackReturn::SUCCESS; -} - -std::vector -OpenArmHW::export_state_interfaces() { - std::vector state_interfaces; - for (size_t i = 0; i < curr_dof; ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &pos_states_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &vel_states_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &tau_states_[i])); - RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), - "Exporting state interface for joint %s", - info_.joints[i].name.c_str()); - } - - return state_interfaces; -} - -std::vector -OpenArmHW::export_command_interfaces() { - std::vector command_interfaces; - for (size_t i = 0; i < curr_dof; ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &pos_commands_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &vel_commands_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &tau_ff_commands_[i])); - } - - return command_interfaces; -} - -void OpenArmHW::refresh_motors() { - for (const auto& motor : motors_) { - motor_control_->controlMIT(*motor, 0.0, 0.0, 0.0, 0.0, 0.0); - } -} - -hardware_interface::CallbackReturn OpenArmHW::on_activate( - const rclcpp_lifecycle::State& /*previous_state*/) { - read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - bool zeroed = false; - for (const auto& motor : motors_) { - motor_control_->enable(*motor); - } - - while (!zeroed) { - bool all_zero = true; - for (std::size_t m = 0; m < curr_dof; ++m) { - const double diff = pos_commands_[m] - pos_states_[m]; - if (std::abs(diff) > START_POS_TOLERANCE_RAD) { - all_zero = false; - } - - const double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff)); - double command = pos_states_[m]; - if (pos_states_[m] < pos_commands_[m]) { - command += max_step; - } else { - command -= max_step; - } - motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0); - } - if (all_zero) { - zeroed = true; - } else { - sleep(0.01); - read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - } - } - - read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn OpenArmHW::on_deactivate( - const rclcpp_lifecycle::State& /*previous_state*/) { - refresh_motors(); - for (const auto& motor : motors_) { - motor_control_->disable(*motor); - } - - return CallbackReturn::SUCCESS; -} - -hardware_interface::return_type OpenArmHW::read( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - for (size_t i = 0; i < ARM_DOF; ++i) { - pos_states_[i] = motors_[i]->getPosition(); - vel_states_[i] = motors_[i]->getVelocity(); - tau_states_[i] = motors_[i]->getTorque(); - } - if (USING_GRIPPER) { - pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * - GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER; - vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * - GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER; - tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * - GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER; - } - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type OpenArmHW::write( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (disable_torque_) { - // refresh motor state on write - for (size_t i = 0; i < curr_dof; ++i) { - motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0); - return hardware_interface::return_type::OK; - } - } - - for (size_t i = 0; i < ARM_DOF; ++i) { - if (std::abs(pos_commands_[i] - pos_states_[i]) > POS_JUMP_TOLERANCE_RAD) { - RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), - "Position jump detected for joint %s: %f -> %f", - info_.joints[i].name.c_str(), pos_states_[i], - pos_commands_[i]); - return hardware_interface::return_type::ERROR; - } - motor_control_->controlMIT(*motors_[i], KP.at(i), KD.at(i), - pos_commands_[i], vel_commands_[i], - tau_ff_commands_[i]); - } - if (USING_GRIPPER) { - motor_control_->controlMIT( - *motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), - -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER, - vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER, - tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * - GRIPPER_GEAR_DIRECTION_MULTIPLIER); - } - return hardware_interface::return_type::OK; -} - -} // namespace openarm_hardware - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmHW, - hardware_interface::SystemInterface) diff --git a/openarm_hardware/src/v10_simple_hardware.cpp b/openarm_hardware/src/v10_simple_hardware.cpp new file mode 100644 index 0000000..6d31ab1 --- /dev/null +++ b/openarm_hardware/src/v10_simple_hardware.cpp @@ -0,0 +1,322 @@ +// Copyright 2025 Reazon Holdings, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "openarm_hardware/v10_simple_hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace openarm_hardware { + +OpenArm_v10HW::OpenArm_v10HW() = default; + +bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { + // Parse CAN interface (default: can0) + auto it = info.hardware_parameters.find("can_interface"); + can_interface_ = (it != info.hardware_parameters.end()) ? it->second : "can0"; + + // Parse arm prefix (default: empty for single arm, "left_" or "right_" for + // bimanual) + it = info.hardware_parameters.find("arm_prefix"); + arm_prefix_ = (it != info.hardware_parameters.end()) ? it->second : ""; + + // Parse gripper enable (default: true for V10) + it = info.hardware_parameters.find("hand"); + if (it == info.hardware_parameters.end()) { + hand_ = true; // Default to true for V10 + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + hand_ = (value == "true"); + } + + // Parse CAN-FD enable (default: true for V10) + it = info.hardware_parameters.find("can_fd"); + if (it == info.hardware_parameters.end()) { + can_fd_ = true; // Default to true for V10 + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + can_fd_ = (value == "true"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s", + can_interface_.c_str(), arm_prefix_.c_str(), + hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); + + return true; +} + +void OpenArm_v10HW::generate_joint_names() { + joint_names_.clear(); + // TODO: read from urdf properly in the future + // Generate arm joint names: openarm_{arm_prefix}joint{N} + for (size_t i = 1; i <= ARM_DOF; ++i) { + std::string joint_name = + "openarm_" + arm_prefix_ + "joint" + std::to_string(i); + joint_names_.push_back(joint_name); + } + + // Generate gripper joint name if enabled + if (hand_) { + std::string gripper_joint_name = "openarm_" + arm_prefix_ + "finger_joint1"; + joint_names_.push_back(gripper_joint_name); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Added gripper joint: %s", + gripper_joint_name.c_str()); + } else { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Gripper joint NOT added because hand_=false"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Generated %zu joint names for arm prefix '%s'", + joint_names_.size(), arm_prefix_.c_str()); +} + +hardware_interface::CallbackReturn OpenArm_v10HW::on_init( + const hardware_interface::HardwareInfo& info) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + // Parse configuration + if (!parse_config(info)) { + return CallbackReturn::ERROR; + } + + // Generate joint names based on arm prefix + generate_joint_names(); + + // Validate joint count (7 arm joints + optional gripper) + size_t expected_joints = ARM_DOF + (hand_ ? 1 : 0); + if (joint_names_.size() != expected_joints) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "Generated %zu joint names, expected %zu", joint_names_.size(), + expected_joints); + return CallbackReturn::ERROR; + } + + // Initialize OpenArm with configurable CAN-FD setting + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Initializing OpenArm on %s with CAN-FD %s...", + can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); + openarm_ = + std::make_unique(can_interface_, can_fd_); + + // Initialize arm motors with V10 defaults + openarm_->init_arm_motors(DEFAULT_MOTOR_TYPES, DEFAULT_SEND_CAN_IDS, + DEFAULT_RECV_CAN_IDS); + + // Initialize gripper if enabled + if (hand_) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Initializing gripper..."); + openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, + DEFAULT_GRIPPER_SEND_CAN_ID, + DEFAULT_GRIPPER_RECV_CAN_ID); + } + + // Initialize state and command vectors based on generated joint count + const size_t total_joints = joint_names_.size(); + pos_commands_.resize(total_joints, 0.0); + vel_commands_.resize(total_joints, 0.0); + tau_commands_.resize(total_joints, 0.0); + pos_states_.resize(total_joints, 0.0); + vel_states_.resize(total_joints, 0.0); + tau_states_.resize(total_joints, 0.0); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "OpenArm V10 Simple HW initialized successfully"); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArm_v10HW::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + // Set callback mode to ignore during configuration + openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE); + openarm_->refresh_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + return CallbackReturn::SUCCESS; +} + +std::vector +OpenArm_v10HW::export_state_interfaces() { + std::vector state_interfaces; + + for (size_t i = 0; i < joint_names_.size(); ++i) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &pos_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &vel_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_states_[i])); + } + + return state_interfaces; +} + +std::vector +OpenArm_v10HW::export_command_interfaces() { + std::vector command_interfaces; + + for (size_t i = 0; i < joint_names_.size(); ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + &pos_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + &vel_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn OpenArm_v10HW::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Activating OpenArm V10..."); + + // Set callback mode to state and enable all motors (like full_arm.cpp) + openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + openarm_->enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + // Return to zero position smoothly + return_to_zero(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 activated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArm_v10HW::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Deactivating OpenArm V10..."); + + // Disable all motors (like full_arm.cpp exit) + openarm_->disable_all(); + openarm_->recv_all(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 deactivated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type OpenArm_v10HW::read( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Receive all motor states + openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + openarm_->recv_all(); + std::this_thread::sleep_for(std::chrono::microseconds(300)); + openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE); + + // Read arm joint states + const auto& arm_motors = openarm_->get_arm().get_motors(); + for (size_t i = 0; i < ARM_DOF && i < arm_motors.size(); ++i) { + pos_states_[i] = arm_motors[i].get_position(); + vel_states_[i] = arm_motors[i].get_velocity(); + tau_states_[i] = arm_motors[i].get_torque(); + } + + // Read gripper state if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + const auto& gripper_motors = openarm_->get_gripper().get_motors(); + if (!gripper_motors.empty()) { + // TODO the mappings are unimplemented, need to actually implement the + // mappings for pos, vel, tau Convert motor position (radians) to joint + // value (0-1) + double motor_pos = gripper_motors[0].get_position(); + pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); + + // Velocity and torque can be passed through directly for now + vel_states_[ARM_DOF] = 0; // gripper_motors[0].get_velocity(); + tau_states_[ARM_DOF] = 0; // gripper_motors[0].get_torque(); + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type OpenArm_v10HW::write( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Control arm motors with MIT control + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + arm_params.push_back({DEFAULT_KP[i], DEFAULT_KD[i], pos_commands_[i], + vel_commands_[i], tau_commands_[i]}); + } + openarm_->get_arm().mit_control_all(arm_params); + // Control gripper if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + // Convert joint value (0-1) to motor position (radians) + double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); + + openarm_->get_gripper().mit_control_all({{5.0, 1.0, motor_command, 0, 0}}); + } + std::this_thread::sleep_for(std::chrono::microseconds(300)); + openarm_->recv_all(); + return hardware_interface::return_type::OK; +} + +void OpenArm_v10HW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "Returning to zero position..."); + + // Return arm to zero with MIT control + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + arm_params.push_back({DEFAULT_KP[i], DEFAULT_KD[i], 0.0, 0.0, 0.0}); + } + openarm_->get_arm().mit_control_all(arm_params); + + // Return gripper to zero if enabled + if (hand_) { + openarm_->get_gripper().mit_control_all({{5.0, 1.0, 0.0, 0.0, 0.0}}); + } + + // // Wait for motors to settle + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); +} + +// Gripper mapping helper functions +double OpenArm_v10HW::joint_to_motor_radians(double joint_value) { + // Joint 0=closed -> motor 0 rad, Joint 1=open -> motor -1.0472 rad + return joint_value * (-1.0472); // Scale from 0-1 to 0 to -1.0472 +} + +double OpenArm_v10HW::motor_radians_to_joint(double motor_radians) { + // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 1 + return motor_radians / (-1.0472); // Scale from 0 to -1.0472 to 0-1 +} + +} // namespace openarm_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArm_v10HW, + hardware_interface::SystemInterface) \ No newline at end of file diff --git a/openarm_hardware/test/test_openarm_hardware.cpp b/openarm_hardware/test/test_openarm_hardware.cpp deleted file mode 100644 index c2cc4f7..0000000 --- a/openarm_hardware/test/test_openarm_hardware.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2025 Reazon Holdings, Inc. -// Copyright 2025 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. - -#include - -#include - -#include "hardware_interface/resource_manager.hpp" -#include "ros2_control_test_assets/components_urdfs.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -class TestOpenArmHW : public ::testing::Test { - protected: - void SetUp() override { - openarm_hardware_7dof_ = - R"( - - - - mock_components/GenericSystem - openarm_hardware/OpenArmHW - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - - - 0 - - - - - - - - 0 - - - - - - 0 - - - - )"; - } - - std::string openarm_hardware_7dof_; -}; - -TEST_F(TestOpenArmHW, load_openarm_hardware_7dof) { - auto urdf = ros2_control_test_assets::urdf_head + openarm_hardware_7dof_ + - ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); -}