openarm/openarm_ros2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

clang-format: apply clang-format suggestions (#10)

Browse Source 
Executed the following command and accept suggestions.

```console
$ nice pre-commit run --show-diff-on-failure --color=always --all-files
```
This commit is contained in:
takuya kodama 2025-05-19 15:36:06 +08:00 committed by GitHub
parent 30d11b4d28
commit d109f32963
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
10 changed files with 831 additions and 832 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
openarm_hardware/include/openarm_hardware/canbus.hpp
View File

@ -1,38 +1,34 @@
#pragma once #pragma once
#include <cstdint> #include <fcntl.h>
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h> #include <linux/can.h>
#include <linux/can/raw.h> #include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <array> #include <array>
#include <fcntl.h> #include <cstdint>
#include <cstring>
#include <iostream>
#include <string> #include <string>
enum CANMode { enum CANMode { CAN_MODE_CLASSIC = 0, CAN_MODE_FD = 1 };
CAN_MODE_CLASSIC = 0,
CAN_MODE_FD = 1
};
class CANBus { class CANBus {
public: public:
explicit CANBus(const std::string& interface, int mode); explicit CANBus(const std::string& interface, int mode);
~CANBus(); ~CANBus();
int whichCAN(); int whichCAN();
bool send(uint16_t motor_id, const std::array<uint8_t, 8>& data); bool send(uint16_t motor_id, const std::array<uint8_t, 8>& data);
std::array<uint8_t, 64> recv(uint16_t& out_id, uint8_t& out_len); std::array<uint8_t, 64> recv(uint16_t& out_id, uint8_t& out_len);
private: private:
bool sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data); bool sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data);
bool sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data); bool sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data);
struct can_frame recvClassic(); struct can_frame recvClassic();
struct canfd_frame recvFD(); struct canfd_frame recvFD();
int sock_; int sock_;
int mode_; int mode_;
}; };

210
openarm_hardware/include/openarm_hardware/motor.hpp
View File

@ -1,121 +1,117 @@
#pragma once #pragma once
#include <cstdint>
#include <map>
#include <vector>
#include <iostream>
#include <array>
#include <algorithm>
#include <cstring>
#include <stdio.h> #include <stdio.h>
#include <algorithm>
#include <array>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <map>
#include <vector>
enum class DM_Motor_Type : uint8_t { enum class DM_Motor_Type : uint8_t {
DM4310 = 0, DM4310 = 0,
DM4310_48V, DM4310_48V,
DM4340, DM4340,
DM4340_48V, DM4340_48V,
DM6006, DM6006,
DM8006, DM8006,
DM8009, DM8009,
DM10010L, DM10010L,
DM10010, DM10010,
DMH3510, DMH3510,
DMH6215, DMH6215,
DMG6220, DMG6220,
COUNT COUNT
}; };
enum class DM_variable : uint8_t { enum class DM_variable : uint8_t {
UV_Value = 0, UV_Value = 0,
KT_Value, KT_Value,
OT_Value, OT_Value,
OC_Value, OC_Value,
ACC, ACC,
DEC, DEC,
MAX_SPD, MAX_SPD,
MST_ID, MST_ID,
ESC_ID, ESC_ID,
TIMEOUT, TIMEOUT,
CTRL_MODE, CTRL_MODE,
Damp, Damp,
Inertia, Inertia,
hw_ver, hw_ver,
sw_ver, sw_ver,
SN, SN,
NPP, NPP,
Rs, Rs,
LS, LS,
Flux, Flux,
Gr, Gr,
PMAX, PMAX,
VMAX, VMAX,
TMAX, TMAX,
I_BW, I_BW,
KP_ASR, KP_ASR,
KI_ASR, KI_ASR,
KP_APR, KP_APR,
KI_APR, KI_APR,
OV_Value, OV_Value,
GREF, GREF,
Deta, Deta,
V_BW, V_BW,
IQ_c1, IQ_c1,
VL_c1, VL_c1,
can_br, can_br,
sub_ver, sub_ver,
u_off = 50, u_off = 50,
v_off, v_off,
k1, k1,
k2, k2,
m_off, m_off,
dir, dir,
p_m = 80, p_m = 80,
xout, xout,
COUNT COUNT
}; };
enum class Control_Type : uint8_t { enum class Control_Type : uint8_t { MIT = 1, POS_VEL, VEL, Torque_Pos, COUNT };
MIT = 1,
POS_VEL,
VEL,
Torque_Pos,
COUNT
};
class Motor { class Motor {
public: public:
Motor() = default; Motor() = default;
Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID); Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID);
void recv_data(double q, double dq, double tau, int tmos, int trotor); void recv_data(double q, double dq, double tau, int tmos, int trotor);
double getPosition() const; double getPosition() const;
double getVelocity() const; double getVelocity() const;
double getTorque() const; double getTorque() const;
int getParam(int RID) const; int getParam(int RID) const;
void setTempParam(int RID, int val); void setTempParam(int RID, int val);
uint16_t SlaveID; uint16_t SlaveID;
uint16_t MasterID; uint16_t MasterID;
bool isEnable; bool isEnable;
Control_Type NowControlMode; Control_Type NowControlMode;
DM_Motor_Type MotorType; DM_Motor_Type MotorType;
int getStateTmos() const; int getStateTmos() const;
int getStateTrotor() const; int getStateTrotor() const;
double getGoalPosition() const; double getGoalPosition() const;
double getGoalVelocity() const; double getGoalVelocity() const;
double getGoalTau() const; double getGoalTau() const;
void setGoalPosition(double pos); void setGoalPosition(double pos);
void setGoalVelocity(double vel); void setGoalVelocity(double vel);
void setGoalTau(double tau); void setGoalTau(double tau);
void setStateTmos(int tmos); void setStateTmos(int tmos);
void setStateTrotor(int trotor); void setStateTrotor(int trotor);
private:
double Pd, Vd; private:
double goal_position,goal_velocity, goal_tau; double Pd, Vd;
double state_q, state_dq, state_tau; double goal_position, goal_velocity, goal_tau;
int state_tmos, state_trotor; double state_q, state_dq, state_tau;
std::map<int, int> temp_param_dict; int state_tmos, state_trotor;
std::map<int, int> temp_param_dict;
}; };
double LIMIT_MIN_MAX(double x, double min, double max); double LIMIT_MIN_MAX(double x, double min, double max);
@ -130,12 +126,13 @@ std::array<uint8_t, 4> float_to_uint8s(float value);
float uint8s_to_float(const std::array<uint8_t, 4>& bytes); float uint8s_to_float(const std::array<uint8_t, 4>& bytes);
std::array<uint8_t, 8> data_to_uint8s(uint32_t value); std::array<uint8_t, 8> 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); bool is_in_ranges(int number);
@ -143,4 +140,3 @@ void print_hex(const std::vector<uint8_t>& data);
template <typename T> template <typename T>
T get_enum_by_index(int index); T get_enum_by_index(int index);

