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

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

@ -1,21 +1,19 @@
#pragma once
#include <cstdint>
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <fcntl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <array>
#include <fcntl.h>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <string>
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);
@ -34,5 +32,3 @@ private:
int sock_;
int mode_;
};

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

@ -1,13 +1,14 @@
#pragma once
#include <stdio.h>
#include <algorithm>
#include <array>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <map>
#include <vector>
#include <iostream>
#include <array>
#include <algorithm>
#include <cstring>
#include <stdio.h>
enum class DM_Motor_Type : uint8_t {
DM4310 = 0,
@ -74,13 +75,7 @@ enum class DM_variable : uint8_t {
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:
@ -110,6 +105,7 @@ public:
void setGoalTau(double tau);
void setStateTmos(int tmos);
void setStateTrotor(int trotor);
private:
double Pd, Vd;
double goal_position, goal_velocity, goal_tau;
@ -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);
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);
@ -143,4 +140,3 @@ void print_hex(const std::vector<uint8_t>& data);
template <typename T>
T get_enum_by_index(int index);

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

@ -1,17 +1,19 @@
#pragma once
#include <iostream>
#include <vector>
#include <map>
#include <array>
#include <cstring>
#include <unistd.h>
#include "motor.hpp"
#include "canbus.hpp"
#include <array>
#include <atomic>
#include <functional>
#include <thread>
#include <chrono>
#include <cstring>
#include <functional>
#include <iostream>
#include <map>
#include <thread>
#include <vector>
#include "canbus.hpp"
#include "motor.hpp"
class MotorControl {
public:
@ -20,12 +22,15 @@ class MotorControl {
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 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<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,
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);
@ -36,7 +41,8 @@ class MotorControl {
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 change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX,
// double TMAX);
void recv_set_param_data();
@ -59,12 +65,9 @@ class MotorControl {
{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);
};

44
openarm_hardware/include/openarm_hardware/openarm_hardware.hpp
View File

@ -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 <memory>
#include <string>
#include <vector>
#include <memory>
#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<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<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_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<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> KD = {2.75, 2.5, 0.7, 0.4, 0.7, 0.6, 0.5, 0.1};
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> 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,8 +54,7 @@ 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
{
class OpenArmHW : public hardware_interface::SystemInterface {
public:
OpenArmHW();
@ -66,10 +67,12 @@ public:
const rclcpp_lifecycle::State& previous_state) override;
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
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces()
override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_activate(
@ -80,8 +83,8 @@ public:
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(
@ -105,4 +108,3 @@ private:
};
} // 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, 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

18
openarm_hardware/src/canbus.cpp
View File

@ -1,7 +1,6 @@
#include "openarm_hardware/canbus.hpp"
CANBus::CANBus(const std::string& interface, int mode)
: mode_(mode) {
CANBus::CANBus(const std::string& interface, int mode) : mode_(mode) {
struct ifreq ifr;
struct sockaddr_can addr;
@ -23,7 +22,8 @@ CANBus::CANBus(const std::string& interface, int mode)
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) {
if (setsockopt(sock_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
sizeof(enable_canfd)) < 0) {
perror("CAN FD setsockopt failed");
exit(EXIT_FAILURE);
}
@ -35,13 +35,9 @@ CANBus::CANBus(const std::string& interface, int mode)
}
}
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<uint8_t, 8>& data) {
if (mode_ == CAN_MODE_FD)
@ -66,7 +62,8 @@ std::array<uint8_t, 64> CANBus::recv(uint16_t& out_id, uint8_t& out_len) {
return buffer;
}
bool CANBus::sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
bool CANBus::sendClassic(uint16_t motor_id,
const std::array<uint8_t, 8>& data) {
struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
@ -118,4 +115,3 @@ struct canfd_frame CANBus::recvFD() {
}
return frame;
}

75
openarm_hardware/src/motor.cpp
View File

@ -4,11 +4,20 @@
#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;
@ -22,55 +31,32 @@ 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;
}
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));
@ -113,14 +99,16 @@ std::array<uint8_t, 8> data_to_uint8s(uint32_t value) {
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,
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 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));
@ -128,7 +116,8 @@ double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byt
}
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) {
@ -144,8 +133,6 @@ T get_enum_by_index(int index) {
return static_cast<T>(index);
}
return static_cast<T>(-1);
}
#endif // OPENARM_MOTOR_H_

77
openarm_hardware/src/motor_control.cpp
View File

