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 LEG_LINK_LENGTH = {0.08, 0.25, 0.25}; +// 获取默认三连杆长度 +const std::array& get_leg_link_length(); + +// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL） +const std::array& get_hip_signs(); + // 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, const std::array& 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 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> 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 theta; - std::array 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& get_leg_link_length() { + return LEG_LINK_LENGTH; +} + +// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL） +const std::array& get_hip_signs() { + static constexpr std::array HIP_SIGNS = {1, -1, 1, -1}; + return HIP_SIGNS; +} + // 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, const std::array& link, @@ -49,5 +60,6 @@ bool inverse_kinematics(const std::array& 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( - "/euler_angles", 10, + "/eulr_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); @@ -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::SharedPtr gps_pub_; - rclcpp::Publisher::SharedPtr Euler_angles_pub_; + rclcpp::Publisher::SharedPtr Eulr_angles_pub_; rclcpp::Publisher::SharedPtr Magnetic_pub_; rclcpp::Publisher::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("mag_pose_2d_topic","/mag_pose_2d"); this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic); - this->declare_parameter("Euler_angles_topic","/euler_angles"); - this->get_parameter("Euler_angles_topic", Euler_angles_topic); + this->declare_parameter("Eulr_angles_topic","/eulr_angles"); + this->get_parameter("Eulr_angles_topic", Eulr_angles_topic); this->declare_parameter("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(mag_pose_2d_topic.c_str(), 10); - Euler_angles_pub_ = create_publisher(Euler_angles_topic.c_str(), 10); + Eulr_angles_pub_ = create_publisher(Eulr_angles_topic.c_str(), 10); Magnetic_pub_ = create_publisher(Magnetic_topic.c_str(), 10); twist_pub_ = create_publisher(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); }