From 6b2f364d0ec06652e85f0ea71541903895250ee7 Mon Sep 17 00:00:00 2001
From: robofish <1683502971@qq.com>
Date: Sun, 18 May 2025 09:19:09 +0800
Subject: [PATCH] =?UTF-8?q?=E8=B5=B7=E7=A0=81=E5=A5=BD=E7=94=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../FSM/state_balance.hpp | 2 +-
.../common/balance_ctrl.hpp | 0
.../common/kinematics.hpp | 6 +++++
.../mdog_simple_controller/common/pid.hpp | 24 ++++++++++++-------
.../common/user_math.hpp | 6 ++---
.../mdog_simple_controller.hpp | 5 ++--
.../src/FSM/state_balance.cpp | 7 ++++--
.../src/FSM/state_troting.cpp | 24 ++++++++++++-------
.../src/common/balance_ctrl.cpp | 0
.../src/common/kinematics.cpp | 12 ++++++++++
.../mdog_simple_controller/src/common/pid.cpp | 24 ++++++++-----------
.../src/mdog_simple_controller.cpp | 10 ++++----
.../fdilink_ahrs_ROS2/include/ahrs_driver.h | 4 ++--
.../launch/ahrs_driver.launch.py | 2 +-
.../fdilink_ahrs_ROS2/src/ahrs_driver.cpp | 18 +++++++-------
15 files changed, 87 insertions(+), 57 deletions(-)
create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/balance_ctrl.hpp
create mode 100644 src/controllers/mdog_simple_controller/src/common/balance_ctrl.cpp
diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp
index f98d185..007b8f3 100644
--- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp
+++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp
@@ -10,4 +10,4 @@ public:
void exit(MdogSimpleController*) override;
};
-}
\ No newline at end of file
+} // namespace mdog_simple_controller
\ No newline at end of file
diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/balance_ctrl.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/balance_ctrl.hpp
new file mode 100644
index 0000000..e69de29
diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp
index 7ee3898..b591db7 100644
--- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp
+++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp
@@ -7,6 +7,12 @@ namespace kinematics {
// 固定三连杆长度（单位：米，可根据实际机器人参数修改）
constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length();
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs();
+
// 正运动学：已知关节角，求末端位置
void forward_kinematics(const std::array<double, 3>& theta,
const std::array<double, 3>& link,
diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp
index 1b56dfe..0dbe184 100644
--- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp
+++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp
@@ -2,21 +2,29 @@
namespace mdog_simple_controller {
+struct PIDParam {
+ double kp{0.0};
+ double ki{0.0};
+ double kd{0.0};
+ double dt{0.01};
+ double max_out{1e6};
+ double min_out{-1e6};
+};
+
class PID {
public:
- PID(double kp, double ki, double kd, double dt, double max_out = 1e6, double min_out = -1e6);
+ PID(const PIDParam& param);
- void set_gains(double kp, double ki, double kd);
- void set_limits(double min_out, double max_out);
+ void set_param(const PIDParam& param);
void reset();
double compute(double setpoint, double measurement);
+ const PIDParam& get_param() const { return param_; }
+
private:
- double kp_, ki_, kd_;
- double dt_;
- double max_out_, min_out_;
- double prev_error_;
- double integral_;
+ PIDParam param_;
+ double prev_error_{0.0};
+ double integral_{0.0};
};
} // 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
index 8fa481e..0f359ae 100644
--- 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
@@ -6,11 +6,11 @@ namespace mdog_simple_controller {
// 关节0点位置offset和减速比定义
constexpr float JOINT_OFFSET[4][3] = {
- {0.0f, 0.0f, 0.0f}, // 可根据实际填写
- {0.0f, 0.0f, 0.0f},
+ {-6.349f, -2.856f, 22.610f}, // 可根据实际填写
+ {-3.156f, 0.0f, 0.0f},
{0.0f, 0.0f, 0.0f},
// {0.2f, 0.393f, 0.5507f}
- {5.63f, 5.29f, 4.34f}
+ {5.63f, 2.9f, 0.0f}
};
constexpr float JOINT_RATIO[4][3] = {
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 a87372f..f5cabb3 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
@@ -43,8 +43,7 @@ struct AHRS_Eulr_t {
struct InputCmd {
Voltage voltage;
- AHRS_Eulr_t imu_euler;
- AHRS_Eulr_t machine_euler;
+ AHRS_Eulr_t eulr;
float hight;
int8_t status;
};
@@ -54,6 +53,8 @@ struct MdogControllerData {
LegJointData feedback_real[4];
LegJointCmd command[4];
LegJointCmd command_real[4];
+ AHRS_Eulr_t imu_eulr;
+ AHRS_Eulr_t cmd_eulr;
};
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 1b4098a..55695c6 100644
--- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp
+++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp
@@ -1,5 +1,9 @@
#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 <cmath>
namespace mdog_simple_controller {
@@ -8,8 +12,7 @@ void StateBalance::enter(MdogSimpleController* ctrl) {
}
void StateBalance::run(MdogSimpleController* ctrl) {
- // 贝塞尔控制点
-
+
}
void StateBalance::exit(MdogSimpleController* ctrl) {
diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp
index f19515b..f8ff538 100644
--- a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp
+++ b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp
@@ -9,7 +9,7 @@ namespace mdog_simple_controller {
namespace {
double bezier_time = 0.0;
-const double bezier_period = 1.0;
+const double bezier_period = 1.2;
}
void StateTroting::enter(MdogSimpleController* ctrl) {
@@ -19,9 +19,15 @@ void StateTroting::enter(MdogSimpleController* ctrl) {
void StateTroting::run(MdogSimpleController* ctrl) {
//设置所有cmd数据为0
std::vector<std::array<double, 3>> control_points = {
- {0.30, 0.0, 0.15}, // 起点
- {0.10, 0.0, 0.0}, // 抬腿中点
- {0.30, 0.0, -0.15}, // 落点
+ // {0.40, 0.0, 0.15}, // 起点
+ // {0.10, 0.0, 0.0}, // 抬腿中点
+ // {0.40, 0.0, -0.05}, // 落点
+ // {0.40, 0.0, 0.05}, // 起点
+ {0.40, 0.0, 0.05}, // 抬腿中点
+
+ {0.40, 0.0, 0.05}, // 抬腿中点
+ {0.40, 0.0, 0.05}, // 抬腿中点
+ // {0.40, 0.0, 0.05}, // 落点
};
// 起点和终点
const auto& p0 = control_points.front();
@@ -36,6 +42,9 @@ void StateTroting::run(MdogSimpleController* ctrl) {
if (bezier_time > bezier_period) bezier_time -= bezier_period;
double t = bezier_time / bezier_period; // [0,1]
+ const auto& link = kinematics::get_leg_link_length();
+ const auto& hip_signs = kinematics::get_hip_signs();
+
// 对角腿同步，交错运动，相位差0.5
for (int leg = 0; leg < 4; ++leg) {
// FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
@@ -56,13 +65,10 @@ void StateTroting::run(MdogSimpleController* ctrl) {
}
std::array<double, 3> theta;
- std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
if (ok) {
- // 左右腿hip方向相反，FR(0) RR(2)为右腿，FL(1) RL(3)为左腿
- if (leg == 1 || leg == 3) { // 右腿
- theta[0] = -theta[0];
- }
+ // 使用参数化的hip方向
+ theta[0] *= hip_signs[leg];
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;
diff --git a/src/controllers/mdog_simple_controller/src/common/balance_ctrl.cpp b/src/controllers/mdog_simple_controller/src/common/balance_ctrl.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp
index f476ddb..5b5cfc5 100644
--- a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp
+++ b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp
@@ -4,6 +4,17 @@
namespace mdog_simple_controller {
namespace kinematics {
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length() {
+ return LEG_LINK_LENGTH;
+}
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs() {
+ static constexpr std::array<int, 4> HIP_SIGNS = {1, -1, 1, -1};
+ return HIP_SIGNS;
+}
+
// 正运动学：已知关节角，求末端位置
void forward_kinematics(const std::array<double, 3>& theta,
const std::array<double, 3>& link,
@@ -49,5 +60,6 @@ bool inverse_kinematics(const std::array<double, 3>& pos,
return true;
}
+
} // namespace kinematics
} // namespace mdog_simple_controller
\ No newline at end of file
diff --git a/src/controllers/mdog_simple_controller/src/common/pid.cpp b/src/controllers/mdog_simple_controller/src/common/pid.cpp
index c5e94d2..00b7f66 100644
--- a/src/controllers/mdog_simple_controller/src/common/pid.cpp
+++ b/src/controllers/mdog_simple_controller/src/common/pid.cpp
@@ -2,16 +2,12 @@
namespace mdog_simple_controller {
-PID::PID(double kp, double ki, double kd, double dt, double max_out, double min_out)
- : kp_(kp), ki_(ki), kd_(kd), dt_(dt), max_out_(max_out), min_out_(min_out),
- prev_error_(0.0), integral_(0.0) {}
+PID::PID(const PIDParam& param)
+ : param_(param), prev_error_(0.0), integral_(0.0) {}
-void PID::set_gains(double kp, double ki, double kd) {
- kp_ = kp; ki_ = ki; kd_ = kd;
-}
-
-void PID::set_limits(double min_out, double max_out) {
- min_out_ = min_out; max_out_ = max_out;
+void PID::set_param(const PIDParam& param) {
+ param_ = param;
+ reset();
}
void PID::reset() {
@@ -21,11 +17,11 @@ void PID::reset() {
double PID::compute(double setpoint, double measurement) {
double error = setpoint - measurement;
- integral_ += error * dt_;
- double derivative = (error - prev_error_) / dt_;
- double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
- if (output > max_out_) output = max_out_;
- if (output < min_out_) output = min_out_;
+ integral_ += error * param_.dt;
+ double derivative = (error - prev_error_) / param_.dt;
+ double output = param_.kp * error + param_.ki * integral_ + param_.kd * derivative;
+ if (output > param_.max_out) output = param_.max_out;
+ if (output < param_.min_out) output = param_.min_out;
prev_error_ = error;
return output;
}
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 0f5efdc..f2236fb 100644
--- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp
+++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp
@@ -12,7 +12,7 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
"/control_input", 10,
std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
- "/euler_angles", 10,
+ "/eulr_angles", 10,
std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1));
cmd_pub_ = this->create_publisher<rc_msgs::msg::LegCmd>("/leg_cmd", 10);
joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
@@ -53,9 +53,9 @@ void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs:
}
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;
+ data_.imu_eulr.yaw = msg->z;
+ data_.imu_eulr.rol = msg->x;
+ data_.imu_eulr.pit = msg->y;
}
void MdogSimpleController::control_loop() {
@@ -107,8 +107,6 @@ 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);
- 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);
diff --git a/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h b/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h
index baaa193..cf141f6 100644
--- a/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h
+++ b/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h
@@ -87,7 +87,7 @@ private:
//topic
std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
std::string latlon_topic="latlon";
- std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
+ std::string Eulr_angles_topic="/Eulr_angles", Magnetic_topic="/Magnetic";
std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
@@ -97,7 +97,7 @@ private:
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
- rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
+ rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Eulr_angles_pub_;
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
diff --git a/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py b/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py
index 02a4662..dd65def 100644
--- a/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py
+++ b/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py
@@ -23,7 +23,7 @@ def generate_launch_description():
'imu_frame_id_':'gyro_link',
'mag_pose_2d_topic':'/mag_pose_2d',
'Magnetic_topic':'/magnetic',
- 'Euler_angles_topic':'/euler_angles',
+ 'Eulr_angles_topic':'/eulr_angles',
'gps_topic':'/gps/fix',
'twist_topic':'/system_speed',
'NED_odom_topic':'/NED_odometry',
diff --git a/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp b/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp
index f08193e..f0977f7 100644
--- a/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp
+++ b/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp
@@ -24,8 +24,8 @@ ahrsBringup::ahrsBringup()
this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
- this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
- this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+ this->declare_parameter<std::string>("Eulr_angles_topic","/eulr_angles");
+ this->get_parameter("Eulr_angles_topic", Eulr_angles_topic);
this->declare_parameter<std::string>("gps_topic","/gps/fix");
this->get_parameter("gps_topic", gps_topic);
@@ -51,7 +51,7 @@ ahrsBringup::ahrsBringup()
// pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
// pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
// pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
- // pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
+ // pravite_nh.param("Eulr_angles_pub_", Eulr_angles_topic_, std::string("/eulr_angles"));
// pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
//serial
@@ -64,7 +64,7 @@ ahrsBringup::ahrsBringup()
mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
- Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
+ Eulr_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Eulr_angles_topic.c_str(), 10);
Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
@@ -500,15 +500,15 @@ void ahrsBringup::processLoop() // 数据处理过程
pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
mag_pose_pub_->publish(pose_2d);
//std::cout << "YAW: " << pose_2d.theta << std::endl;
- geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;
- Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
- Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
- Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+ geometry_msgs::msg::Vector3 Eulr_angles_2d,Magnetic;
+ Eulr_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+ Eulr_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+ Eulr_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
- Euler_angles_pub_->publish(Euler_angles_2d);
+ Eulr_angles_pub_->publish(Eulr_angles_2d);
Magnetic_pub_->publish(Magnetic);
}