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-04-09 06:08:41 +08:00
parent db1f33fd7d
commit dff4d0c2ed
7 changed files with 396 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/robot_bringup/launch/robot_bringup.launch.py
View File

@ -106,11 +106,23 @@ def generate_launch_description():
output="screen"
)
# 定义 ps2_controller 节点
ps2_controller_node = Node(
package='ps2_controller',
executable='ps2_reader',
name='ps2_controller_node',
output='screen',
parameters=[
{'device': '/dev/input/js0'} # 可根据需要修改设备路径
]
)
# 返回主启动文件的描述
return LaunchDescription([
lf_container,
rf_container,
lr_container,
rr_container,
ahrs_driver
ahrs_driver,
ps2_controller_node # 添加 ps2_controller 节点
])

38
src/simple_quadruped_control/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 3.8)
project(simple_quadruped_control)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rc_msgs REQUIRED)
# 添加头文件目录
include_directories(include)
add_executable(simple_control src/simple_control.cpp)
ament_target_dependencies(simple_control rclcpp rc_msgs)
install(TARGETS
simple_control
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

100
src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp Normal file
View File

@ -0,0 +1,100 @@
#ifndef SIMPLE_QUADRUPED_CONTROL_HPP
#define SIMPLE_QUADRUPED_CONTROL_HPP
#include "rclcpp/rclcpp.hpp"
#include "rc_msgs/msg/ps2_data.hpp"
#include "rc_msgs/msg/motor_cmds.hpp"
#include "rc_msgs/msg/data_motors.hpp"
struct ps2_t
{
float lx;
float ly;
float rx;
float ry;
float up_down;
float left_right;
bool l1;
bool l2;
bool r1;
bool r2;
bool x;
bool y;
bool a;
bool b;
bool select;
bool start;
};
struct motor_cmd_t
{
float pos;
float vw;
float tau;
float kp;
float kd;
};
struct motor_cmds_t
{
motor_cmd_t motors[3]; // 使用数组替代单独的字段
};
struct data_motor_t
{
float pos;
float vw;
float tau;
};
struct data_motors_t
{
data_motor_t motors[3]; // 使用数组替代单独的字段
};
struct mech_zero_t
{
float motors[3]; // 使用数组替代单独的字段
};
struct cmd_t
{
float pos;
float vw;
float tau;
float kp;
float kd;
};
struct cmds_t
{
cmd_t motors[3]; // 使用数组替代单独的字段
};
ps2_t ps2;
motor_cmds_t motor_cmds;
data_motors_t data_motors;
mech_zero_t mech_zero;
cmds_t cmds;
class QuadrupedSubscriber : public rclcpp::Node
{
public:
QuadrupedSubscriber();
private:
void ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg);
void data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg);
float limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit);
void publish_motor_cmds();
rclcpp::Subscription<rc_msgs::msg::Ps2Data>::SharedPtr ps2_data_subscriber_;
rclcpp::Publisher<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmds_publisher_;
rclcpp::Subscription<rc_msgs::msg::DataMotors>::SharedPtr data_motors_subscriber_;
};
#endif // SIMPLE_QUADRUPED_CONTROL_HPP

12
src/simple_quadruped_control/launch/simple_control.launch.py Normal file
View File

@ -0,0 +1,12 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='simple_quadruped_control',
executable='simple_control',
name='simple_control_node',
output='screen'
)
])

21
src/simple_quadruped_control/package.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>simple_quadruped_control</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="1683502971@qq.com">robofish</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rc_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

208
src/simple_quadruped_control/src/simple_control.cpp Normal file
View File