115
openarm_hardware/include/openarm_hardware/motor_control.hpp
View File

@ -1,70 +1,73 @@
#pragma once #pragma once
#include <iostream>
#include <vector>
#include <map>
#include <array>
#include <cstring>
#include <unistd.h> #include <unistd.h>
#include "motor.hpp"
#include "canbus.hpp" #include <array>
#include <atomic> #include <atomic>
#include <functional>
#include <thread>
#include <chrono> #include <chrono>
#include <cstring>
#include <functional>
#include <iostream>
#include <map>
#include <thread>
#include <vector>
#include "canbus.hpp"
#include "motor.hpp"
class MotorControl { class MotorControl {
public: public:
explicit MotorControl(CANBus& canbus); explicit MotorControl(CANBus& canbus);
bool addMotor(Motor& motor); bool addMotor(Motor& motor);
void enable(Motor& motor); void enable(Motor& motor);
void disable(Motor& motor); void disable(Motor& motor);
void set_zero_position(Motor& motor); void set_zero_position(Motor& motor);
void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau); void controlMIT(Motor& motor, double kp, double kd, double q, double dq,
void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau); double tau);
void sendData(uint16_t motor_id, const std::array<uint8_t,8>& data); void controlMIT2(Motor& motor, double kp, double kd, double q, double dq,
void recv(); double tau);
void sendData(uint16_t motor_id, const std::array<uint8_t, 8>& data);
void recv();
void control_delay(Motor& motor, double kp, double kd, double q, double dq, double tau, double delay); void control_delay(Motor& motor, double kp, double kd, double q, double dq,
void controlPosVel(Motor& motor,double q, double dq); double tau, double delay);
void controlPosVel2(Motor& motor,double q, double dq); void controlPosVel(Motor& motor, double q, double dq);
void controlVel(Motor& motor, double dq); void controlPosVel2(Motor& motor, double q, double dq);
void controlVel2(Motor& motor, double dq); void controlVel(Motor& motor, double dq);
void controlPosForce(Motor& motor, double q, double vel, double tau); void controlVel2(Motor& motor, double dq);
void controlPosForce2(Motor& motor, double q, double vel, double tau); 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); bool switchControlMode(Motor& motor, Control_Type controlMode);
void save_motor_param(Motor &motor); void save_motor_param(Motor& motor);
void change_limit_param(); void change_limit_param();
//void change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, double TMAX); // 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: private:
CANBus& canbus_; CANBus& canbus_;
std::map<uint16_t, Motor*> motors_map_; std::map<uint16_t, Motor*> motors_map_;
static constexpr double Limit_Param[12][3] = { static constexpr double Limit_Param[12][3] = {
{12.5, 30, 10}, // DM4310 {12.5, 30, 10}, // DM4310
{12.5, 50, 10}, // DM4310_48V {12.5, 50, 10}, // DM4310_48V
{12.5, 8, 28}, // DM4340 {12.5, 8, 28}, // DM4340
{12.5, 10, 28}, // DM4340_48V {12.5, 10, 28}, // DM4340_48V
{12.5, 45, 20}, // DM6006 {12.5, 45, 20}, // DM6006
{12.5, 45, 40}, // DM8006 {12.5, 45, 40}, // DM8006
{12.5, 45, 54}, // DM8009 {12.5, 45, 54}, // DM8009
{12.5, 25, 200}, // DM10010L {12.5, 25, 200}, // DM10010L
{12.5, 20, 200}, // DM10010 {12.5, 20, 200}, // DM10010
{12.5, 280, 1}, // DMH3510 {12.5, 280, 1}, // DMH3510
{12.5, 45, 10}, // DMH6215 {12.5, 45, 10}, // DMH6215
{12.5, 45, 10}, // DMG6220 {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);
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);
}; };

60
openarm_hardware/include/openarm_hardware/openarm_hardware.hpp
View File

