From 1fb3f08f9ad60c39378eca79566c42f09bb78644 Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Thu, 26 Mar 2026 20:12:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=A4=9A=E5=85=B3=E8=8A=82?= =?UTF-8?q?=E4=BC=A0=E6=84=9F=E5=99=A8=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- control/openarm_joint_sensor.cpp | 181 +++++++++++++++++++++---------- 1 file changed, 124 insertions(+), 57 deletions(-) diff --git a/control/openarm_joint_sensor.cpp b/control/openarm_joint_sensor.cpp index 6700112..048ae8c 100644 --- a/control/openarm_joint_sensor.cpp +++ b/control/openarm_joint_sensor.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include +#include +#include +#include #include #include #include @@ -35,15 +38,58 @@ #include std::atomic keep_running(true); -std::atomic sensor_angle_rad(0.0); -std::atomic sensor_velocity_rad_s(0.0); -std::atomic sensor_angle_ready(false); +constexpr std::size_t SENSOR_JOINT_NUM = 7; +std::array, SENSOR_JOINT_NUM> sensor_angle_rad; +std::array, SENSOR_JOINT_NUM> sensor_velocity_rad_s; +std::array, SENSOR_JOINT_NUM> sensor_angle_ready; -constexpr canid_t SENSOR_CAN_ID = 0x01; // TODO: replace with your sensor CAN ID -constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling -constexpr double SENSOR_OFFSET = 0.0; +struct openarm_joint_sensor_status +{ + uint8_t id; + double angle_rad; + double velocity_rad_s; + double torque_Nm; +}; + + +constexpr std::array SENSOR_CAN_ID = {0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07}; +// Per-joint sign: use +1.0 or -1.0 to match each joint's positive rotation direction. +constexpr std::array SENSOR_DIRECTION_SIGN = { + -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, 1.0}; +// Per-joint zero offset in radians: joint_angle = sign * raw_angle + offset. +constexpr std::array SENSOR_ZERO_OFFSET_RAD = { + 2.75, -2.49, 0.08, 0.0, 0.0, 0.0, 0.0}; +// constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling +// constexpr double SENSOR_OFFSET = 0.0; constexpr double SENSOR_FILTER_ALPHA = 0.05; // 0~1, larger = less smoothing constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2; // 0~1, larger = less smoothing +constexpr double pi = 3.14159265358979323846; +constexpr double TWO_PI = 2.0 * pi; + +double wrap_to_pi(double angle_rad) { + return std::atan2(std::sin(angle_rad), std::cos(angle_rad)); +} + +double convert_0_to_2pi_to_signed(double angle_rad_0_to_2pi) { + double angle = std::fmod(angle_rad_0_to_2pi, TWO_PI); + if (angle < 0.0) { + angle += TWO_PI; + } + if (angle > pi) { + angle -= TWO_PI; + } + return angle; +} + +int sensor_id_to_joint_index(canid_t id) { + for (std::size_t i = 0; i < SENSOR_CAN_ID.size(); ++i) { + if (SENSOR_CAN_ID[i] == id) { + return static_cast(i); + } + } + return -1; +} int open_can_socket(const std::string &can_interface) { int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW); @@ -69,8 +115,9 @@ int open_can_socket(const std::string &can_interface) { } struct can_filter filter; - filter.can_id = SENSOR_CAN_ID; - filter.can_mask = CAN_SFF_MASK; + filter.can_id = 0x01; + filter.can_mask = CAN_SFF_MASK & 0xF0; + // filter.can_mask = CAN_SFF_MASK; if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { close(sock); throw std::runtime_error("Failed to set CAN filter"); @@ -79,7 +126,7 @@ int open_can_socket(const std::string &can_interface) { return sock; } -bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) { +bool read_sensor(int sock, openarm_joint_sensor_status &status_out) { struct pollfd pfd; pfd.fd = sock; pfd.events = POLLIN; @@ -95,7 +142,15 @@ bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) { } const canid_t frame_id = frame.can_id & CAN_SFF_MASK; - if (frame_id != SENSOR_CAN_ID || frame.can_dlc < 4) { + bool valid_id = false; + for (const auto &id : SENSOR_CAN_ID) { + if (frame_id == id) { + valid_id = true; + break; + } + // std::cout << "Received CAN ID: " << frame_id << ", expected: " << id << std::endl; + } + if (!valid_id || frame.can_dlc < 5) { return false; } @@ -106,10 +161,12 @@ bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) { rawVel |= static_cast(frame.data[3]) << 8; rawVel |= static_cast(frame.data[4]); - velocity_rad_s_out = (static_cast(rawVel) / 100.0 / 4096.0) * 360.0 * (M_PI / 180.0); // Example conversion, adjust as needed + status_out.id = frame_id; + status_out.velocity_rad_s = (static_cast(rawVel) / 100.0 / 4096.0) * 360.0 * (pi / 180.0); - angle_rad_out = (static_cast(rawPos) / 4096.0) * 360.0; // Example conversion, adjust as needed - angle_rad_out = angle_rad_out * (M_PI / 180.0) - 3.14; + // Sensor reports angle in [0, 2pi). Keep it as-is here and normalize later. + status_out.angle_rad = (static_cast(rawPos) / 4096.0) * TWO_PI; + std::cout << "CAN ID: " << frame_id << " angle: " << status_out.angle_rad << " velocity: " << status_out.velocity_rad_s << std::endl; return true; } @@ -123,8 +180,9 @@ void signal_handler(int signal) { class FollowerArmThread : public PeriodicTimerThread { public: FollowerArmThread(std::shared_ptr robot_state, Control *control_f, - double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} + std::size_t joint_num, double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f), + joint_num_(joint_num) {} protected: void before_start() override { std::cout << "follower start thread " << std::endl; } @@ -133,15 +191,16 @@ protected: void on_timer() override { static auto prev_time = std::chrono::steady_clock::now(); - if (sensor_angle_ready.load()) { - // control_f_->SetArmJointReference(0, 0, 0.01); - control_f_->SetArmJointReference(0, sensor_angle_rad.load(), - sensor_velocity_rad_s.load()); - std::cout << "Sensor angle (rad): " << sensor_angle_rad.load() << std::endl; - std::cout << "Sensor velocity (rad/s): " << sensor_velocity_rad_s.load() << std::endl; + for (std::size_t i = 0; i < joint_num_; ++i) { + if (!sensor_angle_ready[i].load()) { + continue; + } + control_f_->SetArmJointReference(static_cast(i), sensor_angle_rad[i].load(), + sensor_velocity_rad_s[i].load()); + // std::cout << "Joint " << i << " angle: " << sensor_angle_rad[i].load() + // << " velocity: " << sensor_velocity_rad_s[i].load() << std::endl; } - // control_f_->SetArmJointReference(0, 0.2, 0.01); // Example: set joint 0 to 0 radians - + control_f_->unilateral_step(); auto now = std::chrono::steady_clock::now(); @@ -159,12 +218,18 @@ protected: private: std::shared_ptr robot_state_; Control *control_f_; + std::size_t joint_num_; }; int main(int argc, char **argv) { try { std::signal(SIGINT, signal_handler); + for (std::size_t i = 0; i < SENSOR_JOINT_NUM; ++i) { + sensor_angle_rad[i].store(0.0); + sensor_velocity_rad_s[i].store(0.0); + sensor_angle_ready[i].store(false); + } // default configration std::string arm_side = "right_arm"; @@ -232,41 +297,40 @@ int main(int argc, char **argv) { int sensor_can_socket = open_can_socket(sensor_can_interface); std::thread can_read_thread([&]() { - double filtered_angle = 0.0; - bool filter_initialized = false; - double filtered_velocity = 0.0; - double prev_angle = 0.0; - auto prev_time = std::chrono::steady_clock::now(); - double velocity = 0.0; + std::array filtered_angle{}; + std::array filtered_velocity{}; + std::array filter_initialized{}; + openarm_joint_sensor_status status; while (keep_running) { - double angle = 0.0; - if (read_sensor(sensor_can_socket, angle, velocity)) { - auto now = std::chrono::steady_clock::now(); - double dt = - std::chrono::duration_cast>(now - prev_time) - .count(); - // std::cout << "dt: " << dt << " s, raw angle: " << angle << " rad" << std::endl; - prev_time = now; - - if (!filter_initialized) { - filtered_angle = angle; - filter_initialized = true; - prev_angle = angle; - filtered_velocity = 0.0; - } else { - filtered_angle = SENSOR_FILTER_ALPHA * angle + - (1.0 - SENSOR_FILTER_ALPHA) * filtered_angle; - - if (dt > 1e-6) { - filtered_velocity = SENSOR_VEL_FILTER_ALPHA * velocity + - (1.0 - SENSOR_VEL_FILTER_ALPHA) * - filtered_velocity; - } - prev_angle = angle; + if (read_sensor(sensor_can_socket, status)) { + const int joint_idx = sensor_id_to_joint_index(status.id); + if (joint_idx < 0) { + continue; } - sensor_angle_rad.store(filtered_angle); - sensor_velocity_rad_s.store(filtered_velocity); - sensor_angle_ready.store(true); + + const std::size_t i = static_cast(joint_idx); + const double sensor_angle_signed = convert_0_to_2pi_to_signed(status.angle_rad); + const double angle = wrap_to_pi( + SENSOR_DIRECTION_SIGN[i] * sensor_angle_signed + SENSOR_ZERO_OFFSET_RAD[i]); + const double velocity = SENSOR_DIRECTION_SIGN[i] * status.velocity_rad_s; + + if (!filter_initialized[i]) { + filtered_angle[i] = angle; + filtered_velocity[i] = velocity; + filter_initialized[i] = true; + } else { + filtered_angle[i] = SENSOR_FILTER_ALPHA * angle + + (1.0 - SENSOR_FILTER_ALPHA) * filtered_angle[i]; + filtered_velocity[i] = SENSOR_VEL_FILTER_ALPHA * velocity + + (1.0 - SENSOR_VEL_FILTER_ALPHA) * + filtered_velocity[i]; + } + + sensor_angle_rad[i].store(filtered_angle[i]); + sensor_velocity_rad_s[i].store(filtered_velocity[i]); + sensor_angle_ready[i].store(true); + std::cout << "Joint " << i << " filtered_angle: " << filtered_angle[i] + << ", filtered_velocity: " << filtered_velocity[i] << std::endl; } } }); @@ -318,7 +382,10 @@ int main(int argc, char **argv) { // thread_f.join(); // Start control process - FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); + const std::size_t controlled_joint_num = + std::min(follower_arm_motor_num, SENSOR_JOINT_NUM); + FollowerArmThread follower_thread(follower_state, control_follower, controlled_joint_num, + FREQUENCY); follower_thread.start_thread();