From dff4d0c2ed7b9c8095ef61b4a15f288be60e772a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 9 Apr 2025 06:08:41 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=81=A5=E6=8E=A7=E5=99=A8?= =?UTF-8?q?=E5=A4=8D=E4=BD=8D+=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/robot_bringup.launch.py | 14 +- src/simple_quadruped_control/CMakeLists.txt | 38 ++++ .../simple_control.hpp | 100 +++++++++ .../launch/simple_control.launch.py | 12 + src/simple_quadruped_control/package.xml | 21 ++ .../src/simple_control.cpp | 208 ++++++++++++++++++ .../src/unitree_motor_serial_driver.cpp | 6 +- 7 files changed, 396 insertions(+), 3 deletions(-) create mode 100644 src/simple_quadruped_control/CMakeLists.txt create mode 100644 src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp create mode 100644 src/simple_quadruped_control/launch/simple_control.launch.py create mode 100644 src/simple_quadruped_control/package.xml create mode 100644 src/simple_quadruped_control/src/simple_control.cpp diff --git a/src/robot_bringup/launch/robot_bringup.launch.py b/src/robot_bringup/launch/robot_bringup.launch.py index 6baa4d6..569b09b 100644 --- a/src/robot_bringup/launch/robot_bringup.launch.py +++ b/src/robot_bringup/launch/robot_bringup.launch.py @@ -106,11 +106,23 @@ def generate_launch_description(): output="screen" ) + # 定义 ps2_controller 节点 + ps2_controller_node = Node( + package='ps2_controller', + executable='ps2_reader', + name='ps2_controller_node', + output='screen', + parameters=[ + {'device': '/dev/input/js0'} # 可根据需要修改设备路径 + ] + ) + # 返回主启动文件的描述 return LaunchDescription([ lf_container, rf_container, lr_container, rr_container, - ahrs_driver + ahrs_driver, + ps2_controller_node # 添加 ps2_controller 节点 ]) \ No newline at end of file diff --git a/src/simple_quadruped_control/CMakeLists.txt b/src/simple_quadruped_control/CMakeLists.txt new file mode 100644 index 0000000..6986986 --- /dev/null +++ b/src/simple_quadruped_control/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.8) +project(simple_quadruped_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rc_msgs REQUIRED) + +# 添加头文件目录 +include_directories(include) + +add_executable(simple_control src/simple_control.cpp) +ament_target_dependencies(simple_control rclcpp rc_msgs) + +install(TARGETS + simple_control + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp b/src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp new file mode 100644 index 0000000..ce2dbd6 --- /dev/null +++ b/src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp @@ -0,0 +1,100 @@ +#ifndef SIMPLE_QUADRUPED_CONTROL_HPP +#define SIMPLE_QUADRUPED_CONTROL_HPP + +#include "rclcpp/rclcpp.hpp" +#include "rc_msgs/msg/ps2_data.hpp" +#include "rc_msgs/msg/motor_cmds.hpp" +#include "rc_msgs/msg/data_motors.hpp" + +struct ps2_t +{ + float lx; + float ly; + float rx; + float ry; + + float up_down; + float left_right; + + bool l1; + bool l2; + bool r1; + bool r2; + + bool x; + bool y; + bool a; + bool b; + + bool select; + bool start; +}; + +struct motor_cmd_t +{ + float pos; + float vw; + float tau; + float kp; + float kd; +}; + +struct motor_cmds_t +{ + motor_cmd_t motors[3]; // 使用数组替代单独的字段 +}; + +struct data_motor_t +{ + float pos; + float vw; + float tau; +}; + +struct data_motors_t +{ + data_motor_t motors[3]; // 使用数组替代单独的字段 +}; + +struct mech_zero_t +{ + float motors[3]; // 使用数组替代单独的字段 +}; + +struct cmd_t +{ + float pos; + float vw; + float tau; + float kp; + float kd; +}; + +struct cmds_t +{ + cmd_t motors[3]; // 使用数组替代单独的字段 +}; + +ps2_t ps2; +motor_cmds_t motor_cmds; +data_motors_t data_motors; +mech_zero_t mech_zero; +cmds_t cmds; + +class QuadrupedSubscriber : public rclcpp::Node +{ +public: + QuadrupedSubscriber(); + +private: + void ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg); + void data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg); + float limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit); + void publish_motor_cmds(); + + rclcpp::Subscription::SharedPtr ps2_data_subscriber_; + rclcpp::Publisher::SharedPtr motor_cmds_publisher_; + rclcpp::Subscription::SharedPtr data_motors_subscriber_; +}; + +#endif // SIMPLE_QUADRUPED_CONTROL_HPP \ No newline at end of file diff --git a/src/simple_quadruped_control/launch/simple_control.launch.py b/src/simple_quadruped_control/launch/simple_control.launch.py new file mode 100644 index 0000000..b20cbf5 --- /dev/null +++ b/src/simple_quadruped_control/launch/simple_control.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='simple_quadruped_control', + executable='simple_control', + name='simple_control_node', + output='screen' + ) + ]) \ No newline at end of file diff --git a/src/simple_quadruped_control/package.xml b/src/simple_quadruped_control/package.xml new file mode 100644 index 0000000..812a73b --- /dev/null +++ b/src/simple_quadruped_control/package.xml @@ -0,0 +1,21 @@ + + + + simple_quadruped_control + 0.0.0 + TODO: Package description + robofish + TODO: License declaration + + ament_cmake + + rclcpp + rc_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/simple_quadruped_control/src/simple_control.cpp b/src/simple_quadruped_control/src/simple_control.cpp new file mode 100644 index 0000000..72471a6 --- /dev/null +++ b/src/simple_quadruped_control/src/simple_control.cpp @@ -0,0 +1,208 @@ +#include "simple_quadruped_control/simple_control.hpp" + +// tau = tff + kp(q - q0) + kd(dq - dq0) +// tau :输出力矩 +// tff :前馈力矩 +// q：期望角度位置 +// q0：当前角度位置 +// dq 期望角速度 +// dq0 当前角速度 +// kp：位置刚度 +// kd：速度阻尼 + +QuadrupedSubscriber::QuadrupedSubscriber() : Node("simple_control") +{ + // 订阅 Ps2Data 话题 + ps2_data_subscriber_ = this->create_subscription( + "ps2_data", 10, + std::bind(&QuadrupedSubscriber::ps2_data_callback, this, std::placeholders::_1)); + + // 发布 MotorCmds 话题 + motor_cmds_publisher_ = this->create_publisher( + "/LF/motor_cmds", 10); + + // 订阅 DataMotors 话题 + data_motors_subscriber_ = this->create_subscription( + "/LF/data_motors", 10, + std::bind(&QuadrupedSubscriber::data_motors_callback, this, std::placeholders::_1)); + + // 初始化机械零点 + mech_zero.motors[0] = 0.7438; + mech_zero.motors[1] = 0.5693; + mech_zero.motors[2] = 0.6547; +} + +void QuadrupedSubscriber::ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg) +{ + ps2.lx = msg->lx; + ps2.ly = msg->ly; + ps2.rx = msg->rx; + ps2.ry = msg->ry; + + ps2.up_down = msg->up_down; + ps2.left_right = msg->left_right; + + ps2.l1 = msg->l1; + ps2.l2 = msg->l2; + ps2.r1 = msg->r1; + ps2.r2 = msg->r2; + + ps2.x = msg->x; + ps2.y = msg->y; + ps2.a = msg->a; + ps2.b = msg->b; + + ps2.select = msg->select; + ps2.start = msg->start; + + // 回零 + if (ps2.l1) + { + for (int i = 0; i < 3; ++i) + { // 重置commonds + cmds.motors[i].pos = mech_zero.motors[i]; + cmds.motors[i].vw = 0.0; + cmds.motors[i].tau = 0.0; + cmds.motors[i].kp = 1.2; + cmds.motors[i].kd = 0.05; + // 限制力矩输出 + motor_cmds.motors[i].pos = cmds.motors[i].pos; + motor_cmds.motors[i].vw = cmds.motors[i].vw; + motor_cmds.motors[i].tau = cmds.motors[i].tau; + motor_cmds.motors[i].kp = cmds.motors[i].kp; + motor_cmds.motors[i].kd = cmds.motors[i].kd; + motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5); + } + publish_motor_cmds(); + RCLCPP_INFO(this->get_logger(), "Zeroing motors"); + return; + } + if (ps2.l2) + { + + cmds.motors[0].pos = cmds.motors[0].pos + ps2.lx * 0.01; + cmds.motors[0].vw = 0.0; + cmds.motors[0].tau = 0.0; + cmds.motors[0].kp = 1.2; + cmds.motors[0].kd = 0.05; + + cmds.motors[1].pos = cmds.motors[1].pos + ps2.ly * 0.01; + cmds.motors[1].vw = 0.0; + cmds.motors[1].tau = 0.0; + cmds.motors[1].kp = 1.2; + cmds.motors[1].kd = 0.05; + + cmds.motors[2].pos = cmds.motors[2].pos + ps2.ly * 0.01; + cmds.motors[2].vw = 0.0; + cmds.motors[2].tau = 0.0; + cmds.motors[2].kp = 1.2; + cmds.motors[2].kd = 0.05; + // 限制力矩输出 + for (int i = 0; i < 3; ++i) + { + motor_cmds.motors[i].pos = cmds.motors[i].pos; + motor_cmds.motors[i].vw = cmds.motors[i].vw; + motor_cmds.motors[i].tau = cmds.motors[i].tau; + motor_cmds.motors[i].kp = cmds.motors[i].kp; + motor_cmds.motors[i].kd = cmds.motors[i].kd; + motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5); + } + publish_motor_cmds(); + // RCLCPP_INFO(this->get_logger(), "Zeroing motors"); + return; + } +} + +void QuadrupedSubscriber::data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg) +{ + data_motors.motors[0].pos = msg->data_motor_0.pos; + data_motors.motors[0].vw = msg->data_motor_0.vw; + data_motors.motors[0].tau = msg->data_motor_0.tau; + + data_motors.motors[1].pos = msg->data_motor_1.pos; + data_motors.motors[1].vw = msg->data_motor_1.vw; + data_motors.motors[1].tau = msg->data_motor_1.tau; + + data_motors.motors[2].pos = msg->data_motor_2.pos; + data_motors.motors[2].vw = msg->data_motor_2.vw; + data_motors.motors[2].tau = msg->data_motor_2.tau; + + // RCLCPP_INFO(this->get_logger(), "Updated DataMotors for motors 0, 1, 2"); +} + +void QuadrupedSubscriber::publish_motor_cmds() +{ + auto motor_cmds_msg = rc_msgs::msg::MotorCmds(); + + // // 设置每个电机的命令 + // for (int i = 0; i < 3; ++i) + // { + // motor_cmds_msg.motor_cmds[i].tau = motor_cmds.motors[i].tau; + // motor_cmds_msg.motor_cmds[i].vw = motor_cmds.motors[i].vw; + // motor_cmds_msg.motor_cmds[i].pos = motor_cmds.motors[i].pos; + // motor_cmds_msg.motor_cmds[i].kp = motor_cmds.motors[i].kp; + // motor_cmds_msg.motor_cmds[i].kd = motor_cmds.motors[i].kd; + // } + // 设置每个电机的命令 + motor_cmds_msg.motor_cmd_0.tau = motor_cmds.motors[0].tau; + motor_cmds_msg.motor_cmd_0.vw = motor_cmds.motors[0].vw; + motor_cmds_msg.motor_cmd_0.pos = motor_cmds.motors[0].pos; + motor_cmds_msg.motor_cmd_0.kp = motor_cmds.motors[0].kp; + motor_cmds_msg.motor_cmd_0.kd = motor_cmds.motors[0].kd; + + motor_cmds_msg.motor_cmd_1.tau = motor_cmds.motors[1].tau; + motor_cmds_msg.motor_cmd_1.vw = motor_cmds.motors[1].vw; + motor_cmds_msg.motor_cmd_1.pos = motor_cmds.motors[1].pos; + motor_cmds_msg.motor_cmd_1.kp = motor_cmds.motors[1].kp; + motor_cmds_msg.motor_cmd_1.kd = motor_cmds.motors[1].kd; + + motor_cmds_msg.motor_cmd_2.tau = motor_cmds.motors[2].tau; + motor_cmds_msg.motor_cmd_2.vw = motor_cmds.motors[2].vw; + motor_cmds_msg.motor_cmd_2.pos = motor_cmds.motors[2].pos; + motor_cmds_msg.motor_cmd_2.kp = motor_cmds.motors[2].kp; + motor_cmds_msg.motor_cmd_2.kd = motor_cmds.motors[2].kd; + + + // 发布 MotorCmds 消息 + motor_cmds_publisher_->publish(motor_cmds_msg); +} + +// 限制输出力矩的函数 +float QuadrupedSubscriber::limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit) +{ + // 读取结构体中的参数 + float tff = motor_cmd.tau; // 前馈力矩 + float kp = motor_cmd.kp; // 位置刚度 + float kd = motor_cmd.kd; // 速度阻尼 + float q = motor_cmd.pos*6.33; // 期望角度位置 + float q0 = data_motor.pos*6.33; // 当前角度位置 + float dq = motor_cmd.vw*6.33; // 期望角速度 + float dq0 = data_motor.vw*6.33; // 当前角速度 + + // 计算输出力矩 + float tau = tff + kp * (q - q0) + kd * (dq - dq0); + + // 如果力矩超过限制值，调整前馈力矩 + if (tau > limit) + { + tff -= (tau - limit) / 2; + tau = limit; + tff = tau - kp * (q - q0) - kd * (dq - dq0); + return tff; + }else if (tau < -limit) + { + tff += (tau + limit) / 2; + tau = -limit; + tff = tau - kp * (q - q0) - kd * (dq - dq0); + return tff; + } + return tff; +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp index 82e4074..1013aec 100644 --- a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp +++ b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp @@ -56,6 +56,8 @@ void MotorControlNode::control_motor() { rc_msgs::msg::DataMotors motor_msg; + timer_count_++; + for (int i = 0; i < 3; ++i) { MotorCmd cmd; @@ -70,11 +72,11 @@ void MotorControlNode::control_motor() cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6); cmd.tau = motor[i].cmd.tau; - if (timer_count_ == 100) + if (timer_count_ > 50) { Motor_Ctrl_Offline(&motor[i]); + timer_count_ = 200; } - timer_count_++; // 添加异常处理以防止程序崩溃 try {