@ -1,5 +1,5 @@
// Copyright (c) 2025, Reazon Holdings, Inc. // 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"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License. // you may not use this file except in compliance with the License.
@ -15,35 +15,37 @@
#pragma once #pragma once
#include <memory>
#include <string> #include <string>
#include <vector> #include <vector>
#include <memory>
#include "openarm_hardware/visibility_control.h" #include "canbus.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp" #include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.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/macros.hpp"
#include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/state.hpp"
#include "canbus.hpp" namespace openarm_hardware {
#include "motor.hpp"
#include "motor_control.hpp"
std::vector<DM_Motor_Type> motor_types{
namespace openarm_hardware 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<DM_Motor_Type> 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<uint16_t> can_device_ids{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; std::vector<uint16_t> can_device_ids{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
std::vector<uint16_t> can_master_ids{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; std::vector<uint16_t> can_master_ids{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17};
static const Control_Type CONTROL_MODE = Control_Type::MIT; static const Control_Type CONTROL_MODE = Control_Type::MIT;
static const std::size_t ARM_DOF = 7; static const std::size_t ARM_DOF = 7;
static const std::size_t GRIPPER_DOF = 1; static const std::size_t GRIPPER_DOF = 1;
static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF; static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF;
static const std::array<double, TOTAL_DOF> KP = {80.0, 80.0, 20.0, 55.0, 5.0, 5.0, 5.0, 0.5}; static const std::array<double, TOTAL_DOF> KP = {80.0, 80.0, 20.0, 55.0,
static const std::array<double, TOTAL_DOF> KD = {2.75, 2.5, 0.7, 0.4, 0.7, 0.6, 0.5, 0.1}; 5.0, 5.0, 5.0, 0.5};
static const std::array<double, TOTAL_DOF> 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 START_POS_TOLERANCE_RAD = 0.1;
static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 16.0; 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 double GRIPPER_GEAR_DIRECTION_MULTIPLIER = -1.0;
static const int GRIPPER_INDEX = TOTAL_DOF - 1; static const int GRIPPER_INDEX = TOTAL_DOF - 1;
class OpenArmHW : public hardware_interface::SystemInterface class OpenArmHW : public hardware_interface::SystemInterface {
{ public:
public:
OpenArmHW(); OpenArmHW();
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_init( hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo & info) override; const hardware_interface::HardwareInfo& info) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_configure( hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override; std::vector<hardware_interface::StateInterface> export_state_interfaces()
override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override; std::vector<hardware_interface::CommandInterface> export_command_interfaces()
override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_activate( hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_deactivate( hardware_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override; const rclcpp_lifecycle::State& previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::return_type read( hardware_interface::return_type read(const rclcpp::Time& time,
const rclcpp::Time & time, const rclcpp::Duration & period) override; const rclcpp::Duration& period) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::return_type write( 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 std::size_t curr_dof = ARM_DOF; // minus gripper
private: private:
std::string prefix_; std::string prefix_;
std::unique_ptr<CANBus> canbus_; std::unique_ptr<CANBus> canbus_;
std::unique_ptr<MotorControl> motor_control_; std::unique_ptr<MotorControl> motor_control_;
@ -105,4 +108,3 @@ private:
}; };
} // namespace openarm_hardware } // namespace openarm_hardware

21
openarm_hardware/include/openarm_hardware/visibility_control.h
View File

@ -1,5 +1,5 @@
// Copyright (c) 2025, Reazon Holdings, Inc. // 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"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with 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) #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
#endif #endif
#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL #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 #else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \
TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
#endif #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 #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
#else #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 #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
#if __GNUC__ >= 4 #if __GNUC__ >= 4
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) __attribute__((visibility("default")))
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL \
__attribute__((visibility("hidden")))
#else #else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
#endif #endif
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
#endif #endif

176
openarm_hardware/src/canbus.cpp
View File

@ -1,121 +1,117 @@
#include "openarm_hardware/canbus.hpp" #include "openarm_hardware/canbus.hpp"
CANBus::CANBus(const std::string& interface, int mode) CANBus::CANBus(const std::string& interface, int mode) : mode_(mode) {
: mode_(mode) { struct ifreq ifr;
struct ifreq ifr; struct sockaddr_can addr;
struct sockaddr_can addr;
sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sock_ < 0) { if (sock_ < 0) {
perror("Error while opening CAN socket"); perror("Error while opening CAN socket");
exit(EXIT_FAILURE); 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 (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) { perror("Error in CAN socket bind");
perror("Error getting interface index"); exit(EXIT_FAILURE);
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() { CANBus::~CANBus() { close(sock_); }
close(sock_);
}
int CANBus::whichCAN(){ int CANBus::whichCAN() { return mode_; }
return mode_;
}
bool CANBus::send(uint16_t motor_id, const std::array<uint8_t, 8>& data) { bool CANBus::send(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
if (mode_ == CAN_MODE_FD) if (mode_ == CAN_MODE_FD)
return sendFD(motor_id, data); return sendFD(motor_id, data);
else else
return sendClassic(motor_id, data); return sendClassic(motor_id, data);
} }
std::array<uint8_t, 64> CANBus::recv(uint16_t& out_id, uint8_t& out_len) { std::array<uint8_t, 64> CANBus::recv(uint16_t& out_id, uint8_t& out_len) {
std::array<uint8_t, 64> buffer = {}; std::array<uint8_t, 64> buffer = {};
if (mode_ == CAN_MODE_FD) { if (mode_ == CAN_MODE_FD) {
auto frame = recvFD(); auto frame = recvFD();
out_id = frame.can_id; out_id = frame.can_id;
out_len = frame.len; out_len = frame.len;
std::copy(frame.data, frame.data + frame.len, buffer.begin()); std::copy(frame.data, frame.data + frame.len, buffer.begin());
} else { } else {
auto frame = recvClassic(); auto frame = recvClassic();
out_id = frame.can_id; out_id = frame.can_id;
out_len = frame.can_dlc; out_len = frame.can_dlc;
std::copy(frame.data, frame.data + frame.can_dlc, buffer.begin()); std::copy(frame.data, frame.data + frame.can_dlc, buffer.begin());
} }
return buffer; return buffer;
} }
bool CANBus::sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data) { bool CANBus::sendClassic(uint16_t motor_id,
struct can_frame frame; const std::array<uint8_t, 8>& data) {
std::memset(&frame, 0, sizeof(frame)); struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
frame.can_id = motor_id; frame.can_id = motor_id;
frame.can_dlc = data.size(); frame.can_dlc = data.size();
std::copy(data.begin(), data.end(), frame.data); std::copy(data.begin(), data.end(), frame.data);
if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
perror("Error sending CAN frame"); perror("Error sending CAN frame");
return false; return false;
} }
return true; return true;
} }
bool CANBus::sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data) { bool CANBus::sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
struct canfd_frame frame; struct canfd_frame frame;
std::memset(&frame, 0, sizeof(frame)); std::memset(&frame, 0, sizeof(frame));
frame.can_id = motor_id; frame.can_id = motor_id;
frame.len = data.size(); frame.len = data.size();
frame.flags = CANFD_BRS; frame.flags = CANFD_BRS;
std::copy(data.begin(), data.end(), frame.data); std::copy(data.begin(), data.end(), frame.data);
if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
perror("Error sending CAN FD frame"); perror("Error sending CAN FD frame");
return false; return false;
} }
return true; return true;
} }
struct can_frame CANBus::recvClassic() { struct can_frame CANBus::recvClassic() {
struct can_frame frame; struct can_frame frame;
std::memset(&frame, 0, sizeof(frame)); std::memset(&frame, 0, sizeof(frame));
int nbytes = read(sock_, &frame, sizeof(struct can_frame)); int nbytes = read(sock_, &frame, sizeof(struct can_frame));
if (nbytes < 0) { if (nbytes < 0) {
perror("CAN read error"); perror("CAN read error");
} }
return frame; return frame;
} }
struct canfd_frame CANBus::recvFD() { struct canfd_frame CANBus::recvFD() {
struct canfd_frame frame; struct canfd_frame frame;
std::memset(&frame, 0, sizeof(frame)); std::memset(&frame, 0, sizeof(frame));
int nbytes = read(sock_, &frame, sizeof(struct canfd_frame)); int nbytes = read(sock_, &frame, sizeof(struct canfd_frame));
if (nbytes < 0) { if (nbytes < 0) {
perror("CAN FD read error"); perror("CAN FD read error");
} }
return frame; return frame;
} }

167
openarm_hardware/src/motor.cpp
View File

@ -4,148 +4,135 @@
#include "openarm_hardware/motor.hpp" #include "openarm_hardware/motor.hpp"
Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID) Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID)
: MotorType(motorType), SlaveID(slaveID), MasterID(masterID), : MotorType(motorType),
Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0), SlaveID(slaveID),
state_q(0.0), state_dq(0.0), state_tau(0.0), MasterID(masterID),
state_tmos(0), state_trotor(0), Pd(0.0),
isEnable(false), NowControlMode(Control_Type::MIT) {} 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) { void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) {
state_q = q; state_q = q;
state_dq = dq; state_dq = dq;
state_tau = tau; state_tau = tau;
state_tmos = tmos; state_tmos = tmos;
state_trotor = trotor; state_trotor = trotor;
} }
double Motor::getPosition() const { return state_q; } double Motor::getPosition() const { return state_q; }
double Motor::getVelocity() const { return state_dq; } double Motor::getVelocity() const { return state_dq; }
double Motor::getTorque() const { return state_tau; } double Motor::getTorque() const { return state_tau; }
double Motor::getGoalPosition() const { return goal_position; }
double Motor::getGoalPosition() const { void Motor::setGoalPosition(double pos) { goal_position = pos; }
return goal_position;
}
void Motor::setGoalPosition(double pos) { double Motor::getGoalVelocity() const { return goal_velocity; }
goal_position = pos;
}
double Motor::getGoalVelocity() const { void Motor::setGoalVelocity(double velocity) { goal_velocity = velocity; }
return goal_velocity;
}
void Motor::setGoalVelocity(double velocity) { double Motor::getGoalTau() const { return goal_tau; }
goal_velocity = velocity;
}
double Motor::getGoalTau() const { void Motor::setGoalTau(double tau) { goal_tau = tau; }
return goal_tau;
}
void Motor::setGoalTau(double tau) { int Motor::getStateTmos() const { return state_tmos; }
goal_tau = tau;
}
int Motor::getStateTmos() const { void Motor::setStateTmos(int tmos) { state_tmos = tmos; }
return state_tmos;
}
void Motor::setStateTmos(int tmos) { int Motor::getStateTrotor() const { return state_trotor; }
state_tmos = tmos;
}
int Motor::getStateTrotor() const { void Motor::setStateTrotor(int trotor) { state_trotor = trotor; }
return state_trotor;
}
void Motor::setStateTrotor(int trotor) {
state_trotor = trotor;
}
int Motor::getParam(int RID) const { int Motor::getParam(int RID) const {
auto it = temp_param_dict.find(RID); auto it = temp_param_dict.find(RID);
return (it != temp_param_dict.end()) ? it->second : -1; return (it != temp_param_dict.end()) ? it->second : -1;
} }
void Motor::setTempParam(int RID, int value) { void Motor::setTempParam(int RID, int value) { temp_param_dict[RID] = value; }
temp_param_dict[RID] = value;
}
double LIMIT_MIN_MAX(double x, double min, double max) { 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) { uint16_t double_to_uint(double x, double x_min, double x_max, int bits) {
x = LIMIT_MIN_MAX(x, x_min, x_max); x = LIMIT_MIN_MAX(x, x_min, x_max);
double span = x_max - x_min; double span = x_max - x_min;
double data_norm = (x - x_min) / span; double data_norm = (x - x_min) / span;
return static_cast<uint16_t>(data_norm * ((1 << bits) - 1)); return static_cast<uint16_t>(data_norm * ((1 << bits) - 1));
} }
double uint_to_double(uint16_t x, double min, double max, int bits) { double uint_to_double(uint16_t x, double min, double max, int bits) {
double span = max - min; double span = max - min;
double data_norm = static_cast<double>(x) / ((1 << bits) - 1); double data_norm = static_cast<double>(x) / ((1 << bits) - 1);
return data_norm * span + min; return data_norm * span + min;
} }
std::array<uint8_t, 8> double_to_uint8s(double value) { std::array<uint8_t, 8> double_to_uint8s(double value) {
std::array<uint8_t, 8> bytes; std::array<uint8_t, 8> bytes;
std::memcpy(bytes.data(), &value, sizeof(double)); std::memcpy(bytes.data(), &value, sizeof(double));
return bytes; return bytes;
} }
std::array<uint8_t, 4> float_to_uint8s(float value) { std::array<uint8_t, 4> float_to_uint8s(float value) {
std::array<uint8_t, 4> bytes{}; std::array<uint8_t, 4> bytes{};
std::memcpy(bytes.data(), &value, sizeof(float)); std::memcpy(bytes.data(), &value, sizeof(float));
return bytes; return bytes;
} }
float uint8s_to_float(const std::array<uint8_t, 4>& bytes) { float uint8s_to_float(const std::array<uint8_t, 4>& bytes) {
float value; float value;
std::memcpy(&value, bytes.data(), sizeof(float)); std::memcpy(&value, bytes.data(), sizeof(float));
return value; return value;
} }
std::array<uint8_t, 8> data_to_uint8s(uint32_t value) { std::array<uint8_t, 8> data_to_uint8s(uint32_t value) {
std::array<uint8_t, 8> bytes; std::array<uint8_t, 8> bytes;
std::memcpy(bytes.data(), &value, sizeof(uint32_t)); std::memcpy(bytes.data(), &value, sizeof(uint32_t));
return bytes; return bytes;
} }
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,
uint32_t value; uint8_t byte4) {
uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; uint32_t value;
std::memcpy(&value, bytes, sizeof(uint32_t)); uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
return value; 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 uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3,
double value; uint8_t byte4) {
uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; double value;
std::memcpy(&value, bytes, sizeof(double)); uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
return value; std::memcpy(&value, bytes, sizeof(double));
return value;
} }
bool is_in_ranges(int number) { 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<uint8_t>& data) { void print_hex(const std::vector<uint8_t>& data) {
for (auto byte : data) { for (auto byte : data) {
std::cout << std::hex << std::uppercase << (int)byte << " "; std::cout << std::hex << std::uppercase << (int)byte << " ";
} }
std::cout << std::dec << std::endl; std::cout << std::dec << std::endl;
} }
template <typename T> template <typename T>
T get_enum_by_index(int index) { T get_enum_by_index(int index) {
if (index >= 0 && index < static_cast<int>(T::COUNT)) { if (index >= 0 && index < static_cast<int>(T::COUNT)) {
return static_cast<T>(index); return static_cast<T>(index);
} }
return static_cast<T>(-1); return static_cast<T>(-1);
} }
#endif // OPENARM_MOTOR_H_
#endif // OPENARM_MOTOR_H_

