robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

抽象力控

Browse Source
This commit is contained in:
robofish 2025-05-20 16:50:36 +08:00
parent 5f7c7042bf
commit 493e8082be
6 changed files with 130 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp
View File

@ -1,5 +1,6 @@
#pragma once
#include <array>
#include <cstdint>
#include "mdog_simple_controller/FSM/fsm_state.hpp"
namespace mdog_simple_controller {
@ -12,10 +13,27 @@ public:
private:
bool inited = false;
bool if_stand = false;
uint16_t joint_reached = 0;
float max_speed = 0.001;
float pos_threshold = 0.01;
float speed_threshold = 0.01;
// 默认PD参数
std::array<double, 3> kp = {4.5, 4.5, 4.5};
std::array<double, 3> kd = {0.05, 0.05, 0.05};
std::array<std::array<double, 3>, 4> stand_pos;
std::array<std::array<double, 3>, 4> stand_foot_pos = {
{
{0.0, 0.05, -0.25}, // FR
{0.0, 0.05, -0.25}, // FL
{0.0, 0.05, -0.25}, // RR
{0.0, 0.05, -0.25} // RL
}
};
std::array<std::array<double, 3>, 4> start_pos;
std::array<std::array<double, 3>, 4> target_joint_pos;
std::array<std::array<double, 3>, 4> start_joint_pos;
std::array<std::array<double, 3>, 4> last_joint_pos;
};

4
src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp
View File

@ -29,6 +29,8 @@ void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst);
// real cmd → 输出 cmd
void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd);
double limit_speed(double current, double target, double max_speed);
int8_t get_direction(double target, double current);
} // namespace mdog_simple_controller

5
src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp
View File

@ -68,6 +68,9 @@ public:
MdogControllerData data_;
InputCmd input_cmd_;
bool if_no_hardware_{false};
bool if_debug_{false};
private:
void fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg);
void input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg);
@ -82,7 +85,7 @@ private:
StateBalance state_balance_;
StateTroting state_troting_;
bool if_no_hardware_{false};
rclcpp::Subscription<rc_msgs::msg::LegFdb>::SharedPtr fdb_sub_;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr input_sub_;

55
src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp
View File

@ -1,14 +1,65 @@
#include "mdog_simple_controller/FSM/state_stand.hpp"
#include "mdog_simple_controller/mdog_simple_controller.hpp"
#include "mdog_simple_controller/common/kinematics.hpp"
#include "mdog_simple_controller/common/user_math.hpp"
namespace mdog_simple_controller {
void StateStand::enter(MdogSimpleController* ctrl) {
inited = false;
if_stand = false;
max_speed = 0.04;
joint_reached = 0;
pos_threshold = 0.01;
speed_threshold = 0.01;
const auto& hip_signs = kinematics::get_hip_signs();
for (int leg = 0; leg < 4; ++leg) {
kinematics::inverse_kinematics(stand_foot_pos[leg], kinematics::get_leg_link_length(), target_joint_pos[leg]);
for (int j = 0; j < 3; ++j) {
start_joint_pos[leg][j] = ctrl->data_.feedback_real[leg].pos[j];
last_joint_pos[leg][j] = start_joint_pos[leg][j];
ctrl->data_.command_real[leg].pos_des[j] = 0;
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] = kp[j];
ctrl->data_.command_real[leg].kd[j] = kd[j];
}
target_joint_pos[leg][0] *= hip_signs[leg];
}
}
void StateStand::run(MdogSimpleController* ctrl) {
// SAFE状态下的控制逻辑
if (!if_stand) {
for (int leg = 0; leg < 4; ++leg) {
for (int j = 0; j < 3; ++j) {
ctrl->data_.command_real[leg].pos_des[j] = limit_speed(ctrl->data_.feedback_real[leg].pos[j], target_joint_pos[leg][j], max_speed);
if (!ctrl->if_no_hardware_){
if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) > pos_threshold) {
if (std::abs(ctrl->data_.feedback_real[leg].speed[j]) < speed_threshold) {
ctrl->data_.command_real[leg].torque_des[j] += 0.01*get_direction(target_joint_pos[leg][j], ctrl->data_.feedback_real[leg].pos[j]);
RCLCPP_INFO(ctrl->get_logger(), "leg %d joint %d torque_des: %f", leg, j, ctrl->data_.command_real[leg].torque_des[j]);
} else {
ctrl->data_.command_real[leg].speed_des[j] = 0;
}
}
if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) < 0.01) {
//将对应标志位置为1
joint_reached |= (1 << (leg * 3 + j));
}
}
}
if (joint_reached == 0b0000111111111111) {
if_stand = true;
}
}
}
realcmd_to_cmd(ctrl->data_, ctrl->data_);
}
void StateStand::exit(MdogSimpleController* ctrl) {

37
src/controllers/mdog_simple_controller/src/common/user_math.cpp
View File

@ -47,9 +47,40 @@ void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
}
}
// 插补函数
double interpolate(double start, double end, double alpha) {
return (1 - alpha) * start + alpha * end;
void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd) {
for (int leg = 0; leg < 4; ++leg) {
for (int j = 0; j < 3; ++j) {
data.command[leg].kp[j] = kp[j];
data.command[leg].kd[j] = kd[j];
}
}
}
//pd计算公式：
// u =du + kp * (target - current) + kd * (target_speed - current_speed)
// double simple_torque_ctrl(double detal_pos, )
//计算方向
int8_t get_direction(double target, double current) {
if (target > current) {
return 1;
} else if (target < current) {
return -1;
}
return 0;
}
// 移动速度最大限制器
double limit_speed(double current, double target, double max_speed) {
double diff = target - current;
if (diff > max_speed) {
return current + max_speed;
} else if (diff < -max_speed) {
return current - max_speed;
}
return target;
}
} // namespace mdog_simple_controller