@ -1,9 +1,9 @@
#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) {
@ -28,7 +28,8 @@ void MotorControl::set_zero_position(Motor& motor){
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,
double dq, double tau) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
return;
@ -54,15 +55,14 @@ void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, doub
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)
};
static_cast<uint8_t>(tau_uint & 0xFF)};
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;
@ -88,13 +88,13 @@ void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, dou
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)
};
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,
const std::array<uint8_t, 8>& data) {
canbus_.send(motor_id, data);
}
@ -111,8 +111,7 @@ void MotorControl::recv() {
std::memcpy(frame.data, data.data(), len);
processPacket(frame);
}
else if (canbus_.whichCAN() == CAN_MODE_FD) {
} else if (canbus_.whichCAN() == CAN_MODE_FD) {
canfd_frame fd_frame;
std::memset(&fd_frame, 0, sizeof(fd_frame));
fd_frame.can_id = id;
@ -123,9 +122,11 @@ void MotorControl::recv() {
}
}
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,
double dq, double tau, double delay) {
controlMIT(motor, kp, kd, q, dq, tau);
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(delay)));
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int>(delay)));
}
void MotorControl::controlPosVel(Motor& motor, double pos, double vel) {
@ -147,11 +148,9 @@ void MotorControl::controlPosVel(Motor& motor, double pos, double vel) {
sendData(motor_id, data_buf);
recv();
}
void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl;
@ -172,7 +171,6 @@ void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) {
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;
@ -191,7 +189,6 @@ void MotorControl::controlVel(Motor& motor, double vel) {
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;
@ -207,10 +204,10 @@ void MotorControl::controlVel2(Motor& motor, double vel) {
}
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,
double tau) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl;
return;
@ -237,7 +234,8 @@ void MotorControl::controlPosForce(Motor& motor, double pos, double vel, double
recv();
}
void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, double tau) {
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;
@ -263,7 +261,6 @@ void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, double
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;
@ -274,7 +271,8 @@ bool MotorControl::switchControlMode(Motor& motor, Control_Type control_mode) {
for (int i = 0; i < max_retries; ++i) {
usleep(static_cast<useconds_t>(retry_interval * 1e6));
recv_set_param_data();
if (motor.getParam(static_cast<int>(RID)) == static_cast<int>(control_mode)) {
if (motor.getParam(static_cast<int>(RID)) ==
static_cast<int>(control_mode)) {
return true;
}
}
@ -284,14 +282,18 @@ void MotorControl::save_motor_param(Motor& motor) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0xAA, 0x00, 0x00, 0x00, 0x00, 0x00
};
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<int>(motor_type);
// void MotorControl::change_limit_param(DM_Motor_Type motor_type, double PMAX,
// double VMAX, double TMAX) { int index = static_cast<int>(motor_type);
// Limit_Param[index][0] = PMAX;
// Limit_Param[index][1] = VMAX;
// Limit_Param[index][2] = TMAX;
@ -305,7 +307,8 @@ void MotorControl::recv_set_param_data() {
uint8_t cmd = 0x11;
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"
<< static_cast<int>(cmd) << std::dec << std::endl;
for (int i = 0; i < 8; ++i) {
std::cout << "0x" << std::hex << static_cast<int>(data[i]) << " ";
}
@ -417,9 +420,9 @@ void MotorControl::processPacketFD(const canfd_frame& frame) {
}
}
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,
0xFF, 0xFF, 0xFF, cmd};
sendData(motor.SlaveID, data_buf);
}
@ -429,18 +432,19 @@ void MotorControl::readRIDParam(Motor& motor, DM_variable RID) {
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x33,
static_cast<uint8_t>(RID),
0x00, 0x00, 0x00, 0x00
};
0x00,
0x00,
0x00,
0x00};
canbus_.send(0x7FF, data);
}
void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) {
void MotorControl::writeMotorParam(Motor& motor, DM_variable RID,
double value) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x55,
static_cast<uint8_t>(RID)
};
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF), 0x55,
static_cast<uint8_t>(RID)};
if (is_in_ranges(static_cast<int>(RID))) {
auto intData = data_to_uint8s(static_cast<uint32_t>(value));
@ -452,4 +456,3 @@ void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value)
canbus_.send(0x7FF, data);
}

146
openarm_hardware/src/openarm_hardware.cpp
View File

@ -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,52 +13,52 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include "openarm_hardware/openarm_hardware.hpp"
#include <limits>
#include <vector>
#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");
if (info.hardware_parameters.find("can_device") ==
info.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"),
"No can_device parameter found");
return CallbackReturn::ERROR;
}
auto it = info.hardware_parameters.find("prefix");
if (it == info.hardware_parameters.end()) {
prefix_ = "";
}
else{
} else {
prefix_ = it->second;
}
it = info.hardware_parameters.find("disable_torque");
if (it == info.hardware_parameters.end()) {
disable_torque_ = false;
}
else{
} else {
disable_torque_ = it->second == "true";
}
// 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_);
if (USING_GRIPPER) {
@ -70,7 +70,8 @@ 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>(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]);
}
@ -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,47 +96,56 @@ hardware_interface::CallbackReturn OpenArmHW::on_configure(
// motor_control_->set_zero_position(*motors_[i]);
// }
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;
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<hardware_interface::CommandInterface> OpenArmHW::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface>
OpenArmHW::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> 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()
{
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_) {
@ -156,17 +164,14 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate(
double command = pos_states_[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) {
zeroed = true;
}
else{
} else {
sleep(0.01);
read(rclcpp::Time(0), rclcpp::Duration(0, 0));
}
@ -178,8 +183,7 @@ 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_) {
motor_control_->disable(*motor);
@ -189,26 +193,29 @@ hardware_interface::CallbackReturn OpenArmHW::on_deactivate(
}
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) {
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;
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*/)
{
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) {
@ -217,20 +224,27 @@ hardware_interface::return_type OpenArmHW::write(
}
}
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]);
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);
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)

11
openarm_hardware/test/test_openarm_hardware.cpp
View File

@ -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,11 +21,9 @@
#include "ros2_control_test_assets/components_urdfs.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
class TestOpenArmHW : public ::testing::Test
{
class TestOpenArmHW : public ::testing::Test {
protected:
void SetUp() override
{
void SetUp() override {
openarm_hardware_7dof_ =
R"(
<ros2_control name="OpenArmHW7DOF" type="system">
@ -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));