@ -0,0 +1,208 @@
#include "simple_quadruped_control/simple_control.hpp"
// tau = tff + kp(q - q0) + kd(dq - dq0)
// tau :输出力矩
// tff :前馈力矩
// q：期望角度位置
// q0：当前角度位置
// dq 期望角速度
// dq0 当前角速度
// kp：位置刚度
// kd：速度阻尼
QuadrupedSubscriber::QuadrupedSubscriber() : Node("simple_control")
{
// 订阅 Ps2Data 话题
ps2_data_subscriber_ = this->create_subscription<rc_msgs::msg::Ps2Data>(
"ps2_data", 10,
std::bind(&QuadrupedSubscriber::ps2_data_callback, this, std::placeholders::_1));
// 发布 MotorCmds 话题
motor_cmds_publisher_ = this->create_publisher<rc_msgs::msg::MotorCmds>(
"/LF/motor_cmds", 10);
// 订阅 DataMotors 话题
data_motors_subscriber_ = this->create_subscription<rc_msgs::msg::DataMotors>(
"/LF/data_motors", 10,
std::bind(&QuadrupedSubscriber::data_motors_callback, this, std::placeholders::_1));
// 初始化机械零点
mech_zero.motors[0] = 0.7438;
mech_zero.motors[1] = 0.5693;
mech_zero.motors[2] = 0.6547;
}
void QuadrupedSubscriber::ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg)
{
ps2.lx = msg->lx;
ps2.ly = msg->ly;
ps2.rx = msg->rx;
ps2.ry = msg->ry;
ps2.up_down = msg->up_down;
ps2.left_right = msg->left_right;
ps2.l1 = msg->l1;
ps2.l2 = msg->l2;
ps2.r1 = msg->r1;
ps2.r2 = msg->r2;
ps2.x = msg->x;
ps2.y = msg->y;
ps2.a = msg->a;
ps2.b = msg->b;
ps2.select = msg->select;
ps2.start = msg->start;
// 回零
if (ps2.l1)
{
for (int i = 0; i < 3; ++i)
{ // 重置commonds
cmds.motors[i].pos = mech_zero.motors[i];
cmds.motors[i].vw = 0.0;
cmds.motors[i].tau = 0.0;
cmds.motors[i].kp = 1.2;
cmds.motors[i].kd = 0.05;
// 限制力矩输出
motor_cmds.motors[i].pos = cmds.motors[i].pos;
motor_cmds.motors[i].vw = cmds.motors[i].vw;
motor_cmds.motors[i].tau = cmds.motors[i].tau;
motor_cmds.motors[i].kp = cmds.motors[i].kp;
motor_cmds.motors[i].kd = cmds.motors[i].kd;
motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
}
publish_motor_cmds();
RCLCPP_INFO(this->get_logger(), "Zeroing motors");
return;
}
if (ps2.l2)
{
cmds.motors[0].pos = cmds.motors[0].pos + ps2.lx * 0.01;
cmds.motors[0].vw = 0.0;
cmds.motors[0].tau = 0.0;
cmds.motors[0].kp = 1.2;
cmds.motors[0].kd = 0.05;
cmds.motors[1].pos = cmds.motors[1].pos + ps2.ly * 0.01;
cmds.motors[1].vw = 0.0;
cmds.motors[1].tau = 0.0;
cmds.motors[1].kp = 1.2;
cmds.motors[1].kd = 0.05;
cmds.motors[2].pos = cmds.motors[2].pos + ps2.ly * 0.01;
cmds.motors[2].vw = 0.0;
cmds.motors[2].tau = 0.0;
cmds.motors[2].kp = 1.2;
cmds.motors[2].kd = 0.05;
// 限制力矩输出
for (int i = 0; i < 3; ++i)
{
motor_cmds.motors[i].pos = cmds.motors[i].pos;
motor_cmds.motors[i].vw = cmds.motors[i].vw;
motor_cmds.motors[i].tau = cmds.motors[i].tau;
motor_cmds.motors[i].kp = cmds.motors[i].kp;
motor_cmds.motors[i].kd = cmds.motors[i].kd;
motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
}
publish_motor_cmds();
// RCLCPP_INFO(this->get_logger(), "Zeroing motors");
return;
}
}
void QuadrupedSubscriber::data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg)
{
data_motors.motors[0].pos = msg->data_motor_0.pos;
data_motors.motors[0].vw = msg->data_motor_0.vw;
data_motors.motors[0].tau = msg->data_motor_0.tau;
data_motors.motors[1].pos = msg->data_motor_1.pos;
data_motors.motors[1].vw = msg->data_motor_1.vw;
data_motors.motors[1].tau = msg->data_motor_1.tau;
data_motors.motors[2].pos = msg->data_motor_2.pos;
data_motors.motors[2].vw = msg->data_motor_2.vw;
data_motors.motors[2].tau = msg->data_motor_2.tau;
// RCLCPP_INFO(this->get_logger(), "Updated DataMotors for motors 0, 1, 2");
}
void QuadrupedSubscriber::publish_motor_cmds()
{
auto motor_cmds_msg = rc_msgs::msg::MotorCmds();
// // 设置每个电机的命令
// for (int i = 0; i < 3; ++i)
// {
// motor_cmds_msg.motor_cmds[i].tau = motor_cmds.motors[i].tau;
// motor_cmds_msg.motor_cmds[i].vw = motor_cmds.motors[i].vw;
// motor_cmds_msg.motor_cmds[i].pos = motor_cmds.motors[i].pos;
// motor_cmds_msg.motor_cmds[i].kp = motor_cmds.motors[i].kp;
// motor_cmds_msg.motor_cmds[i].kd = motor_cmds.motors[i].kd;
// }
// 设置每个电机的命令
motor_cmds_msg.motor_cmd_0.tau = motor_cmds.motors[0].tau;
motor_cmds_msg.motor_cmd_0.vw = motor_cmds.motors[0].vw;
motor_cmds_msg.motor_cmd_0.pos = motor_cmds.motors[0].pos;
motor_cmds_msg.motor_cmd_0.kp = motor_cmds.motors[0].kp;
motor_cmds_msg.motor_cmd_0.kd = motor_cmds.motors[0].kd;
motor_cmds_msg.motor_cmd_1.tau = motor_cmds.motors[1].tau;
motor_cmds_msg.motor_cmd_1.vw = motor_cmds.motors[1].vw;
motor_cmds_msg.motor_cmd_1.pos = motor_cmds.motors[1].pos;
motor_cmds_msg.motor_cmd_1.kp = motor_cmds.motors[1].kp;
motor_cmds_msg.motor_cmd_1.kd = motor_cmds.motors[1].kd;
motor_cmds_msg.motor_cmd_2.tau = motor_cmds.motors[2].tau;
motor_cmds_msg.motor_cmd_2.vw = motor_cmds.motors[2].vw;
motor_cmds_msg.motor_cmd_2.pos = motor_cmds.motors[2].pos;
motor_cmds_msg.motor_cmd_2.kp = motor_cmds.motors[2].kp;
motor_cmds_msg.motor_cmd_2.kd = motor_cmds.motors[2].kd;
// 发布 MotorCmds 消息
motor_cmds_publisher_->publish(motor_cmds_msg);
}
// 限制输出力矩的函数
float QuadrupedSubscriber::limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit)
{
// 读取结构体中的参数
float tff = motor_cmd.tau; // 前馈力矩
float kp = motor_cmd.kp; // 位置刚度
float kd = motor_cmd.kd; // 速度阻尼
float q = motor_cmd.pos*6.33; // 期望角度位置
float q0 = data_motor.pos*6.33; // 当前角度位置
float dq = motor_cmd.vw*6.33; // 期望角速度
float dq0 = data_motor.vw*6.33; // 当前角速度
// 计算输出力矩
float tau = tff + kp * (q - q0) + kd * (dq - dq0);
// 如果力矩超过限制值，调整前馈力矩
if (tau > limit)
{
tff -= (tau - limit) / 2;
tau = limit;
tff = tau - kp * (q - q0) - kd * (dq - dq0);
return tff;
}else if (tau < -limit)
{
tff += (tau + limit) / 2;
tau = -limit;
tff = tau - kp * (q - q0) - kd * (dq - dq0);
return tff;
}
return tff;
}
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<QuadrupedSubscriber>());
rclcpp::shutdown();
return 0;
}

6
src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
View File

@ -56,6 +56,8 @@ void MotorControlNode::control_motor()
{
rc_msgs::msg::DataMotors motor_msg;
timer_count_++;
for (int i = 0; i < 3; ++i)
{
MotorCmd cmd;
@ -70,11 +72,11 @@ void MotorControlNode::control_motor()
cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
cmd.tau = motor[i].cmd.tau;
if (timer_count_ == 100)
if (timer_count_ > 50)
{
Motor_Ctrl_Offline(&motor[i]);
timer_count_ = 200;
}
timer_count_++;
// 添加异常处理以防止程序崩溃
try {