48
src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp
View File

@ -18,6 +18,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
if_debug_ = this->declare_parameter<bool>("if_debug", false);
memset(&data_, 0, sizeof(data_));
current_state_ = &state_zero_;
current_state_->enter(this);
@ -48,7 +50,7 @@ void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs:
input_cmd_.voltage.vx = msg->lx;
input_cmd_.voltage.vy = msg->ly;
input_cmd_.voltage.wz = msg->rx;
// input_cmd_.hight = 0.35;
input_cmd_.hight = 0.35;
input_cmd_.status = msg->command;
}
@ -62,9 +64,10 @@ void MdogSimpleController::control_loop() {
FSMState* target_state = nullptr;
switch (input_cmd_.status) {
case 0: target_state = &state_safe_; break;
case 6: target_state = &state_balance_; break;
case 7: target_state = &state_troting_; break;
case 8: target_state = &state_walk_; break;
// case 5: target_state = &state_balance_; break;
// case 6: target_state = &state_troting_; break;
// case 7: target_state = &state_walk_; break;
case 8: target_state = &state_stand_; break;
default: target_state = &state_safe_; break;
}
if (target_state != current_state_) {
@ -75,9 +78,6 @@ void MdogSimpleController::control_loop() {
if (if_no_hardware_) {
for (int leg = 0; leg < 4; ++leg) {
for (int j = 0; j < 3; ++j) {
// data_.feedback[leg].torque[j] = data_.command[leg].torque_des[j];
// data_.feedback[leg].speed[j] = data_.command[leg].speed_des[j];
// data_.feedback[leg].pos[j] = data_.command[leg].pos_des[j];
data_.feedback_real[leg].torque[j] = data_.command_real[leg].torque_des[j];
data_.feedback_real[leg].speed[j] = data_.command_real[leg].speed_des[j];
data_.feedback_real[leg].pos[j] = data_.command_real[leg].pos_des[j];
@ -85,8 +85,7 @@ void MdogSimpleController::control_loop() {
data_.feedback[leg].speed[j] = data_.command_real[leg].speed_des[j];
}
}
// feedback_to_real(data_, data_);
// real_to_feedback(data_, data_);
real_to_feedback(data_, data_);
}
rc_msgs::msg::LegCmd msg;
@ -102,23 +101,15 @@ void MdogSimpleController::control_loop() {
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(), "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;
// RCLCPP_INFO(this->get_logger(), "cmd: %f %f %f %f %f %f %f %f %f %f %f %f",
// data_.command[0].pos_des[0], data_.command[0].pos_des[1], data_.command[0].pos_des[2],
// data_.command[1].pos_des[0], data_.command[1].pos_des[1], data_.command[1].pos_des[2],
// data_.command[2].pos_des[0], data_.command[2].pos_des[1], data_.command[2].pos_des[2],
// data_.command[3].pos_des[0], data_.command[3].pos_des[1], data_.command[3].pos_des[2]);
sensor_msgs::msg::JointState js_msg;
js_msg.header.stamp = this->now();
js_msg.name = {
"FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
@ -134,13 +125,6 @@ void MdogSimpleController::control_loop() {
}
}
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]);
// }
// }
}