657
openarm_hardware/src/motor_control.cpp
View File

@ -1,455 +1,458 @@
#include "openarm_hardware/motor_control.hpp" #include "openarm_hardware/motor_control.hpp"
#include "openarm_hardware/motor.hpp" #include "openarm_hardware/motor.hpp"
MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {} MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {}
bool MotorControl::addMotor(Motor& motor) { bool MotorControl::addMotor(Motor& motor) {
motors_map_[motor.SlaveID] = &motor; motors_map_[motor.SlaveID] = &motor;
if (motor.MasterID != 0) { if (motor.MasterID != 0) {
motors_map_[motor.MasterID] = &motor; motors_map_[motor.MasterID] = &motor;
} }
return true; return true;
} }
void MotorControl::enable(Motor& motor) { void MotorControl::enable(Motor& motor) {
controlCmd(motor, 0xFC); controlCmd(motor, 0xFC);
sleep(0.3); sleep(0.3);
} }
void MotorControl::disable(Motor& motor) { void MotorControl::disable(Motor& motor) {
controlCmd(motor, 0xFD); controlCmd(motor, 0xFD);
sleep(0.3); sleep(0.3);
} }
void MotorControl::set_zero_position(Motor& motor){ void MotorControl::set_zero_position(Motor& motor) {
controlCmd(motor, 0xFE); controlCmd(motor, 0xFE);
sleep(0.3); sleep(0.3);
recv(); recv();
} }
void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau) { void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q,
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { double dq, double tau) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
return; std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
} return;
}
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
int motor_index = static_cast<int>(motor.MotorType); int motor_index = static_cast<int>(motor.MotorType);
double Q_MAX = Limit_Param[motor_index][0]; double Q_MAX = Limit_Param[motor_index][0];
double DQ_MAX = Limit_Param[motor_index][1]; double DQ_MAX = Limit_Param[motor_index][1];
double TAU_MAX = Limit_Param[motor_index][2]; double TAU_MAX = Limit_Param[motor_index][2];
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); 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 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 tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
std::array<uint8_t, 8> data = { std::array<uint8_t, 8> data = {
static_cast<uint8_t>((q_uint >> 8) & 0xFF), static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF), static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4), static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF), static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4), static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF) static_cast<uint8_t>(tau_uint & 0xFF)};
};
sendData(motor.SlaveID, data); sendData(motor.SlaveID, data);
recv(); 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()) { uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
return;
}
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); int motor_index = static_cast<int>(motor.MotorType);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); 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<int>(motor.MotorType); uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16);
double Q_MAX = Limit_Param[motor_index][0]; uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12);
double DQ_MAX = Limit_Param[motor_index][1]; uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
double TAU_MAX = Limit_Param[motor_index][2];
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); std::array<uint8_t, 8> data = {
uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); static_cast<uint8_t>((q_uint >> 8) & 0xFF),
uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)};
std::array<uint8_t, 8> data = { sendData(motor.SlaveID, data);
static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)
};
sendData(motor.SlaveID, data);
} }
void MotorControl::sendData(uint16_t motor_id, const std::array<uint8_t, 8>& data) { void MotorControl::sendData(uint16_t motor_id,
canbus_.send(motor_id, data); const std::array<uint8_t, 8>& data) {
canbus_.send(motor_id, data);
} }
void MotorControl::recv() { void MotorControl::recv() {
uint16_t id; uint16_t id;
uint8_t len; uint8_t len;
std::array<uint8_t, 64> data = canbus_.recv(id, len); std::array<uint8_t, 64> data = canbus_.recv(id, len);
if (canbus_.whichCAN() == CAN_MODE_CLASSIC) { if (canbus_.whichCAN() == CAN_MODE_CLASSIC) {
can_frame frame; can_frame frame;
std::memset(&frame, 0, sizeof(frame)); std::memset(&frame, 0, sizeof(frame));
frame.can_id = id; frame.can_id = id;
frame.can_dlc = len; frame.can_dlc = len;
std::memcpy(frame.data, data.data(), len); std::memcpy(frame.data, data.data(), len);
processPacket(frame); processPacket(frame);
} } else if (canbus_.whichCAN() == CAN_MODE_FD) {
else if (canbus_.whichCAN() == CAN_MODE_FD) { canfd_frame fd_frame;
canfd_frame fd_frame; std::memset(&fd_frame, 0, sizeof(fd_frame));
std::memset(&fd_frame, 0, sizeof(fd_frame)); fd_frame.can_id = id;
fd_frame.can_id = id; fd_frame.len = len;
fd_frame.len = len; std::memcpy(fd_frame.data, data.data(), 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){ void MotorControl::control_delay(Motor& motor, double kp, double kd, double q,
controlMIT(motor, kp, kd, q, dq, tau); double dq, double tau, double delay) {
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(delay))); controlMIT(motor, kp, kd, q, dq, tau);
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int>(delay)));
} }
void MotorControl::controlPosVel(Motor& motor, double pos, double vel) { void MotorControl::controlPosVel(Motor& motor, double pos, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosVel ERROR: Motor ID not found" << std::endl; std::cerr << "controlPosVel ERROR: Motor ID not found" << std::endl;
return; return;
} }
uint16_t motor_id = 0x100 + motor.SlaveID; uint16_t motor_id = 0x100 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto pos_buf = float_to_uint8s(static_cast<float>(pos)); auto pos_buf = float_to_uint8s(static_cast<float>(pos));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i]; data_buf[i] = pos_buf[i];
data_buf[i + 4] = vel_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) { void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl; std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl;
return; return;
} }
uint16_t motor_id = 0x100 + motor.SlaveID; uint16_t motor_id = 0x100 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos)); auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i]; data_buf[i] = pos_buf[i];
data_buf[i + 4] = vel_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) { void MotorControl::controlVel(Motor& motor, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlVel ERROR: Motor ID not found" << std::endl; std::cerr << "controlVel ERROR: Motor ID not found" << std::endl;
return; return;
} }
uint16_t motor_id = 0x200 + motor.SlaveID; uint16_t motor_id = 0x200 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = vel_buf[i]; data_buf[i] = vel_buf[i];
} }
sendData(motor_id, data_buf); sendData(motor_id, data_buf);
recv(); recv();
} }
void MotorControl::controlVel2(Motor& motor, double vel) { void MotorControl::controlVel2(Motor& motor, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlVel2 ERROR: Motor ID not found" << std::endl; std::cerr << "controlVel2 ERROR: Motor ID not found" << std::endl;
return; return;
} }
uint16_t motor_id = 0x200 + motor.SlaveID; uint16_t motor_id = 0x200 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = vel_buf[i]; data_buf[i] = vel_buf[i];
} }
sendData(motor_id, data_buf);
sendData(motor_id, data_buf);
} }
void MotorControl::controlPosForce(Motor& motor, double pos, double vel, double tau) { void MotorControl::controlPosForce(Motor& motor, double pos, double vel,
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { double tau) {
std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
return; std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl;
} return;
}
uint16_t motor_id = 0x300 + motor.SlaveID; uint16_t motor_id = 0x300 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos)); auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto tau_buf = float_to_uint8s(static_cast<float>(tau)); auto tau_buf = float_to_uint8s(static_cast<float>(tau));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i]; data_buf[i] = pos_buf[i];
} }
data_buf[4] = vel_buf[0]; data_buf[4] = vel_buf[0];
data_buf[5] = vel_buf[1]; data_buf[5] = vel_buf[1];
data_buf[6] = tau_buf[0]; data_buf[6] = tau_buf[0];
data_buf[7] = tau_buf[1]; data_buf[7] = tau_buf[1];
sendData(motor_id, data_buf); sendData(motor_id, data_buf);
recv(); recv();
} }
void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, double tau) { void MotorControl::controlPosForce2(Motor& motor, double pos, double vel,
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { double tau) {
std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
return; std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl;
} return;
}
uint16_t motor_id = 0x300 + motor.SlaveID; uint16_t motor_id = 0x300 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0}; std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos)); auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel)); auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto tau_buf = float_to_uint8s(static_cast<float>(tau)); auto tau_buf = float_to_uint8s(static_cast<float>(tau));
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i]; data_buf[i] = pos_buf[i];
} }
data_buf[4] = vel_buf[0]; data_buf[4] = vel_buf[0];
data_buf[5] = vel_buf[1]; data_buf[5] = vel_buf[1];
data_buf[6] = tau_buf[0]; data_buf[6] = tau_buf[0];
data_buf[7] = tau_buf[1]; 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) { bool MotorControl::switchControlMode(Motor& motor, Control_Type control_mode) {
const int max_retries = 20; const int max_retries = 20;
const double retry_interval = 0.1; const double retry_interval = 0.1;
DM_variable RID = DM_variable::CTRL_MODE; DM_variable RID = DM_variable::CTRL_MODE;
writeMotorParam(motor, RID, static_cast<int>(control_mode)); writeMotorParam(motor, RID, static_cast<int>(control_mode));
for (int i = 0; i < max_retries; ++i) { for (int i = 0; i < max_retries; ++i) {
usleep(static_cast<useconds_t>(retry_interval * 1e6)); usleep(static_cast<useconds_t>(retry_interval * 1e6));
recv_set_param_data(); recv_set_param_data();
if (motor.getParam(static_cast<int>(RID)) == static_cast<int>(control_mode)) { if (motor.getParam(static_cast<int>(RID)) ==
return true; static_cast<int>(control_mode)) {
} return true;
} }
return false; }
return false;
} }
void MotorControl::save_motor_param(Motor& motor) { void MotorControl::save_motor_param(Motor& motor) {
std::array<uint8_t, 8> data = { std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF), static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF), static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0xAA, 0x00, 0x00, 0x00, 0x00, 0x00 0xAA,
}; 0x00,
disable(motor); 0x00,
canbus_.send(0x7FF, data); 0x00,
usleep(1000); 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) { // void MotorControl::change_limit_param(DM_Motor_Type motor_type, double PMAX,
//int index = static_cast<int>(motor_type); // double VMAX, double TMAX) { int index = static_cast<int>(motor_type);
//Limit_Param[index][0] = PMAX; // Limit_Param[index][0] = PMAX;
//Limit_Param[index][1] = VMAX; // Limit_Param[index][1] = VMAX;
//Limit_Param[index][2] = TMAX; // Limit_Param[index][2] = TMAX;
//} // }
void MotorControl::recv_set_param_data() { void MotorControl::recv_set_param_data() {
uint16_t id; uint16_t id;
uint8_t len; uint8_t len;
std::array<uint8_t, 64> data = canbus_.recv(id, len); std::array<uint8_t, 64> data = canbus_.recv(id, len);
uint8_t cmd = 0x11; uint8_t cmd = 0x11;
if (len >= 8) { if (len >= 8) {
std::cout << "CANID: 0x" << std::hex << id << ", CMD: 0x" << static_cast<int>(cmd) << std::dec << std::endl; std::cout << "CANID: 0x" << std::hex << id << ", CMD: 0x"
for (int i = 0; i < 8; ++i) { << static_cast<int>(cmd) << std::dec << std::endl;
std::cout << "0x" << std::hex << static_cast<int>(data[i]) << " "; for (int i = 0; i < 8; ++i) {
} std::cout << "0x" << std::hex << static_cast<int>(data[i]) << " ";
std::cout << std::dec << std::endl; }
} std::cout << std::dec << std::endl;
}
} }
void MotorControl::processPacket(const can_frame& frame) { void MotorControl::processPacket(const can_frame& frame) {
uint16_t motorID = frame.data[0]; uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11; // someday fix uint8_t cmd = 0x11; // someday fix
if (cmd == 0x11) { if (cmd == 0x11) {
if (motorID != 0x00) { if (motorID != 0x00) {
auto it = motors_map_.find(motorID); auto it = motors_map_.find(motorID);
if (it != motors_map_.end() && it->second) { if (it != motors_map_.end() && it->second) {
Motor* motor = it->second; Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; 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 dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6]; int t_mos = frame.data[6];
int t_rotor = frame.data[7]; int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0]; double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1]; double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2]; double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); 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_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_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);
} }
} else { } else {
uint16_t MasterID = frame.data[0] & 0x0F; uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map_.find(MasterID); auto it = motors_map_.find(MasterID);
if (it != motors_map_.end() && it->second) { if (it != motors_map_.end() && it->second) {
Motor* motor = it->second; Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; 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 dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6]; int t_mos = frame.data[6];
int t_rotor = frame.data[7]; int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0]; double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1]; double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2]; double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); 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_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_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) { void MotorControl::processPacketFD(const canfd_frame& frame) {
uint16_t motorID = frame.data[0]; uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11; // someday fix uint8_t cmd = 0x11; // someday fix
if (cmd == 0x11) { if (cmd == 0x11) {
if (motorID != 0x00) { if (motorID != 0x00) {
auto it = motors_map_.find(motorID); auto it = motors_map_.find(motorID);
if (it != motors_map_.end() && it->second) { if (it != motors_map_.end() && it->second) {
Motor* motor = it->second; Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; 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 dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6]; int t_mos = frame.data[6];
int t_rotor = frame.data[7]; int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0]; double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1]; double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2]; double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); 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_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_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);
} }
} else { } else {
uint16_t MasterID = frame.data[0] & 0x0F; uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map_.find(MasterID); auto it = motors_map_.find(MasterID);
if (it != motors_map_.end() && it->second) { if (it != motors_map_.end() && it->second) {
Motor* motor = it->second; Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; 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 dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6]; int t_mos = frame.data[6];
int t_rotor = frame.data[7]; int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0]; double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1]; double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2]; double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); 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_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_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) { void MotorControl::controlCmd(Motor& motor, uint8_t cmd) {
std::array<uint8_t, 8> data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd}; std::array<uint8_t, 8> data_buf = {0xFF, 0xFF, 0xFF, 0xFF,
sendData(motor.SlaveID, data_buf); 0xFF, 0xFF, 0xFF, cmd};
sendData(motor.SlaveID, data_buf);
} }
void MotorControl::readRIDParam(Motor& motor, DM_variable RID) { void MotorControl::readRIDParam(Motor& motor, DM_variable RID) {
std::array<uint8_t, 8> data = { std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF), static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF), static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x33, 0x33,
static_cast<uint8_t>(RID), static_cast<uint8_t>(RID),
0x00, 0x00, 0x00, 0x00 0x00,
}; 0x00,
canbus_.send(0x7FF, data); 0x00,
0x00};
canbus_.send(0x7FF, data);
} }
void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) { void MotorControl::writeMotorParam(Motor& motor, DM_variable RID,
std::array<uint8_t, 8> data = { double value) {
static_cast<uint8_t>(motor.SlaveID & 0xFF), std::array<uint8_t, 8> data = {
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF), static_cast<uint8_t>(motor.SlaveID & 0xFF),
0x55, static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF), 0x55,
static_cast<uint8_t>(RID) static_cast<uint8_t>(RID)};
};
if (is_in_ranges(static_cast<int>(RID))) { if (is_in_ranges(static_cast<int>(RID))) {
auto intData = data_to_uint8s(static_cast<uint32_t>(value)); auto intData = data_to_uint8s(static_cast<uint32_t>(value));
std::copy(intData.begin(), intData.end(), data.begin() + 4); std::copy(intData.begin(), intData.end(), data.begin() + 4);
} else { } else {
auto doubleData = double_to_uint8s(value); auto doubleData = double_to_uint8s(value);
std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4); std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4);
} }
canbus_.send(0x7FF, data); canbus_.send(0x7FF, data);
} }

