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-17 17:57:38 +08:00
parent 861a4d9a5c
commit cca513ae0b
8 changed files with 60 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,21 +31,21 @@ add_executable(mdog_simple_controller
)
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)
# 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
# mdog_sim_controller
DESTINATION lib/${PROJECT_NAME}
)

2
src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp
View File

@ -5,7 +5,7 @@ namespace mdog_simple_controller {
namespace kinematics {
// 固定三连杆长度（单位：米，可根据实际机器人参数修改）
constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.05, 0.30, 0.30};
constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
// 正运动学：已知关节角，求末端位置
void forward_kinematics(const std::array<double, 3>& theta,

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

@ -72,10 +72,13 @@ private:
void control_loop();
FSMState* current_state_{nullptr};
StateSafe state_safe_;
StateZero state_zero_;
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_;
rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr ahrs_sub_;

13
src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py Normal file
View File

@ -0,0 +1,13 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='mdog_simple_controller',
executable='mdog_simple_controller',
name='mdog_simple_controller',
output='screen',
parameters=[{'if_no_hardware': False}]
)
])

13
src/controllers/mdog_simple_controller/launch/no_hardware.launch.py Normal file
View File

@ -0,0 +1,13 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='mdog_simple_controller',
executable='mdog_simple_controller',
name='mdog_simple_controller',
output='screen',
parameters=[{'if_no_hardware': True}]
)
])

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

@ -20,8 +20,8 @@ void StateBalance::run(MdogSimpleController* ctrl) {
// 贝塞尔控制点
std::vector<std::array<double, 3>> control_points = {
{0.0, 0.0, -0.30}, // 起点
{0.01, 0.0, -0.05}, // 抬腿中点
{0.02, 0.0, -0.30} // 落点
{0.05, 0.0, 0.05}, // 抬腿中点
{0.10, 0.0, -0.30} // 落点
};
// 起点和终点
const auto& p0 = control_points.front();
@ -56,7 +56,7 @@ void StateBalance::run(MdogSimpleController* ctrl) {
}
std::array<double, 3> theta;
std::array<double, 3> link = {0.05, 0.25, 0.25}; // 根据实际参数
std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
if (ok) {
for (int j = 0; j < 3; ++j) {

142
src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp
View File

@ -1,142 +0,0 @@
#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<rc_msgs::msg::LegFdb>(
"/leg_fdb", 10,
std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1));
input_sub_ = this->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10,
std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
"/euler_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);
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<mdog_simple_controller::MdogSimpleController>());
rclcpp::shutdown();
return 0;
}

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

@ -17,6 +17,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
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);
if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
current_state_ = &state_zero_;
current_state_->enter(this);
timer_ = this->create_wall_timer(
@ -69,6 +71,18 @@ void MdogSimpleController::control_loop() {
}
if (current_state_) current_state_->run(this);
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];
}
}
// 若有feedback_to_real处理，也可调用
feedback_to_real(data_, data_);
}
rc_msgs::msg::LegCmd msg;
for (int leg = 0; leg < 4; ++leg) {
for (int j = 0; j < 3; ++j) {
@ -124,6 +138,7 @@ void MdogSimpleController::control_loop() {
// }
}
} // namespace mdog_simple_controller
int main(int argc, char **argv) {