添加遥控器复位+控制
This commit is contained in:
parent
db1f33fd7d
commit
dff4d0c2ed
@ -106,11 +106,23 @@ def generate_launch_description():
|
|||||||
output="screen"
|
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([
|
return LaunchDescription([
|
||||||
lf_container,
|
lf_container,
|
||||||
rf_container,
|
rf_container,
|
||||||
lr_container,
|
lr_container,
|
||||||
rr_container,
|
rr_container,
|
||||||
ahrs_driver
|
ahrs_driver,
|
||||||
|
ps2_controller_node # 添加 ps2_controller 节点
|
||||||
])
|
])
|
||||||
38
src/simple_quadruped_control/CMakeLists.txt
Normal file
38
src/simple_quadruped_control/CMakeLists.txt
Normal 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()
|
||||||
@ -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
12
src/simple_quadruped_control/launch/simple_control.launch.py
Normal 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
21
src/simple_quadruped_control/package.xml
Normal 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
208
src/simple_quadruped_control/src/simple_control.cpp
Normal 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;
|
||||||
|
}
|
||||||
@ -56,6 +56,8 @@ void MotorControlNode::control_motor()
|
|||||||
{
|
{
|
||||||
rc_msgs::msg::DataMotors motor_msg;
|
rc_msgs::msg::DataMotors motor_msg;
|
||||||
|
|
||||||
|
timer_count_++;
|
||||||
|
|
||||||
for (int i = 0; i < 3; ++i)
|
for (int i = 0; i < 3; ++i)
|
||||||
{
|
{
|
||||||
MotorCmd cmd;
|
MotorCmd cmd;
|
||||||
@ -70,11 +72,11 @@ void MotorControlNode::control_motor()
|
|||||||
cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
|
cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
|
||||||
cmd.tau = motor[i].cmd.tau;
|
cmd.tau = motor[i].cmd.tau;
|
||||||
|
|
||||||
if (timer_count_ == 100)
|
if (timer_count_ > 50)
|
||||||
{
|
{
|
||||||
Motor_Ctrl_Offline(&motor[i]);
|
Motor_Ctrl_Offline(&motor[i]);
|
||||||
|
timer_count_ = 200;
|
||||||
}
|
}
|
||||||
timer_count_++;
|
|
||||||
|
|
||||||
// 添加异常处理以防止程序崩溃
|
// 添加异常处理以防止程序崩溃
|
||||||
try {
|
try {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user