186
openarm_hardware/src/openarm_hardware.cpp
View File

@ -1,5 +1,5 @@
// Copyright (c) 2025, Reazon Holdings, Inc. // 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"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with 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 // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include "openarm_hardware/openarm_hardware.hpp"
#include <limits> #include <limits>
#include <vector> #include <vector>
#include "openarm_hardware/openarm_hardware.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/logging.hpp" #include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
namespace openarm_hardware namespace openarm_hardware {
{
static const std::string& can_device_name = "can0"; static const std::string& can_device_name = "can0";
OpenArmHW::OpenArmHW() = default; OpenArmHW::OpenArmHW() = default;
hardware_interface::CallbackReturn OpenArmHW::on_init( hardware_interface::CallbackReturn OpenArmHW::on_init(
const hardware_interface::HardwareInfo & info) const hardware_interface::HardwareInfo& info) {
{ if (hardware_interface::SystemInterface::on_init(info) !=
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) CallbackReturn::SUCCESS) {
{
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
//read hardware parameters // read hardware parameters
if (info.hardware_parameters.find("can_device") == info.hardware_parameters.end()){ if (info.hardware_parameters.find("can_device") ==
RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), "No can_device parameter found"); info.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"),
"No can_device parameter found");
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
auto it = info.hardware_parameters.find("prefix"); auto it = info.hardware_parameters.find("prefix");
if (it == info.hardware_parameters.end()){ if (it == info.hardware_parameters.end()) {
prefix_ = ""; prefix_ = "";
} } else {
else{
prefix_ = it->second; prefix_ = it->second;
} }
it = info.hardware_parameters.find("disable_torque"); it = info.hardware_parameters.find("disable_torque");
if (it == info.hardware_parameters.end()){ if (it == info.hardware_parameters.end()) {
disable_torque_ = false; disable_torque_ = false;
} } else {
else{
disable_torque_ = it->second == "true"; disable_torque_ = it->second == "true";
} }
//temp CANFD // temp CANFD
canbus_ = std::make_unique<CANBus>(info.hardware_parameters.at("can_device"),CAN_MODE_FD); canbus_ = std::make_unique<CANBus>(info.hardware_parameters.at("can_device"),
CAN_MODE_FD);
motor_control_ = std::make_unique<MotorControl>(*canbus_); motor_control_ = std::make_unique<MotorControl>(*canbus_);
if(USING_GRIPPER){ if (USING_GRIPPER) {
motor_types.emplace_back(DM_Motor_Type::DM4310); motor_types.emplace_back(DM_Motor_Type::DM4310);
can_device_ids.emplace_back(0x08); can_device_ids.emplace_back(0x08);
can_master_ids.emplace_back(0x18); can_master_ids.emplace_back(0x18);
@ -69,8 +69,9 @@ hardware_interface::CallbackReturn OpenArmHW::on_init(
} }
motors_.resize(curr_dof); motors_.resize(curr_dof);
for(size_t i = 0; i < curr_dof; ++i){ for (size_t i = 0; i < curr_dof; ++i) {
motors_[i] = std::make_unique<Motor>(motor_types[i], can_device_ids[i], can_master_ids[i]); motors_[i] = std::make_unique<Motor>(motor_types[i], can_device_ids[i],
can_master_ids[i]);
motor_control_->addMotor(*motors_[i]); motor_control_->addMotor(*motors_[i]);
} }
@ -87,9 +88,7 @@ hardware_interface::CallbackReturn OpenArmHW::on_init(
} }
hardware_interface::CallbackReturn OpenArmHW::on_configure( 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)); read(rclcpp::Time(0), rclcpp::Duration(0, 0));
// zero position or calibrate to pose // zero position or calibrate to pose
// for (std::size_t i = 0; i < curr_dof; ++i) // 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]); // motor_control_->set_zero_position(*motors_[i]);
// } // }
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
std::vector<hardware_interface::StateInterface> OpenArmHW::export_state_interfaces() std::vector<hardware_interface::StateInterface>
{ OpenArmHW::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces; std::vector<hardware_interface::StateInterface> state_interfaces;
for (size_t i = 0; i < curr_dof; ++i) for (size_t i = 0; i < curr_dof; ++i) {
{ state_interfaces.emplace_back(hardware_interface::StateInterface(
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_states_[i])); info_.joints[i].name, hardware_interface::HW_IF_POSITION,
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_states_[i])); &pos_states_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_states_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface(
RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Exporting state interface for joint %s", info_.joints[i].name.c_str()); 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; return state_interfaces;
} }
std::vector<hardware_interface::CommandInterface> OpenArmHW::export_command_interfaces() std::vector<hardware_interface::CommandInterface>
{ OpenArmHW::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces; std::vector<hardware_interface::CommandInterface> command_interfaces;
for (size_t i = 0; i < curr_dof; ++i) for (size_t i = 0; i < curr_dof; ++i) {
{ command_interfaces.emplace_back(hardware_interface::CommandInterface(
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_commands_[i])); info_.joints[i].name, hardware_interface::HW_IF_POSITION,
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_commands_[i])); &pos_commands_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_ff_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; return command_interfaces;
} }
void OpenArmHW::refresh_motors() void OpenArmHW::refresh_motors() {
{ for (const auto& motor : motors_) {
for(const auto& motor: motors_){
motor_control_->controlMIT(*motor, 0.0, 0.0, 0.0, 0.0, 0.0); motor_control_->controlMIT(*motor, 0.0, 0.0, 0.0, 0.0, 0.0);
} }
} }
hardware_interface::CallbackReturn OpenArmHW::on_activate( 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)); read(rclcpp::Time(0), rclcpp::Duration(0, 0));
bool zeroed = false; bool zeroed = false;
for(const auto& motor: motors_){ for (const auto& motor : motors_) {
motor_control_->enable(*motor); motor_control_->enable(*motor);
} }
while(!zeroed){ while (!zeroed) {
bool all_zero = true; 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]; 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; all_zero = false;
} }
const double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff)); const double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff));
double command = pos_states_[m]; double command = pos_states_[m];
if (pos_states_[m] < pos_commands_[m]){ if (pos_states_[m] < pos_commands_[m]) {
command += max_step; command += max_step;
} } else {
else{
command -= max_step; command -= max_step;
} }
motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0); motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0);
} }
if (all_zero){ if (all_zero) {
zeroed = true; zeroed = true;
} } else {
else{
sleep(0.01); sleep(0.01);
read(rclcpp::Time(0), rclcpp::Duration(0, 0)); read(rclcpp::Time(0), rclcpp::Duration(0, 0));
} }
@ -178,10 +183,9 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate(
} }
hardware_interface::CallbackReturn OpenArmHW::on_deactivate( hardware_interface::CallbackReturn OpenArmHW::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) const rclcpp_lifecycle::State& /*previous_state*/) {
{
refresh_motors(); refresh_motors();
for(const auto& motor: motors_){ for (const auto& motor : motors_) {
motor_control_->disable(*motor); motor_control_->disable(*motor);
} }
@ -189,48 +193,58 @@ hardware_interface::CallbackReturn OpenArmHW::on_deactivate(
} }
hardware_interface::return_type OpenArmHW::read( hardware_interface::return_type OpenArmHW::read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
{ for (size_t i = 0; i < ARM_DOF; ++i) {
for(size_t i = 0; i < ARM_DOF; ++i){
pos_states_[i] = motors_[i]->getPosition(); pos_states_[i] = motors_[i]->getPosition();
vel_states_[i] = motors_[i]->getVelocity(); vel_states_[i] = motors_[i]->getVelocity();
tau_states_[i] = motors_[i]->getTorque(); tau_states_[i] = motors_[i]->getTorque();
} }
if(USING_GRIPPER){ if (USING_GRIPPER) {
pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() *
vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; GRIPPER_REFERENCE_GEAR_RADIUS_M *
tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; 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; return hardware_interface::return_type::OK;
} }
hardware_interface::return_type OpenArmHW::write( hardware_interface::return_type OpenArmHW::write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
{ if (disable_torque_) {
if (disable_torque_){
// refresh motor state on write // 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); motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0);
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
} }
for (size_t i = 0; i < ARM_DOF; ++i) {
for(size_t i = 0; i < ARM_DOF; ++i){ if (std::abs(pos_commands_[i] - pos_states_[i]) > POS_JUMP_TOLERANCE_RAD) {
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",
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]); info_.joints[i].name.c_str(), pos_states_[i],
pos_commands_[i]);
return hardware_interface::return_type::ERROR; 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){ if (USING_GRIPPER) {
motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), motor_control_->controlMIT(
-pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER, *motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX),
vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER, -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M *
tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER); 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; return hardware_interface::return_type::OK;
} }
@ -239,5 +253,5 @@ hardware_interface::return_type OpenArmHW::write(
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmHW,
openarm_hardware::OpenArmHW, hardware_interface::SystemInterface) hardware_interface::SystemInterface)

15
openarm_hardware/test/test_openarm_hardware.cpp
View File

@ -1,5 +1,5 @@
// Copyright (c) 2025, Reazon Holdings, Inc. // 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"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with 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/components_urdfs.hpp"
#include "ros2_control_test_assets/descriptions.hpp" #include "ros2_control_test_assets/descriptions.hpp"
class TestOpenArmHW : public ::testing::Test class TestOpenArmHW : public ::testing::Test {
{ protected:
protected: void SetUp() override {
void SetUp() override
{
openarm_hardware_7dof_ = openarm_hardware_7dof_ =
R"( R"(
<ros2_control name="OpenArmHW7DOF" type="system"> <ros2_control name="OpenArmHW7DOF" type="system">
<hardware> <hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware --> <!-- By default, set up controllers for simulation. This won't work on real hardware -->
@ -123,8 +121,7 @@ protected:
std::string openarm_hardware_7dof_; 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_ + auto urdf = ros2_control_test_assets::urdf_head + openarm_hardware_7dof_ +
ros2_control_test_assets::urdf_tail; ros2_control_test_assets::urdf_tail;
ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf));