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

修改了balance和trot逻辑

Browse Source
This commit is contained in:
robofish 2025-05-18 01:43:28 +08:00
parent 1ec5910f1c
commit a1a29818e8
6 changed files with 117 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/controllers/mdog_simple_controller/CMakeLists.txt
View File

@ -28,6 +28,7 @@ add_executable(mdog_simple_controller
src/common/kinematics.cpp
src/common/user_math.cpp
src/common/bezier_curve.cpp
src/common/pid.cpp
)
ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )

22
src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp Normal file
View File

@ -0,0 +1,22 @@
#pragma once
namespace mdog_simple_controller {
class PID {
public:
PID(double kp, double ki, double kd, double dt, double max_out = 1e6, double min_out = -1e6);
void set_gains(double kp, double ki, double kd);
void set_limits(double min_out, double max_out);
void reset();
double compute(double setpoint, double measurement);
private:
double kp_, ki_, kd_;
double dt_;
double max_out_, min_out_;
double prev_error_;
double integral_;
};
} // namespace mdog_simple_controller

63
src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp
View File

@ -1,78 +1,15 @@
#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 {
namespace {
double bezier_time = 0.0;
const double bezier_period = 1.0;
}
void StateBalance::enter(MdogSimpleController* ctrl) {
// 进入BALANCE状态时的初始化
}
void StateBalance::run(MdogSimpleController* ctrl) {
// 贝塞尔控制点
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}, // 落点
};
// 起点和终点
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<double, 3> 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<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];
}
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) {

67
src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp
View File

@ -1,23 +1,78 @@
#include "mdog_simple_controller/FSM/state_troting.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 {
namespace {
double bezier_time = 0.0;
const double bezier_period = 1.0;
}
void StateTroting::enter(MdogSimpleController* ctrl) {
// 进入TROTING状态时的初始化
}
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}, // 落点
};
// 起点和终点
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) {
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;
// 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<double, 3> 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<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];
}
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 StateTroting::exit(MdogSimpleController* ctrl) {

33
src/controllers/mdog_simple_controller/src/common/pid.cpp Normal file
View File

@ -0,0 +1,33 @@
#include "mdog_simple_controller/common/pid.hpp"
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) {}
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::reset() {
prev_error_ = 0.0;
integral_ = 0.0;
}
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_;
prev_error_ = error;
return output;
}
} // namespace mdog_simple_controller

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

@ -77,9 +77,6 @@ void MdogSimpleController::control_loop() {
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[leg].torque_des[j];
// data_.feedback_real[leg].speed[j] = data_.command[leg].speed_des[j];
// data_.feedback_real[leg].pos[j] = data_.command[leg].pos_des[j];
}
}
feedback_to_real(data_, data_);