From 861a4d9a5c75bc9f1ad58f85ad34fb122b8f3cd4 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Sat, 17 May 2025 11:17:22 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=89=E4=B8=AA=E4=BC=9A=E5=8D=A1=E4=BD=8F?= =?UTF-8?q?=E7=9A=84bug=EF=BC=8C=E6=9C=89=E7=82=B9=E7=83=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 5 +- README.md | 1 + .../mdog_simple_controller/CMakeLists.txt | 19 ++- .../mdog_simple_controller/FSM/state_safe.hpp | 13 ++ .../common/bezier_curve.hpp | 18 +++ .../common/user_math.hpp | 29 ++++ .../mdog_simple_controller.hpp | 9 +- .../mdog_simple_controller/package.xml | 1 + .../src/FSM/state_balance.cpp | 62 +++++++- .../src/FSM/state_safe.cpp | 27 ++++ .../src/FSM/state_zero.cpp | 2 +- .../src/common/bezier_curve.cpp | 46 ++++++ .../src/common/user_math.cpp | 36 +++++ .../src/mdog_sim_controller.cpp | 142 ++++++++++++++++++ .../src/mdog_simple_controller.cpp | 32 +++- .../launch/visualize.launch.py | 12 +- .../qut/mdog_description/xacro/leg.xacro | 6 +- 17 files changed, 443 insertions(+), 17 deletions(-) create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp create mode 100644 src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp create mode 100644 src/controllers/mdog_simple_controller/src/common/user_math.cpp create mode 100644 src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index c7e459a..23e9642 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,7 @@ { "files.associations": { - "chrono": "cpp", - "functional": "cpp" + "deque": "cpp", + "string": "cpp", + "vector": "cpp" } } \ No newline at end of file diff --git a/README.md b/README.md index e3e46be..c7f656a 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,4 @@ A simple ros2 program for legged robot . Robocon2025 +有点不太会用ros2 controller 所以直接做的控制。 \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index a5dafa1..dcfbbb1 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED) find_package(rc_msgs REQUIRED) find_package(control_input_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) include_directories( @@ -20,15 +21,31 @@ include_directories( add_executable(mdog_simple_controller src/mdog_simple_controller.cpp + src/FSM/state_safe.cpp src/FSM/state_zero.cpp src/FSM/state_balance.cpp src/FSM/state_troting.cpp src/common/kinematics.cpp + src/common/user_math.cpp + src/common/bezier_curve.cpp ) -ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs) +ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs ) + +add_executable(mdog_sim_controller + src/mdog_sim_controller.cpp + src/FSM/state_safe.cpp + src/FSM/state_zero.cpp + src/FSM/state_balance.cpp + src/FSM/state_troting.cpp + src/common/kinematics.cpp + src/common/user_math.cpp + src/common/bezier_curve.cpp +) +ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs) install(TARGETS mdog_simple_controller + mdog_sim_controller DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp new file mode 100644 index 0000000..e26decc --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateSafe : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; +}; + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp new file mode 100644 index 0000000..31417d2 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp @@ -0,0 +1,18 @@ +#pragma once +#include +#include + +namespace mdog_simple_controller { +namespace bezier { + +// 一维贝塞尔曲线 +double bezier_curve_1d(const std::vector& control_points, double t); + +// 二维贝塞尔曲线 +std::array bezier_curve_2d(const std::vector>& control_points, double t); + +// 三维贝塞尔曲线 +std::array bezier_curve_3d(const std::vector>& control_points, double t); + +} // namespace bezier +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp new file mode 100644 index 0000000..8fa481e --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +// 关节0点位置offset和减速比定义 +constexpr float JOINT_OFFSET[4][3] = { + {0.0f, 0.0f, 0.0f}, // 可根据实际填写 + {0.0f, 0.0f, 0.0f}, + {0.0f, 0.0f, 0.0f}, + // {0.2f, 0.393f, 0.5507f} + {5.63f, 5.29f, 4.34f} +}; + +constexpr float JOINT_RATIO[4][3] = { + {6.33f, 6.33f, 6.33f}, + {6.33f, 6.33f, 6.33f}, + {6.33f, 6.33f, 6.33f}, + {6.33f, 6.33f, 6.33f} +}; + +// feedback → real +void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst); + +// real cmd → 输出 cmd +void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst); + +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp index 92d6739..e6d28d5 100644 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp @@ -4,11 +4,14 @@ #include "rc_msgs/msg/leg_fdb.hpp" #include "rc_msgs/msg/leg_cmd.hpp" #include "control_input_msgs/msg/inputs.hpp" +#include "sensor_msgs/msg/joint_state.hpp" #include "geometry_msgs/msg/vector3.hpp" +#include "mdog_simple_controller/FSM/state_safe.hpp" #include "mdog_simple_controller/FSM/state_zero.hpp" #include "mdog_simple_controller/FSM/state_balance.hpp" #include "mdog_simple_controller/FSM/state_troting.hpp" + namespace mdog_simple_controller { @@ -48,7 +51,9 @@ struct InputCmd { struct MdogControllerData { LegJointData feedback[4]; + LegJointData feedback_real[4]; LegJointCmd command[4]; + LegJointCmd command_real[4]; }; @@ -57,7 +62,7 @@ public: MdogSimpleController(); void change_state(FSMState* new_state); - + MdogControllerData data_; InputCmd input_cmd_; private: @@ -74,6 +79,8 @@ private: rclcpp::Subscription::SharedPtr fdb_sub_; rclcpp::Subscription::SharedPtr input_sub_; rclcpp::Subscription::SharedPtr ahrs_sub_; + rclcpp::Publisher::SharedPtr joint_state_pub_; + rclcpp::Publisher::SharedPtr cmd_pub_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/src/controllers/mdog_simple_controller/package.xml b/src/controllers/mdog_simple_controller/package.xml index 41588b5..8102528 100644 --- a/src/controllers/mdog_simple_controller/package.xml +++ b/src/controllers/mdog_simple_controller/package.xml @@ -13,6 +13,7 @@ control_input_msgs geometry_msgs std_msgs + sensor_msgs ament_lint_auto ament_lint_common diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp index 8436bfb..c6bfb6c 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -1,14 +1,74 @@ #include "mdog_simple_controller/FSM/state_balance.hpp" #include "mdog_simple_controller/mdog_simple_controller.hpp" +#include "mdog_simple_controller/common/user_math.hpp" +#include "mdog_simple_controller/common/kinematics.hpp" +#include "mdog_simple_controller/common/bezier_curve.hpp" +#include namespace mdog_simple_controller { +namespace { +double bezier_time = 0.0; +const double bezier_period = 1.0; +} + void StateBalance::enter(MdogSimpleController* ctrl) { // 进入BALANCE状态时的初始化 } void StateBalance::run(MdogSimpleController* ctrl) { - // BALANCE状态下的控制逻辑 + // 贝塞尔控制点 + std::vector> control_points = { + {0.0, 0.0, -0.30}, // 起点 + {0.01, 0.0, -0.05}, // 抬腿中点 + {0.02, 0.0, -0.30} // 落点 + }; + // 起点和终点 + const auto& p0 = control_points.front(); + const auto& p1 = control_points.back(); + + // 时间推进 + static rclcpp::Time last_time = ctrl->now(); + rclcpp::Time now = ctrl->now(); + double dt = (now - last_time).seconds(); + last_time = now; + bezier_time += dt; + if (bezier_time > bezier_period) bezier_time -= bezier_period; + double t = bezier_time / bezier_period; // [0,1] + + // 对角腿同步，交错运动，相位差0.5 + for (int leg = 0; leg < 4; ++leg) { + // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步 + double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5; + double phase = std::fmod(t + phase_offset, 1.0); + + std::array foot_pos; + if (phase < 0.5) { + // 摆动相：贝塞尔曲线 + double swing_t = phase / 0.5; // 归一化到[0,1] + foot_pos = bezier::bezier_curve_3d(control_points, swing_t); + } else { + // 支撑相：直线连接首尾 + double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1] + for (int i = 0; i < 3; ++i) { + foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t; + } + } + + std::array theta; + std::array link = {0.05, 0.25, 0.25}; // 根据实际参数 + bool ok = kinematics::inverse_kinematics(foot_pos, link, theta); + if (ok) { + for (int j = 0; j < 3; ++j) { + ctrl->data_.command_real[leg].pos_des[j] = theta[j]; + ctrl->data_.command_real[leg].speed_des[j] = 0; + ctrl->data_.command_real[leg].torque_des[j] = 0; + ctrl->data_.command_real[leg].kp[j] = 0.8; + ctrl->data_.command_real[leg].kd[j] = 0.01; + } + } + } + realcmd_to_cmd(ctrl->data_, ctrl->data_); } void StateBalance::exit(MdogSimpleController* ctrl) { diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp new file mode 100644 index 0000000..bcdb9f2 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp @@ -0,0 +1,27 @@ +#include "mdog_simple_controller/FSM/state_safe.hpp" +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +void StateSafe::enter(MdogSimpleController* ctrl) { + // 清空电机命令 + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + ctrl->data_.command[leg].torque_des[j] = 0; + ctrl->data_.command[leg].speed_des[j] = 0; + ctrl->data_.command[leg].pos_des[j] = 0; + ctrl->data_.command[leg].kp[j] = 0; + ctrl->data_.command[leg].kd[j] = 0.05; + } + } +} + +void StateSafe::run(MdogSimpleController* ctrl) { + // SAFE状态下的控制逻辑 +} + +void StateSafe::exit(MdogSimpleController* ctrl) { + // 离开SAFE状态时的清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp index 9d7cde7..526ad7d 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp @@ -11,7 +11,7 @@ void StateZero::enter(MdogSimpleController* ctrl) { ctrl->data_.command[leg].speed_des[j] = 0; ctrl->data_.command[leg].pos_des[j] = 0; ctrl->data_.command[leg].kp[j] = 0; - ctrl->data_.command[leg].kd[j] = 0; + ctrl->data_.command[leg].kd[j] = 0.05; } } } diff --git a/src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp b/src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp new file mode 100644 index 0000000..3d25c46 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp @@ -0,0 +1,46 @@ +#include "mdog_simple_controller/common/bezier_curve.hpp" +#include + +namespace mdog_simple_controller { +namespace bezier { + +// 组合数 +static double binomial_coefficient(int n, int k) { + double res = 1.0; + for (int i = 1; i <= k; ++i) { + res *= (n - i + 1) / double(i); + } + return res; +} + +double bezier_curve_1d(const std::vector& control_points, double t) { + int n = control_points.size() - 1; + double result = 0.0; + for (int i = 0; i <= n; ++i) { + double binom = binomial_coefficient(n, i); + result += binom * std::pow(1 - t, n - i) * std::pow(t, i) * control_points[i]; + } + return result; +} + +std::array bezier_curve_2d(const std::vector>& control_points, double t) { + std::vector x, y; + for (const auto& pt : control_points) { + x.push_back(pt[0]); + y.push_back(pt[1]); + } + return { bezier_curve_1d(x, t), bezier_curve_1d(y, t) }; +} + +std::array bezier_curve_3d(const std::vector>& control_points, double t) { + std::vector x, y, z; + for (const auto& pt : control_points) { + x.push_back(pt[0]); + y.push_back(pt[1]); + z.push_back(pt[2]); + } + return { bezier_curve_1d(x, t), bezier_curve_1d(y, t), bezier_curve_1d(z, t) }; +} + +} // namespace bezier +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/common/user_math.cpp b/src/controllers/mdog_simple_controller/src/common/user_math.cpp new file mode 100644 index 0000000..f738b54 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/common/user_math.cpp @@ -0,0 +1,36 @@ +#include "mdog_simple_controller/common/user_math.hpp" + +namespace mdog_simple_controller { + +// feedback → real +void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst) { + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + // 位置：去掉offset，除以减速比 + dst.feedback_real[leg].pos[j] = (src.feedback[leg].pos[j] - JOINT_OFFSET[leg][j]) / JOINT_RATIO[leg][j]; + // 速度：除以减速比 + dst.feedback_real[leg].speed[j] = src.feedback[leg].speed[j] / JOINT_RATIO[leg][j]; + // 力矩：乘以减速比 + dst.feedback_real[leg].torque[j] = src.feedback[leg].torque[j] * JOINT_RATIO[leg][j]; + } + } +} + +// real cmd → 输出 cmd +void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) { + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + // 位置：乘以减速比，加offset + dst.command[leg].pos_des[j] = src.command_real[leg].pos_des[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j]; + // 速度：乘以减速比 + dst.command[leg].speed_des[j] = src.command_real[leg].speed_des[j] * JOINT_RATIO[leg][j]; + // 力矩：除以减速比 + dst.command[leg].torque_des[j] = src.command_real[leg].torque_des[j] / JOINT_RATIO[leg][j]; + // kp/kd直接赋值 + dst.command[leg].kp[j] = src.command_real[leg].kp[j]; + dst.command[leg].kd[j] = src.command_real[leg].kd[j]; + } + } +} + +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp new file mode 100644 index 0000000..f868bbc --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp @@ -0,0 +1,142 @@ +#include "mdog_simple_controller/mdog_simple_controller.hpp" +#include "mdog_simple_controller/common/user_math.hpp" + +namespace mdog_simple_controller +{ + +MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") { + fdb_sub_ = this->create_subscription( + "/leg_fdb", 10, + std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1)); + input_sub_ = this->create_subscription( + "/control_input", 10, + std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1)); + ahrs_sub_ = this->create_subscription( + "/euler_angles", 10, + std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1)); + cmd_pub_ = this->create_publisher("/leg_cmd", 10); + joint_state_pub_ = this->create_publisher("joint_states", 10); + + current_state_ = &state_zero_; + current_state_->enter(this); + timer_ = this->create_wall_timer( + std::chrono::microseconds(2000), + std::bind(&MdogSimpleController::control_loop, this)); +} + +void MdogSimpleController::change_state(FSMState* new_state) { + if (current_state_) current_state_->exit(this); + current_state_ = new_state; + if (current_state_) current_state_->enter(this); +} + +void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg) { + // for (int leg = 0; leg < 4; ++leg) { + // for (int j = 0; j < 3; ++j) { + // int idx = leg * 3 + j; + // data_.feedback[leg].torque[j] = msg->leg_fdb[idx].torque; + // data_.feedback[leg].speed[j] = msg->leg_fdb[idx].speed; + // data_.feedback[leg].pos[j] = msg->leg_fdb[idx].pos; + // } + // } + feedback_to_real(data_, data_); +} + +void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){ + input_cmd_.voltage.vx = msg->lx; + input_cmd_.voltage.vy = msg->ly; + input_cmd_.voltage.wz = msg->rx; + input_cmd_.hight = 0.5; + input_cmd_.status = msg->command; +} + +void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) { + input_cmd_.imu_euler.yaw = msg->z; + input_cmd_.imu_euler.rol = msg->x; + input_cmd_.imu_euler.pit = msg->y; +} + +void MdogSimpleController::control_loop() { + FSMState* target_state = nullptr; + switch (input_cmd_.status) { + case 0: target_state = &state_zero_; break; + case 6: target_state = &state_balance_; break; + case 7: target_state = &state_troting_; break; + default: target_state = &state_zero_; break; + } + if (target_state != current_state_) { + change_state(target_state); + } + if (current_state_) current_state_->run(this); + + rc_msgs::msg::LegCmd msg; + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + msg.leg_cmd[leg * 3 + j].torque_des = data_.command[leg].torque_des[j]; + msg.leg_cmd[leg * 3 + j].speed_des = data_.command[leg].speed_des[j]; + msg.leg_cmd[leg * 3 + j].pos_des = data_.command[leg].pos_des[j]; + msg.leg_cmd[leg * 3 + j].kp = data_.command[leg].kp[j]; + msg.leg_cmd[leg * 3 + j].kd = data_.command[leg].kd[j]; + } + } + msg.header.stamp = this->now(); + msg.header.frame_id = "leg_cmd"; + cmd_pub_->publish(msg); + + if (0){ + RCLCPP_INFO(this->get_logger(), "MdogControllerData:"); + for (int leg = 0; leg < 4; ++leg) { + RCLCPP_INFO(this->get_logger(), "Leg %d:", leg); + for (int j = 0; j < 3; ++j) { + RCLCPP_INFO(this->get_logger(), "Joint %d: Torque: %f, Speed: %f, Pos: %f", + j, data_.feedback[leg].torque[j], data_.feedback[leg].speed[j], data_.feedback[leg].pos[j]); + } + } + RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", + input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); + RCLCPP_INFO(this->get_logger(), "AHRS: Yaw: %f, Roll: %f, Pitch: %f", + input_cmd_.imu_euler.yaw, input_cmd_.imu_euler.rol, input_cmd_.imu_euler.pit); + } + RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", + input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); + sensor_msgs::msg::JointState js_msg; + js_msg.header.stamp = this->now(); + js_msg.name = { + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint" + }; + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + js_msg.position.push_back(data_.feedback_real[leg].pos[j]); + js_msg.velocity.push_back(data_.feedback_real[leg].speed[j]); + js_msg.effort.push_back(data_.feedback_real[leg].torque[j]); + } + } + joint_state_pub_->publish(js_msg); + + //将publish的数据直接赋值给feedback + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + data_.feedback_real[leg].pos[j] = data_.command_real[leg].pos_des[j]; + } + } + + //打印真实数据 + // for (int leg = 0; leg < 4; ++leg) { + // for (int j = 0; j < 3; ++j) { + // RCLCPP_INFO(this->get_logger(), "Leg %d Joint %d: Torque: %f, Speed: %f, Pos: %f", + // leg, j, data_.feedback_real[leg].torque[j], data_.feedback_real[leg].speed[j], data_.feedback_real[leg].pos[j]); + // } + // } +} + +} // namespace mdog_simple_controller + +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/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp index 6a96a64..b63f837 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -1,4 +1,5 @@ #include "mdog_simple_controller/mdog_simple_controller.hpp" +#include "mdog_simple_controller/common/user_math.hpp" namespace mdog_simple_controller { @@ -14,6 +15,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") { "/euler_angles", 10, std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1)); cmd_pub_ = this->create_publisher("/leg_cmd", 10); + joint_state_pub_ = this->create_publisher("joint_states", 10); + current_state_ = &state_zero_; current_state_->enter(this); timer_ = this->create_wall_timer( @@ -36,6 +39,7 @@ void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr ms data_.feedback[leg].pos[j] = msg->leg_fdb[idx].pos; } } + feedback_to_real(data_, data_); } void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){ @@ -55,10 +59,10 @@ void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::Shar void MdogSimpleController::control_loop() { FSMState* target_state = nullptr; switch (input_cmd_.status) { - case 0: target_state = &state_zero_; break; + case 0: target_state = &state_safe_; break; case 6: target_state = &state_balance_; break; case 7: target_state = &state_troting_; break; - default: target_state = &state_zero_; break; + default: target_state = &state_safe_; break; } if (target_state != current_state_) { change_state(target_state); @@ -95,7 +99,29 @@ void MdogSimpleController::control_loop() { } RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); - + sensor_msgs::msg::JointState js_msg; + js_msg.header.stamp = this->now(); + js_msg.name = { + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint" + }; + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + js_msg.position.push_back(data_.feedback_real[leg].pos[j]); + js_msg.velocity.push_back(data_.feedback_real[leg].speed[j]); + js_msg.effort.push_back(data_.feedback_real[leg].torque[j]); + } + } + joint_state_pub_->publish(js_msg); + //打印真实数据 + // for (int leg = 0; leg < 4; ++leg) { + // for (int j = 0; j < 3; ++j) { + // RCLCPP_INFO(this->get_logger(), "Leg %d Joint %d: Torque: %f, Speed: %f, Pos: %f", + // leg, j, data_.feedback_real[leg].torque[j], data_.feedback_real[leg].speed[j], data_.feedback_real[leg].pos[j]); + // } + // } } } // namespace mdog_simple_controller diff --git a/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py b/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py index 5780e6d..cbcfc2f 100644 --- a/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py +++ b/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py @@ -40,10 +40,10 @@ def generate_launch_description(): } ], ), - Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher', - output='screen', - ) + # Node( + # package='joint_state_publisher_gui', + # executable='joint_state_publisher_gui', + # name='joint_state_publisher', + # output='screen', + # ) ]) \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro index 47761e2..090ece8 100644 --- a/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro +++ b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro @@ -35,7 +35,8 @@ - + + @@ -65,7 +66,8 @@ - + +