diff --git a/openarm_hardware/include/openarm_hardware/canbus.hpp b/openarm_hardware/include/openarm_hardware/canbus.hpp index 78f8847..023bac5 100644 --- a/openarm_hardware/include/openarm_hardware/canbus.hpp +++ b/openarm_hardware/include/openarm_hardware/canbus.hpp @@ -1,38 +1,34 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include #include #include +#include +#include +#include + #include -#include +#include +#include +#include #include -enum CANMode { - CAN_MODE_CLASSIC = 0, - CAN_MODE_FD = 1 -}; +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); + 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); + 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(); + struct can_frame recvClassic(); + struct canfd_frame recvFD(); - int sock_; - int mode_; + int sock_; + int mode_; }; - - diff --git a/openarm_hardware/include/openarm_hardware/motor.hpp b/openarm_hardware/include/openarm_hardware/motor.hpp index 7bdc6e6..5fd2968 100644 --- a/openarm_hardware/include/openarm_hardware/motor.hpp +++ b/openarm_hardware/include/openarm_hardware/motor.hpp @@ -1,121 +1,117 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include #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 + 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 + 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 -}; +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); + 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; + 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; + 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; + 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); @@ -130,12 +126,13 @@ 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); +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); +double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, + uint8_t byte4); bool is_in_ranges(int number); @@ -143,4 +140,3 @@ 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 index ab3eac9..62495c2 100644 --- a/openarm_hardware/include/openarm_hardware/motor_control.hpp +++ b/openarm_hardware/include/openarm_hardware/motor_control.hpp @@ -1,70 +1,73 @@ #pragma once -#include -#include -#include -#include -#include #include -#include "motor.hpp" -#include "canbus.hpp" + +#include #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(); + 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); + 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); + 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(); + void recv_set_param_data(); - private: - CANBus& canbus_; + 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); + 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 index 9ecbd5f..6662a69 100644 --- a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp @@ -1,5 +1,5 @@ // Copyright (c) 2025, Reazon Holdings, Inc. -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 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. @@ -15,35 +15,37 @@ #pragma once +#include #include #include -#include -#include "openarm_hardware/visibility_control.h" -#include "hardware_interface/system_interface.hpp" +#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" -#include "canbus.hpp" -#include "motor.hpp" -#include "motor_control.hpp" +namespace openarm_hardware { - -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 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 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; @@ -52,43 +54,44 @@ 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: +class OpenArmHW : public hardware_interface::SystemInterface { + public: OpenArmHW(); - + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; + const hardware_interface::HardwareInfo& info) override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_state_interfaces() override; + std::vector export_state_interfaces() + override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_command_interfaces() override; + std::vector export_command_interfaces() + override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + const rclcpp_lifecycle::State& previous_state) override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + 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; + 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; + const rclcpp::Time& time, const rclcpp::Duration& period) override; - std::size_t curr_dof = ARM_DOF; // minus gripper -private: + std::size_t curr_dof = ARM_DOF; // minus gripper + private: std::string prefix_; std::unique_ptr canbus_; std::unique_ptr motor_control_; @@ -105,4 +108,3 @@ private: }; } // namespace openarm_hardware - diff --git a/openarm_hardware/include/openarm_hardware/visibility_control.h b/openarm_hardware/include/openarm_hardware/visibility_control.h index 5311acb..a2ed64d 100644 --- a/openarm_hardware/include/openarm_hardware/visibility_control.h +++ b/openarm_hardware/include/openarm_hardware/visibility_control.h @@ -1,5 +1,5 @@ // Copyright (c) 2025, Reazon Holdings, Inc. -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 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. @@ -26,22 +26,27 @@ #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) #endif #ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ + TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ + TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE \ + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT \ + __attribute__((visibility("default"))) #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT #if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ + __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL \ + __attribute__((visibility("hidden"))) #else #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL #endif #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE #endif - diff --git a/openarm_hardware/src/canbus.cpp b/openarm_hardware/src/canbus.cpp index f0f7e30..615c07b 100644 --- a/openarm_hardware/src/canbus.cpp +++ b/openarm_hardware/src/canbus.cpp @@ -1,121 +1,117 @@ #include "openarm_hardware/canbus.hpp" -CANBus::CANBus(const std::string& interface, int mode) - : mode_(mode) { - struct ifreq ifr; - struct sockaddr_can addr; +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); + 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); } + } - 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); - } + if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + perror("Error in CAN socket bind"); + exit(EXIT_FAILURE); + } } -CANBus::~CANBus() { - close(sock_); -} +CANBus::~CANBus() { close(sock_); } -int CANBus::whichCAN(){ - return mode_; -} +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); + 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; + 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)); +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); + 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; + 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)); + 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); + 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; + 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)); + 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; + 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)); + 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; + 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 index e9b86e2..85d7cc4 100644 --- a/openarm_hardware/src/motor.cpp +++ b/openarm_hardware/src/motor.cpp @@ -4,148 +4,135 @@ #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) {} + : 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; - } +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; } -double Motor::getGoalPosition() const { - return goal_position; -} +void Motor::setGoalPosition(double pos) { goal_position = pos; } -void Motor::setGoalPosition(double pos) { - goal_position = pos; -} +double Motor::getGoalVelocity() const { return goal_velocity; } -double Motor::getGoalVelocity() const { - return goal_velocity; -} +void Motor::setGoalVelocity(double velocity) { goal_velocity = velocity; } -void Motor::setGoalVelocity(double velocity) { - goal_velocity = velocity; -} +double Motor::getGoalTau() const { return goal_tau; } -double Motor::getGoalTau() const { - return goal_tau; -} +void Motor::setGoalTau(double tau) { goal_tau = tau; } -void Motor::setGoalTau(double tau) { - goal_tau = tau; -} +int Motor::getStateTmos() const { return state_tmos; } -int Motor::getStateTmos() const { - return state_tmos; -} +void Motor::setStateTmos(int tmos) { state_tmos = tmos; } -void Motor::setStateTmos(int tmos) { - state_tmos = tmos; -} +int Motor::getStateTrotor() const { return state_trotor; } -int Motor::getStateTrotor() const { - return state_trotor; -} - -void Motor::setStateTrotor(int trotor) { - state_trotor = 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; + 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; -} +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)); + 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)); + 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; + 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 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; + 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; + 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; + 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; +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; +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); + 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; + 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); - + if (index >= 0 && index < static_cast(T::COUNT)) { + return static_cast(index); + } + return static_cast(-1); } - -#endif // OPENARM_MOTOR_H_ +#endif // OPENARM_MOTOR_H_ diff --git a/openarm_hardware/src/motor_control.cpp b/openarm_hardware/src/motor_control.cpp index 5b9c885..61f41a5 100644 --- a/openarm_hardware/src/motor_control.cpp +++ b/openarm_hardware/src/motor_control.cpp @@ -1,455 +1,458 @@ #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; + 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); + controlCmd(motor, 0xFC); + sleep(0.3); } void MotorControl::disable(Motor& motor) { - controlCmd(motor, 0xFD); - sleep(0.3); + controlCmd(motor, 0xFD); + sleep(0.3); } -void MotorControl::set_zero_position(Motor& motor){ - controlCmd(motor, 0xFE); - sleep(0.3); - recv(); +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; - } +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); + 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]; + 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); + 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) - }; + 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(); + sendData(motor.SlaveID, data); + recv(); } -void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau) { +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; + } - 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); - 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]; - 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); - 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)}; - 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); + sendData(motor.SlaveID, data); } -void MotorControl::sendData(uint16_t motor_id, const std::array& data) { - canbus_.send(motor_id, 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); + 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); + 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); + 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); - } + 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::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; - } + 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}; + 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)); + 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]; - } + 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); + sendData(motor_id, data_buf); - - recv(); + 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; - } + 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}; + 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)); + 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]; - } + 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); + 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; - } + 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}; + 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]; - } + 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(); + 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; - } + 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}; + 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); + 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; - } +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}; + 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)); + 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]; - } + 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[4] = vel_buf[0]; + data_buf[5] = vel_buf[1]; - data_buf[6] = tau_buf[0]; - data_buf[7] = tau_buf[1]; + data_buf[6] = tau_buf[0]; + data_buf[7] = tau_buf[1]; - sendData(motor_id, data_buf); - recv(); + 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; - } +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}; + 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)); + 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]; - } + 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[4] = vel_buf[0]; + data_buf[5] = vel_buf[1]; - data_buf[6] = tau_buf[0]; - data_buf[7] = tau_buf[1]; + data_buf[6] = tau_buf[0]; + data_buf[7] = tau_buf[1]; - sendData(motor_id, data_buf); + 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; + 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)); + 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; + 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); + 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::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); + uint16_t id; + uint8_t len; + std::array data = canbus_.recv(id, len); - uint8_t cmd = 0x11; + 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; - } + 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 + 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; + 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]; + 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 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); + 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; + 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]; + 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 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); + 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); - } - } - } + 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 + 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; + 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]; + 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 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); + 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; + 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]; + 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 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); + 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); - } - } - } + 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); + 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); + 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) - }; +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); - } + 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); + canbus_.send(0x7FF, data); } - diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp index c841065..0a632b5 100644 --- a/openarm_hardware/src/openarm_hardware.cpp +++ b/openarm_hardware/src/openarm_hardware.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2025, Reazon Holdings, Inc. -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 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. @@ -13,55 +13,55 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "openarm_hardware/openarm_hardware.hpp" + #include #include -#include "openarm_hardware/openarm_hardware.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" -namespace openarm_hardware -{ +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) - { + 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"); + + // 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()){ + if (it == info.hardware_parameters.end()) { prefix_ = ""; - } - else{ + } else { prefix_ = it->second; } it = info.hardware_parameters.find("disable_torque"); - if (it == info.hardware_parameters.end()){ + if (it == info.hardware_parameters.end()) { disable_torque_ = false; - } - else{ + } else { disable_torque_ = it->second == "true"; } - //temp CANFD - canbus_ = std::make_unique(info.hardware_parameters.at("can_device"),CAN_MODE_FD); + // temp CANFD + canbus_ = std::make_unique(info.hardware_parameters.at("can_device"), + CAN_MODE_FD); motor_control_ = std::make_unique(*canbus_); - if(USING_GRIPPER){ + if (USING_GRIPPER) { motor_types.emplace_back(DM_Motor_Type::DM4310); can_device_ids.emplace_back(0x08); can_master_ids.emplace_back(0x18); @@ -69,8 +69,9 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( } 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]); + 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]); } @@ -87,9 +88,7 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( } hardware_interface::CallbackReturn OpenArmHW::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - + 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) @@ -97,76 +96,82 @@ hardware_interface::CallbackReturn OpenArmHW::on_configure( // motor_control_->set_zero_position(*motors_[i]); // } - return CallbackReturn::SUCCESS; } -std::vector OpenArmHW::export_state_interfaces() -{ +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()); + 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 +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])); + 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_){ +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*/) -{ + const rclcpp_lifecycle::State& /*previous_state*/) { read(rclcpp::Time(0), rclcpp::Duration(0, 0)); bool zeroed = false; - for(const auto& motor: motors_){ + for (const auto& motor : motors_) { motor_control_->enable(*motor); } - while(!zeroed){ + while (!zeroed) { bool all_zero = true; - for (std::size_t m = 0; m < curr_dof; ++m){ + 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){ + 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]){ + if (pos_states_[m] < pos_commands_[m]) { command += max_step; - } - else{ + } else { command -= max_step; } motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0); - } - if (all_zero){ + if (all_zero) { zeroed = true; - } - else{ + } else { sleep(0.01); read(rclcpp::Time(0), rclcpp::Duration(0, 0)); } @@ -178,10 +183,9 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate( } hardware_interface::CallbackReturn OpenArmHW::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ + const rclcpp_lifecycle::State& /*previous_state*/) { refresh_motors(); - for(const auto& motor: motors_){ + for (const auto& motor : motors_) { motor_control_->disable(*motor); } @@ -189,48 +193,58 @@ hardware_interface::CallbackReturn OpenArmHW::on_deactivate( } hardware_interface::return_type OpenArmHW::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - for(size_t i = 0; i < ARM_DOF; ++i){ + 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; + 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_){ + 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){ + 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]); + 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]); + 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); + 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; } @@ -239,5 +253,5 @@ hardware_interface::return_type OpenArmHW::write( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - openarm_hardware::OpenArmHW, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmHW, + hardware_interface::SystemInterface) diff --git a/openarm_hardware/test/test_openarm_hardware.cpp b/openarm_hardware/test/test_openarm_hardware.cpp index ffdbb7e..88d7706 100644 --- a/openarm_hardware/test/test_openarm_hardware.cpp +++ b/openarm_hardware/test/test_openarm_hardware.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2025, Reazon Holdings, Inc. -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 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. @@ -21,13 +21,11 @@ #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" -class TestOpenArmHW : public ::testing::Test -{ -protected: - void SetUp() override - { +class TestOpenArmHW : public ::testing::Test { + protected: + void SetUp() override { openarm_hardware_7dof_ = - R"( + R"( @@ -123,8 +121,7 @@ protected: std::string openarm_hardware_7dof_; }; -TEST_F(TestOpenArmHW, load_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));